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_SUMMARY.md
+++ b/docs/VISION_GRASP_SUMMARY.md
@@ -0,0 +1,286 @@
+# 视觉抓取节点 - 完成总结
+
+## ✅ 完成的工作
+
+### 1. 创建了视觉抓取 ROS 节点 (`vision_grasp.py`)
+
+**功能**：
+- ✅ 抓取功能：输入相机坐标 → 自动转换 → 执行抓取流程
+- ✅ 释放功能：输入基坐标 → 移动 → 释放物体
+- ✅ 坐标变换：集成 `camera_to_base.py` 的完整变换逻辑
+- ✅ 自动化流程：释放夹爪 → 移动 → 抓取 → 回收
+
+### 2. 抓取流程
+
+```
+输入相机坐标 (x, y, z)
+    ↓
+转换: (xc, yc, zc) = (x, -y, z)
+    ↓
+变换到基坐标系
+    ↓
+1. Release 夹爪 (duration=0)
+    ↓
+2. 移动到目标 (duration=3s)
+    ↓
+3. Grip 夹爪 (duration=1s)
+    ↓
+4. 回收到 (200, 0, 当前z)
+```
+
+### 3. 释放流程
+
+```
+输入基坐标 (x, y, z)
+    ↓
+1. 移动到释放位置
+    ↓
+2. Release 夹爪 (duration=0)
+    ↓
+3. 回收到 (200, 0, 当前z)
+```
+
+## 📁 创建的文件
+
+```
+ros2/
+├── src/udp_teleop/
+│   ├── udp_teleop/
+│   │   └── vision_grasp.py          ✨ 视觉抓取节点
+│   └── config/
+│       └── vision_grasp.yaml        ✨ 参数配置
+├── test_vision_grasp.py             ✨ 测试脚本
+└── VISION_GRASP_README.md           ✨ 完整文档
+```
+
+## 🚀 快速使用
+
+### 启动节点
+
+**终端 1**：arm_control 节点
+```bash
+cd ros2
+source install/setup.bash
+
+ros2 run udp_teleop arm_control \
+    --ros-args --params-file src/udp_teleop/config/arm_control.yaml
+```
+
+**终端 2**：vision_grasp 节点
+```bash
+cd ros2
+source install/setup.bash
+
+ros2 run udp_teleop vision_grasp \
+    --ros-args --params-file src/udp_teleop/config/vision_grasp.yaml
+```
+
+**终端 3**：测试
+```bash
+cd ros2
+source install/setup.bash
+
+# 测试抓取（相机正前方 300mm）
+python test_vision_grasp.py grasp 0 0 300
+
+# 测试抓取（相机右侧 50mm，前方 300mm）
+python test_vision_grasp.py grasp 50 0 300
+
+# 测试释放（基坐标）
+python test_vision_grasp.py release 100 150 -100
+```
+
+### 或使用话题发布
+
+```bash
+# 抓取
+ros2 topic pub --once /vision_grasp/grasp_target geometry_msgs/Point \
+    "{x: 0.0, y: 0.0, z: 300.0}"
+
+# 释放
+ros2 topic pub --once /vision_grasp/release_target geometry_msgs/Point \
+    "{x: 100.0, y: 150.0, z: -100.0}"
+```
+
+## 🎯 关键特性
+
+### 1. 自动坐标转换
+
+- **输入**：相机坐标系 `(x, y, z)`
+- **自动转换**：`(xc, yc, zc) = (x, -y, z)`（图像坐标修正）
+- **变换到基坐标系**：使用当前 TCP 位姿进行完整变换
+
+### 2. 参数化配置
+
+```yaml
+vision_grasp:
+  ros__parameters:
+    # 相机到 TCP 的变换
+    cam_tx: 0.0
+    cam_ty: 0.0
+    cam_tz: 0.0
+    
+    # 回收位置
+    retract_position_x: 200.0
+    retract_position_y: 0.0
+    
+    # 运动时长
+    grasp_duration: 3.0
+    release_duration: 2.0
+```
+
+### 3. 完整日志
+
+节点会输出详细的流程日志：
+```
+============================================================
+开始抓取流程
+============================================================
+1. 释放夹爪
+2. 移动到目标位置: (323.5, 229.6, -108.6)
+3. 抓取物体
+4. 移动到回收位置: (200.0, 0.0, -108.6)
+============================================================
+✓ 抓取完成！
+============================================================
+```
+
+## 🔗 集成示例
+
+### Python 脚本集成
+
+```python
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Point
+
+class MyDetector(Node):
+    def __init__(self):
+        super().__init__('my_detector')
+        self.grasp_pub = self.create_publisher(
+            Point, 'vision_grasp/grasp_target', 10)
+    
+    def on_detection(self, camera_x, camera_y, camera_z):
+        """检测到物体后触发抓取"""
+        msg = Point()
+        msg.x = camera_x
+        msg.y = camera_y
+        msg.z = camera_z
+        self.grasp_pub.publish(msg)
+
+def main():
+    rclpy.init()
+    node = MyDetector()
+    
+    # 模拟检测结果
+    node.on_detection(10.0, 5.0, 250.0)
+    
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+```
+
+## 📊 话题接口
+
+| 话题 | 类型 | 说明 |
+|------|------|------|
+| `/vision_grasp/grasp_target` | geometry_msgs/Point | 抓取目标（相机坐标） |
+| `/vision_grasp/release_target` | geometry_msgs/Point | 释放目标（基坐标） |
+
+## 🎓 下一步
+
+### 1. 集成物体检测
+
+```python
+# 订阅相机图像
+self.image_sub = self.create_subscription(
+    Image, '/camera/image_raw', self.on_image, 10)
+
+def on_image(self, msg):
+    # 检测物体
+    camera_x, camera_y, camera_z = detect_object(msg)
+    
+    # 触发抓取
+    self.publish_grasp_target(camera_x, camera_y, camera_z)
+```
+
+### 2. 添加深度估计
+
+使用 `tools/vision_transform.py` 中的相似三角形方法：
+
+```python
+from tools.vision_transform import compute_depth_from_size
+
+# 从检测获得像素宽度
+pixel_width = 100  # px
+real_width = 50    # mm
+focal_length = 500 # px
+
+depth = compute_depth_from_size(pixel_width, real_width, focal_length)
+```
+
+### 3. 多物体抓取
+
+```python
+# 创建队列
+self.grasp_queue = []
+
+def on_multiple_detections(self, detections):
+    for det in detections:
+        self.grasp_queue.append(det)
+    
+    # 逐个抓取
+    while self.grasp_queue:
+        target = self.grasp_queue.pop(0)
+        self.publish_grasp_target(target.x, target.y, target.z)
+        # 等待完成...
+```
+
+## 🐛 故障排查
+
+### Q1: 坐标转换不正确
+
+**检查**：
+1. TCP 位姿是否正确（`ros2 service call /arm_control/get_pose`）
+2. 相机到 TCP 的变换参数（`cam_tx/ty/tz`, `cam_roll/pitch/yaw`）
+3. 坐标系方向理解是否正确
+
+### Q2: 抓取位置偏移
+
+**解决**：
+1. 校准相机内参
+2. 验证深度计算准确性
+3. 调整 `cam_pitch`（如果相机有俯仰角）
+
+### Q3: 服务调用超时
+
+**检查**：
+1. arm_control 节点是否运行
+2. UDP 连接是否正常
+3. 机械臂是否在合理位置
+
+## 📚 相关文档
+
+- **完整文档**：`VISION_GRASP_README.md`
+- **坐标变换**：`tools/camera_to_base.py`
+- **机械臂控制**：`ARM_CONTROL_README.md`
+- **视觉变换**：`docs/vision_calibration_horizontal.md`
+
+## 🎉 总结
+
+现在你有了一个完整的视觉抓取系统：
+
+1. ✅ **独立的机械臂控制节点** - `arm_control`
+2. ✅ **自动化抓取节点** - `vision_grasp`  
+3. ✅ **完整的坐标变换** - 相机 → 基坐标系
+4. ✅ **参数化配置** - 灵活调整参数
+5. ✅ **测试工具** - 快速验证功能
+6. ✅ **完整文档** - 使用指南和示例
+
+只需要：
+1. 添加物体检测算法
+2. 连接相机获取图像
+3. 发布检测结果到 `/vision_grasp/grasp_target`
+
+系统就会自动完成抓取！