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/IMPLEMENTATION_SUMMARY.md
+++ b/docs/IMPLEMENTATION_SUMMARY.md
@@ -0,0 +1,252 @@
+# arm_control ROS 节点封装总结
+
+## ✅ 完成的工作
+
+### 1. 创建了消息和服务定义包 (`arm_control_msgs`)
+
+**消息类型**：
+- `TCPPose.msg` - TCP 位姿（x, y, z, phi）
+- `JointState.msg` - 关节状态（height, j2-j6）
+
+**服务类型**：
+- `MoveJoints.srv` - 关节空间运动控制
+- `MovePose.srv` - 笛卡尔空间运动控制（带逆运动学）
+- `GetPose.srv` - 查询当前位姿（正运动学）
+- `SetGripper.srv` - 夹爪控制
+
+### 2. 封装了控制节点 (`arm_control.py`)
+
+**核心功能**：
+- ✅ 关节空间插值运动
+- ✅ 笛卡尔空间逆运动学求解
+- ✅ 正运动学位姿计算
+- ✅ UDP 命令发送（与 ESP32 通信）
+- ✅ 状态缓存（平滑运动）
+- ✅ 参数化配置
+- ✅ 状态发布（10Hz）
+
+**服务接口**：
+- `/arm_control/move_joints` - 关节运动
+- `/arm_control/move_pose` - 位姿运动
+- `/arm_control/get_pose` - 查询位姿
+- `/arm_control/set_gripper` - 夹爪控制
+
+**话题发布**：
+- `/arm_control/joint_states` - 关节状态（10Hz）
+- `/arm_control/tcp_pose` - TCP 位姿（10Hz）
+
+### 3. 创建了示例客户端 (`arm_control_client.py`)
+
+**演示功能**：
+- 查询当前位姿
+- 完整抓取流程：
+  1. 移动到物体上方
+  2. 下降
+  3. 抓取
+  4. 提升
+  5. 移动到目标位置
+  6. 下降
+  7. 释放
+  8. 提升
+
+### 4. 配置和文档
+
+**配置文件**：
+- `config/arm_control.yaml` - 完整参数配置
+
+**文档**：
+- `ARM_CONTROL_README.md` - 完整使用文档
+- `QUICKSTART.md` - 快速开始指南
+
+**脚本**：
+- `build_arm_control.sh` - 一键编译脚本
+
+## 📁 文件清单
+
+```
+ros2/
+├── build_arm_control.sh                    # 编译脚本 ✨
+├── ARM_CONTROL_README.md                   # 完整文档 ✨
+├── QUICKSTART.md                           # 快速指南 ✨
+└── src/
+    ├── arm_control_msgs/                   # 消息包 ✨
+    │   ├── CMakeLists.txt
+    │   ├── package.xml
+    │   ├── msg/
+    │   │   ├── TCPPose.msg
+    │   │   └── JointState.msg
+    │   └── srv/
+    │       ├── MoveJoints.srv
+    │       ├── MovePose.srv
+    │       ├── GetPose.srv
+    │       └── SetGripper.srv
+    └── udp_teleop/
+        ├── setup.py                        # 已更新 ✨
+        ├── package.xml                     # 已更新 ✨
+        ├── udp_teleop/
+        │   ├── keyboard_control.py         # 原有
+        │   ├── arm_control.py              # 新增 ✨
+        │   └── arm_control_client.py       # 新增 ✨
+        └── config/
+            ├── params.yaml                 # 原有
+            └── arm_control.yaml            # 新增 ✨
+```
+
+## 🚀 快速使用
+
+### 编译
+
+```bash
+cd ros2
+./build_arm_control.sh
+```
+
+### 运行节点
+
+```bash
+ros2 run udp_teleop arm_control \
+    --ros-args --params-file src/udp_teleop/config/arm_control.yaml
+```
+
+### 测试服务
+
+```bash
+# 查询位姿
+ros2 service call /arm_control/get_pose arm_control_msgs/srv/GetPose
+
+# 移动
+ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \
+    "{x: 200.0, y: 100.0, z: -100.0, phi: 45.0, duration: 2.0}"
+```
+
+### 运行示例
+
+```bash
+ros2 run udp_teleop arm_control_client
+```
+
+## 🎯 与原始 udp_control.py 对比
+
+| 特性 | udp_control.py | arm_control 节点 |
+|------|---------------|-----------------|
+| **接口方式** | 命令行参数 | ROS 服务调用 |
+| **状态查询** | 读取 JSON 文件 | 服务调用 + 话题订阅 |
+| **多客户端** | ❌ 不支持 | ✅ 支持 |
+| **实时监控** | ❌ 无 | ✅ 10Hz 状态发布 |
+| **参数配置** | 命令行参数 | YAML 配置文件 |
+| **集成度** | 独立工具 | ROS 生态集成 |
+| **可编程性** | Shell 脚本 | Python/C++ 客户端 |
+
+## 💡 优势
+
+### 1. **标准化接口**
+- 使用 ROS 服务和话题，符合 ROS 生态标准
+- 易于与其他 ROS 节点集成（如视觉、规划器）
+
+### 2. **多客户端支持**
+- 多个客户端可同时连接
+- 适合复杂系统（如视觉 + 手动控制）
+
+### 3. **实时状态监控**
+- 10Hz 状态发布
+- 可用于可视化、日志记录、故障诊断
+
+### 4. **灵活配置**
+- YAML 参数文件
+- 运行时参数覆盖
+- 无需重新编译
+
+### 5. **易于扩展**
+- 添加新服务：只需定义 .srv 文件
+- 添加新话题：只需定义 .msg 文件
+- 集成其他功能：订阅/发布话题即可
+
+## 🔧 使用场景
+
+### 场景 1：视觉抓取
+
+```python
+# 视觉节点订阅相机话题，检测物体
+# 调用 arm_control 服务控制机械臂
+class VisionGraspNode(Node):
+    def __init__(self):
+        self.arm_cli = self.create_client(MovePose, 'arm_control/move_pose')
+        self.sub = self.create_subscription(Image, '/camera/image', self.on_image, 10)
+    
+    def on_image(self, msg):
+        # 检测物体
+        x, y, z = detect_object(msg)
+        
+        # 控制机械臂抓取
+        self.move_to(x, y, z, phi=45.0)
+```
+
+### 场景 2：示教编程
+
+```python
+# 记录示教点位
+class TeachPendant(Node):
+    def __init__(self):
+        self.get_cli = self.create_client(GetPose, 'arm_control/get_pose')
+        self.move_cli = self.create_client(MovePose, 'arm_control/move_pose')
+        self.waypoints = []
+    
+    def record_waypoint(self):
+        # 记录当前位置
+        pose = self.get_current_pose()
+        self.waypoints.append(pose)
+    
+    def replay(self):
+        # 重放示教轨迹
+        for pose in self.waypoints:
+            self.move_to(pose.x, pose.y, pose.z, pose.phi)
+```
+
+### 场景 3：轨迹规划
+
+```python
+# 使用规划器生成轨迹
+class TrajectoryPlanner(Node):
+    def __init__(self):
+        self.move_cli = self.create_client(MovePose, 'arm_control/move_pose')
+    
+    def execute_trajectory(self, waypoints):
+        # 执行轨迹点序列
+        for wp in waypoints:
+            self.move_to(wp.x, wp.y, wp.z, wp.phi, duration=0.5)
+```
+
+## 📚 下一步建议
+
+### 1. **视觉集成**
+创建视觉抓取节点，结合 `camera_to_base.py` 实现自动抓取
+
+### 2. **GUI 控制面板**
+使用 RQt 创建图形界面，实时显示状态和控制
+
+### 3. **轨迹记录与回放**
+实现示教编程功能
+
+### 4. **碰撞检测**
+添加工作空间限制和简单碰撞检测
+
+### 5. **MoveIt 集成**
+创建 URDF 和 MoveIt 配置，使用高级运动规划
+
+## 🎓 学习资源
+
+- ROS 2 服务教程：https://docs.ros.org/en/humble/Tutorials/Services.html
+- ROS 2 话题教程：https://docs.ros.org/en/humble/Tutorials/Topics.html
+- 自定义消息：https://docs.ros.org/en/humble/Tutorials/Custom-ROS2-Interfaces.html
+
+## ✨ 总结
+
+现在你有了一个完整的 ROS 节点化的机械臂控制系统：
+
+1. ✅ **功能完整** - 保留了 udp_control.py 的所有功能
+2. ✅ **接口标准** - 使用 ROS 服务和话题
+3. ✅ **易于集成** - 可与其他 ROS 节点无缝配合
+4. ✅ **文档齐全** - 提供了完整的文档和示例
+5. ✅ **开箱即用** - 一键编译，快速上手
+
+祝你使用愉快！🎉