feat: 添加方框检测与自动抓取节点

功能： - 基于 YOLO 实时检测方框 - 自动模式：启动后自动检测并抓取 - 手动模式：通过服务触发检测 - 检测成功后自动停止并发布抓取目标 - 3D 坐标估计（基于方框尺寸和相机 FOV）节点：box_detection_grasp - 话题：/vision_grasp/grasp_target (发布) - 服务：/box_detection/start (启动检测) - 服务：/box_detection/stop (停止检测) 配置： - auto_grasp: 自动/手动模式切换 - box_size_m: 方框尺寸（用于深度估计） - show_debug_window: 调试窗口使用： ros2 run udp_teleop box_detection_grasp \ --ros-args --params-file config/box_detection_grasp.yaml \ -p auto_grasp:=true 详细文档：docs/box_detection_grasp.md
2026-06-16 21:28:09 +08:00
parent aa1bc2bf75
commit a832dfaeb1
5 changed files with 701 additions and 1 deletions
--- a/docs/box_detection_grasp.md
+++ b/docs/box_detection_grasp.md
@@ -0,0 +1,290 @@
+# 方框检测与自动抓取节点
+
+基于 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
+```