FallenSigh
83b32542ef
- 创建 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
253 lines
7.0 KiB
Markdown
253 lines
7.0 KiB
Markdown
# 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. ✅ **开箱即用** - 一键编译，快速上手
|
||
|
||
祝你使用愉快！🎉
|