# 方框检测与自动抓取节点

基于 YOLO 模型检测方框，并自动调用视觉抓取节点完成抓取。

## 功能特性

- ✅ 实时 YOLO 方框检测
- ✅ 自动/手动模式切换
- ✅ 检测成功后自动停止识别
- ✅ 自动发布抓取目标到 `/vision_grasp/grasp_target`
- ✅ 3D 坐标估计（基于方框尺寸）

## 快速开始

### 1. 启动必要的节点

```bash
# 终端 1: 启动 arm_control 节点
ros2 run udp_teleop arm_control \
    --ros-args --params-file src/udp_teleop/config/arm_control.yaml

# 终端 2: 启动 vision_grasp 节点
ros2 run udp_teleop vision_grasp \
    --ros-args --params-file src/udp_teleop/config/vision_grasp.yaml
```

### 2. 启动方框检测节点

#### 自动模式（检测到方框后自动抓取）

```bash
ros2 run udp_teleop box_detection_grasp \
    --ros-args --params-file src/udp_teleop/config/box_detection_grasp.yaml \
    -p auto_grasp:=true
```

#### 手动模式（仅检测，不自动抓取）

```bash
ros2 run udp_teleop box_detection_grasp \
    --ros-args --params-file src/udp_teleop/config/box_detection_grasp.yaml \
    -p auto_grasp:=false
```

## 工作流程

### 自动模式 (auto_grasp=true)

1. 节点启动后立即开始实时检测
2. 检测到方框后：
   - 停止检测（避免重复）
   - 计算 3D 坐标（相机坐标系，单位：mm）
   - 发布到 `/vision_grasp/grasp_target`
   - vision_grasp 节点接收并执行抓取
3. 等待 10 秒后完成（可自定义）
4. 节点保持运行但不再检测

### 手动模式 (auto_grasp=false)

- 节点启动后不检测
- 通过服务触发检测：

```bash
# 启动检测
ros2 service call /box_detection/start std_srvs/srv/Trigger

# 停止检测
ros2 service call /box_detection/stop std_srvs/srv/Trigger
```

## 服务接口

### `/box_detection/start`

启动方框检测。

**类型**：`std_srvs/srv/Trigger`

**示例**：
```bash
ros2 service call /box_detection/start std_srvs/srv/Trigger
```

**响应**：
- `success: true` - 检测已启动
- `success: false` - 检测已在运行或抓取进行中

### `/box_detection/stop`

停止方框检测。

**类型**：`std_srvs/srv/Trigger`

**示例**：
```bash
ros2 service call /box_detection/stop std_srvs/srv/Trigger
```

**响应**：
- `success: true` - 检测已停止
- `success: false` - 检测未运行

## 使用场景

### 场景 1：自动抓取流水线

```bash
# 启动自动模式
ros2 run udp_teleop box_detection_grasp \
    --ros-args --params-file src/udp_teleop/config/box_detection_grasp.yaml \
    -p auto_grasp:=true

# 节点自动检测并抓取，无需人工干预
```

### 场景 2：手动触发抓取

```bash
# 终端 1: 启动手动模式
ros2 run udp_teleop box_detection_grasp \
    --ros-args --params-file src/udp_teleop/config/box_detection_grasp.yaml \
    -p auto_grasp:=false

# 终端 2: 需要抓取时手动触发
ros2 service call /box_detection/start std_srvs/srv/Trigger

# 检测到方框后自动抓取，完成后停止
# 需要再次抓取时，再次调用 start 服务
ros2 service call /box_detection/start std_srvs/srv/Trigger
```

### 场景 3：紧急停止

```bash
# 在检测过程中紧急停止
ros2 service call /box_detection/stop std_srvs/srv/Trigger
```

## 配置参数

编辑 `config/box_detection_grasp.yaml`：

```yaml
box_detection_grasp:
  ros__parameters:
    # 相机流
    stream_url: "http://192.168.4.1/stream"
    
    # 模型路径
    model_path: "/path/to/model.pt"
    
    # 检测参数
    confidence: 0.35        # 置信度阈值
    imgsz: 768             # YOLO 输入尺寸
    device: ""             # "cpu", "cuda", 或 "" (自动)
    detection_rate: 10.0   # 检测频率 (Hz)
    
    # 方框尺寸（用于深度估计）
    box_size_m: 0.03       # 方框边长 (米)
    
    # 相机参数
    horizontal_fov_deg: 66.0  # 水平视场角
    
    # 调试
    show_debug_window: false  # 显示检测窗口
    
    # 控制
    auto_grasp: false      # 自动抓取开关
```

## 调试

### 显示检测窗口

```bash
ros2 run udp_teleop box_detection_grasp \
    --ros-args --params-file src/udp_teleop/config/box_detection_grasp.yaml \
    -p auto_grasp:=true \
    -p show_debug_window:=true
```

### 监控话题

```bash
# 查看抓取目标
ros2 topic echo /vision_grasp/grasp_target

# 查看日志
ros2 node list
ros2 node info /box_detection_grasp
```

## 坐标系说明

- **输入**：YOLO 检测框（图像像素坐标）
- **输出**：相机坐标系 3D 坐标（单位：mm）
  - X: 向右
  - Y: 向下
  - Z: 向前（深度）
- **传递**：vision_grasp 节点自动转换到基坐标系

## 深度估计原理

使用透视投影原理：

```
Z = (实际尺寸 × 焦距) / 像素尺寸
X = (u - cx) × Z / fx
Y = (v - cy) × Z / fy
```

其中：
- `box_size_m` = 方框实际边长（米）
- `horizontal_fov_deg` = 相机水平视场角

## 故障排除

### 问题：无法连接到 ESP32 相机

**解决**：
1. 检查 ESP32 IP 地址：`ping 192.168.4.1`
2. 浏览器访问：`http://192.168.4.1`
3. 修改配置：`-p stream_url:=http://<IP>/stream`

### 问题：检测不到方框

**解决**：
1. 降低置信度：`-p confidence:=0.25`
2. 检查模型路径：`ls -lh src/udp_teleop/models/box_detection.pt`
3. 启用调试窗口：`-p show_debug_window:=true`

### 问题：深度估计不准确

**解决**：
1. 校准方框尺寸：`-p box_size_m:=0.03`
2. 校准相机 FOV：`-p horizontal_fov_deg:=66.0`

## 依赖

- ROS 2 Humble
- Python 3.8+
- OpenCV (`pip install opencv-python`)
- NumPy (`pip install numpy`)
- Ultralytics YOLO (`pip install ultralytics`)

## 示例：完整启动脚本

```bash
#!/bin/bash
# 启动完整的方框检测与抓取系统

# 确保在 ros2 目录
cd ~/Dev/craic/ros2
source install/setup.bash

# 启动 arm_control
gnome-terminal -- bash -c "
    source install/setup.bash
    ros2 run udp_teleop arm_control \
        --ros-args --params-file src/udp_teleop/config/arm_control.yaml
    exec bash"

sleep 2

# 启动 vision_grasp
gnome-terminal -- bash -c "
    source install/setup.bash
    ros2 run udp_teleop vision_grasp \
        --ros-args --params-file src/udp_teleop/config/vision_grasp.yaml
    exec bash"

sleep 2

# 启动方框检测（自动抓取模式）
gnome-terminal -- bash -c "
    source install/setup.bash
    ros2 run udp_teleop box_detection_grasp \
        --ros-args --params-file src/udp_teleop/config/box_detection_grasp.yaml \
        -p auto_grasp:=true \
        -p show_debug_window:=true
    exec bash"

echo "所有节点已启动！"
```

保存为 `launch_box_grasp.sh` 并运行：
```bash
chmod +x launch_box_grasp.sh
./launch_box_grasp.sh
```