openarm/openarm_test
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
shenchenyang 2026-02-12 19:22:57 +08:00
parent ca79489052
commit 6df7b03bb6
3 changed files with 81 additions and 110 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/openarm_camera/launch/launch_camera.launch.py
View File

@ -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]

46
src/openarm_camera/openarm_camera/stereo_hand_depth_node.py
View File

@ -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)
# ----------- 可视化（调试用）-----------

132
src/openarm_moveit_control/openarm_moveit_control/aruco_ik_controller.py
View File

@ -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)