docs: 添加 CLAUDE.md 项目指南

为 Claude Code 提供项目概览和常用命令指南
feat: 添加方框检测与自动抓取节点
2026-06-16 21:34:38 +08:00 · 2026-06-16 21:28:09 +08:00
6 changed files with 943 additions and 1 deletions
--- a/CLAUDE.md
+++ b/CLAUDE.md
@@ -0,0 +1,242 @@
+# CLAUDE.md
+
+This file provides guidance to Claude Code (claude.ai/code) when working with code in this repository.
+
+## Project Overview
+
+CRAIC (Camera-Robot AI Control System) — Competition code for the China Robot and Artificial Intelligence Competition (中国机器人及人工智能大赛), Robot Task Challenge (Small Desktop Level).
+
+**Hardware**: ESP32-S3-WROOM-1-N16R8 with OV2640 camera, 6-DOF mechanical arm with Feettech SCS/STS serial servos.
+
+**Architecture**: Three-tier system communicating via UDP port 8888:
+1. **ESP32-S3 firmware** (`jxbeye/`) — Dual-core camera streaming + UDP command receiver
+2. **ROS 2 teleop** (`ros2/src/udp_teleop/`) — Keyboard control node for chassis and arm
+3. **Python tools** (`tools/`) — Standalone control scripts with inverse kinematics
+
+## Build & Run Commands
+
+### ESP32-S3 Firmware
+
+```bash
+cd jxbeye
+pio run -t upload          # Build and flash firmware
+pio device monitor         # Serial monitor (1000000 baud)
+```
+
+**First boot**: ESP32 creates AP `ESP32-S3-Camera` (password `12345678`), access web UI at `http://192.168.4.1`.
+
+**WiFi configuration via serial**: Send `WIFI:SSID:PASSWORD` to configure station mode.
+
+### ROS 2 Teleop
+
+```bash
+# Build (from ros2/ directory)
+conda activate ros2_humble
+colcon build --symlink-install --packages-select udp_teleop
+source install/setup.bash
+
+# Run keyboard control
+ros2 run udp_teleop keyboard_control \
+    --ros-args --params-file src/udp_teleop/config/params.yaml
+
+# Override target IP
+ros2 run udp_teleop keyboard_control \
+    --ros-args -p udp_ip:=192.168.4.1 -p udp_port:=8888
+```
+
+**Keyboard mappings**:
+- Chassis: W/S (forward/back), A/D (strafe), Q/E (rotate)
+- Arm: ↑/↓ (height), 2-6 (select joint), ←/→ (adjust angle)
+
+**Important**: Must use `ros2 run`, not `ros2 launch` — the `stdin` keyboard backend requires an interactive terminal.
+
+### Python Arm Control Tools
+
+```bash
+# Direct joint command with interpolation
+python tools/udp_control.py joints \
+    --height -100 --j2 10 --j3 20 --j4 30 \
+    --duration 1.0 --rate 20
+
+# Cartesian pose mode (uses inverse kinematics)
+python tools/udp_control.py pose \
+    --x 150 --y 50 --z -100 --phi 45 \
+    --duration 1.0
+
+# Dry run (print commands without sending)
+python tools/udp_control.py pose --x 200 --y 0 --z -50 --phi 0 --dry-run
+
+# Camera frame capture
+python tools/camera_capture.py --ip 192.168.4.1
+python tools/camera_capture.py --scan  # Auto-detect camera on subnet
+```
+
+### UDP Testing
+
+```bash
+# Start echo server
+python tools/udp_server.py
+
+# Send test commands
+echo 'XYW:100:0:0:XZHY' | nc -u 192.168.4.1 8888
+echo 'JXB:-100:10:20:30:0:0:0:0:EZHY' | nc -u 192.168.4.1 8888
+```
+
+## Architecture Details
+
+### ESP32-S3 Dual-Core Design
+
+- **Core 0**: Camera capture loop (OV2640 → JPEG)
+- **Core 1**: WiFi streaming (MJPEG HTTP server on port 80)
+- **AsyncUDP**: Non-blocking UDP command receiver runs on Core 1, handled via interrupt callbacks
+
+The dual-core split ensures camera capture never blocks on WiFi transmission. UDP commands are processed asynchronously and do not interfere with streaming.
+
+### UDP Protocol
+
+All commands are ASCII text ending with terminator (varies by command type):
+
+```
+# Chassis control (XYZ cartesian velocity)
+XYW:<X_speed>:<Y_speed>:<W_angular>:XZHY\n
+
+# Arm control (6 motors: height, J2-J6)
+JXB:<height>:<J2>:<J3>:<J4>:<J5>:<J6>:0:0:EZHY\n
+
+# Laser control
+LASERON\n
+LASEROFF\n
+
+# Serial passthrough (any payload with ZHY or \n terminator)
+<payload>ZHY\n
+```
+
+**Critical**: The ESP32 firmware parses based on terminator suffix, not command prefix.
+
+### Mechanical Arm Coordinate System & Kinematics
+
+**Coordinate frame**: Base frame with Z-axis pointing UP (not down). Origin at the bottom of the J1 linear slide.
+
+**Height coordinate (d1)**:
+- User-facing coordinate: `-290 mm` (bottom) to `0 mm` (top)
+- Physical meaning: vertical position of J2 relative to base origin
+- **Z-up convention**: positive d1 = higher position
+
+**Planar joints (J2, J3, J4)**:
+- All three rotate around vertical Z-axis in the XY plane
+- J2 is base rotation, J3/J4 are elbow/wrist
+- TCP yaw angle `phi = J2 + J3 + J4` (additive)
+
+**Geometry parameters** (see `docs/arm.md` for full derivation):
+- `L1 = 125 mm`: J2-J3 link length
+- `L2 = 125 mm`: J3-J4 link length  
+- `x4 = 110 mm`: J4-to-TCP horizontal offset
+- `z4 = 80 mm`: J4-to-TCP vertical offset (variable: 55mm when gripper down, -100mm when up)
+
+**Zero offsets**: Physical mechanical zero does not align with math zero (straight line):
+- `J2_zero = 3°`, `J3_zero = 7°`, `J4_zero = 25°`
+- UDP command angles = math angles + zero offsets
+
+**Inverse kinematics** (`tools/udp_control.py`):
+- Solves for joint angles given TCP pose `(x, y, z, phi)`
+- Uses standard 2-link planar arm solution (atan2-based)
+- Two solutions: `--elbow-up` vs default elbow-down
+- Validates workspace limits and singularity checks
+- See `docs/arm.md` for full mathematical derivation
+
+**State persistence**: `tools/.udp_control_state.json` caches last sent joint command. This enables smooth interpolated motion from previous position without re-homing. Use `--no-state-cache` to disable.
+
+### ROS 2 Keyboard Backend Selection
+
+The `udp_teleop` node supports three keyboard input backends:
+
+- `stdin` (default on Linux/macOS): Terminal raw mode, zero dependencies, **requires interactive terminal**
+- `pynput`: Cross-platform library, works in background
+- `win_poll`: Windows-specific Win32 API polling
+
+Backend auto-selected by platform. Override with `keyboard_backend` parameter in `config/params.yaml`.
+
+**Limitation**: `stdin` backend fails when launched via `ros2 launch` because child processes lack TTY. Always use `ros2 run` for interactive keyboard control.
+
+### Camera Capture Tool Auto-Detection
+
+`camera_capture.py` implements subnet scanning and ESP32 identification:
+
+1. Probes common DHCP IPs on local subnet (`x.x.x.1`, `x.x.x.100-110`, etc.)
+2. Verifies ESP32 by checking `/status` endpoint for JSON keys `capture_fps` and `has_client`
+3. Falls back to full subnet scan if not found in common range
+4. Connects to `/stream` MJPEG endpoint, parses multipart frames, extracts first valid JPEG (SOI `0xFFD8` to EOI `0xFFD9`)
+
+Use `--scan` to force full subnet scan, or `--ip` to skip detection.
+
+## Environment Setup
+
+### ROS 2 Humble via Conda (robostack)
+
+```bash
+# Create environment (one-time)
+conda create -n ros2_humble -c robostack-staging -c conda-forge ros-humble-desktop
+conda activate ros2_humble
+conda install -c robostack-staging -c conda-forge colcon-common-extensions
+pip install pynput
+
+# Every session
+conda activate ros2_humble
+cd ros2
+source install/setup.bash  # After first colcon build
+```
+
+**Alternative**: Native apt installation on Ubuntu 22.04 — see `ros2/README.md`.
+
+### PlatformIO ESP32
+
+```bash
+pip install platformio
+cd jxbeye
+pio pkg install  # Install dependencies
+```
+
+**Board configuration** (`platformio.ini`):
+- Custom board definition: `esp32-s3-wroom-1-n16r8`
+- PSRAM: Octal mode (`board_build.psram_type = octal`)
+- Flash: 16MB QIO mode at 80MHz
+- Partition table: `default_16MB.csv`
+
+## Important Files
+
+### Configuration
+
+- `jxbeye/platformio.ini` — ESP32 build config (PSRAM settings critical)
+- `ros2/src/udp_teleop/config/params.yaml` — ROS node parameters (IP, speeds, steps)
+- `tools/.udp_control_state.json` — Cached arm joint state for interpolation
+
+### Documentation
+
+- `docs/arm.md` — Full inverse kinematics derivation with LaTeX equations
+- `README.md` — Project overview and quick start
+- `ros2/src/udp_teleop/README.md` — ROS package details and keyboard mappings
+
+### Core Implementations
+
+- `jxbeye/src/main.cpp` — ESP32 dual-core firmware entry point
+- `ros2/src/udp_teleop/udp_teleop/keyboard_control.py` — ROS keyboard node
+- `tools/udp_control.py` — Standalone arm controller with full IK solver
+- `tools/camera_capture.py` — MJPEG stream frame extractor
+
+## Common Tasks
+
+**Change arm geometry parameters**: Edit constants in `tools/udp_control.py` (`DEFAULT_L1`, `DEFAULT_L2`, `DEFAULT_X4`, `DEFAULT_Z4`) or pass as CLI args.
+
+**Modify joint limits**: Edit `DEFAULT_*_MIN/MAX` in `tools/udp_control.py` or use `--height-min`, `--j2-max`, etc.
+
+**Adjust interpolation smoothness**: Change `--duration` (total time) and `--rate` (Hz) in `udp_control.py`. Default is 1.0s at 20Hz = 20 steps.
+
+**Debug UDP protocol**: Use `tools/udp_server.py` as echo server, point ROS/tools at `127.0.0.1:8888` to inspect raw commands.
+
+**Test kinematics without hardware**: Use `--dry-run` flag with `udp_control.py` to print UDP commands without sending.
+
+**Verify IK correctness**: Use `--show-fk` flag to compute forward kinematics of the solved joint angles and compare to target pose.
+
+## Dataset
+
+The `dataset.zip` and `dataset/` directory contain competition-specific training data (exact format unknown from structure alone).
--- 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
+```
--- a/ros2/src/udp_teleop/config/box_detection_grasp.yaml
+++ b/ros2/src/udp_teleop/config/box_detection_grasp.yaml
@@ -0,0 +1,26 @@
+box_detection_grasp:
+  ros__parameters:
+    # ESP32 相机流
+    stream_url: "http://192.168.4.1/stream"
+
+    # YOLO 模型路径
+    model_path: "/home/fallensigh/Dev/craic/ros2/src/udp_teleop/models/box_detection.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  # 水平视场角 (度)
+    max_frame_age: 0.5        # 最大帧延迟 (秒)
+
+    # 调试
+    show_debug_window: false  # 是否显示调试窗口
+
+    # 控制参数
+    auto_grasp: false         # 是否自动抓取（true=启动后自动检测并抓取）
--- a/ros2/src/udp_teleop/models/box_detection.pt
+++ b/ros2/src/udp_teleop/models/box_detection.pt
--- a/ros2/src/udp_teleop/setup.py
+++ b/ros2/src/udp_teleop/setup.py
@@ -15,6 +15,8 @@ setup(
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, 'config'),
            glob('config/*.yaml')),
+        (os.path.join('share', package_name, 'models'),
+            glob('models/*.pt')),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
@@ -31,7 +33,8 @@ setup(
        'console_scripts': [
            'keyboard_control = udp_teleop.keyboard_control:main',
            'arm_control = udp_teleop.arm_control:main',
-            'vision_grasp = udp_teleop.vision_grasp:main'
+            'vision_grasp = udp_teleop.vision_grasp:main',
+            'box_detection_grasp = udp_teleop.box_detection_grasp:main',
        ],
    },
 )
--- a/ros2/src/udp_teleop/udp_teleop/box_detection_grasp.py
+++ b/ros2/src/udp_teleop/udp_teleop/box_detection_grasp.py
@@ -0,0 +1,381 @@
+#!/usr/bin/env python3
+"""方框检测与自动抓取节点
+
+基于 YOLO 模型检测方框，并自动调用视觉抓取节点完成抓取。
+
+功能：
+1. 启动参数控制：auto_grasp=true 时自动抓取
+2. 实时检测方框
+3. 检测成功后停止识别
+4. 发布方框中心坐标到 /vision_grasp/grasp_target
+5. 等待抓取完成
+"""
+
+import math
+import threading
+import time
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Point
+from std_srvs.srv import Trigger
+
+try:
+    import cv2
+except ImportError:
+    cv2 = None
+
+try:
+    import numpy as np
+except ImportError:
+    np = None
+
+try:
+    from ultralytics import YOLO
+except ImportError:
+    YOLO = None
+
+
+class MjpegFrameReader:
+    """持续读取 ESP32 MJPEG 流并保持最新帧"""
+
+    def __init__(self, url, logger, reconnect_delay=1.0):
+        self.url = url
+        self.logger = logger
+        self.reconnect_delay = reconnect_delay
+        self.lock = threading.Lock()
+        self.latest_frame = None
+        self.latest_stamp = None
+        self.running = False
+        self.thread = None
+        self.capture = None
+        self.last_log_time = 0.0
+
+    def start(self):
+        self.running = True
+        self.thread = threading.Thread(target=self._run, daemon=True)
+        self.thread.start()
+
+    def stop(self):
+        self.running = False
+        if self.thread is not None:
+            self.thread.join(timeout=2.0)
+        if self.capture is not None:
+            self.capture.release()
+            self.capture = None
+
+    def get_latest(self):
+        with self.lock:
+            if self.latest_frame is None:
+                return None, None
+            return self.latest_frame.copy(), self.latest_stamp
+
+    def _log_periodic(self, message, period=5.0):
+        now = time.monotonic()
+        if now - self.last_log_time >= period:
+            self.logger.warning(message)
+            self.last_log_time = now
+
+    def _open_capture(self):
+        cap = cv2.VideoCapture(self.url)
+        if not cap.isOpened():
+            cap.release()
+            return None
+
+        cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
+        self.logger.info(f"已连接到 ESP32 流: {self.url}")
+        return cap
+
+    def _run(self):
+        while self.running:
+            if self.capture is None:
+                self.capture = self._open_capture()
+                if self.capture is None:
+                    self._log_periodic(f"无法打开 ESP32 流 {self.url}，重试中...")
+                    time.sleep(self.reconnect_delay)
+                    continue
+
+            ok, frame = self.capture.read()
+            if not ok or frame is None:
+                self._log_periodic("ESP32 流读取失败，重新连接中...")
+                self.capture.release()
+                self.capture = None
+                time.sleep(self.reconnect_delay)
+                continue
+
+            with self.lock:
+                self.latest_frame = frame
+                self.latest_stamp = time.monotonic()
+
+
+class BoxDetectionGraspNode(Node):
+    """方框检测与自动抓取节点"""
+
+    def __init__(self):
+        super().__init__('box_detection_grasp')
+
+        # 检查依赖
+        if cv2 is None:
+            raise RuntimeError("需要 OpenCV。安装: pip install opencv-python")
+        if np is None:
+            raise RuntimeError("需要 NumPy。安装: pip install numpy")
+        if YOLO is None:
+            raise RuntimeError("需要 ultralytics。安装: pip install ultralytics")
+
+        # 声明参数
+        self.declare_parameter('stream_url', 'http://192.168.4.1/stream')
+        self.declare_parameter('model_path', '/home/fallensigh/Dev/craic/tmp/best.pt')
+        self.declare_parameter('confidence', 0.35)
+        self.declare_parameter('imgsz', 768)
+        self.declare_parameter('device', '')
+        self.declare_parameter('detection_rate', 10.0)
+        self.declare_parameter('box_size_m', 0.03)
+        self.declare_parameter('horizontal_fov_deg', 66.0)
+        self.declare_parameter('max_frame_age', 0.5)
+        self.declare_parameter('show_debug_window', False)
+        self.declare_parameter('auto_grasp', False)  # 是否自动抓取
+
+        # 获取参数
+        self.stream_url = self.get_parameter('stream_url').value
+        self.model_path = self.get_parameter('model_path').value
+        self.confidence = float(self.get_parameter('confidence').value)
+        self.imgsz = int(self.get_parameter('imgsz').value)
+        self.device = str(self.get_parameter('device').value)
+        self.detection_rate = float(self.get_parameter('detection_rate').value)
+        self.box_size_m = float(self.get_parameter('box_size_m').value)
+        self.horizontal_fov_deg = float(self.get_parameter('horizontal_fov_deg').value)
+        self.max_frame_age = float(self.get_parameter('max_frame_age').value)
+        self.show_debug_window = bool(self.get_parameter('show_debug_window').value)
+        self.auto_grasp = bool(self.get_parameter('auto_grasp').value)
+
+        # 状态标志
+        self.detection_active = self.auto_grasp  # 如果 auto_grasp=true，自动开始检测
+        self.grasp_in_progress = False
+
+        # 发布器：发布到视觉抓取节点
+        self.grasp_target_pub = self.create_publisher(Point, '/vision_grasp/grasp_target', 10)
+
+        # 服务：外部触发检测
+        self.start_detection_srv = self.create_service(
+            Trigger,
+            'box_detection/start',
+            self.handle_start_detection
+        )
+        self.stop_detection_srv = self.create_service(
+            Trigger,
+            'box_detection/stop',
+            self.handle_stop_detection
+        )
+
+        # 启动相机流读取器
+        self.reader = MjpegFrameReader(self.stream_url, self.get_logger())
+        self.reader.start()
+
+        # 加载 YOLO 模型
+        self.get_logger().info(f'加载 YOLO 模型: {self.model_path}')
+        self.model = YOLO(self.model_path)
+        self.last_no_detection_log = 0.0
+
+        # 创建定时器
+        period = 1.0 / max(self.detection_rate, 0.1)
+        self.timer = self.create_timer(period, self.process_frame)
+
+        if self.auto_grasp:
+            self.get_logger().info('自动抓取模式已启动，开始实时检测方框...')
+        else:
+            self.get_logger().info('手动模式，等待外部触发检测')
+
+    def process_frame(self):
+        """处理最新帧"""
+        if not self.detection_active or self.grasp_in_progress:
+            return
+
+        frame, frame_stamp = self.reader.get_latest()
+        if frame is None:
+            return
+
+        # 检查帧是否过期
+        if frame_stamp is not None and time.monotonic() - frame_stamp > self.max_frame_age:
+            return
+
+        # YOLO 检测
+        predict_kwargs = {
+            'source': frame,
+            'imgsz': self.imgsz,
+            'conf': self.confidence,
+            'verbose': False,
+        }
+        if self.device:
+            predict_kwargs['device'] = self.device
+
+        results = self.model.predict(**predict_kwargs)
+        detection = self.select_best_box(results[0], frame)
+
+        if detection is None:
+            self.log_no_detection()
+            if self.show_debug_window:
+                cv2.imshow('box_detection', frame)
+                cv2.waitKey(1)
+            return
+
+        # 估计 3D 坐标
+        point = self.estimate_camera_point(detection, frame.shape)
+        if point is None:
+            return
+
+        # 检测成功！停止检测并触发抓取
+        self.get_logger().info(f'✓ 检测到方框: 相机坐标 ({point[0]*1000:.1f}, {point[1]*1000:.1f}, {point[2]*1000:.1f}) mm')
+        self.detection_active = False  # 停止检测
+        self.grasp_in_progress = True
+
+        if self.show_debug_window:
+            self.show_debug_frame(frame, detection, point)
+
+        # 发布抓取目标（转换为 mm）
+        msg = Point()
+        msg.x = point[0] * 1000.0  # m -> mm
+        msg.y = point[1] * 1000.0
+        msg.z = point[2] * 1000.0
+        self.grasp_target_pub.publish(msg)
+        self.get_logger().info('已发布抓取目标到 /vision_grasp/grasp_target')
+
+        # 在独立线程中等待抓取完成
+        threading.Thread(target=self._wait_for_grasp_completion, daemon=True).start()
+
+    def select_best_box(self, result, frame):
+        """选择最佳检测框"""
+        if result.boxes is None or len(result.boxes) == 0:
+            return None
+
+        best = None
+        best_conf = -1.0
+
+        for box in result.boxes:
+            conf = float(box.conf[0].item()) if box.conf is not None else 0.0
+            xyxy = box.xyxy[0].detach().cpu().numpy().astype(float)
+            x1, y1, x2, y2 = xyxy
+            w = max(0.0, x2 - x1)
+            h = max(0.0, y2 - y1)
+
+            if w < 2.0 or h < 2.0:
+                continue
+
+            if conf > best_conf:
+                best_conf = conf
+                best = {
+                    'xyxy': xyxy,
+                    'confidence': conf,
+                }
+
+        return best
+
+    def estimate_camera_point(self, detection, frame_shape):
+        """估计相机坐标系 3D 坐标（单位：米）"""
+        frame_h, frame_w = frame_shape[:2]
+        x1, y1, x2, y2 = detection['xyxy']
+        bbox_w = max(1.0, x2 - x1)
+        bbox_h = max(1.0, y2 - y1)
+        pixel_side = (bbox_w + bbox_h) * 0.5
+
+        # 相机内参
+        cx = frame_w * 0.5
+        cy = frame_h * 0.5
+        fov_rad = math.radians(max(1.0, min(self.horizontal_fov_deg, 179.0)))
+        fx = frame_w / (2.0 * math.tan(fov_rad * 0.5))
+        fy = fx
+
+        if fx <= 0.0 or fy <= 0.0:
+            self.get_logger().error('无效的相机内参')
+            return None
+
+        # 估计深度
+        focal = (fx + fy) * 0.5
+        z = self.box_size_m * focal / pixel_side
+
+        # 计算 3D 坐标
+        u = (x1 + x2) * 0.5
+        v = (y1 + y2) * 0.5
+        x = (u - cx) * z / fx
+        y = (v - cy) * z / fy
+
+        return x, y, z
+
+    def show_debug_frame(self, frame, detection, point):
+        """显示调试窗口"""
+        x1, y1, x2, y2 = detection['xyxy'].astype(int)
+        cv2.rectangle(frame, (x1, y1), (x2, y2), (0, 255, 0), 2)
+        text = f"x={point[0]*1000:.1f} y={point[1]*1000:.1f} z={point[2]*1000:.1f} mm"
+        cv2.putText(
+            frame, text, (x1, max(20, y1 - 8)),
+            cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1, cv2.LINE_AA
+        )
+        cv2.imshow('box_detection', frame)
+        cv2.waitKey(1)
+
+    def log_no_detection(self):
+        """周期性记录未检测到目标"""
+        now = time.monotonic()
+        if now - self.last_no_detection_log >= 5.0:
+            self.get_logger().info('当前帧未检测到方框')
+            self.last_no_detection_log = now
+
+    def _wait_for_grasp_completion(self):
+        """等待抓取完成（独立线程）"""
+        self.get_logger().info('等待抓取完成...')
+        time.sleep(10.0)  # 等待抓取流程完成
+        self.grasp_in_progress = False
+        self.get_logger().info('抓取流程完成，节点已停止检测')
+
+    def handle_start_detection(self, request, response):
+        """处理启动检测服务"""
+        if self.detection_active:
+            response.success = False
+            response.message = '检测已在运行中'
+            return response
+
+        if self.grasp_in_progress:
+            response.success = False
+            response.message = '抓取流程正在进行中，请等待完成'
+            return response
+
+        self.detection_active = True
+        self.get_logger().info('✓ 启动方框检测')
+        response.success = True
+        response.message = '已启动检测'
+        return response
+
+    def handle_stop_detection(self, request, response):
+        """处理停止检测服务"""
+        if not self.detection_active:
+            response.success = False
+            response.message = '检测未运行'
+            return response
+
+        self.detection_active = False
+        self.get_logger().info('✓ 停止方框检测')
+        response.success = True
+        response.message = '已停止检测'
+        return response
+
+    def destroy_node(self):
+        """节点销毁时的清理"""
+        self.reader.stop()
+        if self.show_debug_window:
+            cv2.destroyAllWindows()
+        super().destroy_node()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = BoxDetectionGraspNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()