上决策
This commit is contained in:
parent
589161596d
commit
e7c6ad8511
107
src/rm_decision/README.md
Normal file
107
src/rm_decision/README.md
Normal 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] | 增益点坐标 |
|
||||||
@ -10,20 +10,33 @@
|
|||||||
#include <geometry_msgs/msg/twist.hpp>
|
#include <geometry_msgs/msg/twist.hpp>
|
||||||
#include <atomic>
|
#include <atomic>
|
||||||
#include <mutex>
|
#include <mutex>
|
||||||
|
#include <random>
|
||||||
|
#include <chrono>
|
||||||
|
|
||||||
namespace rm_decision
|
namespace rm_decision
|
||||||
{
|
{
|
||||||
|
|
||||||
/**
|
// 决策状态机
|
||||||
* @brief 决策节点 - 系统核心控制器
|
enum class State : uint8_t
|
||||||
*
|
{
|
||||||
* 功能：
|
IDLE, // 等待比赛开始 (game_progress 0~3)
|
||||||
* 1. 接收 MCU 状态和裁判系统数据
|
GO_POINT1, // 前往1号点
|
||||||
* 2. 接收视觉自瞄控制指令
|
GO_POINT2, // 前往2号点
|
||||||
* 3. 接收导航状态反馈
|
GO_GAIN, // 前往增益点
|
||||||
* 4. 控制导航目标和底盘模式
|
HOLD_GAIN, // 占领增益点 (陀螺 + search/自瞄)
|
||||||
* 5. 综合所有信息生成最终控制指令发送给下位机
|
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
|
class DecisionNode : public rclcpp::Node
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
@ -37,7 +50,6 @@ private:
|
|||||||
void data_aim_callback(const rm_msgs::msg::DataAim::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 nav_status_callback(const rm_msgs::msg::NavStatus::SharedPtr msg);
|
||||||
void cmd_vel_move_callback(const geometry_msgs::msg::Twist::SharedPtr msg);
|
void cmd_vel_move_callback(const geometry_msgs::msg::Twist::SharedPtr msg);
|
||||||
|
|
||||||
// 定时器回调
|
// 定时器回调
|
||||||
void control_timer_callback();
|
void control_timer_callback();
|
||||||
|
|
||||||
@ -45,6 +57,10 @@ private:
|
|||||||
void make_decision();
|
void make_decision();
|
||||||
void send_control_command();
|
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::DataMCU>::SharedPtr data_mcu_sub_;
|
||||||
rclcpp::Subscription<rm_msgs::msg::DataRef>::SharedPtr data_ref_sub_;
|
rclcpp::Subscription<rm_msgs::msg::DataRef>::SharedPtr data_ref_sub_;
|
||||||
@ -59,7 +75,7 @@ private:
|
|||||||
// 定时器
|
// 定时器
|
||||||
rclcpp::TimerBase::SharedPtr control_timer_;
|
rclcpp::TimerBase::SharedPtr control_timer_;
|
||||||
|
|
||||||
// 状态数据
|
// 状态数据 (mutex保护)
|
||||||
std::mutex mutex_;
|
std::mutex mutex_;
|
||||||
rm_msgs::msg::DataMCU latest_mcu_data_;
|
rm_msgs::msg::DataMCU latest_mcu_data_;
|
||||||
rm_msgs::msg::DataRef latest_ref_data_;
|
rm_msgs::msg::DataRef latest_ref_data_;
|
||||||
@ -67,12 +83,41 @@ private:
|
|||||||
rm_msgs::msg::NavStatus latest_nav_status_;
|
rm_msgs::msg::NavStatus latest_nav_status_;
|
||||||
geometry_msgs::msg::Twist latest_cmd_vel_;
|
geometry_msgs::msg::Twist latest_cmd_vel_;
|
||||||
|
|
||||||
// 控制状态
|
// 原子状态标志
|
||||||
std::atomic<bool> has_mcu_data_{false};
|
std::atomic<bool> has_mcu_data_{false};
|
||||||
std::atomic<bool> has_aim_data_{false};
|
std::atomic<bool> has_aim_data_{false};
|
||||||
std::atomic<bool> has_nav_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
|
} // namespace rm_decision
|
||||||
|
|||||||
@ -4,37 +4,45 @@ namespace rm_decision
|
|||||||
{
|
{
|
||||||
|
|
||||||
DecisionNode::DecisionNode(const rclcpp::NodeOptions &options)
|
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("control_frequency", 50.0);
|
||||||
this->declare_parameter("enable_aim", true); // 默认启用自瞄
|
this->declare_parameter("nav_max_speed", 2.0);
|
||||||
this->declare_parameter("chassis_mode", 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();
|
auto control_freq = this->get_parameter("control_frequency").as_double();
|
||||||
enable_aim_ = this->get_parameter("enable_aim").as_bool();
|
nav_max_speed_ = this->get_parameter("nav_max_speed").as_double();
|
||||||
chassis_mode_ = this->get_parameter("chassis_mode").as_int();
|
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_sub_ = this->create_subscription<rm_msgs::msg::DataMCU>(
|
||||||
"/data_mcu", 10,
|
"/data_mcu", 10, std::bind(&DecisionNode::data_mcu_callback, this, std::placeholders::_1));
|
||||||
std::bind(&DecisionNode::data_mcu_callback, this, std::placeholders::_1));
|
|
||||||
|
|
||||||
data_ref_sub_ = this->create_subscription<rm_msgs::msg::DataRef>(
|
data_ref_sub_ = this->create_subscription<rm_msgs::msg::DataRef>(
|
||||||
"/data_ref", 10,
|
"/data_ref", 10, std::bind(&DecisionNode::data_ref_callback, this, std::placeholders::_1));
|
||||||
std::bind(&DecisionNode::data_ref_callback, this, std::placeholders::_1));
|
|
||||||
|
|
||||||
data_aim_sub_ = this->create_subscription<rm_msgs::msg::DataAim>(
|
data_aim_sub_ = this->create_subscription<rm_msgs::msg::DataAim>(
|
||||||
"/data_aim", 10,
|
"/data_aim", 10, std::bind(&DecisionNode::data_aim_callback, this, std::placeholders::_1));
|
||||||
std::bind(&DecisionNode::data_aim_callback, this, std::placeholders::_1));
|
|
||||||
|
|
||||||
nav_status_sub_ = this->create_subscription<rm_msgs::msg::NavStatus>(
|
nav_status_sub_ = this->create_subscription<rm_msgs::msg::NavStatus>(
|
||||||
"/nav_status", 10,
|
"/nav_status", 10, std::bind(&DecisionNode::nav_status_callback, this, std::placeholders::_1));
|
||||||
std::bind(&DecisionNode::nav_status_callback, this, std::placeholders::_1));
|
|
||||||
|
|
||||||
cmd_vel_move_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
|
cmd_vel_move_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
|
||||||
"/cmd_vel_move", 10,
|
"/cmd_vel_move", 10, std::bind(&DecisionNode::cmd_vel_move_callback, this, std::placeholders::_1));
|
||||||
std::bind(&DecisionNode::cmd_vel_move_callback, this, std::placeholders::_1));
|
|
||||||
|
|
||||||
// 创建发布者
|
// 创建发布者
|
||||||
data_ai_pub_ = this->create_publisher<rm_msgs::msg::DataAI>("/data_ai", 10);
|
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::chrono::duration_cast<std::chrono::milliseconds>(period),
|
||||||
std::bind(&DecisionNode::control_timer_callback, this));
|
std::bind(&DecisionNode::control_timer_callback, this));
|
||||||
|
|
||||||
RCLCPP_INFO(this->get_logger(), "========================================");
|
aim_lost_time_ = this->now();
|
||||||
RCLCPP_INFO(this->get_logger(), "Decision Node 启动");
|
|
||||||
RCLCPP_INFO(this->get_logger(), " 控制频率: %.1f Hz", control_freq);
|
RCLCPP_INFO(this->get_logger(), "Decision Node 启动 | 频率: %.0fHz", control_freq);
|
||||||
RCLCPP_INFO(this->get_logger(), " 自瞄启用: %s", enable_aim_ ? "是" : "否");
|
RCLCPP_INFO(this->get_logger(), " 家: (%.3f, %.3f)", wp_home_.x, wp_home_.y);
|
||||||
RCLCPP_INFO(this->get_logger(), " 底盘模式: %d", chassis_mode_.load());
|
RCLCPP_INFO(this->get_logger(), " 1号: (%.3f, %.3f)", wp_point1_.x, wp_point1_.y);
|
||||||
RCLCPP_INFO(this->get_logger(), "========================================");
|
RCLCPP_INFO(this->get_logger(), " 2号: (%.3f, %.3f)", wp_point2_.x, wp_point2_.y);
|
||||||
RCLCPP_INFO(this->get_logger(), "话题配置:");
|
RCLCPP_INFO(this->get_logger(), " 增益: (%.3f, %.3f)", wp_gain_.x, wp_gain_.y);
|
||||||
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(), "========================================");
|
|
||||||
}
|
}
|
||||||
|
|
||||||
DecisionNode::~DecisionNode()
|
DecisionNode::~DecisionNode()
|
||||||
@ -68,6 +68,8 @@ DecisionNode::~DecisionNode()
|
|||||||
RCLCPP_INFO(this->get_logger(), "Decision Node 关闭");
|
RCLCPP_INFO(this->get_logger(), "Decision Node 关闭");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// ==================== 回调函数 ====================
|
||||||
|
|
||||||
void DecisionNode::data_mcu_callback(const rm_msgs::msg::DataMCU::SharedPtr msg)
|
void DecisionNode::data_mcu_callback(const rm_msgs::msg::DataMCU::SharedPtr msg)
|
||||||
{
|
{
|
||||||
std::lock_guard<std::mutex> lock(mutex_);
|
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;
|
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()
|
void DecisionNode::control_timer_callback()
|
||||||
{
|
{
|
||||||
// 检查是否有必要的数据
|
if (!has_mcu_data_) return;
|
||||||
if (!has_mcu_data_) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 执行决策逻辑
|
|
||||||
make_decision();
|
make_decision();
|
||||||
|
|
||||||
// 发送控制指令
|
|
||||||
send_control_command();
|
send_control_command();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// ==================== 决策逻辑 ====================
|
||||||
|
|
||||||
void DecisionNode::make_decision()
|
void DecisionNode::make_decision()
|
||||||
{
|
{
|
||||||
// 这里实现决策逻辑
|
|
||||||
// 示例：根据裁判系统数据、导航状态等做决策
|
|
||||||
|
|
||||||
// 示例决策逻辑：
|
|
||||||
// 1. 如果导航到达目标，切换到自瞄模式
|
|
||||||
// 2. 如果血量低，切换到防御模式
|
|
||||||
// 3. 根据比赛阶段调整策略
|
|
||||||
|
|
||||||
std::lock_guard<std::mutex> lock(mutex_);
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
|
||||||
// 示例：根据导航状态决定是否启用自瞄
|
uint8_t game_progress = latest_ref_data_.game_progress;
|
||||||
if (has_nav_data_ && latest_nav_status_.status == 2) { // 到达目标
|
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;
|
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,
|
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()
|
void DecisionNode::send_control_command()
|
||||||
{
|
{
|
||||||
std::lock_guard<std::mutex> lock(mutex_);
|
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_) {
|
if (enable_aim_ && aim_target_found_ && has_aim_data_) {
|
||||||
// 使用视觉自瞄控制
|
// 自瞄模式: 转发视觉数据
|
||||||
data_ai_msg.mode = latest_aim_data_.mode;
|
msg.mode = latest_aim_data_.mode;
|
||||||
data_ai_msg.yaw = latest_aim_data_.yaw;
|
msg.yaw = latest_aim_data_.yaw;
|
||||||
data_ai_msg.yaw_vel = latest_aim_data_.yaw_vel;
|
msg.yaw_vel = latest_aim_data_.yaw_vel;
|
||||||
data_ai_msg.yaw_acc = latest_aim_data_.yaw_acc;
|
msg.yaw_acc = latest_aim_data_.yaw_acc;
|
||||||
data_ai_msg.pitch = latest_aim_data_.pitch;
|
msg.pitch = latest_aim_data_.pitch;
|
||||||
data_ai_msg.pitch_vel = latest_aim_data_.pitch_vel;
|
msg.pitch_vel = latest_aim_data_.pitch_vel;
|
||||||
data_ai_msg.pitch_acc = latest_aim_data_.pitch_acc;
|
msg.pitch_acc = latest_aim_data_.pitch_acc;
|
||||||
} else {
|
} else {
|
||||||
// 不控制云台
|
msg.mode = 0;
|
||||||
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;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// 2. 底盘控制：使用 rm_simple_move 输出的 /cmd_vel_move
|
// 底盘速度: 来自 simple_move
|
||||||
data_ai_msg.vx = static_cast<float>(latest_cmd_vel_.linear.x);
|
// 在 HOLD_GAIN 且发现目标时停止移动(原地陀螺)
|
||||||
data_ai_msg.vy = static_cast<float>(latest_cmd_vel_.linear.y);
|
if (state_ == State::HOLD_GAIN && aim_target_found_) {
|
||||||
data_ai_msg.wz = static_cast<float>(latest_cmd_vel_.angular.z);
|
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 字段传递
|
// reserved 标志位
|
||||||
data_ai_msg.reserved = chassis_mode_;
|
uint8_t flags = 0;
|
||||||
|
if (enable_rotor_) flags |= 0x01; // bit0: 底盘小陀螺
|
||||||
|
if (enable_search_) flags |= 0x02; // bit1: 云台搜索
|
||||||
|
msg.reserved = flags;
|
||||||
|
|
||||||
// 发布综合控制指令
|
data_ai_pub_->publish(msg);
|
||||||
data_ai_pub_->publish(data_ai_msg);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace rm_decision
|
} // namespace rm_decision
|
||||||
|
|||||||
@ -43,6 +43,15 @@ typedef struct __attribute__((packed))
|
|||||||
} PackageReferee_t;
|
} PackageReferee_t;
|
||||||
|
|
||||||
// AI 控制数据结构（上位机 -> MCU）
|
// 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))
|
typedef struct __attribute__((packed))
|
||||||
{
|
{
|
||||||
uint8_t mode; // 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火
|
uint8_t mode; // 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火
|
||||||
@ -55,7 +64,7 @@ typedef struct __attribute__((packed))
|
|||||||
float vx; // X 方向速度
|
float vx; // X 方向速度
|
||||||
float vy; // Y 方向速度
|
float vy; // Y 方向速度
|
||||||
float wz; // Z 方向角速度
|
float wz; // Z 方向角速度
|
||||||
uint8_t reserved; // 预留字段
|
uint8_t reserved; // 标志位: bit0-底盘小陀螺 | bit1-云台搜索 | bit2~7-预留
|
||||||
} DataAI_t;
|
} DataAI_t;
|
||||||
|
|
||||||
typedef struct __attribute__((packed))
|
typedef struct __attribute__((packed))
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user