launch文件增加tf发布，增加丢失目标停止功能

src/openarm_camera/launch/launch_camera.launch.py

@@ -24,8 +24,21 @@ def generate_launch_description():
         executable='stereo_hand_depth_node',
         output='screen'
     )
+
+    static_tf_node = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        name='camera_static_tf',
+        arguments=[
+            '0.0', '0.0', '0.0',   # x y z
+            '0', '0', '0',         # roll pitch yaw
+            'openarm_body_link0',
+            'camera_link'
+        ]
+    )
     return LaunchDescription([
         camera_init,
+        static_tf_node,
         # TimerAction(
         # period=4.0,
         # actions=[camera_cal_init]

src/openarm_camera/openarm_camera/stereo_hand_depth_node.py

@@ -11,6 +11,7 @@ from cv_bridge import CvBridge
 from sensor_msgs.msg import CameraInfo
 import mediapipe as mp
 from scipy.spatial.transform import Rotation as R
+import time
 # from tf_transformations import quaternion_from_euler, euler_from_quaternion
 
 
@@ -18,6 +19,11 @@ class StereoHandDepthNode(Node):
 
     def __init__(self):
         super().__init__('stereo_hand_depth_node')
+        self.marker_timeout = 0.3   # 秒，建议 0.2 ~ 0.5
+        self.last_valid_pos = None
+        self.last_valid_quat = None
+        self.target_pos = np.array([0.0, 0.0, 0.0])
+        self.target_quat = np.array([1.0, 0.0, 0.0, 0.0])
 
         # ---------- 参数 ----------
         self.fx = 600.0   # 焦距（像素）👉 后面用标定值替换
@@ -201,13 +207,14 @@ class StereoHandDepthNode(Node):
                     roll, pitch, yaw = rot.as_euler('xyz')
                     self.get_logger().info(f"quat: ({qx}, {qy}, {qz}, {qw})")
                     self.get_logger().info(f"euler: ({roll*180/3.14:.1f}, {pitch*180/3.14:.1f}, {yaw*180/3.14:.1f})")
-                    msg_out.pose.orientation.x = float(qx)
+
-                    msg_out.pose.orientation.y = float(qy)
+                    # 记录时间
-                    msg_out.pose.orientation.z = float(qz)
+                    self.last_marker_time = time.time()
-                    msg_out.pose.orientation.w = float(qw)
+
-                    msg_out.pose.position.x = float(t_ros[0]) + 0.7
+                    # 记录最后一次有效目标
-                    msg_out.pose.position.y = float(t_ros[1])
+                    self.last_valid_pos = np.array([t_ros[0] + 0.7, t_ros[1], t_ros[2]])
-                    msg_out.pose.position.z = float(t_ros[2])
+                    self.last_valid_quat = np.array([qx, qy, qz, qw])
+                    
             # u = int(u * (w / 2))  # 注意这里是左图宽度
             # v = int(v * h)
             # p.header = msg.header
@@ -234,7 +241,22 @@ class StereoHandDepthNode(Node):
         # Z = (self.fx * self.baseline) / d
         # X = (x - self.cx) * Z / self.fx
         # Y = (y - self.cy) * Z / self.fy
+        now = time.time()
+        marker_valid = (
+            self.last_valid_pos is not None and
+            (now - self.last_marker_time) < self.marker_timeout
+        )
 
+        if marker_valid:
+            self.target_pos = self.last_valid_pos
+            self.target_quat = self.last_valid_quat
+        else:
+            # 🔥 marker 丢失 → 保持最后位姿
+            if self.last_valid_pos is None:
+                cv2.waitKey(1)
+                return  # 连一次都没看到 marker，啥也不做
+            self.target_pos = self.last_valid_pos
+            self.target_quat = self.last_valid_quat
         # 发布
         
         msg_out.header.stamp = self.get_clock().now().to_msg()
@@ -243,7 +265,13 @@ class StereoHandDepthNode(Node):
         # msg_out.pose.position.y = float(X)
         # msg_out.pose.position.z = -float(Y)
         # self.get_logger().info(f"X:{X:.3f}, Y:{Y:.3f}, Z:{Z:.3f}")
-
+        msg_out.pose.orientation.x = float(self.target_quat[0])
+        msg_out.pose.orientation.y = float(self.target_quat[1])
+        msg_out.pose.orientation.z = float(self.target_quat[2])
+        msg_out.pose.orientation.w = float(self.target_quat[3])
+        msg_out.pose.position.x = float(self.target_pos[0])
+        msg_out.pose.position.y = float(self.target_pos[1])
+        msg_out.pose.position.z = float(self.target_pos[2])
         self.pub.publish(msg_out)
 
         # ----------- 可视化（调试用）-----------

src/openarm_moveit_control/openarm_moveit_control/aruco_ik_controller.py

@@ -6,85 +6,18 @@ from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 from moveit_msgs.srv import GetPositionIK
 from sensor_msgs.msg import JointState
 import numpy as np
-from scipy.spatial.transform import Rotation as R
+import time
 
 
-
+POSITION_DEADZONE = 0.010  # 5 mm
-
-POSITION_DEADZONE = 0.050  # 5 mm
-# ---- 配置 ----
-LEFT_ARM_JOINTS = [
-    "openarm_left_joint1",
-    "openarm_left_joint2",
-    "openarm_left_joint3",
-    "openarm_left_joint4",
-    "openarm_left_joint5",
-    "openarm_left_joint6",
-    "openarm_left_joint7",
-]
-
-CONTROL_PERIOD = 0.02  # 20ms
-MAX_VELOCITY = 0.5
-MAX_ACCELERATION = 1.0
-POSE_ALPHA = 0.3
-
-# ---- 平滑/限制类 ----
-class PoseFilter:
-    def __init__(self, alpha=0.3):
-        self.alpha = alpha
-        self.pos = None
-        self.ori = None
-
-    def slerp_quat(q0, q1, alpha):
-        """
-        q0, q1: numpy array [x, y, z, w]
-        alpha: float, 0~1
-        """
-        r0 = R.from_quat(q0)
-        r1 = R.from_quat(q1)
-        r_slerp = R.slerp(0, 1, [r0, r1])([alpha])[0]
-        return r_slerp.as_quat()
-    
-    def filter(self, new_pos, new_ori):
-        if self.pos is None:
-            self.pos = np.array([new_pos.x, new_pos.y, new_pos.z])
-            self.ori = np.array([new_ori.x, new_ori.y, new_ori.z, new_ori.w])
-            return self.pos, self.ori
-        self.pos = self.alpha * np.array([new_pos.x, new_pos.y, new_pos.z]) + (1 - self.alpha) * self.pos
-        self.ori = self.slerp_quat(self.ori, np.array([new_ori.x, new_ori.y, new_ori.z, new_ori.w]), self.alpha)
-        return self.pos, self.ori
-    
-class JointLimiter:
-    def __init__(self, joint_names, max_velocity=0.5, max_acceleration=1.0, dt=0.02):
-        self.joint_names = joint_names
-        self.max_velocity = max_velocity
-        self.max_acceleration = max_acceleration
-        self.dt = dt
-        self.prev_q = np.zeros(len(joint_names))
-        self.prev_delta = np.zeros(len(joint_names))
-
-    def limit(self, q_target):
-        q_target = np.array(q_target)
-        delta = q_target - self.prev_q
-        max_delta = self.max_velocity * self.dt
-        delta = np.clip(delta, -max_delta, max_delta)
-        delta_change = delta - self.prev_delta
-        max_delta_change = self.max_acceleration * self.dt
-        delta_change = np.clip(delta_change, -max_delta_change, max_delta_change)
-        delta = self.prev_delta + delta_change
-        q_next = self.prev_q + delta
-        self.prev_q = q_next
-        self.prev_delta = delta
-        return q_next
 
 class ArucoIKController(Node):
 
     def __init__(self):
         super().__init__("aruco_ik_controller")
-        self.pose_filter = PoseFilter(POSE_ALPHA)
-        self.joint_limiter = JointLimiter(LEFT_ARM_JOINTS, MAX_VELOCITY, MAX_ACCELERATION, CONTROL_PERIOD)
         self.ik_ready = False
         self.prev_filtered_pos = None
+        self.prev_filtered_quat = None
         self.prev_joint_angles = None
         # === 参数（根据你的 openarm 改）===
         self.arm_group = "left_arm"
@@ -167,47 +100,44 @@ class ArucoIKController(Node):
         if seed_state is None:
             self.get_logger().warn("No valid seed, skip IK")
             return
+        curr_pos = np.array([pose.pose.position.x, pose.pose.position.y, pose.pose.position.z])
+        curr_quat = np.array([pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w])
+        # self.get_logger().info(f"Received target pose: {curr_pos}, {curr_quat}")
             
         req = GetPositionIK.Request()
         req.ik_request.group_name = self.arm_group
         
-        pos, ori = self.pose_filter.filter(pose.pose.position, pose.pose.orientation)
+        # if self.prev_filtered_pos is not None:
+        #     if np.linalg.norm(curr_pos - self.prev_filtered_pos) < POSITION_DEADZONE:
+        #         return  # EE 位置变化太小，跳过
             
-        if self.prev_filtered_pos is not None:
+        self.prev_filtered_pos = curr_pos
-            if np.linalg.norm(filtered_pose.pose.position - self.prev_filtered_pos) < POSITION_DEADZONE:
+        self.prev_filtered_quat = curr_quat
-                return  # EE 位置变化太小，跳过
 
-        self.prev_filtered_pos = pos
+        curr_pos[0] = np.clip(curr_pos[0], 0, 0.8)
-
+        curr_pos[1] = np.clip(curr_pos[1], -0.2, 0.2) + 0.15
-        filtered_pose = PoseStamped()
+        curr_pos[2] = np.clip(curr_pos[2], -0.3, 0.3) + 0.3
-        filtered_pose.header = pose.header
-        filtered_pose.pose.position.x, filtered_pose.pose.position.y, filtered_pose.pose.position.z = pos
-        # filtered_pose.pose.orientation.x, filtered_pose.pose.orientation.y, filtered_pose.pose.orientation.z, filtered_pose.pose.orientation.w = ori
-        # smoothed_ori = slerp(last_ori, new_ori, alpha)
-        target_pos = [
-            pos.x,
-            pos.y,
-            pos.z
-        ]
-
-        target_pos[0] = np.clip(target_pos[0], 0, 0.8)
-        target_pos[1] = np.clip(target_pos[1], -0.2, 0.2) + 0.15
-        target_pos[2] = np.clip(target_pos[2], -0.3, 0.3) + 0.3
         # pose.pose.position.x = 0.1
         # pose.pose.position.y = 0.25
         # pose.pose.position.z = 0.4
-        pose.pose.orientation.x = 1.0
+        # pose.pose.orientation.x = 1.0
-        pose.pose.orientation.y = 0.0
+        # pose.pose.orientation.y = 0.0
-        pose.pose.orientation.z = 0.0
+        # pose.pose.orientation.z = 0.0
-        pose.pose.orientation.w = 0.0
+        # pose.pose.orientation.w = 0.0
+
+        # curr_pos = np.array([0.0, 0.0, 0.0])  # 相对于机械臂 base 的位置
+        curr_quat = np.array([1.0, 0.0, 0.0, 0.0])
 
         
 
-        req.ik_request.pose_stamped.pose.position.x = target_pos[0]
+        req.ik_request.pose_stamped.pose.position.x = curr_pos[0]
-        req.ik_request.pose_stamped.pose.position.y = target_pos[1]
+        req.ik_request.pose_stamped.pose.position.y = curr_pos[1]
-        req.ik_request.pose_stamped.pose.position.z = target_pos[2]
+        req.ik_request.pose_stamped.pose.position.z = curr_pos[2]
         # req.ik_request.pose_stamped.pose.position = pose.pose.position
-        req.ik_request.pose_stamped.pose.orientation = pose.pose.orientation
+        req.ik_request.pose_stamped.pose.orientation.x = curr_quat[0]
+        req.ik_request.pose_stamped.pose.orientation.y = curr_quat[1]
+        req.ik_request.pose_stamped.pose.orientation.z = curr_quat[2]
+        req.ik_request.pose_stamped.pose.orientation.w = curr_quat[3]
         req.ik_request.ik_link_name = self.ee_link
         req.ik_request.timeout.sec = 1
         req.ik_request.timeout.nanosec = 0  # 0.2s
@@ -216,7 +146,7 @@ class ArucoIKController(Node):
         req.ik_request.robot_state.joint_state = seed_state
         self.get_logger().info(f"Seed joints: {seed_state.position}")
         # self.get_logger().info(f"joint names: {seed_state.name}")
-        self.get_logger().info(f"Target pose: {pose.pose.position}, {pose.pose.orientation}")
+        self.get_logger().info(f"Target pose: {curr_pos}, {curr_quat}")
 
         future = self.ik_client.call_async(req)
         future.add_done_callback(self.on_ik_result)