From 50f0c914430bb874c4207c253942991dc23d3ba5 Mon Sep 17 00:00:00 2001 From: eye-on Date: Fri, 22 May 2026 20:57:16 +0800 Subject: [PATCH] feat/speed-reference-modify --- src/component.cpp | 38 ++++++++++++++++++++++++++++++++++ src/kernel/fire_control.cpp | 19 +++++++++++------ src/kernel/fire_control.hpp | 5 ++++- src/runtime.cpp | 2 +- src/utility/shared/context.hpp | 2 ++ 5 files changed, 58 insertions(+), 8 deletions(-) diff --git a/src/component.cpp b/src/component.cpp index e2b5481..9cbe848 100644 --- a/src/component.cpp +++ b/src/component.cpp @@ -39,6 +39,12 @@ class AutoAimComponent final : public rmcs_executor::Component { OutputInterface feedforward_valid; InputInterface robot_id; + InputInterface referee_initial_speed_; + InputInterface referee_shoot_timestamp_; + + float latest_referee_bullet_speed_ = std::numeric_limits::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,6 +84,15 @@ 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::quiet_NaN()); + if (!referee_shoot_timestamp_.ready()) + referee_shoot_timestamp_.bind_directly(0.0); } auto update() -> void override { @@ -84,6 +100,7 @@ class AutoAimComponent final : public rmcs_executor::Component { 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::quiet_NaN(); + } }; } // namespace rmcs diff --git a/src/kernel/fire_control.cpp b/src/kernel/fire_control.cpp index 411a2cb..1617606 100644 --- a/src/kernel/fire_control.cpp +++ b/src/kernel/fire_control.cpp @@ -76,9 +76,16 @@ struct FireControl::Impl { return {}; } - auto solve(predictor::Snapshot const& snapshot, double current_yaw) -> std::optional { + auto solve( + predictor::Snapshot const& snapshot, double current_yaw, double runtime_bullet_speed) + -> std::optional { + 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 { - 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::expectedinitialize(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 { - return pimpl->solve(snapshot, current_yaw); + return pimpl->solve(snapshot, current_yaw, runtime_bullet_speed); } diff --git a/src/kernel/fire_control.hpp b/src/kernel/fire_control.hpp index 8130f88..fbbaa17 100644 --- a/src/kernel/fire_control.hpp +++ b/src/kernel/fire_control.hpp @@ -28,7 +28,10 @@ class FireControl { auto initialize(const YAML::Node&) noexcept -> std::expected; - auto solve(const predictor::Snapshot& snapshot, double current_yaw) -> std::optional; + auto solve( + const predictor::Snapshot& snapshot, double current_yaw, + double runtime_bullet_speed = std::numeric_limits::quiet_NaN()) + -> std::optional; }; } // namespace rmcs::kernel diff --git a/src/runtime.cpp b/src/runtime.cpp index 5ec40d8..b65b80f 100644 --- a/src/runtime.cpp +++ b/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; diff --git a/src/utility/shared/context.hpp b/src/utility/shared/context.hpp index ea3e2f3..28617a3 100644 --- a/src/utility/shared/context.hpp +++ b/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(), }; }