craic/docs/box_detection_grasp.md

方框检测与自动抓取节点

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

功能特性

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

快速开始

1. 启动必要的节点

# 终端 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. 启动方框检测节点

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

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

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

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)

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

# 启动检测
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

示例：

ros2 service call /box_detection/start std_srvs/srv/Trigger

响应：

• success: true - 检测已启动
• success: false - 检测已在运行或抓取进行中

/box_detection/stop

停止方框检测。

类型：std_srvs/srv/Trigger

示例：

ros2 service call /box_detection/stop std_srvs/srv/Trigger

响应：

• success: true - 检测已停止
• success: false - 检测未运行

使用场景

场景 1：自动抓取流水线

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

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

场景 2：手动触发抓取

# 终端 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：紧急停止

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

配置参数

编辑 config/box_detection_grasp.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      # 自动抓取开关

调试

显示检测窗口

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

监控话题

# 查看抓取目标
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)

示例：完整启动脚本

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

chmod +x launch_box_grasp.sh
./launch_box_grasp.sh