robofish/MOVE_AI
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

上决策

Browse Source
This commit is contained in:
Robofish 2026-03-20 06:18:15 +08:00
parent 589161596d
commit e7c6ad8511
4 changed files with 535 additions and 99 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

107
src/rm_decision/README.md Normal file
View File

@ -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] | 增益点坐标 |

75
src/rm_decision/include/rm_decision/decision_node.hpp
View File

@ -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

437
src/rm_decision/src/decision_node.cpp
View File

@ -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::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<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

11
src/rm_serial_driver/include/rm_serial_driver/protocol.hpp
View File

@ -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))