craic/docs/VISION_GRASP_SUMMARY.md

视觉抓取节点 - 完成总结

✅ 完成的工作

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 节点

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 节点

cd ros2
source install/setup.bash

ros2 run udp_teleop vision_grasp \
    --ros-args --params-file src/udp_teleop/config/vision_grasp.yaml

终端 3：测试

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

或使用话题发布

# 抓取
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. 参数化配置

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 脚本集成

#!/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. 集成物体检测

# 订阅相机图像
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 中的相似三角形方法：

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. 多物体抓取

# 创建队列
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

系统就会自动完成抓取！