From 404ceea8ac428744386fcc969581491da3f57526 Mon Sep 17 00:00:00 2001
From: shenchenyang <664376944@qq.com>
Date: Mon, 2 Feb 2026 18:48:46 +0800
Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E4=B8=80=E9=94=AE=E5=90=AF?=
 =?UTF-8?q?=E5=8A=A8=E7=9B=B8=E6=9C=BA=E8=84=9A=E6=9C=AC=EF=BC=8C=E4=BF=AE?=
 =?UTF-8?q?=E6=94=B9=E7=9B=AE=E6=A0=87=E4=BD=8D=E7=BD=AE=E9=99=90=E5=B9=85?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../launch/launch_camera.launch.py            | 33 ++++++++++++++++---
 .../openarm_camera/hand_to_moveit_planning.py | 33 +++++++++++--------
 src/openarm_camera/package.xml                |  2 +-
 3 files changed, 50 insertions(+), 18 deletions(-)

diff --git a/src/openarm_camera/launch/launch_camera.launch.py b/src/openarm_camera/launch/launch_camera.launch.py
index 122cbb0..166e2b9 100644
--- a/src/openarm_camera/launch/launch_camera.launch.py
+++ b/src/openarm_camera/launch/launch_camera.launch.py
@@ -1,12 +1,37 @@
+import os
 from launch import LaunchDescription
+from launch.actions import ExecuteProcess, TimerAction, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import PathJoinSubstitution
 from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from ament_index_python.packages import get_package_share_directory
+import xacro
 
 def generate_launch_description():
-    stereo_proc = Node(
-        package='stereo_image_proc',
-        executable='stereo_image_proc',
+    camera_init = Node(
+        package='openarm_camera',
+        executable='camera_pub_node',
+        output='screen'
+    )
+    camera_cal_init = Node(
+        package='openarm_camera',
+        executable='camera_node',
+        output='screen'
+    )
+    stereo_hand_depth = Node(
+        package='openarm_camera',
+        executable='stereo_hand_depth_node',
         output='screen'
     )
     return LaunchDescription([
-        stereo_proc
+        camera_init,
+        # TimerAction(
+        # period=4.0,
+        # actions=[camera_cal_init]
+        # ),
+        TimerAction(
+        period=8.0,
+        actions=[stereo_hand_depth]
+        ),
     ])
diff --git a/src/openarm_camera/openarm_camera/hand_to_moveit_planning.py b/src/openarm_camera/openarm_camera/hand_to_moveit_planning.py
index 8d547c1..60b37dd 100644
--- a/src/openarm_camera/openarm_camera/hand_to_moveit_planning.py
+++ b/src/openarm_camera/openarm_camera/hand_to_moveit_planning.py
@@ -26,7 +26,7 @@ class HandFollowMoveIt(Node):
         self.p_prev = None
 
         self.plan_period = 1.0       # 秒（规划频率）
-        self.min_delta = 0.05        # m，手移动超过才重新规划
+        self.min_delta = 0.01        # m，手移动超过才重新规划
         self.z_min = 0.15            # 防止撞桌
         self.z_max = 0.60
         self.latest_hand = None
@@ -37,6 +37,8 @@ class HandFollowMoveIt(Node):
         # ====== TF ======
         self.tf_buffer = tf2_ros.Buffer()
         self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
+        self.send_goal([0.0, 0.15, 0.3])
+        time.sleep(3)
 
         # ====== 订阅 ======
         self.sub = self.create_subscription(
@@ -69,9 +71,9 @@ class HandFollowMoveIt(Node):
         """ 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)
+        p[0] = np.clip(p[0], -0.1, 0.1)
+        p[1] = np.clip(p[1], -0.20, 0.20)
+        p[2] = np.clip(p[2], 0.0, 0.30)
 
         # 2️⃣ 低通滤波
         if self.p_prev is None:
@@ -96,17 +98,20 @@ class HandFollowMoveIt(Node):
     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
+        # pose.pose.position.x = float(p[0])
+        # pose.pose.position.y = float(p[0])
+        # pose.pose.position.z = float(p[2])
+        pose.pose.orientation.x = 0.72
+        pose.pose.orientation.y = 0.0
+        pose.pose.orientation.z = 0.68
+        pose.pose.orientation.w = 0.0
+        pose.pose.position.x = 0.1
+        pose.pose.position.y = 0.155
+        pose.pose.position.z = 0.31
         goal = MoveGroup.Goal()
         req = goal.request
         req.group_name = "left_arm"
-        req.allowed_planning_time = 2.0
+        req.allowed_planning_time = 5.0
         req.max_velocity_scaling_factor = 0.2
 
         pc = PositionConstraint()
@@ -132,7 +137,9 @@ class HandFollowMoveIt(Node):
 
             # 限制 workspace
             target = self.latest_hand.copy()
-            target[2] = np.clip(target[2], self.z_min, self.z_max)
+            target[0] = np.clip(target[0], -0.1, 0.1)
+            target[1] = np.clip(target[1], -0.2, 0.2)
+            target[2] = np.clip(target[2], 0.0, 0.3)
 
             # 判断是否值得重新规划
             if self.last_target is not None:
diff --git a/src/openarm_camera/package.xml b/src/openarm_camera/package.xml
index 4f6cbc0..295ba13 100644
--- a/src/openarm_camera/package.xml
+++ b/src/openarm_camera/package.xml
@@ -10,7 +10,7 @@
   <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>cv_bridge</exec_depend>
   <exec_depend>usb_cam</exec_depend>
-  <exec_depend>stereo_image_proc</exec_depend>
+  <exec_depend>ros2launch</exec_depend>
 
 
   <test_depend>ament_copyright</test_depend>