feat: 添加机械臂 ROS 2 控制节点和视觉抓取系统

- 创建 arm_control_msgs 包：定义机械臂控制的消息和服务接口 - 消息：JointState, TCPPose - 服务：MoveJoints, MovePose, GetPose, SetGripper - 实现 arm_control 节点：独立的机械臂控制 ROS 节点 - 完整的逆运动学和正运动学 - 关节空间和笛卡尔空间运动控制 - UDP 通信与 ESP32 - 状态发布（10Hz） - 实现 vision_grasp 节点：自动化视觉抓取 - 相机坐标系到基坐标系的完整变换 - 自动抓取流程：释放→移动→抓取→回收 - 自动释放流程：移动→释放→回收 - 多线程执行器支持 - 添加完整文档 - ARM_CONTROL_README.md: 机械臂控制节点使用指南 - VISION_GRASP_README.md: 视觉抓取节点使用指南 - QUICKSTART.md: 快速开始指南 - 文档重命名：docs/craic.md → docs/arm.md
2026-06-16 18:45:01 +08:00
parent 7144005be2
commit 83b32542ef
22 changed files with 3119 additions and 3 deletions
--- a/docs/VISION_GRASP_README.md
+++ b/docs/VISION_GRASP_README.md
@@ -0,0 +1,359 @@
+# 视觉抓取节点使用指南
+
+## 概述
+
+`vision_grasp` 节点基于 `camera_to_base.py` 实现自动抓取和释放功能，将相机坐标系的检测结果转换为机械臂基坐标系，并自动执行抓取流程。
+
+## 功能
+
+### 1. 抓取功能
+
+**输入**：相机坐标系 `(x, y, z)`
+
+**流程**：
+1. 坐标转换：`(xc, yc, zc) = (x, -y, z)`（图像坐标到相机坐标）
+2. 转换到基坐标系
+3. 释放夹爪（duration=0）
+4. 移动到目标位置（duration=3s）
+5. 抓取（duration=1s）
+6. 回收到 (200, 0, 当前z)
+
+### 2. 释放功能
+
+**输入**：基坐标系 `(x, y, z)`
+
+**流程**：
+1. 移动到释放位置
+2. 释放夹爪（duration=0）
+3. 回收到 (200, 0, 当前z)
+
+## 编译
+
+```bash
+cd ros2
+colcon build --packages-select udp_teleop
+source install/setup.bash
+```
+
+## 运行
+
+### 启动节点
+
+**终端 1**：启动机械臂控制节点
+```bash
+ros2 run udp_teleop arm_control \
+    --ros-args --params-file src/udp_teleop/config/arm_control.yaml
+```
+
+**终端 2**：启动视觉抓取节点
+```bash
+ros2 run udp_teleop vision_grasp \
+    --ros-args --params-file src/udp_teleop/config/vision_grasp.yaml
+```
+
+## 使用
+
+### 方法 1：发布话题触发抓取
+
+```bash
+# 抓取：输入相机坐标
+ros2 topic pub --once /vision_grasp/grasp_target geometry_msgs/Point \
+    "{x: 10.0, y: 5.0, z: 250.0}"
+
+# 释放：输入基坐标
+ros2 topic pub --once /vision_grasp/release_target geometry_msgs/Point \
+    "{x: 100.0, y: 150.0, z: -100.0}"
+```
+
+### 方法 2：Python 脚本集成
+
+```python
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Point
+
+class VisionDetector(Node):
+    def __init__(self):
+        super().__init__('vision_detector')
+        
+        # 创建发布者
+        self.grasp_pub = self.create_publisher(
+            Point,
+            'vision_grasp/grasp_target',
+            10
+        )
+    
+    def detect_and_grasp(self):
+        # 模拟检测结果（相机坐标系）
+        camera_x = 10.0  # 相机右侧 10mm
+        camera_y = 5.0   # 相机下方 5mm
+        camera_z = 250.0 # 前方 250mm
+        
+        # 发布抓取目标
+        msg = Point()
+        msg.x = camera_x
+        msg.y = camera_y
+        msg.z = camera_z
+        
+        self.grasp_pub.publish(msg)
+        self.get_logger().info(f'发送抓取目标: ({camera_x}, {camera_y}, {camera_z})')
+
+def main():
+    rclpy.init()
+    node = VisionDetector()
+    
+    # 检测并抓取
+    node.detect_and_grasp()
+    
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
+```
+
+### 方法 3：与检测节点集成
+
+```python
+#!/usr/bin/env python3
+"""完整的视觉检测+抓取示例"""
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Image
+from geometry_msgs.msg import Point
+import cv2
+from cv_bridge import CvBridge
+
+class VisionPipeline(Node):
+    def __init__(self):
+        super().__init__('vision_pipeline')
+        
+        # 订阅相机图像
+        self.image_sub = self.create_subscription(
+            Image,
+            '/camera/image_raw',
+            self.on_image,
+            10
+        )
+        
+        # 发布抓取目标
+        self.grasp_pub = self.create_publisher(
+            Point,
+            'vision_grasp/grasp_target',
+            10
+        )
+        
+        self.bridge = CvBridge()
+    
+    def on_image(self, msg):
+        # 转换 ROS 图像到 OpenCV
+        image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
+        
+        # 检测物体（示例：使用轮廓检测）
+        detected = self.detect_object(image)
+        
+        if detected:
+            camera_x, camera_y, camera_z = detected
+            
+            # 发布抓取目标
+            target = Point()
+            target.x = camera_x
+            target.y = camera_y
+            target.z = camera_z
+            
+            self.grasp_pub.publish(target)
+            self.get_logger().info(f'检测到物体，发送抓取指令')
+    
+    def detect_object(self, image):
+        """检测物体并返回相机坐标"""
+        # TODO: 实现你的检测算法
+        # 1. 图像处理（阈值、轮廓等）
+        # 2. 获取像素坐标 (u, v) 和像素宽度
+        # 3. 使用相似三角形计算深度
+        # 4. 转换到相机坐标系
+        
+        # 示例返回值
+        return (10.0, 5.0, 250.0)  # (xc, yc, zc)
+
+def main():
+    rclpy.init()
+    node = VisionPipeline()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
+```
+
+## 参数配置
+
+编辑 `config/vision_grasp.yaml`：
+
+```yaml
+vision_grasp:
+  ros__parameters:
+    # 相机到 TCP 的变换（如果相机不在 TCP 中心）
+    cam_tx: 0.0      # X 偏移
+    cam_ty: 0.0      # Y 偏移（高度）
+    cam_tz: 0.0      # Z 偏移（前后）
+    
+    # 回收位置
+    retract_position_x: 200.0
+    retract_position_y: 0.0
+    
+    # 运动时长
+    grasp_duration: 3.0      # 抓取移动时长
+    release_duration: 2.0    # 释放移动时长
+```
+
+## 坐标系说明
+
+### 相机坐标系
+
+```
+       Yc (下)
+        |
+        |
+        o-----> Zc (前，水平)
+       /
+      /
+    Xc (右)
+```
+
+### 坐标转换
+
+检测结果 `(x, y, z)` 表示：
+- `x`: 图像列方向（右为正）
+- `y`: 图像行方向（下为正）
+- `z`: 深度方向（前为正）
+
+节点会自动转换：
+```
+(xc, yc, zc) = (x, -y, z)
+```
+
+这是因为：
+- 图像 Y 向下 → 相机 Y 向下（负号修正方向）
+- 然后再转换到基坐标系
+
+## 调试
+
+### 查看节点状态
+
+```bash
+# 查看节点列表
+ros2 node list
+
+# 查看话题列表
+ros2 topic list | grep vision_grasp
+
+# 监听抓取目标
+ros2 topic echo /vision_grasp/grasp_target
+```
+
+### 测试流程
+
+1. **启动节点**
+   ```bash
+   # 终端 1
+   ros2 run udp_teleop arm_control --ros-args --params-file src/udp_teleop/config/arm_control.yaml
+   
+   # 终端 2
+   ros2 run udp_teleop vision_grasp --ros-args --params-file src/udp_teleop/config/vision_grasp.yaml
+   ```
+
+2. **发送测试抓取**
+   ```bash
+   # 终端 3
+   ros2 topic pub --once /vision_grasp/grasp_target geometry_msgs/Point \
+       "{x: 0.0, y: 0.0, z: 300.0}"
+   ```
+
+3. **观察日志**
+   - 终端 2 会显示详细的抓取流程日志
+   - 确认坐标转换和每一步动作
+
+## 常见问题
+
+### Q1: 坐标转换不正确
+
+**检查**：
+1. 相机内参是否准确标定
+2. 相机到 TCP 的变换参数是否正确
+3. 当前 TCP 位姿是否正确
+
+### Q2: 抓取位置偏移
+
+**可能原因**：
+1. 深度计算不准确
+2. 相机安装角度有偏差
+3. 坐标系定义理解错误
+
+**解决**：
+1. 调整 `cam_pitch` 参数（如果相机有俯仰角）
+2. 校准相机内参
+3. 使用已知位置物体验证
+
+### Q3: 夹爪动作失败
+
+**检查**：
+1. arm_control 节点是否正常运行
+2. UDP 连接是否正常
+3. 关节限位是否合理
+
+## 扩展功能
+
+### 添加安全检查
+
+```python
+def execute_grasp(self, x: float, y: float, z: float, phi: float):
+    # 检查目标是否在工作空间内
+    if not self.is_in_workspace(x, y, z):
+        self.get_logger().warn(f'目标超出工作空间: ({x}, {y}, {z})')
+        return
+    
+    # 执行抓取...
+```
+
+### 添加碰撞检测
+
+```python
+def is_path_safe(self, start, end):
+    # 检查路径是否安全
+    # TODO: 实现碰撞检测逻辑
+    return True
+```
+
+### 多物体抓取
+
+```python
+# 订阅物体列表
+self.objects_sub = self.create_subscription(
+    PointArray,  # 自定义消息类型
+    'vision_grasp/object_list',
+    self.handle_objects,
+    10
+)
+
+def handle_objects(self, msg):
+    for obj in msg.points:
+        self.execute_grasp(obj.x, obj.y, obj.z, self.current_phi)
+        # 等待完成...
+```
+
+## 相关文件
+
+- 节点实现：`udp_teleop/vision_grasp.py`
+- 配置文件：`udp_teleop/config/vision_grasp.yaml`
+- 坐标变换工具：`tools/camera_to_base.py`
+- 机械臂控制：`udp_teleop/arm_control.py`
+
+## 下一步
+
+1. 集成物体检测算法（YOLO、轮廓检测等）
+2. 添加深度估计（相似三角形、双目视觉等）
+3. 优化抓取策略（多物体排序、路径规划等）
+4. 添加可视化（RViz 显示检测结果和机械臂状态）