feat/speed-reference-modify

src/component.cpp

@@ -39,6 +39,12 @@ class AutoAimComponent final : public rmcs_executor::Component {
     OutputInterface<bool> feedforward_valid;
 
     InputInterface<rmcs_msgs::RobotId> robot_id;
+    InputInterface<float> referee_initial_speed_;
+    InputInterface<double> referee_shoot_timestamp_;
+
+    float latest_referee_bullet_speed_ = std::numeric_limits<float>::quiet_NaN();
+    double last_referee_event_timestamp_ = 0.0;
+    TimePoint latest_referee_bullet_speed_update_time_ { };
 
     auto make_context() const {
         auto context = SystemContext { };
@@ -52,6 +58,7 @@ class AutoAimComponent final : public rmcs_executor::Component {
         const auto iso                       = adapter.camera_transform();
         context.camera_transform.translation = iso.translation();
         context.camera_transform.orientation = Eigen::Quaterniond(iso.rotation());
+        context.bullet_speed                 = latest_referee_bullet_speed_;
 
         // TODO:无敌状态下的装甲板需要从裁判系统获取并在此更新
         context.invincible_devices = DeviceIds::None();
@@ -77,13 +84,23 @@ class AutoAimComponent final : public rmcs_executor::Component {
         register_output("/auto_aim/feedforward_valid", feedforward_valid, false);
 
         register_input("/referee/id", robot_id, true);
+        register_input("/referee/shooter/initial_speed", referee_initial_speed_, false);
+        register_input("/referee/shooter/shoot_timestamp", referee_shoot_timestamp_, false);
+    }
+
+    auto before_updating() -> void override {
+        if (!referee_initial_speed_.ready())
+            referee_initial_speed_.bind_directly(std::numeric_limits<float>::quiet_NaN());
+        if (!referee_shoot_timestamp_.ready())
+            referee_shoot_timestamp_.bind_directly(0.0);
     }
 
     auto update() -> void override {
         if (!adapter.ready()) [[unlikely]] {
             feishu.send(SystemContext::kInvalid());
             return;
         }
+        update_referee_bullet_speed_cache_();
         feishu.send(make_context());
 
         // Reset all command
@@ -135,6 +152,27 @@ class AutoAimComponent final : public rmcs_executor::Component {
             std::sin(pitch),
         };
     }
+
+    auto update_referee_bullet_speed_cache_() -> void {
+        using namespace std::chrono_literals;
+
+        const auto shoot_timestamp = *referee_shoot_timestamp_;
+        if (shoot_timestamp != last_referee_event_timestamp_) {
+            last_referee_event_timestamp_ = shoot_timestamp;
+
+            const auto initial_speed = *referee_initial_speed_;
+            if (std::isfinite(initial_speed) && initial_speed > 0.0F) {
+                latest_referee_bullet_speed_ = initial_speed;
+                latest_referee_bullet_speed_update_time_ = Clock::now();
+            }
+        }
+
+        if (latest_referee_bullet_speed_update_time_ == TimePoint { })
+            return;
+
+        if (Clock::now() - latest_referee_bullet_speed_update_time_ > 2s)
+            latest_referee_bullet_speed_ = std::numeric_limits<float>::quiet_NaN();
+    }
 };
 
 } // namespace rmcs

src/kernel/fire_control.cpp

@@ -76,9 +76,16 @@ struct FireControl::Impl {
         return {};
     }
 
-    auto solve(predictor::Snapshot const& snapshot, double current_yaw) -> std::optional<Result> {
+    auto solve(
+        predictor::Snapshot const& snapshot, double current_yaw, double runtime_bullet_speed)
+        -> std::optional<Result> {
+        const double bullet_speed =
+            std::isfinite(runtime_bullet_speed) && runtime_bullet_speed > 0.0
+                ? runtime_bullet_speed
+                : config.initial_bullet_speed;
+
         auto target_solution = target_solution_solver.solve(
-            snapshot, aim_point_chooser, config.initial_bullet_speed, config.shoot_delay);
+            snapshot, aim_point_chooser, bullet_speed, config.shoot_delay);
         if (!target_solution.has_value()) return std::nullopt;
 
         auto center_position    = target_solution->center_position;
@@ -94,8 +101,7 @@ struct FireControl::Impl {
 
         if (config.mpc_enable && snapshot.device_id() != DeviceId::OUTPOST) {
             auto sample_attitude = [&](TimePoint t) -> std::expected<AimAttitude, std::string> {
-                auto sample = AimPointSampler::sample_at(
-                    snapshot, aim_point_chooser, t, config.initial_bullet_speed);
+                auto sample = AimPointSampler::sample_at(snapshot, aim_point_chooser, t, bullet_speed);
                 if (!sample.has_value()) return std::unexpected { sample.error() };
                 return sample->attitude;
             };
@@ -161,7 +167,8 @@ auto FireControl::initialize(const YAML::Node& yaml) noexcept -> std::expected<v
     return pimpl->initialize(yaml);
 }
 
-auto FireControl::solve(const predictor::Snapshot& snapshot, double current_yaw)
+auto FireControl::solve(
+    const predictor::Snapshot& snapshot, double current_yaw, double runtime_bullet_speed)
     -> std::optional<Result> {
-    return pimpl->solve(snapshot, current_yaw);
+    return pimpl->solve(snapshot, current_yaw, runtime_bullet_speed);
 }

src/kernel/fire_control.hpp

@@ -28,7 +28,10 @@ class FireControl {
 
     auto initialize(const YAML::Node&) noexcept -> std::expected<void, std::string>;
 
-    auto solve(const predictor::Snapshot& snapshot, double current_yaw) -> std::optional<Result>;
+    auto solve(
+        const predictor::Snapshot& snapshot, double current_yaw,
+        double runtime_bullet_speed = std::numeric_limits<double>::quiet_NaN())
+        -> std::optional<Result>;
 };
 
 } // namespace rmcs::kernel

src/runtime.cpp

@@ -192,7 +192,7 @@ auto main() -> int {
             visualization.update_visible_robot(armors);
 
             const auto yaw = context.yaw;
-            if (auto result = fire_control.solve(*snapshot, yaw)) {
+            if (auto result = fire_control.solve(*snapshot, yaw, context.bullet_speed)) {
                 command.should_control    = true;
                 command.target            = target.target_id;
                 command.should_shoot      = result->shoot_permitted;

src/utility/shared/context.hpp

@@ -54,6 +54,7 @@ struct SystemContext {
 
     double yaw { kNaN };
     double pitch { kNaN };
+    double bullet_speed { kNaN };
 
     Transform camera_transform = Transform::kNaN(); // Imu Odom Link
 
@@ -75,6 +76,7 @@ struct SystemContext {
             .timestamp        = Clock::now(),
             .yaw              = 0,
             .pitch            = 0,
+            .bullet_speed     = kNaN,
             .camera_transform = Transform::kIdentity(),
         };
     }