diff --git a/CMakeLists.txt b/CMakeLists.txt index a499c9ad..e6891245 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,7 @@ cmake_minimum_required(VERSION 3.22) project(rmcs_auto_aim_v2) + set(CMAKE_EXPORT_COMPILE_COMMANDS ON) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_STANDARD 23) @@ -134,4 +135,8 @@ pluginlib_export_plugin_description_file( ) find_package(ament_cmake REQUIRED) +if(BUILD_TESTING) + add_subdirectory(test) +endif() + ament_package() diff --git a/config/config.yaml b/config/config.yaml index 9145e575..c3b67eeb 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -14,17 +14,17 @@ capturer: # float exposure_us: 2000.0 # float - framerate: 60 + framerate: 120 # float gain: 16.9807 invert_image: false software_sync: false trigger_mode: false - fixed_framerate: true + fixed_framerate: false local_video: # 替换为你具体的路径 - location: "/workspaces/alliance/test_videos/translation.mp4" + location: "/workspaces/alliance/test_videos/outpost.mp4" # double 帧率 frame_rate: 60 # bool 是否循环播放 @@ -40,20 +40,23 @@ identifier: - "tongji-yolov5.xml" - "shenzhen-0526.onnx" - "shenzhen-0708.onnx" - model_location: "shenzhen-0526.onnx" - infer_device: "AUTO" + model_location: "tongji-yolov5.xml" + infer_device: "GPU" use_roi_segment: false roi_rows: 640 roi_cols: 640 input_rows: 640 input_cols: 640 min_confidence: 0.5 - score_threshold: 0.8 - nms_threshold: 0.45 + score_threshold: 0.7 + nms_threshold: 0.3 tracker: # blue or red - enemy_color: blue + enemy_color: red + max_temporary_loss_frames: 4 + max_unconfirmed_loss_frames: 2 + tracking_confirm_frames: 2 pose_estimator: camera_matrix: [1.722231837421459e+03, 0, 7.013056440882832e+02, 0, 1.724876404292754e+03,5.645821718351237e+02 , 0, 0, 1] @@ -74,21 +77,22 @@ pose_estimator: q: [1., 0., 0., 0.] fire_control: - initial_bullet_speed: 20 # m/s + initial_bullet_speed: 26.6 # m/s shoot_delay: 0.1 # s - shoot_offset_x: 0.0 # m - shoot_offset_y: 0.0 # m - shoot_offset_z: 0.0 # m + yaw_offset: 0.0 # degree + pitch_offset: 0.0 # degree - k: 0.019 - bias_scale: 1.0 - - coming_angle: 60.0 # degree + coming_angle: 70.0 # degree leaving_angle: 20.0 # degree outpost_coming_angle: 70.0 # degree outpost_leaving_angle: 30.0 # degree angular_velocity_threshold: 120 # degree/s + first_tolerance: 3 # 近距离射击容差，degree + second_tolerance: 2 # 远距离射击容差，degree + judge_distance: 2 #距离判断阈值 + auto_fire: true # 是否由自瞄控制射击 + visualization: framerate: 60 monitor_host: "127.0.0.1" diff --git a/package.xml b/package.xml index 4412f511..5534fd19 100644 --- a/package.xml +++ b/package.xml @@ -9,6 +9,7 @@ ament_cmake + ament_cmake_gtest ament_lint_auto ament_lint_common diff --git a/src/component.cpp b/src/component.cpp index f6228a59..3850cb5e 100644 --- a/src/component.cpp +++ b/src/component.cpp @@ -5,7 +5,9 @@ #include "utility/rclcpp/visual/transform.hpp" #include "utility/shared/context.hpp" +#include #include +#include #include #include @@ -20,12 +22,11 @@ class AutoAimComponent final : public rmcs_executor::Component { : rclcpp { get_component_name() } { register_input("/tf", rmcs_tf); - register_input("/referee/shooter/initial_speed", bullet_speed); - register_output("/gimbal/auto_aim/controllable", gimbal_takeover, false); + register_output("/gimbal/auto_aim/auto_aim_enabled", gimbal_takeover, false); register_output( "/gimbal/auto_aim/control_direction", target_direction, Eigen::Vector3d::Zero()); - register_output("/gimbal/auto_aim/shoot_permit", shoot_permitted, false); + register_output("/gimbal/auto_aim/shoot_enable", shoot_permitted, false); using namespace std::chrono_literals; framerate.set_interval(2s); @@ -39,56 +40,26 @@ class AutoAimComponent final : public rmcs_executor::Component { visual_odom_to_camera = std::make_unique(config); action_throttler.register_action("tf_not_ready"); - action_throttler.register_action("bullet_speed_not_ready"); action_throttler.register_action("commit_control_state_failed"); } auto update() -> void override { - using namespace rmcs_description; - if (!rmcs_tf.ready()) [[unlikely]] { - action_throttler.dispatch("tf_not_ready", [&] { rclcpp.warn("rmcs_tf is not ready"); }); - control_state.set_identity(); - reset_control_commands(); - return; - } - if (!bullet_speed.ready()) [[unlikely]] { - action_throttler.dispatch( - "bullet_speed_not_ready", [&] { rclcpp.warn("bullet_speed is not ready"); }); - control_state.set_identity(); - reset_control_commands(); + handle_tf_not_ready(); return; } - // TODO:适时交出云台和发射机构控制权 - { - update_gimbal_direction(); - update_control_state(); - - auto success = feishu.commit(control_state); - if (!success) { - action_throttler.dispatch("commit_control_state_failed", - [&] { rclcpp.info("commit control state failed!"); }); - } else { - action_throttler.reset("commit_control_state_failed"); - } - } - { - if (feishu.updated()) { - auto_aim_state = feishu.fetch(); - } - - *gimbal_takeover = auto_aim_state.gimbal_takeover; - *shoot_permitted = auto_aim_state.shoot_permitted; - update_target_direction(); - } + + publish_control_state(); + forward_auto_aim_outputs(); } private: + static constexpr auto auto_aim_state_timeout { std::chrono::milliseconds { 100 } }; + InputInterface rmcs_tf; - double current_gimbal_yaw { 0. }; - double current_gimbal_pitch { 0. }; - InputInterface bullet_speed; + double current_gimbal_yaw { std::numeric_limits::quiet_NaN() }; + double current_gimbal_pitch { std::numeric_limits::quiet_NaN() }; RclcppNode rclcpp; std::unique_ptr visual_odom_to_camera; @@ -96,6 +67,7 @@ class AutoAimComponent final : public rmcs_executor::Component { Feishu feishu; ControlState control_state; AutoAimState auto_aim_state; + bool auto_aim_state_received_ { false }; OutputInterface gimbal_takeover; OutputInterface shoot_permitted; @@ -104,7 +76,73 @@ class AutoAimComponent final : public rmcs_executor::Component { FramerateCounter framerate; ActionThrottler action_throttler { std::chrono::seconds(1), 233 }; -private: + auto has_fresh_auto_aim_state() const -> bool { + return auto_aim_state_received_ + && Clock::now() - auto_aim_state.timestamp <= auto_aim_state_timeout; + } + + static auto make_invalid_auto_aim_state() -> AutoAimState { + auto state = AutoAimState {}; + state.reset(); + return state; + } + + auto resolve_auto_aim_state() -> AutoAimState { + if (feishu.updated()) { + auto_aim_state = feishu.fetch(); + auto_aim_state_received_ = true; + } + + if (has_fresh_auto_aim_state()) { + return auto_aim_state; + } + + return make_invalid_auto_aim_state(); + } + + auto publish_auto_aim_outputs(const AutoAimState& state) -> void { + *gimbal_takeover = state.gimbal_takeover; + *shoot_permitted = state.shoot_permitted; + *target_direction = compute_target_direction(state); + } + + static auto compute_target_direction(const AutoAimState& state) -> Eigen::Vector3d { + if (!state.has_control_direction()) { + return Eigen::Vector3d::Zero(); + } + + const auto& [yaw, pitch] = std::tie(state.yaw, state.pitch); + + // clang-format off + return Eigen::Vector3d { + std::cos(pitch) * std::cos(yaw), + std::cos(pitch) * std::sin(yaw), + std::sin(pitch) + }; + // clang-format on + } + + auto forward_auto_aim_outputs() -> void { publish_auto_aim_outputs(resolve_auto_aim_state()); } + + auto handle_tf_not_ready() -> void { + action_throttler.dispatch("tf_not_ready", [&] { rclcpp.warn("rmcs_tf is not ready"); }); + control_state.reset(); + publish_auto_aim_outputs(make_invalid_auto_aim_state()); + } + + auto publish_control_state() -> void { + update_gimbal_direction(); + update_control_state(); + + auto success = feishu.commit(control_state); + if (!success) { + action_throttler.dispatch("commit_control_state_failed", + [&] { rclcpp.info("commit control state failed!"); }); + } else { + action_throttler.reset("commit_control_state_failed"); + } + } + auto update_control_state() -> void { control_state.timestamp = Clock::now(); @@ -123,41 +161,24 @@ class AutoAimComponent final : public rmcs_executor::Component { // TODO:无敌状态下的装甲板需要从裁判系统获取并在此更新 control_state.invincible_devices = DeviceIds::None(); - control_state.bullet_speed = *bullet_speed; - control_state.yaw = current_gimbal_yaw; - control_state.pitch = current_gimbal_pitch; - } - - auto update_target_direction() -> void { - const auto& [yaw, pitch] = std::tie(auto_aim_state.yaw, auto_aim_state.pitch); - - // clang-format off - *target_direction = Eigen::Vector3d { - std::cos(pitch) * std::cos(yaw), - std::cos(pitch) * std::sin(yaw), - std::sin(pitch) - }; - // clang-format on - } - - auto reset_control_commands() -> void { - *gimbal_takeover = false; - *shoot_permitted = false; - *target_direction = Eigen::Vector3d::Zero(); + control_state.yaw = current_gimbal_yaw; + control_state.pitch = current_gimbal_pitch; } auto update_gimbal_direction() -> void { - auto odom_to_muzzle_transform = - fast_tf::lookup_transform( + using namespace rmcs_description; + + auto odom_to_pitch_transform = + fast_tf::lookup_transform( *rmcs_tf); - auto quat = Eigen::Quaterniond { odom_to_muzzle_transform.rotation() }; + auto quat = Eigen::Quaterniond { odom_to_pitch_transform.toRotationMatrix() }; - auto current_muzzle_direction = quat * Eigen::Vector3d::UnitX(); + auto current_pitch_direction = quat * Eigen::Vector3d::UnitX(); - current_gimbal_yaw = std::atan2(current_muzzle_direction.y(), current_muzzle_direction.x()); - current_gimbal_pitch = std::atan2(current_muzzle_direction.z(), - std::hypot(current_muzzle_direction.x(), current_muzzle_direction.y())); + current_gimbal_yaw = std::atan2(current_pitch_direction.y(), current_pitch_direction.x()); + current_gimbal_pitch = std::atan2(current_pitch_direction.z(), + std::hypot(current_pitch_direction.x(), current_pitch_direction.y())); } }; diff --git a/src/kernel/fire_control.cpp b/src/kernel/fire_control.cpp index 54123532..0a0ef471 100644 --- a/src/kernel/fire_control.cpp +++ b/src/kernel/fire_control.cpp @@ -1,9 +1,14 @@ #include "fire_control.hpp" +#include +#include +#include +#include + #include "module/fire_control/aim_point_chooser.hpp" +#include "module/fire_control/shoot_evaluator.hpp" #include "module/fire_control/trajectory_solution.hpp" #include "module/predictor/snapshot.hpp" -#include "utility/logging/printer.hpp" #include "utility/math/angle.hpp" #include "utility/serializable.hpp" @@ -14,12 +19,8 @@ struct FireControl::Impl { struct Config : util::Serializable { double initial_bullet_speed; // m/s double shoot_delay; // s - double shoot_offset_x; // m - double shoot_offset_y; // m - double shoot_offset_z; // m - - double k; // 基础阻力系数 (小弹丸~0.019, 大弹丸~0.005) - double bias_scale; // 动态补偿系数：修正额外阻力,阻力越大，该系数越大，default=1 + double yaw_offset; // rad (config in degree) + double pitch_offset; // rad (config in degree) double coming_angle; // rad double leaving_angle; // rad @@ -31,12 +32,8 @@ struct FireControl::Impl { constexpr static std::tuple metas { &Config::initial_bullet_speed, "initial_bullet_speed", &Config::shoot_delay,"shoot_delay", - &Config::shoot_offset_x,"shoot_offset_x", - &Config::shoot_offset_y,"shoot_offset_y", - &Config::shoot_offset_z,"shoot_offset_z", - - &Config::k,"k", - &Config::bias_scale,"bias_scale", + &Config::yaw_offset,"yaw_offset", + &Config::pitch_offset,"pitch_offset", &Config::coming_angle,"coming_angle", &Config::leaving_angle,"leaving_angle", @@ -49,21 +46,23 @@ struct FireControl::Impl { Config config; - double bullet_speed_buffer { 0. }; - double bullet_speed { 0. }; - AimPointChooser aim_point_chooser; + ShootEvaluator shoot_evaluator; - rmcs::Printer log { "FireControl" }; + const double kMinValidBulletSpeed { 10. }; auto initialize(const YAML::Node& yaml) noexcept -> std::expected { auto result = config.serialize(yaml); if (!result.has_value()) { return std::unexpected { result.error() }; } + if (!(config.initial_bullet_speed > kMinValidBulletSpeed)) { + return std::unexpected { std::format( + "Invalid initial_bullet_speed: {}", config.initial_bullet_speed) }; + } - bullet_speed = config.initial_bullet_speed; - + config.yaw_offset = util::deg2rad(config.yaw_offset); + config.pitch_offset = util::deg2rad(config.pitch_offset); config.coming_angle = util::deg2rad(config.coming_angle); config.leaving_angle = util::deg2rad(config.leaving_angle); config.outpost_coming_angle = util::deg2rad(config.outpost_coming_angle); @@ -79,34 +78,71 @@ struct FireControl::Impl { }; aim_point_chooser.initialize(chooser_config); + auto evaluate_result = shoot_evaluator.initialize(yaml); + if (!evaluate_result.has_value()) { + return std::unexpected { std::format( + "shoot_evaluator init failed: {}", evaluate_result.error()) }; + } return {}; } const int kMaxIterateCount { 5 }; const double kMaxFlyTimeThreshold { 0.001 }; - auto set_bullet_speed(double speed) -> void { bullet_speed_buffer = speed; } - - auto solve(const predictor::Snapshot& snapshot, Translation const& odom_to_muzzle_translation) + auto make_result(const Armor3D& armor, bool control, double current_yaw) -> std::optional { - auto state = snapshot.ekf_x(); - auto target_position_in_world = Eigen::Vector3d { state[0], state[2], state[4] }; + auto armor_position_in_world = Eigen::Vector3d {}; + armor.translation.copy_to(armor_position_in_world); + + auto target_d = std::sqrt(armor_position_in_world.x() * armor_position_in_world.x() + + armor_position_in_world.y() * armor_position_in_world.y()); + auto target_h = armor_position_in_world.z(); + if (!(target_d > 0.0)) { + return std::nullopt; + } + + auto solution = TrajectorySolution {}; + solution.input.v0 = config.initial_bullet_speed; + solution.input.target_d = target_d; + solution.input.target_h = target_h; - if (bullet_speed_buffer > 10.) { - bullet_speed = bullet_speed_buffer; - } else { - bullet_speed = config.initial_bullet_speed; + auto trajectory_result = solution.solve(); + if (!trajectory_result) { + return std::nullopt; } - auto current_fly_time = target_position_in_world.norm() / bullet_speed; + auto final_yaw = std::atan2(armor_position_in_world.y(), armor_position_in_world.x()); + final_yaw += config.yaw_offset; + + auto command = ShootEvaluator::Command { + .control = control, + .auto_aim_enabled = control, + .aim_point_valid = true, + .yaw = final_yaw, + .distance = target_d, + }; + auto shoot_permitted = shoot_evaluator.evaluate(command, current_yaw); + const auto final_pitch = trajectory_result->pitch + config.pitch_offset; + + return Result { + .pitch = final_pitch, + .yaw = final_yaw, + .horizon_distance = target_d, + .shoot_permitted = shoot_permitted, + }; + } + + auto solve(const predictor::Snapshot& snapshot, bool control, double current_yaw) + -> std::optional { + auto target_kinematics = snapshot.kinematics(); + + // 以整车位置来初步迭代飞行时间 + auto target_position_in_world = target_kinematics.center_position; - auto best_armor_opt = std::optional {}; - auto trajectory_result = TrajectorySolution::Output {}; - auto horizon_distance = 0.0; + const double bullet_speed = config.initial_bullet_speed; + auto current_fly_time = target_position_in_world.norm() / bullet_speed; - auto solution_params = fire_control::TrajectorySolution::TrajectoryParams {}; - solution_params.k = config.k; - solution_params.bias_scale = config.bias_scale; + auto best_armor_opt = std::optional {}; for (int i = 0; i < kMaxIterateCount; ++i) { // 计算预测的时间点 = 子弹飞行时间 + 系统响应延迟 @@ -115,53 +151,43 @@ struct FireControl::Impl { + std::chrono::duration_cast( std::chrono::duration(total_predict_time)); - auto predicted_armors = snapshot.predicted_armors(t_target); - auto predicted_ekf_x = snapshot.predict_at(t_target); - - best_armor_opt = aim_point_chooser.choose_armor(predicted_armors, predicted_ekf_x); - if (!best_armor_opt) return std::nullopt; + auto predicted_armors = snapshot.predicted_armors(t_target); + auto predicted_kinematics = snapshot.kinematics_at(t_target); - auto const& armor_translation = best_armor_opt->translation; + auto chosen_armor_opt = aim_point_chooser.choose_armor(predicted_armors, + predicted_kinematics.center_position, predicted_kinematics.angular_velocity); + if (!chosen_armor_opt) { + continue; + } + best_armor_opt = chosen_armor_opt; auto armor_position_in_world = Eigen::Vector3d {}; - armor_translation.copy_to(armor_position_in_world); - - auto _odom_to_muzzle_translation = Eigen::Vector3d {}; - odom_to_muzzle_translation.copy_to(_odom_to_muzzle_translation); + best_armor_opt->translation.copy_to(armor_position_in_world); - auto bullet_in_muzzle = armor_position_in_world - _odom_to_muzzle_translation; - - auto target_d = std::sqrt(bullet_in_muzzle.x() * bullet_in_muzzle.x() - + bullet_in_muzzle.y() * bullet_in_muzzle.y()); - auto target_h = bullet_in_muzzle.z(); + auto target_d = std::sqrt(armor_position_in_world.x() * armor_position_in_world.x() + + armor_position_in_world.y() * armor_position_in_world.y()); + if (!(target_d > 0.0)) { + continue; + } auto solution = TrajectorySolution {}; solution.input.v0 = bullet_speed; solution.input.target_d = target_d; - solution.input.target_h = target_h; - solution.input.params = solution_params; + solution.input.target_h = armor_position_in_world.z(); auto result = solution.solve(); - if (!result) { - return std::nullopt; + continue; } - auto time_error = std::abs(result->fly_time - current_fly_time); - current_fly_time = result->fly_time; - trajectory_result = *result; - horizon_distance = target_d; - + auto time_error = std::abs(result->fly_time - current_fly_time); + current_fly_time = result->fly_time; if (time_error < kMaxFlyTimeThreshold) break; } - auto final_yaw = std::atan2(best_armor_opt->translation.y, best_armor_opt->translation.x); + if (!best_armor_opt) return std::nullopt; - return Result { - .pitch = trajectory_result.pitch, - .yaw = final_yaw, - .horizon_distance = horizon_distance, - }; + return make_result(*best_armor_opt, control, current_yaw); } }; @@ -173,9 +199,7 @@ auto FireControl::initialize(const YAML::Node& yaml) noexcept -> std::expectedinitialize(yaml); } -auto FireControl::set_bullet_speed(double speed) -> void { return pimpl->set_bullet_speed(speed); } - -auto FireControl::solve(const predictor::Snapshot& snapshot, - Translation const& odom_to_muzzle_translation) -> std::optional { - return pimpl->solve(snapshot, odom_to_muzzle_translation); +auto FireControl::solve(const predictor::Snapshot& snapshot, bool control, double current_yaw) + -> std::optional { + return pimpl->solve(snapshot, control, current_yaw); } diff --git a/src/kernel/fire_control.hpp b/src/kernel/fire_control.hpp index 8ac03c89..4284bcc3 100644 --- a/src/kernel/fire_control.hpp +++ b/src/kernel/fire_control.hpp @@ -5,7 +5,6 @@ #include "module/predictor/snapshot.hpp" #include "utility/clock.hpp" -#include "utility/math/linear.hpp" #include "utility/pimpl.hpp" namespace rmcs::kernel { @@ -20,13 +19,12 @@ class FireControl { double pitch; double yaw; double horizon_distance; + bool shoot_permitted; }; auto initialize(const YAML::Node&) noexcept -> std::expected; - auto set_bullet_speed(double speed) -> void; - - auto solve(const predictor::Snapshot& snapshot, Translation const& odom_to_muzzle_translation) + auto solve(const predictor::Snapshot& snapshot, bool control, double current_yaw) -> std::optional; }; } diff --git a/src/kernel/tracker.cpp b/src/kernel/tracker.cpp index 5eea75a0..35914b15 100644 --- a/src/kernel/tracker.cpp +++ b/src/kernel/tracker.cpp @@ -32,6 +32,11 @@ struct Tracker::Impl { return std::unexpected { "enemy_color 应该是 [blue] or [red]." }; } + result = decider.initialize(yaml); + if (!result.has_value()) { + return std::unexpected { result.error() }; + } + return {}; } diff --git a/src/kernel/tracker.hpp b/src/kernel/tracker.hpp index 654e4eae..22afea0f 100644 --- a/src/kernel/tracker.hpp +++ b/src/kernel/tracker.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include "module/tracker/decider.hpp" diff --git a/src/module/debug/action_throttler.hpp b/src/module/debug/action_throttler.hpp index c4f2bf3f..dea49e59 100644 --- a/src/module/debug/action_throttler.hpp +++ b/src/module/debug/action_throttler.hpp @@ -18,35 +18,32 @@ class ActionThrottler { using duration = std::chrono::milliseconds; ActionThrottler(duration interval, std::size_t default_quota) noexcept - : default_quota_ { default_quota } { - metronome_.set_interval(interval); - } + : interval_ { interval } + , default_quota_ { default_quota } { } auto register_action(std::string_view tag, std::optional quota = std::nullopt) -> void { - auto [it, inserted] = - actions_.try_emplace(std::string { tag }, quota.value_or(default_quota_)); + auto [it, inserted] = actions_.try_emplace( + std::string { tag }, Action { interval_, quota.value_or(default_quota_) }); if (!inserted) { - it->second.limit = quota.value_or(default_quota_); - it->second.reset(); - it->second.enable(); + it->second = Action { interval_, quota.value_or(default_quota_) }; } } template auto dispatch(std::string_view tag, Fn&& action) -> bool { - if (!metronome_.tick()) return false; - auto it = actions_.find(tag); if (it == actions_.end()) return false; - auto& limit = it->second; - if (limit.tick()) { + auto& action_state = it->second; + if (!action_state.metronome.tick()) return false; + + if (action_state.limit.tick()) { std::forward(action)(); return true; } - limit.disable(); + action_state.limit.disable(); return false; } @@ -58,6 +55,23 @@ class ActionThrottler { } private: + struct Action { + explicit Action(duration interval, std::size_t quota) noexcept + : limit { quota } { + metronome.set_interval(interval); + } + + TimesLimit limit; + FramerateCounter metronome; + + auto reset() noexcept -> void { + limit.reset(); + metronome.last_reach_interval_timestamp = {}; + } + + auto enable() noexcept -> void { limit.enable(); } + }; + struct string_hash { using is_transparent = void; auto operator()(std::string_view sv) const -> std::size_t { @@ -65,9 +79,9 @@ class ActionThrottler { } }; - FramerateCounter metronome_; + duration interval_; std::size_t default_quota_; - std::unordered_map> actions_; + std::unordered_map> actions_; }; } // namespace rmcs::util diff --git a/src/module/debug/visualization/armor_visualizer.cpp b/src/module/debug/visualization/armor_visualizer.cpp index 634ad5ed..af3044f8 100644 --- a/src/module/debug/visualization/armor_visualizer.cpp +++ b/src/module/debug/visualization/armor_visualizer.cpp @@ -1,79 +1,165 @@ #include "armor_visualizer.hpp" -#include "utility/rclcpp/visual/armor.hpp" +#include "utility/panic.hpp" +#include "utility/rclcpp/node.details.hpp" #include "utility/robot/armor.hpp" +#include +#include +#include + using namespace rmcs::debug; -using VisualArmor = rmcs::util::visual::Armor; +using Marker = visualization_msgs::msg::Marker; +using MarkerArray = visualization_msgs::msg::MarkerArray; -struct ArmorShadow { - decltype(rmcs::Armor3D::genre) genre; - decltype(rmcs::Armor3D::color) color; - decltype(rmcs::Armor3D::id) id; - std::string ns; +namespace { - bool operator==(ArmorShadow const& other) const = default; - bool operator!=(ArmorShadow const& other) const { return !(*this == other); } -}; +auto make_unique_marker_id(rmcs::DeviceId device, int armor_index) -> int { + constexpr auto kArmorIndexBitWidth = 16; + constexpr auto kArmorIndexLimit = 1 << kArmorIndexBitWidth; + + if (armor_index < 0 || armor_index >= kArmorIndexLimit) { + rmcs::util::panic(std::format("Armor marker index out of range: {}", armor_index)); + } + + auto const device_index = static_cast(rmcs::to_index(device)); + return (device_index << kArmorIndexBitWidth) | armor_index; +} + +auto set_marker_scale(Marker& marker, rmcs::DeviceId device, bool is_arrow) -> void { + if (is_arrow) { + marker.scale.x = 0.2; + marker.scale.y = 0.01; + marker.scale.z = 0.01; + return; + } + + if (rmcs::DeviceIds::kSmallArmor().contains(device)) { + marker.scale.x = 0.003; + marker.scale.y = 0.140; + marker.scale.z = 0.125; + } else if (rmcs::DeviceIds::kLargeArmor().contains(device)) { + marker.scale.x = 0.003; + marker.scale.y = 0.235; + marker.scale.z = 0.127; + } +} + +auto set_marker_color(Marker& marker, rmcs::CampColor camp) -> void { + if (camp == rmcs::CampColor::RED) { + marker.color.r = 1.; + marker.color.g = 0.; + marker.color.b = 0.; + marker.color.a = 1.; + } else if (camp == rmcs::CampColor::BLUE) { + marker.color.r = 0.; + marker.color.g = 0.; + marker.color.b = 1.; + marker.color.a = 1.; + } else { + marker.color.r = 1.; + marker.color.g = 0.; + marker.color.b = 1.; + marker.color.a = 1.; + } +} + +auto make_marker(std::string_view frame_id, std::string_view ns, int id, int type, int action, + rmcs::DeviceId device, rmcs::CampColor camp, const rmcs::Armor3D* armor, rclcpp::Time stamp) + -> Marker { + auto marker = Marker {}; + marker.header.frame_id = frame_id; + marker.header.stamp = stamp; + marker.ns = std::string { ns }; + marker.id = id; + marker.type = type; + marker.action = action; + marker.lifetime = rclcpp::Duration::from_seconds(0.1); + + if (type == Marker::ARROW) { + set_marker_scale(marker, device, true); + } else { + set_marker_scale(marker, device, false); + } + + set_marker_color(marker, camp); + + if (armor) { + armor->translation.copy_to(marker.pose.position); + armor->orientation.copy_to(marker.pose.orientation); + } + + return marker; +} + +} // namespace struct ArmorVisualizer::Impl final { auto initialize(util::RclcppNode& visual_node) noexcept -> void { node = std::ref(visual_node); } - auto visualize(std::span _armors, std::string const& name, + auto visualize(std::span armors, std::string const& name, std::string const& link_name) -> bool { if (!node.has_value()) { return false; } - auto new_size = _armors.size(); - visual_armors.reserve(new_size); - current_armors.reserve(new_size); - visual_armors.resize(new_size); - current_armors.resize(new_size); - - for (size_t i = 0; i < new_size; i++) { - auto const& input = _armors[i]; - auto& armor_ptr = visual_armors[i]; - auto& shadow = current_armors[i]; - - bool changed = !armor_ptr || needs_rebuild(shadow, input, name); - - if (changed) { - auto const config = VisualArmor::Config { - .rclcpp = node.value().get(), - .device = input.genre, - .camp = armor_color2camp_color(input.color), - .id = input.id, - .name = name, - .tf = link_name, - }; - - armor_ptr = std::make_unique(config); - - shadow.genre = input.genre; - shadow.color = input.color; - shadow.id = input.id; - shadow.ns = name; + if (!rmcs::util::prefix::check_naming(name) + || !rmcs::util::prefix::check_naming(link_name)) { + util::panic(std::format( + "Not a valid naming for armor name or tf: {}", + rmcs::util::prefix::naming_standard)); + } + + auto const topic_name = node.value().get().get_pub_topic_prefix() + name; + if (!rclcpp_pub || published_topic != topic_name) { + rclcpp_pub = node.value().get().details->make_pub( + topic_name, rmcs::util::qos::debug); + published_topic = topic_name; + previous_ids.clear(); + } + + auto visual_marker = MarkerArray {}; + const auto current_time = rclcpp_clock.now(); + auto current_ids = std::unordered_set {}; + auto const arrow_name = std::format("{}_arrow", name); + current_ids.reserve(armors.size()); + + for (auto const& armor : armors) { + auto const camp = armor_color2camp_color(armor.color); + auto const marker_id = make_unique_marker_id(armor.genre, armor.id); + current_ids.emplace(marker_id); + + visual_marker.markers.emplace_back(make_marker(link_name, name, marker_id, Marker::CUBE, + Marker::ADD, armor.genre, camp, &armor, current_time)); + visual_marker.markers.emplace_back(make_marker(link_name, arrow_name, marker_id, + Marker::ARROW, Marker::ADD, armor.genre, camp, &armor, current_time)); + } + + for (auto const id : previous_ids) { + if (current_ids.contains(id)) { + continue; } - armor_ptr->move(input.translation, input.orientation); - armor_ptr->update(); + visual_marker.markers.emplace_back(make_marker(link_name, name, id, Marker::CUBE, + Marker::DELETE, rmcs::DeviceId {}, rmcs::CampColor {}, nullptr, current_time)); + visual_marker.markers.emplace_back(make_marker(link_name, arrow_name, id, + Marker::ARROW, Marker::DELETE, rmcs::DeviceId {}, rmcs::CampColor {}, nullptr, + current_time)); } + previous_ids = std::move(current_ids); + rclcpp_pub->publish(visual_marker); return true; } - static auto needs_rebuild( - ArmorShadow const& shadow, Armor3D const& input, std::string_view name) -> bool { - return shadow.genre != input.genre || shadow.color != input.color || shadow.id != input.id - || shadow.ns != name; - } + static inline rclcpp::Clock rclcpp_clock { RCL_STEADY_TIME }; std::optional> node; - std::vector current_armors; - std::vector> visual_armors; + std::shared_ptr> rclcpp_pub; + std::string published_topic; + std::unordered_set previous_ids; }; auto ArmorVisualizer::initialize(util::RclcppNode& visual_node) noexcept -> void { diff --git a/src/module/fire_control/aim_point_chooser.cpp b/src/module/fire_control/aim_point_chooser.cpp index 5daf46d3..62b13c8f 100644 --- a/src/module/fire_control/aim_point_chooser.cpp +++ b/src/module/fire_control/aim_point_chooser.cpp @@ -1,5 +1,7 @@ #include "aim_point_chooser.hpp" +#include + #include "utility/math/conversion.hpp" using namespace rmcs::fire_control; @@ -26,16 +28,14 @@ struct AimPointChooser::Impl { angular_velocity_threshold = config.angular_velocity_threshold; } - auto choose_armor(std::span armors, Eigen::Vector const& ekf_x) - -> std::optional { + auto choose_armor(std::span armors, Eigen::Vector3d const& center_position, + double angular_velocity) -> std::optional { if (armors.empty()) { last_chosen_id = -1; return std::nullopt; } - const auto car_y = ekf_x[2], car_x = ekf_x[0]; - const auto center_yaw = std::atan2(car_y, car_x); - const auto angular_velocity = ekf_x[7]; + const auto center_yaw = std::atan2(center_position.y(), center_position.x()); struct ArmorCandidate { int index; @@ -129,6 +129,6 @@ auto AimPointChooser::initialize(Config const& config) noexcept -> void { } auto AimPointChooser::choose_armor(std::span armors, - Eigen::Vector const& ekf_x) -> std::optional { - return pimpl->choose_armor(armors, ekf_x); + Eigen::Vector3d const& center_position, double angular_velocity) -> std::optional { + return pimpl->choose_armor(armors, center_position, angular_velocity); } diff --git a/src/module/fire_control/aim_point_chooser.hpp b/src/module/fire_control/aim_point_chooser.hpp index 9d968c5c..930d6f07 100644 --- a/src/module/fire_control/aim_point_chooser.hpp +++ b/src/module/fire_control/aim_point_chooser.hpp @@ -5,15 +5,11 @@ #include #include -#include "utility/math/kalman_filter/ekf.hpp" #include "utility/pimpl.hpp" #include "utility/robot/armor.hpp" namespace rmcs::fire_control { class AimPointChooser { -private: - using EKF = util::EKF<11, 4>; - public: struct Config { double coming_angle; // rad @@ -24,8 +20,8 @@ class AimPointChooser { }; auto initialize(Config const& config) noexcept -> void; - auto choose_armor(std::span armors, EKF::XVec const& ekf_x) - -> std::optional; + auto choose_armor(std::span armors, Eigen::Vector3d const& center_position, + double angular_velocity) -> std::optional; RMCS_PIMPL_DEFINITION(AimPointChooser) }; diff --git a/src/module/fire_control/shoot_evaluator.cpp b/src/module/fire_control/shoot_evaluator.cpp new file mode 100644 index 00000000..ee72b785 --- /dev/null +++ b/src/module/fire_control/shoot_evaluator.cpp @@ -0,0 +1,98 @@ +#include "shoot_evaluator.hpp" + +#include +#include + +#include "utility/math/angle.hpp" +#include "utility/serializable.hpp" + +using namespace rmcs::fire_control; + +struct ShootEvaluator::Impl { + struct Config : util::Serializable { + double first_tolerance { 4.0 }; // degree + double second_tolerance { 2.0 }; // degree + double judge_distance { 3.0 }; // m + bool auto_fire { true }; + + constexpr static std::tuple metas { + &Config::first_tolerance, + "first_tolerance", + &Config::second_tolerance, + "second_tolerance", + &Config::judge_distance, + "judge_distance", + &Config::auto_fire, + "auto_fire", + }; + }; + + Config config {}; + + double first_tolerance_ { 4.0 / 57.3 }; + double second_tolerance_ { 2.0 / 57.3 }; + double judge_distance_ { 3.0 }; + bool auto_fire_ { true }; + + std::optional last_command_ {}; + + auto initialize(const YAML::Node& yaml) noexcept -> std::expected { + auto result = config.serialize(yaml); + if (!result.has_value()) { + return std::unexpected { result.error() }; + } + + first_tolerance_ = config.first_tolerance / 57.3; + second_tolerance_ = config.second_tolerance / 57.3; + judge_distance_ = config.judge_distance; + auto_fire_ = config.auto_fire; + last_command_.reset(); + + if (!(first_tolerance_ > 0.0) || !(second_tolerance_ > 0.0)) { + return std::unexpected { "first_tolerance and second_tolerance must be > 0" }; + } + if (judge_distance_ < 0.0) { + return std::unexpected { "judge_distance must be >= 0" }; + } + + return {}; + } + + auto evaluate(Command const& command, double current_yaw) noexcept -> bool { + auto should_fire = false; + + if (!command.control || !auto_fire_) { + last_command_ = command; + return false; + } + + const auto tolerance = + (command.distance > judge_distance_) ? second_tolerance_ : first_tolerance_; + + if (last_command_.has_value() && command.auto_aim_enabled && command.aim_point_valid) { + const auto yaw_delta = + std::abs(util::normalize_angle(last_command_->yaw - command.yaw)); + const auto track_delta = + std::abs(util::normalize_angle(current_yaw - last_command_->yaw)); + + should_fire = (yaw_delta < tolerance * 2.0) && (track_delta < tolerance); + } + + last_command_ = command; + return should_fire; + } +}; + +ShootEvaluator::ShootEvaluator() noexcept + : pimpl { std::make_unique() } { } + +ShootEvaluator::~ShootEvaluator() noexcept = default; + +auto ShootEvaluator::initialize(const YAML::Node& yaml) noexcept + -> std::expected { + return pimpl->initialize(yaml); +} + +auto ShootEvaluator::evaluate(Command const& command, double current_yaw) noexcept -> bool { + return pimpl->evaluate(command, current_yaw); +} diff --git a/src/module/fire_control/shoot_evaluator.hpp b/src/module/fire_control/shoot_evaluator.hpp new file mode 100644 index 00000000..f0795eca --- /dev/null +++ b/src/module/fire_control/shoot_evaluator.hpp @@ -0,0 +1,29 @@ +#pragma once + +#include +#include + +#include + +#include "utility/pimpl.hpp" + +namespace rmcs::fire_control { + +class ShootEvaluator { + RMCS_PIMPL_DEFINITION(ShootEvaluator) + +public: + struct Command { + bool control { false }; + bool auto_aim_enabled { false }; + bool aim_point_valid { false }; + double yaw { 0. }; + double distance { 0. }; + }; + + auto initialize(const YAML::Node& yaml) noexcept -> std::expected; + + auto evaluate(Command const& command, double current_yaw) noexcept -> bool; +}; + +} // namespace rmcs::fire_control diff --git a/src/module/fire_control/trajectory_solution.cpp b/src/module/fire_control/trajectory_solution.cpp index 599460a3..6518ca4c 100644 --- a/src/module/fire_control/trajectory_solution.cpp +++ b/src/module/fire_control/trajectory_solution.cpp @@ -15,13 +15,10 @@ using namespace rmcs::fire_control; auto TrajectorySolution::solve() const -> std::optional { if (input.v0 <= 0 || input.target_d <= 0) return std::nullopt; - // 实际参与计算的阻力系数 = 基础系数 * 动态补偿 - const double k_effective = input.params.k * input.params.bias_scale; - double pitch = std::atan2(input.target_h, input.target_d); for (int i = 0; i < kMaxIterateCount; ++i) { - auto [actual_h, t] = Estimate(input.v0, pitch, input.target_d, k_effective); + auto [actual_h, t] = Estimate(input.v0, pitch, input.target_d, kAirResistanceCoefficient); auto h_error = input.target_h - actual_h; if (std::abs(h_error) < kHeightErrorThreold) { @@ -45,7 +42,7 @@ auto TrajectorySolution::solve() const -> std::optional { * @brief 弹道前向仿真（数值积分） * 计算在给定仰角下，飞行到水平距离 d 时的高度和时间 */ -auto TrajectorySolution::Estimate(double v0, double pitch, double d, double k) const +auto TrajectorySolution::Estimate(double v0, double pitch, double d, double air_resistance) const -> std::tuple { double x = 0, y = 0, t = 0; double vx = v0 * std::cos(pitch); @@ -58,12 +55,12 @@ auto TrajectorySolution::Estimate(double v0, double pitch, double d, double k) c prev_y = y; prev_t = t; - // F = -k * v * v_vec => a = -k * v * v_vec + // F = -c * v * v_vec => a = -c * v * v_vec const double v = std::sqrt(vx * vx + vy * vy); // dv = a * dt - vx -= k * v * vx * kEstimateDeltaTime; - vy -= (kGravity + k * v * vy) * kEstimateDeltaTime; + vx -= air_resistance * v * vx * kEstimateDeltaTime; + vy -= (kGravity + air_resistance * v * vy) * kEstimateDeltaTime; x += vx * kEstimateDeltaTime; y += vy * kEstimateDeltaTime; diff --git a/src/module/fire_control/trajectory_solution.hpp b/src/module/fire_control/trajectory_solution.hpp index f7c02b61..1757a074 100644 --- a/src/module/fire_control/trajectory_solution.hpp +++ b/src/module/fire_control/trajectory_solution.hpp @@ -6,16 +6,10 @@ namespace rmcs::fire_control { struct TrajectorySolution { - struct TrajectoryParams { - double k { 0.019 }; // 基础阻力系数 (小弹丸~0.019, 大弹丸~0.005) - double bias_scale { 1. }; // 动态补偿系数：修正额外阻力,阻力越大，该系数越大，default=1 - }; - struct Input { double v0 { 0. }; double target_d { 0. }; double target_h { 0. }; - TrajectoryParams params; } input; struct Output { @@ -26,7 +20,8 @@ struct TrajectorySolution { auto solve() const -> std::optional; private: - auto Estimate(double v0, double pitch, double d, double k) const -> std::tuple; + auto Estimate(double v0, double pitch, double d, double air_resistance) const + -> std::tuple; const int kMaxIterateCount { 10 }; const double kMaxPitchThreold { 57.3 / 57.3 }; // rad @@ -35,5 +30,6 @@ struct TrajectorySolution { const double kEstimateTimeOutThreold { 4.0 }; const double kMinVelocityX { 0.1 }; const double kGravity { 9.81 }; + const double kAirResistanceCoefficient { 0.003 }; }; } diff --git a/src/module/predictor/backend/robot_state_backend.cpp b/src/module/predictor/backend/robot_state_backend.cpp new file mode 100644 index 00000000..d66b80a4 --- /dev/null +++ b/src/module/predictor/backend/robot_state_backend.cpp @@ -0,0 +1,47 @@ +#include "module/predictor/backend/robot_state_backend.hpp" + +#include +#include + +#include "module/predictor/outpost/robot_state.hpp" +#include "module/predictor/regular/robot_state.hpp" + +namespace rmcs::predictor { + +template +class RobotStateBackendAdapter final : public IRobotStateBackend { +public: + explicit RobotStateBackendAdapter(Clock::time_point stamp) noexcept + : state { stamp } { } + + auto initialize(Armor3D const& armor, Clock::time_point t) -> void override { + state.initialize(armor, t); + } + + auto predict(Clock::time_point t) -> void override { state.predict(t); } + + auto update(std::span armors) -> bool override { return state.update(armors); } + + [[nodiscard]] auto is_converged() const -> bool override { return state.is_converged(); } + + [[nodiscard]] auto get_snapshot() const -> Snapshot override { return state.get_snapshot(); } + + [[nodiscard]] auto distance() const -> double override { return state.distance(); } + +private: + State state; +}; + +[[nodiscard]] auto make_robot_state_backend(RobotStateBackendKind kind, + IRobotStateBackend::Clock::time_point stamp) -> std::unique_ptr { + switch (kind) { + case RobotStateBackendKind::Outpost: + return std::make_unique>(stamp); + case RobotStateBackendKind::Regular: + return std::make_unique>(stamp); + } + + std::unreachable(); +} + +} // namespace rmcs::predictor::detail diff --git a/src/module/predictor/backend/robot_state_backend.hpp b/src/module/predictor/backend/robot_state_backend.hpp new file mode 100644 index 00000000..956bebe9 --- /dev/null +++ b/src/module/predictor/backend/robot_state_backend.hpp @@ -0,0 +1,45 @@ +#pragma once + +#include +#include +#include + +#include "module/predictor/snapshot.hpp" + +namespace rmcs::predictor { + +enum class RobotStateBackendKind : std::uint8_t { + Regular, + Outpost, +}; + +class IRobotStateBackend { +public: + using Clock = util::Clock; + + virtual ~IRobotStateBackend() noexcept = default; + + virtual auto initialize(Armor3D const& armor, Clock::time_point t) -> void = 0; + virtual auto predict(Clock::time_point t) -> void = 0; + + virtual auto update(std::span armors) -> bool = 0; + + [[nodiscard]] virtual auto is_converged() const -> bool = 0; + [[nodiscard]] virtual auto get_snapshot() const -> Snapshot = 0; + [[nodiscard]] virtual auto distance() const -> double = 0; +}; + +[[nodiscard]] constexpr auto classify_robot_state_backend(DeviceId device) noexcept + -> RobotStateBackendKind { + switch (device) { + case DeviceId::OUTPOST: + return RobotStateBackendKind::Outpost; + default: + return RobotStateBackendKind::Regular; + } +} + +[[nodiscard]] auto make_robot_state_backend(RobotStateBackendKind kind, + IRobotStateBackend::Clock::time_point stamp) -> std::unique_ptr; + +} // namespace rmcs::predictor diff --git a/src/module/predictor/backend/snapshot_backend.hpp b/src/module/predictor/backend/snapshot_backend.hpp new file mode 100644 index 00000000..9f909c8c --- /dev/null +++ b/src/module/predictor/backend/snapshot_backend.hpp @@ -0,0 +1,32 @@ +#pragma once + +#include "module/predictor/snapshot.hpp" +#include "utility/robot/color.hpp" +#include "utility/robot/id.hpp" + +namespace rmcs::predictor { + +struct ISnapshotBackend { + DeviceId device; + CampColor color; + int armor_num; + Snapshot::Clock::time_point stamp; + + ISnapshotBackend(DeviceId device, CampColor color, int armor_num, + Snapshot::Clock::time_point stamp) noexcept + : device { device } + , color { color } + , armor_num { armor_num } + , stamp { stamp } { } + + virtual ~ISnapshotBackend() noexcept = default; + + [[nodiscard]] virtual auto kinematics_at(Snapshot::Clock::time_point t) const + -> Snapshot::Kinematics = 0; + [[nodiscard]] virtual auto predicted_armors(Snapshot::Clock::time_point t) const + -> std::vector = 0; + + auto time_stamp() const -> Snapshot::Clock::time_point { return stamp; } +}; + +} // namespace rmcs::predictor diff --git a/src/module/predictor/outpost/armor_layout.hpp b/src/module/predictor/outpost/armor_layout.hpp new file mode 100644 index 00000000..6ea7e931 --- /dev/null +++ b/src/module/predictor/outpost/armor_layout.hpp @@ -0,0 +1,17 @@ +#pragma once + +#include + +namespace rmcs::predictor { + +struct OutpostArmorSlot { + double phase_offset { 0.0 }; + double height_offset { 0.0 }; + bool assigned { false }; +}; + +struct OutpostArmorLayout { + std::array slots {}; +}; + +} // namespace rmcs::predictor diff --git a/src/module/predictor/outpost/ekf_parameter.hpp b/src/module/predictor/outpost/ekf_parameter.hpp new file mode 100644 index 00000000..5ac392b9 --- /dev/null +++ b/src/module/predictor/outpost/ekf_parameter.hpp @@ -0,0 +1,223 @@ +#pragma once + +#include +#include +#include + +#include "module/predictor/outpost/armor_layout.hpp" +#include "utility/math/angle.hpp" +#include "utility/math/conversion.hpp" +#include "utility/math/kalman_filter/ekf.hpp" +#include "utility/robot/armor.hpp" +#include "utility/robot/constant.hpp" + +namespace rmcs::predictor { + +struct OutpostEKFParameters { + using EKF = util::EKF<6, 4>; + + static constexpr int kOutpostArmorCount = 3; + static constexpr double kPhaseStep = 2.0 * std::numbers::pi / kOutpostArmorCount; + + // x vx y vy z a + // x, y：前哨站旋转中心在世界坐标系下的位置 + // vx, vy：前哨站旋转中心在世界坐标系下的线速度 + // z：参考装甲板(id 0)在世界坐标系下的 z 坐标 + // a：参考装甲板(id 0)的 yaw 角 + static auto x(Armor3D const& armor) -> EKF::XVec { + const auto [trans_x, trans_y, trans_z] = armor.translation; + const auto [quat_x, quat_y, quat_z, quat_w] = armor.orientation; + const auto orientation = Eigen::Quaterniond { quat_w, quat_x, quat_y, quat_z }; + + const auto ypr = util::eulers(orientation); + const auto yaw = ypr[0]; + const auto center_x = trans_x + kOutpostRadius * std::cos(yaw); + const auto center_y = trans_y + kOutpostRadius * std::sin(yaw); + + auto x = EKF::XVec {}; + x << center_x, 0.0, center_y, 0.0, trans_z, yaw; + return x; + } + + static auto P_initial_dig() -> EKF::PDig { + auto P_dig = EKF::PDig {}; + P_dig << 1.0, 64.0, 1.0, 64.0, 1.0, 0.4; + return P_dig; + } + + static auto armor_yaw(EKF::XVec const& x, double phase_offset) -> double { + return util::normalize_angle(x[5] + phase_offset); + } + + static auto armor_yaw(EKF::XVec const& x, OutpostArmorLayout const& layout, int id) -> double { + auto normalized_id = std::clamp(id, 0, kOutpostArmorCount - 1); + return armor_yaw(x, layout.slots[normalized_id].phase_offset); + } + + static auto h_armor_z(EKF::XVec const& x, double height_offset) -> double { + return x[4] + height_offset; + } + + static auto h_armor_z(EKF::XVec const& x, OutpostArmorLayout const& layout, int id) -> double { + auto normalized_id = std::clamp(id, 0, kOutpostArmorCount - 1); + return h_armor_z(x, layout.slots[normalized_id].height_offset); + } + + static auto h_armor_xyz(EKF::XVec const& x, double phase_offset, double height_offset) + -> Eigen::Vector3d { + const auto phase = armor_yaw(x, phase_offset); + const auto pos_x = x[0] - kOutpostRadius * std::cos(phase); + const auto pos_y = x[2] - kOutpostRadius * std::sin(phase); + const auto pos_z = h_armor_z(x, height_offset); + return { pos_x, pos_y, pos_z }; + } + + static auto h_armor_xyz(EKF::XVec const& x, OutpostArmorLayout const& layout, int id) + -> Eigen::Vector3d { + auto normalized_id = std::clamp(id, 0, kOutpostArmorCount - 1); + return h_armor_xyz( + x, layout.slots[normalized_id].phase_offset, layout.slots[normalized_id].height_offset); + } + + static auto h(EKF::XVec const& x, double phase_offset, double height_offset) -> EKF::ZVec { + const auto xyz = h_armor_xyz(x, phase_offset, height_offset); + const auto ypd = util::xyz2ypd(xyz); + const auto yaw = armor_yaw(x, phase_offset); + + auto z = EKF::ZVec {}; + z << ypd[0], ypd[1], ypd[2], yaw; + return z; + } + + static auto h(EKF::XVec const& x, OutpostArmorLayout const& layout, int id) -> EKF::ZVec { + auto normalized_id = std::clamp(id, 0, kOutpostArmorCount - 1); + return h( + x, layout.slots[normalized_id].phase_offset, layout.slots[normalized_id].height_offset); + } + + static auto x_add(EKF::XVec const& a, EKF::XVec const& b) -> EKF::XVec { + auto result = EKF::XVec { a + b }; + result[5] = util::normalize_angle(result[5]); + return result; + } + + static auto z_subtract(EKF::ZVec const& a, EKF::ZVec const& b) -> EKF::ZVec { + auto result = EKF::ZVec { a - b }; + result[0] = util::normalize_angle(result[0]); + result[1] = util::normalize_angle(result[1]); + result[3] = util::normalize_angle(result[3]); + return result; + } + + static auto F(double dt) -> EKF::AMat { + auto F = EKF::AMat {}; + // clang-format off + F << + 1, dt, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, + 0, 0, 1, dt, 0, 0, + 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1; + // clang-format on + return F; + } + + static auto Q(double dt) -> EKF::QMat { + // 平面匀速模型中的未建模线加速度噪声，作用在 x/vx 与 y/vy + constexpr double linear_acc_var = 10.0; + // 参考装甲板 z 的随机游走噪声 + constexpr double z_ref_rw_var = 1e-3; + // 参考装甲板 yaw 角的随机游走噪声，用于补偿固定角速度模型误差 + constexpr double angle_rw_var = 1e-3; + const auto v1 = linear_acc_var; + const auto v2 = z_ref_rw_var; + const auto v3 = angle_rw_var; + + const auto a = dt * dt * dt * dt / 4.0; + const auto b = dt * dt * dt / 2.0; + const auto c = dt * dt; + + auto Q = EKF::QMat {}; + // clang-format off + Q << a * v1, b * v1, 0, 0, 0, 0, + b * v1, c * v1, 0, 0, 0, 0, + 0, 0, a * v1, b * v1, 0, 0, + 0, 0, b * v1, c * v1, 0, 0, + 0, 0, 0, 0, v2 * dt, 0, + 0, 0, 0, 0, 0, v3 * dt; + // clang-format on + return Q; + } + + static auto f(double dt, int spin_sign) -> auto { + return [dt, spin_sign](EKF::XVec const& x) { + EKF::XVec x_prior = x; + const auto angular_speed = spin_sign > 0 ? kOutpostAngularSpeed + : spin_sign < 0 ? -kOutpostAngularSpeed + : 0.0; + + x_prior[0] = x[0] + x[1] * dt; + x_prior[1] = x[1]; + x_prior[2] = x[2] + x[3] * dt; + x_prior[3] = x[3]; + x_prior[4] = x[4]; + x_prior[5] = util::normalize_angle(x[5] + angular_speed * dt); + return x_prior; + }; + } + + static auto R(Eigen::Vector3d const& xyz, Eigen::Vector3d const& ypr, + Eigen::Vector3d const& ypd) -> EKF::RMat { + const auto center_yaw = std::atan2(xyz[1], xyz[0]); + const auto delta_yaw = util::normalize_angle(ypr[0] - center_yaw); + const auto distance = ypd[2]; + + auto R_dig = EKF::RDig {}; + // clang-format off + R_dig << 4e-3, 4e-3, std::log(std::abs(delta_yaw) + 1.0) + 1.0, + std::log(std::abs(distance) + 1.0) / 200.0 + 9e-2; + // clang-format on + + return R_dig.asDiagonal(); + } + + static auto H(EKF::XVec const& x, double phase_offset, double height_offset) -> EKF::HMat { + const auto phase = armor_yaw(x, phase_offset); + const auto cos_phase = std::cos(phase); + const auto sin_phase = std::sin(phase); + const auto dx_da = kOutpostRadius * sin_phase; + const auto dy_da = -kOutpostRadius * cos_phase; + + auto H_armor_xyza = Eigen::Matrix {}; + // clang-format off + H_armor_xyza << + 1, 0, 0, 0, 0, dx_da, + 0, 0, 1, 0, 0, dy_da, + 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1; + // clang-format on + + const auto xyz = h_armor_xyz(x, phase_offset, height_offset); + const auto H_armor_ypd = util::xyz2ypd_jacobian(xyz); + + Eigen::Matrix H_armor_ypda; + // clang-format off + H_armor_ypda << + H_armor_ypd(0, 0), H_armor_ypd(0, 1), H_armor_ypd(0, 2), 0, + H_armor_ypd(1, 0), H_armor_ypd(1, 1), H_armor_ypd(1, 2), 0, + H_armor_ypd(2, 0), H_armor_ypd(2, 1), H_armor_ypd(2, 2), 0, + 0, 0, 0, 1; + // clang-format on + + return H_armor_ypda * H_armor_xyza; + } + + static auto H(EKF::XVec const& x, OutpostArmorLayout const& layout, int id) -> EKF::HMat { + auto normalized_id = std::clamp(id, 0, kOutpostArmorCount - 1); + return H( + x, layout.slots[normalized_id].phase_offset, layout.slots[normalized_id].height_offset); + } +}; + +} // namespace rmcs::predictor diff --git a/src/module/predictor/outpost/robot_state.cpp b/src/module/predictor/outpost/robot_state.cpp new file mode 100644 index 00000000..2af4aa5f --- /dev/null +++ b/src/module/predictor/outpost/robot_state.cpp @@ -0,0 +1,446 @@ +#include "robot_state.hpp" + +#include +#include +#include +#include +#include +#include + +#include "module/predictor/outpost/snapshot.hpp" +#include "utility/math/angle.hpp" +#include "utility/math/mahalanobis.hpp" +#include "utility/time.hpp" + +using namespace rmcs::predictor; + +namespace { + +constexpr int kUnknownArmorId = -1; +using OutpostEKF = OutpostRobotState::EKF; + +auto normalize_spin_sign(int spin_sign) -> int { + if (spin_sign > 0) return +1; + if (spin_sign < 0) return -1; + return 0; +} + +struct OutpostObservation { + OutpostEKF::ZVec z; + OutpostEKF::RMat R; + Eigen::Vector3d xyz; + Eigen::Vector3d ypr; + Eigen::Vector3d ypd; +}; + +auto make_observation(rmcs::Armor3D const& armor) -> OutpostObservation { + auto const [pos_x, pos_y, pos_z] = armor.translation; + auto const xyz = Eigen::Vector3d { pos_x, pos_y, pos_z }; + + auto const [quat_x, quat_y, quat_z, quat_w] = armor.orientation; + auto const orientation = Eigen::Quaterniond { quat_w, quat_x, quat_y, quat_z }; + + auto const ypr = rmcs::util::eulers(orientation); + auto const ypd = rmcs::util::xyz2ypd(xyz); + + auto z = OutpostEKF::ZVec {}; + z << ypd[0], ypd[1], ypd[2], ypr[0]; + + return { z, OutpostEKFParameters::R(xyz, ypr, ypd), xyz, ypr, ypd }; +} + +enum class SwitchEvent { + Stay, + ClockwiseSwitch, + CounterClockwiseSwitch, + Invalid, +}; + +struct AssociationDecision { + int armor_id { kUnknownArmorId }; + double error { std::numeric_limits::infinity() }; + bool is_valid { false }; + SwitchEvent event { SwitchEvent::Invalid }; + int inferred_spin_sign { 0 }; + double phase_offset { 0.0 }; + double height_offset { 0.0 }; + bool extends_layout { false }; +}; + +struct BestMatch { + OutpostObservation observation; + AssociationDecision decision; +}; + +struct MatchingConfig { + double azimuth_gate { rmcs::util::deg2rad(35.0) }; + double z_gate { 0.05 }; + double slot_phase_tolerance { rmcs::util::deg2rad(8.0) }; + double slot_height_tolerance { 0.02 }; + double mahalanobis_gate { 25.0 }; + double layout_extension_penalty { 0.25 }; + double continuity_bonus { 0.10 }; +}; + +struct TrackingConfig { + std::chrono::duration reset_interval { 1.5 }; + int spin_confirm_switches { 2 }; + int min_converged_updates { 6 }; + MatchingConfig matching {}; +}; + +struct SpinTracker { + int locked_sign { 0 }; + int candidate_sign { 0 }; + int candidate_count { 0 }; + bool locked { false }; + + auto reset() -> void { *this = {}; } + + auto current_sign() const -> int { + if (locked) return locked_sign; + return candidate_sign; + } + + auto observe_switch(int inferred_spin_sign, int confirm_switches) -> void { + auto const normalized = normalize_spin_sign(inferred_spin_sign); + if (normalized == 0 || locked) return; + + if (candidate_sign == normalized) { + candidate_count++; + } else { + candidate_sign = normalized; + candidate_count = 1; + } + + if (candidate_count < confirm_switches) return; + + locked_sign = candidate_sign; + locked = true; + } +}; + +auto assigned_count(OutpostArmorLayout const& layout) -> int { + return static_cast(std::ranges::count(layout.slots, true, &OutpostArmorSlot::assigned)); +} + +auto has_assigned_slot(OutpostArmorLayout const& layout, int armor_id) -> bool { + return armor_id >= 0 && armor_id < OutpostEKFParameters::kOutpostArmorCount + && layout.slots[armor_id].assigned; +} + +auto first_unassigned_slot(OutpostArmorLayout const& layout) -> int { + for (int id = 0; id < OutpostEKFParameters::kOutpostArmorCount; ++id) { + if (!layout.slots[id].assigned) return id; + } + return kUnknownArmorId; +} + +auto layout_after_association(OutpostArmorLayout const& layout, AssociationDecision const& decision) + -> OutpostArmorLayout { + auto next_layout = layout; + if (!decision.extends_layout) return next_layout; + + next_layout.slots[decision.armor_id].phase_offset = decision.phase_offset; + next_layout.slots[decision.armor_id].height_offset = decision.height_offset; + next_layout.slots[decision.armor_id].assigned = true; + return next_layout; +} + +auto switch_phase_delta(int inferred_spin_sign) -> double { + return -normalize_spin_sign(inferred_spin_sign) * OutpostEKFParameters::kPhaseStep; +} + +auto switch_height_deltas(int inferred_spin_sign) -> std::array { + if (normalize_spin_sign(inferred_spin_sign) > 0) { + return { rmcs::kOutpostArmorHeightStep, -2.0 * rmcs::kOutpostArmorHeightStep }; + } + return { -rmcs::kOutpostArmorHeightStep, 2.0 * rmcs::kOutpostArmorHeightStep }; +} + +auto consider_candidate(AssociationDecision const& candidate, AssociationDecision& best_decision) + -> void { + if (!candidate.is_valid || candidate.error >= best_decision.error) return; + best_decision = candidate; +} + +class AssociationEngine { +public: + AssociationEngine(OutpostEKF::XVec const& x, OutpostEKF::PMat const& P, + OutpostArmorLayout const& layout, int current_armor_id, SpinTracker const& spin, + MatchingConfig const& config) noexcept + : x_ { x } + , P_ { P } + , layout_ { layout } + , current_armor_id_ { current_armor_id } + , spin_ { spin } + , config_ { config } { } + + auto decide(OutpostObservation const& observation) const -> AssociationDecision { + if (!has_assigned_slot(layout_, current_armor_id_)) return {}; + + auto best_decision = AssociationDecision {}; + auto const current_phase = layout_.slots[current_armor_id_].phase_offset; + auto const current_height = layout_.slots[current_armor_id_].height_offset; + + consider_candidate(evaluate_candidate(observation, current_armor_id_, current_phase, + current_height, SwitchEvent::Stay, 0, false), + best_decision); + + if (spin_.locked) { + auto const event = spin_.locked_sign > 0 ? SwitchEvent::CounterClockwiseSwitch + : SwitchEvent::ClockwiseSwitch; + consider_switch_direction(observation, current_phase, current_height, spin_.locked_sign, + event, best_decision); + } else { + consider_switch_direction(observation, current_phase, current_height, -1, + SwitchEvent::ClockwiseSwitch, best_decision); + consider_switch_direction(observation, current_phase, current_height, +1, + SwitchEvent::CounterClockwiseSwitch, best_decision); + } + + return best_decision; + } + +private: + auto evaluate_candidate(OutpostObservation const& observation, int armor_id, + double phase_offset, double height_offset, SwitchEvent event, int inferred_spin_sign, + bool extends_layout) const -> AssociationDecision { + auto const predicted_xyz = + OutpostEKFParameters::h_armor_xyz(x_, phase_offset, height_offset); + auto const predicted_ypd = rmcs::util::xyz2ypd(predicted_xyz); + + auto const azimuth_error = + std::abs(rmcs::util::normalize_angle(observation.ypd[0] - predicted_ypd[0])); + auto const z_error = std::abs(observation.xyz[2] - predicted_xyz[2]); + + // 这里没有加yaw约束，一是因为yaw的抖动太大，二是因为大部分图像中 一帧只有一块装甲板 + if (azimuth_error > config_.azimuth_gate || z_error > config_.z_gate) { + return {}; + } + + auto const H = OutpostEKFParameters::H(x_, phase_offset, height_offset); + auto const z_hat = OutpostEKFParameters::h(x_, phase_offset, height_offset); + auto const innovation = OutpostEKFParameters::z_subtract(observation.z, z_hat); + auto const S = H * P_ * H.transpose() + observation.R; + auto const mahalanobis = rmcs::util::mahalanobis_distance(innovation, S); + if (!mahalanobis.has_value() || *mahalanobis > config_.mahalanobis_gate) { + return {}; + } + + auto error = *mahalanobis; + if (extends_layout) error += config_.layout_extension_penalty; + if (event == SwitchEvent::Stay) error -= config_.continuity_bonus; + + return { armor_id, error, true, event, normalize_spin_sign(inferred_spin_sign), + rmcs::util::normalize_angle(phase_offset), height_offset, extends_layout }; + } + + auto find_matching_slot(double phase_offset, double height_offset, int excluded_armor_id) const + -> int { + auto best_slot = kUnknownArmorId; + auto best_mismatch = std::numeric_limits::infinity(); + + for (int id = 0; id < OutpostEKFParameters::kOutpostArmorCount; ++id) { + if (!layout_.slots[id].assigned || id == excluded_armor_id) continue; + + auto const phase_error = std::abs( + rmcs::util::normalize_angle(layout_.slots[id].phase_offset - phase_offset)); + auto const height_error = std::abs(layout_.slots[id].height_offset - height_offset); + if (phase_error > config_.slot_phase_tolerance + || height_error > config_.slot_height_tolerance) + continue; + + auto const mismatch = phase_error + height_error / rmcs::kOutpostArmorHeightStep; + if (mismatch >= best_mismatch) continue; + + best_mismatch = mismatch; + best_slot = id; + } + + return best_slot; + } + + auto consider_switch_direction(OutpostObservation const& observation, double current_phase, + double current_height, int inferred_spin_sign, SwitchEvent event, + AssociationDecision& best_decision) const -> void { + auto const candidate_phase = + rmcs::util::normalize_angle(current_phase + switch_phase_delta(inferred_spin_sign)); + + for (auto const height_delta : switch_height_deltas(inferred_spin_sign)) { + auto const candidate_height = current_height + height_delta; + + auto armor_id = + find_matching_slot(candidate_phase, candidate_height, current_armor_id_); + auto extends_layout = false; + if (armor_id == kUnknownArmorId) { + armor_id = first_unassigned_slot(layout_); + extends_layout = (armor_id != kUnknownArmorId); + } + if (armor_id == kUnknownArmorId) continue; + + consider_candidate(evaluate_candidate(observation, armor_id, candidate_phase, + candidate_height, event, inferred_spin_sign, extends_layout), + best_decision); + } + } + + OutpostEKF::XVec const& x_; + OutpostEKF::PMat const& P_; + OutpostArmorLayout const& layout_; + int current_armor_id_ { kUnknownArmorId }; + SpinTracker const& spin_; + MatchingConfig const& config_; +}; + +} // namespace + +struct OutpostRobotState::Impl { + explicit Impl(Clock::time_point stamp) noexcept + : time_stamp { stamp } { } + + auto initialize(Armor3D const& armor, Clock::time_point t) -> void { + color = armor_color2camp_color(armor.color); + ekf = EKF { OutpostEKFParameters::x(armor), + OutpostEKFParameters::P_initial_dig().asDiagonal() }; + time_stamp = t; + + layout = OutpostArmorLayout {}; + layout.slots[0].assigned = true; + + spin.reset(); + update_count = 0; + current_armor_id = 0; + initialized = true; + } + + auto predict(Clock::time_point t) -> void { + if (initialized) { + auto dt = rmcs::util::delta_time(t, time_stamp); + if (dt > config.reset_interval) { + reset_runtime_state(t); + return; + } + + auto const dt_s = dt.count(); + ekf.predict( + OutpostEKFParameters::f(dt_s, spin.current_sign()), + [dt_s](EKF::XVec const&) { return OutpostEKFParameters::F(dt_s); }, + OutpostEKFParameters::Q(dt_s)); + } + + time_stamp = t; + } + + auto update(std::span armors) -> bool { + if (armors.empty()) return false; + + if (!initialized) { + initialize(armors.front(), time_stamp); + return true; + } + + auto best_match = select_best_match(armors); + if (!best_match.has_value()) return false; + + apply_association(best_match->decision, best_match->observation); + return true; + } + + auto is_converged() const -> bool { + return initialized && spin.locked + && assigned_count(layout) == OutpostEKFParameters::kOutpostArmorCount + && update_count >= config.min_converged_updates; + } + + auto get_snapshot() const -> Snapshot { + return detail::make_outpost_snapshot( + ekf.x, color, assigned_count(layout), time_stamp, spin.current_sign(), layout); + } + + auto distance() const -> double { return std::sqrt(ekf.x[0] * ekf.x[0] + ekf.x[2] * ekf.x[2]); } + +private: + auto reset_runtime_state(Clock::time_point t) -> void { + color = CampColor::UNKNOWN; + ekf = EKF {}; + layout = OutpostArmorLayout {}; + time_stamp = t; + initialized = false; + current_armor_id = kUnknownArmorId; + spin.reset(); + update_count = 0; + } + + auto select_best_match(std::span armors) const -> std::optional { + auto best_match = std::optional {}; + auto matcher = + AssociationEngine { ekf.x, ekf.P(), layout, current_armor_id, spin, config.matching }; + + for (auto const& armor : armors) { + auto observation = make_observation(armor); + auto decision = matcher.decide(observation); + if (!decision.is_valid) continue; + if (best_match.has_value() && decision.error >= best_match->decision.error) continue; + + best_match = BestMatch { observation, decision }; + } + + return best_match; + } + + auto apply_association( + AssociationDecision const& decision, OutpostObservation const& observation) -> void { + auto const next_layout = layout_after_association(layout, decision); + + ekf.update( + observation.z, + [layout = next_layout, armor_id = decision.armor_id]( + EKF::XVec const& x) { return OutpostEKFParameters::h(x, layout, armor_id); }, + [layout = next_layout, armor_id = decision.armor_id]( + EKF::XVec const& x) { return OutpostEKFParameters::H(x, layout, armor_id); }, + observation.R, OutpostEKFParameters::x_add, OutpostEKFParameters::z_subtract); + + layout = next_layout; + current_armor_id = decision.armor_id; + if (decision.event != SwitchEvent::Stay) { + spin.observe_switch(decision.inferred_spin_sign, config.spin_confirm_switches); + } + update_count++; + } + + CampColor color { CampColor::UNKNOWN }; + EKF ekf { EKF {} }; + OutpostArmorLayout layout {}; + Clock::time_point time_stamp; + + bool initialized { false }; + int current_armor_id { kUnknownArmorId }; + SpinTracker spin {}; + int update_count { 0 }; + TrackingConfig config {}; +}; + +OutpostRobotState::OutpostRobotState() noexcept + : OutpostRobotState(Clock::now()) { } + +OutpostRobotState::OutpostRobotState(Clock::time_point stamp) noexcept + : pimpl { std::make_unique(stamp) } { } + +OutpostRobotState::~OutpostRobotState() noexcept = default; + +auto OutpostRobotState::initialize(Armor3D const& armor, Clock::time_point t) -> void { + return pimpl->initialize(armor, t); +} + +auto OutpostRobotState::predict(Clock::time_point t) -> void { return pimpl->predict(t); } + +auto OutpostRobotState::update(std::span armors) -> bool { + return pimpl->update(armors); +} + +auto OutpostRobotState::is_converged() const -> bool { return pimpl->is_converged(); } + +auto OutpostRobotState::get_snapshot() const -> Snapshot { return pimpl->get_snapshot(); } + +auto OutpostRobotState::distance() const -> double { return pimpl->distance(); } diff --git a/src/module/predictor/outpost/robot_state.hpp b/src/module/predictor/outpost/robot_state.hpp new file mode 100644 index 00000000..b3168b1f --- /dev/null +++ b/src/module/predictor/outpost/robot_state.hpp @@ -0,0 +1,32 @@ +#pragma once + +#include +#include + +#include "module/predictor/outpost/ekf_parameter.hpp" +#include "module/predictor/snapshot.hpp" +#include "utility/clock.hpp" +#include "utility/pimpl.hpp" + +namespace rmcs::predictor { + +class OutpostRobotState { +public: + using Clock = util::Clock; + using EKF = OutpostEKFParameters::EKF; + + explicit OutpostRobotState(Clock::time_point stamp) noexcept; + + auto initialize(Armor3D const& armor, Clock::time_point t) -> void; + auto predict(Clock::time_point t) -> void; + + auto update(std::span armors) -> bool; + + auto is_converged() const -> bool; + auto get_snapshot() const -> Snapshot; + auto distance() const -> double; + + RMCS_PIMPL_DEFINITION(OutpostRobotState) +}; + +} // namespace rmcs::predictor diff --git a/src/module/predictor/outpost/snapshot.cpp b/src/module/predictor/outpost/snapshot.cpp new file mode 100644 index 00000000..0da020c8 --- /dev/null +++ b/src/module/predictor/outpost/snapshot.cpp @@ -0,0 +1,105 @@ +#include "module/predictor/outpost/snapshot.hpp" + +#include +#include + +#include "module/predictor/outpost/ekf_parameter.hpp" +#include "module/predictor/backend/snapshot_backend.hpp" +#include "utility/math/conversion.hpp" +#include "utility/time.hpp" + +namespace rmcs::predictor { + +namespace { + + auto normalize_spin_sign(int spin_sign) -> int { + if (spin_sign > 0) return +1; + if (spin_sign < 0) return -1; + return 0; + } + + auto make_armor(DeviceId device, CampColor color, int id) -> Armor3D { + auto armor = Armor3D {}; + armor.genre = device; + armor.color = camp_color2armor_color(color); + armor.id = id; + return armor; + } + + struct OutpostSnapshotBackend final : ISnapshotBackend { + explicit OutpostSnapshotBackend( + Snapshot::OutpostEKF::XVec x, CampColor color, int armor_num, + Snapshot::Clock::time_point stamp, int spin_sign, OutpostArmorLayout layout) noexcept + : ISnapshotBackend { DeviceId::OUTPOST, color, armor_num, stamp } + , x { std::move(x) } + , spin_sign { normalize_spin_sign(spin_sign) } + , layout { layout } { } + + [[nodiscard]] auto kinematics_at(Snapshot::Clock::time_point t) const + -> Snapshot::Kinematics override { + return kinematics_of(predict_state_at(t)); + } + + [[nodiscard]] auto predicted_armors(Snapshot::Clock::time_point t) const + -> std::vector override { + auto const predicted_x = predict_state_at(t); + auto const max_armors = + std::clamp(armor_num, 0, OutpostEKFParameters::kOutpostArmorCount); + + auto armors = std::vector {}; + armors.reserve(max_armors); + + for (int id = 0; id < max_armors; ++id) { + if (!layout.slots[id].assigned) continue; + + auto armor = make_armor(device, color, id); + auto const angle = OutpostEKFParameters::armor_yaw(predicted_x, layout, id); + auto const position = OutpostEKFParameters::h_armor_xyz(predicted_x, layout, id); + + armor.translation = position; + armor.orientation = + util::euler_to_quaternion(angle, kPredictedOutpostArmorPitch, 0); + armors.emplace_back(armor); + } + + return armors; + } + + private: + auto kinematics_of(Snapshot::OutpostEKF::XVec const& x) const -> Snapshot::Kinematics { + auto const max_armors = + std::clamp(armor_num, 0, OutpostEKFParameters::kOutpostArmorCount); + double height_sum = 0.0; + int assigned_count = 0; + for (int id = 0; id < max_armors; ++id) { + if (!layout.slots[id].assigned) continue; + height_sum += layout.slots[id].height_offset; + assigned_count++; + } + + auto const center_z = x[4] + + (assigned_count == 0 ? 0.0 : height_sum / static_cast(assigned_count)); + auto const angular_velocity = static_cast(spin_sign) * kOutpostAngularSpeed; + return { Eigen::Vector3d { x[0], x[2], center_z }, angular_velocity }; + } + + auto predict_state_at(Snapshot::Clock::time_point t) const -> Snapshot::OutpostEKF::XVec { + auto const dt = util::delta_time(t, stamp).count(); + return OutpostEKFParameters::f(dt, spin_sign)(x); + } + + Snapshot::OutpostEKF::XVec x; + int spin_sign; + OutpostArmorLayout layout; + }; + +} // namespace + +auto detail::make_outpost_snapshot(Snapshot::OutpostEKF::XVec ekf_x, CampColor color, int armor_num, + Snapshot::Clock::time_point stamp, int outpost_spin_sign, + OutpostArmorLayout outpost_layout) noexcept -> Snapshot { + return detail::make_snapshot(std::make_unique( + std::move(ekf_x), color, armor_num, stamp, outpost_spin_sign, outpost_layout)); +} + +} // namespace rmcs::predictor diff --git a/src/module/predictor/outpost/snapshot.hpp b/src/module/predictor/outpost/snapshot.hpp new file mode 100644 index 00000000..d65d652c --- /dev/null +++ b/src/module/predictor/outpost/snapshot.hpp @@ -0,0 +1,13 @@ +#pragma once + +#include "module/predictor/outpost/armor_layout.hpp" +#include "module/predictor/snapshot.hpp" +#include "utility/robot/color.hpp" + +namespace rmcs::predictor::detail { + +auto make_outpost_snapshot(Snapshot::OutpostEKF::XVec ekf_x, CampColor color, int armor_num, + Snapshot::Clock::time_point stamp, int outpost_spin_sign, + OutpostArmorLayout outpost_layout) noexcept -> Snapshot; + +} // namespace rmcs::predictor::detail diff --git a/src/module/predictor/ekf_parameter.hpp b/src/module/predictor/regular/ekf_parameter.hpp similarity index 72% rename from src/module/predictor/ekf_parameter.hpp rename to src/module/predictor/regular/ekf_parameter.hpp index 486510f7..9b714466 100644 --- a/src/module/predictor/ekf_parameter.hpp +++ b/src/module/predictor/regular/ekf_parameter.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include "utility/math/angle.hpp" #include "utility/math/conversion.hpp" @@ -14,6 +15,24 @@ namespace rmcs::predictor { struct EKFParameters { using EKF = util::EKF<11, 4>; + static auto armor_yaw(DeviceId const& device, EKF::XVec const& x, int id) -> double { + return util::normalize_angle(x[6] + id * 2 * std::numbers::pi / armor_num(device)); + } + + static auto h_armor_z(DeviceId const& device, EKF::XVec const& x, int id) -> double { + auto num = armor_num(device); + const auto use_l_h = (num == 4) && (id == 1 || id == 3); + return use_l_h ? (x[4] + x[10]) : x[4]; + } + + // x vx y vy z vz a w r l h + // x, y, z：装甲板旋转中心在世界坐标系下的位置 + // vx, vy, vz：装甲板旋转中心在世界坐标系下的线速度 + // a: angle,装甲板相对于旋转中心的 yaw 角 + // w: angular velocity 角速度 + // r: 装甲板中心到旋转中心的半径 + // l: 连续两次观测到的半径差 r2 - r1，用于描述装甲板切换时的半径变化 + // h: 连续两次观测到的高度差 z2 - z1，反映不同装甲板之间的竖直偏移 static auto x(Armor3D const& armor) -> EKF::XVec { const auto r = radius(armor.genre); @@ -25,17 +44,14 @@ struct EKFParameters { const double yaw = ypr[0]; const auto center_x = trans_x + r * std::cos(yaw); const auto center_y = trans_y + r * std::sin(yaw); - const auto center_z = trans_z; - auto x = EKF::XVec { center_x, 0, center_y, 0, center_z, 0, yaw, 0, r, 0, 0 }; + auto x = EKF::XVec { center_x, 0, center_y, 0, trans_z, 0, yaw, 0, r, 0, 0 }; return x; } static auto P_initial_dig(DeviceId const& device) -> EKF::PDig { auto P_dig = EKF::PDig {}; - if (device == DeviceId::OUTPOST) { - P_dig << 1, 64, 1, 64, 1, 81, 0.4, 100, 1e-4, 0, 0; - } else if (device == DeviceId::BASE) { + if (device == DeviceId::BASE) { P_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1e-4, 0, 0; } else { P_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; @@ -45,7 +61,6 @@ struct EKFParameters { } static auto radius(DeviceId const& device) -> double { - switch (device) { case DeviceId::OUTPOST: return kOutpostRadius; @@ -98,15 +113,10 @@ struct EKFParameters { // Piecewise White Noise Model // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb - static auto Q(DeviceId const& device, double dt) -> EKF::QMat { + static auto Q(double dt) -> EKF::QMat { double acc_var, angular_acc_var; - if (device == DeviceId::OUTPOST) { - acc_var = 10; - angular_acc_var = 0.1; - } else { - acc_var = 100; - angular_acc_var = 400; - } + acc_var = 100; + angular_acc_var = 400; const auto a = dt * dt * dt * dt / 4.; const auto b = dt * dt * dt / 2; @@ -133,36 +143,27 @@ struct EKFParameters { } // 计算出装甲板中心的坐标（考虑长短轴） - static auto h_armor_xyz(EKF::XVec const& x, int id, int armor_num) -> Eigen::Vector3d { - // x vx y vy z vz a w r l h - // x, y, z：装甲板旋转中心在世界坐标系下的位置 - // vx, vy, vz：装甲板旋转中心在世界坐标系下的线速度 - // a: angle,装甲板相对于旋转中心的 yaw 角 - // w: angular velocity 角速度 - // r: 装甲板中心到旋转中心的半径 - // l: 连续两次观测到的半径差 r2 - r1，用于描述装甲板切换时的半径变化 - // h: 连续两次观测到的高度差 z2 - z1，反映不同装甲板之间的竖直偏移 - auto angle = x[6]; - angle = util::normalize_angle(angle + id * 2 * std::numbers::pi / armor_num); + static auto h_armor_xyz(DeviceId const& device, EKF::XVec const& x, int id, int armor_num) + -> Eigen::Vector3d { + const auto phase = armor_yaw(device, x, id); + auto radius = x[8]; const auto use_l_h = (armor_num == 4) && (id == 1 || id == 3); - const auto r_min = x[8], l = x[9]; - const auto r = (use_l_h) ? (r_min + l) : r_min; + if (use_l_h) radius += x[9]; - const auto center_x = x[0], center_y = x[2], z_min = x[4], h = x[10]; - const auto pos_x = center_x - r * std::cos(angle); - const auto pos_y = center_y - r * std::sin(angle); - const auto pos_z = (use_l_h) ? (z_min + h) : z_min; + const auto center_x = x[0], center_y = x[2]; + const auto pos_x = center_x - radius * std::cos(phase); + const auto pos_y = center_y - radius * std::sin(phase); + const auto pos_z = h_armor_z(device, x, id); const auto result = Eigen::Vector3d { pos_x, pos_y, pos_z }; return result; } - static auto h(EKF::XVec const& x, int id, int armor_num) -> EKF::ZVec { - const auto xyz = h_armor_xyz(x, id, armor_num); + static auto h(DeviceId const& device, EKF::XVec const& x, int id, int armor_num) -> EKF::ZVec { + const auto xyz = h_armor_xyz(device, x, id, armor_num); const auto ypd = util::xyz2ypd(xyz); - auto angle = x(6); - const auto yaw = util::normalize_angle(angle + id * 2 * std::numbers::pi / armor_num); + const auto yaw = armor_yaw(device, x, id); const auto result = EKF::ZVec { ypd[0], ypd[1], ypd[2], yaw }; return result; @@ -171,8 +172,7 @@ struct EKFParameters { static auto f(double dt) -> auto { return [dt](EKF::XVec const& x) { EKF::XVec x_prior = F(dt) * x; - const auto yaw = x_prior[6]; - x_prior[6] = util::normalize_angle(yaw); + x_prior[6] = util::normalize_angle(x_prior[6]); return x_prior; }; } @@ -199,15 +199,7 @@ struct EKFParameters { return R; } - static auto H(EKF::XVec const& x, int id, int armor_num) -> EKF::HMat { - // x vx y vy z vz a w r l h - // x, y, z：装甲板旋转中心在世界坐标系下的位置 - // vx, vy, vz：装甲板旋转中心在世界坐标系下的线速度 - // a: angle,装甲板相对于旋转中心的 yaw 角 - // w: angular velocity 角速度 - // r: 装甲板中心到旋转中心的半径 - // l: 连续两次观测到的半径差 r2 - r1，用于描述装甲板切换时的半径变化 - // h: 连续两次观测到的高度差 z2 - z1，反映不同装甲板之间的竖直偏移 + static auto H(DeviceId const& device, EKF::XVec const& x, int id, int armor_num) -> EKF::HMat { auto angle = x[6]; angle = util::normalize_angle(angle + id * 2 * std::numbers::pi / armor_num); const auto cos_angle = std::cos(angle); @@ -237,7 +229,7 @@ struct EKFParameters { 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0; // clang-format on - auto xyz = h_armor_xyz(x, id, armor_num); + auto xyz = h_armor_xyz(device, x, id, armor_num); auto H_armor_ypd = util::xyz2ypd_jacobian(xyz); Eigen::Matrix H_armor_ypda; diff --git a/src/module/predictor/regular/robot_state.cpp b/src/module/predictor/regular/robot_state.cpp new file mode 100644 index 00000000..e23a42a8 --- /dev/null +++ b/src/module/predictor/regular/robot_state.cpp @@ -0,0 +1,200 @@ +#include "robot_state.hpp" + +#include +#include +#include +#include + +#include "module/predictor/regular/snapshot.hpp" +#include "utility/time.hpp" + +using namespace rmcs::predictor; + +struct RegularRobotState::Impl { + struct MatchResult { + int armor_id; + double error; + bool is_valid; + }; + + explicit Impl(Clock::time_point stamp) noexcept + : time_stamp { stamp } { } + + auto initialize(Armor3D const& armor, Clock::time_point t) -> void { + device = armor.genre; + color = armor_color2camp_color(armor.color); + armor_num = EKFParameters::armor_num(armor.genre); + time_stamp = t; + update_count = 0; + ekf = EKF { EKFParameters::x(armor), EKFParameters::P_initial_dig(device).asDiagonal() }; + initialized = true; + } + + auto predict(Clock::time_point t) -> void { + if (initialized) { + auto dt = util::delta_time(t, time_stamp); + if (dt > reset_interval) { + initialized = false; + update_count = 0; + time_stamp = t; + return; + } + + auto dt_s = dt.count(); + ekf.predict( + EKFParameters::f(dt_s), [dt_s](EKF::XVec const&) { return EKFParameters::F(dt_s); }, + EKFParameters::Q(dt_s)); + } + + time_stamp = t; + } + + auto update(std::span armors) -> bool { + bool fused = false; + for (auto const& armor : armors) + fused = update_single(armor) || fused; + + if (fused) ++update_count; + + return fused; + } + + auto is_converged() const -> bool { + if (!initialized) return false; + + auto const r = ekf.x[8]; + auto const l = ekf.x[8] + ekf.x[9]; + + auto const r_ok = (r > 0.05) && (r < 0.5); + auto const l_ok = (l > 0.05) && (l < 0.5); + + int min_updates = 3; + return r_ok && l_ok && update_count >= min_updates; + } + + auto get_snapshot() const -> Snapshot { + if (!initialized) return Snapshot::empty(time_stamp); + return detail::make_regular_snapshot(ekf.x, device, color, armor_num, time_stamp); + } + + auto distance() const -> double { + if (!initialized) return std::numeric_limits::infinity(); + return std::sqrt(ekf.x[0] * ekf.x[0] + ekf.x[2] * ekf.x[2]); + } + +private: + auto match(Armor3D const& armor) const -> MatchResult { + if (!initialized || armor.genre != device) return { -1, 1e10, false }; + + auto armors_xyza = calculate_armors(ekf.x); + + auto const [pos_x, pos_y, pos_z] = armor.translation; + auto const xyz = Eigen::Vector3d { pos_x, pos_y, pos_z }; + auto const orientation = Eigen::Quaterniond { armor.orientation.w, armor.orientation.x, + armor.orientation.y, armor.orientation.z }; + auto const ypr_in_world = util::eulers(orientation); + auto const ypd_in_world = util::xyz2ypd(xyz); + + auto it = + std::ranges::min_element(armors_xyza, [&](auto const& a_xyza, auto const& b_xyza) { + auto get_error = [&](auto const& pred) { + auto ypd_pred = util::xyz2ypd(pred.template head<3>()); + return std::abs(util::normalize_angle(ypr_in_world[0] - pred[3])) + + std::abs(util::normalize_angle(ypd_in_world[0] - ypd_pred[0])); + }; + + return get_error(a_xyza) < get_error(b_xyza); + }); + + auto const best_id = static_cast(std::distance(armors_xyza.begin(), it)); + auto const min_error = [&] { + auto ypd_pred = util::xyz2ypd(it->template head<3>()); + return std::abs(util::normalize_angle(ypr_in_world[0] - (*it)[3])) + + std::abs(util::normalize_angle(ypd_in_world[0] - ypd_pred[0])); + }(); + + return { best_id, min_error, min_error < angle_error_threshold }; + } + + auto update_single(Armor3D const& armor) -> bool { + if (!initialized) { + initialize(armor, time_stamp); + return true; + } + if (armor.genre != device) return false; + + auto match_result = match(armor); + if (!match_result.is_valid) return false; + + auto const [pos_x, pos_y, pos_z] = armor.translation; + auto const xyz = Eigen::Vector3d { pos_x, pos_y, pos_z }; + auto const ypd = util::xyz2ypd(xyz); + + auto const [quat_x, quat_y, quat_z, quat_w] = armor.orientation; + auto const orientation = Eigen::Quaterniond { quat_w, quat_x, quat_y, quat_z }; + auto const ypr = util::eulers(orientation); + + auto z = EKF::ZVec {}; + z << ypd[0], ypd[1], ypd[2], ypr[0]; + + ekf.update( + z, + [id = match_result.armor_id, this]( + EKF::XVec const& x) { return EKFParameters::h(device, x, id, armor_num); }, + [id = match_result.armor_id, this]( + EKF::XVec const& x) { return EKFParameters::H(device, x, id, armor_num); }, + EKFParameters::R(xyz, ypr, ypd), EKFParameters::x_add, EKFParameters::z_subtract); + + return true; + } + + auto calculate_armors(EKF::XVec const& x) const -> std::vector { + auto armors = std::vector {}; + armors.reserve(armor_num); + for (int i = 0; i < armor_num; ++i) { + auto angle = EKFParameters::armor_yaw(device, x, i); + auto xyz = EKFParameters::h_armor_xyz(device, x, i, armor_num); + armors.emplace_back(xyz[0], xyz[1], xyz[2], angle); + } + return armors; + } + + DeviceId device { DeviceId::UNKNOWN }; + CampColor color { CampColor::UNKNOWN }; + int armor_num { 0 }; + + EKF ekf { EKF {} }; + Clock::time_point time_stamp; + + bool initialized { false }; + int update_count { 0 }; + + const std::chrono::duration reset_interval { 1.0 }; + const double angle_error_threshold { 0.65 }; +}; + +RegularRobotState::RegularRobotState() noexcept + : RegularRobotState(Clock::now()) { } + +RegularRobotState::RegularRobotState(Clock::time_point stamp) noexcept + : pimpl { std::make_unique(stamp) } { } + +RegularRobotState::~RegularRobotState() noexcept = default; +RegularRobotState::RegularRobotState(RegularRobotState&&) noexcept = default; +auto RegularRobotState::operator=(RegularRobotState&&) noexcept -> RegularRobotState& = default; + +auto RegularRobotState::initialize(Armor3D const& armor, Clock::time_point t) -> void { + return pimpl->initialize(armor, t); +} + +auto RegularRobotState::predict(Clock::time_point t) -> void { return pimpl->predict(t); } + +auto RegularRobotState::update(std::span armors) -> bool { + return pimpl->update(armors); +} + +auto RegularRobotState::is_converged() const -> bool { return pimpl->is_converged(); } + +auto RegularRobotState::get_snapshot() const -> Snapshot { return pimpl->get_snapshot(); } + +auto RegularRobotState::distance() const -> double { return pimpl->distance(); } diff --git a/src/module/predictor/regular/robot_state.hpp b/src/module/predictor/regular/robot_state.hpp new file mode 100644 index 00000000..c8ee4078 --- /dev/null +++ b/src/module/predictor/regular/robot_state.hpp @@ -0,0 +1,34 @@ +#pragma once + +#include +#include + +#include "module/predictor/regular/ekf_parameter.hpp" +#include "module/predictor/snapshot.hpp" +#include "utility/pimpl.hpp" + +namespace rmcs::predictor { + +class RegularRobotState { +public: + using Clock = util::Clock; + using EKF = EKFParameters::EKF; + + explicit RegularRobotState(Clock::time_point stamp) noexcept; + + auto initialize(Armor3D const& armor, Clock::time_point t) -> void; + auto predict(Clock::time_point t) -> void; + + auto update(std::span armors) -> bool; + + auto is_converged() const -> bool; + auto get_snapshot() const -> Snapshot; + auto distance() const -> double; + + RMCS_PIMPL_DEFINITION(RegularRobotState) +public: + RegularRobotState(RegularRobotState&&) noexcept; + auto operator=(RegularRobotState&&) noexcept -> RegularRobotState&; +}; + +} // namespace rmcs::predictor diff --git a/src/module/predictor/regular/snapshot.cpp b/src/module/predictor/regular/snapshot.cpp new file mode 100644 index 00000000..9a61d916 --- /dev/null +++ b/src/module/predictor/regular/snapshot.cpp @@ -0,0 +1,77 @@ +#include "module/predictor/regular/snapshot.hpp" + +#include + +#include "module/predictor/regular/ekf_parameter.hpp" +#include "module/predictor/backend/snapshot_backend.hpp" +#include "utility/math/conversion.hpp" +#include "utility/time.hpp" + +namespace rmcs::predictor { + +namespace { + + auto make_armor(DeviceId device, CampColor color, int id) -> Armor3D { + auto armor = Armor3D {}; + armor.genre = device; + armor.color = camp_color2armor_color(color); + armor.id = id; + return armor; + } + + struct RegularSnapshotBackend final : ISnapshotBackend { + explicit RegularSnapshotBackend( + Snapshot::NormalEKF::XVec x, DeviceId device, CampColor color, + int armor_num, Snapshot::Clock::time_point stamp) noexcept + : ISnapshotBackend { device, color, armor_num, stamp } + , x { std::move(x) } { } + + [[nodiscard]] auto kinematics_at(Snapshot::Clock::time_point t) const + -> Snapshot::Kinematics override { + return kinematics_of(predict_state_at(t)); + } + + [[nodiscard]] auto predicted_armors(Snapshot::Clock::time_point t) const + -> std::vector override { + auto const predicted_x = predict_state_at(t); + + auto armors = std::vector {}; + armors.reserve(armor_num); + + for (int id = 0; id < armor_num; ++id) { + auto armor = make_armor(device, color, id); + auto const angle = EKFParameters::armor_yaw(device, predicted_x, id); + auto const position = + EKFParameters::h_armor_xyz(device, predicted_x, id, armor_num); + + armor.translation = position; + armor.orientation = util::euler_to_quaternion(angle, kPredictedOtherArmorPitch, 0); + armors.emplace_back(armor); + } + + return armors; + } + + private: + static auto kinematics_of(Snapshot::NormalEKF::XVec const& x) -> Snapshot::Kinematics { + return { Eigen::Vector3d { x[0], x[2], x[4] }, x[7] }; + } + + auto predict_state_at(Snapshot::Clock::time_point t) const -> Snapshot::NormalEKF::XVec { + auto const dt = util::delta_time(t, stamp).count(); + return EKFParameters::f(dt)(x); + } + + Snapshot::NormalEKF::XVec x; + }; + +} // namespace + +auto detail::make_regular_snapshot(Snapshot::NormalEKF::XVec ekf_x, DeviceId device, + CampColor color, int armor_num, Snapshot::Clock::time_point stamp) noexcept -> Snapshot { + return detail::make_snapshot( + std::make_unique( + std::move(ekf_x), device, color, armor_num, stamp)); +} + +} // namespace rmcs::predictor diff --git a/src/module/predictor/regular/snapshot.hpp b/src/module/predictor/regular/snapshot.hpp new file mode 100644 index 00000000..b934e4fb --- /dev/null +++ b/src/module/predictor/regular/snapshot.hpp @@ -0,0 +1,12 @@ +#pragma once + +#include "module/predictor/snapshot.hpp" +#include "utility/robot/color.hpp" +#include "utility/robot/id.hpp" + +namespace rmcs::predictor::detail { + +auto make_regular_snapshot(Snapshot::NormalEKF::XVec ekf_x, DeviceId device, CampColor color, + int armor_num, Snapshot::Clock::time_point stamp) noexcept -> Snapshot; + +} // namespace rmcs::predictor::detail diff --git a/src/module/predictor/robot_state.cpp b/src/module/predictor/robot_state.cpp index ab2ac930..fb0088a5 100644 --- a/src/module/predictor/robot_state.cpp +++ b/src/module/predictor/robot_state.cpp @@ -1,175 +1,60 @@ #include "robot_state.hpp" -#include "module/predictor/ekf_parameter.hpp" -#include "utility/time.hpp" +#include "module/predictor/backend/robot_state_backend.hpp" using namespace rmcs::predictor; -struct RobotState::Impl { - using EKF = util::EKF<11, 4>; +namespace { - explicit Impl() - : device { DeviceId::UNKNOWN } - , color { CampColor::UNKNOWN } - , armor_num { 0 } - , ekf { EKF {} } - , time_stamp { Clock::now() } - , initialized { false } { } +auto empty_snapshot(RobotState::Clock::time_point stamp) -> Snapshot { + return Snapshot::empty(stamp); +} - auto initialize(Armor3D const& armor, Clock::time_point t) -> void { - device = armor.genre; - color = armor_color2camp_color(armor.color); - armor_num = EKFParameters::armor_num(armor.genre); - ekf = EKF { EKFParameters::x(armor), EKFParameters::P_initial_dig(device).asDiagonal() }; - time_stamp = t; +} // namespace - initialized = true; - } +struct RobotState::Impl { + std::unique_ptr backend {}; + Clock::time_point pending_time_stamp { Clock::now() }; - auto get_snapshot() const -> Snapshot { - return { ekf.x, device, color, armor_num, time_stamp }; + [[nodiscard]] static auto make_backend(DeviceId device, Clock::time_point stamp) + -> std::unique_ptr { + auto const kind = classify_robot_state_backend(device); + return make_robot_state_backend(kind, stamp); } - auto distance() const -> double { - auto x = ekf.x[0], y = ekf.x[2]; - return std::sqrt(x * x + y * y); + auto reset_backend(Armor3D const& armor, Clock::time_point stamp) -> void { + backend = make_backend(armor.genre, stamp); + pending_time_stamp = stamp; } - auto predict(Clock::time_point t) -> void { - if (initialized) { - auto dt = util::delta_time(t, time_stamp); - if (dt > reset_interval) { - initialized = false; - update_count = 0; - time_stamp = t; - return; - } - - auto dt_s = dt.count(); - ekf.predict( - EKFParameters::f(dt_s), [dt_s](EKF::XVec const&) { return EKFParameters::F(dt_s); }, - EKFParameters::Q(device, dt_s)); - } - - time_stamp = t; - } - - auto update(Armor3D const& armor) -> void { - if (!initialized) { - initialize(armor, time_stamp); - return; - } - - auto [id, error, valid] = match(armor); - if (!valid) return; - - last_id = id; - update_count++; - - auto const [pos_x, pos_y, pos_z] = armor.translation; - auto const xyz = Eigen::Vector3d { pos_x, pos_y, pos_z }; - - auto const& ypd = util::xyz2ypd(Eigen::Vector3d { pos_x, pos_y, pos_z }); - - auto const [quat_x, quat_y, quat_z, quat_w] = armor.orientation; - auto const& orientation = Eigen::Quaterniond { quat_w, quat_x, quat_y, quat_z }; - - auto const& ypr = util::eulers(orientation); - auto z = EKF::ZVec {}; - z << ypd[0], ypd[1], ypd[2], ypr[0]; - - ekf.update( - z, [id, this](EKF::XVec const& x) { return EKFParameters::h(x, id, armor_num); }, - [id, this](EKF::XVec const& x) { return EKFParameters::H(x, id, armor_num); }, - EKFParameters::R(xyz, ypr, ypd), EKFParameters::x_add, EKFParameters::z_subtract); - - // 前哨站转速特判 - correct(); + auto ensure_backend(Armor3D const& armor) -> void { + if (backend) return; + backend = make_backend(armor.genre, pending_time_stamp); } - constexpr auto is_converged() const -> bool { - auto const r = ekf.x[8]; - auto const l = ekf.x[8] + ekf.x[9]; - - auto const r_ok = (r > 0.05) && (r < 0.5); - auto const l_ok = (l > 0.05) && (l < 0.5); - - int min_updates = (device == DeviceId::OUTPOST) ? 10 : 3; - if (r_ok && l_ok && update_count > min_updates) return true; - - return false; + auto initialize(Armor3D const& armor, Clock::time_point t) -> void { + reset_backend(armor, t); + backend->initialize(armor, t); } - DeviceId device; - CampColor color; - int armor_num; - - EKF ekf; - Clock::time_point time_stamp; - - bool initialized; - int last_id { 0 }; - int update_count { 0 }; - const std::chrono::duration reset_interval { 1.0 }; - - const double angle_error_threshold { 0.5 }; - // 前哨站转速特判 - constexpr auto correct() -> void { - if (device == DeviceId::OUTPOST) { - constexpr auto max_outpost_w = double { 2.51 }; - auto& w = ekf.x[7]; - if (std::abs(w) > 2.0) { - w = w > 0 ? max_outpost_w : (-max_outpost_w); - } - } + auto predict(Clock::time_point t) -> void { + pending_time_stamp = t; + if (backend) backend->predict(t); } - constexpr auto calculate_armors(EKF::XVec const& x) const -> std::vector { - auto armors = std::vector {}; - for (int i = 0; i < armor_num; i++) { - auto angle = x[6]; - angle = util::normalize_angle(angle + i * 2 * std::numbers::pi / armor_num); - - auto xyz = EKFParameters::h_armor_xyz(x, i, armor_num); - auto xyza = Eigen::Vector4d { xyz[0], xyz[1], xyz[2], angle }; - armors.emplace_back(xyza); - } - return armors; + auto update(std::span armors) -> bool { + if (armors.empty()) return false; + ensure_backend(armors.front()); + return backend ? backend->update(armors) : false; } - constexpr auto match(Armor3D const& armor) const -> MatchResult { - if (!initialized) return { -1, 1e10, false }; - - auto armors_xyza = calculate_armors(ekf.x); + auto is_converged() const -> bool { return backend ? backend->is_converged() : false; } - auto const& [pos_x, pos_y, pos_z] = armor.translation; - const auto xyz = Eigen::Vector3d { pos_x, pos_y, pos_z }; - const auto orientation = Eigen::Quaterniond { armor.orientation.w, armor.orientation.x, - armor.orientation.y, armor.orientation.z }; - const auto ypr_in_world = util::eulers(orientation); - const auto ypd_in_world = util::xyz2ypd(xyz); - - auto it = - std::ranges::min_element(armors_xyza, [&](auto const& a_xyza, auto const& b_xyza) { - auto get_error = [&](auto const& pred) { - auto ypd_pred = util::xyz2ypd(pred.template head<3>()); - return std::abs(util::normalize_angle(ypr_in_world[0] - pred[3])) - + std::abs(util::normalize_angle(ypd_in_world[0] - ypd_pred[0])); - }; - - return get_error(a_xyza) < get_error(b_xyza); - }); - - int best_id = static_cast(std::distance(armors_xyza.begin(), it)); - - auto min_error = [&](const auto& pred) { - auto ypd_pred = util::xyz2ypd(pred.template head<3>()); - return std::abs(util::normalize_angle(ypr_in_world[0] - pred[3])) - + std::abs(util::normalize_angle(ypd_in_world[0] - ypd_pred[0])); - }(*it); - - return { best_id, min_error, (min_error < angle_error_threshold) }; + auto get_snapshot() const -> Snapshot { + return backend ? backend->get_snapshot() : empty_snapshot(pending_time_stamp); } + + auto distance() const -> double { return backend ? backend->distance() : 0.0; } }; RobotState::RobotState() noexcept @@ -182,8 +67,7 @@ auto RobotState::initialize(rmcs::Armor3D const& armor, Clock::time_point t) -> auto RobotState::predict(Clock::time_point t) -> void { return pimpl->predict(t); } -auto RobotState::match(Armor3D const& armor) const -> MatchResult { return pimpl->match(armor); } -auto RobotState::update(rmcs::Armor3D const& armor) -> void { return pimpl->update(armor); } +auto RobotState::update(std::span armors) -> bool { return pimpl->update(armors); } auto RobotState::is_converged() const -> bool { return pimpl->is_converged(); } diff --git a/src/module/predictor/robot_state.hpp b/src/module/predictor/robot_state.hpp index 148c23eb..46ac98f3 100644 --- a/src/module/predictor/robot_state.hpp +++ b/src/module/predictor/robot_state.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include "module/predictor/snapshot.hpp" #include "utility/clock.hpp" @@ -12,18 +13,11 @@ struct RobotState { RMCS_PIMPL_DEFINITION(RobotState) public: - struct MatchResult { - int armor_id; - double error; - bool is_valid; - }; - auto initialize(Armor3D const&, Clock::time_point) -> void; auto predict(Clock::time_point t) -> void; - auto match(Armor3D const& armor) const -> MatchResult; - auto update(Armor3D const& armor) -> void; + auto update(std::span armors) -> bool; auto is_converged() const -> bool; diff --git a/src/module/predictor/snapshot.cpp b/src/module/predictor/snapshot.cpp index 69236cbe..56c522ff 100644 --- a/src/module/predictor/snapshot.cpp +++ b/src/module/predictor/snapshot.cpp @@ -1,81 +1,57 @@ #include "snapshot.hpp" -#include "module/predictor/ekf_parameter.hpp" -#include "utility/math/conversion.hpp" -#include "utility/robot/armor.hpp" -#include "utility/time.hpp" - -using namespace rmcs::predictor; -using TimePoint = std::chrono::steady_clock::time_point; - -struct Snapshot::Impl { - EKF::XVec ekf_x_; - DeviceId device; - CampColor color; - int armor_num; - TimePoint stamp; - - Impl(EKF::XVec ekf_x, DeviceId device, CampColor color, int armor_num, TimePoint stamp) noexcept - : ekf_x_ { std::move(ekf_x) } - , device { device } - , color { color } - , armor_num { armor_num } - , stamp { stamp } { } - - auto predict_at(TimePoint t) const -> EKF::XVec { - double dt = util::delta_time(t, stamp).count(); - return EKFParameters::f(dt)(ekf_x_); +#include +#include + +#include "module/predictor/backend/snapshot_backend.hpp" + +namespace rmcs::predictor { + +namespace { + +struct EmptySnapshotBackend final : ISnapshotBackend { + explicit EmptySnapshotBackend(Snapshot::Clock::time_point stamp) noexcept + : ISnapshotBackend { DeviceId::UNKNOWN, CampColor::UNKNOWN, 0, stamp } { } + + [[nodiscard]] auto kinematics_at(Snapshot::Clock::time_point) const + -> Snapshot::Kinematics override { + return { Eigen::Vector3d::Zero(), 0.0 }; } - auto ekf_x() const -> EKF::XVec { return ekf_x_; } - auto time_stamp() const -> TimePoint { return stamp; } - - auto predicted_armors(TimePoint t) const -> std::vector { - auto const& ekf_x = predict_at(t); - auto _angle = ekf_x[6]; - - auto armors = std::vector {}; - armors.reserve(armor_num); - - for (int id = 0; id < armor_num; ++id) { - auto angle = util::normalize_angle(_angle + id * 2 * std::numbers::pi / armor_num); - auto position = EKFParameters::h_armor_xyz(ekf_x, id, armor_num); - - auto armor = Armor3D {}; - armor.genre = device; - armor.color = camp_color2armor_color(color); - armor.id = id; - armor.translation = position; - armor.orientation = util::euler_to_quaternion(angle, 15. / 180 * std::numbers::pi, 0); - armors.emplace_back(armor); - } - return armors; + [[nodiscard]] auto predicted_armors(Snapshot::Clock::time_point) const + -> std::vector override { + return {}; } }; -Snapshot::Snapshot( - EKF::XVec ekf_x, DeviceId device, CampColor color, int armor_num, TimePoint stamp) noexcept - : pimpl { std::make_unique(std::move(ekf_x), device, color, armor_num, stamp) } { } +} // namespace -Snapshot::Snapshot(Snapshot const& other) - : pimpl { std::make_unique(*other.pimpl) } { } +auto detail::make_snapshot(std::unique_ptr backend) noexcept -> Snapshot { + return Snapshot { std::move(backend) }; +} -Snapshot::Snapshot(Snapshot&&) noexcept = default; -Snapshot& Snapshot::operator=(Snapshot const& other) { - if (this != &other) { - pimpl = std::make_unique(*other.pimpl); - } - return *this; +auto Snapshot::empty(Clock::time_point stamp) noexcept -> Snapshot { + return detail::make_snapshot(std::make_unique(stamp)); } -Snapshot& Snapshot::operator=(Snapshot&&) noexcept = default; -Snapshot::~Snapshot() noexcept = default; -auto Snapshot::ekf_x() const -> EKF::XVec { return pimpl->ekf_x(); } +Snapshot::Snapshot(std::unique_ptr backend) noexcept + : backend { std::move(backend) } { } + +Snapshot::Snapshot(Snapshot&&) noexcept = default; +auto Snapshot::operator=(Snapshot&&) noexcept -> Snapshot& = default; + +Snapshot::~Snapshot() noexcept = default; -auto Snapshot::time_stamp() const -> TimePoint { return pimpl->time_stamp(); } +auto Snapshot::time_stamp() const -> Clock::time_point { return backend->time_stamp(); } -auto Snapshot::predict_at(TimePoint t) const -> EKF::XVec { return pimpl->predict_at(t); } +auto Snapshot::kinematics() const -> Kinematics { return backend->kinematics_at(time_stamp()); } -auto Snapshot::predicted_armors(TimePoint t) const -> std::vector { - return pimpl->predicted_armors(t); +auto Snapshot::kinematics_at(Clock::time_point t) const -> Kinematics { + return backend->kinematics_at(t); } + +auto Snapshot::predicted_armors(Clock::time_point t) const -> std::vector { + return backend->predicted_armors(t); +} + +} // namespace rmcs::predictor diff --git a/src/module/predictor/snapshot.hpp b/src/module/predictor/snapshot.hpp index f7efcd2b..c2db3ee0 100644 --- a/src/module/predictor/snapshot.hpp +++ b/src/module/predictor/snapshot.hpp @@ -1,38 +1,56 @@ #pragma once +#include #include +#include #include "utility/clock.hpp" #include "utility/math/kalman_filter/ekf.hpp" #include "utility/robot/armor.hpp" -#include "utility/robot/color.hpp" -#include "utility/robot/id.hpp" namespace rmcs::predictor { +struct ISnapshotBackend; +class Snapshot; + +namespace detail { + +auto make_snapshot(std::unique_ptr backend) noexcept -> Snapshot; + +} // namespace detail + class Snapshot { public: - using EKF = util::EKF<11, 4>; - using Clock = util::Clock; + using NormalEKF = util::EKF<11, 4>; + using OutpostEKF = util::EKF<6, 4>; + using Clock = util::Clock; + + struct Kinematics { + Eigen::Vector3d center_position; + double angular_velocity; + }; - Snapshot(EKF::XVec ekf_x, DeviceId device, CampColor color, int armor_num, - Clock::time_point stamp) noexcept; - Snapshot(Snapshot const&); + static auto empty(Clock::time_point stamp) noexcept -> Snapshot; + + Snapshot(Snapshot const&) = delete; Snapshot(Snapshot&&) noexcept; - Snapshot& operator=(Snapshot const&); + Snapshot& operator=(Snapshot const&) = delete; Snapshot& operator=(Snapshot&&) noexcept; ~Snapshot() noexcept; - auto ekf_x() const -> EKF::XVec; - auto time_stamp() const -> Clock::time_point; + auto kinematics() const -> Kinematics; + auto kinematics_at(Clock::time_point t) const -> Kinematics; - auto predict_at(Clock::time_point t) const -> EKF::XVec; auto predicted_armors(Clock::time_point t) const -> std::vector; private: - struct Impl; - std::unique_ptr pimpl; + explicit Snapshot(std::unique_ptr backend) noexcept; + + std::unique_ptr backend; + + friend auto detail::make_snapshot(std::unique_ptr backend) noexcept + -> Snapshot; }; } // namespace rmcs::predictor diff --git a/src/module/tracker/decider.cpp b/src/module/tracker/decider.cpp index 0af3e55a..0f3148f2 100644 --- a/src/module/tracker/decider.cpp +++ b/src/module/tracker/decider.cpp @@ -1,6 +1,14 @@ #include "decider.hpp" +#include +#include +#include +#include +#include +#include + #include "module/predictor/robot_state.hpp" +#include "utility/serializable.hpp" #include "utility/time.hpp" using namespace rmcs::tracker; @@ -8,59 +16,158 @@ using namespace rmcs::predictor; using namespace std::chrono_literals; struct Decider::Impl { + static constexpr auto kDefaultCleanupInterval = 1s; + static constexpr auto kOutpostCleanupInterval = 1.5s; + static constexpr double kPriorityScoreBase = 10.0; + static constexpr double kDistanceScoreWeight = 5.0; + static constexpr double kDistanceScoreBias = 1.0; + static constexpr double kConvergedScoreBonus = 4.0; + static constexpr double kPrimaryTargetScoreBonus = 2.0; + + struct TargetMemory { + std::optional last_seen_time {}; + std::size_t consecutive_missing_frames { 0 }; + std::size_t consecutive_stable_frames { 0 }; + bool temporary_lost_armed { false }; + }; + + struct Config : util::Serializable { + std::size_t max_temporary_loss_frames { 5 }; + std::size_t max_unconfirmed_loss_frames { 3 }; + std::size_t tracking_confirm_frames { 3 }; + + constexpr static std::tuple metas { + &Config::max_temporary_loss_frames, + "max_temporary_loss_frames", + &Config::max_unconfirmed_loss_frames, + "max_unconfirmed_loss_frames", + &Config::tracking_confirm_frames, + "tracking_confirm_frames", + }; + }; + + auto initialize(const YAML::Node& yaml) noexcept -> std::expected { + auto result = config.serialize(yaml); + if (!result.has_value()) { + return std::unexpected { result.error() }; + } + + if (config.max_temporary_loss_frames == 0) { + return std::unexpected { "tracker.max_temporary_loss_frames must be > 0" }; + } + if (config.max_unconfirmed_loss_frames == 0) { + return std::unexpected { "tracker.max_unconfirmed_loss_frames must be > 0" }; + } + if (config.tracking_confirm_frames == 0) { + return std::unexpected { "tracker.tracking_confirm_frames must be > 0" }; + } + + return {}; + } + auto set_priority_mode(PriorityMode const& mode) -> void { priority_mode = mode; } + static auto cleanup_interval_for(DeviceId device_id) -> std::chrono::duration { + switch (device_id) { + case DeviceId::OUTPOST: + return kOutpostCleanupInterval; + default: + return kDefaultCleanupInterval; + } + } + auto update(std::span armors, Clock::time_point t) -> Output { // 推进所有现有追踪器的时间轴 for (auto& [id, tracker] : trackers) { tracker->predict(t); } - // 将检测到的装甲板按 DeviceId 分发给对应的 RobotState + auto observed_ids = std::unordered_set {}; + auto grouped_armors = std::unordered_map> {}; + for (const auto& armor : armors) { - auto id = armor.genre; + grouped_armors[armor.genre].emplace_back(armor); + } - // 发现新 ID，创建新的追踪器 + for (auto& [id, grouped] : grouped_armors) { + auto& target_memory = target_memories[id]; if (!trackers.contains(id)) { trackers[id] = std::make_unique(); - trackers[id]->initialize(armor, t); + trackers[id]->initialize(grouped.front(), t); + } + + auto grouped_span = std::span { grouped.data(), grouped.size() }; + bool fused = trackers[id]->update(grouped_span); + + if (fused) { + observed_ids.insert(id); + target_memory.last_seen_time = t; + target_memory.consecutive_missing_frames = 0; } + } + + // 状态机： + // 1. unconfirmed: 已有 tracker，但还没稳定到可接管； + // 2. confirmed: 连续稳定若干帧后允许控制接管； + // 3. temporary lost: confirmed 目标短暂丢失时，保留控制输出窗口。 + for (const auto& [id, tracker] : trackers) { + auto& target_memory = target_memories[id]; + auto was_tracking_confirmed = tracking_confirmed(id); - // RobotState 内部会调用 match() 自动处理多装甲板逻辑 - trackers[id]->update(armor); - last_seen_time[id] = t; + if (observed_ids.contains(id) && tracker->is_converged()) { + ++target_memory.consecutive_stable_frames; + target_memory.temporary_lost_armed = false; + continue; + } + + target_memory.consecutive_stable_frames = 0; + if (!observed_ids.contains(id)) { + if (was_tracking_confirmed) { + target_memory.temporary_lost_armed = true; + } + ++target_memory.consecutive_missing_frames; + } else { + target_memory.temporary_lost_armed = false; + } } std::erase_if(trackers, [&](const auto& item) { - bool expired = util::delta_time(t, last_seen_time[item.first]) > cleanup_interval; + auto memory_it = target_memories.find(item.first); + auto cleanup_interval = cleanup_interval_for(item.first); + bool expired = memory_it == target_memories.end() || !memory_it->second.last_seen_time + || util::delta_time(t, *memory_it->second.last_seen_time) > cleanup_interval; if (expired) { if (item.first == primary_target_id) primary_target_id = DeviceId::UNKNOWN; - last_seen_time.erase(item.first); + target_memories.erase(item.first); } return expired; }); - primary_target_id = arbitrate(t); + auto fresh_target_id = arbitrate(observed_ids); + if (fresh_target_id != DeviceId::UNKNOWN) { + primary_target_id = fresh_target_id; - if (primary_target_id != DeviceId::UNKNOWN) { - auto& target_tracker = trackers[primary_target_id]; - return { - .state = target_tracker->is_converged() ? State::Tracking : State::Detecting, - .target_id = primary_target_id, - .snapshot = target_tracker->get_snapshot(), - }; + auto confirmed = tracking_confirmed(primary_target_id); + return make_output(primary_target_id, confirmed, confirmed); } - return { - .state = State::Lost, - .target_id = DeviceId::UNKNOWN, - .snapshot = std::nullopt, + + if (auto output = hold_output(primary_target_id)) { + return std::move(*output); + } + + primary_target_id = DeviceId::UNKNOWN; + return Output { + .target_id = DeviceId::UNKNOWN, + .snapshot = std::nullopt, + .allow_takeover = false, + .tracking_confirmed = false, }; } - auto arbitrate(Clock::time_point now) -> DeviceId { + auto arbitrate(const std::unordered_set& observed_ids) -> DeviceId { auto candidates = trackers | std::views::filter([&](const auto& pair) { - return util::delta_time(now, last_seen_time[pair.first]) < active_interval; + return observed_ids.contains(pair.first); }); if (std::ranges::empty(candidates)) return DeviceId::UNKNOWN; @@ -71,6 +178,57 @@ struct Decider::Impl { return it->first; } + auto tracking_confirmed(DeviceId device_id) const -> bool { + auto memory_it = target_memories.find(device_id); + if (memory_it == target_memories.end()) { + return false; + } + + return memory_it->second.consecutive_stable_frames >= config.tracking_confirm_frames; + } + + auto make_output(DeviceId device_id, bool allow_takeover, bool confirmed) const -> Output { + return Output { + .target_id = device_id, + .snapshot = trackers.at(device_id)->get_snapshot(), + .allow_takeover = allow_takeover, + .tracking_confirmed = confirmed, + }; + } + + auto hold_output(DeviceId device_id) const -> std::optional { + if (device_id == DeviceId::UNKNOWN || !trackers.contains(device_id)) { + return std::nullopt; + } + + const auto& target_tracker = *trackers.at(device_id); + const auto& target_memory = target_memories.at(device_id); + + if (target_tracker.is_converged()) { + if (target_memory.temporary_lost_armed + && is_within_loss_window(device_id, config.max_temporary_loss_frames)) { + return make_output(device_id, true, false); + } + return std::nullopt; + } + + if (is_within_loss_window(device_id, config.max_unconfirmed_loss_frames)) { + return make_output(device_id, false, false); + } + + return std::nullopt; + } + + auto is_within_loss_window(DeviceId device_id, std::size_t max_missing_frames) const -> bool { + auto memory_it = target_memories.find(device_id); + if (device_id == DeviceId::UNKNOWN || !trackers.contains(device_id) + || memory_it == target_memories.end() || !memory_it->second.last_seen_time) { + return false; + } + + return memory_it->second.consecutive_missing_frames <= max_missing_frames; + } + // TODO:需要进一步确定 // 评分函数：结合优先级模式、距离、收敛情况 auto calculate_score(DeviceId device, RobotState const& tracker) const -> double { @@ -79,28 +237,29 @@ struct Decider::Impl { // 基础优先级评分 if (priority_mode.contains(device)) { // RobotPriority 枚举值越小，优先级越高 - score += (10.0 - static_cast(priority_mode.at(device))); + score += (kPriorityScoreBase - static_cast(priority_mode.at(device))); } // 距离加权：优先锁定近处的目标 (简单的 1/dist) double dist = tracker.distance(); - score += 5.0 / (dist + 1.0); + score += kDistanceScoreWeight / (dist + kDistanceScoreBias); + + // 优先延续已经收敛的目标，避免频繁切到未收敛目标导致停留 Detecting。 + if (tracker.is_converged()) score += kConvergedScoreBonus; // 粘滞性：如果已经是主目标，额外加分防止“摇头” - if (device == primary_target_id) score += 2.0; + if (device == primary_target_id) score += kPrimaryTargetScoreBonus; return score; } DeviceId primary_target_id { DeviceId::UNKNOWN }; std::unordered_map> trackers; - std::unordered_map last_seen_time; + std::unordered_map target_memories; + Config config {}; PriorityMode priority_mode; - std::chrono::duration cleanup_interval { 500ms }; - std::chrono::duration active_interval { 100ms }; - const PriorityMode mode1 = { { DeviceId::HERO, 2 }, { DeviceId::ENGINEER, 4 }, @@ -130,6 +289,10 @@ Decider::Decider() noexcept : pimpl { std::make_unique() } { } Decider::~Decider() noexcept = default; +auto Decider::initialize(const YAML::Node& yaml) noexcept -> std::expected { + return pimpl->initialize(yaml); +} + auto Decider::set_priority_mode(PriorityMode const& mode) -> void { return pimpl->set_priority_mode(mode); } diff --git a/src/module/tracker/decider.hpp b/src/module/tracker/decider.hpp index 64e17102..3b214a1a 100644 --- a/src/module/tracker/decider.hpp +++ b/src/module/tracker/decider.hpp @@ -1,7 +1,12 @@ #pragma once +#include +#include +#include + +#include + #include "module/predictor/snapshot.hpp" -#include "state.hpp" #include "utility/clock.hpp" #include "utility/pimpl.hpp" #include "utility/robot/id.hpp" @@ -16,11 +21,14 @@ struct Decider { public: struct Output { - State state; DeviceId target_id; std::optional snapshot; + bool allow_takeover { false }; + bool tracking_confirmed { false }; }; + auto initialize(const YAML::Node& yaml) noexcept -> std::expected; + auto set_priority_mode(PriorityMode const& mode) -> void; auto update(std::span armors, Clock::time_point t) -> Output; diff --git a/src/module/tracker/state.hpp b/src/module/tracker/state.hpp deleted file mode 100644 index af7c5c32..00000000 --- a/src/module/tracker/state.hpp +++ /dev/null @@ -1,29 +0,0 @@ -#pragma once - -#include - -namespace rmcs::tracker { -enum class State { - Lost, - Detecting, - Tracking, - TemporaryLost, - Switching, -}; - -constexpr auto to_string(State state) -> std::string { - switch (state) { - case State::Lost: - return "Lost"; - case State::Detecting: - return "Detecting"; - case State::Tracking: - return "Tracking"; - case State::TemporaryLost: - return "TemporaryLost"; - case State::Switching: - return "Switching"; - } - return "Unknown"; -}; -} \ No newline at end of file diff --git a/src/runtime.cpp b/src/runtime.cpp index b7cd0ca7..d2d2f2d2 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -7,8 +7,6 @@ #include "kernel/visualization.hpp" #include "module/debug/action_throttler.hpp" -#include "module/debug/framerate.hpp" - #include "utility/image/armor.hpp" #include "utility/panic.hpp" #include "utility/rclcpp/configuration.hpp" @@ -16,15 +14,15 @@ #include "utility/rclcpp/parameters.hpp" #include "utility/singleton/running.hpp" -#include #include #include +#include +#include #include using namespace rmcs; using namespace rmcs::util; using namespace rmcs::kernel; -using TrackerState = rmcs::tracker::State; auto main() -> int { using namespace std::chrono_literals; @@ -34,9 +32,6 @@ auto main() -> int { auto rclcpp_node = util::RclcppNode { "AutoAim" }; rclcpp_node.set_pub_topic_prefix("/rmcs/auto_aim/"); - auto framerate = FramerateCounter {}; - framerate.set_interval(5s); - { /// Runtime auto feishu = kernel::Feishu {}; @@ -106,18 +101,12 @@ auto main() -> int { } // DEBUG constexpr auto control_state_label { "control_state_not_updated" }; - constexpr auto tracker_tracking_label { "tracker_tracking" }; - constexpr auto armor_detected_label { "armor_not_detected" }; - constexpr auto visualization_pnp_label { "visualization_pnp_failed" }; - constexpr auto fire_control_label { "fire_control_failed" }; + constexpr auto identifier_failed_label { "identifier_failed" }; constexpr auto feishu_commit_label { "feishu_commit_failed" }; { action_throttler.register_action(control_state_label, 1); - action_throttler.register_action(tracker_tracking_label, 1); - action_throttler.register_action(armor_detected_label); - action_throttler.register_action(visualization_pnp_label, 3); - action_throttler.register_action(fire_control_label); - action_throttler.register_action(feishu_commit_label); + action_throttler.register_action(identifier_failed_label, 1); + action_throttler.register_action(feishu_commit_label, 1); } /// @@ -125,10 +114,8 @@ auto main() -> int { /// const auto fetch_control_state = [&] -> ControlState { if (is_local_runtime) { - action_throttler.dispatch(control_state_label, - [&] { rclcpp_node.info("在本机环境下运行，将Control State 设置为默认值"); }); auto state = ControlState {}; - state.set_identity(); + state.reset(); return state; } if (!feishu.updated()) { @@ -140,6 +127,19 @@ auto main() -> int { return feishu.fetch(); }; + const auto commit_state = [&](const AutoAimState& state) { + if (!feishu.commit(state)) { + action_throttler.dispatch(feishu_commit_label, [&] { + rclcpp_node.warn( + "Commit auto_aim_state failed (target={})", rmcs::to_string(state.target)); + }); + return false; + } + + action_throttler.reset(feishu_commit_label); + return true; + }; + for (;;) { if (!util::get_running()) [[unlikely]] break; @@ -155,6 +155,8 @@ auto main() -> int { std::ignore = stream_guard; auto control_state = fetch_control_state(); + auto next_state = AutoAimState {}; + next_state.reset(); /// 1. Identify Armor /// @@ -162,100 +164,67 @@ auto main() -> int { { auto result = identifier.sync_identify(*image); if (!result.has_value()) { - action_throttler.dispatch( - armor_detected_label, [&] { rclcpp_node.warn("未识别到装甲板"); }); - continue; - } - action_throttler.reset(armor_detected_label); + action_throttler.dispatch(identifier_failed_label, + [&] { rclcpp_node.error("Armor detection failed"); }); + } else { + action_throttler.reset(identifier_failed_label); - tracker.set_invincible_armors(control_state.invincible_devices); - auto filtered = tracker.filter_armors(*result); - // No available armors to shoot - if (filtered.empty()) continue; + tracker.set_invincible_armors(control_state.invincible_devices); + auto filtered = tracker.filter_armors(*result); + if (use_painted_image) { + for (const auto& armor_2d : filtered) + util::draw(*image, armor_2d); + } - if (use_painted_image) { - for (const auto& armor_2d : filtered) - util::draw(*image, armor_2d); + armors_2d = std::move(filtered); } - - armors_2d = std::move(filtered); } /// 2. Transform 2d to 3d /// - auto armors_3d = pose_estimator.solve_pnp(armors_2d); - if (armors_3d && visualization.initialized()) { - auto success = visualization.solved_pnp_armors(*armors_3d); - if (!success) { - action_throttler.dispatch(visualization_pnp_label, - [&] { rclcpp_node.error("可视化PNP结算后的装甲板失败"); }); - } else { - action_throttler.reset(visualization_pnp_label); + auto armors_3d = Armor3Ds {}; + if (!armors_2d.empty()) { + auto solved_armors_3d = pose_estimator.solve_pnp(armors_2d); + if (solved_armors_3d && visualization.initialized()) { + std::ignore = visualization.solved_pnp_armors(*solved_armors_3d); } - } - if (armors_3d) { - auto transform = control_state.odom_to_camera_transform; - pose_estimator.set_odom_to_camera_transform(transform); - armors_3d = pose_estimator.odom_to_camera(*armors_3d); - } else { - continue; + if (solved_armors_3d) { + pose_estimator.set_odom_to_camera_transform( + control_state.odom_to_camera_transform); + armors_3d = pose_estimator.odom_to_camera(*solved_armors_3d); + } } /// 3. Apply Tracker /// - auto snapshot = std::optional { std::nullopt }; { - auto result = tracker.decide(*armors_3d, Clock::now()); + auto tracker_output = tracker.decide(armors_3d, image->get_timestamp()); + auto tracked_target = tracker_output.target_id; + auto snapshot = std::move(tracker_output.snapshot); - if (result.state == TrackerState::Tracking) { - action_throttler.dispatch(tracker_tracking_label, - [&] { rclcpp_node.info("已进入 Tracking 状态"); }); - } else { - action_throttler.reset(tracker_tracking_label); - continue; + if (tracker_output.allow_takeover) { + next_state.set_hold_state( + control_state.yaw, control_state.pitch, tracked_target); } - snapshot = result.snapshot; - if (!snapshot) continue; - } - - /// 4. Fire Control - /// - auto control_cmd = std::optional { std::nullopt }; - { - fire_control.set_bullet_speed(control_state.bullet_speed); - auto translation = control_state.odom_to_muzzle_translation; + if (tracker_output.allow_takeover && snapshot) { + if (auto control_cmd = fire_control.solve( + *snapshot, tracker_output.tracking_confirmed, control_state.yaw)) { + next_state.set_tracking_state(control_cmd->yaw, control_cmd->pitch, + tracked_target, control_cmd->shoot_permitted); + } + } - control_cmd = fire_control.solve(*snapshot, translation); - if (!control_cmd) { - action_throttler.dispatch(fire_control_label, - [&] { rclcpp_node.warn("Fire control solve failed"); }); - continue; - } else { - action_throttler.reset(fire_control_label); + if (visualization.initialized() && snapshot) { + visualization.predicted_armors(snapshot->predicted_armors(Clock::now())); } } - /// 5. Transmit State + /// 4. Transmit State /// - auto state = AutoAimState {}; - state.timestamp = Clock::now(); - state.gimbal_takeover = true; - state.shoot_permitted = true; - state.yaw = control_cmd->yaw; - state.pitch = control_cmd->pitch; - if (!feishu.commit(state)) { - action_throttler.dispatch(feishu_commit_label, - [&] { rclcpp_node.warn("Commit auto_aim_state failed"); }); - } - - if (visualization.initialized()) { - visualization.predicted_armors(snapshot->predicted_armors(Clock::now())); - } - - } // image receive scope - + commit_state(next_state); + } } // runtime loop scope } // runtime objects scope diff --git a/src/utility/math/kalman_filter/ekf.hpp b/src/utility/math/kalman_filter/ekf.hpp index a75a0287..037b0864 100644 --- a/src/utility/math/kalman_filter/ekf.hpp +++ b/src/utility/math/kalman_filter/ekf.hpp @@ -1,6 +1,6 @@ #pragma once -#include #include +#include namespace rmcs::util { @@ -49,6 +49,8 @@ class EKF { : x(initial_x) , P_(initial_P) { } + auto P() const -> PMat const& { return P_; } + /** * @brief 预测步 * @param f 状态转移 Lambda: (XVec) -> XVec diff --git a/src/utility/math/mahalanobis.hpp b/src/utility/math/mahalanobis.hpp new file mode 100644 index 00000000..4c88510a --- /dev/null +++ b/src/utility/math/mahalanobis.hpp @@ -0,0 +1,19 @@ +#pragma once + +#include +#include + +#include + +namespace rmcs::util { + +template +inline auto mahalanobis_distance(TVec const& innovation, TMat const& covariance) + -> std::optional { + auto solved = covariance.ldlt().solve(innovation); + auto distance = innovation.dot(solved); + if (!std::isfinite(distance)) return std::nullopt; + return distance; +} + +} diff --git a/src/utility/model/armor_detection.hpp b/src/utility/model/armor_detection.hpp index 162603ec..3926954e 100644 --- a/src/utility/model/armor_detection.hpp +++ b/src/utility/model/armor_detection.hpp @@ -1,5 +1,11 @@ #pragma once #include "utility/robot/armor.hpp" + +#include +#include +#include +#include + #include namespace rmcs { @@ -51,6 +57,7 @@ struct InferResultAdapter { // Util auto unsafe_from(std::span raw) noexcept -> void { static_assert(std::is_trivially_copyable_v); + static_assert(std::is_standard_layout_v); if (raw.size() < length()) return; std::memcpy(&data, raw.data(), sizeof(data_type)); } diff --git a/src/utility/robot/constant.hpp b/src/utility/robot/constant.hpp index c5920213..e331d5b3 100644 --- a/src/utility/robot/constant.hpp +++ b/src/utility/robot/constant.hpp @@ -1,9 +1,16 @@ #pragma once -namespace rmcs { +#include +namespace rmcs { constexpr double kBaseRadius = 0.3205; -constexpr double kOutpostRadius = 0.2765; +constexpr double kOutpostRadius = 0.275; constexpr double kOtherRadius = 0.2; +constexpr double kPredictedOutpostArmorPitch = -15. / 180. * std::numbers::pi; +constexpr double kPredictedOtherArmorPitch = 15. / 180. * std::numbers::pi; + +constexpr double kOutpostArmorHeightStep = 0.102; +constexpr double kOutpostAngularSpeed = 0.8 * std::numbers::pi; + } diff --git a/src/utility/shared/context.hpp b/src/utility/shared/context.hpp index aa4f170c..a0654089 100644 --- a/src/utility/shared/context.hpp +++ b/src/utility/shared/context.hpp @@ -3,6 +3,8 @@ #include "utility/clock.hpp" #include "utility/math/linear.hpp" #include "utility/robot/id.hpp" +#include +#include namespace rmcs::util { @@ -25,18 +27,45 @@ struct AutoAimState { bool gimbal_takeover { false }; bool shoot_permitted = { false }; - double yaw { 0. }; - double pitch { 0. }; + double yaw { std::numeric_limits::quiet_NaN() }; + double pitch { std::numeric_limits::quiet_NaN() }; DeviceId target { DeviceId::UNKNOWN }; - auto set_identity() noexcept -> void { + auto reset() noexcept -> void { timestamp = Clock::now(); gimbal_takeover = false; shoot_permitted = false; - yaw = 0.; - pitch = 0.; + yaw = std::numeric_limits::quiet_NaN(); + pitch = std::numeric_limits::quiet_NaN(); + target = DeviceId::UNKNOWN; + } + + auto set_hold_state(double current_yaw, double current_pitch, DeviceId current_target) noexcept + -> void { + timestamp = Clock::now(); + + gimbal_takeover = true; + shoot_permitted = false; + yaw = current_yaw; + pitch = current_pitch; + target = current_target; + } + + auto set_tracking_state(double target_yaw, double target_pitch, DeviceId tracked_target, + bool allow_shoot) noexcept -> void { + timestamp = Clock::now(); + + gimbal_takeover = true; + shoot_permitted = allow_shoot; + yaw = target_yaw; + pitch = target_pitch; + target = tracked_target; + } + + [[nodiscard]] auto has_control_direction() const noexcept -> bool { + return gimbal_takeover && std::isfinite(yaw) && std::isfinite(pitch); } }; static_assert(std::is_trivially_copyable_v); @@ -45,24 +74,20 @@ struct ControlState { Clock::time_point timestamp {}; ShootMode shoot_mode { ShootMode::BATTLE }; - double bullet_speed { 0. }; - double yaw { 0. }; - double pitch { 0. }; + double yaw { std::numeric_limits::quiet_NaN() }; + double pitch { std::numeric_limits::quiet_NaN() }; DeviceIds invincible_devices { DeviceIds::None() }; Transform odom_to_camera_transform {}; - Translation odom_to_muzzle_translation {}; - - auto set_identity() noexcept -> void { - timestamp = Clock::now(); - shoot_mode = ShootMode::STOPPING; - bullet_speed = 0.0; - yaw = 0.0; - pitch = 0.0; - invincible_devices = DeviceIds::None(); - odom_to_camera_transform = {}; - odom_to_muzzle_translation = {}; + + auto reset() noexcept -> void { + timestamp = Clock::now(); + shoot_mode = ShootMode::STOPPING; + yaw = std::numeric_limits::quiet_NaN(); + pitch = std::numeric_limits::quiet_NaN(); + invincible_devices = DeviceIds::None(); + odom_to_camera_transform = {}; } }; static_assert(std::is_trivially_copyable_v); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 98861ad2..f2c3fdb0 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -126,3 +126,4 @@ ament_add_gtest( test_action_throttler ${TEST_DIR}/action_throttler.cpp ) + diff --git a/test/action_throttler.cpp b/test/action_throttler.cpp index 795f4ad5..25d2659c 100644 --- a/test/action_throttler.cpp +++ b/test/action_throttler.cpp @@ -27,6 +27,21 @@ TEST(ActionThrottler, DispatchByIntervalAndQuota) { EXPECT_EQ(count, 2); } +TEST(ActionThrottler, DifferentTagsDoNotShareInterval) { + ActionThrottler throttler { 10ms, 1 }; + throttler.register_action("foo"); + throttler.register_action("bar"); + + int foo_count = 0; + int bar_count = 0; + + EXPECT_TRUE(throttler.dispatch("foo", [&] { ++foo_count; })); + EXPECT_TRUE(throttler.dispatch("bar", [&] { ++bar_count; })); + + EXPECT_EQ(foo_count, 1); + EXPECT_EQ(bar_count, 1); +} + TEST(ActionThrottler, ResetRestoreQuota) { ActionThrottler throttler { 1ms, 1 }; throttler.register_action("bar"); diff --git a/test/feishu_test.cpp b/test/feishu_test.cpp index 4a83622b..31d5d9ce 100644 --- a/test/feishu_test.cpp +++ b/test/feishu_test.cpp @@ -45,9 +45,8 @@ TEST(FeishuIntegration, BidirectionalCommunication) { auto feishu_parent = Feishu {}; std::this_thread::sleep_for(50ms); // ensure shm is ready - auto ctrl = ControlState {}; - ctrl.bullet_speed = 42.0; - ctrl.timestamp = Clock::now(); + auto ctrl = ControlState {}; + ctrl.timestamp = Clock::now(); ASSERT_TRUE(feishu_parent.commit(ctrl)); diff --git a/test/model_infer.cpp b/test/model_infer.cpp index bc7c4250..8b665868 100644 --- a/test/model_infer.cpp +++ b/test/model_infer.cpp @@ -9,8 +9,10 @@ #include #include #include +#include #include #include +#include #include #include @@ -40,6 +42,22 @@ constexpr auto config = R"( // - 运行前需执行: cd test && ./download_assets.sh AssetsManager assets_manager; +template +auto make_detector(YAML::Node yaml = YAML::Load(config)) + -> std::unique_ptr { + const auto model_name = std::string { model_type::kLocation }; + const auto model_location = location / "../models" / model_name; + yaml["model_location"] = model_location.string(); + + auto detector = std::make_unique(); + auto configure_result = detector->initialize(yaml); + if (!configure_result.has_value()) { + throw std::runtime_error( + std::string { error_head } + model_name + " | " + configure_result.error()); + } + return detector; +} + struct ExpectedCorners { float lt_x; float lt_y; @@ -61,20 +79,16 @@ auto assert_sync_infer_with_expected(const Image& image, const std::array& expected, bool use_roi_segment = false) -> void { const auto model_name = std::string { model_type::kLocation }; auto yaml = YAML::Load(config); - const auto model_location = location / "../models" / model_name; - yaml["model_location"] = model_location.string(); yaml["use_roi_segment"] = use_roi_segment; - auto detector = identifier::ArmorDetection {}; - auto configure_result = detector.initialize(yaml); - ASSERT_TRUE(configure_result.has_value()) - << error_head << model_name << " | " << configure_result.error(); + auto detector = make_detector(yaml); auto infer_begin = std::chrono::steady_clock::now(); - auto detect_result = detector.sync_detect(image); + auto detect_result = detector->sync_detect(image); auto infer_elapsed = std::chrono::duration(std::chrono::steady_clock::now() - infer_begin); - ASSERT_TRUE(detect_result.has_value()) << error_head << model_name << " | detect failed"; + ASSERT_TRUE(detect_result.has_value()) + << error_head << model_name << " | detect failed"; const auto& armors = detect_result.value(); @@ -157,3 +171,40 @@ TEST(model, sync_infer_with_roi_segment) { assert_sync_infer_with_expected(image, expected, true); assert_sync_infer_with_expected(image, expected, true); } + +TEST(model, sync_infer_rejects_empty_image) { + auto detector = make_detector(); + auto empty_image = Image {}; + auto detect_result = detector->sync_detect(empty_image); + + ASSERT_FALSE(detect_result.has_value()); +} + +TEST(model, sync_infer_rejects_invalid_roi) { + const auto image_location = assets_manager.path("model_infer_example.jpg"); + auto image { Image {} }; + image.details().mat = cv::imread(image_location); + ASSERT_FALSE(image.details().mat.empty()) + << error_head << std::format("Failed to read image from '{}'", image_location.string()); + + auto yaml = YAML::Load(config); + yaml["use_roi_segment"] = true; + yaml["roi_cols"] = image.details().mat.cols + 1; + yaml["roi_rows"] = image.details().mat.rows; + + auto detector = make_detector(yaml); + auto detect_result = detector->sync_detect(image); + + ASSERT_FALSE(detect_result.has_value()); +} + +TEST(model, initialize_reports_unknown_model) { + auto yaml = YAML::Load(config); + yaml["model_location"] = "unknown-model.bin"; + + auto detector = identifier::ArmorDetection {}; + auto configure_result = detector.initialize(yaml); + + ASSERT_FALSE(configure_result.has_value()); + EXPECT_NE(configure_result.error().find("Unsupported model type"), std::string::npos); +}