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