FallenSigh
df436a9a31
问题： - 相机水平安装，但随 J5 状态旋转 180° - J5 张开（UP）：相机正向，图像正常 - J5 闭合（DOWN）：相机旋转 180°，图像上下颠倒 解决方案： - 从 GetPose 获取 J5 状态 - J5 > 0 (UP): (xc, yc, zc) = (x_img, -y_img, z) - J5 < 0 (DOWN): (xc, yc, zc) = (-x_img, y_img, z) - 自动处理相机旋转，无需手动调整 更新文档说明相机旋转机制
266 lines
6.3 KiB
Markdown
266 lines
6.3 KiB
Markdown
# ROS 2 机械臂控制系统
|
||
|
||
CRAIC 项目的 ROS 2 机械臂控制和视觉抓取系统。
|
||
|
||
## 📦 包含组件
|
||
|
||
### 1. arm_control_msgs
|
||
消息和服务定义包。
|
||
|
||
**消息类型**：
|
||
- `JointState` - 关节状态
|
||
- `TCPPose` - TCP 位姿
|
||
|
||
**服务类型**：
|
||
- `MoveJoints` - 关节空间运动
|
||
- `MovePose` - 笛卡尔空间运动
|
||
- `GetPose` - 查询当前位姿
|
||
- `SetGripper` - 夹爪控制
|
||
|
||
### 2. arm_control 节点
|
||
独立的机械臂控制节点（不依赖 tools/udp_control.py）。
|
||
|
||
**功能**：
|
||
- 关节空间和笛卡尔空间运动控制
|
||
- 完整的逆运动学和正运动学
|
||
- UDP 通信（与 ESP32）
|
||
- 状态发布（10Hz）
|
||
|
||
### 3. vision_grasp 节点
|
||
自动化视觉抓取节点。
|
||
|
||
**功能**：
|
||
- 相机坐标到基坐标系的自动转换
|
||
- **自动处理相机旋转**：根据 J5 状态自动调整图像坐标转换
|
||
- 抓取流程：释放 → 移动 → 抓取 → 回收
|
||
- 释放流程：移动 → 释放 → 回收
|
||
|
||
**相机旋转说明**：
|
||
- J5 > 0°（张开/UP）：相机正向，`(xc, yc, zc) = (x_img, -y_img, z)`
|
||
- J5 < 0°（闭合/DOWN）：相机旋转 180°，`(xc, yc, zc) = (-x_img, y_img, z)`
|
||
|
||
## 🚀 快速开始
|
||
|
||
### 编译
|
||
|
||
```bash
|
||
cd ros2
|
||
|
||
# 设置 Python 环境变量（robostack 需要）
|
||
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
|
||
|
||
# 编译
|
||
colcon build --packages-select arm_control_msgs \
|
||
--cmake-args \
|
||
-DPython_EXECUTABLE=$PYTHON_EXECUTABLE \
|
||
-DPython_INCLUDE_DIR=$PYTHON_INCLUDE_DIR \
|
||
-DPython_LIBRARY=$PYTHON_LIBRARY
|
||
|
||
colcon build --packages-select udp_teleop
|
||
|
||
# Source 环境
|
||
source install/setup.bash
|
||
```
|
||
|
||
### 运行
|
||
|
||
**机械臂控制**：
|
||
```bash
|
||
ros2 run udp_teleop arm_control \
|
||
--ros-args --params-file src/udp_teleop/config/arm_control.yaml
|
||
```
|
||
|
||
**视觉抓取**：
|
||
```bash
|
||
# 终端 1: 启动 arm_control
|
||
ros2 run udp_teleop arm_control \
|
||
--ros-args --params-file src/udp_teleop/config/arm_control.yaml
|
||
|
||
# 终端 2: 启动 vision_grasp
|
||
ros2 run udp_teleop vision_grasp \
|
||
--ros-args --params-file src/udp_teleop/config/vision_grasp.yaml
|
||
```
|
||
|
||
## 📚 使用示例
|
||
|
||
### 1. 控制机械臂
|
||
|
||
```bash
|
||
# 查询位姿
|
||
ros2 service call /arm_control/get_pose arm_control_msgs/srv/GetPose
|
||
|
||
# 关节运动
|
||
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}"
|
||
|
||
# 笛卡尔运动
|
||
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}"
|
||
```
|
||
|
||
### 2. 视觉抓取
|
||
|
||
```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}"
|
||
```
|
||
|
||
### 3. Python 集成
|
||
|
||
```python
|
||
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, 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 = VisionDetector()
|
||
node.detect_and_grasp(10.0, 5.0, 250.0)
|
||
rclpy.spin(node)
|
||
```
|
||
|
||
## ⚙️ 配置
|
||
|
||
### arm_control 参数
|
||
|
||
编辑 `src/udp_teleop/config/arm_control.yaml`：
|
||
|
||
```yaml
|
||
arm_control:
|
||
ros__parameters:
|
||
udp_ip: '192.168.4.1' # ESP32 IP
|
||
udp_port: 8888
|
||
|
||
# 几何参数 (mm)
|
||
l1: 125.0
|
||
l2: 125.0
|
||
x4: 110.0
|
||
z4: 80.0
|
||
|
||
# 关节限位 (mm 或度)
|
||
height_min: -290
|
||
height_max: 0
|
||
j2_min: -110
|
||
j2_max: 115
|
||
|
||
# 运动参数
|
||
default_duration: 1.0 # 默认运动时长 (秒)
|
||
default_rate: 20.0 # 插值频率 (Hz)
|
||
```
|
||
|
||
### vision_grasp 参数
|
||
|
||
编辑 `src/udp_teleop/config/vision_grasp.yaml`：
|
||
|
||
```yaml
|
||
vision_grasp:
|
||
ros__parameters:
|
||
# 相机到 TCP 的变换
|
||
cam_tx: 0.0
|
||
cam_ty: 0.0
|
||
cam_tz: 0.0
|
||
cam_pitch: 0.0 # 如果相机有俯仰角
|
||
|
||
# 回收位置
|
||
retract_position_x: 200.0
|
||
retract_position_y: 0.0
|
||
|
||
# 运动时长
|
||
grasp_duration: 3.0
|
||
release_duration: 2.0
|
||
```
|
||
|
||
## 📡 话题和服务
|
||
|
||
### arm_control
|
||
|
||
**服务**：
|
||
- `/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)
|
||
|
||
### vision_grasp
|
||
|
||
**话题**（订阅）：
|
||
- `/vision_grasp/grasp_target` - 抓取目标（相机坐标）
|
||
- `/vision_grasp/release_target` - 释放目标（基坐标）
|
||
|
||
## 🐛 故障排查
|
||
|
||
### 编译失败
|
||
|
||
**问题**：找不到 Python 开发文件
|
||
|
||
**解决**：设置环境变量
|
||
```bash
|
||
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
|
||
```
|
||
|
||
### 服务调用超时
|
||
|
||
**问题**：vision_grasp 节点服务调用超时
|
||
|
||
**原因**：在回调中使用 `time.sleep()` 阻塞了执行器
|
||
|
||
**解决**：已使用多线程执行器和独立线程处理
|
||
|
||
### 移动失败
|
||
|
||
**检查**：
|
||
1. ESP32 是否在线：`ping 192.168.4.1`
|
||
2. UDP 是否可达：`echo 'XYW:0:0:0:XZHY' | nc -u 192.168.4.1 8888`
|
||
3. 目标是否在工作空间内
|
||
4. 关节限位是否合理
|
||
|
||
## 📖 相关文档
|
||
|
||
- **机械臂运动学**：`docs/arm.md` - 完整的运动学推导
|
||
- **视觉标定**：`docs/vision_calibration_horizontal.md` - 相机标定指南
|
||
- **原始工具**：`tools/README.md` - 命令行工具文档
|
||
|
||
## 🔗 依赖关系
|
||
|
||
```
|
||
vision_grasp
|
||
↓ (依赖)
|
||
arm_control
|
||
↓ (依赖)
|
||
arm_control_msgs
|
||
```
|
||
|
||
所有节点都独立运行，通过 ROS 服务通信。
|
||
|
||
## 📝 下一步
|
||
|
||
1. **集成物体检测**：订阅相机图像，检测物体，发布到 `/vision_grasp/grasp_target`
|
||
2. **添加轨迹规划**：避障和路径优化
|
||
3. **可视化**：RViz 显示机械臂状态和检测结果
|
||
4. **多物体处理**：队列管理和优先级排序
|