From 98797842f4f1fded1171843f805b09bc51950100 Mon Sep 17 00:00:00 2001
From: shen <664376944@qq.com>
Date: Thu, 29 Jan 2026 00:38:58 +0800
Subject: [PATCH] =?UTF-8?q?=E8=A7=A3=E5=86=B3=E6=89=8B=E8=AF=86=E5=88=AB?=
=?UTF-8?q?=E5=81=8F=E7=A7=BB=E7=9A=84=E9=97=AE=E9=A2=98=EF=BC=8C=E5=A2=9E?=
=?UTF-8?q?=E5=8A=A0=E6=89=8B=E8=B7=9F=E8=B8=AA(=E6=9C=AA=E5=AE=8C?=
=?UTF-8?q?=E6=88=90)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../openarm_camera/camera_pub.py | 4 +-
.../openarm_camera/hand_to_moveit_planning.py | 156 ++++++++++++++++++
.../openarm_camera/stereo_hand_depth_node.py | 11 +-
src/openarm_camera/package.xml | 6 +
src/openarm_camera/setup.py | 1 +
5 files changed, 172 insertions(+), 6 deletions(-)
create mode 100644 src/openarm_camera/openarm_camera/hand_to_moveit_planning.py
diff --git a/src/openarm_camera/openarm_camera/camera_pub.py b/src/openarm_camera/openarm_camera/camera_pub.py
index 6bdadb3..c5b43ca 100644
--- a/src/openarm_camera/openarm_camera/camera_pub.py
+++ b/src/openarm_camera/openarm_camera/camera_pub.py
@@ -8,8 +8,8 @@ import numpy as np
from camera_info_manager import CameraInfoManager
from sensor_msgs.msg import CameraInfo
-CAMERA_WIDTH = int(1280)
-CAMERA_HEIGHT = int(480)
+CAMERA_WIDTH = 1280
+CAMERA_HEIGHT = 480
FRAME_WIDTH = int(CAMERA_WIDTH / 2)
FRAME_HEIGHT = int(CAMERA_HEIGHT / 2)
diff --git a/src/openarm_camera/openarm_camera/hand_to_moveit_planning.py b/src/openarm_camera/openarm_camera/hand_to_moveit_planning.py
new file mode 100644
index 0000000..8d547c1
--- /dev/null
+++ b/src/openarm_camera/openarm_camera/hand_to_moveit_planning.py
@@ -0,0 +1,156 @@
+import rclpy
+from rclpy.node import Node
+from rclpy.action import ActionClient
+import numpy as np
+import time
+
+from moveit_msgs.action import MoveGroup
+from moveit_msgs.msg import Constraints, PositionConstraint
+from geometry_msgs.msg import PointStamped, PoseStamped
+from shape_msgs.msg import SolidPrimitive
+
+import tf2_ros
+import tf2_geometry_msgs
+
+
+
+class HandFollowMoveIt(Node):
+ def __init__(self):
+ super().__init__('hand_follow_moveit')
+
+ self.client = ActionClient(self, MoveGroup, '/move_action')
+
+ self.last_sent = time.time()
+ self.send_period = 0.3 # 约 3 Hz
+
+ self.p_prev = None
+
+ self.plan_period = 1.0 # 秒（规划频率）
+ self.min_delta = 0.05 # m，手移动超过才重新规划
+ self.z_min = 0.15 # 防止撞桌
+ self.z_max = 0.60
+ self.latest_hand = None
+ self.last_target = None
+
+ self.base_frame = 'openarm_body_link0'
+
+ # ====== TF ======
+ self.tf_buffer = tf2_ros.Buffer()
+ self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
+
+ # ====== 订阅 ======
+ self.sub = self.create_subscription(
+ PointStamped,
+ '/hand_3d',
+ self.hand_cb,
+ 10
+ )
+ self.timer = self.create_timer(self.plan_period, self.plan_timer)
+
+ self.get_logger().info('Hand → MoveIt Planning node started')
+ # ---------- 回调 ----------
+ def hand_cb(self, msg):
+ try:
+ hand = self.tf_buffer.transform(
+ msg,
+ self.base_frame,
+ timeout=rclpy.duration.Duration(seconds=0.1)
+ )
+ except Exception:
+ return
+ print(f"手坐标（{self.base_frame}):{hand.point.x:.3f}, {hand.point.y:.3f}, {hand.point.z:.3f}")
+ self.latest_hand = np.array([
+ hand.point.x,
+ hand.point.y,
+ hand.point.z
+ ])
+
+ def update_hand(self, p):
+ """ p: np.array([x,y,z]) in base_link """
+
+ # 1️⃣ 限幅（按你机械臂改）
+ p[0] = np.clip(p[0], 0.25, 0.55)
+ p[1] = np.clip(p[1], -0.30, 0.30)
+ p[2] = np.clip(p[2], 0.25, 0.30)
+
+ # 2️⃣ 低通滤波
+ if self.p_prev is None:
+ self.p_prev = p
+ return
+
+ alpha = 0.2
+ p = alpha * p + (1 - alpha) * self.p_prev
+
+ # 3️⃣ 死区
+ if np.linalg.norm(p - self.p_prev) < 0.015:
+ return
+
+ # 4️⃣ 控制频率
+ if time.time() - self.last_sent < self.send_period:
+ return
+
+ self.send_goal(p)
+ self.p_prev = p
+ self.last_sent = time.time()
+
+ def send_goal(self, p):
+ pose = PoseStamped()
+ pose.header.frame_id = "openarm_body_link0"
+ pose.pose.position.x = float(p[0])
+ pose.pose.position.y = float(p[1])
+ pose.pose.position.z = float(p[2])
+ pose.pose.orientation.w = 1.0
+ # pose.pose.position.x = 0.0
+ # pose.pose.position.y = 0.153
+ # pose.pose.position.z = 0.262
+ goal = MoveGroup.Goal()
+ req = goal.request
+ req.group_name = "left_arm"
+ req.allowed_planning_time = 2.0
+ req.max_velocity_scaling_factor = 0.2
+
+ pc = PositionConstraint()
+ pc.header = pose.header
+ pc.link_name = "openarm_left_link7"
+
+ box = SolidPrimitive()
+ box.type = SolidPrimitive.BOX
+ box.dimensions = [0.02, 0.02, 0.02]
+ pc.constraint_region.primitives.append(box)
+ pc.constraint_region.primitive_poses.append(pose.pose)
+
+ constraints = Constraints()
+ constraints.position_constraints.append(pc)
+ req.goal_constraints.append(constraints)
+
+ self.client.wait_for_server()
+ self.client.send_goal_async(goal)
+
+ def plan_timer(self):
+ if self.latest_hand is None:
+ return
+
+ # 限制 workspace
+ target = self.latest_hand.copy()
+ target[2] = np.clip(target[2], self.z_min, self.z_max)
+
+ # 判断是否值得重新规划
+ if self.last_target is not None:
+ if np.linalg.norm(target - self.last_target) < self.min_delta:
+ return
+
+ self.last_target = target.copy()
+ self.send_goal(target)
+ print(f"发送新目标：{target[0]:.3f}, {target[1]:.3f}, {target[2]:.3f}")
+
+def main():
+ rclpy.init()
+ node = HandFollowMoveIt()
+ rclpy.spin(node)
+ rclpy.shutdown()
+ # while True:
+ # p_hand_base = get_hand_3d() # 你已有
+ # node.update_hand(p_hand_base)
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
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 e91b809..08f58e2 100644
--- a/src/openarm_camera/openarm_camera/stereo_hand_depth_node.py
+++ b/src/openarm_camera/openarm_camera/stereo_hand_depth_node.py
@@ -132,7 +132,7 @@ class StereoHandDepthNode(Node):
if results.multi_hand_landmarks:
wrist = results.multi_hand_landmarks[0].landmark[0]
- u = int(wrist.x * w)
+ u = int(wrist.x * (w / 2)) # 注意这里是左图宽度
v = int(wrist.y * h)
p.header = msg.header
p.point.x = float(u)
@@ -142,7 +142,7 @@ class StereoHandDepthNode(Node):
# 转灰度
left_gray = cv2.cvtColor(left_rect, cv2.COLOR_BGR2GRAY)
right_gray = cv2.cvtColor(right_rect, cv2.COLOR_BGR2GRAY)
- # cv2.imshow('left', left_gray)
+
# 计算视差
disparity = self.stereo.compute(left_gray, right_gray).astype(np.float32) / 16.0
@@ -150,11 +150,14 @@ class StereoHandDepthNode(Node):
# ----------- 这里先用“假手点”（画面中心）-----------
x = int(p.point.x)
y = int(p.point.y)
- print(f'Wrist pixel: ({x}, {y})')
+ cv2.circle(left_gray, (x, y), 5, (255, 0, 0), -1)
+ # print("left_gray.shape:", left_gray.shape)
+ # cv2.imshow('left', left_gray)
+ # print(f'Wrist pixel: ({x}, {y})')
# ROI 取中值，稳很多
roi = disparity[y-3:y+3, x-3:x+3]
d = np.nanmedian(roi)
- print(f'Disparity at wrist: {roi}')
+ # print(f'Disparity at wrist: {roi}')
if d <= 0:
return
diff --git a/src/openarm_camera/package.xml b/src/openarm_camera/package.xml
index c3e07f8..4f6cbc0 100644
--- a/src/openarm_camera/package.xml
+++ b/src/openarm_camera/package.xml
@@ -6,6 +6,12 @@
TODO: Package description
shen
TODO: License declaration
+ rclpy
+ sensor_msgs
+ cv_bridge
+ usb_cam
+ stereo_image_proc
+
ament_copyright
ament_flake8
diff --git a/src/openarm_camera/setup.py b/src/openarm_camera/setup.py
index 61850cd..0fe476d 100644
--- a/src/openarm_camera/setup.py
+++ b/src/openarm_camera/setup.py
@@ -32,6 +32,7 @@ setup(
'camera_node = openarm_camera.camera:main',
'camera_pub_node = openarm_camera.camera_pub:main',
'stereo_hand_depth_node = openarm_camera.stereo_hand_depth_node:main',
+ 'hand_to_moveit_planning = openarm_camera.hand_to_moveit_planning:main',
],
},
)