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

25
ros2/src/arm_control_msgs/CMakeLists.txt Normal file
View File

@@ -0,0 +1,25 @@
cmake_minimum_required(VERSION 3.10)
project(arm_control_msgs)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(std_msgs REQUIRED)
# Generate messages and services
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/TCPPose.msg"
"msg/JointState.msg"
"srv/MoveJoints.srv"
"srv/MovePose.srv"
"srv/GetPose.srv"
"srv/SetGripper.srv"
DEPENDENCIES std_msgs
)
ament_export_dependencies(rosidl_default_runtime)
ament_package()

8
ros2/src/arm_control_msgs/msg/JointState.msg Normal file
View File

@@ -0,0 +1,8 @@
# 关节状态消息
std_msgs/Header header
int32 height # 高度 (mm)
int32 j2 # 关节 2 角度 (度)
int32 j3 # 关节 3 角度 (度)
int32 j4 # 关节 4 角度 (度)
int32 j5 # 关节 5 角度 (度)
int32 j6 # 关节 6 角度 (度)

6
ros2/src/arm_control_msgs/msg/TCPPose.msg Normal file
View File

@@ -0,0 +1,6 @@
# TCP 位姿消息
std_msgs/Header header
float64 x # X 坐标 (mm)
float64 y # Y 坐标 (mm)
float64 z # Z 坐标 (mm)
float64 phi # 偏航角 (度)

22
ros2/src/arm_control_msgs/package.xml Normal file
View File

@@ -0,0 +1,22 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>arm_control_msgs</name>
<version>0.0.1</version>
<description>Message and service definitions for arm control</description>
<maintainer email="fallensigh@gmail.com">fallensigh</maintainer>
<license>MIT</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<depend>std_msgs</depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

14
ros2/src/arm_control_msgs/srv/GetPose.srv Normal file
View File

@@ -0,0 +1,14 @@
# 查询当前位姿服务
---
bool success # 是否成功
string message # 返回信息
float64 x # 当前 X 坐标 (mm)
float64 y # 当前 Y 坐标 (mm)
float64 z # 当前 Z 坐标 (mm)
float64 phi # 当前偏航角 (度)
int32 height # 当前高度 (mm)
int32 j2 # 当前 J2 角度 (度)
int32 j3 # 当前 J3 角度 (度)
int32 j4 # 当前 J4 角度 (度)
int32 j5 # 当前 J5 角度 (度)
int32 j6 # 当前 J6 角度 (度)

11
ros2/src/arm_control_msgs/srv/MoveJoints.srv Normal file
View File

@@ -0,0 +1,11 @@
# 关节空间运动服务
int32 height # 目标高度 (mm)
int32 j2 # 目标 J2 角度 (度)
int32 j3 # 目标 J3 角度 (度)
int32 j4 # 目标 J4 角度 (度)
int32 j5 # 目标 J5 角度 (度)
int32 j6 # 目标 J6 角度 (度)
float64 duration # 运动时长 (秒，0 表示使用默认值)
---
bool success # 是否成功
string message # 返回信息

18
ros2/src/arm_control_msgs/srv/MovePose.srv Normal file
View File

@@ -0,0 +1,18 @@
# 笛卡尔空间运动服务
float64 x # 目标 X 坐标 (mm)
float64 y # 目标 Y 坐标 (mm)
float64 z # 目标 Z 坐标 (mm)
float64 phi # 目标偏航角 (度)
bool elbow_up # 是否使用肘部向上解
uint8 gripper_state # 夹爪状态: 0=保持, 1=打开, 2=闭合
bool grip # 是否抓取
bool release # 是否释放
float64 duration # 运动时长 (秒，0 表示使用默认值)
# 夹爪状态常量
uint8 GRIPPER_KEEP = 0
uint8 GRIPPER_OPEN = 1
uint8 GRIPPER_CLOSED = 2
---
bool success # 是否成功
string message # 返回信息

12
ros2/src/arm_control_msgs/srv/SetGripper.srv Normal file
View File

@@ -0,0 +1,12 @@
# 夹爪控制服务
uint8 gripper_state # 夹爪状态: 0=保持, 1=打开, 2=闭合
bool grip # 是否抓取
bool release # 是否释放
# 夹爪状态常量
uint8 GRIPPER_KEEP = 0
uint8 GRIPPER_OPEN = 1
uint8 GRIPPER_CLOSED = 2
---
bool success # 是否成功
string message # 返回信息

38
ros2/src/udp_teleop/config/arm_control.yaml Normal file
View File

@@ -0,0 +1,38 @@
# 机械臂控制节点参数配置
arm_control:
ros__parameters:
# UDP 通信配置
udp_ip: '192.168.4.1'
udp_port: 8888
# 机械臂几何参数 (mm)
l1: 125.0 # J2-J3 连杆长度
l2: 125.0 # J3-J4 连杆长度
x4: 110.0 # J4-TCP 水平偏移
z4: 80.0 # J4-TCP 垂直偏移
# 关节限位 (mm 或 度)
height_min: -290
height_max: 0
j2_min: -110
j2_max: 115
j3_min: -120
j3_max: 145
j4_min: -90
j4_max: 130
joint_min: -180 # J5/J6 通用限位
joint_max: 180
# 零点偏移 (度)
zero_j2: 3
zero_j3: 7
zero_j4: 25
# 运动参数
default_duration: 3.0 # 默认运动时长 (秒)
default_rate: 100.0 # 插值频率 (Hz)
use_state_cache: true # 是否使用状态缓存
# 状态发布频率
publish_rate: 10.0 # Hz

21
ros2/src/udp_teleop/config/vision_grasp.yaml Normal file
View File

@@ -0,0 +1,21 @@
# 视觉抓取节点参数配置
vision_grasp:
ros__parameters:
# 相机到 TCP 的变换参数
cam_tx: 0.0 # X 平移 (mm)
cam_ty: 0.0 # Y 平移 (mm)
cam_tz: 0.0 # Z 平移 (mm)
cam_roll: 0.0 # 绕 X 轴旋转 (度)
cam_pitch: 0.0 # 绕 Y 轴旋转 (度)
cam_yaw: 0.0 # 绕 Z 轴旋转 (度)
# 抓取参数
approach_height_offset: 50.0 # 接近高度偏移 (mm)
retract_position_x: 200.0 # 回收位置 X (mm)
retract_position_y: 0.0 # 回收位置 Y (mm)
# 运动时长
grasp_duration: 3.0 # 抓取移动时长 (秒)
release_duration: 2.0 # 释放移动时长 (秒)
gripper_duration: 1.0 # 夹爪动作时长 (秒)

8
ros2/src/udp_teleop/package.xml
View File

@@ -3,9 +3,13 @@
<package format="3">
<name>udp_teleop</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<description>UDP teleoperation and arm control for CRAIC robot</description>
<maintainer email="fallensigh@gmail.com">fallensigh</maintainer>
<license>TODO: License declaration</license>
<license>MIT</license>
<depend>rclpy</depend>
<depend>std_msgs</depend>
<depend>arm_control_msgs</depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>

4
ros2/src/udp_teleop/setup.py
View File

@@ -29,7 +29,9 @@ setup(
},
entry_points={
'console_scripts': [
'keyboard_control = udp_teleop.keyboard_control:main'
'keyboard_control = udp_teleop.keyboard_control:main',
'arm_control = udp_teleop.arm_control:main',
'vision_grasp = udp_teleop.vision_grasp:main'
],
},
)

743
ros2/src/udp_teleop/udp_teleop/arm_control.py Normal file
View File

@@ -0,0 +1,743 @@
#!/usr/bin/env python3
"""机械臂控制 ROS 节点（独立版本）
完全独立，不依赖 udp_control.py
包含所有必要的运动学和控制代码
"""
import json
import math
import socket
import time
from dataclasses import dataclass
from pathlib import Path
from typing import Optional, List
import rclpy
from rclpy.node import Node
# 导入自定义消息
from arm_control_msgs.srv import (
MoveJoints,
MovePose,
GetPose,
SetGripper
)
from arm_control_msgs.msg import (
JointState,
TCPPose
)
# ============================================================================
# 常量定义
# ============================================================================
DEFAULT_UDP_IP = "192.168.4.1"
DEFAULT_UDP_PORT = 8888
DEFAULT_HEIGHT_MIN = -290
DEFAULT_HEIGHT_MAX = 0
DEFAULT_JOINT_MIN = -180
DEFAULT_JOINT_MAX = 180
DEFAULT_J2_MIN = -110
DEFAULT_J2_MAX = 115
DEFAULT_J3_MIN = -120
DEFAULT_J3_MAX = 145
DEFAULT_J4_MIN = -90
DEFAULT_J4_MAX = 130
J5_OPEN = 81
J5_CLOSED = -100
DEFAULT_FIXED_J5 = J5_OPEN
GRIP_ANGLE = -5
RELEASE_ANGLE = 80
DEFAULT_FIXED_J6 = RELEASE_ANGLE
DEFAULT_ZERO_J2 = 3
DEFAULT_ZERO_J3 = 7
DEFAULT_ZERO_J4 = 25
DEFAULT_L1 = 125.0
DEFAULT_L2 = 125.0
DEFAULT_X4 = 110.0
DEFAULT_Z4 = 80.0
DEFAULT_INTERP_DURATION = 1.0
DEFAULT_INTERP_RATE = 20.0
STATE_FILE = Path.home() / ".ros" / "udp_control_state.json"
# ============================================================================
# 数据类定义
# ============================================================================
class ArmControlError(ValueError):
"""机械臂控制错误"""
pass
@dataclass(frozen=True)
class ArmGeometry:
"""机械臂几何参数"""
l1: float
l2: float
x4: float
z4: float
@dataclass(frozen=True)
class ArmLimits:
"""关节限位"""
height_min: int = DEFAULT_HEIGHT_MIN
height_max: int = DEFAULT_HEIGHT_MAX
joint_min: int = DEFAULT_JOINT_MIN
joint_max: int = DEFAULT_JOINT_MAX
j2_min: int = DEFAULT_J2_MIN
j2_max: int = DEFAULT_J2_MAX
j3_min: int = DEFAULT_J3_MIN
j3_max: int = DEFAULT_J3_MAX
j4_min: int = DEFAULT_J4_MIN
j4_max: int = DEFAULT_J4_MAX
@dataclass(frozen=True)
class ArmZeroOffsets:
"""零点偏移"""
j2: int = DEFAULT_ZERO_J2
j3: int = DEFAULT_ZERO_J3
j4: int = DEFAULT_ZERO_J4
@dataclass(frozen=True)
class ArmJointState:
"""关节状态"""
height: int
j2: int
j3: int
j4: int
j5: int = DEFAULT_FIXED_J5
j6: int = DEFAULT_FIXED_J6
def to_udp_message(self) -> bytes:
"""转换为 UDP 消息"""
return (
f"JXB:{self.height}:{self.j2}:{self.j3}:{self.j4}:"
f"{self.j5}:{self.j6}:0:0:EZHY\n"
).encode("utf-8")
@dataclass(frozen=True)
class ArmPose:
"""TCP 位姿"""
x: float
y: float
z: float
phi_deg: float
@dataclass(frozen=True)
class ArmMathState:
"""数学坐标系的关节状态"""
d1: float
theta2_deg: float
theta3_deg: float
theta4_deg: float
# ============================================================================
# 运动学函数
# ============================================================================
def clamp_int(value: float, lower: int, upper: int, name: str) -> int:
"""限幅并转换为整数"""
rounded = int(round(value))
if rounded < lower or rounded > upper:
raise ArmControlError(f"{name}={rounded} 超出范围 [{lower}, {upper}]")
return rounded
def normalize_angle_deg(angle_deg: float) -> float:
"""角度归一化到 [-180, 180)"""
normalized = (angle_deg + 180.0) % 360.0 - 180.0
if normalized == -180.0 and angle_deg > 0:
return 180.0
return normalized
def forward_kinematics(geometry: ArmGeometry, state: ArmMathState) -> ArmPose:
"""正运动学：关节角度 → TCP 位姿"""
theta2 = math.radians(state.theta2_deg)
theta3 = math.radians(state.theta3_deg)
theta4 = math.radians(state.theta4_deg)
phi = theta2 + theta3 + theta4
j4_center_x = (
geometry.l1 * math.cos(theta2) +
geometry.l2 * math.cos(theta2 + theta3)
)
j4_center_y = (
geometry.l1 * math.sin(theta2) +
geometry.l2 * math.sin(theta2 + theta3)
)
x = j4_center_x + geometry.x4 * math.cos(phi)
y = j4_center_y + geometry.x4 * math.sin(phi)
z = state.d1 - geometry.z4
return ArmPose(x=x, y=y, z=z, phi_deg=math.degrees(phi))
def inverse_kinematics(
geometry: ArmGeometry,
pose: ArmPose,
limits: ArmLimits,
elbow_up: bool,
j5: int,
j6: int,
) -> ArmMathState:
"""逆运动学：TCP 位姿 → 关节角度"""
# 计算 J4 中心位置
phi = math.radians(pose.phi_deg)
j4_x = pose.x - geometry.x4 * math.cos(phi)
j4_y = pose.y - geometry.x4 * math.sin(phi)
j4_z = pose.z + geometry.z4
d1 = j4_z
# 计算平面距离
r2 = j4_x * j4_x + j4_y * j4_y
if r2 < 1e-9:
raise ArmControlError("目标点过于接近奇异点")
# 计算 theta3
denom = 2.0 * geometry.l1 * geometry.l2
if abs(denom) < 1e-9:
raise ArmControlError("几何参数无效")
c3 = (r2 - geometry.l1 * geometry.l1 - geometry.l2 * geometry.l2) / denom
if c3 < -1.0 - 1e-9 or c3 > 1.0 + 1e-9:
reach = math.sqrt(r2)
raise ArmControlError(f"目标超出工作空间，距离={reach:.3f}mm")
c3 = max(-1.0, min(1.0, c3))
s3_abs = math.sqrt(max(0.0, 1.0 - c3 * c3))
s3 = -s3_abs if elbow_up else s3_abs
theta3 = math.atan2(s3, c3)
theta2 = math.atan2(j4_y, j4_x) - math.atan2(
geometry.l2 * s3,
geometry.l1 + geometry.l2 * c3,
)
theta4 = phi - theta2 - theta3
return ArmMathState(
d1=d1,
theta2_deg=normalize_angle_deg(math.degrees(theta2)),
theta3_deg=normalize_angle_deg(math.degrees(theta3)),
theta4_deg=normalize_angle_deg(math.degrees(theta4)),
)
def command_to_math_state(
command_state: ArmJointState,
zero_offsets: ArmZeroOffsets,
) -> ArmMathState:
"""命令状态 → 数学状态"""
return ArmMathState(
d1=command_state.height,
theta2_deg=command_state.j2 - zero_offsets.j2,
theta3_deg=command_state.j3 - zero_offsets.j3,
theta4_deg=command_state.j4 - zero_offsets.j4,
)
def math_to_command_state(
math_state: ArmMathState,
zero_offsets: ArmZeroOffsets,
limits: ArmLimits,
j5: int,
j6: int,
) -> ArmJointState:
"""数学状态 → 命令状态"""
return ArmJointState(
height=clamp_int(
round(math_state.d1),
limits.height_min,
limits.height_max,
"height",
),
j2=clamp_int(
math_state.theta2_deg + zero_offsets.j2,
limits.j2_min,
limits.j2_max,
"J2",
),
j3=clamp_int(
math_state.theta3_deg + zero_offsets.j3,
limits.j3_min,
limits.j3_max,
"J3",
),
j4=clamp_int(
math_state.theta4_deg + zero_offsets.j4,
limits.j4_min,
limits.j4_max,
"J4",
),
j5=clamp_int(j5, limits.joint_min, limits.joint_max, "J5"),
j6=clamp_int(j6, limits.joint_min, limits.joint_max, "J6"),
)
def interpolate_command_states(
start: ArmJointState,
end: ArmJointState,
steps: int,
) -> List[ArmJointState]:
"""关节空间插值"""
if steps <= 1:
return [end]
states = []
for step_index in range(1, steps + 1):
t = step_index / steps
states.append(
ArmJointState(
height=int(round(start.height + (end.height - start.height) * t)),
j2=int(round(start.j2 + (end.j2 - start.j2) * t)),
j3=int(round(start.j3 + (end.j3 - start.j3) * t)),
j4=int(round(start.j4 + (end.j4 - start.j4) * t)),
j5=int(round(start.j5 + (end.j5 - start.j5) * t)),
j6=int(round(start.j6 + (end.j6 - start.j6) * t)),
)
)
return states
# ============================================================================
# ROS 节点
# ============================================================================
class ArmControlNode(Node):
"""机械臂控制 ROS 节点"""
def __init__(self):
super().__init__('arm_control')
# 声明参数
self.declare_parameters(
namespace='',
parameters=[
('udp_ip', DEFAULT_UDP_IP),
('udp_port', DEFAULT_UDP_PORT),
('l1', DEFAULT_L1),
('l2', DEFAULT_L2),
('x4', DEFAULT_X4),
('z4', DEFAULT_Z4),
('height_min', DEFAULT_HEIGHT_MIN),
('height_max', DEFAULT_HEIGHT_MAX),
('j2_min', DEFAULT_J2_MIN),
('j2_max', DEFAULT_J2_MAX),
('j3_min', DEFAULT_J3_MIN),
('j3_max', DEFAULT_J3_MAX),
('j4_min', DEFAULT_J4_MIN),
('j4_max', DEFAULT_J4_MAX),
('joint_min', DEFAULT_JOINT_MIN),
('joint_max', DEFAULT_JOINT_MAX),
('zero_j2', DEFAULT_ZERO_J2),
('zero_j3', DEFAULT_ZERO_J3),
('zero_j4', DEFAULT_ZERO_J4),
('default_duration', DEFAULT_INTERP_DURATION),
('default_rate', DEFAULT_INTERP_RATE),
('use_state_cache', True),
('publish_rate', 10.0),
]
)
# 获取参数
self.udp_ip = self.get_parameter('udp_ip').value
self.udp_port = self.get_parameter('udp_port').value
self.publish_rate = self.get_parameter('publish_rate').value
# 机械臂几何参数
self.geometry = ArmGeometry(
l1=self.get_parameter('l1').value,
l2=self.get_parameter('l2').value,
x4=self.get_parameter('x4').value,
z4=self.get_parameter('z4').value,
)
# 关节限位
self.limits = ArmLimits(
height_min=self.get_parameter('height_min').value,
height_max=self.get_parameter('height_max').value,
j2_min=self.get_parameter('j2_min').value,
j2_max=self.get_parameter('j2_max').value,
j3_min=self.get_parameter('j3_min').value,
j3_max=self.get_parameter('j3_max').value,
j4_min=self.get_parameter('j4_min').value,
j4_max=self.get_parameter('j4_max').value,
joint_min=self.get_parameter('joint_min').value,
joint_max=self.get_parameter('joint_max').value,
)
# 零点偏移
self.zero_offsets = ArmZeroOffsets(
j2=self.get_parameter('zero_j2').value,
j3=self.get_parameter('zero_j3').value,
j4=self.get_parameter('zero_j4').value,
)
# 默认插值参数
self.default_duration = self.get_parameter('default_duration').value
self.default_rate = self.get_parameter('default_rate').value
self.use_state_cache = self.get_parameter('use_state_cache').value
# 当前状态
self.current_state: Optional[ArmJointState] = None
self.load_state()
# UDP socket
self.udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
# 创建服务
self.srv_move_joints = self.create_service(
MoveJoints,
'arm_control/move_joints',
self.handle_move_joints
)
self.srv_move_pose = self.create_service(
MovePose,
'arm_control/move_pose',
self.handle_move_pose
)
self.srv_get_pose = self.create_service(
GetPose,
'arm_control/get_pose',
self.handle_get_pose
)
self.srv_set_gripper = self.create_service(
SetGripper,
'arm_control/set_gripper',
self.handle_set_gripper
)
# 创建发布者
self.pub_joint_states = self.create_publisher(
JointState,
'arm_control/joint_states',
10
)
self.pub_tcp_pose = self.create_publisher(
TCPPose,
'arm_control/tcp_pose',
10
)
# 创建定时器发布状态
self.timer = self.create_timer(
1.0 / self.publish_rate,
self.publish_state
)
self.get_logger().info(f'机械臂控制节点已启动')
self.get_logger().info(f'UDP 目标: {self.udp_ip}:{self.udp_port}')
def load_state(self):
"""从缓存加载状态"""
if not self.use_state_cache or not STATE_FILE.exists():
self.current_state = ArmJointState(
height=0,
j2=self.zero_offsets.j2,
j3=self.zero_offsets.j3,
j4=self.zero_offsets.j4,
j5=DEFAULT_FIXED_J5,
j6=DEFAULT_FIXED_J6,
)
return
try:
data = json.loads(STATE_FILE.read_text())
self.current_state = ArmJointState(
height=data['height'],
j2=data['j2'],
j3=data['j3'],
j4=data['j4'],
j5=data.get('j5', DEFAULT_FIXED_J5),
j6=data.get('j6', DEFAULT_FIXED_J6),
)
self.get_logger().info(f'从缓存加载状态')
except Exception as e:
self.get_logger().warn(f'加载状态失败: {e}，使用默认状态')
self.current_state = ArmJointState(
height=0,
j2=self.zero_offsets.j2,
j3=self.zero_offsets.j3,
j4=self.zero_offsets.j4,
j5=DEFAULT_FIXED_J5,
j6=DEFAULT_FIXED_J6,
)
def save_state(self):
"""保存状态到缓存"""
if not self.use_state_cache or self.current_state is None:
return
try:
STATE_FILE.parent.mkdir(parents=True, exist_ok=True)
data = {
'height': self.current_state.height,
'j2': self.current_state.j2,
'j3': self.current_state.j3,
'j4': self.current_state.j4,
'j5': self.current_state.j5,
'j6': self.current_state.j6,
}
STATE_FILE.write_text(json.dumps(data, indent=2))
except Exception as e:
self.get_logger().warn(f'保存状态失败: {e}')
def send_udp_commands(
self,
states: List[ArmJointState],
duration: float
) -> bool:
"""发送 UDP 命令序列"""
if not states:
return True
delay = duration / len(states) if len(states) > 1 and duration > 0.0 else 0.0
try:
for i, state in enumerate(states):
msg = state.to_udp_message()
self.udp_socket.sendto(msg, (self.udp_ip, self.udp_port))
if delay > 0.0 and i < len(states) - 1:
time.sleep(delay)
self.current_state = states[-1]
self.save_state()
return True
except Exception as e:
self.get_logger().error(f'发送 UDP 命令失败: {e}')
return False
def handle_move_joints(self, request, response):
"""处理关节空间运动服务"""
try:
target_state = ArmJointState(
height=clamp_int(request.height, self.limits.height_min, self.limits.height_max, 'height'),
j2=clamp_int(request.j2, self.limits.j2_min, self.limits.j2_max, 'j2'),
j3=clamp_int(request.j3, self.limits.j3_min, self.limits.j3_max, 'j3'),
j4=clamp_int(request.j4, self.limits.j4_min, self.limits.j4_max, 'j4'),
j5=clamp_int(request.j5, self.limits.joint_min, self.limits.joint_max, 'j5'),
j6=clamp_int(request.j6, self.limits.joint_min, self.limits.joint_max, 'j6'),
)
duration = request.duration if request.duration > 0 else self.default_duration
steps = max(1, int(math.ceil(duration * self.default_rate)))
path = interpolate_command_states(self.current_state, target_state, steps)
success = self.send_udp_commands(path, duration)
response.success = success
response.message = "运动完成" if success else "运动失败"
self.get_logger().info(f'关节运动 -> {response.message}')
except Exception as e:
response.success = False
response.message = f'错误: {str(e)}'
self.get_logger().error(f'关节运动失败: {e}')
return response
def handle_move_pose(self, request, response):
"""处理笛卡尔空间运动服务"""
try:
target_pose = ArmPose(
x=request.x,
y=request.y,
z=request.z,
phi_deg=request.phi
)
# 解析夹爪状态
if request.gripper_state == SetGripper.Request.GRIPPER_OPEN:
j5 = J5_OPEN
elif request.gripper_state == SetGripper.Request.GRIPPER_CLOSED:
j5 = J5_CLOSED
else:
j5 = self.current_state.j5
if request.grip:
j6 = GRIP_ANGLE
elif request.release:
j6 = RELEASE_ANGLE
else:
j6 = self.current_state.j6
# 逆运动学
math_state = inverse_kinematics(
geometry=self.geometry,
pose=target_pose,
limits=self.limits,
elbow_up=request.elbow_up,
j5=j5,
j6=j6,
)
# 转换为命令状态
target_state = math_to_command_state(
math_state,
self.zero_offsets,
self.limits,
j5=j5,
j6=j6,
)
duration = request.duration if request.duration > 0 else self.default_duration
steps = max(1, int(math.ceil(duration * self.default_rate)))
path = interpolate_command_states(self.current_state, target_state, steps)
success = self.send_udp_commands(path, duration)
response.success = success
response.message = "运动完成" if success else "运动失败"
self.get_logger().info(
f'位姿运动: ({request.x:.1f}, {request.y:.1f}, {request.z:.1f}, {request.phi:.1f}) '
f'-> {response.message}'
)
except Exception as e:
response.success = False
response.message = f'错误: {str(e)}'
self.get_logger().error(f'位姿运动失败: {e}')
return response
def handle_get_pose(self, request, response):
"""处理查询位姿服务"""
try:
if self.current_state is None:
response.success = False
response.message = "当前状态未初始化"
return response
math_state = command_to_math_state(self.current_state, self.zero_offsets)
pose = forward_kinematics(self.geometry, math_state)
response.success = True
response.x = pose.x
response.y = pose.y
response.z = pose.z
response.phi = pose.phi_deg
response.height = self.current_state.height
response.j2 = self.current_state.j2
response.j3 = self.current_state.j3
response.j4 = self.current_state.j4
response.j5 = self.current_state.j5
response.j6 = self.current_state.j6
except Exception as e:
response.success = False
response.message = f'错误: {str(e)}'
self.get_logger().error(f'查询位姿失败: {e}')
return response
def handle_set_gripper(self, request, response):
"""处理夹爪控制服务"""
try:
if request.gripper_state == SetGripper.Request.GRIPPER_OPEN:
j5 = J5_OPEN
elif request.gripper_state == SetGripper.Request.GRIPPER_CLOSED:
j5 = J5_CLOSED
else:
j5 = self.current_state.j5
if request.grip:
j6 = GRIP_ANGLE
elif request.release:
j6 = RELEASE_ANGLE
else:
j6 = self.current_state.j6
target_state = ArmJointState(
height=self.current_state.height,
j2=self.current_state.j2,
j3=self.current_state.j3,
j4=self.current_state.j4,
j5=j5,
j6=j6,
)
success = self.send_udp_commands([target_state], 0.0)
response.success = success
response.message = "夹爪控制完成" if success else "夹爪控制失败"
self.get_logger().info(f'夹爪控制: j5={j5}, j6={j6} -> {response.message}')
except Exception as e:
response.success = False
response.message = f'错误: {str(e)}'
self.get_logger().error(f'夹爪控制失败: {e}')
return response
def publish_state(self):
"""定时发布状态"""
if self.current_state is None:
return
try:
# 发布关节状态
joint_msg = JointState()
joint_msg.header.stamp = self.get_clock().now().to_msg()
joint_msg.height = self.current_state.height
joint_msg.j2 = self.current_state.j2
joint_msg.j3 = self.current_state.j3
joint_msg.j4 = self.current_state.j4
joint_msg.j5 = self.current_state.j5
joint_msg.j6 = self.current_state.j6
self.pub_joint_states.publish(joint_msg)
# 计算并发布 TCP 位姿
math_state = command_to_math_state(self.current_state, self.zero_offsets)
pose = forward_kinematics(self.geometry, math_state)
pose_msg = TCPPose()
pose_msg.header.stamp = self.get_clock().now().to_msg()
pose_msg.x = pose.x
pose_msg.y = pose.y
pose_msg.z = pose.z
pose_msg.phi = pose.phi_deg
self.pub_tcp_pose.publish(pose_msg)
except Exception as e:
self.get_logger().error(f'发布状态失败: {e}')
def destroy_node(self):
"""节点销毁时的清理"""
self.udp_socket.close()
super().destroy_node()
def main(args=None):
rclpy.init(args=args)
node = ArmControlNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

182
ros2/src/udp_teleop/udp_teleop/arm_control_client.py Normal file
View File

@@ -0,0 +1,182 @@
#!/usr/bin/env python3
"""机械臂控制客户端示例
演示如何调用机械臂控制服务
"""
import rclpy
from rclpy.node import Node
from arm_control_msgs.srv import MoveJoints, MovePose, GetPose, SetGripper
class ArmControlClient(Node):
"""机械臂控制客户端"""
def __init__(self):
super().__init__('arm_control_client')
# 创建服务客户端
self.cli_move_joints = self.create_client(MoveJoints, 'arm_control/move_joints')
self.cli_move_pose = self.create_client(MovePose, 'arm_control/move_pose')
self.cli_get_pose = self.create_client(GetPose, 'arm_control/get_pose')
self.cli_set_gripper = self.create_client(SetGripper, 'arm_control/set_gripper')
# 等待服务可用
self.get_logger().info('等待服务...')
self.cli_move_joints.wait_for_service()
self.cli_move_pose.wait_for_service()
self.cli_get_pose.wait_for_service()
self.cli_set_gripper.wait_for_service()
self.get_logger().info('服务已连接')
def get_current_pose(self):
"""查询当前位姿"""
req = GetPose.Request()
future = self.cli_get_pose.call_async(req)
rclpy.spin_until_future_complete(self, future)
if future.result().success:
result = future.result()
self.get_logger().info(
f'当前位姿: x={result.x:.1f}, y={result.y:.1f}, '
f'z={result.z:.1f}, phi={result.phi:.1f}°'
)
self.get_logger().info(
f'关节角度: height={result.height}, j2={result.j2}, '
f'j3={result.j3}, j4={result.j4}, j5={result.j5}, j6={result.j6}'
)
return result
else:
self.get_logger().error(f'查询失败: {future.result().message}')
return None
def move_to_joints(self, height, j2, j3, j4, j5=81, j6=30, duration=2.0):
"""关节空间运动"""
req = MoveJoints.Request()
req.height = height
req.j2 = j2
req.j3 = j3
req.j4 = j4
req.j5 = j5
req.j6 = j6
req.duration = duration
self.get_logger().info(f'关节运动: height={height}, j2={j2}, j3={j3}, j4={j4}')
future = self.cli_move_joints.call_async(req)
rclpy.spin_until_future_complete(self, future)
if future.result().success:
self.get_logger().info('运动完成')
return True
else:
self.get_logger().error(f'运动失败: {future.result().message}')
return False
def move_to_pose(self, x, y, z, phi, duration=2.0, grip=False, release=False):
"""笛卡尔空间运动"""
req = MovePose.Request()
req.x = x
req.y = y
req.z = z
req.phi = phi
req.duration = duration
req.grip = grip
req.release = release
req.gripper_state = MovePose.Request.GRIPPER_KEEP
req.elbow_up = False
self.get_logger().info(f'位姿运动: ({x:.1f}, {y:.1f}, {z:.1f}), phi={phi:.1f}°')
future = self.cli_move_pose.call_async(req)
rclpy.spin_until_future_complete(self, future)
if future.result().success:
self.get_logger().info('运动完成')
return True
else:
self.get_logger().error(f'运动失败: {future.result().message}')
return False
def set_gripper(self, grip=False, release=False):
"""夹爪控制"""
req = SetGripper.Request()
req.grip = grip
req.release = release
req.gripper_state = SetGripper.Request.GRIPPER_KEEP
action = "抓取" if grip else ("释放" if release else "保持")
self.get_logger().info(f'夹爪控制: {action}')
future = self.cli_set_gripper.call_async(req)
rclpy.spin_until_future_complete(self, future)
if future.result().success:
self.get_logger().info('夹爪控制完成')
return True
else:
self.get_logger().error(f'夹爪控制失败: {future.result().message}')
return False
def demo_sequence(client):
"""演示抓取流程"""
print("\n" + "="*60)
print("演示序列：查询 → 移动 → 抓取")
print("="*60 + "\n")
# 1. 查询当前位姿
print("1. 查询当前位姿...")
client.get_current_pose()
# 2. 移动到上方
print("\n2. 移动到物体上方...")
client.move_to_pose(x=200.0, y=100.0, z=-50.0, phi=45.0, duration=2.0, release=True)
# 3. 下降到抓取位置
print("\n3. 下降到抓取位置...")
client.move_to_pose(x=200.0, y=100.0, z=-150.0, phi=45.0, duration=1.0, release=True)
# 4. 抓取
print("\n4. 执行抓取...")
client.set_gripper(grip=True)
# 5. 提升
print("\n5. 提升物体...")
client.move_to_pose(x=200.0, y=100.0, z=-50.0, phi=45.0, duration=1.0, grip=True)
# 6. 移动到目标位置
print("\n6. 移动到目标位置...")
client.move_to_pose(x=100.0, y=200.0, z=-50.0, phi=90.0, duration=2.0, grip=True)
# 7. 下降
print("\n7. 下降...")
client.move_to_pose(x=100.0, y=200.0, z=-150.0, phi=90.0, duration=1.0, grip=True)
# 8. 释放
print("\n8. 释放物体...")
client.set_gripper(release=True)
# 9. 提升
print("\n9. 提升...")
client.move_to_pose(x=100.0, y=200.0, z=-50.0, phi=90.0, duration=1.0, release=True)
print("\n" + "="*60)
print("演示完成！")
print("="*60 + "\n")
def main():
rclpy.init()
client = ArmControlClient()
try:
# 运行演示序列
demo_sequence(client)
except KeyboardInterrupt:
pass
finally:
client.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

428
ros2/src/udp_teleop/udp_teleop/vision_grasp.py Normal file
View File

@@ -0,0 +1,428 @@
#!/usr/bin/env python3
"""视觉抓取 ROS 节点
基于相机坐标系到基坐标系的变换，实现自动抓取和释放功能。
功能：
1. 抓取服务：输入相机坐标 (x, y, z)，自动转换并执行抓取
2. 释放服务：输入基坐标系位置，移动并释放物体
"""
import math
from typing import Tuple
import numpy as np
import rclpy
from rclpy.node import Node
from arm_control_msgs.srv import MovePose, GetPose, SetGripper
from std_srvs.srv import Trigger
from geometry_msgs.msg import Point
# ============================================================================
# 坐标变换函数（从 camera_to_base.py 复制）
# ============================================================================
def euler_to_rotation_matrix(roll_deg: float, pitch_deg: float, yaw_deg: float) -> np.ndarray:
"""欧拉角转旋转矩阵（ZYX顺序）"""
roll = math.radians(roll_deg)
pitch = math.radians(pitch_deg)
yaw = math.radians(yaw_deg)
Rx = np.array([
[1, 0, 0],
[0, math.cos(roll), -math.sin(roll)],
[0, math.sin(roll), math.cos(roll)]
])
Ry = np.array([
[math.cos(pitch), 0, math.sin(pitch)],
[0, 1, 0],
[-math.sin(pitch), 0, math.cos(pitch)]
])
Rz = np.array([
[math.cos(yaw), -math.sin(yaw), 0],
[math.sin(yaw), math.cos(yaw), 0],
[0, 0, 1]
])
return Rz @ Ry @ Rx
def camera_to_tcp(
xc: float, yc: float, zc: float,
tx: float = 0.0, ty: float = 0.0, tz: float = 0.0,
roll: float = 0.0, pitch: float = 0.0, yaw: float = 0.0
) -> Tuple[float, float, float]:
"""相机坐标系 → TCP 坐标系"""
R = euler_to_rotation_matrix(roll, pitch, yaw)
T = np.array([tx, ty, tz])
P_cam = np.array([xc, yc, zc])
P_tcp = R @ P_cam + T
return float(P_tcp[0]), float(P_tcp[1]), float(P_tcp[2])
def tcp_to_base(
xt: float, yt: float, zt: float,
tcp_x: float, tcp_y: float, tcp_z: float, tcp_phi_deg: float
) -> Tuple[float, float, float]:
"""TCP 坐标系 → 机械臂基坐标系（水平相机版本）"""
phi = math.radians(tcp_phi_deg)
R_tcp_to_base = np.array([
[-math.sin(phi), 0, math.cos(phi)],
[ math.cos(phi), 0, math.sin(phi)],
[0, -1, 0]
])
P_tcp = np.array([xt, yt, zt])
P_base_relative = R_tcp_to_base @ P_tcp
P_base = P_base_relative + np.array([tcp_x, tcp_y, tcp_z])
return float(P_base[0]), float(P_base[1]), float(P_base[2])
def camera_to_base(
xc: float, yc: float, zc: float,
tcp_x: float, tcp_y: float, tcp_z: float, tcp_phi_deg: float,
cam_tx: float = 0.0, cam_ty: float = 0.0, cam_tz: float = 0.0,
cam_roll: float = 0.0, cam_pitch: float = 0.0, cam_yaw: float = 0.0
) -> Tuple[float, float, float]:
"""完整变换：相机坐标系 → 基坐标系"""
xt, yt, zt = camera_to_tcp(xc, yc, zc, cam_tx, cam_ty, cam_tz,
cam_roll, cam_pitch, cam_yaw)
xb, yb, zb = tcp_to_base(xt, yt, zt, tcp_x, tcp_y, tcp_z, tcp_phi_deg)
return xb, yb, zb
# ============================================================================
# 自定义服务定义
# ============================================================================
# 由于没有预定义服务，我们使用简化的接口
# 实际使用时可以创建自定义 .srv 文件
# ============================================================================
# 视觉抓取节点
# ============================================================================
class VisionGraspNode(Node):
"""视觉抓取 ROS 节点"""
def __init__(self):
super().__init__('vision_grasp')
# 声明参数
self.declare_parameters(
namespace='',
parameters=[
# 相机到 TCP 的变换参数
('cam_tx', 0.0),
('cam_ty', 0.0),
('cam_tz', 0.0),
('cam_roll', 0.0),
('cam_pitch', 0.0),
('cam_yaw', 0.0),
# 抓取参数
('approach_height_offset', 50.0), # 接近高度偏移 (mm)
('retract_position_x', 200.0), # 回收位置 X
('retract_position_y', 0.0), # 回收位置 Y
('grasp_duration', 3.0), # 抓取移动时长 (秒)
('release_duration', 2.0), # 释放移动时长 (秒)
('gripper_duration', 1.0), # 夹爪动作时长 (秒)
]
)
# 获取参数
self.cam_tx = self.get_parameter('cam_tx').value
self.cam_ty = self.get_parameter('cam_ty').value
self.cam_tz = self.get_parameter('cam_tz').value
self.cam_roll = self.get_parameter('cam_roll').value
self.cam_pitch = self.get_parameter('cam_pitch').value
self.cam_yaw = self.get_parameter('cam_yaw').value
self.approach_offset = self.get_parameter('approach_height_offset').value
self.retract_x = self.get_parameter('retract_position_x').value
self.retract_y = self.get_parameter('retract_position_y').value
self.grasp_duration = self.get_parameter('grasp_duration').value
self.release_duration = self.get_parameter('release_duration').value
self.gripper_duration = self.get_parameter('gripper_duration').value
# 创建服务客户端（连接到 arm_control 节点）
self.move_cli = self.create_client(MovePose, 'arm_control/move_pose')
self.get_pose_cli = self.create_client(GetPose, 'arm_control/get_pose')
self.gripper_cli = self.create_client(SetGripper, 'arm_control/set_gripper')
# 等待服务可用
self.get_logger().info('等待 arm_control 服务...')
self.move_cli.wait_for_service(timeout_sec=5.0)
self.get_pose_cli.wait_for_service(timeout_sec=5.0)
self.gripper_cli.wait_for_service(timeout_sec=5.0)
self.get_logger().info('arm_control 服务已连接')
# 创建订阅者：接收检测结果
self.grasp_sub = self.create_subscription(
Point,
'vision_grasp/grasp_target',
self.handle_grasp_target,
10
)
self.release_sub = self.create_subscription(
Point,
'vision_grasp/release_target',
self.handle_release_target,
10
)
self.get_logger().info('视觉抓取节点已启动')
self.get_logger().info('订阅话题:')
self.get_logger().info(' - /vision_grasp/grasp_target (geometry_msgs/Point)')
self.get_logger().info(' - /vision_grasp/release_target (geometry_msgs/Point)')
def get_current_tcp_pose(self) -> Tuple[float, float, float, float]:
"""查询当前 TCP 位姿"""
req = GetPose.Request()
future = self.get_pose_cli.call_async(req)
# 等待结果（不阻塞其他回调）
import time
start_time = time.time()
timeout = 5.0
while not future.done():
if time.time() - start_time > timeout:
raise RuntimeError("获取 TCP 位姿超时")
time.sleep(0.01)
result = future.result()
if not result.success:
raise RuntimeError(f"获取 TCP 位姿失败: {result.message}")
return result.x, result.y, result.z, result.phi
def move_to(self, x: float, y: float, z: float, phi: float,
duration: float, grip: bool = False, release: bool = False) -> bool:
"""移动到指定位置"""
req = MovePose.Request()
req.x = x
req.y = y
req.z = z
req.phi = phi
req.duration = duration
req.grip = grip
req.release = release
req.gripper_state = MovePose.Request.GRIPPER_KEEP
req.elbow_up = False
future = self.move_cli.call_async(req)
# 等待结果
import time
start_time = time.time()
timeout = duration + 5.0
while not future.done():
if time.time() - start_time > timeout:
self.get_logger().error("移动超时")
return False
time.sleep(0.01)
result = future.result()
if result is None:
self.get_logger().error("移动失败：无响应")
return False
return result.success
def set_gripper(self, grip: bool = False, release: bool = False) -> bool:
"""控制夹爪"""
req = SetGripper.Request()
req.grip = grip
req.release = release
req.gripper_state = SetGripper.Request.GRIPPER_KEEP
future = self.gripper_cli.call_async(req)
# 等待结果
import time
start_time = time.time()
timeout = 3.0
while not future.done():
if time.time() - start_time > timeout:
self.get_logger().error("夹爪控制超时")
return False
time.sleep(0.01)
result = future.result()
if result is None:
self.get_logger().error("夹爪控制失败：无响应")
return False
return result.success
def handle_grasp_target(self, msg: Point):
"""处理抓取目标"""
self.get_logger().info(f'收到抓取目标: 相机坐标 ({msg.x:.1f}, {msg.y:.1f}, {msg.z:.1f})')
# 在单独的线程中执行抓取，避免阻塞回调
import threading
thread = threading.Thread(
target=self._execute_grasp_thread,
args=(msg.x, msg.y, msg.z)
)
thread.start()
def _execute_grasp_thread(self, x: float, y: float, z: float):
"""在独立线程中执行抓取流程"""
try:
# 转换坐标：(x, y, z) -> (xc, yc, zc) = (x, -y, z)
xc = x
yc = -y
zc = z
self.get_logger().info(f'转换后相机坐标: ({xc:.1f}, {yc:.1f}, {zc:.1f})')
# 获取当前 TCP 位姿
tcp_x, tcp_y, tcp_z, tcp_phi = self.get_current_tcp_pose()
self.get_logger().info(f'当前 TCP: ({tcp_x:.1f}, {tcp_y:.1f}, {tcp_z:.1f}), phi={tcp_phi:.1f}°')
# 坐标变换
target_x, target_y, target_z = camera_to_base(
xc, yc, zc,
tcp_x, tcp_y, tcp_z, tcp_phi,
self.cam_tx, self.cam_ty, self.cam_tz,
self.cam_roll, self.cam_pitch, self.cam_yaw
)
self.get_logger().info(f'目标基坐标: ({target_x:.1f}, {target_y:.1f}, {target_z:.1f})')
# 执行抓取流程
self.execute_grasp(target_x, target_y, target_z, tcp_phi)
except Exception as e:
self.get_logger().error(f'抓取失败: {e}')
def handle_release_target(self, msg: Point):
"""处理释放目标"""
self.get_logger().info(f'收到释放目标: 基坐标 ({msg.x:.1f}, {msg.y:.1f}, {msg.z:.1f})')
# 在单独的线程中执行释放，避免阻塞回调
import threading
thread = threading.Thread(
target=self._execute_release_thread,
args=(msg.x, msg.y, msg.z)
)
thread.start()
def _execute_release_thread(self, x: float, y: float, z: float):
"""在独立线程中执行释放流程"""
try:
# 获取当前 TCP 位姿（用于获取 phi）
_, _, _, tcp_phi = self.get_current_tcp_pose()
# 执行释放流程
self.execute_release(x, y, z, tcp_phi)
except Exception as e:
self.get_logger().error(f'释放失败: {e}')
def execute_grasp(self, x: float, y: float, z: float, phi: float):
"""执行抓取流程
1. release 夹爪 (duration=0)
2. 移动到目标位置 (duration=3)
3. grip 夹爪 (duration=1)
4. 移动到回收位置 (200, 0, 当前z)
"""
self.get_logger().info('=' * 60)
self.get_logger().info('开始抓取流程')
self.get_logger().info('=' * 60)
# 步骤 1: 释放夹爪
self.get_logger().info('1. 释放夹爪')
if not self.set_gripper(release=True):
self.get_logger().error('释放夹爪失败')
return
# 步骤 2: 移动到目标位置
self.get_logger().info(f'2. 移动到目标位置: ({x:.1f}, {y:.1f}, {z:.1f})')
if not self.move_to(x, y, z, phi, self.grasp_duration, release=True):
self.get_logger().error('移动到目标位置失败')
return
# 步骤 3: 抓取
self.get_logger().info('3. 抓取物体')
if not self.move_to(x, y, z, phi, self.gripper_duration, grip=True):
self.get_logger().error('抓取失败')
return
# 步骤 4: 移动到回收位置
self.get_logger().info(f'4. 移动到回收位置: ({self.retract_x:.1f}, {self.retract_y:.1f}, {z:.1f})')
if not self.move_to(self.retract_x, self.retract_y, z, phi, self.grasp_duration, grip=True):
self.get_logger().error('移动到回收位置失败')
return
self.get_logger().info('=' * 60)
self.get_logger().info('✓ 抓取完成！')
self.get_logger().info('=' * 60)
def execute_release(self, x: float, y: float, z: float, phi: float):
"""执行释放流程
1. 移动到指定位置
2. release 夹爪 (duration=0)
3. 回收到 (200, 0, 当前z)
"""
self.get_logger().info('=' * 60)
self.get_logger().info('开始释放流程')
self.get_logger().info('=' * 60)
# 步骤 1: 移动到释放位置
self.get_logger().info(f'1. 移动到释放位置: ({x:.1f}, {y:.1f}, {z:.1f})')
if not self.move_to(x, y, z, phi, self.release_duration, grip=True):
self.get_logger().error('移动到释放位置失败')
return
# 步骤 2: 释放夹爪
self.get_logger().info('2. 释放物体')
if not self.set_gripper(release=True):
self.get_logger().error('释放夹爪失败')
return
# 步骤 3: 回收到初始位置
self.get_logger().info(f'3. 回收到初始位置: ({self.retract_x:.1f}, {self.retract_y:.1f}, {z:.1f})')
if not self.move_to(self.retract_x, self.retract_y, z, phi, self.release_duration, release=True):
self.get_logger().error('回收失败')
return
self.get_logger().info('=' * 60)
self.get_logger().info('✓ 释放完成！')
self.get_logger().info('=' * 60)
def main(args=None):
rclpy.init(args=args)
# 使用多线程执行器以支持在回调中调用服务
from rclpy.executors import MultiThreadedExecutor
node = VisionGraspNode()
executor = MultiThreadedExecutor()
executor.add_node(node)
try:
executor.spin()
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()