上决策

- 在 optimal_planner.h 中添加缺失的 node_ 成员变量声明
- 修复 teb_local_planner_ros.cpp 中的异常类型（ControllerException -> PlannerException）

这修复了启动时的符号未定义错误：
undefined symbol: _ZN17teb_local_planner17TebOptimalPlannerC1E...

Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>

.github/copilot-instructions.md

@@ -0,0 +1,6 @@
+---
+description: 当助手已完成当前请求的工作、准备结束回复或不再有明确下一步可执行内容时，必须先调用提问工具 askQuestions 获取进一步指示；不得直接结束对话。 / When the assistant has completed the current task and is about to end the reply or has no clear next action, it must call the askQuestions tool to obtain further instructions instead of ending the conversation.
+applyTo: ‘当检测到回复将以收尾语句结束或无后续动作时 / When a reply is about to end with a closing statement or has no follow-up action’
+---
+当你完成工作时，请调用提问工具（askQuestions）来获取进一步的指示，而不是直接结束对话。
+When you finish the task, call the askQuestions tool to request further instructions instead of ending the conversation.

.gitignore

@@ -122,7 +122,7 @@ devel_isolated/
 CMakeLists.txt.user
 
 srv/_*.py
-# *.pcd
+
 *.pyc
 qtcreator-*
 *.user
@@ -156,7 +156,7 @@ build/
 CMakeLists.txt.user
 
 srv/_*.py
-# *.pcd
+
 *.pyc
 qtcreator-*
 *.user
@@ -170,4 +170,3 @@ qtcreator-*
 COLCON_IGNORE
 AMENT_IGNORE
 
-.claude/

.vscode/settings.json

@@ -0,0 +1,3 @@
+{
+    "cmake.sourceDirectory": "/home/robofish/MOVE_AI/src/rm_decision"
+}

CHANGELOG.md

@@ -0,0 +1,142 @@
+# 更新日志
+
+## 2026-03-05 - Sentry 分支重大更新
+
+### 🎯 核心架构重构
+
+#### 1. 决策节点 (rm_decision)
+- ✅ 新增决策节点作为系统核心控制器
+- ✅ 整合视觉自瞄控制（云台）和运动控制（底盘）
+- ✅ 订阅话题：`/data_mcu`, `/data_ref`, `/vision/aim`, `/nav_status`, `/cmd_vel_move`
+- ✅ 发布话题：`/data_ai` (综合控制), `/nav_goal` (导航目标)
+- ✅ 50Hz 控制频率，实时决策
+
+#### 2. 消息系统重构
+- ✅ 新增 `DataAim.msg` - 视觉自瞄控制（仅云台）
+- ✅ 新增 `ChassisMode.msg` - 底盘模式定义
+- ✅ 新增 `GimbalState.msg` - 云台状态数据
+- ✅ 修改 `DataAI.msg` - 综合控制指令（云台+底盘）
+
+#### 3. 视觉系统 ROS2 集成
+- ✅ 创建 `GimbalROS` 适配器，替代直接串口通信
+- ✅ 订阅 `/data_mcu` 获取云台状态
+- ✅ 发布 `/vision/aim` 输出云台控制指令
+- ✅ 修改所有视觉任务文件使用 ROS2 接口
+- ✅ 创建 `rm_vision_bringup` 包，支持标准 ROS2 启动
+
+#### 4. 视觉系统标准化
+- ✅ 添加 `package.xml` 使 rm_vision 成为标准 ROS2 包
+- ✅ 添加 install 规则，可执行文件和配置文件正确安装
+- ✅ 创建 `sentry_vision.launch.py` - Sentry 专用启动文件
+- ✅ 创建 `calibration.launch.py` - 统一标定工具启动
+- ✅ 支持参数化启动不同类型的视觉程序
+
+#### 5. 运动控制增强
+- ✅ `rm_simple_move` 订阅 `/cmd_vel_move` 输出底盘速度
+- ✅ 决策节点整合底盘速度到 `/data_ai` 消息
+
+### 📊 数据流架构
+
+```
+MCU (下位机)
+  ↕ 串口
+Serial Driver
+  ↓ /data_mcu, /data_ref, /gimbal/state
+  ↓
+Vision System ──→ /vision/aim ──→ Decision Node ←── /nav_status
+                                        ↓              ↑
+                                   /nav_goal      /cmd_vel_move
+                                        ↓              ↑
+                                   Simple Move ────────┘
+                                        ↓
+                                   Nav2 Stack
+```
+
+### 🚀 启动方式改进
+
+**之前（复杂）：**
+```bash
+cd build/mr_vision && ./standard_mpc configs/standard4.yaml
+```
+
+**现在（标准化）：**
+```bash
+ros2 launch rm_vision_bringup sentry_vision.launch.py
+```
+
+### 🎯 Sentry 专用支持
+
+- ✅ 默认使用 `sentry.yaml` 配置
+- ✅ 专用启动文件 `sentry_vision.launch.py`
+- ✅ 完整的标定工具链支持
+
+### 📝 文档更新
+
+- ✅ 完善 README.md，添加完整的系统架构图
+- ✅ 添加详细的启动流程说明
+- ✅ 添加话题接口文档
+- ✅ 添加标定工具使用指南
+- ✅ 添加测试命令和调试技巧
+
+### 🔧 技术改进
+
+- ✅ 所有模块统一使用 ROS2 标准接口
+- ✅ 消息类型明确分离（云台/底盘）
+- ✅ 决策节点作为唯一的控制指令输出点
+- ✅ 支持 50Hz 高频控制循环
+- ✅ 线程安全的数据访问（mutex 保护）
+
+### 📦 新增包
+
+- `rm_decision` - 决策节点
+- `rm_vision_bringup` - 视觉系统启动包
+
+### 🐛 修复问题
+
+- ✅ 修复视觉系统 ROS2 类型支持库链接问题
+- ✅ 修复 `capture.cpp` 缺少 node 参数传递
+- ✅ 修复多个视觉任务文件缺少 ROS2 头文件
+- ✅ 修复 launch 文件可执行文件路径错误
+
+### 📋 待完成
+
+- ⏳ 一键启动脚本 `full_system.launch.py`
+- ⏳ 完整的系统测试和验证
+- ⏳ 性能优化和参数调优
+
+---
+
+## 使用说明
+
+### 完整启动流程
+
+```bash
+# 1. 串口驱动
+ros2 launch rm_serial_driver serial_driver.launch.py
+
+# 2. 导航系统
+ros2 launch rm_nav_bringup nav.launch.py
+
+# 3. Sentry 视觉系统
+ros2 launch rm_vision_bringup sentry_vision.launch.py
+
+# 4. 决策节点
+ros2 launch rm_decision decision.launch.py
+```
+
+### 标定工具
+
+```bash
+# 采集标定图像
+ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=capture
+
+# 标定相机
+ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=calibrate_camera
+
+# 手眼标定
+ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=calibrate_handeye
+```
+
+---
+
+**青岛理工大学 QUT MOVE 战队** | RoboMaster 2026

COMPLETE_STARTUP_GUIDE.md

@@ -0,0 +1,392 @@
+# 完整启动流程文档
+
+## 系统架构
+
+```
+┌─────────────────┐
+│   rm_vision     │ 视觉节点
+│  (GimbalROS)    │
+└────────┬────────┘
+         │ /gimbal/vision (DataAI)
+         │ 云台控制指令
+         ↓
+┌─────────────────┐
+│ rm_serial_driver│ 串口驱动
+└────────┬────────┘
+         │ 串口
+         ↓
+┌─────────────────┐
+│      MCU        │ 下位机
+└─────────────────┘
+         ↑
+         │ /data_mcu (DataMCU)
+         │ 云台状态反馈
+         │
+┌─────────────────┐
+│  rm_simple_move │ 导航节点
+└─────────────────┘
+         │ /cmd_vel_move (Twist)
+         │ 底盘控制指令
+         ↓
+┌─────────────────┐
+│ rm_serial_driver│
+└─────────────────┘
+```
+
+## 话题映射表
+
+| 话题 | 类型 | 方向 | 说明 |
+|------|------|------|------|
+| `/data_mcu` | `rm_msgs/DataMCU` | MCU → 上位机 | 云台状态（姿态、弹速等） |
+| `/data_ref` | `rm_msgs/DataRef` | MCU → 上位机 | 裁判系统数据 |
+| `/gimbal/vision` | `rm_msgs/DataAI` | 视觉 → MCU | 视觉云台控制指令 |
+| `/cmd_vel_move` | `geometry_msgs/Twist` | 导航 → MCU | 底盘运动控制指令 |
+| `/nav_goal` | `rm_msgs/NavGoal` | 决策 → 导航 | 导航目标点 |
+| `/nav_status` | `rm_msgs/NavStatus` | 导航 → 决策 | 导航状态反馈 |
+| `/current_pose` | `geometry_msgs/PoseStamped` | 导航 → 决策 | 当前位置 |
+
+## 完整启动流程
+
+### 步骤 1: 编译所有包
+
+```bash
+cd ~/MOVE_AI
+
+# 编译消息包
+colcon build --packages-select rm_msgs
+
+# 编译串口驱动
+colcon build --packages-select rm_serial_driver
+
+# 编译导航节点
+colcon build --packages-select rm_simpal_move
+
+# 编译视觉（如果需要）
+colcon build --packages-select mr_vision
+
+# 加载环境
+source install/setup.bash
+```
+
+### 步骤 2: 启动串口驱动（核心节点）
+
+**终端 1：启动 rm_serial_driver**
+
+```bash
+source install/setup.bash
+ros2 run rm_serial_driver rm_serial_driver --ros-args \
+  -p device_name:=/dev/ttyACM0 \
+  -p baud_rate:=115200
+```
+
+**预期输出：**
+```
+========================================
+RM Serial Driver 启动中...
+  设备: /dev/ttyACM0
+  波特率: 115200
+========================================
+话题配置:
+  发布: /data_mcu (MCU状态)
+  发布: /data_ref (裁判系统)
+  订阅: /gimbal/vision (视觉控制)
+  订阅: /cmd_vel_move (导航控制)
+✓ 串口打开成功
+RM Serial Driver 启动完成！
+```
+
+**故障排查：**
+```bash
+# 如果串口打开失败，检查设备
+ls -l /dev/ttyACM*
+ls -l /dev/ttyUSB*
+
+# 添加串口权限
+sudo usermod -a -G dialout $USER
+# 重新登录生效
+
+# 或临时授权
+sudo chmod 666 /dev/ttyACM0
+```
+
+### 步骤 3: 验证串口通信
+
+**终端 2：查看 MCU 数据**
+
+```bash
+source install/setup.bash
+ros2 topic echo /data_mcu
+```
+
+**预期输出：**
+```yaml
+mode: 0
+q0: 1.0
+q1: 0.0
+q2: 0.0
+q3: 0.0
+yaw: 0.0
+yaw_vel: 0.0
+pitch: 0.0
+pitch_vel: 0.0
+bullet_speed: 15.0
+bullet_count: 0
+---
+```
+
+如果能看到数据，说明串口通信正常！
+
+### 步骤 4: 启动导航节点（可选）
+
+**终端 3：启动 rm_simple_move**
+
+```bash
+source install/setup.bash
+ros2 launch rm_simpal_move simple_move.launch.py
+```
+
+**预期输出：**
+```
+[rm_simple_move]: 话题配置:
+[rm_simple_move]:   输入: /nav_goal
+[rm_simple_move]:   输出: /cmd_vel_move (统一输出)
+[rm_simple_move]:   PID模式: 直接计算发布到 /cmd_vel_move
+[rm_simple_move]:   NAV模式: Action /navigate_to_pose → 导航 → /cmd_vel → 转发到 /cmd_vel_move
+[rm_simple_move]:   状态: /nav_status, /current_pose
+[rm_simple_move]: RMSimpleMove 启动
+```
+
+### 步骤 5: 启动视觉节点（如果需要）
+
+**终端 4：启动视觉**
+
+```bash
+source install/setup.bash
+
+# 方式 1: 使用已修复的 standard_mpc
+./build/mr_vision/standard_mpc configs/standard.yaml
+
+# 方式 2: 使用其他视觉节点（需要先修复）
+# ./build/mr_vision/balance_infantry_mpc configs/balance_infantry.yaml
+```
+
+**预期输出：**
+```
+[GimbalROS] Initialized with ROS2 topics
+[GimbalROS]   Subscribe: /data_mcu
+[GimbalROS]   Publish: /gimbal/vision
+[GimbalROS] Waiting for first quaternion...
+[GimbalROS] First q received.
+```
+
+## 测试流程
+
+### 测试 1: 验证话题连接
+
+```bash
+# 查看所有话题
+ros2 topic list
+
+# 应该看到：
+# /data_mcu
+# /data_ref
+# /gimbal/vision
+# /cmd_vel_move
+# /nav_goal
+# /nav_status
+# /current_pose
+
+# 查看话题信息
+ros2 topic info /gimbal/vision
+ros2 topic info /data_mcu
+```
+
+### 测试 2: 测试导航功能
+
+```bash
+# PID 模式导航到 (2, 2)
+ros2 topic pub /nav_goal rm_msgs/msg/NavGoal "{
+  mode: 0,
+  target_x: 2.0,
+  target_y: 2.0,
+  target_angle: 0.0,
+  max_speed: 1.0,
+  tolerance: 0.2
+}" --once
+
+# 查看导航状态
+ros2 topic echo /nav_status
+
+# 查看底盘控制指令
+ros2 topic echo /cmd_vel_move
+```
+
+### 测试 3: 测试视觉控制（如果视觉节点运行）
+
+```bash
+# 查看视觉发送的云台控制指令
+ros2 topic echo /gimbal/vision
+
+# 应该看到：
+# mode: 0/1/2
+# yaw: ...
+# pitch: ...
+# vx, vy, wz: ...
+```
+
+## 最小启动配置
+
+如果只需要测试串口通信和导航：
+
+```bash
+# 终端 1: 串口驱动
+ros2 run rm_serial_driver rm_serial_driver
+
+# 终端 2: 导航节点
+ros2 launch rm_simpal_move simple_move.launch.py
+
+# 终端 3: 发送导航目标
+ros2 topic pub /nav_goal rm_msgs/msg/NavGoal "{mode: 0, target_x: 1.0, target_y: 1.0, target_angle: 0.0, max_speed: 1.0, tolerance: 0.2}" --once
+```
+
+## 完整启动配置（包含视觉）
+
+```bash
+# 终端 1: 串口驱动
+ros2 run rm_serial_driver rm_serial_driver
+
+# 终端 2: 导航节点
+ros2 launch rm_simpal_move simple_move.launch.py
+
+# 终端 3: 视觉节点
+./build/mr_vision/standard_mpc configs/standard.yaml
+
+# 终端 4: 监控（可选）
+ros2 topic echo /data_mcu
+```
+
+## 启动脚本
+
+创建一个启动脚本 `start_all.sh`：
+
+```bash
+#!/bin/bash
+
+# 启动所有节点
+cd ~/MOVE_AI
+source install/setup.bash
+
+# 启动串口驱动（后台）
+gnome-terminal -- bash -c "source install/setup.bash && ros2 run rm_serial_driver rm_serial_driver; exec bash"
+
+# 等待串口驱动启动
+sleep 2
+
+# 启动导航节点（后台）
+gnome-terminal -- bash -c "source install/setup.bash && ros2 launch rm_simpal_move simple_move.launch.py; exec bash"
+
+# 等待导航节点启动
+sleep 2
+
+# 启动视觉节点（如果需要）
+# gnome-terminal -- bash -c "source install/setup.bash && ./build/mr_vision/standard_mpc configs/standard.yaml; exec bash"
+
+echo "所有节点已启动！"
+echo "使用 'ros2 topic list' 查看话题"
+echo "使用 'ros2 topic echo /data_mcu' 查看 MCU 数据"
+```
+
+使用方法：
+```bash
+chmod +x start_all.sh
+./start_all.sh
+```
+
+## 关闭流程
+
+```bash
+# 方式 1: 在每个终端按 Ctrl+C
+
+# 方式 2: 使用脚本关闭所有节点
+pkill -f rm_serial_driver
+pkill -f rm_simple_move
+pkill -f standard_mpc
+```
+
+## 常见问题
+
+### Q1: 串口打开失败
+```bash
+# 检查设备
+ls -l /dev/ttyACM*
+
+# 添加权限
+sudo usermod -a -G dialout $USER
+# 重新登录
+
+# 或临时授权
+sudo chmod 666 /dev/ttyACM0
+```
+
+### Q2: 收不到 /data_mcu 数据
+- 检查串口连接是否正常
+- 检查 MCU 是否正常运行
+- 检查波特率是否匹配（默认 115200）
+
+### Q3: 视觉节点等待四元数超时
+- 确保 `rm_serial_driver` 已启动
+- 确保 MCU 正在发送数据
+- 检查 `/data_mcu` 话题是否有数据
+
+### Q4: 导航不动
+- 检查 `/cmd_vel_move` 是否有数据
+- 检查 `rm_serial_driver` 是否订阅了 `/cmd_vel_move`
+- 检查 TF 变换是否正常
+
+## 调试工具
+
+```bash
+# 查看所有节点
+ros2 node list
+
+# 查看所有话题
+ros2 topic list
+
+# 查看话题频率
+ros2 topic hz /data_mcu
+
+# 查看话题信息
+ros2 topic info /gimbal/vision
+
+# 录制数据
+ros2 bag record -a
+
+# 查看 TF 树
+ros2 run tf2_tools view_frames
+```
+
+## 性能监控
+
+```bash
+# 查看节点资源占用
+top -p $(pgrep -f rm_serial_driver)
+
+# 查看话题带宽
+ros2 topic bw /data_mcu
+
+# 查看话题延迟
+ros2 topic delay /data_mcu
+```
+
+## 下一步
+
+1. ✅ 串口驱动正常运行
+2. ✅ 导航节点正常运行
+3. ⏳ 修复剩余的视觉节点文件
+4. ⏳ 测试完整的视觉+导航+串口流程
+
+## 文档
+
+- **使用指南**: `GIMBAL_ROS_GUIDE.md`
+- **迁移状态**: `ROS2_MIGRATION_STATUS.md`
+- **本文档**: `COMPLETE_STARTUP_GUIDE.md`

README.md

@@ -1,20 +1,378 @@
 # MOVE_AI
 
-青岛理工大学QUT
+青岛理工大学 QUT MOVE 战队 RoboMaster 比赛上位机代码仓库
-MOVE战队
-RoboMaster比赛
-纯mid360激光雷达导航项目
 
+支持步兵/英雄/无人机、哨兵、雷达等兵种，集成导航、自瞄、决策等功能。
 
-ros2 launch rm_nav_bringup bringup_sim.launch.py \
+---
-    world:=RC2027 \
-    mode:=mapping \
-    lio:=fastlio \
-    lio_rviz:=True \
-    nav_rviz:=True
 
+## 📋 目录
 
+- [系统架构](#系统架构)
+- [功能模块](#功能模块)
+- [快速开始](#快速开始)
+- [启动流程](#启动流程)
+- [话题接口](#话题接口)
+- [测试命令](#测试命令)
 
-    sudo mv /opt/MVS/lib/64/libusb-1.0.so.0 /opt/MVS/lib/64/libusb-1.0.so.0.bak
+---
-[sudo] robofish 的密码： 
+
-robofish@robofish-ASUS-TUF-Gaming-F15-FX507ZM-FX507ZM:~/MOVE_AI$ sudo mv /opt/MVS/lib/32/libusb-1.0.so.0 /opt/MVS/lib/32/libusb-1.0.so.0.bak
+## 🏗️ 系统架构
+
+```
+┌─────────────────────────────────────────────────────────────────┐
+│                          下位机 (MCU)                            │
+│                    (STM32 + 电机 + IMU + 传感器)                 │
+└────────────────────────────┬────────────────────────────────────┘
+                             │ 串口通信
+                             ↓
+┌─────────────────────────────────────────────────────────────────┐
+│                    rm_serial_driver (串口驱动)                   │
+│  ┌──────────────────────────────────────────────────────────┐  │
+│  │ 发布: /data_mcu (MCU状态)                                 │  │
+│  │       /data_ref (裁判系统数据)                            │  │
+│  │       /gimbal/state (云台状态)                            │  │
+│  │ 订阅: /data_ai (综合控制指令)                             │  │
+│  └──────────────────────────────────────────────────────────┘  │
+└───────┬─────────────────────────────────────────────┬───────────┘
+        │                                             │
+        ↓                                             ↓
+┌──────────────────┐                         ┌──────────────────┐
+│   rm_vision      │                         │  rm_decision     │
+│   (视觉系统)      │                         │  (决策节点)       │
+├──────────────────┤                         ├──────────────────┤
+│ 订阅:             │                         │ 订阅:             │
+│ /gimbal/state    │                         │ /data_mcu        │
+│                  │                         │ /data_ref        │
+│ 发布:             │                         │ /vision/aim      │
+│ /vision/aim ─────┼─────────────────────────→ /nav_status      │
+│ (云台控制)        │                         │ /cmd_vel_move    │
+└──────────────────┘                         │                  │
+                                             │ 发布:             │
+┌──────────────────┐                         │ /data_ai ────────┼──┐
+│  rm_simple_move  │                         │ /nav_goal        │  │
+│  (运动控制)       │                         └────────┬─────────┘  │
+├──────────────────┤                                  │            │
+│ 订阅:             │                                  │            │
+│ /nav_goal ←──────┼──────────────────────────────────┘            │
+│                  │                                               │
+│ 发布:             │                                               │
+│ /cmd_vel_move ───┼───────────────────────────────────────────────┘
+│ /nav_status      │
+└────────┬─────────┘
+         │
+         ↓
+┌──────────────────┐
+│   Nav2 Stack     │
+│   (导航系统)      │
+├──────────────────┤
+│ - 路径规划        │
+│ - 避障            │
+│ - 定位            │
+└──────────────────┘
+```
+
+---
+
+## 🔧 功能模块
+
+### 1. **rm_serial_driver** - 串口驱动
+- 与下位机 MCU 进行串口通信
+- 接收 MCU 状态、裁判系统数据、云台状态
+- 发送综合控制指令到 MCU
+
+### 2. **rm_vision** - 视觉系统
+- 自瞄识别与跟踪
+- 能量机关识别
+- 输出云台控制指令（yaw/pitch 角度、速度、加速度）
+
+### 3. **rm_simple_move** - 运动控制
+- **PID 模式**：直接速度控制
+- **NAV 模式**：调用 Nav2 进行路径规划和导航
+- 输出底盘速度指令
+
+### 4. **rm_decision** - 决策节点（核心控制器）
+- 整合视觉自瞄控制（云台）
+- 整合运动控制指令（底盘）
+- 根据裁判系统、导航状态做决策
+- 输出统一的 `/data_ai` 控制指令
+
+### 5. **Nav2** - 导航系统
+- 全局路径规划
+- 局部避障
+- SLAM 定位
+
+---
+
+## 🚀 快速开始
+
+### 环境要求
+- Ubuntu 22.04
+- ROS 2 Humble
+- C++17
+
+### 编译
+```bash
+cd /home/robofish/MOVE_AI
+colcon build --symlink-install
+source install/setup.bash
+```
+
+---
+
+## 📡 启动流程
+
+### 方式一：分步启动（推荐调试）
+
+#### 1️⃣ 启动串口驱动
+```bash
+ros2 launch rm_serial_driver serial_driver.launch.py
+```
+
+#### 2️⃣ 启动导航系统
+```bash
+ros2 launch rm_nav_bringup nav.launch.py
+```
+
+#### 3️⃣ 启动视觉系统
+
+**Sentry 哨兵（推荐）：**
+```bash
+ros2 launch rm_vision_bringup sentry_vision.launch.py
+```
+
+**其他机器人类型：**
+```bash
+# 标准步兵
+ros2 launch rm_vision_bringup vision.launch.py vision_type:=standard_mpc
+
+# 平衡步兵
+ros2 launch rm_vision_bringup vision.launch.py vision_type:=balance_infantry_mpc
+
+# 自瞄调试模式
+ros2 launch rm_vision_bringup vision.launch.py vision_type:=auto_aim_debug_mpc
+
+# 能量机关调试
+ros2 launch rm_vision_bringup vision.launch.py vision_type:=auto_buff_debug_mpc
+
+# 自定义配置文件
+ros2 launch rm_vision_bringup vision.launch.py config:=sentry.yaml
+```
+
+#### 4️⃣ 启动决策节点
+```bash
+ros2 launch rm_decision decision.launch.py
+```
+
+### 方式二：一键启动（待实现）
+```bash
+ros2 launch rm_bringup full_system.launch.py
+```
+
+---
+
+## 🎯 标定工具
+
+视觉系统提供了完整的标定工具链：
+
+### 相机标定
+```bash
+# 1. 采集标定图像（按 's' 保存，'q' 退出）
+ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=capture
+
+# 2. 标定相机内参
+ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=calibrate_camera
+
+# 3. 手眼标定
+ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=calibrate_handeye
+
+# 4. 机器人-世界手眼标定
+ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=calibrate_robotworld_handeye
+
+# 5. 视频分割工具
+ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=split_video
+```
+
+**自定义参数：**
+```bash
+# 指定配置文件和输出目录
+ros2 launch rm_vision_bringup calibration.launch.py \
+  calibration_type:=capture \
+  config:=calibration.yaml \
+  output_folder:=my_calibration_data
+```
+
+---
+
+## 📊 话题接口
+
+### 核心话题
+
+| 话题名称 | 消息类型 | 发布者 | 订阅者 | 说明 |
+|---------|---------|--------|--------|------|
+| `/data_mcu` | `rm_msgs/DataMCU` | serial_driver | decision | MCU 状态数据 |
+| `/data_ref` | `rm_msgs/DataRef` | serial_driver | decision | 裁判系统数据 |
+| `/gimbal/state` | `rm_msgs/GimbalState` | serial_driver | vision | 云台状态（姿态、速度） |
+| `/vision/aim` | `rm_msgs/DataAim` | vision | decision | 视觉自瞄控制（仅云台） |
+| `/nav_goal` | `rm_msgs/NavGoal` | decision | simple_move | 导航目标 |
+| `/nav_status` | `rm_msgs/NavStatus` | simple_move | decision | 导航状态反馈 |
+| `/cmd_vel_move` | `geometry_msgs/Twist` | simple_move | decision | 底盘速度指令 |
+| `/data_ai` | `rm_msgs/DataAI` | decision | serial_driver | 综合控制指令（云台+底盘） |
+
+### 数据流向
+
+```
+视觉控制流:
+MCU → /gimbal/state → Vision → /vision/aim → Decision → /data_ai → MCU
+
+底盘控制流:
+Decision → /nav_goal → SimpleMove → /cmd_vel_move → Decision → /data_ai → MCU
+
+导航反馈流:
+SimpleMove → /nav_status → Decision
+```
+
+---
+
+## 🧪 测试命令
+
+### 查看话题列表
+```bash
+ros2 topic list
+```
+
+### 监控关键话题
+```bash
+# 查看决策输出
+ros2 topic echo /data_ai
+
+# 查看底盘速度
+ros2 topic echo /cmd_vel_move
+
+# 查看视觉自瞄
+ros2 topic echo /vision/aim
+
+# 查看导航状态
+ros2 topic echo /nav_status
+```
+
+### 发送导航目标
+
+#### NAV 模式（路径规划）
+```bash
+ros2 topic pub --once /nav_goal rm_msgs/msg/NavGoal \
+  "{mode: 1, target_x: 4.0, target_y: -4.0, target_angle: 0.0, max_speed: 1.0, tolerance: 0.1}"
+```
+
+#### PID 模式（直接控制）
+```bash
+ros2 topic pub --once /nav_goal rm_msgs/msg/NavGoal \
+  "{mode: 0, target_x: 1.0, target_y: 0.5, target_angle: 0.0, max_speed: 0.5, tolerance: 0.05}"
+```
+
+### 可视化
+```bash
+# 启动 RViz 查看导航
+rviz2 -d $(ros2 pkg prefix rm_bringup)/share/rm_bringup/rviz/nav.rviz
+
+# 查看 TF 树
+ros2 run tf2_tools view_frames
+```
+
+---
+
+## 📝 消息定义
+
+### DataAim（视觉自瞄 - 仅云台）
+```
+uint8 mode              # 0:空闲 1:自瞄 2:小符 3:大符
+float32 yaw             # 偏航角
+float32 yaw_vel         # 偏航角速度
+float32 yaw_acc         # 偏航角加速度
+float32 pitch           # 俯仰角
+float32 pitch_vel       # 俯仰角速度
+float32 pitch_acc       # 俯仰角加速度
+```
+
+### DataAI（综合控制 - 云台+底盘）
+```
+uint8 mode              # 云台模式
+float32 yaw, yaw_vel, yaw_acc
+float32 pitch, pitch_vel, pitch_acc
+float32 vx, vy, wz      # 底盘速度
+uint8 reserved          # 底盘模式
+```
+
+### NavGoal（导航目标）
+```
+uint8 MODE_PID = 0      # PID 直接控制
+uint8 MODE_NAV = 1      # Nav2 导航
+uint8 mode
+float32 target_x, target_y, target_angle
+float32 max_speed, tolerance
+```
+
+### ChassisMode（底盘模式）
+```
+uint8 MODE_MANUAL = 0   # 手动控制
+uint8 MODE_FOLLOW = 1   # 跟随云台
+uint8 MODE_SPIN = 2     # 小陀螺
+uint8 MODE_NAV = 3      # 导航模式
+```
+
+---
+
+## 🐛 调试技巧
+
+### 检查节点状态
+```bash
+ros2 node list
+ros2 node info /rm_decision
+```
+
+### 检查话题连接
+```bash
+ros2 topic info /data_ai
+```
+
+### 查看参数
+```bash
+ros2 param list /rm_decision
+ros2 param get /rm_decision enable_aim
+```
+
+### 日志级别
+```bash
+ros2 run rm_decision decision_node --ros-args --log-level debug
+```
+
+---
+
+## 📂 项目结构
+
+```
+MOVE_AI/
+├── src/
+│   ├── rm_serial_driver/    # 串口驱动
+│   ├── rm_vision/           # 视觉系统
+│   ├── rm_nav/              # 导航相关
+│   │   └── rm_simple_move/  # 运动控制
+│   ├── rm_decision/         # 决策节点
+│   ├── rm_msgs/             # 消息定义
+│   └── rm_bringup/          # 启动文件
+└── README.md
+```
+
+---
+
+## 🤝 贡献
+
+欢迎提交 Issue 和 Pull Request！
+
+## 📄 许可证
+
+本项目采用 MIT 许可证。
+
+---
+
+**青岛理工大学 QUT MOVE 战队** | RoboMaster 2026

mapping.sh

@@ -0,0 +1,19 @@
+source install/setup.bash
+
+commands=(
+    "ros2 launch rm_serial_driver rm_serial_driver.launch.py"
+    "ros2 launch rm_decision decision.launch.py"
+    "ros2 launch rm_simpal_move simple_move.launch.py"
+    "ros2 launch rm_nav_bringup bringup_sim.launch.py \
+      world:=2026RMUL \
+      mode:=mapping  \
+      lio:=fastlio \
+      lio_rviz:=True \
+      nav_rviz:=True
+    "
+)
+
+for cmd in "${commands[@]}"; do
+  gnome-terminal -- bash -c "source install/setup.bash; $cmd; exec bash"
+  sleep 0.5
+done

nav.sh

@@ -0,0 +1,20 @@
+source install/setup.bash
+
+commands=(
+    "ros2 launch rm_serial_driver rm_serial_driver.launch.py"
+    "ros2 launch rm_decision decision.launch.py"
+    "ros2 launch rm_simpal_move simple_move.launch.py"
+    "ros2 launch rm_nav_bringup bringup_real.launch.py \
+      world:=wanzheng \
+      mode:=nav \
+      lio:=fastlio \
+      localization:=gicp \
+      lio_rviz:=False \
+      nav_rviz:=True
+    "
+)
+
+for cmd in "${commands[@]}"; do
+  gnome-terminal -- bash -c "source install/setup.bash; $cmd; exec bash"
+  sleep 0.5
+done

sim.sh

@@ -0,0 +1,20 @@
+source install/setup.bash
+
+commands=(
+    # "ros2 launch rm_serial_driver rm_serial_driver.launch.py"
+    # "ros2 launch rm_decision decision.launch.py"
+    "ros2 launch rm_simpal_move simple_move.launch.py"
+    "ros2 launch rm_nav_bringup bringup_sim.launch.py \
+    world:=2026RMUL \
+    mode:=nav \
+    lio:=fastlio \
+    localization:=gicp \
+    lio_rviz:=False \
+    nav_rviz:=True
+    "
+)
+
+for cmd in "${commands[@]}"; do
+  gnome-terminal -- bash -c "source install/setup.bash; $cmd; exec bash"
+  sleep 0.5
+done

src/rm_decision/CMakeLists.txt

@@ -0,0 +1,62 @@
+cmake_minimum_required(VERSION 3.8)
+project(rm_decision)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(rm_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+# 包含头文件目录
+include_directories(include)
+
+# 创建决策节点库
+add_library(decision_node SHARED
+  src/decision_node.cpp
+)
+
+ament_target_dependencies(decision_node
+  rclcpp
+  rclcpp_components
+  rm_msgs
+  geometry_msgs
+)
+
+# 注册为组件
+rclcpp_components_register_node(decision_node
+  PLUGIN "rm_decision::DecisionNode"
+  EXECUTABLE decision_node_exe
+)
+
+# 安装
+install(TARGETS decision_node
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+install(DIRECTORY launch test
+  DESTINATION share/${PROJECT_NAME}
+)
+
+# 安装 Python 测试脚本
+install(PROGRAMS
+  test/simple_nav_test.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/rm_decision/README.md

@@ -0,0 +1,107 @@
+# rm_decision — 2026 决策节点
+
+## 概述
+
+RoboMaster 哨兵机器人决策节点，基于有限状态机实现比赛全流程自主决策。节点以 50Hz 运行，综合裁判系统、自瞄视觉、导航反馈等数据，输出统一控制指令给下位机。
+
+## 导航点 (map 坐标系)
+
+| 名称 | X | Y | Angle |
+|------|---|---|-------|
+| 家 (补血点) | -0.098266 | 0.0092684 | -0.074146 |
+| 1号点 | 0.76813 | -4.5578 | -0.23769 |
+| 2号点 | 4.9211 | -4.5856 | -0.27894 |
+| 增益点 | 5.0031 | -2.3273 | -0.20345 |
+
+## 比赛阶段 (game_progress)
+
+| 值 | 含义 |
+|----|------|
+| 0 | 未开始比赛 |
+| 1 | 准备阶段 |
+| 2 | 十五秒裁判系统自检 |
+| 3 | 五秒倒计时 |
+| 4 | 比赛中 |
+| 5 | 比赛结算中 |
+
+## 状态机
+
+```
+IDLE ──(game=4)──> GO_POINT1 ──> GO_POINT2 ──> GO_GAIN ──> HOLD_GAIN
+                                                              │   ▲
+                                                              ▼   │
+                                                           PATROL─┘
+                                                              │
+                                              (hp<200)        │
+                   ┌──────────────────────────────────────────┘
+                   ▼
+              RETREAT_TO_2 ──> RETREAT_TO_1 ──> RETREAT_HOME ──> HEALING
+                                                                    │
+                                                        (hp>=350)   │
+                                                                    ▼
+                                                              GO_POINT1 (重新出发)
+```
+
+### 状态说明
+
+| 状态 | 行为 | 小陀螺 | 云台搜索 | 自瞄 |
+|------|------|--------|---------|------|
+| IDLE | game_progress 0~3，PID 待在家 | 关 | 关 | 关 |
+| GO_POINT1 | PID 导航到 1 号点 | 关 | 关 | 关 |
+| GO_POINT2 | PID 导航到 2 号点 | 关 | 关 | 关 |
+| GO_GAIN | PID 导航到增益点 | 关 | 关 | 关 |
+| HOLD_GAIN | 到达增益点，原地占领；发现目标则原地陀螺开火 | 开 | 有目标时关 | 有目标时开 |
+| PATROL | 无目标时在增益点 ±0.5m 范围随机移动找敌 | 开 | 开 | 关 |
+| RETREAT_TO_2 | 血量 <200，从增益点撤退到 2 号点 | 开 | 关 | 关 |
+| RETREAT_TO_1 | 从 2 号点撤退到 1 号点 | 开 | 关 | 关 |
+| RETREAT_HOME | 从 1 号点撤退回家 | 开 | 关 | 关 |
+| HEALING | 在家补血，等待血量恢复到 ≥350 | 关 | 关 | 关 |
+| MATCH_END | 比赛结束，停止一切动作 | 关 | 关 | 关 |
+
+## 决策规则
+
+1. **赛前待命**：game_progress 为 0/1/2/3 时，PID 控制待在家的位置。
+2. **快速占点**：收到 game_progress=4 后，PID 依次跑 1号点 → 2号点 → 增益点，全程不开小陀螺，以最快速度占领增益点。
+3. **占点防守**：到达增益点后立刻开启底盘小陀螺 + 云台 search 模式。search 到目标后立刻退出 search，转发自瞄数据开火；目标丢失后有 1 秒过渡延时，之后重新进入 search。
+4. **巡逻找敌**：占领增益点时若自瞄无目标，在增益点附近 0.5m 范围内随机移动 + search 扫描；发现目标立刻原地陀螺开火。
+5. **低血量撤退**：血量 <200 时立刻撤退，路线为 增益点 → 2号点 → 1号点 → 家，撤退全程开小陀螺。回血优先级高于自瞄。
+6. **补血重出**：在家补血至 ≥350 后，重新执行出家占点逻辑 (1号点 → 2号点 → 增益点)。
+
+## 话题接口
+
+### 订阅
+
+| 话题 | 类型 | 说明 |
+|------|------|------|
+| `/data_mcu` | rm_msgs/DataMCU | MCU 状态 (云台角度、弹速等) |
+| `/data_ref` | rm_msgs/DataRef | 裁判系统 (血量、比赛阶段) |
+| `/data_aim` | rm_msgs/DataAim | 视觉自瞄 (目标角度、开火模式) |
+| `/nav_status` | rm_msgs/NavStatus | 导航状态 (到达/运行中/失败) |
+| `/cmd_vel_move` | geometry_msgs/Twist | simple_move 输出的底盘速度 |
+
+### 发布
+
+| 话题 | 类型 | 说明 |
+|------|------|------|
+| `/data_ai` | rm_msgs/DataAI | 综合控制指令 → 串口 → MCU |
+| `/nav_goal` | rm_msgs/NavGoal | 导航目标 → simple_move |
+
+## reserved 标志位 (DataAI.reserved)
+
+| Bit | 宏定义 | 含义 |
+|-----|--------|------|
+| 0 | AI_CHASSIS_ROTOR | 底盘小陀螺 |
+| 1 | AI_GIMBAL_SEARCH | 云台搜索模式 |
+| 2~7 | — | 预留 |
+
+## 参数
+
+| 参数名 | 默认值 | 说明 |
+|--------|--------|------|
+| control_frequency | 50.0 | 决策循环频率 (Hz) |
+| nav_max_speed | 2.0 | 导航最大线速度 (m/s) |
+| nav_tolerance | 0.15 | 导航到达容差 (m) |
+| wp_home | [-0.098266, 0.0092684, -0.074146] | 家坐标 [x, y, angle] |
+| wp_point1 | [0.76813, -4.5578, -0.23769] | 1号点坐标 |
+| wp_point2 | [4.9211, -4.5856, -0.27894] | 2号点坐标 |
+| wp_gain | [5.0031, -2.3273, -0.20345] | 增益点坐标 |

src/rm_decision/include/rm_decision/decision_node.hpp

@@ -0,0 +1,123 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <rm_msgs/msg/data_mcu.hpp>
+#include <rm_msgs/msg/data_ref.hpp>
+#include <rm_msgs/msg/data_ai.hpp>
+#include <rm_msgs/msg/data_aim.hpp>
+#include <rm_msgs/msg/nav_goal.hpp>
+#include <rm_msgs/msg/nav_status.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <atomic>
+#include <mutex>
+#include <random>
+#include <chrono>
+
+namespace rm_decision
+{
+
+// 决策状态机
+enum class State : uint8_t
+{
+  IDLE,             // 等待比赛开始 (game_progress 0~3)
+  GO_POINT1,        // 前往1号点
+  GO_POINT2,        // 前往2号点
+  GO_GAIN,          // 前往增益点
+  HOLD_GAIN,        // 占领增益点 (陀螺 + search/自瞄)
+  PATROL,           // 增益点附近巡逻 (无目标时随机移动)
+  RETREAT_TO_2,     // 低血量撤退: 增益点 → 2号点
+  RETREAT_TO_1,     // 低血量撤退: 2号点 → 1号点
+  RETREAT_HOME,     // 低血量撤退: 1号点 → 家
+  HEALING,          // 在家补血
+  MATCH_END,        // 比赛结束
+};
+
+// 导航点坐标
+struct Waypoint {
+  float x, y, angle;
+};
+
+class DecisionNode : public rclcpp::Node
+{
+public:
+  explicit DecisionNode(const rclcpp::NodeOptions &options);
+  ~DecisionNode() override;
+
+private:
+  // 回调函数
+  void data_mcu_callback(const rm_msgs::msg::DataMCU::SharedPtr msg);
+  void data_ref_callback(const rm_msgs::msg::DataRef::SharedPtr msg);
+  void data_aim_callback(const rm_msgs::msg::DataAim::SharedPtr msg);
+  void nav_status_callback(const rm_msgs::msg::NavStatus::SharedPtr msg);
+  void cmd_vel_move_callback(const geometry_msgs::msg::Twist::SharedPtr msg);
+  // 定时器回调
+  void control_timer_callback();
+
+  // 决策逻辑
+  void make_decision();
+  void send_control_command();
+
+  // 辅助函数
+  void send_nav_goal(const Waypoint &wp, uint8_t mode, float max_speed, float tolerance);
+  Waypoint random_patrol_point();
+
+  // 订阅者
+  rclcpp::Subscription<rm_msgs::msg::DataMCU>::SharedPtr data_mcu_sub_;
+  rclcpp::Subscription<rm_msgs::msg::DataRef>::SharedPtr data_ref_sub_;
+  rclcpp::Subscription<rm_msgs::msg::DataAim>::SharedPtr data_aim_sub_;
+  rclcpp::Subscription<rm_msgs::msg::NavStatus>::SharedPtr nav_status_sub_;
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_move_sub_;
+
+  // 发布者
+  rclcpp::Publisher<rm_msgs::msg::DataAI>::SharedPtr data_ai_pub_;
+  rclcpp::Publisher<rm_msgs::msg::NavGoal>::SharedPtr nav_goal_pub_;
+
+  // 定时器
+  rclcpp::TimerBase::SharedPtr control_timer_;
+
+  // 状态数据 (mutex保护)
+  std::mutex mutex_;
+  rm_msgs::msg::DataMCU latest_mcu_data_;
+  rm_msgs::msg::DataRef latest_ref_data_;
+  rm_msgs::msg::DataAim latest_aim_data_;
+  rm_msgs::msg::NavStatus latest_nav_status_;
+  geometry_msgs::msg::Twist latest_cmd_vel_;
+
+  // 原子状态标志
+  std::atomic<bool> has_mcu_data_{false};
+  std::atomic<bool> has_aim_data_{false};
+  std::atomic<bool> has_nav_data_{false};
+
+  // 决策状态机
+  State state_{State::IDLE};
+  bool nav_goal_sent_{false};          // 当前状态的导航目标是否已发送
+  bool aim_target_found_{false};       // 自瞄是否发现目标
+  bool enable_rotor_{false};           // 底盘小陀螺
+  bool enable_search_{false};          // 云台搜索模式
+  bool enable_aim_{false};             // 启用自瞄转发
+
+  // 自瞄延时控制
+  rclcpp::Time aim_lost_time_;         // 自瞄丢失目标的时间
+  bool aim_transition_active_{false};  // 自瞄→search过渡中
+  static constexpr double AIM_TRANSITION_DELAY = 1.0;  // 1秒过渡延时
+
+  // 血量阈值
+  static constexpr uint16_t HP_RETREAT_THRESHOLD = 200;
+  static constexpr uint16_t HP_HEALED_THRESHOLD = 350;
+
+  // 导航点
+  Waypoint wp_home_;
+  Waypoint wp_point1_;
+  Waypoint wp_point2_;
+  Waypoint wp_gain_;
+
+  // 巡逻随机数
+  std::mt19937 rng_;
+  std::uniform_real_distribution<float> patrol_dist_{-0.5f, 0.5f};
+
+  // 导航速度参数
+  float nav_max_speed_{2.0f};
+  float nav_tolerance_{0.5f};
+};
+
+}  // namespace rm_decision

src/rm_decision/launch/decision.launch.py

@@ -0,0 +1,18 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='rm_decision',
+            executable='decision_node_exe',
+            name='rm_decision',
+            output='screen',
+            parameters=[{
+                'control_frequency': 50.0,
+                'enable_aim': True,
+                'chassis_mode': 0,
+            }]
+        )
+    ])

src/rm_decision/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rm_decision</name>
+  <version>1.0.0</version>
+  <description>RoboMaster Decision Node - Core Controller</description>
+  <maintainer email="move@qut.edu.cn">MOVE Team</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>rm_msgs</depend>
+  <depend>geometry_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/rm_decision/src/decision_node.cpp

@@ -0,0 +1,455 @@
+#include "rm_decision/decision_node.hpp"
+
+namespace rm_decision
+{
+
+DecisionNode::DecisionNode(const rclcpp::NodeOptions &options)
+  : Node("rm_decision", options),
+    rng_(std::chrono::steady_clock::now().time_since_epoch().count())
+{
+  // 声明参数
+  this->declare_parameter("control_frequency", 50.0);
+  this->declare_parameter("nav_max_speed", 2.0);
+  this->declare_parameter("nav_tolerance", 0.15);
+
+  // 导航点参数 (来自 README)
+  this->declare_parameter("wp_home", std::vector<double>{-0.098266, 0.0092684, -0.074146});
+  this->declare_parameter("wp_point1", std::vector<double>{0.76813, -4.5578, -0.23769});
+  this->declare_parameter("wp_point2", std::vector<double>{4.9211, -4.5856, -0.27894});
+  this->declare_parameter("wp_gain", std::vector<double>{5.0031, -2.3273, -0.20345});
+
+  auto control_freq = this->get_parameter("control_frequency").as_double();
+  nav_max_speed_ = this->get_parameter("nav_max_speed").as_double();
+  nav_tolerance_ = this->get_parameter("nav_tolerance").as_double();
+
+  // 加载导航点
+  auto load_wp = [this](const std::string &name) -> Waypoint {
+    auto v = this->get_parameter(name).as_double_array();
+    return Waypoint{static_cast<float>(v[0]), static_cast<float>(v[1]), static_cast<float>(v[2])};
+  };
+  wp_home_   = load_wp("wp_home");
+  wp_point1_ = load_wp("wp_point1");
+  wp_point2_ = load_wp("wp_point2");
+  wp_gain_   = load_wp("wp_gain");
+
+  // 创建订阅者
+  data_mcu_sub_ = this->create_subscription<rm_msgs::msg::DataMCU>(
+    "/data_mcu", 10, std::bind(&DecisionNode::data_mcu_callback, this, std::placeholders::_1));
+  data_ref_sub_ = this->create_subscription<rm_msgs::msg::DataRef>(
+    "/data_ref", 10, std::bind(&DecisionNode::data_ref_callback, this, std::placeholders::_1));
+  data_aim_sub_ = this->create_subscription<rm_msgs::msg::DataAim>(
+    "/data_aim", 10, std::bind(&DecisionNode::data_aim_callback, this, std::placeholders::_1));
+  nav_status_sub_ = this->create_subscription<rm_msgs::msg::NavStatus>(
+    "/nav_status", 10, std::bind(&DecisionNode::nav_status_callback, this, std::placeholders::_1));
+  cmd_vel_move_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
+    "/cmd_vel_move", 10, std::bind(&DecisionNode::cmd_vel_move_callback, this, std::placeholders::_1));
+
+  // 创建发布者
+  data_ai_pub_ = this->create_publisher<rm_msgs::msg::DataAI>("/data_ai", 10);
+  nav_goal_pub_ = this->create_publisher<rm_msgs::msg::NavGoal>("/nav_goal", 10);
+
+  // 创建控制定时器
+  auto period = std::chrono::duration<double>(1.0 / control_freq);
+  control_timer_ = this->create_wall_timer(
+    std::chrono::duration_cast<std::chrono::milliseconds>(period),
+    std::bind(&DecisionNode::control_timer_callback, this));
+
+  aim_lost_time_ = this->now();
+
+  RCLCPP_INFO(this->get_logger(), "Decision Node 启动 | 频率: %.0fHz", control_freq);
+  RCLCPP_INFO(this->get_logger(), "  家: (%.3f, %.3f)", wp_home_.x, wp_home_.y);
+  RCLCPP_INFO(this->get_logger(), "  1号: (%.3f, %.3f)", wp_point1_.x, wp_point1_.y);
+  RCLCPP_INFO(this->get_logger(), "  2号: (%.3f, %.3f)", wp_point2_.x, wp_point2_.y);
+  RCLCPP_INFO(this->get_logger(), "  增益: (%.3f, %.3f)", wp_gain_.x, wp_gain_.y);
+}
+
+DecisionNode::~DecisionNode()
+{
+  RCLCPP_INFO(this->get_logger(), "Decision Node 关闭");
+}
+
+// ==================== 回调函数 ====================
+
+void DecisionNode::data_mcu_callback(const rm_msgs::msg::DataMCU::SharedPtr msg)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  latest_mcu_data_ = *msg;
+  has_mcu_data_ = true;
+}
+
+void DecisionNode::data_ref_callback(const rm_msgs::msg::DataRef::SharedPtr msg)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  latest_ref_data_ = *msg;
+}
+
+void DecisionNode::data_aim_callback(const rm_msgs::msg::DataAim::SharedPtr msg)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  latest_aim_data_ = *msg;
+  has_aim_data_ = true;
+}
+
+void DecisionNode::nav_status_callback(const rm_msgs::msg::NavStatus::SharedPtr msg)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  latest_nav_status_ = *msg;
+  has_nav_data_ = true;
+}
+
+void DecisionNode::cmd_vel_move_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  latest_cmd_vel_ = *msg;
+}
+
+// ==================== 辅助函数 ====================
+
+void DecisionNode::send_nav_goal(const Waypoint &wp, uint8_t mode, float max_speed, float tolerance)
+{
+  auto msg = rm_msgs::msg::NavGoal();
+  msg.control_mode = mode;
+  msg.x = wp.x;
+  msg.y = wp.y;
+  msg.angle = wp.angle;
+  msg.max_speed = max_speed;
+  msg.tolerance = tolerance;
+  nav_goal_pub_->publish(msg);
+}
+
+Waypoint DecisionNode::random_patrol_point()
+{
+  return Waypoint{
+    wp_gain_.x + patrol_dist_(rng_),
+    wp_gain_.y + patrol_dist_(rng_),
+    wp_gain_.angle
+  };
+}
+
+// ==================== 定时器 ====================
+
+void DecisionNode::control_timer_callback()
+{
+  if (!has_mcu_data_) return;
+  make_decision();
+  send_control_command();
+}
+
+// ==================== 决策逻辑 ====================
+
+void DecisionNode::make_decision()
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  uint8_t game_progress = latest_ref_data_.game_progress;
+  uint16_t hp = latest_ref_data_.remain_hp;
+  uint8_t nav_status = has_nav_data_ ? latest_nav_status_.status : 0;
+
+  // 自瞄目标检测: mode >= 1 表示发现目标
+  bool aim_has_target = has_aim_data_ && (latest_aim_data_.mode >= 1);
+
+  // 自瞄过渡延时逻辑: 丢失目标后延时1s再切回search
+  if (aim_has_target) {
+    aim_target_found_ = true;
+    aim_transition_active_ = false;
+  } else if (aim_target_found_ && !aim_transition_active_) {
+    // 刚丢失目标，开始过渡计时
+    aim_transition_active_ = true;
+    aim_lost_time_ = this->now();
+  } else if (aim_transition_active_) {
+    double elapsed = (this->now() - aim_lost_time_).seconds();
+    if (elapsed >= AIM_TRANSITION_DELAY) {
+      aim_target_found_ = false;
+      aim_transition_active_ = false;
+    }
+  }
+
+  State prev_state = state_;
+
+  // ===== 比赛结束 =====
+  if (game_progress == 5) {
+    state_ = State::MATCH_END;
+    enable_rotor_ = false;
+    enable_search_ = false;
+    enable_aim_ = false;
+    nav_goal_sent_ = false;
+    return;
+  }
+
+  // ===== 比赛未开始 (0,1,2,3): PID待在家 =====
+  if (game_progress < 4) {
+    if (state_ != State::IDLE) {
+      state_ = State::IDLE;
+      nav_goal_sent_ = false;
+    }
+    enable_rotor_ = false;
+    enable_search_ = false;
+    enable_aim_ = false;
+    if (!nav_goal_sent_) {
+      send_nav_goal(wp_home_, 0, nav_max_speed_, nav_tolerance_);
+      nav_goal_sent_ = true;
+      RCLCPP_INFO(this->get_logger(), "[IDLE] PID待在家");
+    }
+    return;
+  }
+
+  // ===== 以下为 game_progress == 4 (比赛中) =====
+
+  // --- 回血优先级最高 (优先于自瞄) ---
+  // 在非撤退/补血状态下，血量低于阈值触发撤退
+  bool in_retreat = (state_ == State::RETREAT_TO_2 || state_ == State::RETREAT_TO_1 ||
+                     state_ == State::RETREAT_HOME || state_ == State::HEALING);
+
+  if (!in_retreat && hp > 0 && hp < HP_RETREAT_THRESHOLD) {
+    // 根据当前位置决定撤退起点
+    if (state_ == State::HOLD_GAIN || state_ == State::PATROL) {
+      state_ = State::RETREAT_TO_2;
+    } else if (state_ == State::GO_GAIN) {
+      state_ = State::RETREAT_TO_2;
+    } else if (state_ == State::GO_POINT2) {
+      state_ = State::RETREAT_TO_1;
+    } else {
+      state_ = State::RETREAT_HOME;
+    }
+    nav_goal_sent_ = false;
+    RCLCPP_WARN(this->get_logger(), "[RETREAT] 血量 %u < %u, 开始撤退!", hp, HP_RETREAT_THRESHOLD);
+  }
+
+  // ===== 状态机执行 =====
+  switch (state_) {
+    case State::IDLE: {
+      // 比赛刚开始(4)，从家出发
+      state_ = State::GO_POINT1;
+      nav_goal_sent_ = false;
+      enable_rotor_ = false;  // 出家过程不开小陀螺
+      enable_search_ = false;
+      enable_aim_ = false;
+      RCLCPP_INFO(this->get_logger(), "[GO_POINT1] 比赛开始，出发前往1号点");
+      break;
+    }
+
+    case State::GO_POINT1: {
+      enable_rotor_ = false;
+      enable_search_ = false;
+      enable_aim_ = false;
+      if (!nav_goal_sent_) {
+        send_nav_goal(wp_point1_, 0, nav_max_speed_, nav_tolerance_);
+        nav_goal_sent_ = true;
+      }
+      if (nav_status == 2) {  // 到达1号点
+        state_ = State::GO_POINT2;
+        nav_goal_sent_ = false;
+        RCLCPP_INFO(this->get_logger(), "[GO_POINT2] 到达1号点，前往2号点");
+      }
+      break;
+    }
+
+    case State::GO_POINT2: {
+      enable_rotor_ = false;
+      enable_search_ = false;
+      enable_aim_ = false;
+      if (!nav_goal_sent_) {
+        send_nav_goal(wp_point2_, 0, nav_max_speed_, nav_tolerance_);
+        nav_goal_sent_ = true;
+      }
+      if (nav_status == 2) {
+        state_ = State::GO_GAIN;
+        nav_goal_sent_ = false;
+        RCLCPP_INFO(this->get_logger(), "[GO_GAIN] 到达2号点，前往增益点");
+      }
+      break;
+    }
+
+    case State::GO_GAIN: {
+      enable_rotor_ = false;
+      enable_search_ = false;
+      enable_aim_ = false;
+      if (!nav_goal_sent_) {
+        send_nav_goal(wp_gain_, 0, nav_max_speed_, nav_tolerance_);
+        nav_goal_sent_ = true;
+      }
+      if (nav_status == 2) {
+        state_ = State::HOLD_GAIN;
+        nav_goal_sent_ = false;
+        enable_rotor_ = true;   // 到达增益点立刻开启小陀螺
+        enable_search_ = true;  // 开启云台search
+        RCLCPP_INFO(this->get_logger(), "[HOLD_GAIN] 到达增益点，开启陀螺+搜索");
+      }
+      break;
+    }
+
+    case State::HOLD_GAIN: {
+      enable_rotor_ = true;
+      if (aim_target_found_) {
+        // 发现目标: 原地陀螺 + 自瞄开火
+        enable_search_ = false;
+        enable_aim_ = true;
+        // 停止导航，原地不动
+        if (nav_goal_sent_) {
+          nav_goal_sent_ = false;
+        }
+      } else {
+        // 没有目标: 进入巡逻模式
+        enable_search_ = true;
+        enable_aim_ = false;
+        state_ = State::PATROL;
+        nav_goal_sent_ = false;
+        RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000,
+                             "[PATROL] 无目标，增益点附近巡逻");
+      }
+      break;
+    }
+
+    case State::PATROL: {
+      enable_rotor_ = true;
+      enable_search_ = true;
+      if (aim_target_found_) {
+        // 发现目标: 立刻停下，原地陀螺开火
+        state_ = State::HOLD_GAIN;
+        nav_goal_sent_ = false;
+        enable_aim_ = true;
+        enable_search_ = false;
+        RCLCPP_INFO(this->get_logger(), "[HOLD_GAIN] 巡逻中发现目标，原地开火");
+      } else {
+        // 随机巡逻
+        if (!nav_goal_sent_ || nav_status == 2) {
+          auto patrol_wp = random_patrol_point();
+          send_nav_goal(patrol_wp, 0, 1.0f, 0.2f);
+          nav_goal_sent_ = true;
+        }
+      }
+      break;
+    }
+
+    case State::RETREAT_TO_2: {
+      enable_rotor_ = true;  // 撤退过程开着小陀螺
+      enable_search_ = false;
+      enable_aim_ = false;
+      if (!nav_goal_sent_) {
+        send_nav_goal(wp_point2_, 0, nav_max_speed_, nav_tolerance_);
+        nav_goal_sent_ = true;
+        RCLCPP_INFO(this->get_logger(), "[RETREAT] 撤退到2号点");
+      }
+      if (nav_status == 2) {
+        state_ = State::RETREAT_TO_1;
+        nav_goal_sent_ = false;
+      }
+      break;
+    }
+
+    case State::RETREAT_TO_1: {
+      enable_rotor_ = true;
+      enable_search_ = false;
+      enable_aim_ = false;
+      if (!nav_goal_sent_) {
+        send_nav_goal(wp_point1_, 0, nav_max_speed_, nav_tolerance_);
+        nav_goal_sent_ = true;
+        RCLCPP_INFO(this->get_logger(), "[RETREAT] 撤退到1号点");
+      }
+      if (nav_status == 2) {
+        state_ = State::RETREAT_HOME;
+        nav_goal_sent_ = false;
+      }
+      break;
+    }
+
+    case State::RETREAT_HOME: {
+      enable_rotor_ = true;
+      enable_search_ = false;
+      enable_aim_ = false;
+      if (!nav_goal_sent_) {
+        send_nav_goal(wp_home_, 0, nav_max_speed_, nav_tolerance_);
+        nav_goal_sent_ = true;
+        RCLCPP_INFO(this->get_logger(), "[RETREAT] 撤退回家");
+      }
+      if (nav_status == 2) {
+        state_ = State::HEALING;
+        nav_goal_sent_ = false;
+        RCLCPP_INFO(this->get_logger(), "[HEALING] 到家，开始补血");
+      }
+      break;
+    }
+
+    case State::HEALING: {
+      enable_rotor_ = false;
+      enable_search_ = false;
+      enable_aim_ = false;
+      // 待在家等血量恢复
+      if (!nav_goal_sent_) {
+        send_nav_goal(wp_home_, 0, nav_max_speed_, nav_tolerance_);
+        nav_goal_sent_ = true;
+      }
+      if (hp >= HP_HEALED_THRESHOLD) {
+        // 血量恢复，重新出发
+        state_ = State::GO_POINT1;
+        nav_goal_sent_ = false;
+        RCLCPP_INFO(this->get_logger(), "[GO_POINT1] 血量恢复到 %u，重新出发!", hp);
+      }
+      break;
+    }
+
+    case State::MATCH_END: {
+      enable_rotor_ = false;
+      enable_search_ = false;
+      enable_aim_ = false;
+      break;
+    }
+  }
+
+  if (state_ != prev_state) {
+    RCLCPP_INFO(this->get_logger(), "状态切换: %d -> %d",
+                static_cast<int>(prev_state), static_cast<int>(state_));
+  }
+}
+
+// ==================== 发送控制指令 ====================
+
+void DecisionNode::send_control_command()
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  auto msg = rm_msgs::msg::DataAI();
+
+  // 云台控制
+  if (enable_aim_ && aim_target_found_ && has_aim_data_) {
+    // 自瞄模式: 转发视觉数据
+    msg.mode = latest_aim_data_.mode;
+    msg.yaw = latest_aim_data_.yaw;
+    msg.yaw_vel = latest_aim_data_.yaw_vel;
+    msg.yaw_acc = latest_aim_data_.yaw_acc;
+    msg.pitch = latest_aim_data_.pitch;
+    msg.pitch_vel = latest_aim_data_.pitch_vel;
+    msg.pitch_acc = latest_aim_data_.pitch_acc;
+  } else {
+    msg.mode = 0;
+  }
+
+  // 底盘速度: 来自 simple_move
+  // 在 HOLD_GAIN 且发现目标时停止移动(原地陀螺)
+  if (state_ == State::HOLD_GAIN && aim_target_found_) {
+    msg.vx = 0.0f;
+    msg.vy = 0.0f;
+    msg.wz = 0.0f;
+  } else if (state_ == State::MATCH_END) {
+    msg.vx = 0.0f;
+    msg.vy = 0.0f;
+    msg.wz = 0.0f;
+  } else {
+    msg.vx = static_cast<float>(latest_cmd_vel_.linear.x);
+    msg.vy = static_cast<float>(latest_cmd_vel_.linear.y);
+    msg.wz = static_cast<float>(latest_cmd_vel_.angular.z);
+  }
+
+  // reserved 标志位
+  uint8_t flags = 0;
+  if (enable_rotor_) flags |= 0x01;  // bit0: 底盘小陀螺
+  if (enable_search_) flags |= 0x02; // bit1: 云台搜索
+  msg.reserved = flags;
+
+  data_ai_pub_->publish(msg);
+}
+
+}  // namespace rm_decision
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(rm_decision::DecisionNode)

src/rm_decision/test/README.md

@@ -0,0 +1,79 @@
+# 简单导航测试脚本
+
+## 快速使用
+
+```bash
+# 直接运行
+python3 src/rm_decision/test/simple_nav_test.py
+
+# 或者通过 ros2 run
+ros2 run rm_decision simple_nav_test.py
+```
+
+## 修改目标点
+
+直接编辑 `simple_nav_test.py` 文件中的 `waypoints` 列表：
+
+```python
+# 格式: [x, y, angle, max_speed, tolerance, name]
+self.waypoints = [
+    [0.0, 0.0, 0.0, 1.0, 0.15, "起点"],
+    [1.0, 0.0, 0.0, 1.5, 0.15, "右"],
+    [1.0, 1.0, math.pi/2, 1.5, 0.15, "右上"],
+    [0.0, 1.0, math.pi, 1.5, 0.15, "左上"],
+    [0.0, 0.0, 0.0, 1.0, 0.15, "回到起点"],
+]
+```
+
+### 参数说明
+
+- `x, y`: 目标坐标（米）
+- `angle`: 目标角度（弧度）
+  - 0° = 0
+  - 90° = math.pi/2 = 1.57
+  - 180° = math.pi = 3.14
+  - -90° = -math.pi/2 = -1.57
+- `max_speed`: 最大速度（米/秒）
+- `tolerance`: 到达容差（米）
+- `name`: 目标点名称（用于日志显示）
+
+## 示例输出
+
+```
+[simple_nav_test]: ========================================
+[simple_nav_test]: 简单导航测试脚本启动
+[simple_nav_test]: 目标点数量: 5
+[simple_nav_test]: ========================================
+[simple_nav_test]:   [0] 起点: (0.00, 0.00, 0°) speed=1.0 tol=0.15
+[simple_nav_test]:   [1] 右: (1.00, 0.00, 0°) speed=1.5 tol=0.15
+[simple_nav_test]:   [2] 右上: (1.00, 1.00, 90°) speed=1.5 tol=0.15
+[simple_nav_test]:   [3] 左上: (0.00, 1.00, 180°) speed=1.5 tol=0.15
+[simple_nav_test]:   [4] 回到起点: (0.00, 0.00, 0°) speed=1.0 tol=0.15
+[simple_nav_test]: ========================================
+[simple_nav_test]: 📍 发送目标点 [1/5] 起点: (0.00, 0.00, 0°) speed=1.0
+[simple_nav_test]: 🚗 导航中 [1/5] 起点: 距离=0.50m 状态=1 (5s)
+[simple_nav_test]: ✓ 到达目标点 [1/5] 起点 (用时 8.2秒)
+[simple_nav_test]: 📍 发送目标点 [2/5] 右: (1.00, 0.00, 0°) speed=1.5
+...
+[simple_nav_test]: ✅ 所有目标点完成！
+```
+
+## 优点
+
+- ✅ 无需编译，直接运行
+- ✅ 修改目标点只需编辑 Python 代码
+- ✅ 代码简单易懂
+- ✅ 实时显示导航进度
+- ✅ 自动处理超时和失败
+
+## 与 C++ 版本对比
+
+| 特性 | Python 脚本 | C++ test_nav_node |
+|------|------------|-------------------|
+| 修改目标点 | 编辑 Python 代码 | 编辑 YAML 配置 |
+| 运行方式 | 直接运行 | 需要编译 |
+| 灵活性 | 高（可随时修改） | 中（需重新编译） |
+| 性能 | 足够 | 更高 |
+| 适用场景 | 快速测试 | 生产环境 |
+
+推荐使用 Python 脚本进行快速测试！

src/rm_decision/test/simple_nav_test.py

@@ -0,0 +1,174 @@
+#!/usr/bin/env python3
+"""
+简单的导航测试脚本
+直接在代码中定义目标点，通过 /nav_goal 话题发送给 simple_move
+"""
+
+import rclpy
+from rclpy.node import Node
+from rm_msgs.msg import NavGoal, NavStatus
+import time
+import math
+
+
+class SimpleNavTest(Node):
+    def __init__(self):
+        super().__init__('simple_nav_test')
+
+        # 发布者和订阅者
+        self.nav_goal_pub = self.create_publisher(NavGoal, '/nav_goal', 10)
+        self.nav_status_sub = self.create_subscription(
+            NavStatus, '/nav_status', self.nav_status_callback, 10)
+
+        # 状态
+        self.current_status = None
+        self.current_distance = 0.0
+        self.navigation_started = False  # 标记导航是否真正开始
+
+        # 定义目标点列表 [x, y, angle, max_speed, tolerance, name]
+        self.waypoints = [
+            [0.0, 0.0, 0.0, 0.8, 1.0, "起点"],
+            [0.12, -6.75, 0.0, 0.8, 1.0, "1"],
+            [1.65, -6.76, 0.0, 0.8, 1.0, "2"],
+            [1.15, 0.5, 0.0, 0.8, 1.0, "3"],
+            [2.70, 0.7, 0.0, 0.8, 1.0, "4"],
+            [2.10, -6.72, 0.0, 0.8, 1.0, "5"],
+            [3.56, -6.82, 0.0, 0.8, 1.0, "6"],
+            [3.2, 0.9, 0.0, 0.8, 1.0, "7"],
+            [5.0, 0.56, 0.0, 0.8, 1.0, "8"],
+            [4.96, -5.96, 0.0, 0.8, 1.0, "9"],
+            # [0.0, 0.0, 0.0, 1.0, 0.25, "回到起点"],
+        ]
+
+        self.current_index = 0
+        self.goal_sent = False
+        self.goal_timeout = 120.0  # 超时时间（秒）
+        self.goal_start_time = None
+
+        self.get_logger().info('========================================')
+        self.get_logger().info('简单导航测试脚本启动')
+        self.get_logger().info(f'目标点数量: {len(self.waypoints)}')
+        self.get_logger().info('========================================')
+
+        # 显示所有目标点
+        for i, wp in enumerate(self.waypoints):
+            self.get_logger().info(
+                f'  [{i}] {wp[5]}: ({wp[0]:.2f}, {wp[1]:.2f}, {wp[2]*180/math.pi:.0f}°) '
+                f'speed={wp[3]:.1f} tol={wp[4]:.2f}')
+
+        self.get_logger().info('========================================')
+
+        # 等待2秒后开始
+        time.sleep(2.0)
+        self.send_next_goal()
+
+        # 创建定时器检查状态
+        self.timer = self.create_timer(0.5, self.check_status)
+
+    def nav_status_callback(self, msg):
+        """接收导航状态"""
+        self.current_status = msg.status
+        self.current_distance = msg.distance
+
+    def send_next_goal(self):
+        """发送下一个目标点"""
+        if self.current_index >= len(self.waypoints):
+            self.get_logger().info('✅ 所有目标点完成！')
+            rclpy.shutdown()
+            return
+
+        wp = self.waypoints[self.current_index]
+
+        # 创建导航目标消息
+        goal_msg = NavGoal()
+        goal_msg.control_mode = 0  # 0: PID模式
+        goal_msg.x = float(wp[0])
+        goal_msg.y = float(wp[1])
+        goal_msg.angle = float(wp[2])
+        goal_msg.max_speed = float(wp[3])
+        goal_msg.tolerance = float(wp[4])
+
+        # 重置状态标志
+        self.navigation_started = False
+
+        # 发布目标
+        self.nav_goal_pub.publish(goal_msg)
+        self.goal_sent = True
+        self.goal_start_time = time.time()
+
+        self.get_logger().info(
+            f'📍 发送目标点 [{self.current_index + 1}/{len(self.waypoints)}] {wp[5]}: '
+            f'({wp[0]:.2f}, {wp[1]:.2f}, {wp[2]*180/math.pi:.0f}°) speed={wp[3]:.1f}')
+
+    def check_status(self):
+        """检查导航状态"""
+        if not self.goal_sent:
+            return
+
+        if self.current_status is None:
+            return
+
+        wp = self.waypoints[self.current_index]
+        elapsed = time.time() - self.goal_start_time
+
+        # 标记导航已开始（状态为1表示正在导航）
+        if self.current_status == 1:
+            self.navigation_started = True
+
+        # 检查是否到达 (status == 2)，但必须先经过导航中状态
+        if self.current_status == 2:
+            # 如果导航还没开始就到达，说明是误判，等待真正开始
+            if not self.navigation_started and elapsed < 2.0:
+                return
+
+            self.get_logger().info(
+                f'✓ 到达目标点 [{self.current_index + 1}/{len(self.waypoints)}] {wp[5]} '
+                f'(用时 {elapsed:.1f}秒)')
+            self.current_index += 1
+            self.goal_sent = False
+            # time.sleep(0.1)  # 短暂延迟，避免消息丢失
+            self.send_next_goal()
+            return
+
+        # 检查是否失败 (status == 3)
+        if self.current_status == 3:
+            self.get_logger().warn(
+                f'✗ 目标点 [{self.current_index + 1}/{len(self.waypoints)}] {wp[5]} 导航失败')
+            self.current_index += 1
+            self.goal_sent = False
+            # time.sleep(0.1)
+            self.send_next_goal()
+            return
+
+        # 检查超时
+        if elapsed > self.goal_timeout:
+            self.get_logger().warn(
+                f'⏱ 目标点 [{self.current_index + 1}/{len(self.waypoints)}] {wp[5]} '
+                f'超时 ({elapsed:.1f}秒)，跳过')
+            self.current_index += 1
+            self.goal_sent = False
+            # time.sleep(0.1)
+            self.send_next_goal()
+            return
+
+        # 每5秒显示一次进度
+        if int(elapsed) % 5 == 0 and int(elapsed * 2) % 2 == 0:
+            self.get_logger().info(
+                f'🚗 导航中 [{self.current_index + 1}/{len(self.waypoints)}] {wp[5]}: '
+                f'距离={self.current_distance:.2f}m 状态={self.current_status} ({elapsed:.0f}s)')
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    try:
+        node = SimpleNavTest()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        print('\n用户中断')
+    finally:
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/rm_msgs/.gitignore

@@ -0,0 +1,52 @@
+devel/
+logs/
+build/
+bin/
+lib/
+msg_gen/
+srv_gen/
+msg/*Action.msg
+msg/*ActionFeedback.msg
+msg/*ActionGoal.msg
+msg/*ActionResult.msg
+msg/*Feedback.msg
+msg/*Goal.msg
+!msg/NavGoal.msg
+msg/*Result.msg
+msg/_*.py
+build_isolated/
+devel_isolated/
+
+# Generated by dynamic reconfigure
+*.cfgc
+/cfg/cpp/
+/cfg/*.py
+
+# Ignore generated docs
+*.dox
+*.wikidoc
+
+# eclipse stuff
+.project
+.cproject
+
+# qcreator stuff
+CMakeLists.txt.user
+
+srv/_*.py
+*.pcd
+*.pyc
+qtcreator-*
+*.user
+
+/planning/cfg
+/planning/docs
+/planning/src
+
+*~
+
+# Emacs
+.#*
+
+# Catkin custom files
+CATKIN_IGNORE

src/rm_msgs/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.8)
+project(rm_msgs)
+
+find_package(rosidl_default_generators REQUIRED)
+rosidl_generate_interfaces(${PROJECT_NAME}
+        "msg/DataMCU.msg"
+        "msg/DataRef.msg"
+        "msg/DataAI.msg"
+        "msg/DataAim.msg"
+        "msg/NavGoal.msg"
+        "msg/NavStatus.msg"
+)
+
+ament_package()
+

src/rm_msgs/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2025 zucheng Lv
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

src/rm_msgs/README.md

@@ -0,0 +1,6 @@
+# rm_msg
+Some ROS 2 custom messages for Robotaster
+
+Usage
+Modify or add files in the /msg directory as needed
+colcon build

src/rm_msgs/msg/DataAI.msg

@@ -0,0 +1,13 @@
+# AI 控制数据 (上位机 -> MCU)
+
+uint8 mode          # 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火
+float32 yaw         # 目标偏航角
+float32 yaw_vel     # 偏航角速度
+float32 yaw_acc     # 偏航角加速度
+float32 pitch       # 目标俯仰角
+float32 pitch_vel   # 俯仰角速度
+float32 pitch_acc   # 俯仰角加速度
+float32 vx          # X 方向速度
+float32 vy          # Y 方向速度
+float32 wz          # Z 方向角速度
+uint8 reserved      # 预留字段（用于传递底盘模式等）

src/rm_msgs/msg/DataAim.msg

@@ -0,0 +1,9 @@
+# 视觉自瞄控制数据
+
+uint8 mode          # 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火
+float32 yaw         # 目标偏航角
+float32 yaw_vel     # 偏航角速度
+float32 yaw_acc     # 偏航角加速度
+float32 pitch       # 目标俯仰角
+float32 pitch_vel   # 俯仰角速度
+float32 pitch_acc   # 俯仰角加速度

src/rm_msgs/msg/DataMCU.msg

@@ -0,0 +1,13 @@
+# MCU 状态数据 (MCU -> 上位机)
+
+uint8 mode              # 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符
+float32 q0              # 四元数 w
+float32 q1              # 四元数 x
+float32 q2              # 四元数 y
+float32 q3              # 四元数 z
+float32 yaw             # 偏航角
+float32 yaw_vel         # 偏航角速度
+float32 pitch           # 俯仰角
+float32 pitch_vel       # 俯仰角速度
+float32 bullet_speed    # 弹速
+uint16 bullet_count     # 子弹累计发送次数

src/rm_msgs/msg/DataRef.msg

@@ -0,0 +1,5 @@
+# 裁判系统数据
+
+uint16 remain_hp            # 剩余血量
+uint8 game_progress         # 比赛进度
+uint16 stage_remain_time    # 比赛剩余时间

src/rm_msgs/msg/NavGoal.msg

@@ -0,0 +1,11 @@
+# 导航目标
+
+uint8 MODE_PID=0
+uint8 MODE_NAV=1
+
+uint8 control_mode      # 控制模式: 0=PID, 1=NAV
+float32 x               # 目标 X 坐标 (map)
+float32 y               # 目标 Y 坐标 (map)
+float32 angle           # 目标角度 (-pi ~ pi)
+float32 max_speed       # 最大线速度 (PID 模式使用)
+float32 tolerance       # 到达容差

src/rm_msgs/msg/NavStatus.msg

@@ -0,0 +1,7 @@
+# 导航状态
+
+uint8 status            # 0: 空闲, 1: 运行中, 2: 到达目标, 3: 失败
+float32 distance        # 距离目标的距离
+float32 current_x       # 当前 X 坐标
+float32 current_y       # 当前 Y 坐标
+float32 current_angle   # 当前角度

src/rm_msgs/package.xml

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<package format="3">
+    <name>rm_msgs</name>
+    <version>0.0.1</version>
+    <description>Describe custom messages</description>
+    <maintainer email="1683502971@qq.com">biao</maintainer>
+    <license>MIT</license>
+
+    <buildtool_depend>rosidl_default_generators</buildtool_depend>
+    <exec_depend>rosidl_default_runtime</exec_depend>
+    <member_of_group>rosidl_interface_packages</member_of_group>
+
+    <export>
+        <build_type>ament_cmake</build_type>
+    </export>
+</package>

src/rm_nav/.gitignore

@@ -0,0 +1,173 @@
+# ---> C
+# Prerequisites
+*.d
+
+# Object files
+*.o
+*.ko
+*.obj
+*.elf
+
+# Linker output
+*.ilk
+*.map
+*.exp
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Libraries
+*.lib
+*.a
+*.la
+*.lo
+
+# Shared objects (inc. Windows DLLs)
+*.dll
+*.so
+*.so.*
+*.dylib
+
+# Executables
+*.exe
+*.out
+*.app
+*.i*86
+*.x86_64
+*.hex
+
+# Debug files
+*.dSYM/
+*.su
+*.idb
+*.pdb
+
+# Kernel Module Compile Results
+*.mod*
+*.cmd
+.tmp_versions/
+modules.order
+Module.symvers
+Mkfile.old
+dkms.conf
+
+# ---> C++
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+# ---> ROS
+devel/
+logs/
+build/
+bin/
+lib/
+msg_gen/
+srv_gen/
+msg/*Action.msg
+msg/*ActionFeedback.msg
+msg/*ActionGoal.msg
+msg/*ActionResult.msg
+msg/*Feedback.msg
+msg/*Goal.msg
+msg/*Result.msg
+msg/_*.py
+build_isolated/
+devel_isolated/
+
+# Generated by dynamic reconfigure
+*.cfgc
+/cfg/cpp/
+/cfg/*.py
+
+# Ignore generated docs
+*.dox
+*.wikidoc
+
+# eclipse stuff
+.project
+.cproject
+
+# qcreator stuff
+CMakeLists.txt.user
+
+srv/_*.py
+# *.pcd
+*.pyc
+qtcreator-*
+*.user
+
+/planning/cfg
+/planning/docs
+/planning/src
+
+*~
+
+# Emacs
+.#*
+
+# Catkin custom files
+CATKIN_IGNORE
+
+# ---> ROS2
+install/
+log/
+build/
+
+# Ignore generated docs
+*.dox
+*.wikidoc
+
+# eclipse stuff
+.project
+.cproject
+
+# qcreator stuff
+CMakeLists.txt.user
+
+srv/_*.py
+# *.pcd
+*.pyc
+qtcreator-*
+*.user
+
+*~
+
+# Emacs
+.#*
+
+# Colcon custom files
+COLCON_IGNORE
+AMENT_IGNORE
+
+.claude/

src/rm_nav/README.md

@@ -0,0 +1,20 @@
+# MOVE_AI
+
+青岛理工大学QUT
+MOVE战队
+RoboMaster比赛
+纯mid360激光雷达导航项目
+
+
+ros2 launch rm_nav_bringup bringup_real.launch.py \
+    world:=RUML \
+    mode:=nav \
+    lio:=fastlio \
+    lio_rviz:=True \
+    nav_rviz:=True
+
+
+
+    sudo mv /opt/MVS/lib/64/libusb-1.0.so.0 /opt/MVS/lib/64/libusb-1.0.so.0.bak
+[sudo] robofish 的密码： 
+robofish@robofish-ASUS-TUF-Gaming-F15-FX507ZM-FX507ZM:~/MOVE_AI$ sudo mv /opt/MVS/lib/32/libusb-1.0.so.0 /opt/MVS/lib/32/libusb-1.0.so.0.bak