增加一键启动相机脚本，修改目标位置限幅

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]
+        ),
     ])

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:

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>