# 机械臂控制 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 实现跨机器控制