# 视觉抓取节点 - 完成总结

## ✅ 完成的工作

### 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`

系统就会自动完成抓取！