# 视觉抓取节点使用指南

## 概述

`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 显示检测结果和机械臂状态）