diff --git a/src/rm_decision/README.md b/src/rm_decision/README.md new file mode 100644 index 0000000..3d24068 --- /dev/null +++ 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] | 增益点坐标 | diff --git a/src/rm_decision/include/rm_decision/decision_node.hpp b/src/rm_decision/include/rm_decision/decision_node.hpp index 2024d66..e91cb9b 100644 --- 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 #include #include +#include +#include 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::SharedPtr data_mcu_sub_; rclcpp::Subscription::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 has_mcu_data_{false}; std::atomic has_aim_data_{false}; std::atomic has_nav_data_{false}; - std::atomic chassis_mode_{0}; // 底盘模式 - std::atomic 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 patrol_dist_{-0.5f, 0.5f}; + + // 导航速度参数 + float nav_max_speed_{2.0f}; + float nav_tolerance_{0.5f}; }; } // namespace rm_decision diff --git a/src/rm_decision/src/decision_node.cpp b/src/rm_decision/src/decision_node.cpp index 1796846..8c27d4b 100644 --- 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{-0.098266, 0.0092684, -0.074146}); + this->declare_parameter("wp_point1", std::vector{0.76813, -4.5578, -0.23769}); + this->declare_parameter("wp_point2", std::vector{4.9211, -4.5856, -0.27894}); + this->declare_parameter("wp_gain", std::vector{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(v[0]), static_cast(v[1]), static_cast(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( - "/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( - "/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( - "/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( - "/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( - "/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("/data_ai", 10); @@ -46,21 +54,13 @@ DecisionNode::DecisionNode(const rclcpp::NodeOptions &options) std::chrono::duration_cast(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 lock(mutex_); @@ -101,77 +103,350 @@ void DecisionNode::cmd_vel_move_callback(const geometry_msgs::msg::Twist::Shared 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; - } - - // 执行决策逻辑 + if (!has_mcu_data_) return; make_decision(); - - // 发送控制指令 send_control_command(); } +// ==================== 决策逻辑 ==================== + void DecisionNode::make_decision() { - // 这里实现决策逻辑 - // 示例：根据裁判系统数据、导航状态等做决策 - - // 示例决策逻辑： - // 1. 如果导航到达目标，切换到自瞄模式 - // 2. 如果血量低，切换到防御模式 - // 3. 根据比赛阶段调整策略 - std::lock_guard lock(mutex_); - // 示例：根据导航状态决定是否启用自瞄 - if (has_nav_data_ && latest_nav_status_.status == 2) { // 到达目标 - enable_aim_ = true; - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, - "导航到达目标，启用自瞄"); + 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(prev_state), static_cast(state_)); } } +// ==================== 发送控制指令 ==================== + void DecisionNode::send_control_command() { std::lock_guard 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(latest_cmd_vel_.linear.x); - data_ai_msg.vy = static_cast(latest_cmd_vel_.linear.y); - data_ai_msg.wz = static_cast(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(latest_cmd_vel_.linear.x); + msg.vy = static_cast(latest_cmd_vel_.linear.y); + msg.wz = static_cast(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 diff --git a/src/rm_serial_driver/include/rm_serial_driver/protocol.hpp b/src/rm_serial_driver/include/rm_serial_driver/protocol.hpp index 1a3329e..9766991 100644 --- 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))