Dev/zlq 7 27

src/cxx/component.cc

@@ -10,6 +10,9 @@
 #include <rmcs_executor/component.hpp>
 #include <rmcs_msgs/rmcs_msgs.hpp>
 
+#include <string>
+#include <unordered_map>
+
 namespace rmcs::navigation {
 
 class Navigation final
@@ -22,6 +25,7 @@ class Navigation final
     static inline auto kVecNaN = Eigen::Vector2d{kNan, kNan};
 
     std::atomic<std::uint16_t> lua_tick_count = 0;
+    double pending_climb_world_yaw = kNan;
 
     details::LuaContext lua{*this};
     details::Navigation nav{*this};
@@ -31,37 +35,40 @@ class Navigation final
 
     struct Command {
         using ChassisMode = rmcs_msgs::ChassisMode;
+        using SentryEventCounts = std::unordered_map<rmcs_msgs::SentryEvent, std::uint16_t>;
 
         OutputInterface<bool> enable_control;
         OutputInterface<bool> enable_autoaim;
+        OutputInterface<bool> enable_supercap_;
         OutputInterface<ChassisMode> chassis_behavior;
         OutputInterface<Eigen::Vector2d> chassis_speed;
         OutputInterface<Eigen::Vector2d> gimbal_toward;
         OutputInterface<double> climb_cross_direction;
         OutputInterface<bool> climb_is_climb;
+        OutputInterface<SentryEventCounts> sentry_events;
 
         explicit Command(Navigation& component) {
             component.register_output("/rmcs_navigation/enable_control", enable_control, true);
             component.register_output("/rmcs_navigation/enable_autoaim", enable_autoaim, true);
+            component.register_output("/rmcs_navigation/enable_supercap", enable_supercap_, false);
             component.register_output(
                 "/rmcs_navigation/chassis_behavior", chassis_behavior, ChassisMode::AUTO);
             component.register_output("/rmcs_navigation/chassis_velocity", chassis_speed, kVecNaN);
             component.register_output("/rmcs_navigation/gimbal_toward", gimbal_toward, kVecNaN);
             component.register_output(
                 "/rmcs_navigation/request/cross_direction", climb_cross_direction, kNan);
             component.register_output("/rmcs_navigation/request/is_climb", climb_is_climb, false);
+            component.register_output(
+                "/rmcs_navigation/sentry_events", sentry_events, SentryEventCounts{});
         }
     } command{*this};
 
     auto sync_blackboard() {
         auto [x, y, yaw] = nav.check_position();
 
         // 高频查询 TF 是不对的，所以应该先缓存一份
-        // context 保留 NaN 作为 world 不可用的真实信号，blackboard 单独 fallback
         motion.context.current_world_yaw = yaw;
-        if (std::isnan(yaw)) {
-            yaw = motion.context.current_local_yaw;
-        }
+        ++motion.context.yaw_sample;
         motion.context.x = x;
         motion.context.y = y;
 
@@ -77,6 +84,8 @@ class Navigation final
 
         auto game = blackboard["game"].get<sol::table>();
         game["stage"] = rmcs_msgs::to_string(*rmcs.game_stage);
+        game["enemy_outpost_hp"] = *rmcs.enemy_outpost_hp;
+        game["enemy_base_hp"] = *rmcs.enemy_base_hp;
 
         auto play = blackboard["play"].get<sol::table>();
         play["rswitch"] = rmcs_msgs::to_string(*rmcs.switch_right);
@@ -106,22 +115,58 @@ class Navigation final
         lua.inject(
             "switch_motion_mode", [this](const std::string& mode) { motion.switch_mode(mode); });
         lua.inject("update_under_attack", [this](bool yes) { motion.context.under_attack = yes; });
+        lua.inject(
+            "update_supercap_boost", [this](bool enable) { *command.enable_supercap_ = enable; });
 
         lua.inject("set_climb_direction", [this](double world_yaw) {
-            *command.climb_cross_direction = motion.world2odom(world_yaw);
+            if (std::isfinite(world_yaw)) {
+                pending_climb_world_yaw = world_yaw;
+            } else {
+                pending_climb_world_yaw = kNan;
+                *command.climb_cross_direction = kNan;
+            }
         });
         lua.inject(
             "set_climb_switch", [this](bool is_climb) { *command.climb_is_climb = is_climb; });
         lua.inject("get_climb_status", [this] { return *rmcs.climber_status; });
 
         lua.inject("relocalize", [this] { nav.relocalize(rmcs.robot_id->color()); });
 
+        lua.inject("sentry_event", [this](const std::string& name) {
+            using SentryEvent = rmcs_msgs::SentryEvent;
+            static const auto table = std::unordered_map<std::string, SentryEvent>{
+                {"SWITCH_POSE_ATTACK", SentryEvent::SWITCH_POSE_ATTACK},
+                {"SWITCH_POSE_DEFENSE", SentryEvent::SWITCH_POSE_DEFENSE},
+                {"SWITCH_POSE_MOVE", SentryEvent::SWITCH_POSE_MOVE},
+                {"SWITCH_POSE_POWERED_ATTACK", SentryEvent::SWITCH_POSE_POWERED_ATTACK},
+                {"SWITCH_POSE_POWERED_DEFENSE", SentryEvent::SWITCH_POSE_POWERED_DEFENSE},
+                {"SWITCH_POSE_POWERED_MOVE", SentryEvent::SWITCH_POSE_POWERED_MOVE},
+                {"CONFIRM_REBIRTH", SentryEvent::CONFIRM_REBIRTH},
+                {"CONFIRM_INSTANT_REBIRTH", SentryEvent::CONFIRM_INSTANT_REBIRTH},
+                {"EXCHANGE_AMMO_SUPPLY_POINT", SentryEvent::EXCHANGE_AMMO_SUPPLY_POINT},
+                {"EXCHANGE_AMMO_REMOTE", SentryEvent::EXCHANGE_AMMO_REMOTE},
+                {"EXCHANGE_HP_REMOTE", SentryEvent::EXCHANGE_HP_REMOTE},
+                {"ACTIVATE_ENERGY_CORE", SentryEvent::ACTIVATE_ENERGY_CORE},
+            };
+
+            if (const auto it = table.find(name); it != table.end()) {
+                ++(*command.sentry_events)[it->second];
+            } else {
+                node::warn("Unknown sentry event: {}", name);
+            }
+        });
+
         node::info("Navigation is initialized");
     }
 
     auto before_updating() -> void override { rmcs.ensure_defaults(); }
 
     auto update() -> void override {
+        const auto gimbal_direction = fast_tf::cast<rmcs_description::OdomGimbalImu>(
+            rmcs_description::BottomYawLink::DirectionVector{Eigen::Vector3d::UnitX()}, *rmcs.tf);
+        motion.context.current_gimbal_yaw =
+            std::atan2(gimbal_direction->y(), gimbal_direction->x());
+
         if (lua_tick_count++ == 10) [[unlikely]] {
             lua_tick_count = 0;
             sync_blackboard();
@@ -139,11 +184,14 @@ class Navigation final
         motion.context.target_chassis_speed =
             (elapsed > kCmdVelTimeout) ? Eigen::Vector2d::Zero() : nav_cmd.speed;
 
-        const auto direction = fast_tf::cast<rmcs_description::OdomGimbalImu>(
-            rmcs_description::BaseLink::DirectionVector{Eigen::Vector3d::UnitX()}, *rmcs.tf);
-        motion.context.current_local_yaw = std::atan2(direction->y(), direction->x());
-
         const auto cmd = motion.spin_once();
+        if (std::isfinite(pending_climb_world_yaw)) {
+            const auto target = motion.world2gimbal_odom(pending_climb_world_yaw);
+            if (std::isfinite(target)) {
+                *command.climb_cross_direction = target;
+                pending_climb_world_yaw = kNan;
+            }
+        }
         if (*command.enable_control) {
             *command.chassis_speed = cmd.chassis_speed;
             *command.gimbal_toward = cmd.gimbal_toward;

src/cxx/context.cc

@@ -68,6 +68,8 @@ struct RmcsContext::Impl {
         make_input("/referee/current_hp", context.robot_health, std::uint16_t{400});
         make_input("/referee/shooter/bullet_allowance", context.robot_bullet, std::uint16_t{300});
         make_input("/referee/id", context.robot_id, rmcs_msgs::RobotId{});
+        make_input("/referee/enemy/outpost/hp", context.enemy_outpost_hp, std::uint16_t{0});
+        make_input("/referee/enemy/base/hp", context.enemy_base_hp, std::uint16_t{0});
 
         make_input("/remote/switch/right", context.switch_right, rmcs_msgs::Switch::UNKNOWN);
         make_input("/remote/switch/left", context.switch_left, rmcs_msgs::Switch::UNKNOWN);

src/cxx/context.hh

@@ -21,6 +21,8 @@ public:
     InputInterface<rmcs_msgs::RobotId> robot_id;
     InputInterface<std::uint16_t> robot_health;
     InputInterface<std::uint16_t> robot_bullet;
+    InputInterface<std::uint16_t> enemy_outpost_hp;
+    InputInterface<std::uint16_t> enemy_base_hp;
 
     InputInterface<rmcs_msgs::Switch> switch_right;
     InputInterface<rmcs_msgs::Switch> switch_left;

src/cxx/controller/motion.cc

@@ -23,28 +23,44 @@ struct MotionFsm::Impl {
 
     LowPassFilter<Eigen::Vector2d> slope_filter{Tau<3.0>{}, Eigen::Vector2d::Zero()};
 
-    double yaw_bias = 0;
-    double last_world_yaw = kNan;
+    bool gimbal_yaw_bias_initialized = false;
+    double gimbal_yaw_bias = kNan;
+    std::uint64_t last_yaw_sample = 0;
 
-    static constexpr auto normalize_yaw(double yaw) noexcept {
+    static auto normalize_yaw(double yaw) -> double {
         return std::atan2(std::sin(yaw), std::cos(yaw));
     }
 
-    auto update_yaw_bias() {
+    auto update_yaw_bias(double local_yaw, bool& initialized, double& bias) -> bool {
         const auto world_yaw = context.current_world_yaw;
-        if (!std::isfinite(world_yaw) || world_yaw == last_world_yaw)
+        if (!std::isfinite(world_yaw) || !std::isfinite(local_yaw))
+            return false;
+
+        constexpr auto kYawBiasCorrectionGain = 0.002;
+        const auto measured_bias = normalize_yaw(world_yaw - local_yaw);
+        if (!initialized) {
+            bias = measured_bias;
+            initialized = true;
+        } else {
+            const auto bias_error = normalize_yaw(measured_bias - bias);
+            bias = normalize_yaw(bias + kYawBiasCorrectionGain * bias_error);
+        }
+        return true;
+    }
+
+    auto update_yaw_biases() -> void {
+        if (last_yaw_sample == context.yaw_sample)
             return;
-        last_world_yaw = world_yaw;
 
-        constexpr auto kGain = 0.002;
-        const auto error = normalize_yaw(world_yaw - context.current_local_yaw - yaw_bias);
-        yaw_bias = normalize_yaw(yaw_bias + kGain * error);
+        last_yaw_sample = context.yaw_sample;
+        update_yaw_bias(
+            context.current_gimbal_yaw, gimbal_yaw_bias_initialized, gimbal_yaw_bias);
     }
 
-    auto world2odom(double world_yaw) const {
-        if (!std::isfinite(world_yaw))
+    auto world2gimbal_odom(double world_yaw) const -> double {
+        if (!std::isfinite(world_yaw) || !gimbal_yaw_bias_initialized)
             return kNan;
-        return normalize_yaw(world_yaw - yaw_bias);
+        return normalize_yaw(world_yaw - gimbal_yaw_bias);
     }
 
     auto update_gimbal_target() -> Eigen::Vector2d {
@@ -58,15 +74,12 @@ struct MotionFsm::Impl {
         const auto pitch = context.target_gimbal_toward.y();
         const auto wy = context.current_world_yaw;
 
-        // world 不可用：目标按 OdomGimbalImu 绝对角直通（与 current_local_yaw 同源）
+        // world 不可用时保持上一帧云台目标，避免切换坐标参考。
         if (!std::isfinite(wy)) {
-            if (!std::isfinite(target_yaw)) {
-                return {command.gimbal_toward.x(), command.gimbal_toward.y()};
-            }
-            return {normalize_yaw(target_yaw), pitch};
+            return {command.gimbal_toward.x(), command.gimbal_toward.y()};
         }
 
-        const auto target_local_yaw = world2odom(target_yaw);
+        const auto target_local_yaw = world2gimbal_odom(target_yaw);
         return {target_local_yaw, pitch};
     }
 
@@ -124,7 +137,7 @@ struct MotionFsm::Impl {
     }
 
     auto spin_once() noexcept -> Command {
-        update_yaw_bias();
+        update_yaw_biases();
         fsm.spin_once();
         return command;
     }
@@ -137,7 +150,7 @@ MotionFsm::~MotionFsm() noexcept = default;
 
 auto MotionFsm::switch_mode(const std::string& mode) -> void { pimpl->switch_mode(mode); }
 
-auto MotionFsm::world2odom(double yaw) -> double { return pimpl->world2odom(yaw); }
+auto MotionFsm::world2gimbal_odom(double yaw) -> double { return pimpl->world2gimbal_odom(yaw); }
 
 auto MotionFsm::spin_once() noexcept -> Command { return pimpl->spin_once(); }
 

src/cxx/controller/motion.hh

@@ -1,6 +1,7 @@
 #pragma once
 #include "cxx/util/pimpl.hh"
 
+#include <cstdint>
 #include <limits>
 #include <rclcpp/node.hpp>
 #include <string>
@@ -23,8 +24,9 @@ public:
         Eigen::Vector2d target_chassis_speed = kVecNan;
         Eigen::Vector2d target_gimbal_toward = kVecNan;
 
-        double current_local_yaw = kNan;
+        double current_gimbal_yaw = kNan;
         double current_world_yaw = kNan;
+        std::uint64_t yaw_sample = 0;
 
         double x = kNan, y = kNan;
 
@@ -39,7 +41,7 @@ public:
 
     explicit MotionFsm(rclcpp::Node& node) noexcept;
     auto switch_mode(const std::string& mode) -> void;
-    auto world2odom(double yaw) -> double;
+    auto world2gimbal_odom(double yaw) -> double;
     auto spin_once() noexcept -> Command;
 };
 

src/lua/action.lua

@@ -33,6 +33,9 @@ local action = {
 		full_scan_start = 0,
 		full_scan_stamp = 0,
 	},
+	climb = {
+		up = false,
+	},
 }
 
 --- 绑定 action 的后台任务。
@@ -167,6 +170,7 @@ function action:gimbal_scan(y1, y2)
 	self.gimbal.p1 = 0 + 0.2
 	self.gimbal.p2 = 0 - 0.2
 end
+
 function action:gimbal_toward(yaw, pitch)
 	action:info("Set gimbal to toward mode")
 	self.gimbal.mode = "toward"
@@ -175,14 +179,17 @@ function action:gimbal_toward(yaw, pitch)
 	self.gimbal.p1 = pitch
 	self.gimbal.p2 = pitch
 end
+
 function action:gimbal_free()
 	action:info("Set gimbal to free mode")
 	self.gimbal.mode = "free"
 end
+
 function action:gimbal_suspend()
 	action:info("Set gimbal to suspended mode")
 	self.gimbal.mode = "suspended"
 end
+
 -- @param interval number
 function action:gimbal_suspend_during(interval)
 	self.gimbal.suspend_timeline = clock:now() + interval
@@ -198,10 +205,12 @@ end
 function action:set_gimbal_pt(t)
 	self.gimbal.pt_per_rad = t
 end
+
 --- @param t number
 function action:set_gimbal_yt(t)
 	self.gimbal.yt_per_rad = t
 end
+
 function action:reset_gimbal_speed()
 	self.gimbal.pt_per_rad = kPtPerRad
 	self.gimbal.yt_per_rad = kYtPerRad
@@ -217,6 +226,11 @@ function action:update_under_attack(yes)
 	api.update_under_attack(yes)
 end
 
+--- @param enable boolean
+function action:update_supercap_boost(enable)
+	api.update_supercap_boost(enable)
+end
+
 --- 触发一次重定位，红/蓝方由 referee robot_id 自动派生；
 --- 服务未就绪或 robot_id 未知时本次调用被丢弃并打 WARN，不抛错。
 function action:relocalize()
@@ -240,6 +254,7 @@ end
 --- @param is_climb boolean true 上台阶，false 下台阶。
 --- @return boolean success
 function action:blocking_cross_step(world_yaw, is_climb)
+	self.climb.up = is_climb
 	api.set_climb_switch(is_climb)
 	api.set_climb_direction(world_yaw)
 	request:yield()

src/lua/api.lua

@@ -13,6 +13,7 @@ local util = require("util.native")
 --- @field fuck fun(message: string)
 ---
 --- @field update_enable_control fun(enable: boolean)
+--- @field update_supercap_boost fun(enable: boolean)
 --- @field send_target fun(x: number, y: number)
 --- @field update_gimbal_direction fun(yaw: number, pitch: number)
 --- @field set_climb_direction fun(angle: number)
@@ -21,6 +22,7 @@ local util = require("util.native")
 --- @field switch_motion_mode fun(mode: "normal" | "attack" | "slope")
 --- @field update_under_attack fun(yes: boolean)
 --- @field relocalize fun() 触发重定位，红/蓝方由 referee robot_id 自动派生
+--- @field sentry_event fun(event: string) 触发一次哨兵裁判系统指令，计数递增后由 SentryDecision 检测变化并发送
 ---
 local api = setmetatable({}, {
 	__index = function(_, name)