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

docs: 整理和精简文档结构

Browse Source 
- 删除冗余文档（6个重复的说明文档）
- 更新 README.md：简洁清晰的项目总览
- 更新 ros2/README.md：完整的 ROS 2 使用指南
- 保留核心技术文档：docs/arm.md（运动学推导）

文档现在更加简洁，避免重复内容
This commit is contained in:
FallenSigh
2026-06-16 19:01:42 +08:00
parent 83b32542ef
commit f00537ebb9
8 changed files with 374 additions and 1724 deletions
Download Patch File Download Diff File Expand all files Collapse all files

208
README.md
View File

@@ -2,9 +2,9 @@
> 本仓库为 **[中国机器人及人工智能大赛](https://www.caairobot.com)**（CRAIC）**机器人任务挑战赛（小型桌面级）** 参赛代码。
ESP32-S3 双核摄像头 + ROS 2 键盘遥控 + UDP 实时通讯的一体化机器人控制系统。
ESP32-S3 双核摄像头 + ROS 2 机械臂控制 + 视觉抓取的一体化机器人系统。
## 赛事
## 赛事信息
**中国机器人及人工智能大赛**（**C**hina **R**obot and **A**rtificial **I**ntelligence **C**ompetition，简称 **CRAIC**）是由 [中国人工智能学会](https://www.caai.cn)（CAAI）主办的全国性学科竞赛，始于 1999 年，已列入**全国普通高等学校学科竞赛排行榜**及**教育部 A 类竞赛名单**。
@@ -23,68 +23,59 @@ ESP32-S3 双核摄像头 + ROS 2 键盘遥控 + UDP 实时通讯的一体化机
```
craic/
├── jxbeye/ # ESP32-S3 摄像头固件 (PlatformIO)
│ ├── src/main.cpp # 双核并行推流 + AsyncUDP + ESP-NOW
│ ├── platformio.ini # 构建配置 (ESP32-S3-WROOM-1-N16R8)
│ ├── boards/ # 自定义板级定义
│ ├── lib/FTServo/ # 飞特舵机控制库
│ └── include/ / test/
├── ros2/ # ROS 2 Humble 键盘遥控
│ └── src/udp_teleop/ # 底盘 + 6轴机械臂 UDP 遥控节点
├── tools/ # 调试工具
│ └── udp_server.py # UDP 回显测试服务器
└── .gitignore
├── jxbeye/ # ESP32-S3 固件 (PlatformIO)
│ ├── src/main.cpp # 双核摄像头推流 + UDP 控制
│ └── platformio.ini # ESP32-S3-WROOM-1-N16R8 配置
├── ros2/ # ROS 2 控制系统
│ ├── src/arm_control_msgs/ # 消息和服务定义
│ └── src/udp_teleop/ # 机械臂控制和视觉抓取节点
├── tools/ # 独立工具脚本
│ ├── udp_control.py # 命令行机械臂控制（带逆运动学）
│ ├── camera_to_base.py # 相机坐标到基坐标变换
│ └── camera_capture.py # MJPEG 流采集
└── docs/ # 技术文档
└── arm.md # 机械臂运动学推导
```
## 硬件
| 组件 | 型号 |
|------|------|
| 主控 | ESP32-S3-WROOM-1-N16R8 (16MB Flash, 8MB PSRAM Octal) |
| 主控 | ESP32-S3-WROOM-1-N16R8 (16MB Flash, 8MB PSRAM) |
| 摄像头 | OV2640 (XGA 1024×768, JPEG) |
| 舵机 | 飞特 SCS/STS 系列串行舵机 |
| 机械臂 | 6-DOF，飞特 SCS/STS 串行舵机 |
## 功能
## 核心功能
### 1. ESP32-S3 固件 (`jxbeye/`)
- **双核并行架构**：Core 0 负责摄像头采集，Core 1 负责 WiFi 推流
- **MJPEG 实时推流**：Web 浏览器直接访问 `http://<IP>` 查看实时画面
- **AsyncUDP 非阻塞通讯**：UDP 指令由中断回调处理，不干扰推流
- **ESP-NOW**：低延迟点对点通信
- **在线 WiFi 配置**：通过 Web 页面或串口修改 SSID/密码，自动保存到 NVS
- **双核架构**：Core 0 采集，Core 1 推流
- **MJPEG 推流**：`http://<IP>` 实时查看
- **UDP 控制**：端口 8888，非阻塞异步处理
- **WiFi 配置**：串口发送 `WIFI:SSID:PASSWORD` 配置
### 2. ROS 2 键盘遥控 (`ros2/src/udp_teleop/`)
### 2. ROS 2 机械臂控制 (`ros2/`)
- **底盘遥控**：W/S 前进后退、A/D 左右平移、Q/E 旋转（差速驱动）
- **机械臂遥控**：6 轴关节角度独立控制，高度升降
- **多平台键盘后端**：
- `stdin` — Linux/macOS 终端原生输入，零依赖
- `pynput` — 跨平台按键监听
- `win_poll` — Windows Win32 API 轮询
- **可配置参数**：速度、步长、目标 IP/端口均通过 YAML 配置
**arm_control 节点**：
- 关节空间和笛卡尔空间运动控制
- 完整逆运动学和正运动学
- 服务接口：`move_joints`, `move_pose`, `get_pose`, `set_gripper`
- 状态发布（10Hz）
### 3. UDP 通讯协议
**vision_grasp 节点**：
- 相机坐标到基坐标系自动转换
- 自动抓取：释放 → 移动 → 抓取 → 回收
- 自动释放：移动 → 释放 → 回收
所有指令通过 UDP 端口 `8888` 发送，格式：
### 3. 工具脚本 (`tools/`)
```
# 底盘控制
XYW:<X速度>:<Y速度>:<W角速度>:XZHY\n
# 机械臂控制
JXB:<高度>:<J2>:<J3>:<J4>:<J5>:<J6>:0:0:EZHY\n
# 激光控制
LASERON / LASEROFF
# 串口透传
<any payload ending with ZHY or \n>
```
- `udp_control.py` - 命令行机械臂控制（支持逆运动学）
- `camera_to_base.py` - 坐标变换工具
- `camera_capture.py` - 相机帧采集（支持自动扫描）
## 快速开始
### 编译 & 烧录 ESP32 固件
### ESP32 固件
```bash
cd jxbeye
@@ -92,55 +83,134 @@ pio run -t upload
pio device monitor
```
首次启动后 ESP32 创建热点 `ESP32-S3-Camera`（密码 `12345678`），浏览器访问 `http://192.168.4.1`。
首次启动创建热点 `ESP32-S3-Camera`（密码 `12345678`），访问 `http://192.168.4.1`。
### 运行 ROS 2 键盘遥控
### ROS 2 控制系统
```bash
# 创建环境（首次）
# 环境准备（首次）
conda create -n ros2_humble -c robostack-staging -c conda-forge ros-humble-desktop
conda install -n ros2_humble -c robostack-staging -c conda-forge colcon-common-extensions
conda run -n ros2_humble pip install pynput
# 构建 & 运行
conda activate ros2_humble
# 编译
cd ros2
colcon build --symlink-install --packages-select udp_teleop
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 install/setup.bash
# 启动键盘遥控（默认目标 192.168.233.67:8888）
ros2 run udp_teleop keyboard_control \
--ros-args --params-file src/udp_teleop/config/params.yaml
# 运行机械臂控制
ros2 run udp_teleop arm_control \
--ros-args --params-file src/udp_teleop/config/arm_control.yaml
# 运行视觉抓取（新终端）
ros2 run udp_teleop vision_grasp \
--ros-args --params-file src/udp_teleop/config/vision_grasp.yaml
```
### 测试 UDP 通讯
### 命令行工具
```bash
# 启动回显服务器
python tools/udp_server.py
# 机械臂控制（关节空间）
python tools/udp_control.py joints \
--height -100 --j2 10 --j3 20 --j4 30 --duration 2.0
# 发送测试指令
echo 'XYW:100:0:0:XZHY' | nc -u 127.0.0.1 8888
# 机械臂控制（笛卡尔空间）
python tools/udp_control.py pose \
--x 200 --y 100 --z -100 --phi 45 --duration 2.0
# 相机采集
python tools/camera_capture.py --ip 192.168.4.1
python tools/camera_capture.py --scan # 自动扫描
```
### 串口配置 WiFi
## UDP 协议
在 ESP32 串口监视器中发送：
所有指令通过 UDP 端口 `8888` 发送：
```
WIFI:热点名称:密码
```bash
# 底盘控制（麦克纳姆轮）
XYW:<X>:<Y>:<W>:XZHY\n
# 机械臂控制（6 轴）
JXB:<height>:<J2>:<J3>:<J4>:<J5>:<J6>:0:0:EZHY\n
# 激光控制
LASERON / LASEROFF
```
设备自动保存配置并重启。
## 使用示例
### ROS 服务调用
```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}"
# 视觉抓取（发布相机坐标）
ros2 topic pub --once /vision_grasp/grasp_target geometry_msgs/Point \
"{x: 10.0, y: 5.0, z: 250.0}"
```
### Python 集成
```python
import rclpy
from geometry_msgs.msg import Point
class VisionDetector:
def __init__(self):
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)
```
## 文档
- **ROS 2 系统**：[ros2/README.md](ros2/README.md) - 完整的 ROS 节点文档
- **运动学推导**：[docs/arm.md](docs/arm.md) - 机械臂逆运动学数学推导
- **视觉标定**：[docs/vision_calibration_horizontal.md](docs/vision_calibration_horizontal.md) - 相机标定指南
## 坐标系说明
**机械臂基坐标系**（Z 轴朝上）：
- 原点：J1 线性滑轨底部
- X 轴：基座正前方
- Y 轴：基座左侧
- Z 轴：竖直向上（高度）
**相机坐标系**（水平安装）：
- X 轴：右侧
- Y 轴：向下
- Z 轴：正前方（光轴）
变换由 `camera_to_base.py` 和 `vision_grasp` 节点自动处理。
## 依赖
| 环境 | 依赖 |
|------|------|
| ESP32 | PlatformIO, Arduino framework, esp32-camera |
| ROS 2 | ROS 2 Humble (Conda robostack), pynput |
| 测试 | Python 3, Unix netcat |
| ROS 2 | ROS 2 Humble (robostack), Python 3.12 |
| 工具 | NumPy, OpenCV (可选) |
## 许可
待定。
MIT License

319
docs/ARM_CONTROL_README.md
View File

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

@@ -1,122 +0,0 @@
# 编译成功！🎉
## ✅ 已完成
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
View File

@@ -1,252 +0,0 @@
# 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
View File

@@ -1,244 +0,0 @@
# 机械臂控制 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
View File

@@ -1,359 +0,0 @@
# 视觉抓取节点使用指南
## 概述
`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
View File

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

308
ros2/README.md
View File

@@ -1,98 +1,260 @@
# ros2 — ROS 2 工作空间
# ROS 2 机械臂控制系统
## 环境搭建
CRAIC 项目的 ROS 2 机械臂控制和视觉抓取系统。
支持以下三种安装方式，任选其一。
## 📦 包含组件
### 方式一：Conda (robostack，跨平台)
### 1. arm_control_msgs
消息和服务定义包。
适用于 Linux / macOS / Windows，无需 root 权限。
**消息类型**：
- `JointState` - 关节状态
- `TCPPose` - TCP 位姿
```bash
# 安装 Miniconda
wget https://repo.anaconda.com/miniconda/Miniconda3-latest-Linux-x86_64.sh
bash Miniconda3-latest-Linux-x86_64.sh
**服务类型**：
- `MoveJoints` - 关节空间运动
- `MovePose` - 笛卡尔空间运动
- `GetPose` - 查询当前位姿
- `SetGripper` - 夹爪控制
# 创建 ROS 2 Humble 环境
conda create -n ros2_humble -c robostack-staging -c conda-forge ros-humble-desktop
conda activate ros2_humble
### 2. arm_control 节点
独立的机械臂控制节点（不依赖 tools/udp_control.py）。
# 安装构建工具
conda install -c robostack-staging -c conda-forge \
colcon-common-extensions \
ros-humble-ament-cmake \
python3-pip
**功能**：
- 关节空间和笛卡尔空间运动控制
- 完整的逆运动学和正运动学
- UDP 通信（与 ESP32）
- 状态发布（10Hz）
# Python 依赖
pip install pynput
```
### 3. vision_grasp 节点
自动化视觉抓取节点。
### 方式二：apt 原生安装 (Ubuntu 22.04)
**功能**：
- 相机坐标到基坐标系的自动转换
- 抓取流程：释放 → 移动 → 抓取 → 回收
- 释放流程：移动 → 释放 → 回收
官方推荐的 Ubuntu 安装方式，系统级集成。
## 🚀 快速开始
```bash
# 添加 ROS 2 源
sudo apt update && sudo apt install curl gnupg lsb-release
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
-o /usr/share/keyrings/ros-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" \
| sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
# 安装 ROS 2 Humble
sudo apt update
sudo apt install ros-humble-desktop python3-colcon-common-extensions
# Python 依赖
pip install pynput
# 环境配置（或写入 ~/.bashrc）
source /opt/ros/humble/setup.bash
```
### 方式三：Docker
推荐用于 CI/CD 或快速体验，无需污染宿主机环境。
```bash
# 拉取镜像
docker pull osrf/ros:humble-desktop
# 启动容器（挂载工作空间）
docker run -it --rm \
-v $(pwd)/ros2:/ws \
osrf/ros:humble-desktop \
bash
# 容器内安装依赖
apt update && apt install -y python3-colcon-common-extensions python3-pip
pip install pynput
```
## 构建
### 编译
```bash
cd ros2
colcon build --symlink-install --packages-select udp_teleop
# 设置 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
```
> `--symlink-install`：修改 Python 源文件后无需重新构建，直接生效。
## 运行
### 运行
**机械臂控制**：
```bash
ros2 run udp_teleop keyboard_control \
--ros-args --params-file src/udp_teleop/config/params.yaml
ros2 run udp_teleop arm_control \
--ros-args --params-file src/udp_teleop/config/arm_control.yaml
```
也可以通过命令行覆盖参数：
**视觉抓取**：
```bash
ros2 run udp_teleop keyboard_control \
--ros-args -p udp_ip:=192.168.1.100 -p udp_port:=9999
# 终端 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
```
## 包文档
## 📚 使用示例
详见 [src/udp_teleop/README.md](src/udp_teleop/README.md)，包含按键映射、UDP 协议、参数配置等。
### 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. **多物体处理**：队列管理和优先级排序