# ROS 2 机械臂控制系统

CRAIC 项目的 ROS 2 机械臂控制和视觉抓取系统。

## 📦 包含组件

### 1. arm_control_msgs
消息和服务定义包。

**消息类型**：
- `JointState` - 关节状态
- `TCPPose` - TCP 位姿

**服务类型**：
- `MoveJoints` - 关节空间运动
- `MovePose` - 笛卡尔空间运动
- `GetPose` - 查询当前位姿
- `SetGripper` - 夹爪控制

### 2. arm_control 节点
独立的机械臂控制节点（不依赖 tools/udp_control.py）。

**功能**：
- 关节空间和笛卡尔空间运动控制
- 完整的逆运动学和正运动学
- **自动 z4 适配**：根据目标 z 坐标自动选择夹爪状态
  - `z > -55mm`: UP（J5=-100，z4=-100），工作范围 z ∈ [-190, 110]mm
  - `z ≤ -55mm`: DOWN（J5=81，z4=55），工作范围 z ∈ [-345, -55]mm
- UDP 通信（与 ESP32）
- 状态发布（10Hz）

### 3. vision_grasp 节点
自动化视觉抓取节点。

**功能**：
- 相机坐标到基坐标系的自动转换
- **自动处理相机旋转**：根据 J5 状态自动调整图像坐标转换
- 抓取流程：释放 → 移动 → 抓取 → 回收
- 释放流程：移动 → 释放 → 回收

**相机旋转说明**：
- J5 < 0°（闭合/UP）：相机正向，`(xc, yc, zc) = (x_img, -y_img, z)`
- J5 > 0°（张开/DOWN）：相机旋转 180°，`(xc, yc, zc) = (-x_img, y_img, z)`

## 🚀 快速开始

### 编译

```bash
cd ros2

# 设置 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
```

### 运行

**机械臂控制**：
```bash
ros2 run udp_teleop arm_control \
    --ros-args --params-file src/udp_teleop/config/arm_control.yaml
```

**视觉抓取**：
```bash
# 终端 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
```

## 📚 使用示例

### 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. **多物体处理**：队列管理和优先级排序