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

docs/BUILD_SUCCESS.md

@@ -0,0 +1,122 @@
+# 编译成功！🎉
+
+## ✅ 已完成
+
+1. **消息包编译** - arm_control_msgs ✓
+2. **控制节点编译** - udp_teleop ✓
+
+## 🚀 快速测试
+
+### 1. 启动控制节点
+
+```bash
+# 激活环境
+conda activate ros2_humble
+
+# Source 工作空间
+cd ros2
+source install/setup.bash
+
+# 启动节点（修改 IP 为你的 ESP32 IP）
+ros2 run udp_teleop arm_control \
+    --ros-args --params-file src/udp_teleop/config/arm_control.yaml
+```
+
+### 2. 测试服务（新终端）
+
+```bash
+# 激活环境
+conda activate ros2_humble
+cd ros2
+source install/setup.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}"
+```
+
+### 3. 查看状态
+
+```bash
+# 查看关节状态
+ros2 topic echo /arm_control/joint_states
+
+# 查看 TCP 位姿
+ros2 topic echo /arm_control/tcp_pose
+
+# 查看所有服务
+ros2 service list | grep arm_control
+```
+
+## ⚠️ 重要提示
+
+### 编译说明
+
+由于 robostack 的 Python 配置问题，编译时需要显式指定 Python 路径：
+
+```bash
+# 已在 build_arm_control.sh 中自动处理
+export PYTHON_EXECUTABLE=$CONDA_PREFIX/bin/python
+export PYTHON_INCLUDE_DIR=$CONDA_PREFIX/include/python3.12
+export PYTHON_LIBRARY=$CONDA_PREFIX/lib/libpython3.12.so
+```
+
+### 修改配置
+
+编辑 `src/udp_teleop/config/arm_control.yaml` 修改参数：
+
+```yaml
+arm_control:
+  ros__parameters:
+    udp_ip: '192.168.4.1'  # 修改为你的 ESP32 IP
+    udp_port: 8888
+```
+
+修改后直接重启节点即可，无需重新编译。
+
+## 📝 下一步
+
+1. **修改 ESP32 IP**: 编辑 `config/arm_control.yaml`
+2. **测试连接**: 启动节点，查看是否有错误
+3. **调用服务**: 使用上面的命令测试
+4. **运行示例**: `ros2 run udp_teleop arm_control_client`
+
+## 🐛 故障排查
+
+### 问题：找不到服务
+
+**解决**：
+```bash
+# 检查节点是否运行
+ros2 node list
+
+# 重新 source 环境
+source install/setup.bash
+```
+
+### 问题：UDP 发送失败
+
+**解决**：
+1. 检查 ESP32 IP 是否正确
+2. 测试网络连接：`ping 192.168.4.1`
+3. 测试 UDP：`echo 'XYW:0:0:0:XZHY' | nc -u 192.168.4.1 8888`
+
+### 问题：重新编译
+
+**解决**：
+```bash
+# 清理后重新编译
+rm -rf build/ install/ log/
+./build_arm_control.sh
+```
+
+## 📚 文档
+
+- 完整文档：[ARM_CONTROL_README.md](ARM_CONTROL_README.md)
+- 快速指南：[QUICKSTART.md](QUICKSTART.md)
+- 实现总结：[IMPLEMENTATION_SUMMARY.md](IMPLEMENTATION_SUMMARY.md)
+
+祝使用愉快！🎉