fallensigh/craic
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: 添加机械臂 ROS 2 控制节点和视觉抓取系统

Browse Source 
- 创建 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
This commit is contained in:
FallenSigh
2026-06-16 18:45:01 +08:00
parent 7144005be2
commit 83b32542ef
22 changed files with 3119 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

319
docs/ARM_CONTROL_README.md Normal file
View File

@@ -0,0 +1,319 @@
# 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`

122
docs/BUILD_SUCCESS.md Normal file
View File

@@ -0,0 +1,122 @@
# 编译成功！🎉
## ✅ 已完成
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)
祝使用愉快！🎉

252
docs/IMPLEMENTATION_SUMMARY.md Normal file
View File

@@ -0,0 +1,252 @@
# 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. ✅ **开箱即用** - 一键编译，快速上手
祝你使用愉快！🎉

244
docs/QUICKSTART.md Normal file
View File

@@ -0,0 +1,244 @@
# 机械臂控制 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 实现跨机器控制

359
docs/VISION_GRASP_README.md Normal file
View File

@@ -0,0 +1,359 @@
# 视觉抓取节点使用指南
## 概述
`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 显示检测结果和机械臂状态）

286
docs/VISION_GRASP_SUMMARY.md Normal file
View File

@@ -0,0 +1,286 @@
# 视觉抓取节点 - 完成总结
## ✅ 完成的工作
### 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`
系统就会自动完成抓取！

0
docs/craic.md → docs/arm.md
View File