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/QUICKSTART.md
+++ b/docs/QUICKSTART.md
@@ -0,0 +1,244 @@
+# 机械臂控制 ROS 节点 - 快速开始
+
+## 🚀 快速编译和运行
+
+### 1. 一键编译
+
+```bash
+cd ros2
+./build_arm_control.sh
+```
+
+### 2. 启动节点
+
+```bash
+# 方法 A: 使用配置文件（推荐）
+ros2 run udp_teleop arm_control \
+    --ros-args --params-file src/udp_teleop/config/arm_control.yaml
+
+# 方法 B: 使用默认参数
+ros2 run udp_teleop arm_control
+
+# 方法 C: 覆盖特定参数
+ros2 run udp_teleop arm_control \
+    --ros-args -p udp_ip:=192.168.233.67
+```
+
+### 3. 测试服务
+
+```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}"
+```
+
+### 4. 运行完整示例
+
+```bash
+# 在新终端运行示例客户端（包含完整抓取流程）
+ros2 run udp_teleop arm_control_client
+```
+
+## 📁 文件结构
+
+```
+ros2/
+├── build_arm_control.sh              # 一键编译脚本
+├── ARM_CONTROL_README.md             # 完整使用文档
+├── QUICKSTART.md                     # 本文件
+└── src/
+    ├── arm_control_msgs/             # 消息和服务定义
+    │   ├── msg/
+    │   │   ├── TCPPose.msg          # TCP 位姿消息
+    │   │   └── JointState.msg       # 关节状态消息
+    │   └── srv/
+    │       ├── MoveJoints.srv       # 关节运动服务
+    │       ├── MovePose.srv         # 位姿运动服务
+    │       ├── GetPose.srv          # 查询位姿服务
+    │       └── SetGripper.srv       # 夹爪控制服务
+    └── udp_teleop/
+        ├── udp_teleop/
+        │   ├── arm_control.py       # 控制节点实现
+        │   └── arm_control_client.py # 示例客户端
+        └── config/
+            └── arm_control.yaml     # 参数配置
+```
+
+## 🎯 常用命令
+
+### 服务调用
+
+```bash
+# 1. 关节空间运动
+ros2 service call /arm_control/move_joints arm_control_msgs/srv/MoveJoints \
+    "{height: -100, j2: 10, j3: 20, j4: 30, j5: 81, j6: 30, duration: 2.0}"
+
+# 2. 笛卡尔空间运动
+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}"
+
+# 3. 查询当前位姿
+ros2 service call /arm_control/get_pose arm_control_msgs/srv/GetPose
+
+# 4. 夹爪控制
+ros2 service call /arm_control/set_gripper arm_control_msgs/srv/SetGripper \
+    "{grip: true}"
+```
+
+### 话题订阅
+
+```bash
+# 查看关节状态（10Hz 发布）
+ros2 topic echo /arm_control/joint_states
+
+# 查看 TCP 位姿（10Hz 发布）
+ros2 topic echo /arm_control/tcp_pose
+```
+
+### 调试命令
+
+```bash
+# 查看所有服务
+ros2 service list | grep arm_control
+
+# 查看所有话题
+ros2 topic list | grep arm_control
+
+# 查看节点信息
+ros2 node info /arm_control
+
+# 查看服务接口定义
+ros2 interface show arm_control_msgs/srv/MovePose
+```
+
+## 📝 Python 客户端模板
+
+```python
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from arm_control_msgs.srv import MovePose, GetPose
+
+class MyController(Node):
+    def __init__(self):
+        super().__init__('my_controller')
+        
+        # 创建服务客户端
+        self.move_cli = self.create_client(MovePose, 'arm_control/move_pose')
+        self.get_cli = self.create_client(GetPose, 'arm_control/get_pose')
+        
+        # 等待服务
+        self.move_cli.wait_for_service()
+        self.get_cli.wait_for_service()
+    
+    def move_to(self, x, y, z, phi, duration=2.0):
+        """移动到指定位置"""
+        req = MovePose.Request()
+        req.x = x
+        req.y = y
+        req.z = z
+        req.phi = phi
+        req.duration = duration
+        
+        future = self.move_cli.call_async(req)
+        rclpy.spin_until_future_complete(self, future)
+        return future.result().success
+    
+    def get_pose(self):
+        """查询当前位姿"""
+        req = GetPose.Request()
+        future = self.get_cli.call_async(req)
+        rclpy.spin_until_future_complete(self, future)
+        return future.result()
+
+def main():
+    rclpy.init()
+    controller = MyController()
+    
+    # 查询位姿
+    pose = controller.get_pose()
+    print(f"当前位置: ({pose.x}, {pose.y}, {pose.z})")
+    
+    # 移动
+    controller.move_to(200.0, 100.0, -100.0, 45.0)
+    
+    controller.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
+```
+
+## 🔧 配置修改
+
+编辑 `src/udp_teleop/config/arm_control.yaml`：
+
+```yaml
+arm_control:
+  ros__parameters:
+    # 修改 ESP32 IP
+    udp_ip: '192.168.4.1'
+    
+    # 修改运动速度
+    default_duration: 1.0  # 更快：0.5，更慢：2.0
+    
+    # 修改关节限位
+    height_min: -290
+    height_max: 0
+```
+
+修改后重新运行节点即可（无需重新编译）。
+
+## ⚠️ 常见问题
+
+### 编译失败
+
+```bash
+# 确保环境激活
+conda activate ros2_humble
+source install/setup.bash
+
+# 清理后重新编译
+rm -rf build/ install/ log/
+./build_arm_control.sh
+```
+
+### 服务不可用
+
+```bash
+# 检查节点是否运行
+ros2 node list
+
+# 检查服务是否存在
+ros2 service list | grep arm_control
+
+# 查看节点日志
+ros2 run udp_teleop arm_control --ros-args --log-level debug
+```
+
+### UDP 连接失败
+
+```bash
+# 测试 UDP 连接
+echo 'XYW:0:0:0:XZHY' | nc -u 192.168.4.1 8888
+
+# 修改 IP 配置
+ros2 run udp_teleop arm_control \
+    --ros-args -p udp_ip:=<你的IP> -p udp_port:=8888
+```
+
+## 📚 更多文档
+
+- 完整使用文档：[ARM_CONTROL_README.md](ARM_CONTROL_README.md)
+- 原始工具文档：[../tools/README.md](../tools/README.md)
+- ROS 2 包文档：[src/udp_teleop/README.md](src/udp_teleop/README.md)
+
+## 🎓 下一步
+
+1. **集成视觉**：创建视觉抓取节点，订阅相机话题，调用 arm_control 服务
+2. **添加规划**：使用 MoveIt 或自定义轨迹规划器
+3. **多机械臂**：启动多个 arm_control 节点控制多个机械臂
+4. **远程控制**：通过 ROS 2 的 DDS 实现跨机器控制