From f00537ebb9988c010dc008b207d25e241c05f6d8 Mon Sep 17 00:00:00 2001
From: FallenSigh <fallensigh@gmail.com>
Date: Tue, 16 Jun 2026 19:01:42 +0800
Subject: [PATCH] =?UTF-8?q?docs:=20=E6=95=B4=E7=90=86=E5=92=8C=E7=B2=BE?=
 =?UTF-8?q?=E7=AE=80=E6=96=87=E6=A1=A3=E7=BB=93=E6=9E=84?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

- 删除冗余文档（6个重复的说明文档）
- 更新 README.md：简洁清晰的项目总览
- 更新 ros2/README.md：完整的 ROS 2 使用指南
- 保留核心技术文档：docs/arm.md（运动学推导）

文档现在更加简洁，避免重复内容
---
 README.md                      | 208 ++++++++++++-------
 docs/ARM_CONTROL_README.md     | 319 -----------------------------
 docs/BUILD_SUCCESS.md          | 122 -----------
 docs/IMPLEMENTATION_SUMMARY.md | 252 -----------------------
 docs/QUICKSTART.md             | 244 ----------------------
 docs/VISION_GRASP_README.md    | 359 ---------------------------------
 docs/VISION_GRASP_SUMMARY.md   | 286 --------------------------
 ros2/README.md                 | 308 +++++++++++++++++++++-------
 8 files changed, 374 insertions(+), 1724 deletions(-)
 delete mode 100644 docs/ARM_CONTROL_README.md
 delete mode 100644 docs/BUILD_SUCCESS.md
 delete mode 100644 docs/IMPLEMENTATION_SUMMARY.md
 delete mode 100644 docs/QUICKSTART.md
 delete mode 100644 docs/VISION_GRASP_README.md
 delete mode 100644 docs/VISION_GRASP_SUMMARY.md

diff --git a/README.md b/README.md
index f66f184..db04ba5 100644
--- a/README.md
+++ b/README.md
@@ -2,9 +2,9 @@
 
 > 本仓库为 **[中国机器人及人工智能大赛](https://www.caairobot.com)**（CRAIC）**机器人任务挑战赛（小型桌面级）** 参赛代码。
 
-ESP32-S3 双核摄像头 + ROS 2 键盘遥控 + UDP 实时通讯的一体化机器人控制系统。
+ESP32-S3 双核摄像头 + ROS 2 机械臂控制 + 视觉抓取的一体化机器人系统。
 
-## 赛事
+## 赛事信息
 
 **中国机器人及人工智能大赛**（**C**hina **R**obot and **A**rtificial **I**ntelligence **C**ompetition，简称 **CRAIC**）是由 [中国人工智能学会](https://www.caai.cn)（CAAI）主办的全国性学科竞赛，始于 1999 年，已列入**全国普通高等学校学科竞赛排行榜**及**教育部 A 类竞赛名单**。
 
@@ -23,68 +23,59 @@ ESP32-S3 双核摄像头 + ROS 2 键盘遥控 + UDP 实时通讯的一体化机
 
 ```
 craic/
-├── jxbeye/                    # ESP32-S3 摄像头固件 (PlatformIO)
-│   ├── src/main.cpp           # 双核并行推流 + AsyncUDP + ESP-NOW
-│   ├── platformio.ini         # 构建配置 (ESP32-S3-WROOM-1-N16R8)
-│   ├── boards/                # 自定义板级定义
-│   ├── lib/FTServo/           # 飞特舵机控制库
-│   └── include/ / test/
-├── ros2/                      # ROS 2 Humble 键盘遥控
-│   └── src/udp_teleop/        # 底盘 + 6轴机械臂 UDP 遥控节点
-├── tools/                     # 调试工具
-│   └── udp_server.py          # UDP 回显测试服务器
-└── .gitignore
+├── jxbeye/                    # ESP32-S3 固件 (PlatformIO)
+│   ├── src/main.cpp           # 双核摄像头推流 + UDP 控制
+│   └── platformio.ini         # ESP32-S3-WROOM-1-N16R8 配置
+├── ros2/                      # ROS 2 控制系统
+│   ├── src/arm_control_msgs/  # 消息和服务定义
+│   └── src/udp_teleop/        # 机械臂控制和视觉抓取节点
+├── tools/                     # 独立工具脚本
+│   ├── udp_control.py         # 命令行机械臂控制（带逆运动学）
+│   ├── camera_to_base.py      # 相机坐标到基坐标变换
+│   └── camera_capture.py      # MJPEG 流采集
+└── docs/                      # 技术文档
+    └── arm.md                 # 机械臂运动学推导
 ```
 
 ## 硬件
 
 | 组件 | 型号 |
 |------|------|
-| 主控 | ESP32-S3-WROOM-1-N16R8 (16MB Flash, 8MB PSRAM Octal) |
+| 主控 | ESP32-S3-WROOM-1-N16R8 (16MB Flash, 8MB PSRAM) |
 | 摄像头 | OV2640 (XGA 1024×768, JPEG) |
-| 舵机 | 飞特 SCS/STS 系列串行舵机 |
+| 机械臂 | 6-DOF，飞特 SCS/STS 串行舵机 |
 
-## 功能
+## 核心功能
 
 ### 1. ESP32-S3 固件 (`jxbeye/`)
 
-- **双核并行架构**：Core 0 负责摄像头采集，Core 1 负责 WiFi 推流
-- **MJPEG 实时推流**：Web 浏览器直接访问 `http://<IP>` 查看实时画面
-- **AsyncUDP 非阻塞通讯**：UDP 指令由中断回调处理，不干扰推流
-- **ESP-NOW**：低延迟点对点通信
-- **在线 WiFi 配置**：通过 Web 页面或串口修改 SSID/密码，自动保存到 NVS
+- **双核架构**：Core 0 采集，Core 1 推流
+- **MJPEG 推流**：`http://<IP>` 实时查看
+- **UDP 控制**：端口 8888，非阻塞异步处理
+- **WiFi 配置**：串口发送 `WIFI:SSID:PASSWORD` 配置
 
-### 2. ROS 2 键盘遥控 (`ros2/src/udp_teleop/`)
+### 2. ROS 2 机械臂控制 (`ros2/`)
 
-- **底盘遥控**：W/S 前进后退、A/D 左右平移、Q/E 旋转（差速驱动）
-- **机械臂遥控**：6 轴关节角度独立控制，高度升降
-- **多平台键盘后端**：
-  - `stdin` — Linux/macOS 终端原生输入，零依赖
-  - `pynput` — 跨平台按键监听
-  - `win_poll` — Windows Win32 API 轮询
-- **可配置参数**：速度、步长、目标 IP/端口均通过 YAML 配置
+**arm_control 节点**：
+- 关节空间和笛卡尔空间运动控制
+- 完整逆运动学和正运动学
+- 服务接口：`move_joints`, `move_pose`, `get_pose`, `set_gripper`
+- 状态发布（10Hz）
 
-### 3. UDP 通讯协议
+**vision_grasp 节点**：
+- 相机坐标到基坐标系自动转换
+- 自动抓取：释放 → 移动 → 抓取 → 回收
+- 自动释放：移动 → 释放 → 回收
 
-所有指令通过 UDP 端口 `8888` 发送，格式：
+### 3. 工具脚本 (`tools/`)
 
-```
-# 底盘控制
-XYW:<X速度>:<Y速度>:<W角速度>:XZHY\n
-
-# 机械臂控制
-JXB:<高度>:<J2>:<J3>:<J4>:<J5>:<J6>:0:0:EZHY\n
-
-# 激光控制
-LASERON / LASEROFF
-
-# 串口透传
-<any payload ending with ZHY or \n>
-```
+- `udp_control.py` - 命令行机械臂控制（支持逆运动学）
+- `camera_to_base.py` - 坐标变换工具
+- `camera_capture.py` - 相机帧采集（支持自动扫描）
 
 ## 快速开始
 
-### 编译 & 烧录 ESP32 固件
+### ESP32 固件
 
 ```bash
 cd jxbeye
@@ -92,55 +83,134 @@ pio run -t upload
 pio device monitor
 ```
 
-首次启动后 ESP32 创建热点 `ESP32-S3-Camera`（密码 `12345678`），浏览器访问 `http://192.168.4.1`。
+首次启动创建热点 `ESP32-S3-Camera`（密码 `12345678`），访问 `http://192.168.4.1`。
 
-### 运行 ROS 2 键盘遥控
+### ROS 2 控制系统
 
 ```bash
-# 创建环境（首次）
+# 环境准备（首次）
 conda create -n ros2_humble -c robostack-staging -c conda-forge ros-humble-desktop
-conda install -n ros2_humble -c robostack-staging -c conda-forge colcon-common-extensions
-conda run -n ros2_humble pip install pynput
-
-# 构建 & 运行
 conda activate ros2_humble
+
+# 编译
 cd ros2
-colcon build --symlink-install --packages-select udp_teleop
+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
+
+colcon build --packages-select arm_control_msgs \
+  --cmake-args \
+    -DPython_EXECUTABLE=$PYTHON_EXECUTABLE \
+    -DPython_INCLUDE_DIR=$PYTHON_INCLUDE_DIR \
+    -DPython_LIBRARY=$PYTHON_LIBRARY
+
+colcon build --packages-select udp_teleop
 source install/setup.bash
 
-# 启动键盘遥控（默认目标 192.168.233.67:8888）
-ros2 run udp_teleop keyboard_control \
-    --ros-args --params-file src/udp_teleop/config/params.yaml
+# 运行机械臂控制
+ros2 run udp_teleop arm_control \
+    --ros-args --params-file src/udp_teleop/config/arm_control.yaml
+
+# 运行视觉抓取（新终端）
+ros2 run udp_teleop vision_grasp \
+    --ros-args --params-file src/udp_teleop/config/vision_grasp.yaml
 ```
 
-### 测试 UDP 通讯
+### 命令行工具
 
 ```bash
-# 启动回显服务器
-python tools/udp_server.py
+# 机械臂控制（关节空间）
+python tools/udp_control.py joints \
+    --height -100 --j2 10 --j3 20 --j4 30 --duration 2.0
 
-# 发送测试指令
-echo 'XYW:100:0:0:XZHY' | nc -u 127.0.0.1 8888
+# 机械臂控制（笛卡尔空间）
+python tools/udp_control.py pose \
+    --x 200 --y 100 --z -100 --phi 45 --duration 2.0
+
+# 相机采集
+python tools/camera_capture.py --ip 192.168.4.1
+python tools/camera_capture.py --scan  # 自动扫描
 ```
 
-### 串口配置 WiFi
+## UDP 协议
 
-在 ESP32 串口监视器中发送：
+所有指令通过 UDP 端口 `8888` 发送：
 
-```
-WIFI:热点名称:密码
+```bash
+# 底盘控制（麦克纳姆轮）
+XYW:<X>:<Y>:<W>:XZHY\n
+
+# 机械臂控制（6 轴）
+JXB:<height>:<J2>:<J3>:<J4>:<J5>:<J6>:0:0:EZHY\n
+
+# 激光控制
+LASERON / LASEROFF
 ```
 
-设备自动保存配置并重启。
+## 使用示例
+
+### ROS 服务调用
+
+```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}"
+
+# 视觉抓取（发布相机坐标）
+ros2 topic pub --once /vision_grasp/grasp_target geometry_msgs/Point \
+    "{x: 10.0, y: 5.0, z: 250.0}"
+```
+
+### Python 集成
+
+```python
+import rclpy
+from geometry_msgs.msg import Point
+
+class VisionDetector:
+    def __init__(self):
+        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)
+```
+
+## 文档
+
+- **ROS 2 系统**：[ros2/README.md](ros2/README.md) - 完整的 ROS 节点文档
+- **运动学推导**：[docs/arm.md](docs/arm.md) - 机械臂逆运动学数学推导
+- **视觉标定**：[docs/vision_calibration_horizontal.md](docs/vision_calibration_horizontal.md) - 相机标定指南
+
+## 坐标系说明
+
+**机械臂基坐标系**（Z 轴朝上）：
+- 原点：J1 线性滑轨底部
+- X 轴：基座正前方
+- Y 轴：基座左侧
+- Z 轴：竖直向上（高度）
+
+**相机坐标系**（水平安装）：
+- X 轴：右侧
+- Y 轴：向下
+- Z 轴：正前方（光轴）
+
+变换由 `camera_to_base.py` 和 `vision_grasp` 节点自动处理。
 
 ## 依赖
 
 | 环境 | 依赖 |
 |------|------|
 | ESP32 | PlatformIO, Arduino framework, esp32-camera |
-| ROS 2 | ROS 2 Humble (Conda robostack), pynput |
-| 测试 | Python 3, Unix netcat |
+| ROS 2 | ROS 2 Humble (robostack), Python 3.12 |
+| 工具 | NumPy, OpenCV (可选) |
 
 ## 许可
 
-待定。
+MIT License
diff --git a/docs/ARM_CONTROL_README.md b/docs/ARM_CONTROL_README.md
deleted file mode 100644
index 38a1906..0000000
--- a/docs/ARM_CONTROL_README.md
+++ /dev/null
@@ -1,319 +0,0 @@
-# arm_control ROS 节点使用指南
-
-## 概述
-
-`arm_control` 是一个封装了机械臂控制功能的 ROS 2 节点，基于 `udp_control.py` 改造，提供服务接口进行机械臂控制。
-
-## 功能特性
-
-- ✅ 关节空间运动控制（带插值）
-- ✅ 笛卡尔空间运动控制（带逆运动学）
-- ✅ 正运动学查询
-- ✅ 夹爪控制
-- ✅ 状态发布（关节状态 + TCP 位姿）
-- ✅ 状态缓存（平滑运动）
-
-## 编译
-
-```bash
-cd ros2
-
-# 1. 编译消息包
-colcon build --packages-select arm_control_msgs
-
-# 2. Source 消息包
-source install/setup.bash
-
-# 3. 编译控制节点
-colcon build --packages-select udp_teleop
-
-# 4. Source 控制节点
-source install/setup.bash
-```
-
-## 运行
-
-### 启动控制节点
-
-```bash
-# 使用默认参数
-ros2 run udp_teleop arm_control
-
-# 使用配置文件
-ros2 run udp_teleop arm_control \
-    --ros-args --params-file src/udp_teleop/config/arm_control.yaml
-
-# 覆盖特定参数
-ros2 run udp_teleop arm_control \
-    --ros-args -p udp_ip:=192.168.233.67 -p udp_port:=8888
-```
-
-## 服务接口
-
-### 1. 关节空间运动
-
-```bash
-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. 笛卡尔空间运动
-
-```bash
-# 基本运动
-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}"
-
-# 带夹爪控制
-ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \
-    "{x: 200.0, y: 100.0, z: -100.0, phi: 45.0, grip: true, duration: 2.0}"
-```
-
-### 3. 查询当前位姿
-
-```bash
-ros2 service call /arm_control/get_pose arm_control_msgs/srv/GetPose
-```
-
-输出示例：
-```
-success: true
-message: ''
-x: 150.234
-y: 75.123
-z: -100.0
-phi: 45.678
-height: -100
-j2: 13
-j3: 27
-j4: 55
-j5: 81
-j6: 30
-```
-
-### 4. 夹爪控制
-
-```bash
-# 抓取
-ros2 service call /arm_control/set_gripper arm_control_msgs/srv/SetGripper \
-    "{grip: true}"
-
-# 释放
-ros2 service call /arm_control/set_gripper arm_control_msgs/srv/SetGripper \
-    "{release: true}"
-```
-
-## 话题订阅
-
-### 1. 关节状态
-
-```bash
-ros2 topic echo /arm_control/joint_states
-```
-
-输出：
-```yaml
-header:
-  stamp:
-    sec: 1234567890
-    nanosec: 123456789
-  frame_id: ''
-height: -100
-j2: 13
-j3: 27
-j4: 55
-j5: 81
-j6: 30
-```
-
-### 2. TCP 位姿
-
-```bash
-ros2 topic echo /arm_control/tcp_pose
-```
-
-输出：
-```yaml
-header:
-  stamp:
-    sec: 1234567890
-    nanosec: 123456789
-  frame_id: ''
-x: 150.234
-y: 75.123
-z: -100.0
-phi: 45.678
-```
-
-## Python 客户端示例
-
-```python
-#!/usr/bin/env python3
-import rclpy
-from rclpy.node import Node
-from arm_control_msgs.srv import MovePose
-
-class MyArmController(Node):
-    def __init__(self):
-        super().__init__('my_controller')
-        self.cli = self.create_client(MovePose, 'arm_control/move_pose')
-        self.cli.wait_for_service()
-
-    def move_to(self, x, y, z, phi):
-        req = MovePose.Request()
-        req.x = x
-        req.y = y
-        req.z = z
-        req.phi = phi
-        req.duration = 2.0
-        
-        future = self.cli.call_async(req)
-        rclpy.spin_until_future_complete(self, future)
-        return future.result().success
-
-def main():
-    rclpy.init()
-    controller = MyArmController()
-    
-    # 移动到目标位置
-    controller.move_to(200.0, 100.0, -100.0, 45.0)
-    
-    controller.destroy_node()
-    rclpy.shutdown()
-
-if __name__ == '__main__':
-    main()
-```
-
-## 完整抓取流程示例
-
-```bash
-# 运行示例客户端（包含完整抓取流程）
-ros2 run udp_teleop arm_control_client
-```
-
-或手动调用：
-
-```bash
-# 1. 查询当前位姿
-ros2 service call /arm_control/get_pose arm_control_msgs/srv/GetPose
-
-# 2. 移动到物体上方
-ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \
-    "{x: 200.0, y: 100.0, z: -50.0, phi: 45.0, release: true, duration: 2.0}"
-
-# 3. 下降到抓取位置
-ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \
-    "{x: 200.0, y: 100.0, z: -150.0, phi: 45.0, release: true, duration: 1.0}"
-
-# 4. 抓取
-ros2 service call /arm_control/set_gripper arm_control_msgs/srv/SetGripper \
-    "{grip: true}"
-
-# 5. 提升
-ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \
-    "{x: 200.0, y: 100.0, z: -50.0, phi: 45.0, grip: true, duration: 1.0}"
-```
-
-## 参数配置
-
-编辑 `config/arm_control.yaml`：
-
-```yaml
-arm_control:
-  ros__parameters:
-    # UDP 配置
-    udp_ip: '192.168.4.1'
-    udp_port: 8888
-    
-    # 机械臂几何参数
-    l1: 125.0
-    l2: 125.0
-    x4: 110.0
-    z4: 80.0
-    
-    # 关节限位
-    height_min: -290
-    height_max: 0
-    j2_min: -110
-    j2_max: 115
-    # ... (更多参数见配置文件)
-```
-
-## 调试
-
-### 查看服务列表
-
-```bash
-ros2 service list | grep arm_control
-```
-
-### 查看话题列表
-
-```bash
-ros2 topic list | grep arm_control
-```
-
-### 查看服务接口定义
-
-```bash
-ros2 interface show arm_control_msgs/srv/MovePose
-```
-
-### 实时监控状态
-
-```bash
-# 终端 1: 查看关节状态
-ros2 topic echo /arm_control/joint_states
-
-# 终端 2: 查看 TCP 位姿
-ros2 topic echo /arm_control/tcp_pose
-
-# 终端 3: 发送控制命令
-ros2 service call /arm_control/move_pose ...
-```
-
-## 常见问题
-
-### Q1: 服务调用失败
-
-**检查**：
-1. 节点是否正在运行？`ros2 node list`
-2. UDP 连接是否正常？检查 `udp_ip` 参数
-3. 关节限位是否合理？查看错误消息
-
-### Q2: 运动不平滑
-
-**调整参数**：
-- 增加 `duration`（运动时长）
-- 增加 `default_rate`（插值频率）
-
-### Q3: 状态不更新
-
-**检查**：
-- `use_state_cache` 是否启用？
-- `tools/.udp_control_state.json` 是否可写？
-
-## 与原始 udp_control.py 对比
-
-| 功能 | udp_control.py | arm_control 节点 |
-|------|---------------|-----------------|
-| 接口 | 命令行 | ROS 服务 + 话题 |
-| 集成 | 独立脚本 | ROS 生态系统 |
-| 状态查询 | 文件缓存 | 服务调用 |
-| 多客户端 | 不支持 | 支持 |
-| 实时监控 | 不支持 | 话题订阅 |
-
-## 下一步
-
-- 集成视觉系统：创建视觉抓取节点，订阅相机话题，调用 arm_control 服务
-- 添加轨迹规划：创建轨迹规划器，生成平滑路径
-- 碰撞检测：添加工作空间限制和碰撞检测
-
-## 相关文件
-
-- 节点实现：`udp_teleop/arm_control.py`
-- 消息定义：`arm_control_msgs/msg/`
-- 服务定义：`arm_control_msgs/srv/`
-- 配置文件：`udp_teleop/config/arm_control.yaml`
-- 示例客户端：`udp_teleop/arm_control_client.py`
diff --git a/docs/BUILD_SUCCESS.md b/docs/BUILD_SUCCESS.md
deleted file mode 100644
index ecacc20..0000000
--- a/docs/BUILD_SUCCESS.md
+++ /dev/null
@@ -1,122 +0,0 @@
-# 编译成功！🎉
-
-## ✅ 已完成
-
-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)
-
-祝使用愉快！🎉
diff --git a/docs/IMPLEMENTATION_SUMMARY.md b/docs/IMPLEMENTATION_SUMMARY.md
deleted file mode 100644
index f43ae00..0000000
--- a/docs/IMPLEMENTATION_SUMMARY.md
+++ /dev/null
@@ -1,252 +0,0 @@
-# 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. ✅ **开箱即用** - 一键编译，快速上手
-
-祝你使用愉快！🎉
diff --git a/docs/QUICKSTART.md b/docs/QUICKSTART.md
deleted file mode 100644
index 96afcfc..0000000
--- a/docs/QUICKSTART.md
+++ /dev/null
@@ -1,244 +0,0 @@
-# 机械臂控制 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 实现跨机器控制
diff --git a/docs/VISION_GRASP_README.md b/docs/VISION_GRASP_README.md
deleted file mode 100644
index 043073f..0000000
--- a/docs/VISION_GRASP_README.md
+++ /dev/null
@@ -1,359 +0,0 @@
-# 视觉抓取节点使用指南
-
-## 概述
-
-`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 显示检测结果和机械臂状态）
diff --git a/docs/VISION_GRASP_SUMMARY.md b/docs/VISION_GRASP_SUMMARY.md
deleted file mode 100644
index 89f1426..0000000
--- a/docs/VISION_GRASP_SUMMARY.md
+++ /dev/null
@@ -1,286 +0,0 @@
-# 视觉抓取节点 - 完成总结
-
-## ✅ 完成的工作
-
-### 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`
-
-系统就会自动完成抓取！
diff --git a/ros2/README.md b/ros2/README.md
index 8e01f13..bb9798d 100644
--- a/ros2/README.md
+++ b/ros2/README.md
@@ -1,98 +1,260 @@
-# ros2 — ROS 2 工作空间
+# ROS 2 机械臂控制系统
 
-## 环境搭建
+CRAIC 项目的 ROS 2 机械臂控制和视觉抓取系统。
 
-支持以下三种安装方式，任选其一。
+## 📦 包含组件
 
-### 方式一：Conda (robostack，跨平台)
+### 1. arm_control_msgs
+消息和服务定义包。
 
-适用于 Linux / macOS / Windows，无需 root 权限。
+**消息类型**：
+- `JointState` - 关节状态
+- `TCPPose` - TCP 位姿
 
-```bash
-# 安装 Miniconda
-wget https://repo.anaconda.com/miniconda/Miniconda3-latest-Linux-x86_64.sh
-bash Miniconda3-latest-Linux-x86_64.sh
+**服务类型**：
+- `MoveJoints` - 关节空间运动
+- `MovePose` - 笛卡尔空间运动
+- `GetPose` - 查询当前位姿
+- `SetGripper` - 夹爪控制
 
-# 创建 ROS 2 Humble 环境
-conda create -n ros2_humble -c robostack-staging -c conda-forge ros-humble-desktop
-conda activate ros2_humble
+### 2. arm_control 节点
+独立的机械臂控制节点（不依赖 tools/udp_control.py）。
 
-# 安装构建工具
-conda install -c robostack-staging -c conda-forge \
-    colcon-common-extensions \
-    ros-humble-ament-cmake \
-    python3-pip
+**功能**：
+- 关节空间和笛卡尔空间运动控制
+- 完整的逆运动学和正运动学
+- UDP 通信（与 ESP32）
+- 状态发布（10Hz）
 
-# Python 依赖
-pip install pynput
-```
+### 3. vision_grasp 节点
+自动化视觉抓取节点。
 
-### 方式二：apt 原生安装 (Ubuntu 22.04)
+**功能**：
+- 相机坐标到基坐标系的自动转换
+- 抓取流程：释放 → 移动 → 抓取 → 回收
+- 释放流程：移动 → 释放 → 回收
 
-官方推荐的 Ubuntu 安装方式，系统级集成。
+## 🚀 快速开始
 
-```bash
-# 添加 ROS 2 源
-sudo apt update && sudo apt install curl gnupg lsb-release
-sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
-    -o /usr/share/keyrings/ros-archive-keyring.gpg
-echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" \
-    | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
-
-# 安装 ROS 2 Humble
-sudo apt update
-sudo apt install ros-humble-desktop python3-colcon-common-extensions
-
-# Python 依赖
-pip install pynput
-
-# 环境配置（或写入 ~/.bashrc）
-source /opt/ros/humble/setup.bash
-```
-
-### 方式三：Docker
-
-推荐用于 CI/CD 或快速体验，无需污染宿主机环境。
-
-```bash
-# 拉取镜像
-docker pull osrf/ros:humble-desktop
-
-# 启动容器（挂载工作空间）
-docker run -it --rm \
-    -v $(pwd)/ros2:/ws \
-    osrf/ros:humble-desktop \
-    bash
-
-# 容器内安装依赖
-apt update && apt install -y python3-colcon-common-extensions python3-pip
-pip install pynput
-```
-
-## 构建
+### 编译
 
 ```bash
 cd ros2
-colcon build --symlink-install --packages-select udp_teleop
+
+# 设置 Python 环境变量（robostack 需要）
+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
+
+# 编译
+colcon build --packages-select arm_control_msgs \
+  --cmake-args \
+    -DPython_EXECUTABLE=$PYTHON_EXECUTABLE \
+    -DPython_INCLUDE_DIR=$PYTHON_INCLUDE_DIR \
+    -DPython_LIBRARY=$PYTHON_LIBRARY
+
+colcon build --packages-select udp_teleop
+
+# Source 环境
 source install/setup.bash
 ```
 
-> `--symlink-install`：修改 Python 源文件后无需重新构建，直接生效。
-
-## 运行
+### 运行
 
+**机械臂控制**：
 ```bash
-ros2 run udp_teleop keyboard_control \
-    --ros-args --params-file src/udp_teleop/config/params.yaml
+ros2 run udp_teleop arm_control \
+    --ros-args --params-file src/udp_teleop/config/arm_control.yaml
 ```
 
-也可以通过命令行覆盖参数：
-
+**视觉抓取**：
 ```bash
-ros2 run udp_teleop keyboard_control \
-    --ros-args -p udp_ip:=192.168.1.100 -p udp_port:=9999
+# 终端 1: 启动 arm_control
+ros2 run udp_teleop arm_control \
+    --ros-args --params-file src/udp_teleop/config/arm_control.yaml
+
+# 终端 2: 启动 vision_grasp
+ros2 run udp_teleop vision_grasp \
+    --ros-args --params-file src/udp_teleop/config/vision_grasp.yaml
 ```
 
-## 包文档
+## 📚 使用示例
 
-详见 [src/udp_teleop/README.md](src/udp_teleop/README.md)，包含按键映射、UDP 协议、参数配置等。
+### 1. 控制机械臂
+
+```bash
+# 查询位姿
+ros2 service call /arm_control/get_pose arm_control_msgs/srv/GetPose
+
+# 关节运动
+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}"
+
+# 笛卡尔运动
+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}"
+```
+
+### 2. 视觉抓取
+
+```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}"
+```
+
+### 3. Python 集成
+
+```python
+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, 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 = VisionDetector()
+    node.detect_and_grasp(10.0, 5.0, 250.0)
+    rclpy.spin(node)
+```
+
+## ⚙️ 配置
+
+### arm_control 参数
+
+编辑 `src/udp_teleop/config/arm_control.yaml`：
+
+```yaml
+arm_control:
+  ros__parameters:
+    udp_ip: '192.168.4.1'      # ESP32 IP
+    udp_port: 8888
+    
+    # 几何参数 (mm)
+    l1: 125.0
+    l2: 125.0
+    x4: 110.0
+    z4: 80.0
+    
+    # 关节限位 (mm 或度)
+    height_min: -290
+    height_max: 0
+    j2_min: -110
+    j2_max: 115
+    
+    # 运动参数
+    default_duration: 1.0      # 默认运动时长 (秒)
+    default_rate: 20.0         # 插值频率 (Hz)
+```
+
+### vision_grasp 参数
+
+编辑 `src/udp_teleop/config/vision_grasp.yaml`：
+
+```yaml
+vision_grasp:
+  ros__parameters:
+    # 相机到 TCP 的变换
+    cam_tx: 0.0
+    cam_ty: 0.0
+    cam_tz: 0.0
+    cam_pitch: 0.0             # 如果相机有俯仰角
+    
+    # 回收位置
+    retract_position_x: 200.0
+    retract_position_y: 0.0
+    
+    # 运动时长
+    grasp_duration: 3.0
+    release_duration: 2.0
+```
+
+## 📡 话题和服务
+
+### arm_control
+
+**服务**：
+- `/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)
+
+### vision_grasp
+
+**话题**（订阅）：
+- `/vision_grasp/grasp_target` - 抓取目标（相机坐标）
+- `/vision_grasp/release_target` - 释放目标（基坐标）
+
+## 🐛 故障排查
+
+### 编译失败
+
+**问题**：找不到 Python 开发文件
+
+**解决**：设置环境变量
+```bash
+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
+```
+
+### 服务调用超时
+
+**问题**：vision_grasp 节点服务调用超时
+
+**原因**：在回调中使用 `time.sleep()` 阻塞了执行器
+
+**解决**：已使用多线程执行器和独立线程处理
+
+### 移动失败
+
+**检查**：
+1. ESP32 是否在线：`ping 192.168.4.1`
+2. UDP 是否可达：`echo 'XYW:0:0:0:XZHY' | nc -u 192.168.4.1 8888`
+3. 目标是否在工作空间内
+4. 关节限位是否合理
+
+## 📖 相关文档
+
+- **机械臂运动学**：`docs/arm.md` - 完整的运动学推导
+- **视觉标定**：`docs/vision_calibration_horizontal.md` - 相机标定指南
+- **原始工具**：`tools/README.md` - 命令行工具文档
+
+## 🔗 依赖关系
+
+```
+vision_grasp
+    ↓ (依赖)
+arm_control
+    ↓ (依赖)
+arm_control_msgs
+```
+
+所有节点都独立运行，通过 ROS 服务通信。
+
+## 📝 下一步
+
+1. **集成物体检测**：订阅相机图像，检测物体，发布到 `/vision_grasp/grasp_target`
+2. **添加轨迹规划**：避障和路径优化
+3. **可视化**：RViz 显示机械臂状态和检测结果
+4. **多物体处理**：队列管理和优先级排序