diff --git a/config/config.yaml b/config/config.yaml index 5c437f33..3f8a45e0 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -25,7 +25,7 @@ capturer: record_fps: 24 max_videos_size_gb: 50 - # source: "local_video" + # source: "hikcamera" or "local_video" source: "hikcamera" hikcamera: @@ -47,7 +47,7 @@ capturer: # 替换为你具体的路径 location: "/path/to/your/video" # double 帧率 - frame_rate: 40 + frame_rate: 80 # bool 是否循环播放 loop_play: true # bool 是否允许跳帧以满足实时性 @@ -90,17 +90,18 @@ pose_estimator: outpost_armor_thickness: 0.022 fire_control: - initial_bullet_speed: 21.4 # m/s - shoot_delay: 0.07 # s + initial_bullet_speed: 23.5 # m/s + shoot_delay: 0.04 # s mpc_enable: true - yaw_offset: 0.0 # degree - pitch_offset: 0.0 # degree + yaw_offset: -0.4 # degree + pitch_offset: +0.8 # degree - aim_point_chooser: - coming_angle: 70.0 # degree + armor_target_selector: + coming_angle: 60.0 # degree leaving_angle: 20.0 # degree - outpost_coming_angle: 70.0 # degree - outpost_leaving_angle: 20.0 # degree + outpost_coming_angle: 60.0 # degree + outpost_leaving_angle: 30.0 # degree + switch_threshold: 8.0 # degree mpc: mpc_max_yaw_acc: 50.0 @@ -111,11 +112,14 @@ fire_control: mpc_pitch_q_angle: 9000000.0 mpc_pitch_q_rate: 0.0 mpc_pitch_r_acc: 1.0 + mpc_max_iter: 5 shoot_evaluator: is_lazy_gimbal: false - near_angle_tolerance: 3 # degree - far_angle_tolerance: 2 # degree + near_yaw_tolerance: 2 # degree + far_yaw_tolerance: 1 # degree + near_pitch_tolerance: 2 # degree + far_pitch_tolerance: 1 # degree split_distance: 2 # m visualization: diff --git a/src/component.cpp b/src/component.cpp index 838fa19c..64d8d7e0 100644 --- a/src/component.cpp +++ b/src/component.cpp @@ -10,7 +10,6 @@ namespace rmcs { class AutoAimComponent final : public rmcs_executor::Component { static inline const auto kNaN = std::numeric_limits::quiet_NaN(); static inline const auto kTNaN = Eigen::Vector3d { kNaN, kNaN, kNaN }; - static inline const auto kQNaN = Eigen::Quaterniond { kNaN, kNaN, kNaN, kNaN }; private: AutoAim auto_aim { }; @@ -27,7 +26,6 @@ class AutoAimComponent final : public rmcs_executor::Component { OutputInterface pitch_rate; OutputInterface yaw_acc; OutputInterface pitch_acc; - OutputInterface feedforward_valid; std::chrono::steady_clock::time_point last_command_timestamp; @@ -45,7 +43,6 @@ class AutoAimComponent final : public rmcs_executor::Component { register_output("/auto_aim/pitch_rate", pitch_rate, kNaN); register_output("/auto_aim/yaw_acc", yaw_acc, kNaN); register_output("/auto_aim/pitch_acc", pitch_acc, kNaN); - register_output("/auto_aim/feedforward_valid", feedforward_valid, false); } auto before_updating() -> void override { @@ -66,12 +63,10 @@ class AutoAimComponent final : public rmcs_executor::Component { ctx.timestamp = Clock::now(); const auto& dir = *barrel_direction; - - ctx.yaw = std::atan2(dir.y(), dir.x()); - ctx.pitch = std::atan2(-dir.z(), std::hypot(dir.x(), dir.y())); + ctx.yaw = std::atan2(dir.y(), dir.x()); + ctx.pitch = std::atan2(-dir.z(), std::hypot(dir.x(), dir.y())); const auto& iso = *camera_transform; - ctx.camera_transform.translation = Translation { iso.translation() }; ctx.camera_transform.orientation = Orientation { Eigen::Quaterniond(iso.rotation()) }; @@ -84,14 +79,13 @@ class AutoAimComponent final : public rmcs_executor::Component { using namespace std::chrono_literals; if (Clock::now() - cmd.timestamp > 100ms) return; - *should_control = cmd.should_control; - *should_shoot = cmd.should_shoot; - *yaw_rate = cmd.yaw_rate; - *pitch_rate = cmd.pitch_rate; - *yaw_acc = cmd.yaw_acc; - *pitch_acc = cmd.pitch_acc; - *feedforward_valid = cmd.feedforward_valid; - *robot_center = { + *should_control = cmd.should_control; + *should_shoot = cmd.should_shoot; + *yaw_rate = cmd.yaw_rate; + *pitch_rate = cmd.pitch_rate; + *yaw_acc = cmd.yaw_acc; + *pitch_acc = cmd.pitch_acc; + *robot_center = { cmd.robot_center.x, cmd.robot_center.y, cmd.robot_center.z, @@ -104,7 +98,7 @@ class AutoAimComponent final : public rmcs_executor::Component { *target_direction = Eigen::Vector3d { std::cos(pitch) * std::cos(yaw), std::cos(pitch) * std::sin(yaw), - std::sin(pitch), + -std::sin(pitch), }; }); last_command_timestamp = now; diff --git a/src/kernel/auto_aim.cpp b/src/kernel/auto_aim.cpp index fee5d755..93ace4ee 100644 --- a/src/kernel/auto_aim.cpp +++ b/src/kernel/auto_aim.cpp @@ -129,23 +129,29 @@ struct AutoAim::Impl { auto armors = snapshot->predicted_armors(Clock::now()); visual.publish(armors, "visible_robot"); - const auto yaw = context.yaw; - if (auto result = fire.solve(*snapshot, yaw)) { - cmd.should_control = true; - cmd.target = target.target_id; - cmd.should_shoot = result->shoot_permitted; - cmd.yaw = result->yaw; - cmd.pitch = result->pitch; - cmd.yaw_rate = result->yaw_rate; - cmd.pitch_rate = result->pitch_rate; - cmd.yaw_acc = result->yaw_acc; - cmd.pitch_acc = result->pitch_acc; - cmd.feedforward_valid = result->feedforward_valid; - cmd.robot_center = result->center_position; - - /// TODO: - /// 统一控制侧和自瞄侧的 Pitch 符号方向定义 - visual.update_aiming_direction(cmd.yaw, -cmd.pitch); + const auto gimbal_state = fire_control::GimbalState { + .timestamp = context.timestamp, + .yaw = context.yaw, + .pitch = context.pitch, + }; + if (auto result = fire.solve(*snapshot, gimbal_state)) { + cmd.should_control = true; + cmd.target = target.target_id; + cmd.should_shoot = result->shoot_permitted; + cmd.yaw = result->yaw; + cmd.pitch = result->pitch; + cmd.robot_center = result->center_position; + + if (result->feedforward.has_value()) { + cmd.yaw_rate = result->feedforward->yaw_rate; + cmd.pitch_rate = result->feedforward->pitch_rate; + cmd.yaw_acc = result->feedforward->yaw_acc; + cmd.pitch_acc = result->feedforward->pitch_acc; + } + + auto impact_armors = snapshot->predicted_armors(result->impact_time); + visual.publish(impact_armors, "impact_robot"); + visual.update_aiming_direction(cmd.yaw, cmd.pitch); visual.update_mpc_plan(cmd.yaw, cmd.pitch, cmd.yaw_rate, cmd.pitch_rate, cmd.yaw_acc, cmd.pitch_acc); } diff --git a/src/kernel/fire_control.cpp b/src/kernel/fire_control.cpp index 69e57685..d12e8bca 100644 --- a/src/kernel/fire_control.cpp +++ b/src/kernel/fire_control.cpp @@ -1,17 +1,15 @@ #include "fire_control.hpp" +#include #include #include -#include #include #include -#include "module/fire_control/aim_point_chooser.hpp" -#include "module/fire_control/planner/mpc_trajectory_planner.hpp" -#include "module/fire_control/planner/reference_trajectory_builder.hpp" +#include "module/fire_control/armor_selector.hpp" +#include "module/fire_control/planner/trajectory_planner.hpp" #include "module/fire_control/shoot_evaluator.hpp" -#include "module/fire_control/solver/aim_point_sampling.hpp" -#include "module/fire_control/solver/target_solution_solver.hpp" +#include "module/fire_control/target_solver.hpp" #include "module/predictor/snapshot.hpp" #include "utility/math/angle.hpp" #include "utility/serializable.hpp" @@ -52,10 +50,10 @@ struct FireControl::Impl { config.yaw_offset = util::deg2rad(config.yaw_offset); config.pitch_offset = util::deg2rad(config.pitch_offset); - auto chooser_result = aim_point_chooser.configure_yaml(yaml["aim_point_chooser"]); - if (!chooser_result.has_value()) { + auto selector_result = armor_selector.configure_yaml(yaml["armor_target_selector"]); + if (!selector_result.has_value()) { return std::unexpected { - std::format("aim_point_chooser init failed: {}", chooser_result.error()), + std::format("armor_target_selector init failed: {}", selector_result.error()), }; } @@ -66,90 +64,113 @@ struct FireControl::Impl { }; } - auto planner_result = mpc_trajectory_planner.configure_yaml(yaml["mpc"]); + auto planner_result = trajectory_planner.configure_yaml(yaml["mpc"]); if (!planner_result.has_value()) { return std::unexpected { - std::format("mpc_trajectory_planner init failed: {}", planner_result.error()), + std::format("trajectory_planner init failed: {}", planner_result.error()), }; } return { }; } - auto solve(predictor::Snapshot const& snapshot, double current_yaw) -> std::optional { - auto target_solution = target_solution_solver.solve( - snapshot, aim_point_chooser, config.initial_bullet_speed, config.shoot_delay); - if (!target_solution.has_value()) return std::nullopt; - - auto center_position = target_solution->center_position; - auto aim_point_position = target_solution->aim_point; - auto pitch = target_solution->impact_pitch; - auto yaw = target_solution->impact_yaw; - - auto pitch_rate = std::numeric_limits::quiet_NaN(); - auto yaw_rate = std::numeric_limits::quiet_NaN(); - auto pitch_acc = std::numeric_limits::quiet_NaN(); - auto yaw_acc = std::numeric_limits::quiet_NaN(); - auto feedforward_valid = false; - - if (config.mpc_enable && snapshot.device_id() != DeviceId::OUTPOST) { - auto sample_attitude = [&](TimePoint t) -> std::expected { - auto sample = AimPointSampler::sample_at( - snapshot, aim_point_chooser, t, config.initial_bullet_speed); - if (!sample.has_value()) return std::unexpected { sample.error() }; - return sample->attitude; - }; + auto solve(predictor::Snapshot const& snapshot, GimbalState const& gimbal_state) + -> std::optional { + + /// 1. 目标选择 + auto selected_armor_id = int {}; + auto aim_point_position = Eigen::Vector3d {}; + auto center_position = Eigen::Vector3d {}; + { + auto const target_motion = snapshot.motion(); + auto const coarse_fly_time = + target_motion.center_position.norm() / config.initial_bullet_speed; + auto const selection_predict_time = config.shoot_delay + coarse_fly_time; + auto const selection_time = snapshot.time_stamp() + + std::chrono::duration_cast( + std::chrono::duration(selection_predict_time)); + + auto target_candidates = target_solver.make_candidates(snapshot, selection_time); + auto selected_index = armor_selector.select(target_candidates, last_selected_armor_id); + + if (!selected_index.has_value()) { + last_selected_armor_id.reset(); + return std::nullopt; + } + + auto const& selected = target_candidates[*selected_index]; + selected_armor_id = selected.armor.id; + center_position = target_motion.center_position; + selected.armor.translation.copy_to(aim_point_position); + } + + /// 2. 弹道解算 + auto target_solution = TargetSolution {}; + { + auto solution = target_solver.solve(snapshot, selected_armor_id, gimbal_state.timestamp, + config.initial_bullet_speed, config.shoot_delay); - auto reference = - reference_trajectory_builder.build(target_solution->impact_time, sample_attitude); - if (reference.has_value()) { - auto planned = mpc_trajectory_planner.plan(*reference); - if (planned.has_value()) { - pitch = planned->pitch; - yaw = planned->yaw; - pitch_rate = planned->pitch_rate; - yaw_rate = planned->yaw_rate; - pitch_acc = planned->pitch_acc; - yaw_acc = planned->yaw_acc; - feedforward_valid = true; - } else { - constexpr int kYawRow = 0; - constexpr int kPitchRow = 2; - constexpr int kCenterCol = kMpcAxisHorizon / 2; - pitch = (*reference)(kPitchRow, kCenterCol); - yaw = util::normalize_angle((*reference)(kYawRow, kCenterCol)); - } + if (!solution.has_value()) { + last_selected_armor_id.reset(); + return std::nullopt; } + + last_selected_armor_id = solution->candidate.armor.id; + target_solution = std::move(*solution); } - yaw = util::normalize_angle(yaw + config.yaw_offset); - pitch = pitch + config.pitch_offset; + /// 3. 轨迹规划 + auto feedforward = std::optional {}; + if (config.mpc_enable) { + auto planned = trajectory_planner.plan(snapshot, target_solution, + gimbal_state.timestamp, config.initial_bullet_speed, config.shoot_delay); + + if (planned.has_value()) { + feedforward = Feedforward { + .yaw = planned->yaw, + .pitch = planned->pitch, + .pitch_rate = planned->pitch_rate, + .yaw_rate = planned->yaw_rate, + .pitch_acc = planned->pitch_acc, + .yaw_acc = planned->yaw_acc, + }; + } + } - auto shoot_command = ShootEvaluator::Command { - .yaw = yaw, - .center_position = center_position, - .aim_point_position = aim_point_position, - }; - auto shoot_permitted = shoot_evaluator.evaluate(shoot_command, current_yaw); + /// 4. 偏移校正 + auto yaw = feedforward.has_value() ? feedforward->yaw : target_solution.attitude.yaw; + auto pitch = feedforward.has_value() ? feedforward->pitch : target_solution.attitude.pitch; + yaw = util::normalize_angle(yaw + config.yaw_offset); + pitch = pitch + config.pitch_offset; + + /// 5. 射击评估 + auto shoot_permitted = bool {}; + { + auto shoot_command = ShootEvaluator::Command { + .yaw = yaw, + .pitch = pitch, + .center_position = center_position, + .aim_point_position = aim_point_position, + }; + shoot_permitted = shoot_evaluator.evaluate(shoot_command, gimbal_state); + } + // 6. 组装结果 return Result { - .pitch = pitch, - .yaw = yaw, - .pitch_rate = pitch_rate, - .yaw_rate = yaw_rate, - .pitch_acc = pitch_acc, - .yaw_acc = yaw_acc, - .feedforward_valid = feedforward_valid, - .shoot_permitted = shoot_permitted, - .center_position = Point3d { center_position }, + .pitch = pitch, + .yaw = yaw, + .feedforward = feedforward, + .shoot_permitted = shoot_permitted, + .center_position = Point3d { center_position }, + .impact_time = target_solution.impact_time, }; } - MpcTrajectoryPlanner mpc_trajectory_planner { }; - ReferenceTrajectoryBuilder reference_trajectory_builder { }; - TargetSolutionSolver target_solution_solver { }; + TrajectoryPlanner trajectory_planner { }; + TargetSolver target_solver { }; ShootEvaluator shoot_evaluator { }; - AimPointChooser aim_point_chooser { }; + ArmorSelector armor_selector { }; + std::optional last_selected_armor_id { }; }; FireControl::FireControl() noexcept @@ -161,7 +182,7 @@ auto FireControl::initialize(const YAML::Node& yaml) noexcept -> std::expectedinitialize(yaml); } -auto FireControl::solve(const predictor::Snapshot& snapshot, double current_yaw) +auto FireControl::solve(const predictor::Snapshot& snapshot, GimbalState const& gimbal_state) -> std::optional { - return pimpl->solve(snapshot, current_yaw); + return pimpl->solve(snapshot, gimbal_state); } diff --git a/src/kernel/fire_control.hpp b/src/kernel/fire_control.hpp index b2f6d22b..9863343e 100644 --- a/src/kernel/fire_control.hpp +++ b/src/kernel/fire_control.hpp @@ -1,8 +1,11 @@ #pragma once #include +#include + #include +#include "module/fire_control/types.hpp" #include "module/predictor/snapshot.hpp" #include "utility/pimpl.hpp" @@ -12,21 +15,28 @@ class FireControl { RMCS_PIMPL_DEFINITION(FireControl) public: - struct Result { - double pitch; + struct Feedforward { double yaw; + double pitch; double pitch_rate; double yaw_rate; double pitch_acc; double yaw_acc; - bool feedforward_valid; + }; + + struct Result { + double pitch; + double yaw; + std::optional feedforward; bool shoot_permitted; Point3d center_position; + TimePoint impact_time; }; auto initialize(const YAML::Node&) noexcept -> std::expected; - auto solve(const predictor::Snapshot& snapshot, double current_yaw) -> std::optional; + auto solve(const predictor::Snapshot& snapshot, fire_control::GimbalState const& gimbal_state) + -> std::optional; }; } // namespace rmcs::kernel diff --git a/src/kernel/pose_estimator.cpp b/src/kernel/pose_estimator.cpp index ecef0258..fe04e31d 100644 --- a/src/kernel/pose_estimator.cpp +++ b/src/kernel/pose_estimator.cpp @@ -117,11 +117,11 @@ struct PoseEstimator::Impl { auto transformed = armor; - auto position = Eigen::Vector3d { }; + auto position = Eigen::Vector3d {}; transformed.translation.copy_to(position); transformed.translation = camera_orientation.inverse() * (position - camera_translation); - auto quat = Eigen::Quaterniond { }; + auto quat = Eigen::Quaterniond {}; transformed.orientation.copy_to(quat); transformed.orientation = camera_orientation.inverse() * quat; diff --git a/src/kernel/visualization.cpp b/src/kernel/visualization.cpp index 9c96893c..3f8341c5 100644 --- a/src/kernel/visualization.cpp +++ b/src/kernel/visualization.cpp @@ -51,7 +51,7 @@ struct Visualization::Impl { }; }; - Config config { }; + Config config {}; RclcppNode rclcpp { "visual" }; debug::StreamSession session; @@ -99,7 +99,7 @@ struct Visualization::Impl { }); is_initialized = true; - return { }; + return {}; } auto initialized() const noexcept { return is_initialized; } diff --git a/src/module/fire_control/aim_point_chooser.cpp b/src/module/fire_control/armor_selector.cpp similarity index 51% rename from src/module/fire_control/aim_point_chooser.cpp rename to src/module/fire_control/armor_selector.cpp index c4ca7564..71baa20b 100644 --- a/src/module/fire_control/aim_point_chooser.cpp +++ b/src/module/fire_control/armor_selector.cpp @@ -1,20 +1,24 @@ -#include "aim_point_chooser.hpp" +#include "armor_selector.hpp" #include #include #include +#include + +#include "utility/math/angle.hpp" #include "utility/math/conversion.hpp" #include "utility/serializable.hpp" using namespace rmcs::fire_control; -struct AimPointChooser::Impl { +struct ArmorSelector::Impl { struct Config : util::Serializable { double coming_angle { 60.0 }; double leaving_angle { 20.0 }; double outpost_coming_angle { 70.0 }; double outpost_leaving_angle { 30.0 }; + double switch_threshold { 8.0 }; constexpr static std::tuple metas { &Config::coming_angle, @@ -25,6 +29,8 @@ struct AimPointChooser::Impl { "outpost_coming_angle", &Config::outpost_leaving_angle, "outpost_leaving_angle", + &Config::switch_threshold, + "switch_threshold", }; } config; @@ -33,17 +39,6 @@ struct AimPointChooser::Impl { bool in_window { false }; }; - const double min_switch_improvement_angle { util::deg2rad(8.0) }; - std::optional last_chosen_armor_id { }; - - auto initialize(AimPointChooser::Config const& external_config) noexcept -> void { - config.coming_angle = external_config.coming_angle; - config.leaving_angle = external_config.leaving_angle; - config.outpost_coming_angle = external_config.outpost_coming_angle; - config.outpost_leaving_angle = external_config.outpost_leaving_angle; - last_chosen_armor_id.reset(); - } - auto configure_yaml(const YAML::Node& yaml) noexcept -> std::expected { auto result = config.serialize(yaml); if (!result.has_value()) return std::unexpected { result.error() }; @@ -52,41 +47,32 @@ struct AimPointChooser::Impl { config.leaving_angle = util::deg2rad(config.leaving_angle); config.outpost_coming_angle = util::deg2rad(config.outpost_coming_angle); config.outpost_leaving_angle = util::deg2rad(config.outpost_leaving_angle); - last_chosen_armor_id.reset(); + config.switch_threshold = util::deg2rad(config.switch_threshold); if (!(config.coming_angle > 0.0) || !(config.leaving_angle > 0.0) - || !(config.outpost_coming_angle > 0.0) || !(config.outpost_leaving_angle > 0.0)) { + || !(config.outpost_coming_angle > 0.0) || !(config.outpost_leaving_angle > 0.0) + || !(config.switch_threshold > 0.0)) { return std::unexpected { - "coming_angle, leaving_angle, outpost_coming_angle and outpost_leaving_angle must " - "be > 0", + "coming_angle, leaving_angle, outpost_coming_angle, outpost_leaving_angle and " + "switch_threshold must be > 0", }; } - return { }; + return {}; } - auto choose_armor(std::span armors, Eigen::Vector3d const& center_position, - double angular_velocity) -> std::optional { - if (armors.empty()) { - last_chosen_armor_id.reset(); - return std::nullopt; - } + auto select(std::span candidates, + std::optional last_selected_armor_id) const -> std::optional { + if (candidates.empty()) return std::nullopt; - const auto center_yaw = std::atan2(center_position.y(), center_position.x()); - const auto is_outpost = armors.front().genre == DeviceId::OUTPOST; + const auto is_outpost = candidates.front().armor.genre == DeviceId::OUTPOST; const auto active_coming = is_outpost ? config.outpost_coming_angle : config.coming_angle; const auto active_leaving = is_outpost ? config.outpost_leaving_angle : config.leaving_angle; - auto candidate_evals = std::vector(armors.size()); - - const auto yaw = [&](size_t index) { - auto orientation = Eigen::Quaterniond { }; - armors[index].orientation.copy_to(orientation); - return util::eulers(orientation)[0]; - }; + auto candidate_evals = std::vector(candidates.size()); - const auto in_window = [&](double delta_yaw) { + const auto in_window = [&](double delta_yaw, double angular_velocity) { const auto abs_delta = std::abs(delta_yaw); const auto in_coming = abs_delta <= active_coming; const auto in_leaving = (angular_velocity > 0.0) ? (delta_yaw <= active_leaving) @@ -95,77 +81,80 @@ struct AimPointChooser::Impl { return in_coming && in_leaving; }; - for (size_t index = 0; index < armors.size(); ++index) { - const auto delta_yaw = util::normalize_angle(yaw(index) - center_yaw); + const auto delta_yaw = [](ArmorCandidate const& candidate) { + auto orientation = Eigen::Quaterniond {}; + candidate.armor.orientation.copy_to(orientation); + const auto armor_yaw = util::eulers(orientation)[0]; + const auto center_yaw = std::atan2( + candidate.motion.center_position.y(), candidate.motion.center_position.x()); + return util::normalize_angle(armor_yaw - center_yaw); + }; + + for (size_t index = 0; index < candidates.size(); ++index) { + const auto delta = delta_yaw(candidates[index]); candidate_evals[index] = { - .delta_yaw = delta_yaw, - .in_window = in_window(delta_yaw), + .delta_yaw = delta, + .in_window = in_window(delta, candidates[index].motion.angular_velocity), }; } const auto priority_key = [&](size_t index) { - const auto preferred_incoming = [&] { + const auto leaving_penalty = [&] { const auto delta = candidate_evals[index].delta_yaw; - if (angular_velocity > 0.0) return (delta > 0.0) ? 1 : 0; - if (angular_velocity < 0.0) return (delta < 0.0) ? 1 : 0; + if (candidates[index].motion.angular_velocity > 0.0) return (delta > 0.0) ? 1 : 0; + if (candidates[index].motion.angular_velocity < 0.0) return (delta < 0.0) ? 1 : 0; return 0; }(); const auto abs_delta = std::abs(candidate_evals[index].delta_yaw); - const auto id = armors[index].id; - const auto is_last = last_chosen_armor_id.has_value() && (id == *last_chosen_armor_id); + const auto id = candidates[index].armor.id; + const auto is_last = + last_selected_armor_id.has_value() && (id == *last_selected_armor_id); const auto last_penalty = is_last ? 0 : 1; - return std::tuple { preferred_incoming, abs_delta, last_penalty, id, index }; + return std::tuple { leaving_penalty, abs_delta, last_penalty, id, index }; }; - auto best_idx = std::optional { }; - auto last_idx = std::optional { }; + auto best_idx = std::optional {}; + auto last_idx = std::optional {}; + + for (size_t index = 0; index < candidates.size(); ++index) { + if (!candidate_evals[index].in_window) continue; - for (size_t index = 0; index < armors.size(); ++index) { - if (last_chosen_armor_id.has_value() && (armors[index].id == *last_chosen_armor_id)) { + if (last_selected_armor_id.has_value() + && (candidates[index].armor.id == *last_selected_armor_id)) { last_idx = index; } - if (!candidate_evals[index].in_window) continue; - if (!best_idx.has_value() || (priority_key(index) < priority_key(*best_idx))) { best_idx = index; } } - if (!best_idx.has_value()) { - last_chosen_armor_id.reset(); - return std::nullopt; - } + if (!best_idx.has_value()) return std::nullopt; if (last_idx.has_value() && (*last_idx != *best_idx)) { const auto last_abs = std::abs(candidate_evals[*last_idx].delta_yaw); const auto best_abs = std::abs(candidate_evals[*best_idx].delta_yaw); const auto improvement = last_abs - best_abs; - if (improvement < min_switch_improvement_angle) { + if (improvement < config.switch_threshold) { best_idx = last_idx; } } - last_chosen_armor_id = armors[*best_idx].id; - return { armors[*best_idx] }; + return best_idx; } }; -AimPointChooser::AimPointChooser() noexcept +ArmorSelector::ArmorSelector() noexcept : pimpl { std::make_unique() } { } -AimPointChooser::~AimPointChooser() noexcept = default; - -auto AimPointChooser::initialize(Config const& config) noexcept -> void { - pimpl->initialize(config); -} +ArmorSelector::~ArmorSelector() noexcept = default; -auto AimPointChooser::configure_yaml(const YAML::Node& yaml) noexcept +auto ArmorSelector::configure_yaml(const YAML::Node& yaml) noexcept -> std::expected { return pimpl->configure_yaml(yaml); } -auto AimPointChooser::choose_armor(std::span armors, - Eigen::Vector3d const& center_position, double angular_velocity) -> std::optional { - return pimpl->choose_armor(armors, center_position, angular_velocity); +auto ArmorSelector::select(std::span candidates, + std::optional last_selected_armor_id) const -> std::optional { + return pimpl->select(candidates, last_selected_armor_id); } diff --git a/src/module/fire_control/aim_point_chooser.hpp b/src/module/fire_control/armor_selector.hpp similarity index 55% rename from src/module/fire_control/aim_point_chooser.hpp rename to src/module/fire_control/armor_selector.hpp index 46914240..b0c6930a 100644 --- a/src/module/fire_control/aim_point_chooser.hpp +++ b/src/module/fire_control/armor_selector.hpp @@ -1,7 +1,5 @@ #pragma once -#include - #include #include #include @@ -9,28 +7,27 @@ #include +#include "module/fire_control/types.hpp" #include "utility/pimpl.hpp" -#include "utility/robot/armor.hpp" namespace rmcs::fire_control { -class AimPointChooser { - RMCS_PIMPL_DEFINITION(AimPointChooser) +class ArmorSelector { + RMCS_PIMPL_DEFINITION(ArmorSelector) public: struct Config { double coming_angle; double leaving_angle; - double angular_velocity_threshold; double outpost_coming_angle; double outpost_leaving_angle; + double switch_threshold; }; - auto initialize(Config const& config) noexcept -> void; auto configure_yaml(const YAML::Node& yaml) noexcept -> std::expected; - auto choose_armor(std::span armors, Eigen::Vector3d const& center_position, - double angular_velocity) -> std::optional; + auto select(std::span candidates, + std::optional last_selected_armor_id) const -> std::optional; }; } // namespace rmcs::fire_control diff --git a/src/module/fire_control/gimbal_state.hpp b/src/module/fire_control/gimbal_state.hpp new file mode 100644 index 00000000..e0a09173 --- /dev/null +++ b/src/module/fire_control/gimbal_state.hpp @@ -0,0 +1,13 @@ +#pragma once + +#include "utility/clock.hpp" + +namespace rmcs::fire_control { + +struct GimbalState { + TimePoint timestamp; + double yaw { 0.0 }; + double pitch { 0.0 }; +}; + +} // namespace rmcs::fire_control diff --git a/src/module/fire_control/planner/tiny_mpc_axis_solver.cpp b/src/module/fire_control/planner/mpc_solver.cpp similarity index 60% rename from src/module/fire_control/planner/tiny_mpc_axis_solver.cpp rename to src/module/fire_control/planner/mpc_solver.cpp index 4b7fe3b0..4f94c116 100644 --- a/src/module/fire_control/planner/tiny_mpc_axis_solver.cpp +++ b/src/module/fire_control/planner/mpc_solver.cpp @@ -1,4 +1,4 @@ -#include "module/fire_control/planner/tiny_mpc_axis_solver.hpp" +#include "module/fire_control/planner/mpc_solver.hpp" #include #include @@ -11,7 +11,6 @@ namespace { constexpr int kAxisStateDim = 2; constexpr int kAxisInputDim = 1; -constexpr double kMpcAxisDt = 0.01; constexpr double kSolverRho = 1.0; constexpr double kUnboundedState = 1e17; @@ -20,19 +19,19 @@ using AxisDynamics = Eigen::Matrix; using AxisInputMatrix = Eigen::Matrix; using AxisStateCost = Eigen::Matrix; using AxisInputCost = Eigen::Matrix; -using AxisSolver = rmcs::util::TinyMpcSolver; +using AxisSolver = rmcs::util::TinyMpcSolver; } // namespace -struct TinyMpcAxisSolver::Impl { +struct MpcAxisSolver::Impl { std::optional solver {}; auto initialize(Config const& config) -> std::expected { auto A = AxisDynamics {}; - A << 1.0, kMpcAxisDt, 0.0, 1.0; + A << 1.0, kMpcDt, 0.0, 1.0; auto B = AxisInputMatrix {}; - B << 0.0, kMpcAxisDt; + B << 0.0, kMpcDt; const auto f = AxisState::Zero(); @@ -70,21 +69,16 @@ struct TinyMpcAxisSolver::Impl { return {}; } - auto solve_center(MpcAxisTrajectory const& reference) -> std::expected { - auto result = solve_center_kinematics(reference); - if (!result.has_value()) { - return std::unexpected { result.error() }; - } - return result->angle; - } - - auto solve_center_kinematics(MpcAxisTrajectory const& reference) - -> std::expected { + auto solve_kinematics(MpcAxisTrajectory const& reference, + MpcAxisSolver::AngularKinematics const& initial) + -> std::expected { if (!solver.has_value()) { return std::unexpected { "solver is not initialized" }; } - if (auto result = solver->set_x0(reference.col(0)); !result.has_value()) { + auto x0 = AxisState {}; + x0 << initial.angle, initial.rate; + if (auto result = solver->set_x0(x0); !result.has_value()) { return std::unexpected { result.error() }; } @@ -96,36 +90,25 @@ struct TinyMpcAxisSolver::Impl { return std::unexpected { result.error() }; } - constexpr auto kCenterStateStep = std::size_t { kMpcAxisHorizon / 2 }; - constexpr auto kCenterInputStep = std::size_t { - (kCenterStateStep < static_cast(kMpcAxisHorizon - 1)) - ? kCenterStateStep - : static_cast(kMpcAxisHorizon - 2), - }; - - return TinyMpcAxisSolver::AngularKinematics { - .angle = solver->template state<0, kCenterStateStep>(), - .rate = solver->template state<1, kCenterStateStep>(), - .acc = solver->template input<0, kCenterInputStep>(), + return MpcAxisSolver::AngularKinematics { + .angle = solver->template state<0, 1>(), + .rate = solver->template state<1, 1>(), + .acc = solver->template input<0, 0>(), }; } }; -TinyMpcAxisSolver::TinyMpcAxisSolver() noexcept +MpcAxisSolver::MpcAxisSolver() noexcept : pimpl { std::make_unique() } { } -TinyMpcAxisSolver::~TinyMpcAxisSolver() noexcept = default; +MpcAxisSolver::~MpcAxisSolver() noexcept = default; -auto TinyMpcAxisSolver::initialize(Config const& config) -> std::expected { +auto MpcAxisSolver::initialize(Config const& config) -> std::expected { return pimpl->initialize(config); } -auto TinyMpcAxisSolver::solve_center(MpcAxisTrajectory const& reference) - -> std::expected { - return pimpl->solve_center(reference); -} - -auto TinyMpcAxisSolver::solve_center_kinematics(MpcAxisTrajectory const& reference) +auto MpcAxisSolver::solve_kinematics( + MpcAxisTrajectory const& reference, AngularKinematics const& initial) -> std::expected { - return pimpl->solve_center_kinematics(reference); + return pimpl->solve_kinematics(reference, initial); } diff --git a/src/module/fire_control/planner/tiny_mpc_axis_solver.hpp b/src/module/fire_control/planner/mpc_solver.hpp similarity index 73% rename from src/module/fire_control/planner/tiny_mpc_axis_solver.hpp rename to src/module/fire_control/planner/mpc_solver.hpp index 1b01d5bf..c60186e2 100644 --- a/src/module/fire_control/planner/tiny_mpc_axis_solver.hpp +++ b/src/module/fire_control/planner/mpc_solver.hpp @@ -4,12 +4,11 @@ #include #include "module/fire_control/planner/mpc_types.hpp" - #include "utility/pimpl.hpp" namespace rmcs::fire_control { -class TinyMpcAxisSolver { +class MpcAxisSolver { public: struct AngularKinematics { double angle { 0.0 }; @@ -26,11 +25,10 @@ class TinyMpcAxisSolver { }; auto initialize(Config const& config) -> std::expected; - auto solve_center(MpcAxisTrajectory const& reference) -> std::expected; - auto solve_center_kinematics(MpcAxisTrajectory const& reference) + auto solve_kinematics(MpcAxisTrajectory const& reference, AngularKinematics const& initial) -> std::expected; - RMCS_PIMPL_DEFINITION(TinyMpcAxisSolver) + RMCS_PIMPL_DEFINITION(MpcAxisSolver) }; } // namespace rmcs::fire_control diff --git a/src/module/fire_control/planner/mpc_trajectory_planner.cpp b/src/module/fire_control/planner/mpc_trajectory_planner.cpp deleted file mode 100644 index e4cc85b7..00000000 --- a/src/module/fire_control/planner/mpc_trajectory_planner.cpp +++ /dev/null @@ -1,118 +0,0 @@ -#include "module/fire_control/planner/mpc_trajectory_planner.hpp" - -#include - -#include "module/fire_control/planner/tiny_mpc_axis_solver.hpp" -#include "utility/math/angle.hpp" -#include "utility/serializable.hpp" - -using namespace rmcs::fire_control; - -struct MpcTrajectoryPlanner::Impl { - struct Config : util::Serializable { - double mpc_max_yaw_acc { 50.0 }; - double mpc_yaw_q_angle { 9e6 }; - double mpc_yaw_q_rate { 0.0 }; - double mpc_yaw_r_acc { 1.0 }; - double mpc_max_pitch_acc { 100.0 }; - double mpc_pitch_q_angle { 9e6 }; - double mpc_pitch_q_rate { 0.0 }; - double mpc_pitch_r_acc { 1.0 }; - - constexpr static std::tuple metas { - &Config::mpc_max_yaw_acc, - "mpc_max_yaw_acc", - &Config::mpc_yaw_q_angle, - "mpc_yaw_q_angle", - &Config::mpc_yaw_q_rate, - "mpc_yaw_q_rate", - &Config::mpc_yaw_r_acc, - "mpc_yaw_r_acc", - &Config::mpc_max_pitch_acc, - "mpc_max_pitch_acc", - &Config::mpc_pitch_q_angle, - "mpc_pitch_q_angle", - &Config::mpc_pitch_q_rate, - "mpc_pitch_q_rate", - &Config::mpc_pitch_r_acc, - "mpc_pitch_r_acc", - }; - } config; - - TinyMpcAxisSolver yaw_solver {}; - TinyMpcAxisSolver pitch_solver {}; - - auto configure_yaml(const YAML::Node& yaml) noexcept -> std::expected { - auto result = config.serialize(yaml); - if (!result.has_value()) { - return std::unexpected { result.error() }; - } - - result = yaw_solver.initialize(TinyMpcAxisSolver::Config { - .max_acc = config.mpc_max_yaw_acc, - .q_angle = config.mpc_yaw_q_angle, - .q_rate = config.mpc_yaw_q_rate, - .r_acc = config.mpc_yaw_r_acc, - }); - if (!result.has_value()) { - return std::unexpected { std::format("yaw solver init failed: {}", result.error()) }; - } - - result = pitch_solver.initialize(TinyMpcAxisSolver::Config { - .max_acc = config.mpc_max_pitch_acc, - .q_angle = config.mpc_pitch_q_angle, - .q_rate = config.mpc_pitch_q_rate, - .r_acc = config.mpc_pitch_r_acc, - }); - if (!result.has_value()) { - return std::unexpected { - std::format("pitch solver init failed: {}", result.error()), - }; - } - - return {}; - } - - auto plan(ReferenceTrajectory const& reference) -> std::expected { - auto const yaw = yaw_solver.solve_center_kinematics( - MpcAxisTrajectory { reference.template block<2, kMpcAxisHorizon>(0, 0) }); - if (!yaw.has_value()) { - return std::unexpected { std::format("yaw solve failed: {}", yaw.error()) }; - } - - auto const pitch = pitch_solver.solve_center_kinematics( - MpcAxisTrajectory { reference.template block<2, kMpcAxisHorizon>(2, 0) }); - if (!pitch.has_value()) { - return std::unexpected { std::format("pitch solve failed: {}", pitch.error()) }; - } - - auto result = Plan {}; - result.yaw = util::normalize_angle(yaw->angle); - result.pitch = pitch->angle; - result.yaw_rate = yaw->rate; - result.pitch_rate = pitch->rate; - result.yaw_acc = yaw->acc; - result.pitch_acc = pitch->acc; - return result; - } -}; - -MpcTrajectoryPlanner::MpcTrajectoryPlanner() noexcept - : pimpl { std::make_unique() } { } - -MpcTrajectoryPlanner::~MpcTrajectoryPlanner() noexcept = default; - -auto MpcTrajectoryPlanner::configure_yaml(const YAML::Node& yaml) noexcept - -> std::expected { - return pimpl->configure_yaml(yaml); -} - -auto MpcTrajectoryPlanner::initialize(const YAML::Node& yaml) noexcept - -> std::expected { - return pimpl->configure_yaml(yaml); -} - -auto MpcTrajectoryPlanner::plan(ReferenceTrajectory const& reference) - -> std::expected { - return pimpl->plan(reference); -} diff --git a/src/module/fire_control/planner/mpc_types.hpp b/src/module/fire_control/planner/mpc_types.hpp index 25d5b2d1..f271cae8 100644 --- a/src/module/fire_control/planner/mpc_types.hpp +++ b/src/module/fire_control/planner/mpc_types.hpp @@ -4,9 +4,10 @@ namespace rmcs::fire_control { -inline constexpr int kMpcAxisHorizon = 100; +inline constexpr int kMpcHorizon = 50; +inline constexpr double kMpcDt = 0.01; -using MpcAxisTrajectory = Eigen::Matrix; -using ReferenceTrajectory = Eigen::Matrix; +using MpcAxisTrajectory = Eigen::Matrix; +using ReferenceTrajectory = Eigen::Matrix; } // namespace rmcs::fire_control diff --git a/src/module/fire_control/planner/reference_builder.cpp b/src/module/fire_control/planner/reference_builder.cpp new file mode 100644 index 00000000..4704a346 --- /dev/null +++ b/src/module/fire_control/planner/reference_builder.cpp @@ -0,0 +1,65 @@ +#include "module/fire_control/planner/reference_builder.hpp" + +#include +#include +#include + +#include "utility/math/angle.hpp" + +using namespace rmcs::fire_control; + +struct ReferenceBuilder::Impl { + TargetSolver sampler {}; + + auto build(predictor::Snapshot const& snapshot, TargetSolution const& raw_solution, + TimePoint command_time, double bullet_speed, double shoot_delay) const + -> std::expected { + + auto samples = std::array {}; + for (int i = 0; i < static_cast(samples.size()); ++i) { + auto const offset = i * kMpcDt; + auto const t = command_time + + std::chrono::duration_cast( + std::chrono::duration { offset }); + + if (i == 0) { + samples[i] = raw_solution.attitude; + continue; + } + + auto solution = sampler.solve( + snapshot, raw_solution.candidate.armor.id, t, bullet_speed, shoot_delay); + if (!solution.has_value()) { + return std::unexpected { std::format( + "reference sample failed at index {}: {}", i, solution.error()) }; + } + samples[i] = solution->attitude; + } + + auto trajectory = ReferenceTrajectory {}; + for (int i = 0; i < kMpcHorizon; ++i) { + auto const& previous = samples[i]; + auto const& current = samples[i + 1]; + auto const& next = samples[i + 2]; + + auto const yaw_velocity = + util::normalize_angle(next.yaw - previous.yaw) / (2.0 * kMpcDt); + auto const pitch_velocity = (next.pitch - previous.pitch) / (2.0 * kMpcDt); + + trajectory.col(i) << current.yaw, yaw_velocity, current.pitch, pitch_velocity; + } + + return trajectory; + } +}; + +ReferenceBuilder::ReferenceBuilder() noexcept + : pimpl { std::make_unique() } { } + +ReferenceBuilder::~ReferenceBuilder() noexcept = default; + +auto ReferenceBuilder::build(predictor::Snapshot const& snapshot, + TargetSolution const& raw_solution, TimePoint command_time, double bullet_speed, + double shoot_delay) -> std::expected { + return pimpl->build(snapshot, raw_solution, command_time, bullet_speed, shoot_delay); +} diff --git a/src/module/fire_control/planner/reference_builder.hpp b/src/module/fire_control/planner/reference_builder.hpp new file mode 100644 index 00000000..ca8abf9d --- /dev/null +++ b/src/module/fire_control/planner/reference_builder.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include +#include + +#include "module/fire_control/planner/mpc_types.hpp" +#include "module/fire_control/types.hpp" +#include "module/predictor/snapshot.hpp" +#include "module/fire_control/target_solver.hpp" +#include "utility/pimpl.hpp" + +namespace rmcs::fire_control { + +class ReferenceBuilder { + RMCS_PIMPL_DEFINITION(ReferenceBuilder) + +public: + auto build(predictor::Snapshot const& snapshot, TargetSolution const& raw_solution, + TimePoint command_time, double bullet_speed, double shoot_delay) + -> std::expected; +}; + +} // namespace rmcs::fire_control diff --git a/src/module/fire_control/planner/reference_trajectory_builder.cpp b/src/module/fire_control/planner/reference_trajectory_builder.cpp deleted file mode 100644 index fe5197cf..00000000 --- a/src/module/fire_control/planner/reference_trajectory_builder.cpp +++ /dev/null @@ -1,45 +0,0 @@ -#include "module/fire_control/planner/reference_trajectory_builder.hpp" - -#include -#include -#include - -#include "utility/math/angle.hpp" - -namespace rmcs::fire_control { - -auto ReferenceTrajectoryBuilder::build( - TimePoint center_time, AimAttitudeSampler const& sample_attitude) const - -> std::expected { - constexpr int kCenterSampleIndex = (kMpcAxisHorizon / 2) + 1; - - auto samples = std::array {}; - for (int i = 0; i < static_cast(samples.size()); ++i) { - auto const offset = (i - kCenterSampleIndex) * kMpcAxisDt; - auto const t = center_time - + std::chrono::duration_cast(std::chrono::duration { offset }); - auto sample = sample_attitude(t); - if (!sample.has_value()) { - return std::unexpected { std::format( - "reference sample failed at index {}: {}", i, sample.error()) }; - } - samples[i] = *sample; - } - - auto trajectory = ReferenceTrajectory {}; - for (int i = 0; i < kMpcAxisHorizon; ++i) { - auto const& previous = samples[i]; - auto const& current = samples[i + 1]; - auto const& next = samples[i + 2]; - - auto const yaw_velocity = - rmcs::util::normalize_angle(next.yaw - previous.yaw) / (2.0 * kMpcAxisDt); - auto const pitch_velocity = (next.pitch - previous.pitch) / (2.0 * kMpcAxisDt); - - trajectory.col(i) << current.yaw, yaw_velocity, current.pitch, pitch_velocity; - } - - return trajectory; -} - -} // namespace rmcs::fire_control diff --git a/src/module/fire_control/planner/reference_trajectory_builder.hpp b/src/module/fire_control/planner/reference_trajectory_builder.hpp deleted file mode 100644 index 93f5131d..00000000 --- a/src/module/fire_control/planner/reference_trajectory_builder.hpp +++ /dev/null @@ -1,24 +0,0 @@ -#pragma once - -#include -#include -#include - -#include "module/fire_control/planner/mpc_types.hpp" -#include "module/fire_control/solver/aim_point_sampling.hpp" -#include "utility/clock.hpp" - -namespace rmcs::fire_control { - -class ReferenceTrajectoryBuilder { -public: - using AimAttitudeSampler = std::function(TimePoint)>; - - auto build(TimePoint center_time, AimAttitudeSampler const& sample_attitude) const - -> std::expected; - -private: - static constexpr double kMpcAxisDt = 0.01; -}; - -} // namespace rmcs::fire_control diff --git a/src/module/fire_control/planner/trajectory_planner.cpp b/src/module/fire_control/planner/trajectory_planner.cpp new file mode 100644 index 00000000..891ff018 --- /dev/null +++ b/src/module/fire_control/planner/trajectory_planner.cpp @@ -0,0 +1,182 @@ +#include "module/fire_control/planner/trajectory_planner.hpp" + +#include +#include +#include + +#include "module/fire_control/planner/mpc_solver.hpp" +#include "module/fire_control/planner/reference_builder.hpp" +#include "utility/math/angle.hpp" +#include "utility/serializable.hpp" + +using namespace rmcs::fire_control; + +struct TrajectoryPlanner::Impl { + static constexpr std::chrono::milliseconds kPlanStateTimeout { 200 }; + + struct Config : util::Serializable { + double mpc_max_yaw_acc { 50.0 }; + double mpc_yaw_q_angle { 9e6 }; + double mpc_yaw_q_rate { 0.0 }; + double mpc_yaw_r_acc { 1.0 }; + double mpc_max_pitch_acc { 100.0 }; + double mpc_pitch_q_angle { 9e6 }; + double mpc_pitch_q_rate { 0.0 }; + double mpc_pitch_r_acc { 1.0 }; + int mpc_max_iter { 10 }; + + constexpr static std::tuple metas { + &Config::mpc_max_yaw_acc, + "mpc_max_yaw_acc", + &Config::mpc_yaw_q_angle, + "mpc_yaw_q_angle", + &Config::mpc_yaw_q_rate, + "mpc_yaw_q_rate", + &Config::mpc_yaw_r_acc, + "mpc_yaw_r_acc", + &Config::mpc_max_pitch_acc, + "mpc_max_pitch_acc", + &Config::mpc_pitch_q_angle, + "mpc_pitch_q_angle", + &Config::mpc_pitch_q_rate, + "mpc_pitch_q_rate", + &Config::mpc_pitch_r_acc, + "mpc_pitch_r_acc", + &Config::mpc_max_iter, + "mpc_max_iter", + }; + } config; + + ReferenceBuilder reference_builder {}; + MpcAxisSolver yaw_solver {}; + MpcAxisSolver pitch_solver {}; + std::optional last_plan {}; + TimePoint last_plan_timestamp {}; + + static auto make_continuous_yaw_reference( + ReferenceTrajectory const& reference, double initial_yaw) -> MpcAxisTrajectory { + auto yaw_reference = + MpcAxisTrajectory { reference.template block<2, kMpcHorizon>(0, 0) }; + + auto previous_yaw = initial_yaw; + for (int i = 0; i < kMpcHorizon; ++i) { + auto const yaw = + previous_yaw + util::normalize_angle(yaw_reference(0, i) - previous_yaw); + yaw_reference(0, i) = yaw; + previous_yaw = yaw; + } + + return yaw_reference; + } + + auto should_reset_plan_state(TimePoint timestamp) const noexcept -> bool { + return !last_plan.has_value() || timestamp < last_plan_timestamp + || timestamp - last_plan_timestamp > kPlanStateTimeout; + } + + static auto make_initial_state(ReferenceTrajectory const& reference, + std::optional const& previous_plan, + int angle_row, int rate_row, + double Plan::*prev_angle, double Plan::*prev_rate) noexcept + -> MpcAxisSolver::AngularKinematics { + if (!previous_plan.has_value()) { + return { + .angle = reference(angle_row, 0), + .rate = reference(rate_row, 0), + }; + } + + return { + .angle = (*previous_plan).*prev_angle, + .rate = (*previous_plan).*prev_rate, + }; + } + + auto configure_yaml(const YAML::Node& yaml) noexcept -> std::expected { + auto result = config.serialize(yaml); + if (!result.has_value()) { + return std::unexpected { result.error() }; + } + + result = yaw_solver.initialize(MpcAxisSolver::Config { + .max_acc = config.mpc_max_yaw_acc, + .q_angle = config.mpc_yaw_q_angle, + .q_rate = config.mpc_yaw_q_rate, + .r_acc = config.mpc_yaw_r_acc, + .max_iter = config.mpc_max_iter, + }); + if (!result.has_value()) { + return std::unexpected { std::format("yaw solver init failed: {}", result.error()) }; + } + + result = pitch_solver.initialize(MpcAxisSolver::Config { + .max_acc = config.mpc_max_pitch_acc, + .q_angle = config.mpc_pitch_q_angle, + .q_rate = config.mpc_pitch_q_rate, + .r_acc = config.mpc_pitch_r_acc, + .max_iter = config.mpc_max_iter, + }); + if (!result.has_value()) { + return std::unexpected { + std::format("pitch solver init failed: {}", result.error()), + }; + } + + return {}; + } + + auto plan(predictor::Snapshot const& snapshot, TargetSolution const& raw_solution, + TimePoint command_time, double bullet_speed, double shoot_delay) + -> std::expected { + auto reference = + reference_builder.build(snapshot, raw_solution, command_time, bullet_speed, shoot_delay); + if (!reference.has_value()) return std::unexpected { reference.error() }; + + auto previous_plan = + should_reset_plan_state(command_time) ? std::optional {} : last_plan; + + auto const yaw_initial = + make_initial_state(*reference, previous_plan, 0, 1, &Plan::yaw, &Plan::yaw_rate); + auto const yaw_reference = make_continuous_yaw_reference(*reference, yaw_initial.angle); + auto const yaw = yaw_solver.solve_kinematics(yaw_reference, yaw_initial); + if (!yaw.has_value()) { + return std::unexpected { std::format("yaw solve failed: {}", yaw.error()) }; + } + + auto const pitch_initial = + make_initial_state(*reference, previous_plan, 2, 3, &Plan::pitch, &Plan::pitch_rate); + auto const pitch = pitch_solver.solve_kinematics( + MpcAxisTrajectory { reference->template block<2, kMpcHorizon>(2, 0) }, + pitch_initial); + if (!pitch.has_value()) { + return std::unexpected { std::format("pitch solve failed: {}", pitch.error()) }; + } + + auto result = Plan {}; + result.yaw = util::normalize_angle(yaw->angle); + result.pitch = pitch->angle; + result.yaw_rate = yaw->rate; + result.pitch_rate = pitch->rate; + result.yaw_acc = yaw->acc; + result.pitch_acc = pitch->acc; + last_plan = result; + last_plan_timestamp = command_time; + return result; + } +}; + +TrajectoryPlanner::TrajectoryPlanner() noexcept + : pimpl { std::make_unique() } { } + +TrajectoryPlanner::~TrajectoryPlanner() noexcept = default; + +auto TrajectoryPlanner::configure_yaml(const YAML::Node& yaml) noexcept + -> std::expected { + return pimpl->configure_yaml(yaml); +} + +auto TrajectoryPlanner::plan(predictor::Snapshot const& snapshot, + TargetSolution const& raw_solution, TimePoint command_time, double bullet_speed, + double shoot_delay) -> std::expected { + return pimpl->plan(snapshot, raw_solution, command_time, bullet_speed, shoot_delay); +} diff --git a/src/module/fire_control/planner/mpc_trajectory_planner.hpp b/src/module/fire_control/planner/trajectory_planner.hpp similarity index 58% rename from src/module/fire_control/planner/mpc_trajectory_planner.hpp rename to src/module/fire_control/planner/trajectory_planner.hpp index 80497454..337ef05e 100644 --- a/src/module/fire_control/planner/mpc_trajectory_planner.hpp +++ b/src/module/fire_control/planner/trajectory_planner.hpp @@ -1,16 +1,19 @@ #pragma once #include +#include #include #include "module/fire_control/planner/mpc_types.hpp" +#include "module/fire_control/types.hpp" +#include "module/predictor/snapshot.hpp" #include "utility/pimpl.hpp" namespace rmcs::fire_control { -class MpcTrajectoryPlanner { - RMCS_PIMPL_DEFINITION(MpcTrajectoryPlanner) +class TrajectoryPlanner { + RMCS_PIMPL_DEFINITION(TrajectoryPlanner) public: struct Plan { @@ -23,9 +26,10 @@ class MpcTrajectoryPlanner { }; auto configure_yaml(const YAML::Node& yaml) noexcept -> std::expected; - auto initialize(const YAML::Node& yaml) noexcept -> std::expected; - auto plan(ReferenceTrajectory const& reference) -> std::expected; + auto plan(predictor::Snapshot const& snapshot, TargetSolution const& raw_solution, + TimePoint command_time, double bullet_speed, double shoot_delay) + -> std::expected; }; } // namespace rmcs::fire_control diff --git a/src/module/fire_control/shoot_evaluator.cpp b/src/module/fire_control/shoot_evaluator.cpp index 81cf4dba..326426dd 100644 --- a/src/module/fire_control/shoot_evaluator.cpp +++ b/src/module/fire_control/shoot_evaluator.cpp @@ -10,16 +10,22 @@ using namespace rmcs::fire_control; struct ShootEvaluator::Impl { struct Config : util::Serializable { - double near_angle_tolerance { 4.0 }; - double far_angle_tolerance { 2.0 }; + double near_yaw_tolerance { 3.0 }; + double far_yaw_tolerance { 2.0 }; + double near_pitch_tolerance { 2.0 }; + double far_pitch_tolerance { 1.0 }; double split_distance { 3.0 }; bool is_lazy_gimbal { false }; constexpr static std::tuple metas { - &Config::near_angle_tolerance, - "near_angle_tolerance", - &Config::far_angle_tolerance, - "far_angle_tolerance", + &Config::near_yaw_tolerance, + "near_yaw_tolerance", + &Config::far_yaw_tolerance, + "far_yaw_tolerance", + &Config::near_pitch_tolerance, + "near_pitch_tolerance", + &Config::far_pitch_tolerance, + "far_pitch_tolerance", &Config::split_distance, "split_distance", &Config::is_lazy_gimbal, @@ -33,13 +39,21 @@ struct ShootEvaluator::Impl { return std::unexpected { result.error() }; } - config.near_angle_tolerance = util::deg2rad(config.near_angle_tolerance); - config.far_angle_tolerance = util::deg2rad(config.far_angle_tolerance); + config.near_yaw_tolerance = util::deg2rad(config.near_yaw_tolerance); + config.far_yaw_tolerance = util::deg2rad(config.far_yaw_tolerance); + config.near_pitch_tolerance = util::deg2rad(config.near_pitch_tolerance); + config.far_pitch_tolerance = util::deg2rad(config.far_pitch_tolerance); last_command_.reset(); - if (!(config.near_angle_tolerance > 0.0) || !(config.far_angle_tolerance > 0.0)) { + if (!(config.near_yaw_tolerance > 0.0) || !(config.far_yaw_tolerance > 0.0)) { return std::unexpected { - "near_angle_tolerance and far_angle_tolerance must be > 0", + "near_yaw_tolerance and far_yaw_tolerance must be > 0", + }; + } + + if (!(config.near_pitch_tolerance > 0.0) || !(config.far_pitch_tolerance > 0.0)) { + return std::unexpected { + "near_pitch_tolerance and far_pitch_tolerance must be > 0", }; } @@ -50,14 +64,14 @@ struct ShootEvaluator::Impl { return {}; } - auto evaluate(Command const& command, double current_yaw) noexcept -> bool { + auto evaluate(Command const& command, GimbalState const& state) noexcept -> bool { auto should_fire = false; if (config.is_lazy_gimbal) { auto const aim_point_yaw = std::atan2(command.aim_point_position.y(), command.aim_point_position.x()); - const auto aim_delta = std::abs(util::normalize_angle(current_yaw - aim_point_yaw)); - const auto is_overlap = aim_delta < config.near_angle_tolerance; + const auto aim_delta = std::abs(util::normalize_angle(state.yaw - aim_point_yaw)); + const auto is_overlap = aim_delta < config.near_yaw_tolerance; if (!is_overlap) { last_command_ = command; return false; @@ -66,17 +80,22 @@ struct ShootEvaluator::Impl { auto const center_distance = std::hypot(command.center_position.x(), command.center_position.y()); - const auto tolerance = (center_distance > config.split_distance) - ? config.far_angle_tolerance - : config.near_angle_tolerance; + const auto yaw_tolerance = (center_distance > config.split_distance) + ? config.far_yaw_tolerance + : config.near_yaw_tolerance; + const auto pitch_tolerance = (center_distance > config.split_distance) + ? config.far_pitch_tolerance + : config.near_pitch_tolerance; if (last_command_.has_value()) { 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)); + std::abs(util::normalize_angle(state.yaw - last_command_->yaw)); + const auto pitch_delta = std::abs(util::normalize_angle(state.pitch - command.pitch)); - should_fire = (yaw_delta < tolerance * 2.0) && (track_delta < tolerance); + should_fire = (yaw_delta < yaw_tolerance) && (track_delta < yaw_tolerance) + && (pitch_delta < pitch_tolerance); } last_command_ = command; @@ -102,6 +121,6 @@ auto ShootEvaluator::configure_yaml(const YAML::Node& yaml) noexcept return pimpl->configure_yaml(yaml); } -auto ShootEvaluator::evaluate(Command const& command, double current_yaw) noexcept -> bool { - return pimpl->evaluate(command, current_yaw); +auto ShootEvaluator::evaluate(Command const& command, GimbalState const& state) noexcept -> bool { + return pimpl->evaluate(command, state); } diff --git a/src/module/fire_control/shoot_evaluator.hpp b/src/module/fire_control/shoot_evaluator.hpp index 627e21f0..2c5c566f 100644 --- a/src/module/fire_control/shoot_evaluator.hpp +++ b/src/module/fire_control/shoot_evaluator.hpp @@ -8,6 +8,7 @@ #include +#include "module/fire_control/gimbal_state.hpp" #include "utility/pimpl.hpp" namespace rmcs::fire_control { @@ -18,6 +19,7 @@ class ShootEvaluator { public: struct Command { double yaw { std::numeric_limits::quiet_NaN() }; + double pitch { std::numeric_limits::quiet_NaN() }; Eigen::Vector3d center_position { Eigen::Vector3d::Zero() }; Eigen::Vector3d aim_point_position { Eigen::Vector3d::Zero() }; }; @@ -25,7 +27,7 @@ class ShootEvaluator { auto initialize(const YAML::Node& yaml) noexcept -> std::expected; auto configure_yaml(const YAML::Node& yaml) noexcept -> std::expected; - auto evaluate(Command const& command, double current_yaw) noexcept -> bool; + auto evaluate(Command const& command, GimbalState const& state) noexcept -> bool; }; } // namespace rmcs::fire_control diff --git a/src/module/fire_control/solver/aim_point_sampling.cpp b/src/module/fire_control/solver/aim_point_sampling.cpp deleted file mode 100644 index a952b8fb..00000000 --- a/src/module/fire_control/solver/aim_point_sampling.cpp +++ /dev/null @@ -1,64 +0,0 @@ -#include "module/fire_control/solver/aim_point_sampling.hpp" - -#include - -#include "module/fire_control/trajectory_solution.hpp" -#include "utility/math/angle.hpp" - -using namespace rmcs::fire_control; - -auto rmcs::fire_control::AimPointSampler::sample_at( - predictor::Snapshot const& snapshot, AimPointChooser& chooser, TimePoint t, double bullet_speed) - -> std::expected { - auto aim_point = sample_aim_point_at(snapshot, chooser, t); - if (!aim_point.has_value()) { - return std::unexpected { aim_point.error() }; - } - - auto attitude = solve_aim_attitude(aim_point.value(), bullet_speed); - if (!attitude.has_value()) { - return std::unexpected { attitude.error() }; - } - - return AimSample { - .attitude = *attitude, - .aim_point = aim_point.value(), - }; -} - -auto rmcs::fire_control::AimPointSampler::sample_aim_point_at(predictor::Snapshot const& snapshot, - AimPointChooser& chooser, TimePoint t) -> std::expected { - auto predicted_armors = snapshot.predicted_armors(t); - auto predicted_kinematics = snapshot.kinematics_at(t); - auto chosen_armor = chooser.choose_armor(predicted_armors, - predicted_kinematics.center_position.make(), - predicted_kinematics.angular_velocity); - if (!chosen_armor.has_value()) return std::unexpected { "choose_armor returned nullopt" }; - - auto aim_point = Eigen::Vector3d { }; - chosen_armor->translation.copy_to(aim_point); - return aim_point; -} - -auto rmcs::fire_control::AimPointSampler::solve_aim_attitude(Eigen::Vector3d const& aim_point, - double bullet_speed) -> std::expected { - auto const target_d = std::hypot(aim_point.x(), aim_point.y()); - if (!(target_d > 0.0)) { - return std::unexpected { "invalid target distance" }; - } - - auto solution = TrajectorySolution { }; - solution.input.v0 = bullet_speed; - solution.input.target_position = aim_point; - - auto trajectory = solution.solve(); - if (!trajectory.has_value()) { - return std::unexpected { "trajectory solve failed" }; - } - - return AimAttitude { - .yaw = util::normalize_angle(trajectory->yaw), - .pitch = trajectory->pitch, - .fly_time = trajectory->fly_time, - }; -} diff --git a/src/module/fire_control/solver/aim_point_sampling.hpp b/src/module/fire_control/solver/aim_point_sampling.hpp deleted file mode 100644 index 2a8a36d8..00000000 --- a/src/module/fire_control/solver/aim_point_sampling.hpp +++ /dev/null @@ -1,37 +0,0 @@ -#pragma once - -#include -#include - -#include - -#include "module/fire_control/aim_point_chooser.hpp" -#include "module/predictor/snapshot.hpp" - -namespace rmcs::fire_control { - -struct AimAttitude { - double yaw { 0.0 }; - double pitch { 0.0 }; - double fly_time { 0.0 }; -}; - -struct AimSample { - AimAttitude attitude; - Eigen::Vector3d aim_point { Eigen::Vector3d::Zero() }; -}; - -class AimPointSampler { -public: - static auto sample_at(predictor::Snapshot const& snapshot, AimPointChooser& chooser, - TimePoint t, double bullet_speed) -> std::expected; - -private: - static auto sample_aim_point_at(predictor::Snapshot const& snapshot, AimPointChooser& chooser, - TimePoint t) -> std::expected; - - static auto solve_aim_attitude(Eigen::Vector3d const& aim_point, double bullet_speed) - -> std::expected; -}; - -} // namespace rmcs::fire_control diff --git a/src/module/fire_control/solver/target_solution_solver.cpp b/src/module/fire_control/solver/target_solution_solver.cpp deleted file mode 100644 index d64b445f..00000000 --- a/src/module/fire_control/solver/target_solution_solver.cpp +++ /dev/null @@ -1,73 +0,0 @@ -#include "module/fire_control/solver/target_solution_solver.hpp" - -#include -#include -#include - -#include "module/fire_control/solver/aim_point_sampling.hpp" - -using namespace rmcs::fire_control; - -struct TargetSolutionSolver::Impl { - static constexpr int kMaxIterateCount = 5; - static constexpr double kMaxFlyTimeThreshold = 0.001; - - static auto solve(predictor::Snapshot const& snapshot, AimPointChooser& chooser, - double bullet_speed, double shoot_delay) -> std::expected { - struct BestCandidate { - AimAttitude attitude; - TimePoint impact_time; - Eigen::Vector3d center_position; - Eigen::Vector3d aim_point; - }; - - auto target_kinematics = snapshot.kinematics(); - auto target_position = target_kinematics.center_position.make(); - auto current_fly_time = target_position.norm() / bullet_speed; - - auto best_candidate = std::optional {}; - - for (int i = 0; i < kMaxIterateCount; ++i) { - auto const total_predict_time = current_fly_time + shoot_delay; - auto const t_target = snapshot.time_stamp() - + std::chrono::duration_cast( - std::chrono::duration(total_predict_time)); - - auto predicted_kinematics = snapshot.kinematics_at(t_target); - auto sample = AimPointSampler::sample_at(snapshot, chooser, t_target, bullet_speed); - if (!sample.has_value()) continue; - - auto const time_error = std::abs(sample->attitude.fly_time - current_fly_time); - current_fly_time = sample->attitude.fly_time; - best_candidate = BestCandidate { - .attitude = sample->attitude, - .impact_time = t_target, - .center_position = predicted_kinematics.center_position.make(), - .aim_point = sample->aim_point, - }; - if (time_error < kMaxFlyTimeThreshold) break; - } - - if (!best_candidate.has_value()) { - return std::unexpected { "no valid impact target" }; - } - - return TargetSolution { - .impact_time = best_candidate->impact_time, - .impact_yaw = best_candidate->attitude.yaw, - .impact_pitch = best_candidate->attitude.pitch, - .center_position = best_candidate->center_position, - .aim_point = best_candidate->aim_point, - }; - } -}; - -TargetSolutionSolver::TargetSolutionSolver() noexcept - : pimpl { std::make_unique() } { } - -TargetSolutionSolver::~TargetSolutionSolver() noexcept = default; - -auto TargetSolutionSolver::solve(predictor::Snapshot const& snapshot, AimPointChooser& chooser, - double bullet_speed, double shoot_delay) const -> std::expected { - return pimpl->solve(snapshot, chooser, bullet_speed, shoot_delay); -} diff --git a/src/module/fire_control/solver/target_solution_solver.hpp b/src/module/fire_control/solver/target_solution_solver.hpp deleted file mode 100644 index 14c9bac0..00000000 --- a/src/module/fire_control/solver/target_solution_solver.hpp +++ /dev/null @@ -1,30 +0,0 @@ -#pragma once - -#include - -#include -#include - -#include "module/fire_control/aim_point_chooser.hpp" -#include "module/predictor/snapshot.hpp" -#include "utility/pimpl.hpp" - -namespace rmcs::fire_control { - -class TargetSolutionSolver { - RMCS_PIMPL_DEFINITION(TargetSolutionSolver) - -public: - struct TargetSolution { - TimePoint impact_time; - double impact_yaw { 0.0 }; - double impact_pitch { 0.0 }; - Eigen::Vector3d center_position { Eigen::Vector3d::Zero() }; - Eigen::Vector3d aim_point { Eigen::Vector3d::Zero() }; - }; - - auto solve(predictor::Snapshot const& snapshot, AimPointChooser& chooser, double bullet_speed, - double shoot_delay) const -> std::expected; -}; - -} // namespace rmcs::fire_control diff --git a/src/module/fire_control/target_solver.cpp b/src/module/fire_control/target_solver.cpp new file mode 100644 index 00000000..40b783b6 --- /dev/null +++ b/src/module/fire_control/target_solver.cpp @@ -0,0 +1,141 @@ +#include "module/fire_control/target_solver.hpp" + +#include +#include +#include +#include +#include + +#include "module/fire_control/trajectory_solution.hpp" +#include "utility/math/angle.hpp" + +using namespace rmcs::fire_control; + +struct TargetSolver::Impl { + static constexpr int kMaxIterateCount = 5; + static constexpr double kMaxFlyTimeThreshold = 0.001; + + struct AimSample { + Armor3d armor; + AimAttitude attitude; + }; + + static auto make_candidates(predictor::Snapshot const& snapshot, TimePoint sample_time) + -> std::vector { + auto candidates = std::vector {}; + auto predicted_armors = snapshot.predicted_armors(sample_time); + auto predicted_motion = snapshot.motion_at(sample_time); + candidates.reserve(predicted_armors.size()); + + for (auto const& armor : predicted_armors) { + candidates.emplace_back(ArmorCandidate { + .armor = armor, + .motion = predicted_motion, + }); + } + + return candidates; + } + + static auto solve(predictor::Snapshot const& snapshot, int armor_id, TimePoint command_time, + double bullet_speed, double shoot_delay) -> std::expected { + auto target_motion = snapshot.motion_at(command_time); + auto target_position = target_motion.center_position; + auto current_fly_time = target_position.norm() / bullet_speed; + + auto result = std::optional {}; + + for (int i = 0; i < kMaxIterateCount; ++i) { + auto const elapsed_command_time = + std::chrono::duration(command_time - snapshot.time_stamp()).count(); + auto const remaining_shoot_delay = std::max(0.0, shoot_delay - elapsed_command_time); + auto const total_predict_time = remaining_shoot_delay + current_fly_time; + auto const t_target = command_time + + std::chrono::duration_cast( + std::chrono::duration(total_predict_time)); + + auto predicted_motion = snapshot.motion_at(t_target); + auto sample = sample_at(snapshot, armor_id, t_target, bullet_speed); + if (!sample.has_value()) return std::unexpected { sample.error() }; + + auto const time_error = std::abs(sample->attitude.fly_time - current_fly_time); + current_fly_time = sample->attitude.fly_time; + + result = { + .impact_time = t_target, + .candidate = { + .armor = sample->armor, + .motion = predicted_motion, + }, + .attitude = sample->attitude, + }; + if (time_error < kMaxFlyTimeThreshold) break; + } + + if (!result.has_value()) return std::unexpected { "no valid impact target" }; + + return *result; + } + +private: + static auto solve_aim_attitude(Eigen::Vector3d const& aim_point, double bullet_speed) + -> std::expected { + auto const target_d = std::hypot(aim_point.x(), aim_point.y()); + if (!(target_d > 0.0)) { + return std::unexpected { "invalid target distance" }; + } + + auto solution = TrajectorySolution {}; + solution.input.v0 = bullet_speed; + solution.input.target_position = aim_point; + + auto trajectory = solution.solve(); + if (!trajectory.has_value()) { + return std::unexpected { "trajectory solve failed" }; + } + + return AimAttitude { + .yaw = util::normalize_angle(trajectory->yaw), + .pitch = -trajectory->pitch, + .fly_time = trajectory->fly_time, + }; + } + + static auto sample_at(predictor::Snapshot const& snapshot, int armor_id, TimePoint t, + double bullet_speed) -> std::expected { + auto predicted_armors = snapshot.predicted_armors(t); + auto chosen_armor = std::optional {}; + for (auto const& armor : predicted_armors) { + if (armor.id != armor_id) continue; + chosen_armor = armor; + break; + } + if (!chosen_armor.has_value()) return std::unexpected { "locked armor id not found" }; + + auto aim_point = Eigen::Vector3d {}; + chosen_armor->translation.copy_to(aim_point); + + auto attitude = solve_aim_attitude(aim_point, bullet_speed); + if (!attitude.has_value()) return std::unexpected { attitude.error() }; + + return AimSample { + .armor = *chosen_armor, + .attitude = *attitude, + }; + } +}; + +TargetSolver::TargetSolver() noexcept + : pimpl { std::make_unique() } { } + +TargetSolver::~TargetSolver() noexcept = default; + +auto TargetSolver::make_candidates(predictor::Snapshot const& snapshot, TimePoint sample_time) const + -> std::vector { + return pimpl->make_candidates(snapshot, sample_time); +} + +auto TargetSolver::solve(predictor::Snapshot const& snapshot, int armor_id, TimePoint command_time, + double bullet_speed, double shoot_delay) const -> std::expected { + return pimpl->solve(snapshot, armor_id, command_time, bullet_speed, shoot_delay); +} diff --git a/src/module/fire_control/target_solver.hpp b/src/module/fire_control/target_solver.hpp new file mode 100644 index 00000000..4df68be6 --- /dev/null +++ b/src/module/fire_control/target_solver.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include +#include +#include + +#include "module/fire_control/types.hpp" +#include "module/predictor/snapshot.hpp" +#include "utility/pimpl.hpp" + +namespace rmcs::fire_control { + +class TargetSolver { + RMCS_PIMPL_DEFINITION(TargetSolver) + +public: + auto make_candidates(predictor::Snapshot const& snapshot, TimePoint sample_time) const + -> std::vector; + + auto solve(predictor::Snapshot const& snapshot, int armor_id, TimePoint command_time, + double bullet_speed, double shoot_delay) const -> std::expected; +}; + +} // namespace rmcs::fire_control diff --git a/src/module/fire_control/trajectory_solution.hpp b/src/module/fire_control/trajectory_solution.hpp index 6e03a960..75828254 100644 --- a/src/module/fire_control/trajectory_solution.hpp +++ b/src/module/fire_control/trajectory_solution.hpp @@ -15,8 +15,8 @@ struct TrajectorySolution { struct Output { double fly_time { 0. }; // s - double yaw { 0. }; // rad - double pitch { 0. }; // rad + double yaw { 0. }; // rad + double pitch { 0. }; // rad } result; auto solve() const -> std::optional; @@ -26,7 +26,7 @@ struct TrajectorySolution { -> std::tuple; const int kMaxIterateCount { 10 }; - const double kMaxPitchThreold { 57.3 / 57.3 }; + const double kMaxPitchThreold { 80.0 / 57.3 }; const double kEstimateDeltaTime { 0.005 }; const double kHeightErrorThreold { 0.001 }; const double kEstimateTimeOutThreold { 4.0 }; diff --git a/src/module/fire_control/types.hpp b/src/module/fire_control/types.hpp new file mode 100644 index 00000000..d83643df --- /dev/null +++ b/src/module/fire_control/types.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include "module/fire_control/gimbal_state.hpp" +#include "module/predictor/snapshot.hpp" +#include "utility/robot/armor.hpp" + +namespace rmcs::fire_control { + +struct AimAttitude { + double yaw { 0.0 }; + double pitch { 0.0 }; + double fly_time { 0.0 }; +}; + +struct ArmorCandidate { + Armor3d armor; + predictor::TargetMotion motion; +}; + +struct TargetSolution { + TimePoint impact_time; + ArmorCandidate candidate; + AimAttitude attitude; +}; + +} // namespace rmcs::fire_control diff --git a/src/module/predictor/backend/robot_state_backend.cpp b/src/module/predictor/backend/robot_state_backend.cpp deleted file mode 100644 index 6b2d01ae..00000000 --- a/src/module/predictor/backend/robot_state_backend.cpp +++ /dev/null @@ -1,47 +0,0 @@ -#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(TimePoint stamp) noexcept - : state { stamp } { } - - auto initialize(Armor3d const& armor, TimePoint t) -> void override { - state.initialize(armor, t); - } - - auto predict(TimePoint 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, TimePoint 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 deleted file mode 100644 index 90ffa75b..00000000 --- a/src/module/predictor/backend/robot_state_backend.hpp +++ /dev/null @@ -1,43 +0,0 @@ -#pragma once - -#include -#include -#include - -#include "module/predictor/snapshot.hpp" - -namespace rmcs::predictor { - -enum class RobotStateBackendKind : std::uint8_t { - Regular, - Outpost, -}; - -class IRobotStateBackend { -public: - virtual ~IRobotStateBackend() noexcept = default; - - virtual auto initialize(Armor3d const& armor, TimePoint t) -> void = 0; - virtual auto predict(TimePoint 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, TimePoint 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 deleted file mode 100644 index 6702d8a7..00000000 --- a/src/module/predictor/backend/snapshot_backend.hpp +++ /dev/null @@ -1,29 +0,0 @@ -#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; - TimePoint stamp; - - ISnapshotBackend(DeviceId device, CampColor color, int armor_num, TimePoint stamp) noexcept - : device { device } - , color { color } - , armor_num { armor_num } - , stamp { stamp } { } - - virtual ~ISnapshotBackend() noexcept = default; - - [[nodiscard]] virtual auto kinematics_at(TimePoint t) const -> Snapshot::Kinematics = 0; - [[nodiscard]] virtual auto predicted_armors(TimePoint t) const -> std::vector = 0; - - auto time_stamp() const -> TimePoint { return stamp; } -}; - -} // namespace rmcs::predictor diff --git a/src/module/predictor/outpost/armor_layout.hpp b/src/module/predictor/outpost/armor_layout.hpp index 6ea7e931..367e2fdc 100644 --- a/src/module/predictor/outpost/armor_layout.hpp +++ b/src/module/predictor/outpost/armor_layout.hpp @@ -11,7 +11,9 @@ struct OutpostArmorSlot { }; struct OutpostArmorLayout { - std::array slots {}; + static constexpr int kSlotCount = 3; + + std::array slots {}; }; } // namespace rmcs::predictor diff --git a/src/module/predictor/outpost/ekf_parameter.hpp b/src/module/predictor/outpost/ekf_parameter.hpp index 8353ecaa..0b085f48 100644 --- a/src/module/predictor/outpost/ekf_parameter.hpp +++ b/src/module/predictor/outpost/ekf_parameter.hpp @@ -1,9 +1,5 @@ #pragma once -#include -#include -#include - #include "module/predictor/outpost/armor_layout.hpp" #include "utility/math/angle.hpp" #include "utility/math/conversion.hpp" @@ -11,11 +7,21 @@ #include "utility/robot/armor.hpp" #include "utility/robot/constant.hpp" +#include +#include +#include + namespace rmcs::predictor { struct OutpostEKFParameters { using EKF = util::EKF<6, 4>; + struct ArmorObservation { + Eigen::Vector3d xyz; + Eigen::Vector3d ypr; + Eigen::Vector3d ypd; + }; + static constexpr int kOutpostArmorCount = 3; static constexpr double kPhaseStep = 2.0 * std::numbers::pi / kOutpostArmorCount; @@ -24,23 +30,18 @@ struct OutpostEKFParameters { // 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; + static auto x(ArmorObservation const& obs) -> EKF::XVec { + auto const yaw = obs.ypr[0]; + auto const center_x = obs.xyz[0] + kOutpostRadius * std::cos(yaw); + auto const center_y = obs.xyz[1] + kOutpostRadius * std::sin(yaw); + + auto x = EKF::XVec {}; + x << center_x, 0.0, center_y, 0.0, obs.xyz[2], yaw; return x; } static auto P_initial_dig() -> EKF::PDig { - auto P_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; } @@ -49,18 +50,9 @@ struct OutpostEKFParameters { 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 armor_yaw(EKF::XVec const& x, OutpostArmorLayout const& layout, int armor_id) + -> double { + return armor_yaw(x, layout.slots.at(armor_id).phase_offset); } static auto h_armor_xyz(EKF::XVec const& x, double phase_offset, double height_offset) @@ -68,33 +60,20 @@ struct OutpostEKFParameters { 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); + const auto pos_z = x[4] + 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 { }; + 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]); @@ -110,7 +89,7 @@ struct OutpostEKFParameters { } static auto F(double dt) -> EKF::AMat { - auto F = EKF::AMat { }; + auto F = EKF::AMat {}; // clang-format off F << 1, dt, 0, 0, 0, 0, @@ -126,19 +105,19 @@ struct OutpostEKFParameters { 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; + // 高度锚点 z 的过程噪声 + constexpr double z_process_noise_var = 1e-2; + // 参考装甲板 yaw 的过程噪声 + constexpr double yaw_process_noise_var = 1e-2; + const auto v1 = linear_acc_var; + const auto v2 = z_process_noise_var; + const auto v3 = yaw_process_noise_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 { }; + auto Q = EKF::QMat {}; // clang-format off Q << a * v1, b * v1, 0, 0, 0, 0, b * v1, c * v1, 0, 0, 0, 0, @@ -150,30 +129,39 @@ struct OutpostEKFParameters { 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); + static auto f(double dt, double angular_velocity) -> auto { + return [dt, angular_velocity](EKF::XVec const& x) { + EKF::XVec x_prior = F(dt) * x; + x_prior[5] = util::normalize_angle(x[5] + angular_velocity * 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]; + static auto observe(Armor3d const& armor) -> ArmorObservation { + 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 = util::eulers(orientation); + auto const ypd = util::xyz2ypd(xyz); + + return { xyz, ypr, ypd }; + } - auto R_dig = EKF::RDig { }; + static auto z(ArmorObservation const& obs) -> EKF::ZVec { + auto z = EKF::ZVec {}; + z << obs.ypd[0], obs.ypd[1], obs.ypd[2], obs.ypr[0]; + return z; + } + + static auto R(ArmorObservation const& obs) -> EKF::RMat { + const auto center_yaw = std::atan2(obs.xyz[1], obs.xyz[0]); + const auto delta_yaw = util::normalize_angle(obs.ypr[0] - center_yaw); + const auto distance = obs.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; @@ -189,7 +177,7 @@ struct OutpostEKFParameters { const auto dx_da = kOutpostRadius * sin_phase; const auto dy_da = -kOutpostRadius * cos_phase; - auto H_armor_xyza = Eigen::Matrix { }; + auto H_armor_xyza = Eigen::Matrix {}; // clang-format off H_armor_xyza << 1, 0, 0, 0, 0, dx_da, @@ -212,12 +200,6 @@ struct OutpostEKFParameters { 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 index 468c5550..9a6da539 100644 --- a/src/module/predictor/outpost/robot_state.cpp +++ b/src/module/predictor/outpost/robot_state.cpp @@ -1,426 +1,379 @@ #include "robot_state.hpp" +#include "module/predictor/outpost/snapshot.hpp" +#include "utility/math/angle.hpp" +#include "utility/time.hpp" + #include #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; -}; +#include -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 }; +namespace rmcs::predictor { - 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 }; -} +struct OutpostRobotState::Impl { + using Params = OutpostEKFParameters; + + enum class MotionMode { STATIC, CW, CCW }; + + struct Candidate { + int slot_id { 0 }; + double phase_offset { 0.0 }; + double height_offset { 0.0 }; + bool assigned { false }; + std::optional motion_mode; + }; + + struct MatchResult { + Params::ArmorObservation observation; + Candidate candidate; + double reference_yaw { 0.0 }; + double score { std::numeric_limits::infinity() }; + }; + + struct RotationTopology { + double angular_velocity { 0.0 }; + int slot_delta { 0 }; + double phase_delta { 0.0 }; + std::array height_steps {}; + }; + + struct RotationEvidence { + std::optional mode; + int count { 0 }; + }; -enum class SwitchEvent { - Stay, - ClockwiseSwitch, - CounterClockwiseSwitch, - Invalid, -}; + explicit Impl(TimePoint stamp) noexcept + : time_stamp { stamp } { } -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 }; -}; + auto initialize(Armor3d const& armor, TimePoint t) -> void { + color = armor_color2camp_color(armor.color); + time_stamp = t; + update_count = 0; + motion_mode = std::nullopt; + rotation_evidence = {}; -struct BestMatch { - OutpostObservation observation; - AssociationDecision decision; -}; + layout = OutpostArmorLayout {}; + assign_slot(0, 0.0, 0.0); -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 }; -}; + ekf = EKF { Params::x(Params::observe(armor)), Params::P_initial_dig().asDiagonal() }; + initialized = true; -struct TrackingConfig { - std::chrono::duration reset_interval { 1.5 }; - int spin_confirm_switches { 2 }; - int min_converged_updates { 6 }; - MatchingConfig matching { }; -}; + auto const observation = Params::observe(armor); + mode_reference_yaw = observation.ypr[0]; + mode_reference_stamp = t; + } -struct SpinTracker { - int locked_sign { 0 }; - int candidate_sign { 0 }; - int candidate_count { 0 }; - bool locked { false }; + auto predict(TimePoint t) -> void { + if (t <= time_stamp) return; - auto reset() -> void { *this = { }; } + if (initialized) { + auto const dt = util::delta_time(t, time_stamp); + auto const dt_s = dt.count(); + ekf.predict( + Params::f(dt_s, angular_velocity()), + [dt_s](EKF::XVec const&) { return Params::F(dt_s); }, Params::Q(dt_s)); + } - auto current_sign() const -> int { - if (locked) return locked_sign; - return candidate_sign; + time_stamp = t; } - 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; + auto update(std::span armors) -> bool { + if (armors.empty()) return false; - if (candidate_sign == normalized) { - candidate_count++; - } else { - candidate_sign = normalized; - candidate_count = 1; + if (!initialized) { + initialize(armors.front(), time_stamp); + ++update_count; + return true; } - if (candidate_count < confirm_switches) return; + auto match = select_best_match(armors); + if (!match.has_value()) return false; - locked_sign = candidate_sign; - locked = true; + if (!apply_match(*match)) return false; + + ++update_count; + return true; } -}; -auto assigned_count(OutpostArmorLayout const& layout) -> int { - return static_cast(std::ranges::count(layout.slots, true, &OutpostArmorSlot::assigned)); -} + auto is_converged() const -> bool { + if (!initialized) return false; + if (!std::isfinite(distance())) return false; + if (!motion_mode.has_value()) return false; -auto has_assigned_slot(OutpostArmorLayout const& layout, int armor_id) -> bool { - return armor_id >= 0 && armor_id < OutpostEKFParameters::kOutpostArmorCount - && layout.slots[armor_id].assigned; -} + constexpr int min_updates = 3; + return layout.slots[0].assigned && update_count >= min_updates; + } -auto first_unassigned_slot(OutpostArmorLayout const& layout) -> int { - for (int id = 0; id < OutpostEKFParameters::kOutpostArmorCount; ++id) { - if (!layout.slots[id].assigned) return id; + auto get_snapshot() const -> std::optional { + if (!initialized) return std::nullopt; + return Snapshot { OutpostSnapshot { + ekf.x, color, time_stamp, layout, angular_velocity() } }; } - return kUnknownArmorId; -} -auto layout_after_association(OutpostArmorLayout const& layout, AssociationDecision const& decision) - -> OutpostArmorLayout { - auto next_layout = layout; - if (!decision.extends_layout) return next_layout; + auto distance() const -> double { + if (!initialized) return std::numeric_limits::infinity(); + return std::hypot(ekf.x[0], ekf.x[2]); + } - 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; -} +private: + static constexpr auto kModeConfirmWindow = std::chrono::duration { 0.2 }; + static constexpr auto kStaticYawDeltaThreshold = util::deg2rad(5.0); + static constexpr auto kRotationYawDeltaThreshold = util::deg2rad(15.0); + static constexpr int kRotationConfirmCandidateCount = 3; + + auto assign_slot(int slot_id, double phase_offset, double height_offset) -> void { + auto& slot = layout.slots.at(slot_id); + slot.phase_offset = phase_offset; + slot.height_offset = height_offset; + slot.assigned = true; + } -auto switch_phase_delta(int inferred_spin_sign) -> double { - return -normalize_spin_sign(inferred_spin_sign) * OutpostEKFParameters::kPhaseStep; -} + auto is_layout_consistent() const -> bool { + if (!layout.slots[0].assigned || !layout.slots[1].assigned || !layout.slots[2].assigned) + return true; -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 height_step = [](double height_offset) { + return static_cast(std::round(height_offset / kOutpostArmorHeightStep)); + }; -auto consider_candidate(AssociationDecision const& candidate, AssociationDecision& best_decision) - -> void { - if (!candidate.is_valid || candidate.error >= best_decision.error) return; - best_decision = candidate; -} + auto s0 = height_step(layout.slots[0].height_offset); + auto s1 = height_step(layout.slots[1].height_offset); + auto s2 = height_step(layout.slots[2].height_offset); -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); - } + auto min_step = std::min({ s0, s1, s2 }); + auto max_step = std::max({ s0, s1, s2 }); - return best_decision; + return max_step - min_step == 2 && s0 != s1 && s1 != s2 && s0 != s2; } -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 { }; + static constexpr auto topology_of(MotionMode mode) -> std::optional { + switch (mode) { + case MotionMode::CW: + return RotationTopology { + .angular_velocity = -kOutpostAngularSpeed, + .slot_delta = 1, + .phase_delta = Params::kPhaseStep, + .height_steps = { -1.0, 2.0 }, + }; + case MotionMode::CCW: + return RotationTopology { + .angular_velocity = kOutpostAngularSpeed, + .slot_delta = -1, + .phase_delta = -Params::kPhaseStep, + .height_steps = { 1.0, -2.0 }, + }; + case MotionMode::STATIC: + return std::nullopt; } - 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 }; + return std::nullopt; } - 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 angular_velocity() const -> double { + if (!motion_mode.has_value()) return 0.0; + auto const topology = topology_of(*motion_mode); + if (!topology.has_value()) return 0.0; + return topology->angular_velocity; + } - 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; + static constexpr auto normalize_slot_id(int slot_id) -> int { + constexpr int slot_count = Params::kOutpostArmorCount; + return (slot_id + slot_count) % slot_count; + } - auto const mismatch = phase_error + height_error / rmcs::kOutpostArmorHeightStep; - if (mismatch >= best_mismatch) continue; + auto make_candidates() const -> std::vector { + auto candidates = std::vector {}; + candidates.reserve(Params::kOutpostArmorCount * 3); + + for (int slot_id = 0; slot_id < Params::kOutpostArmorCount; ++slot_id) { + auto const& slot = layout.slots.at(slot_id); + if (!slot.assigned) continue; + + candidates.emplace_back(Candidate { + .slot_id = slot_id, + .phase_offset = slot.phase_offset, + .height_offset = slot.height_offset, + .assigned = true, + .motion_mode = motion_mode, + }); + } - best_mismatch = mismatch; - best_slot = id; + if (motion_mode.has_value()) { + if (*motion_mode == MotionMode::STATIC) return candidates; + append_next_slot_candidates(candidates, *motion_mode); + return candidates; } - return best_slot; + append_next_slot_candidates(candidates, MotionMode::CW); + append_next_slot_candidates(candidates, MotionMode::CCW); + return candidates; } - 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); + auto append_next_slot_candidates(std::vector& candidates, MotionMode mode) const + -> void { + auto const topology = topology_of(mode); + if (!topology.has_value()) return; + + for (int source_slot_id = 0; source_slot_id < Params::kOutpostArmorCount; + ++source_slot_id) { + auto const& source_slot = layout.slots.at(source_slot_id); + if (!source_slot.assigned) continue; + + auto const target_slot_id = normalize_slot_id(source_slot_id + topology->slot_delta); + auto const& target_slot = layout.slots.at(target_slot_id); + if (target_slot.assigned) continue; + + for (auto const height_step : topology->height_steps) { + auto const phase_offset = + util::normalize_angle(source_slot.phase_offset + topology->phase_delta); + auto const height_offset = + source_slot.height_offset + height_step * kOutpostArmorHeightStep; + + candidates.emplace_back(Candidate { + .slot_id = target_slot_id, + .phase_offset = phase_offset, + .height_offset = height_offset, + .assigned = false, + .motion_mode = mode, + }); } - 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(TimePoint stamp) noexcept - : time_stamp { stamp } { } + auto select_best_match(std::span armors) const -> std::optional { + auto const candidates = make_candidates(); + if (candidates.empty()) return std::nullopt; - auto initialize(Armor3d const& armor, TimePoint t) -> void { - color = armor_color2camp_color(armor.color); - ekf = EKF { OutpostEKFParameters::x(armor), - OutpostEKFParameters::P_initial_dig().asDiagonal() }; - time_stamp = t; + auto best_match = std::optional {}; - layout = OutpostArmorLayout { }; - layout.slots[0].assigned = true; + for (auto const& armor : armors) { + auto const observation = Params::observe(armor); + for (auto const& candidate : candidates) { + // 预测装甲板位置 + auto const predicted_xyz = + Params::h_armor_xyz(ekf.x, candidate.phase_offset, candidate.height_offset); + auto const predicted_yaw = Params::armor_yaw(ekf.x, candidate.phase_offset); + + // 计算匹配分数 + auto const position_error = (observation.xyz - predicted_xyz).norm(); + auto const yaw_error = + std::abs(util::normalize_angle(observation.ypr[0] - predicted_yaw)); + auto const score = position_error + kOutpostRadius * yaw_error; + + if (best_match.has_value() && score >= best_match->score) continue; + + best_match = MatchResult { + .observation = observation, + .candidate = candidate, + .reference_yaw = + util::normalize_angle(observation.ypr[0] - candidate.phase_offset), + .score = score, + }; + } + } - spin.reset(); - update_count = 0; - current_armor_id = 0; - initialized = true; + return best_match; } - auto predict(TimePoint t) -> void { - if (t <= time_stamp) return; + auto apply_match(MatchResult const& match) -> bool { + auto const& candidate = match.candidate; - 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)); + if (!motion_mode.has_value() && !candidate.assigned && candidate.motion_mode.has_value()) { + update_rotation_evidence(*candidate.motion_mode); + return false; } - time_stamp = t; - } + rotation_evidence = {}; - auto update(std::span armors) -> bool { - if (armors.empty()) return false; + assign_slot(candidate.slot_id, candidate.phase_offset, candidate.height_offset); - if (!initialized) { - initialize(armors.front(), time_stamp); - return true; + if (!is_layout_consistent()) { + initialized = false; + return false; } - auto best_match = select_best_match(armors); - if (!best_match.has_value()) return false; + update_motion_mode(match); + + ekf.update( + Params::z(match.observation), + [candidate](EKF::XVec const& x) { + return Params::h(x, candidate.phase_offset, candidate.height_offset); + }, + [candidate](EKF::XVec const& x) { + return Params::H(x, candidate.phase_offset, candidate.height_offset); + }, + Params::R(match.observation), Params::x_add, Params::z_subtract); - 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 update_rotation_evidence(MotionMode candidate_mode) -> void { + if (rotation_evidence.mode == candidate_mode) { + ++rotation_evidence.count; + } else { + rotation_evidence = { + .mode = candidate_mode, + .count = 1, + }; + } - auto get_snapshot() const -> Snapshot { - return detail::make_outpost_snapshot( - ekf.x, color, assigned_count(layout), time_stamp, spin.current_sign(), layout); + if (rotation_evidence.count >= kRotationConfirmCandidateCount) { + motion_mode = candidate_mode; + rotation_evidence = {}; + } } - auto distance() const -> double { return std::sqrt(ekf.x[0] * ekf.x[0] + ekf.x[2] * ekf.x[2]); } - -private: - auto reset_runtime_state(TimePoint t) -> void { - color = CampColor::UNKNOWN; - ekf = EKF { }; - layout = OutpostArmorLayout { }; - time_stamp = t; - initialized = false; - current_armor_id = kUnknownArmorId; - spin.reset(); - update_count = 0; - } + auto update_motion_mode(MatchResult const& match) -> void { + // 使用拓扑候选直接确认方向 + if (!motion_mode.has_value() && match.candidate.motion_mode.has_value()) { + motion_mode = *match.candidate.motion_mode; + return; + } - 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 }; + // 按时间窗口判断 yaw 变化 + auto const elapsed = util::delta_time(time_stamp, mode_reference_stamp); + if (elapsed < kModeConfirmWindow) return; - 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; + auto const mode_delta = util::normalize_angle(match.reference_yaw - mode_reference_yaw); + auto const abs_delta = std::abs(mode_delta); - best_match = BestMatch { observation, decision }; + // 根据 yaw 变化推断当前观测到的运动模式 + auto observed_motion_mode = std::optional {}; + if (abs_delta < kStaticYawDeltaThreshold) observed_motion_mode = MotionMode::STATIC; + else if (abs_delta > kRotationYawDeltaThreshold) { + observed_motion_mode = mode_delta > 0.0 ? MotionMode::CCW : MotionMode::CW; } - 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); + // 应用观测结果 + if (observed_motion_mode.has_value()) { + if (!motion_mode.has_value()) { + motion_mode = *observed_motion_mode; + } else if (*motion_mode != *observed_motion_mode) { + motion_mode = std::nullopt; + rotation_evidence = {}; + } } - update_count++; + + mode_reference_yaw = match.reference_yaw; + mode_reference_stamp = time_stamp; } CampColor color { CampColor::UNKNOWN }; - EKF ekf { EKF { } }; - OutpostArmorLayout layout { }; + OutpostArmorLayout layout {}; + EKF ekf { EKF {} }; TimePoint time_stamp; bool initialized { false }; - int current_armor_id { kUnknownArmorId }; - SpinTracker spin { }; int update_count { 0 }; - TrackingConfig config { }; + + std::optional motion_mode; + RotationEvidence rotation_evidence; + double mode_reference_yaw { 0.0 }; + TimePoint mode_reference_stamp; }; OutpostRobotState::OutpostRobotState() noexcept @@ -443,6 +396,10 @@ auto OutpostRobotState::update(std::span armors) -> bool { auto OutpostRobotState::is_converged() const -> bool { return pimpl->is_converged(); } -auto OutpostRobotState::get_snapshot() const -> Snapshot { return pimpl->get_snapshot(); } +auto OutpostRobotState::get_snapshot() const -> std::optional { + return pimpl->get_snapshot(); +} auto OutpostRobotState::distance() const -> double { return pimpl->distance(); } + +} // namespace rmcs::predictor diff --git a/src/module/predictor/outpost/robot_state.hpp b/src/module/predictor/outpost/robot_state.hpp index 734bdd01..b8f3d924 100644 --- a/src/module/predictor/outpost/robot_state.hpp +++ b/src/module/predictor/outpost/robot_state.hpp @@ -1,16 +1,18 @@ #pragma once -#include -#include - #include "module/predictor/outpost/ekf_parameter.hpp" #include "module/predictor/snapshot.hpp" #include "utility/clock.hpp" #include "utility/pimpl.hpp" +#include +#include + namespace rmcs::predictor { class OutpostRobotState { + RMCS_PIMPL_DEFINITION(OutpostRobotState) + public: using EKF = OutpostEKFParameters::EKF; @@ -22,10 +24,8 @@ class OutpostRobotState { auto update(std::span armors) -> bool; auto is_converged() const -> bool; - auto get_snapshot() const -> Snapshot; + auto get_snapshot() const -> std::optional; 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 index 806dca80..a5e55552 100644 --- a/src/module/predictor/outpost/snapshot.cpp +++ b/src/module/predictor/outpost/snapshot.cpp @@ -1,102 +1,91 @@ #include "module/predictor/outpost/snapshot.hpp" -#include -#include - -#include "module/predictor/backend/snapshot_backend.hpp" -#include "module/predictor/outpost/ekf_parameter.hpp" #include "utility/math/conversion.hpp" #include "utility/time.hpp" -namespace rmcs::predictor { - -namespace { +#include +#include - auto normalize_spin_sign(int spin_sign) -> int { - if (spin_sign > 0) return +1; - if (spin_sign < 0) return -1; - return 0; - } +namespace rmcs::predictor { - auto make_armor(DeviceId device, CampColor color, int id) -> Armor3d { - auto armor = Armor3d { }; +struct OutpostSnapshot::Impl { + explicit Impl(EKF::XVec x, CampColor color, TimePoint stamp, OutpostArmorLayout layout, + double angular_velocity) + : x { std::move(x) } + , color { color } + , stamp { stamp } + , layout { layout } + , angular_velocity { angular_velocity } { } + + static 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(detail::OutpostEKF::XVec x, CampColor color, - int armor_num, TimePoint 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(TimePoint t) const -> Snapshot::Kinematics override { - return kinematics_of(predict_state_at(t)); - } + static auto motion_of(EKF::XVec const& x, double angular_velocity) -> TargetMotion { + return { + Eigen::Vector3d { x[0], x[2], x[4] }, + angular_velocity, + }; + } - [[nodiscard]] auto predicted_armors(TimePoint 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 predict_state_at(TimePoint t) const -> EKF::XVec { + auto const dt = util::delta_time(t, stamp).count(); + return OutpostEKFParameters::f(dt, angular_velocity)(x); + } - auto armors = std::vector { }; - armors.reserve(max_armors); + auto predicted_armors(TimePoint t) const -> std::vector { + auto const predicted_x = predict_state_at(t); - for (int id = 0; id < max_armors; ++id) { - if (!layout.slots[id].assigned) continue; + auto armors = std::vector {}; + armors.reserve(OutpostEKFParameters::kOutpostArmorCount); - 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); + for (int id = 0; id < OutpostEKFParameters::kOutpostArmorCount; ++id) { + auto const& slot = layout.slots[id]; + if (!slot.assigned) continue; - armor.translation = position; - armor.orientation = - util::euler_to_quaternion(angle, kPredictedOutpostArmorPitch, 0); - armors.emplace_back(armor); - } + auto armor = make_armor(DeviceId::OUTPOST, color, id); + auto const angle = OutpostEKFParameters::armor_yaw(predicted_x, layout, id); + auto const position = OutpostEKFParameters::h_armor_xyz( + predicted_x, slot.phase_offset, slot.height_offset); - return armors; + armor.translation = position; + armor.orientation = util::euler_to_quaternion(angle, kPredictedOutpostArmorPitch, 0); + armors.emplace_back(armor); } - private: - auto kinematics_of(detail::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 { Point3d { x[0], x[2], center_z }, angular_velocity }; - } + return armors; + } - auto predict_state_at(TimePoint t) const -> detail::OutpostEKF::XVec { - auto const dt = util::delta_time(t, stamp).count(); - return OutpostEKFParameters::f(dt, spin_sign)(x); - } + EKF::XVec x; + CampColor color; + TimePoint stamp; + OutpostArmorLayout layout; + double angular_velocity { 0.0 }; +}; + +OutpostSnapshot::OutpostSnapshot(EKF::XVec x, CampColor color, TimePoint stamp, + OutpostArmorLayout layout, double angular_velocity) + : pimpl { std::make_unique(std::move(x), color, stamp, layout, angular_velocity) } { } - detail::OutpostEKF::XVec x; - int spin_sign; - OutpostArmorLayout layout; - }; +OutpostSnapshot::OutpostSnapshot(OutpostSnapshot&&) noexcept = default; +auto OutpostSnapshot::operator=(OutpostSnapshot&&) noexcept -> OutpostSnapshot& = default; -} // namespace +OutpostSnapshot::~OutpostSnapshot() noexcept = default; + +auto OutpostSnapshot::time_stamp() const -> TimePoint { return pimpl->stamp; } + +auto OutpostSnapshot::device_id() const -> DeviceId { return DeviceId::OUTPOST; } + +auto OutpostSnapshot::motion_at(TimePoint t) const -> TargetMotion { + return Impl::motion_of(pimpl->predict_state_at(t), pimpl->angular_velocity); +} -auto detail::make_outpost_snapshot(detail::OutpostEKF::XVec ekf_x, CampColor color, int armor_num, - TimePoint 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)); +auto OutpostSnapshot::predicted_armors(TimePoint t) const -> std::vector { + return pimpl->predicted_armors(t); } } // namespace rmcs::predictor diff --git a/src/module/predictor/outpost/snapshot.hpp b/src/module/predictor/outpost/snapshot.hpp index d74acdef..22f60e7d 100644 --- a/src/module/predictor/outpost/snapshot.hpp +++ b/src/module/predictor/outpost/snapshot.hpp @@ -1,15 +1,34 @@ #pragma once #include "module/predictor/outpost/armor_layout.hpp" +#include "module/predictor/outpost/ekf_parameter.hpp" #include "module/predictor/snapshot.hpp" -#include "utility/math/kalman_filter/ekf.hpp" -#include "utility/robot/color.hpp" -namespace rmcs::predictor::detail { +#include +#include -using OutpostEKF = util::EKF<6, 4>; +namespace rmcs::predictor { -auto make_outpost_snapshot(OutpostEKF::XVec ekf_x, CampColor color, int armor_num, TimePoint stamp, - int outpost_spin_sign, OutpostArmorLayout outpost_layout) noexcept -> Snapshot; +class OutpostSnapshot { +public: + using EKF = OutpostEKFParameters::EKF; -} // namespace rmcs::predictor::detail + explicit OutpostSnapshot(EKF::XVec x, CampColor color, TimePoint stamp, + OutpostArmorLayout layout, double angular_velocity); + OutpostSnapshot(OutpostSnapshot const&) = delete; + OutpostSnapshot(OutpostSnapshot&&) noexcept; + OutpostSnapshot& operator=(OutpostSnapshot const&) = delete; + OutpostSnapshot& operator=(OutpostSnapshot&&) noexcept; + ~OutpostSnapshot() noexcept; + + auto time_stamp() const -> TimePoint; + auto device_id() const -> DeviceId; + auto motion_at(TimePoint t) const -> TargetMotion; + auto predicted_armors(TimePoint t) const -> std::vector; + +private: + struct Impl; + std::unique_ptr pimpl; +}; + +} // namespace rmcs::predictor diff --git a/src/module/predictor/regular/robot_state.cpp b/src/module/predictor/regular/robot_state.cpp index 23a6ed8e..cc179bf4 100644 --- a/src/module/predictor/regular/robot_state.cpp +++ b/src/module/predictor/regular/robot_state.cpp @@ -1,15 +1,15 @@ #include "robot_state.hpp" +#include "module/predictor/regular/snapshot.hpp" +#include "utility/time.hpp" + #include #include #include #include #include -#include "module/predictor/regular/snapshot.hpp" -#include "utility/time.hpp" - -using namespace rmcs::predictor; +namespace rmcs::predictor { struct RegularRobotState::Impl { struct MatchDecision { @@ -27,7 +27,7 @@ struct RegularRobotState::Impl { CampColor color { CampColor::UNKNOWN }; int armor_num { 0 }; - EKF ekf { EKF { } }; + EKF ekf { EKF {} }; TimePoint time_stamp; bool initialized { false }; @@ -107,9 +107,9 @@ struct RegularRobotState::Impl { 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 get_snapshot() const -> std::optional { + if (!initialized) return std::nullopt; + return Snapshot { RegularSnapshot { ekf.x, device, color, armor_num, time_stamp } }; } auto distance() const -> double { @@ -119,7 +119,7 @@ struct RegularRobotState::Impl { private: auto decide_match(Armor3d const& armor) const -> MatchDecision { - if (!initialized || armor.genre != device) return { }; + if (!initialized || armor.genre != device) return {}; auto armors_xyza = calculate_armors(ekf.x); @@ -139,8 +139,10 @@ struct RegularRobotState::Impl { return (last_matched_armor_id + armor_num / 2) % armor_num; }(); - for (auto&& [candidate_armor_id, pred] : armors_xyza | std::views::enumerate) { - if (candidate_armor_id == opposite_matched_armor_id) continue; + auto candidate_armor_id = 0; + for (auto const& pred : armors_xyza) { + auto const armor_id = candidate_armor_id++; + if (armor_id == opposite_matched_armor_id) continue; auto const ypd_pred = util::xyz2ypd(pred.template head<3>()); auto const view_delta = std::abs(util::normalize_angle(pred[3] - ypd_pred[0])); @@ -150,11 +152,11 @@ struct RegularRobotState::Impl { + std::abs(util::normalize_angle(ypd_in_world[0] - ypd_pred[0])); if (error >= min_error) continue; - best_matched_armor_id = candidate_armor_id; + best_matched_armor_id = armor_id; min_error = error; } - if (best_matched_armor_id == kUnknownMatchedArmorId) return { }; + if (best_matched_armor_id == kUnknownMatchedArmorId) return {}; return { .matched_armor_id = best_matched_armor_id, @@ -164,7 +166,7 @@ struct RegularRobotState::Impl { } auto select_best_match(std::span armors) const -> std::optional { - auto best_match = std::optional { }; + auto best_match = std::optional {}; for (std::size_t observation_index = 0; observation_index < armors.size(); ++observation_index) { auto decision = decide_match(armors[observation_index]); @@ -191,7 +193,7 @@ struct RegularRobotState::Impl { auto const orientation = Eigen::Quaterniond { quat_w, quat_x, quat_y, quat_z }; auto const ypr = util::eulers(orientation); - auto z = EKF::ZVec { }; + auto z = EKF::ZVec {}; z << ypd[0], ypd[1], ypd[2], ypr[0]; ekf.update( @@ -206,7 +208,7 @@ struct RegularRobotState::Impl { } auto calculate_armors(EKF::XVec const& x) const -> std::vector { - auto armors = 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); @@ -239,6 +241,10 @@ auto RegularRobotState::update(std::span armors) -> bool { auto RegularRobotState::is_converged() const -> bool { return pimpl->is_converged(); } -auto RegularRobotState::get_snapshot() const -> Snapshot { return pimpl->get_snapshot(); } +auto RegularRobotState::get_snapshot() const -> std::optional { + return pimpl->get_snapshot(); +} auto RegularRobotState::distance() const -> double { return pimpl->distance(); } + +} // namespace rmcs::predictor diff --git a/src/module/predictor/regular/robot_state.hpp b/src/module/predictor/regular/robot_state.hpp index ecbe8896..de7b93b6 100644 --- a/src/module/predictor/regular/robot_state.hpp +++ b/src/module/predictor/regular/robot_state.hpp @@ -3,6 +3,7 @@ #include "module/predictor/snapshot.hpp" #include "utility/pimpl.hpp" +#include #include namespace rmcs::predictor { @@ -24,7 +25,7 @@ class RegularRobotState { auto update(std::span armors) -> bool; auto is_converged() const -> bool; - auto get_snapshot() const -> Snapshot; + auto get_snapshot() const -> std::optional; auto distance() const -> double; }; diff --git a/src/module/predictor/regular/snapshot.cpp b/src/module/predictor/regular/snapshot.cpp index 12d3290e..ba70b2c5 100644 --- a/src/module/predictor/regular/snapshot.cpp +++ b/src/module/predictor/regular/snapshot.cpp @@ -1,17 +1,22 @@ #include "module/predictor/regular/snapshot.hpp" -#include - -#include "module/predictor/backend/snapshot_backend.hpp" -#include "module/predictor/regular/ekf_parameter.hpp" #include "utility/math/conversion.hpp" #include "utility/time.hpp" +#include +#include + namespace rmcs::predictor { -namespace { +struct RegularSnapshot::Impl { + explicit Impl(EKF::XVec x, DeviceId device, CampColor color, int armor_num, TimePoint stamp) + : x { std::move(x) } + , device { device } + , color { color } + , armor_num { armor_num } + , stamp { stamp } { } - auto make_armor(DeviceId device, CampColor color, int id) -> Armor3d { + static auto make_armor(DeviceId device, CampColor color, int id) -> Armor3d { auto armor = Armor3d { }; armor.genre = device; armor.color = camp_color2armor_color(color); @@ -19,55 +24,60 @@ namespace { return armor; } - struct RegularSnapshotBackend final : ISnapshotBackend { - explicit RegularSnapshotBackend(detail::NormalEKF::XVec x, DeviceId device, CampColor color, - int armor_num, TimePoint stamp) noexcept - : ISnapshotBackend { device, color, armor_num, stamp } - , x { std::move(x) } { } - - [[nodiscard]] auto kinematics_at(TimePoint t) const -> Snapshot::Kinematics override { - return kinematics_of(predict_state_at(t)); - } + static auto motion_of(EKF::XVec const& x) -> TargetMotion { + return { Eigen::Vector3d { x[0], x[2], x[4] }, x[7] }; + } - [[nodiscard]] auto predicted_armors(TimePoint t) const -> std::vector override { - auto const predicted_x = predict_state_at(t); + auto predict_state_at(TimePoint t) const -> EKF::XVec { + auto const dt = util::delta_time(t, stamp).count(); + return EKFParameters::f(dt)(x); + } - auto armors = std::vector { }; - armors.reserve(armor_num); + auto predicted_armors(TimePoint t) const -> std::vector { + auto const predicted_x = predict_state_at(t); - 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); + auto armors = std::vector { }; + armors.reserve(armor_num); - armor.translation = position; - armor.orientation = util::euler_to_quaternion(angle, kPredictedOtherArmorPitch, 0); - armors.emplace_back(armor); - } + 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); - return armors; + armor.translation = position; + armor.orientation = util::euler_to_quaternion(angle, kPredictedOtherArmorPitch, 0); + armors.emplace_back(armor); } - private: - static auto kinematics_of(detail::NormalEKF::XVec const& x) -> Snapshot::Kinematics { - return { Point3d { x[0], x[2], x[4] }, x[7] }; - } + return armors; + } - auto predict_state_at(TimePoint t) const -> detail::NormalEKF::XVec { - auto const dt = util::delta_time(t, stamp).count(); - return EKFParameters::f(dt)(x); - } + EKF::XVec x; + DeviceId device; + CampColor color; + int armor_num; + TimePoint stamp; +}; + +RegularSnapshot::RegularSnapshot( + EKF::XVec x, DeviceId device, CampColor color, int armor_num, TimePoint stamp) + : pimpl { std::make_unique(std::move(x), device, color, armor_num, stamp) } { } - detail::NormalEKF::XVec x; - }; +RegularSnapshot::RegularSnapshot(RegularSnapshot&&) noexcept = default; +auto RegularSnapshot::operator=(RegularSnapshot&&) noexcept -> RegularSnapshot& = default; -} // namespace +RegularSnapshot::~RegularSnapshot() noexcept = default; + +auto RegularSnapshot::time_stamp() const -> TimePoint { return pimpl->stamp; } + +auto RegularSnapshot::device_id() const -> DeviceId { return pimpl->device; } + +auto RegularSnapshot::motion_at(TimePoint t) const -> TargetMotion { + return Impl::motion_of(pimpl->predict_state_at(t)); +} -auto detail::make_regular_snapshot(NormalEKF::XVec ekf_x, DeviceId device, CampColor color, - int armor_num, TimePoint stamp) noexcept -> Snapshot { - return detail::make_snapshot(std::make_unique( - std::move(ekf_x), device, color, armor_num, stamp)); +auto RegularSnapshot::predicted_armors(TimePoint t) const -> std::vector { + return pimpl->predicted_armors(t); } } // namespace rmcs::predictor diff --git a/src/module/predictor/regular/snapshot.hpp b/src/module/predictor/regular/snapshot.hpp index a275c9f2..5a10fe8b 100644 --- a/src/module/predictor/regular/snapshot.hpp +++ b/src/module/predictor/regular/snapshot.hpp @@ -1,16 +1,33 @@ #pragma once +#include "module/predictor/regular/ekf_parameter.hpp" #include "module/predictor/snapshot.hpp" -#include "utility/math/kalman_filter/ekf.hpp" -#include "utility/robot/color.hpp" -#include "utility/robot/id.hpp" -namespace rmcs::predictor::detail { +#include +#include -using NormalEKF = util::EKF<11, 4>; -using OutpostEKF = util::EKF<6, 4>; +namespace rmcs::predictor { -auto make_regular_snapshot(NormalEKF::XVec ekf_x, DeviceId device, CampColor color, int armor_num, - TimePoint stamp) noexcept -> Snapshot; +class RegularSnapshot { +public: + using EKF = EKFParameters::EKF; -} // namespace rmcs::predictor::detail + explicit RegularSnapshot(EKF::XVec x, DeviceId device, CampColor color, int armor_num, + TimePoint stamp); + RegularSnapshot(RegularSnapshot const&) = delete; + RegularSnapshot(RegularSnapshot&&) noexcept; + RegularSnapshot& operator=(RegularSnapshot const&) = delete; + RegularSnapshot& operator=(RegularSnapshot&&) noexcept; + ~RegularSnapshot() noexcept; + + auto time_stamp() const -> TimePoint; + auto device_id() const -> DeviceId; + auto motion_at(TimePoint t) const -> TargetMotion; + auto predicted_armors(TimePoint t) const -> std::vector; + +private: + struct Impl; + std::unique_ptr pimpl; +}; + +} // namespace rmcs::predictor diff --git a/src/module/predictor/robot_state.cpp b/src/module/predictor/robot_state.cpp index 0ae235f8..b935653c 100644 --- a/src/module/predictor/robot_state.cpp +++ b/src/module/predictor/robot_state.cpp @@ -1,51 +1,62 @@ #include "robot_state.hpp" -#include "module/predictor/backend/robot_state_backend.hpp" + +#include "module/predictor/outpost/robot_state.hpp" +#include "module/predictor/regular/robot_state.hpp" + +#include +#include +#include using namespace rmcs::predictor; struct RobotState::Impl { - std::unique_ptr backend { }; - TimePoint pending_time_stamp { Clock::now() }; - - [[nodiscard]] static auto make_backend(DeviceId device, TimePoint stamp) - -> std::unique_ptr { - auto const kind = classify_robot_state_backend(device); - return make_robot_state_backend(kind, stamp); - } + using State = std::variant; - auto reset_backend(Armor3d const& armor, TimePoint stamp) -> void { - backend = make_backend(armor.genre, stamp); - pending_time_stamp = stamp; - } + std::optional state; + TimePoint pending_time_stamp { Clock::now() }; - auto ensure_backend(Armor3d const& armor) -> void { - if (backend) return; - backend = make_backend(armor.genre, pending_time_stamp); + auto emplace_state(DeviceId device, TimePoint stamp) -> State& { + switch (device) { + case DeviceId::OUTPOST: + return state.emplace(std::in_place_type, stamp); + default: + return state.emplace(std::in_place_type, stamp); + } } auto initialize(Armor3d const& armor, TimePoint t) -> void { - reset_backend(armor, t); - backend->initialize(armor, t); + pending_time_stamp = t; + auto& target_state = emplace_state(armor.genre, t); + std::visit([&](auto& model) { model.initialize(armor, t); }, target_state); } auto predict(TimePoint t) -> void { pending_time_stamp = t; - if (backend) backend->predict(t); + if (!state) return; + std::visit([t](auto& model) { model.predict(t); }, *state); } auto update(std::span armors) -> bool { if (armors.empty()) return false; - ensure_backend(armors.front()); - return backend ? backend->update(armors) : false; + auto& target_state = + state ? *state : emplace_state(armors.front().genre, pending_time_stamp); + return std::visit([armors](auto& model) { return model.update(armors); }, target_state); } - auto is_converged() const -> bool { return backend ? backend->is_converged() : false; } + auto is_converged() const -> bool { + if (!state) return false; + return std::visit([](auto const& model) { return model.is_converged(); }, *state); + } - auto get_snapshot() const -> Snapshot { - return backend ? backend->get_snapshot() : Snapshot::empty(pending_time_stamp); + auto get_snapshot() const -> std::optional { + if (!state) return std::nullopt; + return std::visit([](auto const& model) { return model.get_snapshot(); }, *state); } - auto distance() const -> double { return backend ? backend->distance() : 0.0; } + auto distance() const -> double { + if (!state) return std::numeric_limits::infinity(); + return std::visit([](auto const& model) { return model.distance(); }, *state); + } }; RobotState::RobotState() noexcept @@ -62,6 +73,6 @@ auto RobotState::update(std::span armors) -> bool { return pimpl- auto RobotState::is_converged() const -> bool { return pimpl->is_converged(); } -auto RobotState::get_snapshot() const -> Snapshot { return pimpl->get_snapshot(); } +auto RobotState::get_snapshot() const -> std::optional { return pimpl->get_snapshot(); } auto RobotState::distance() const -> double { return pimpl->distance(); } diff --git a/src/module/predictor/robot_state.hpp b/src/module/predictor/robot_state.hpp index d6376bb4..1af14e03 100644 --- a/src/module/predictor/robot_state.hpp +++ b/src/module/predictor/robot_state.hpp @@ -3,6 +3,7 @@ #include "utility/clock.hpp" #include "utility/pimpl.hpp" +#include #include namespace rmcs::predictor { @@ -19,7 +20,7 @@ struct RobotState { auto is_converged() const -> bool; - auto get_snapshot() const -> Snapshot; + auto get_snapshot() const -> std::optional; auto distance() const -> double; }; diff --git a/src/module/predictor/snapshot.cpp b/src/module/predictor/snapshot.cpp index a6ff5c69..3b3a4d09 100644 --- a/src/module/predictor/snapshot.cpp +++ b/src/module/predictor/snapshot.cpp @@ -1,55 +1,54 @@ #include "snapshot.hpp" +#include "module/predictor/outpost/snapshot.hpp" +#include "module/predictor/regular/snapshot.hpp" + #include #include - -#include "module/predictor/backend/snapshot_backend.hpp" +#include namespace rmcs::predictor { -namespace { +struct Snapshot::Impl { + using SnapshotVariant = std::variant; - struct EmptySnapshotBackend final : ISnapshotBackend { - explicit EmptySnapshotBackend(TimePoint stamp) noexcept - : ISnapshotBackend { DeviceId::UNKNOWN, CampColor::UNKNOWN, 0, stamp } { } + explicit Impl(RegularSnapshot snapshot) + : snapshot { std::move(snapshot) } { } - [[nodiscard]] auto kinematics_at(TimePoint) const -> Snapshot::Kinematics override { - return { Point3d::kZero(), 0.0 }; - } + explicit Impl(OutpostSnapshot snapshot) + : snapshot { std::move(snapshot) } { } - [[nodiscard]] auto predicted_armors(TimePoint) const -> std::vector override { - return { }; - } - }; + SnapshotVariant snapshot; +}; -} // namespace +Snapshot::Snapshot(RegularSnapshot snapshot) + : pimpl { std::make_unique(std::move(snapshot)) } { } -auto detail::make_snapshot(std::unique_ptr backend) noexcept -> Snapshot { - return Snapshot { std::move(backend) }; -} - -auto Snapshot::empty(TimePoint stamp) noexcept -> Snapshot { - return detail::make_snapshot(std::make_unique(stamp)); -} - -Snapshot::Snapshot(std::unique_ptr backend) noexcept - : backend { std::move(backend) } { } +Snapshot::Snapshot(OutpostSnapshot snapshot) + : pimpl { std::make_unique(std::move(snapshot)) } { } Snapshot::Snapshot(Snapshot&&) noexcept = default; auto Snapshot::operator=(Snapshot&&) noexcept -> Snapshot& = default; Snapshot::~Snapshot() noexcept = default; -auto Snapshot::time_stamp() const -> TimePoint { return backend->time_stamp(); } +auto Snapshot::time_stamp() const -> TimePoint { + return std::visit([](auto const& snapshot) { return snapshot.time_stamp(); }, pimpl->snapshot); +} -auto Snapshot::device_id() const -> DeviceId { return backend->device; } +auto Snapshot::device_id() const -> DeviceId { + return std::visit([](auto const& snapshot) { return snapshot.device_id(); }, pimpl->snapshot); +} -auto Snapshot::kinematics() const -> Kinematics { return backend->kinematics_at(time_stamp()); } +auto Snapshot::motion() const -> TargetMotion { return motion_at(time_stamp()); } -auto Snapshot::kinematics_at(TimePoint t) const -> Kinematics { return backend->kinematics_at(t); } +auto Snapshot::motion_at(TimePoint t) const -> TargetMotion { + return std::visit([t](auto const& snapshot) { return snapshot.motion_at(t); }, pimpl->snapshot); +} auto Snapshot::predicted_armors(TimePoint t) const -> std::vector { - return backend->predicted_armors(t); + return std::visit( + [t](auto const& snapshot) { return snapshot.predicted_armors(t); }, pimpl->snapshot); } } // namespace rmcs::predictor diff --git a/src/module/predictor/snapshot.hpp b/src/module/predictor/snapshot.hpp index 6df12cff..5cdb6401 100644 --- a/src/module/predictor/snapshot.hpp +++ b/src/module/predictor/snapshot.hpp @@ -1,30 +1,29 @@ #pragma once -#include -#include - #include "utility/clock.hpp" #include "utility/robot/armor.hpp" #include "utility/robot/id.hpp" +#include + +#include +#include + namespace rmcs::predictor { -struct ISnapshotBackend; +class RegularSnapshot; +class OutpostSnapshot; class Snapshot; -namespace detail { - auto make_snapshot(std::unique_ptr backend) noexcept -> Snapshot; -} +struct TargetMotion { + Eigen::Vector3d center_position; + double angular_velocity; +}; class Snapshot { public: - struct Kinematics { - Point3d center_position; - double angular_velocity; - }; - - static auto empty(TimePoint stamp) noexcept -> Snapshot; - + explicit Snapshot(RegularSnapshot snapshot); + explicit Snapshot(OutpostSnapshot snapshot); Snapshot(Snapshot const&) = delete; Snapshot(Snapshot&&) noexcept; Snapshot& operator=(Snapshot const&) = delete; @@ -33,20 +32,16 @@ class Snapshot { auto time_stamp() const -> TimePoint; auto device_id() const -> DeviceId; - auto kinematics() const -> Kinematics; - auto kinematics_at(TimePoint t) const -> Kinematics; - auto predicted_armors() const { return predicted_armors(Clock::now()); } + auto motion() const -> TargetMotion; + auto motion_at(TimePoint t) const -> TargetMotion; + auto predicted_armors() const { return predicted_armors(Clock::now()); } auto predicted_armors(TimePoint t) const -> std::vector; private: - explicit Snapshot(std::unique_ptr backend) noexcept; - - std::unique_ptr backend; - - friend auto detail::make_snapshot(std::unique_ptr backend) noexcept - -> Snapshot; + struct Impl; + std::unique_ptr pimpl; }; -} +} // namespace rmcs::predictor diff --git a/src/module/tracker/decider.cpp b/src/module/tracker/decider.cpp index d4ff629b..28621251 100644 --- a/src/module/tracker/decider.cpp +++ b/src/module/tracker/decider.cpp @@ -31,19 +31,31 @@ struct Decider::Impl { if (priority_mode.empty()) priority_mode = mode2; - return { }; + return {}; } auto set_priority_mode(PriorityMode const& mode) -> void { priority_mode = mode; } auto update(std::span armors, TimePoint t) -> Output { + std::erase_if(trackers, [&](const auto& item) { + auto last_seen_it = last_seen_times.find(item.first); + auto cleanup_interval = get_cleanup_interval(item.first); + bool expired = last_seen_it == last_seen_times.end() + || util::delta_time(t, last_seen_it->second) > cleanup_interval; + if (expired) { + last_seen_times.erase(item.first); + } + + return expired; + }); + // 推进所有现有追踪器的时间轴 for (auto& [id, tracker] : trackers) { tracker->predict(t); } - auto observed_ids = std::unordered_set { }; - auto grouped_armors = std::unordered_map> { }; + auto observed_ids = std::unordered_set {}; + auto grouped_armors = std::unordered_map> {}; for (const auto& armor : armors) { grouped_armors[armor.genre].emplace_back(armor); @@ -65,18 +77,6 @@ struct Decider::Impl { } } - std::erase_if(trackers, [&](const auto& item) { - auto last_seen_it = last_seen_times.find(item.first); - auto cleanup_interval = get_cleanup_interval(item.first); - bool expired = last_seen_it == last_seen_times.end() - || util::delta_time(t, last_seen_it->second) > cleanup_interval; - if (expired) { - last_seen_times.erase(item.first); - } - - return expired; - }); - auto fresh_target_id = arbitrate(observed_ids); if (fresh_target_id != DeviceId::UNKNOWN) { return make_output(fresh_target_id, trackers.at(fresh_target_id)->is_converged()); @@ -128,9 +128,9 @@ struct Decider::Impl { // 比较顺序：优先级 -> 收敛状态 -> 距离 -> 固定 ID 兜底。 return std::tuple { - priority_of(device_id), // 数值越小，优先级越高。 - !tracker.is_converged(), // 收敛目标映射为 0，未收敛目标映射为 1。 - safe_distance, // 非有限距离按无穷远处理，避免 NaN/Inf 干扰排序。 + priority_of(device_id), // 数值越小，优先级越高。 + !tracker.is_converged(), // 收敛目标映射为 0，未收敛目标映射为 1。 + safe_distance, // 非有限距离按无穷远处理，避免 NaN/Inf 干扰排序。 rmcs::to_index(device_id), // 完全相同时按固定顺序兜底，避免容器遍历顺序抖动。 }; }; diff --git a/src/utility/math/tiny_mpc_solver.hpp b/src/utility/math/tiny_mpc_solver.hpp index f30263f5..6052ce6b 100644 --- a/src/utility/math/tiny_mpc_solver.hpp +++ b/src/utility/math/tiny_mpc_solver.hpp @@ -68,20 +68,20 @@ class TinyMpcSolver { config.setup.Q, config.setup.R, config.setup.rho, kStateDim, kInputDim, kHorizonSteps, config.setup.verbose); if (status != 0 || raw_solver == nullptr) { - TinySolverDeleter { }(raw_solver); + TinySolverDeleter {}(raw_solver); return std::unexpected { - std::format("tiny_setup failed with status {}", status), + std::format("tiny_setup failed: dimension mismatch (status={})", status), }; } auto solver = TinyMpcSolver { TinySolverPtr { raw_solver } }; const auto bound_status = tiny_set_bound_constraints(solver.solver_.get(), config.bounds.x_min, config.bounds.x_max, config.bounds.u_min, config.bounds.u_max); - if (bound_status != 0) { + if (bound_status != 0) return std::unexpected { - std::format("tiny_set_bound_constraints failed with status {}", bound_status), + std::format("tiny_set_bound_constraints failed: dimension mismatch (status={})", + bound_status), }; - } solver.solver_->settings->max_iter = config.max_iter; return solver; @@ -89,46 +89,44 @@ class TinyMpcSolver { auto set_x0(StateVector const& x0) -> std::expected { const auto status = tiny_set_x0(solver_.get(), x0); - if (status != 0) { + if (status != 0) return std::unexpected { - std::format("tiny_set_x0 failed with status {}", status), + std::format("tiny_set_x0 failed: solver is null"), }; - } - return { }; + return {}; } auto set_x_ref(StateTrajectory const& x_ref) -> std::expected { const auto status = tiny_set_x_ref(solver_.get(), x_ref); - if (status != 0) { + if (status != 0) return std::unexpected { - std::format("tiny_set_x_ref failed with status {}", status), + std::format("tiny_set_x_ref failed: solver is null"), }; - } - return { }; + return {}; } auto set_u_ref(InputTrajectory const& u_ref) -> std::expected { const auto status = tiny_set_u_ref(solver_.get(), u_ref); - if (status != 0) { + if (status != 0) return std::unexpected { - std::format("tiny_set_u_ref failed with status {}", status), + std::format("tiny_set_u_ref failed: solver is null"), }; - } - return { }; + return {}; } auto solve() -> std::expected { const auto status = tiny_solve(solver_.get()); - if (status != 0) { + if (status != 0 && status != 1) return std::unexpected { - std::format("tiny_solve failed with status {}", status), + std::format("tiny_solve failed with status {} (expected 0=converged or " + "1=max_iter_reached)", + status), }; - } - return { }; + return {}; } template @@ -165,7 +163,7 @@ class TinyMpcSolver { explicit TinyMpcSolver(TinySolverPtr solver) noexcept : solver_ { std::move(solver) } { } - TinySolverPtr solver_ { }; + TinySolverPtr solver_ {}; }; } // namespace rmcs::util diff --git a/src/utility/robot/armor.hpp b/src/utility/robot/armor.hpp index 73d5c366..ee5b836a 100644 --- a/src/utility/robot/armor.hpp +++ b/src/utility/robot/armor.hpp @@ -180,4 +180,4 @@ constexpr std::array kSmallArmorShapeRos { Point3d { 0.0, -0.5 * kSmallArmorWidth, +0.5 * kLightBarHeight }, // Top-right Point3d { 0.0, +0.5 * kSmallArmorWidth, -0.5 * kLightBarHeight } // Bottom-left }; -} +} \ No newline at end of file diff --git a/src/utility/shared/context.hpp b/src/utility/shared/context.hpp index 6c27f679..876e8744 100644 --- a/src/utility/shared/context.hpp +++ b/src/utility/shared/context.hpp @@ -20,8 +20,6 @@ struct AutoAimState { double pitch_rate { kNaN }; double yaw_acc { kNaN }; double pitch_acc { kNaN }; - bool feedforward_valid { false }; - Translation robot_center { kNaN, kNaN, kNaN }; DeviceId target { DeviceId::UNKNOWN }; @@ -43,7 +41,7 @@ struct SystemContext { double yaw { kNaN }; double pitch { kNaN }; - Transform camera_transform = Transform::kNaN(); // Imu Odom Link + Transform camera_transform = Transform::kNaN(); /// Lazy Context /// diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 68446dd2..86d4d0b9 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -135,17 +135,6 @@ ament_add_gtest( ${TEST_DIR}/transform_communication.cpp ) -# Aim point chooser -ament_add_gtest( - test_aim_point_chooser - ${TEST_DIR}/aim_point_chooser.cpp - ${RMCS_SRC_DIR}/module/fire_control/aim_point_chooser.cpp -) -target_link_libraries( - test_aim_point_chooser - yaml-cpp::yaml-cpp -) - # Tiny MPC solver ament_add_gtest( test_tiny_mpc_solver @@ -155,14 +144,3 @@ target_link_libraries( test_tiny_mpc_solver tinympcstatic ) - -# Tiny MPC axis solver -ament_add_gtest( - test_tiny_mpc_axis_solver - ${TEST_DIR}/tiny_mpc_axis_solver.cpp - ${RMCS_SRC_DIR}/module/fire_control/planner/tiny_mpc_axis_solver.cpp -) -target_link_libraries( - test_tiny_mpc_axis_solver - tinympcstatic -) diff --git a/test/aim_point_chooser.cpp b/test/aim_point_chooser.cpp deleted file mode 100644 index 01d67068..00000000 --- a/test/aim_point_chooser.cpp +++ /dev/null @@ -1,125 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "module/fire_control/aim_point_chooser.hpp" -#include "utility/math/angle.hpp" - -using rmcs::Armor3d; -using rmcs::ArmorColor; -using rmcs::DeviceId; -using rmcs::Orientation; -using rmcs::Translation; -using rmcs::fire_control::AimPointChooser; - -namespace { - -constexpr auto kAngleStepDeg = 5; -constexpr auto kFastPositive = 4.0; -constexpr auto kFastNegative = -4.0; - -auto make_chooser() -> std::unique_ptr { - auto chooser = std::make_unique(); - chooser->initialize(AimPointChooser::Config { - .coming_angle = rmcs::util::deg2rad(60.0), - .leaving_angle = rmcs::util::deg2rad(20.0), - .angular_velocity_threshold = 2.0, - .outpost_coming_angle = rmcs::util::deg2rad(70.0), - .outpost_leaving_angle = rmcs::util::deg2rad(30.0), - }); - return chooser; -} - -auto make_center_position(double yaw_deg) -> Eigen::Vector3d { - auto const yaw_rad = rmcs::util::deg2rad(yaw_deg); - return { std::cos(yaw_rad), std::sin(yaw_rad), 0.0 }; -} - -auto make_armor(double yaw_deg, int id, DeviceId genre) -> Armor3d { - auto armor = Armor3d { }; - armor.genre = genre; - armor.color = ArmorColor::BLUE; - armor.id = id; - - auto const yaw_rad = rmcs::util::deg2rad(yaw_deg); - auto const q = Eigen::Quaterniond { Eigen::AngleAxisd { yaw_rad, Eigen::Vector3d::UnitZ() } }; - - armor.translation = Translation { 0.0, 0.0, 0.0 }; - armor.orientation = Orientation { q }; - return armor; -} - -auto make_armors(std::initializer_list yaws_deg, DeviceId genre = DeviceId::SENTRY) - -> std::vector { - auto armors = std::vector { }; - armors.reserve(yaws_deg.size()); - - auto index = 0; - for (auto const yaw_deg : yaws_deg) { - armors.emplace_back(make_armor(yaw_deg, index, genre)); - ++index; - } - return armors; -} - -auto choose_id(AimPointChooser& chooser, std::span armors, double center_yaw_deg, - double angular_velocity) -> std::optional { - auto const center = make_center_position(center_yaw_deg); - auto const chosen = chooser.choose_armor(armors, center, angular_velocity); - if (!chosen.has_value()) return std::nullopt; - return chosen->id; -} - -auto choose_once(std::span armors, double center_yaw_deg, double angular_velocity) - -> std::optional { - auto chooser = make_chooser(); - return choose_id(*chooser, armors, center_yaw_deg, angular_velocity); -} - -} // namespace - -TEST(AimPointChooser, SingleArmorHighSpeedScanBySpinDirection) { - for (int angle_deg = -180; angle_deg <= 180; angle_deg += kAngleStepDeg) { - auto const armors = make_armors({ static_cast(angle_deg) }); - - for (auto const speed : std::array { 2.0, kFastPositive, -2.0, kFastNegative }) { - auto const in_coming_window = std::abs(angle_deg) <= 60; - auto const in_leaving_window = (speed > 0.0) ? (angle_deg <= 20) : (angle_deg >= -20); - auto const expected = - (in_coming_window && in_leaving_window) ? std::optional { 0 } : std::nullopt; - - auto const actual = choose_once(armors, 0.0, speed); - - SCOPED_TRACE("angle_deg=" + std::to_string(angle_deg)); - SCOPED_TRACE("speed=" + std::to_string(speed)); - EXPECT_EQ(actual, expected); - } - } -} - -TEST(AimPointChooser, PreferIncomingArmorWhenSpinPositive) { - auto const armors = make_armors({ -30.0, 5.0 }); - auto const chosen = choose_once(armors, 0.0, kFastPositive); - EXPECT_EQ(chosen, std::optional { 0 }); -} - -TEST(AimPointChooser, PreferIncomingArmorWhenSpinNegative) { - auto const armors = make_armors({ -5.0, 30.0 }); - auto const chosen = choose_once(armors, 0.0, kFastNegative); - EXPECT_EQ(chosen, std::optional { 1 }); -} - -TEST(AimPointChooser, KeepAbsoluteDeltaPriorityWhenSpinZero) { - auto const armors = make_armors({ -15.0, 5.0 }); - auto const chosen = choose_once(armors, 0.0, 0.0); - EXPECT_EQ(chosen, std::optional { 1 }); -} diff --git a/test/tiny_mpc_axis_solver.cpp b/test/tiny_mpc_axis_solver.cpp deleted file mode 100644 index 83e6aba8..00000000 --- a/test/tiny_mpc_axis_solver.cpp +++ /dev/null @@ -1,85 +0,0 @@ -#include "module/fire_control/planner/tiny_mpc_axis_solver.hpp" - -#include - -using namespace rmcs::fire_control; - -namespace { - -auto make_constant_reference(double angle) -> MpcAxisTrajectory { - auto reference = MpcAxisTrajectory {}; - reference.setZero(); - reference.row(0).setConstant(angle); - return reference; -} - -auto make_config() -> TinyMpcAxisSolver::Config { - return TinyMpcAxisSolver::Config { - .max_acc = 50.0, - .q_angle = 9e6, - .q_rate = 0.0, - .r_acc = 1.0, - .max_iter = 10, - }; -} - -} // namespace - -TEST(tiny_mpc_axis_solver, solves_constant_reference) { - auto solver = TinyMpcAxisSolver {}; - - auto init = solver.initialize(make_config()); - ASSERT_TRUE(init.has_value()) << init.error(); - - auto result = solver.solve_center(make_constant_reference(0.75)); - ASSERT_TRUE(result.has_value()) << result.error(); - EXPECT_NEAR(result.value(), 0.75, 1e-6); -} - -TEST(tiny_mpc_axis_solver, solve_center_requires_initialization) { - auto solver = TinyMpcAxisSolver {}; - - auto result = solver.solve_center(make_constant_reference(0.25)); - ASSERT_FALSE(result.has_value()); - EXPECT_EQ(result.error(), "solver is not initialized"); -} - -TEST(tiny_mpc_axis_solver, can_reinitialize_without_reset) { - auto solver = TinyMpcAxisSolver {}; - - auto init = solver.initialize(make_config()); - ASSERT_TRUE(init.has_value()) << init.error(); - - init = solver.initialize(make_config()); - ASSERT_TRUE(init.has_value()) << init.error(); - - auto result = solver.solve_center(make_constant_reference(0.5)); - ASSERT_TRUE(result.has_value()) << result.error(); - EXPECT_NEAR(result.value(), 0.5, 1e-6); -} - -TEST(tiny_mpc_axis_solver, initialize_rejects_non_positive_max_iter) { - auto solver = TinyMpcAxisSolver {}; - - auto config = make_config(); - config.max_iter = 0; - - auto init = solver.initialize(config); - ASSERT_FALSE(init.has_value()); - EXPECT_EQ(init.error(), "max_iter must be positive, got 0"); -} - -TEST(tiny_mpc_axis_solver, can_solve_multiple_references_after_initialization) { - auto solver = TinyMpcAxisSolver {}; - - auto init = solver.initialize(make_config()); - ASSERT_TRUE(init.has_value()) << init.error(); - - auto first = solver.solve_center(make_constant_reference(0.25)); - ASSERT_TRUE(first.has_value()) << first.error(); - EXPECT_NEAR(first.value(), 0.25, 1e-6); - - auto second = solver.solve_center(make_constant_reference(-0.5)); - ASSERT_TRUE(second.has_value()) << second.error(); - EXPECT_NEAR(second.value(), -0.5, 1e-6); -}