From 6df7b03bb60e7e9fa983c7f37647fe245e2a9806 Mon Sep 17 00:00:00 2001 From: shenchenyang <664376944@qq.com> Date: Thu, 12 Feb 2026 19:22:57 +0800 Subject: [PATCH] =?UTF-8?q?launch=E6=96=87=E4=BB=B6=E5=A2=9E=E5=8A=A0tf?= =?UTF-8?q?=E5=8F=91=E5=B8=83=EF=BC=8C=E5=A2=9E=E5=8A=A0=E4=B8=A2=E5=A4=B1?= =?UTF-8?q?=E7=9B=AE=E6=A0=87=E5=81=9C=E6=AD=A2=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/launch_camera.launch.py | 13 ++ .../openarm_camera/stereo_hand_depth_node.py | 46 ++++-- .../aruco_ik_controller.py | 132 ++++-------------- 3 files changed, 81 insertions(+), 110 deletions(-) diff --git a/src/openarm_camera/launch/launch_camera.launch.py b/src/openarm_camera/launch/launch_camera.launch.py index 166e2b9..29777f5 100644 --- a/src/openarm_camera/launch/launch_camera.launch.py +++ b/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] diff --git a/src/openarm_camera/openarm_camera/stereo_hand_depth_node.py b/src/openarm_camera/openarm_camera/stereo_hand_depth_node.py index 622039e..ef18e37 100644 --- a/src/openarm_camera/openarm_camera/stereo_hand_depth_node.py +++ b/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) - 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]) - msg_out.pose.position.z = float(t_ros[2]) + + # 记录时间 + self.last_marker_time = time.time() + + # 记录最后一次有效目标 + self.last_valid_pos = np.array([t_ros[0] + 0.7, t_ros[1], 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,16 +241,37 @@ 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() - msg_out.header.frame_id = 'camera_link' + msg_out.header.frame_id = 'camera_link' # msg_out.pose.position.x = float(Z) # 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) # ----------- 可视化（调试用）----------- diff --git a/src/openarm_moveit_control/openarm_moveit_control/aruco_ik_controller.py b/src/openarm_moveit_control/openarm_moveit_control/aruco_ik_controller.py index 5e99c15..de6783e 100644 --- a/src/openarm_moveit_control/openarm_moveit_control/aruco_ik_controller.py +++ b/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(filtered_pose.pose.position - self.prev_filtered_pos) < POSITION_DEADZONE: - return # EE 位置变化太小，跳过 + + # if self.prev_filtered_pos is not None: + # if np.linalg.norm(curr_pos - self.prev_filtered_pos) < POSITION_DEADZONE: + # return # EE 位置变化太小，跳过 - self.prev_filtered_pos = pos + self.prev_filtered_pos = curr_pos + self.prev_filtered_quat = curr_quat - filtered_pose = PoseStamped() - 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 + 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 + curr_pos[2] = np.clip(curr_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.y = 0.0 - pose.pose.orientation.z = 0.0 - pose.pose.orientation.w = 0.0 + # pose.pose.orientation.x = 1.0 + # pose.pose.orientation.y = 0.0 + # pose.pose.orientation.z = 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.y = target_pos[1] - req.ik_request.pose_stamped.pose.position.z = target_pos[2] + req.ik_request.pose_stamped.pose.position.x = curr_pos[0] + req.ik_request.pose_stamped.pose.position.y = curr_pos[1] + 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)