FallenSigh
83b32542ef
- 创建 arm_control_msgs 包：定义机械臂控制的消息和服务接口 - 消息：JointState, TCPPose - 服务：MoveJoints, MovePose, GetPose, SetGripper - 实现 arm_control 节点：独立的机械臂控制 ROS 节点 - 完整的逆运动学和正运动学 - 关节空间和笛卡尔空间运动控制 - UDP 通信与 ESP32 - 状态发布（10Hz） - 实现 vision_grasp 节点：自动化视觉抓取 - 相机坐标系到基坐标系的完整变换 - 自动抓取流程：释放→移动→抓取→回收 - 自动释放流程：移动→释放→回收 - 多线程执行器支持 - 添加完整文档 - ARM_CONTROL_README.md: 机械臂控制节点使用指南 - VISION_GRASP_README.md: 视觉抓取节点使用指南 - QUICKSTART.md: 快速开始指南 - 文档重命名：docs/craic.md → docs/arm.md
360 lines
8.0 KiB
Markdown
360 lines
8.0 KiB
Markdown
# 视觉抓取节点使用指南
|
||
|
||
## 概述
|
||
|
||
`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 显示检测结果和机械臂状态）
|