craic/docs/VISION_GRASP_README.md

视觉抓取节点使用指南

概述

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)

编译

cd ros2
colcon build --packages-select udp_teleop
source install/setup.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

使用

方法 1：发布话题触发抓取

# 抓取：输入相机坐标
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 脚本集成

#!/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：与检测节点集成

#!/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：

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 向下（负号修正方向）
• 然后再转换到基坐标系

调试

查看节点状态

# 查看节点列表
ros2 node list

# 查看话题列表
ros2 topic list | grep vision_grasp

# 监听抓取目标
ros2 topic echo /vision_grasp/grasp_target

测试流程

1. 启动节点

   # 终端 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. 发送测试抓取

   # 终端 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. 关节限位是否合理

扩展功能

添加安全检查

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
    
    # 执行抓取...

添加碰撞检测

def is_path_safe(self, start, end):
    # 检查路径是否安全
    # TODO: 实现碰撞检测逻辑
    return True

多物体抓取

# 订阅物体列表
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 显示检测结果和机械臂状态）