上决策

2026-03-20 06:18:15 +08:00 · 2026-03-20 06:18:15 +08:00 · e7c6ad8511
commit e7c6ad8511
parent 589161596d
4 changed files with 535 additions and 99 deletions
--- a/src/rm_decision/README.md
+++ b/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] | 增益点坐标 |
--- a/src/rm_decision/include/rm_decision/decision_node.hpp
+++ b/src/rm_decision/include/rm_decision/decision_node.hpp
@ -10,20 +10,33 @@
 #include <geometry_msgs/msg/twist.hpp>
 #include <atomic>
 #include <mutex>
+#include <random>
+#include <chrono>

 namespace rm_decision
 {

-/**
- * @brief 决策节点 - 系统核心控制器
- *
- * 功能：
- * 1. 接收 MCU 状态和裁判系统数据
- * 2. 接收视觉自瞄控制指令
- * 3. 接收导航状态反馈
- * 4. 控制导航目标和底盘模式
- * 5. 综合所有信息生成最终控制指令发送给下位机
- */
+// 决策状态机
+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:
@ -37,7 +50,6 @@ private:
  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();

@ -45,6 +57,10 @@ private:
  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_;
@ -59,7 +75,7 @@ private:
  // 定时器
  rclcpp::TimerBase::SharedPtr control_timer_;

-  // 状态数据
+  // 状态数据 (mutex保护)
  std::mutex mutex_;
  rm_msgs::msg::DataMCU latest_mcu_data_;
  rm_msgs::msg::DataRef latest_ref_data_;
@ -67,12 +83,41 @@ private:
  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};
-  std::atomic<uint8_t> chassis_mode_{0};  // 底盘模式
-  std::atomic<bool> enable_aim_{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
--- a/src/rm_decision/src/decision_node.cpp
+++ b/src/rm_decision/src/decision_node.cpp
@ -4,37 +4,45 @@ namespace rm_decision
 {

 DecisionNode::DecisionNode(const rclcpp::NodeOptions &options)
-  : Node("rm_decision", options)
+  : Node("rm_decision", options),
+    rng_(std::chrono::steady_clock::now().time_since_epoch().count())
 {
  // 声明参数
-  this->declare_parameter("control_frequency", 50.0);  // 控制频率 50Hz
-  this->declare_parameter("enable_aim", true);         // 默认启用自瞄
-  this->declare_parameter("chassis_mode", 0);          // 默认手动模式
+  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();
-  enable_aim_ = this->get_parameter("enable_aim").as_bool();
-  chassis_mode_ = this->get_parameter("chassis_mode").as_int();
+  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_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_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));
-
+    "/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));
-
+    "/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));
+    "/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);
@ -46,21 +54,13 @@ DecisionNode::DecisionNode(const rclcpp::NodeOptions &options)
    std::chrono::duration_cast<std::chrono::milliseconds>(period),
    std::bind(&DecisionNode::control_timer_callback, this));

-  RCLCPP_INFO(this->get_logger(), "========================================");
-  RCLCPP_INFO(this->get_logger(), "Decision Node 启动");
-  RCLCPP_INFO(this->get_logger(), "  控制频率: %.1f Hz", control_freq);
-  RCLCPP_INFO(this->get_logger(), "  自瞄启用: %s", enable_aim_ ? "是" : "否");
-  RCLCPP_INFO(this->get_logger(), "  底盘模式: %d", chassis_mode_.load());
-  RCLCPP_INFO(this->get_logger(), "========================================");
-  RCLCPP_INFO(this->get_logger(), "话题配置:");
-  RCLCPP_INFO(this->get_logger(), "  订阅: /data_mcu (MCU状态)");
-  RCLCPP_INFO(this->get_logger(), "  订阅: /data_ref (裁判系统)");
-  RCLCPP_INFO(this->get_logger(), "  订阅: /vision/aim (视觉自瞄)");
-  RCLCPP_INFO(this->get_logger(), "  订阅: /nav_status (导航状态)");
-  RCLCPP_INFO(this->get_logger(), "  订阅: /cmd_vel_move (底盘速度)");
-  RCLCPP_INFO(this->get_logger(), "  发布: /data_ai (综合控制指令)");
-  RCLCPP_INFO(this->get_logger(), "  发布: /nav_goal (导航目标)");
-  RCLCPP_INFO(this->get_logger(), "========================================");
+  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()
@ -68,6 +68,8 @@ 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_);
@ -101,77 +103,350 @@ void DecisionNode::cmd_vel_move_callback(const geometry_msgs::msg::Twist::Shared
  latest_cmd_vel_ = *msg;
 }

-void DecisionNode::control_timer_callback()
+// ==================== 辅助函数 ====================
+
+void DecisionNode::send_nav_goal(const Waypoint &wp, uint8_t mode, float max_speed, float tolerance)
 {
-  // 检查是否有必要的数据
-  if (!has_mcu_data_) {
-    return;
+  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);
 }

-  // 执行决策逻辑
-  make_decision();
+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()
 {
-  // 这里实现决策逻辑
-  // 示例：根据裁判系统数据、导航状态等做决策
-
-  // 示例决策逻辑：
-  // 1. 如果导航到达目标，切换到自瞄模式
-  // 2. 如果血量低，切换到防御模式
-  // 3. 根据比赛阶段调整策略
-
  std::lock_guard<std::mutex> lock(mutex_);

-  // 示例：根据导航状态决定是否启用自瞄
-  if (has_nav_data_ && latest_nav_status_.status == 2) {  // 到达目标
+  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 data_ai_msg = rm_msgs::msg::DataAI();
+  auto msg = rm_msgs::msg::DataAI();

-  // 1. 云台控制：优先使用视觉自瞄数据
-  if (enable_aim_ && has_aim_data_) {
-    // 使用视觉自瞄控制
-    data_ai_msg.mode = latest_aim_data_.mode;
-    data_ai_msg.yaw = latest_aim_data_.yaw;
-    data_ai_msg.yaw_vel = latest_aim_data_.yaw_vel;
-    data_ai_msg.yaw_acc = latest_aim_data_.yaw_acc;
-    data_ai_msg.pitch = latest_aim_data_.pitch;
-    data_ai_msg.pitch_vel = latest_aim_data_.pitch_vel;
-    data_ai_msg.pitch_acc = latest_aim_data_.pitch_acc;
+  // 云台控制
+  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 {
-    // 不控制云台
-    data_ai_msg.mode = 0;
-    data_ai_msg.yaw = 0.0f;
-    data_ai_msg.yaw_vel = 0.0f;
-    data_ai_msg.yaw_acc = 0.0f;
-    data_ai_msg.pitch = 0.0f;
-    data_ai_msg.pitch_vel = 0.0f;
-    data_ai_msg.pitch_acc = 0.0f;
+    msg.mode = 0;
  }

-  // 2. 底盘控制：使用 rm_simple_move 输出的 /cmd_vel_move
-  data_ai_msg.vx = static_cast<float>(latest_cmd_vel_.linear.x);
-  data_ai_msg.vy = static_cast<float>(latest_cmd_vel_.linear.y);
-  data_ai_msg.wz = static_cast<float>(latest_cmd_vel_.angular.z);
+  // 底盘速度: 来自 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);
+  }

-  // 3. 底盘模式：通过 reserved 字段传递
-  data_ai_msg.reserved = chassis_mode_;
+  // reserved 标志位
+  uint8_t flags = 0;
+  if (enable_rotor_) flags |= 0x01;  // bit0: 底盘小陀螺
+  if (enable_search_) flags |= 0x02; // bit1: 云台搜索
+  msg.reserved = flags;

-  // 发布综合控制指令
-  data_ai_pub_->publish(data_ai_msg);
+  data_ai_pub_->publish(msg);
 }

 }  // namespace rm_decision
--- a/src/rm_serial_driver/include/rm_serial_driver/protocol.hpp
+++ b/src/rm_serial_driver/include/rm_serial_driver/protocol.hpp
@ -43,6 +43,15 @@ typedef struct __attribute__((packed))
 } PackageReferee_t;

 // AI 控制数据结构（上位机 -> MCU）
+
+#define AI_CHASSIS_ROTOR (1u << 0)  // bit0: 底盘小陀螺
+#define AI_GIMBAL_SEARCH (1u << 1)  // bit1: 云台搜索模式
+#define AI_RESERVED_BIT2 (1u << 2)  // bit2: 预留
+#define AI_RESERVED_BIT3 (1u << 3)  // bit3: 预留
+#define AI_RESERVED_BIT4 (1u << 4)  // bit4: 预留
+#define AI_RESERVED_BIT5 (1u << 5)  // bit5: 预留
+#define AI_RESERVED_BIT6 (1u << 6)  // bit6: 预留
+#define AI_RESERVED_BIT7 (1u << 7)  // bit7: 预留
 typedef struct __attribute__((packed))
 {
    uint8_t mode;        // 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火
@ -55,7 +64,7 @@ typedef struct __attribute__((packed))
    float vx;            // X 方向速度
    float vy;            // Y 方向速度
    float wz;            // Z 方向角速度
-    uint8_t reserved;    // 预留字段
+    uint8_t reserved;    // 标志位: bit0-底盘小陀螺 | bit1-云台搜索 | bit2~7-预留
 } DataAI_t;

 typedef struct __attribute__((packed))