From f421b51dab5956fbadc83e43c8ce83a067b57ca0 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 4 Jun 2026 00:33:15 +0800 Subject: [PATCH 01/21] refactor: outpost EKF with omega state and Mahalanobis-only matching --- .../predictor/outpost/ekf_parameter.hpp | 68 ++- src/module/predictor/outpost/robot_state.cpp | 448 ++++++------------ src/module/predictor/outpost/snapshot.cpp | 30 +- src/module/predictor/outpost/snapshot.hpp | 2 +- src/module/predictor/snapshot.hpp | 2 +- 5 files changed, 185 insertions(+), 365 deletions(-) diff --git a/src/module/predictor/outpost/ekf_parameter.hpp b/src/module/predictor/outpost/ekf_parameter.hpp index 5ac392b9..68bb7ae2 100644 --- a/src/module/predictor/outpost/ekf_parameter.hpp +++ b/src/module/predictor/outpost/ekf_parameter.hpp @@ -14,16 +14,17 @@ namespace rmcs::predictor { struct OutpostEKFParameters { - using EKF = util::EKF<6, 4>; + using EKF = util::EKF<7, 4>; static constexpr int kOutpostArmorCount = 3; static constexpr double kPhaseStep = 2.0 * std::numbers::pi / kOutpostArmorCount; - // x vx y vy z a + // x vx y vy z a w // x, y：前哨站旋转中心在世界坐标系下的位置 // vx, vy：前哨站旋转中心在世界坐标系下的线速度 // z：参考装甲板(id 0)在世界坐标系下的 z 坐标 // a：参考装甲板(id 0)的 yaw 角 + // w：参考装甲板(id 0)的角速度 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; @@ -35,13 +36,13 @@ struct OutpostEKFParameters { 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; + x << center_x, 0.0, center_y, 0.0, trans_z, yaw, 0.0; return x; } static auto P_initial_dig() -> EKF::PDig { auto P_dig = EKF::PDig {}; - P_dig << 1.0, 64.0, 1.0, 64.0, 1.0, 0.4; + P_dig << 1.0, 64.0, 1.0, 64.0, 1.0, 0.4, 16.0; return P_dig; } @@ -113,12 +114,13 @@ struct OutpostEKFParameters { auto F = EKF::AMat {}; // clang-format off F << - 1, dt, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, - 0, 0, 1, dt, 0, 0, - 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 1; + 1, dt, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, 0, + 0, 0, 1, dt, 0, 0, 0, + 0, 0, 0, 1, 0, 0, 0, + 0, 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 0, 1, dt, + 0, 0, 0, 0, 0, 0, 1; // clang-format on return F; } @@ -128,11 +130,11 @@ struct OutpostEKFParameters { constexpr double linear_acc_var = 10.0; // 参考装甲板 z 的随机游走噪声 constexpr double z_ref_rw_var = 1e-3; - // 参考装甲板 yaw 角的随机游走噪声，用于补偿固定角速度模型误差 - constexpr double angle_rw_var = 1e-3; + // 参考装甲板 yaw / 角速度的角加速度噪声 + constexpr double angular_acc_var = 4.0; const auto v1 = linear_acc_var; const auto v2 = z_ref_rw_var; - const auto v3 = angle_rw_var; + const auto v3 = angular_acc_var; const auto a = dt * dt * dt * dt / 4.0; const auto b = dt * dt * dt / 2.0; @@ -140,29 +142,21 @@ struct OutpostEKFParameters { auto Q = EKF::QMat {}; // clang-format off - Q << a * v1, b * v1, 0, 0, 0, 0, - b * v1, c * v1, 0, 0, 0, 0, - 0, 0, a * v1, b * v1, 0, 0, - 0, 0, b * v1, c * v1, 0, 0, - 0, 0, 0, 0, v2 * dt, 0, - 0, 0, 0, 0, 0, v3 * dt; + Q << a * v1, b * v1, 0, 0, 0, 0, 0, + b * v1, c * v1, 0, 0, 0, 0, 0, + 0, 0, a * v1, b * v1, 0, 0, 0, + 0, 0, b * v1, c * v1, 0, 0, 0, + 0, 0, 0, 0, v2 * dt, 0, 0, + 0, 0, 0, 0, 0, a * v3, b * v3, + 0, 0, 0, 0, 0, b * v3, c * v3; // clang-format on return Q; } - static auto f(double dt, int spin_sign) -> auto { - return [dt, spin_sign](EKF::XVec const& x) { - EKF::XVec x_prior = x; - const auto angular_speed = spin_sign > 0 ? kOutpostAngularSpeed - : spin_sign < 0 ? -kOutpostAngularSpeed - : 0.0; - - x_prior[0] = x[0] + x[1] * dt; - x_prior[1] = x[1]; - x_prior[2] = x[2] + x[3] * dt; - x_prior[3] = x[3]; - x_prior[4] = x[4]; - x_prior[5] = util::normalize_angle(x[5] + angular_speed * dt); + static auto f(double dt) -> auto { + return [dt](EKF::XVec const& x) { + EKF::XVec x_prior = F(dt) * x; + x_prior[5] = util::normalize_angle(x_prior[5]); return x_prior; }; } @@ -189,13 +183,13 @@ 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, - 0, 0, 1, 0, 0, dy_da, - 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 1; + 1, 0, 0, 0, 0, dx_da, 0, + 0, 0, 1, 0, 0, dy_da, 0, + 0, 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 0, 1, 0; // clang-format on const auto xyz = h_armor_xyz(x, phase_offset, height_offset); diff --git a/src/module/predictor/outpost/robot_state.cpp b/src/module/predictor/outpost/robot_state.cpp index 2ae9af58..a10c0f3e 100644 --- a/src/module/predictor/outpost/robot_state.cpp +++ b/src/module/predictor/outpost/robot_state.cpp @@ -6,10 +6,8 @@ #include #include #include -#include #include "module/predictor/outpost/snapshot.hpp" -#include "utility/math/angle.hpp" #include "utility/math/mahalanobis.hpp" #include "utility/time.hpp" @@ -20,105 +18,79 @@ 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 ypd; }; -auto make_observation(rmcs::Armor3D const& armor) -> OutpostObservation { - auto const [pos_x, pos_y, pos_z] = armor.translation; - auto const xyz = Eigen::Vector3d { pos_x, pos_y, pos_z }; - - auto const [quat_x, quat_y, quat_z, quat_w] = armor.orientation; - auto const orientation = Eigen::Quaterniond { quat_w, quat_x, quat_y, quat_z }; - - auto const ypr = rmcs::util::eulers(orientation); - auto const ypd = rmcs::util::xyz2ypd(xyz); - - auto z = OutpostEKF::ZVec {}; - z << ypd[0], ypd[1], ypd[2], ypr[0]; - - return { z, OutpostEKFParameters::R(xyz, ypr, ypd), xyz, ypd }; -} - -enum class SwitchEvent { - Stay, - ClockwiseSwitch, - CounterClockwiseSwitch, - Invalid, +struct SlotCandidate { + int armor_id { kUnknownArmorId }; + double phase_offset { 0.0 }; + double height_offset { 0.0 }; + bool extends_layout { false }; }; -struct AssociationDecision { +struct MatchDecision { 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 }; + double error { std::numeric_limits::infinity() }; bool extends_layout { false }; + bool is_valid { false }; }; struct BestMatch { OutpostObservation observation; - AssociationDecision decision; + MatchDecision decision; }; struct MatchingConfig { - double azimuth_gate { rmcs::util::deg2rad(35.0) }; - double z_gate { 0.05 }; - double slot_phase_tolerance { rmcs::util::deg2rad(8.0) }; - double slot_height_tolerance { 0.02 }; double mahalanobis_gate { 25.0 }; - double layout_extension_penalty { 0.25 }; - double continuity_bonus { 0.10 }; }; struct TrackingConfig { std::chrono::duration reset_interval { 1.5 }; - int spin_confirm_switches { 2 }; - int stop_confirm_stays { 4 }; - double stop_phase_rate_gate { 0.8 }; MatchingConfig matching {}; }; -struct SpinTracker { - std::optional confirmed_sign {}; - std::optional pending_sign {}; - int pending_count { 0 }; - int stop_pending_count { 0 }; - std::optional> last_stay_sample {}; +// 模板 A: slot1 = +H, slot2 = -H +// 模板 B: slot1 = -H, slot2 = +H +enum class HeightTemplate { Unknown, PositiveOnSlot1, NegativeOnSlot1 }; - auto reset() -> void { *this = {}; } +auto resolve_template(OutpostArmorLayout const& layout) -> HeightTemplate { + if (layout.slots[1].assigned) { + return layout.slots[1].height_offset > 0.0 ? HeightTemplate::PositiveOnSlot1 + : HeightTemplate::NegativeOnSlot1; + } + if (layout.slots[2].assigned) { + return layout.slots[2].height_offset > 0.0 ? HeightTemplate::NegativeOnSlot1 + : HeightTemplate::PositiveOnSlot1; + } + return HeightTemplate::Unknown; +} - auto current_sign() const -> int { return confirmed_sign.value_or(pending_sign.value_or(0)); } -}; +auto make_observation(rmcs::Armor3D const& armor) -> OutpostObservation { + auto const [pos_x, pos_y, pos_z] = armor.translation; + auto const xyz = Eigen::Vector3d { pos_x, pos_y, pos_z }; -auto assigned_count(OutpostArmorLayout const& layout) -> int { - return static_cast(std::ranges::count(layout.slots, true, &OutpostArmorSlot::assigned)); -} + 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]; -auto has_assigned_slot(OutpostArmorLayout const& layout, int armor_id) -> bool { - return armor_id >= 0 && armor_id < OutpostEKFParameters::kOutpostArmorCount - && layout.slots[armor_id].assigned; + return { z, OutpostEKFParameters::R(xyz, ypr, ypd), xyz }; } -auto first_unassigned_slot(OutpostArmorLayout const& layout) -> int { - for (int id = 0; id < OutpostEKFParameters::kOutpostArmorCount; ++id) { - if (!layout.slots[id].assigned) return id; - } - return kUnknownArmorId; +auto assigned_count(OutpostArmorLayout const& layout) -> int { + return static_cast(std::ranges::count(layout.slots, true, &OutpostArmorSlot::assigned)); } -auto layout_after_association(OutpostArmorLayout const& layout, AssociationDecision const& decision) +auto layout_after_match(OutpostArmorLayout const& layout, MatchDecision const& decision) -> OutpostArmorLayout { auto next_layout = layout; if (!decision.extends_layout) return next_layout; @@ -129,155 +101,83 @@ auto layout_after_association(OutpostArmorLayout const& layout, AssociationDecis return next_layout; } -auto switch_phase_delta(int inferred_spin_sign) -> double { - return -normalize_spin_sign(inferred_spin_sign) * OutpostEKFParameters::kPhaseStep; -} - -auto switch_height_deltas(int inferred_spin_sign) -> std::array { - if (normalize_spin_sign(inferred_spin_sign) > 0) { - return { rmcs::kOutpostArmorHeightStep, -2.0 * rmcs::kOutpostArmorHeightStep }; +auto slot_phase_offset(int armor_id) -> double { + switch (armor_id) { + case 0: + return 0.0; + case 1: + return OutpostEKFParameters::kPhaseStep; + case 2: + return -OutpostEKFParameters::kPhaseStep; + default: + return 0.0; } - return { -rmcs::kOutpostArmorHeightStep, 2.0 * rmcs::kOutpostArmorHeightStep }; } -auto consider_candidate(AssociationDecision const& candidate, AssociationDecision& best_decision) - -> void { - if (!candidate.is_valid || candidate.error >= best_decision.error) return; - best_decision = candidate; -} - -class AssociationEngine { -public: - AssociationEngine(OutpostEKF::XVec const& x, OutpostEKF::PMat const& P, - OutpostArmorLayout const& layout, int current_armor_id, SpinTracker const& spin, - MatchingConfig const& config) noexcept - : x_ { x } - , P_ { P } - , layout_ { layout } - , current_armor_id_ { current_armor_id } - , spin_ { spin } - , config_ { config } { } - - auto decide(OutpostObservation const& observation) const -> AssociationDecision { - if (!has_assigned_slot(layout_, current_armor_id_)) return {}; - - auto best_decision = AssociationDecision {}; - auto const current_phase = layout_.slots[current_armor_id_].phase_offset; - auto const current_height = layout_.slots[current_armor_id_].height_offset; - - consider_candidate(evaluate_candidate(observation, current_armor_id_, current_phase, - current_height, SwitchEvent::Stay, 0, false), - best_decision); - - if (spin_.confirmed_sign == 0) return best_decision; - - if (spin_.confirmed_sign.has_value()) { - auto const sign = *spin_.confirmed_sign; - auto const event = - sign > 0 ? SwitchEvent::CounterClockwiseSwitch : SwitchEvent::ClockwiseSwitch; - consider_switch_direction( - observation, current_phase, current_height, sign, event, best_decision); - return best_decision; - } - - 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 generate_slot_candidates(OutpostArmorLayout const& layout) -> std::array { + using rmcs::kOutpostArmorHeightStep; + + auto candidates = std::array {}; + auto append_at = int { 0 }; + auto append = [&](int armor_id, double height_offset, bool extends_layout) { + candidates[append_at++] = { + armor_id, + slot_phase_offset(armor_id), + height_offset, + extends_layout, + }; + }; - return best_decision; + for (int armor_id = 0; armor_id < OutpostEKFParameters::kOutpostArmorCount; ++armor_id) { + if (!layout.slots[armor_id].assigned) continue; + append(armor_id, layout.slots[armor_id].height_offset, false); } -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 {}; - } + if (assigned_count(layout) == OutpostEKFParameters::kOutpostArmorCount) return candidates; - auto const H = OutpostEKFParameters::H(x_, phase_offset, height_offset); - auto const z_hat = OutpostEKFParameters::h(x_, phase_offset, height_offset); - auto const innovation = OutpostEKFParameters::z_subtract(observation.z, z_hat); - auto const S = H * P_ * H.transpose() + observation.R; - auto const mahalanobis = rmcs::util::mahalanobis_distance(innovation, S); - if (!mahalanobis.has_value() || *mahalanobis > config_.mahalanobis_gate) { - return {}; - } + auto const height_slot1 = [&](HeightTemplate t) -> double { + return t == HeightTemplate::PositiveOnSlot1 ? kOutpostArmorHeightStep + : -kOutpostArmorHeightStep; + }; - auto error = *mahalanobis; - if (extends_layout) error += config_.layout_extension_penalty; - if (event == SwitchEvent::Stay) error -= config_.continuity_bonus; + auto const tmpl = resolve_template(layout); - return { armor_id, error, true, event, normalize_spin_sign(inferred_spin_sign), - rmcs::util::normalize_angle(phase_offset), height_offset, extends_layout }; + if (!layout.slots[1].assigned) { + if (tmpl == HeightTemplate::Unknown) { + append(1, kOutpostArmorHeightStep, true); + append(1, -kOutpostArmorHeightStep, true); + } else { + append(1, height_slot1(tmpl), true); + } } - auto find_matching_slot(double phase_offset, double height_offset, int excluded_armor_id) const - -> int { - auto best_slot = kUnknownArmorId; - auto best_mismatch = std::numeric_limits::infinity(); - - for (int id = 0; id < OutpostEKFParameters::kOutpostArmorCount; ++id) { - if (!layout_.slots[id].assigned || id == excluded_armor_id) continue; - - auto const phase_error = std::abs( - rmcs::util::normalize_angle(layout_.slots[id].phase_offset - phase_offset)); - auto const height_error = std::abs(layout_.slots[id].height_offset - height_offset); - if (phase_error > config_.slot_phase_tolerance - || height_error > config_.slot_height_tolerance) - continue; - - auto const mismatch = phase_error + height_error / rmcs::kOutpostArmorHeightStep; - if (mismatch >= best_mismatch) continue; - - best_mismatch = mismatch; - best_slot = id; + if (!layout.slots[2].assigned) { + if (tmpl == HeightTemplate::Unknown) { + append(2, kOutpostArmorHeightStep, true); + append(2, -kOutpostArmorHeightStep, true); + } else { + append(2, -height_slot1(tmpl), true); } - - return best_slot; } - auto consider_switch_direction(OutpostObservation const& observation, double current_phase, - double current_height, int inferred_spin_sign, SwitchEvent event, - AssociationDecision& best_decision) const -> void { - auto const candidate_phase = - rmcs::util::normalize_angle(current_phase + switch_phase_delta(inferred_spin_sign)); - - for (auto const height_delta : switch_height_deltas(inferred_spin_sign)) { - auto const candidate_height = current_height + height_delta; - - auto armor_id = - find_matching_slot(candidate_phase, candidate_height, current_armor_id_); - auto extends_layout = false; - if (armor_id == kUnknownArmorId) { - armor_id = first_unassigned_slot(layout_); - extends_layout = (armor_id != kUnknownArmorId); - } - if (armor_id == kUnknownArmorId) continue; + return candidates; +} - consider_candidate(evaluate_candidate(observation, armor_id, candidate_phase, - candidate_height, event, inferred_spin_sign, extends_layout), - best_decision); - } - } +auto evaluate_candidate(OutpostObservation const& observation, OutpostEKF::XVec const& x, + OutpostEKF::PMat const& P, MatchingConfig const& config, SlotCandidate const& candidate) + -> MatchDecision { + if (candidate.armor_id == kUnknownArmorId) return {}; - OutpostEKF::XVec const& x_; - OutpostEKF::PMat const& P_; - OutpostArmorLayout const& layout_; - int current_armor_id_ { kUnknownArmorId }; - SpinTracker const& spin_; - MatchingConfig const& config_; -}; + auto const H = OutpostEKFParameters::H(x, candidate.phase_offset, candidate.height_offset); + auto const z_hat = OutpostEKFParameters::h(x, candidate.phase_offset, candidate.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 {}; + + return { candidate.armor_id, candidate.phase_offset, candidate.height_offset, *mahalanobis, + candidate.extends_layout, true }; +} } // namespace @@ -291,13 +191,12 @@ struct OutpostRobotState::Impl { OutpostEKFParameters::P_initial_dig().asDiagonal() }; time_stamp = t; - layout = OutpostArmorLayout {}; - layout.slots[0].assigned = true; - - spin.reset(); - update_count = 0; - current_armor_id = 0; - initialized = true; + layout = OutpostArmorLayout {}; + layout.slots[0].phase_offset = 0.0; + layout.slots[0].height_offset = 0.0; + layout.slots[0].assigned = true; + update_count = 0; + initialized = true; } auto predict(TimePoint t) -> void { @@ -312,7 +211,7 @@ struct OutpostRobotState::Impl { auto const dt_s = dt.count(); ekf.predict( - OutpostEKFParameters::f(dt_s, spin.current_sign()), + OutpostEKFParameters::f(dt_s), [dt_s](EKF::XVec const&) { return OutpostEKFParameters::F(dt_s); }, OutpostEKFParameters::Q(dt_s)); } @@ -331,128 +230,71 @@ struct OutpostRobotState::Impl { auto best_match = select_best_match(armors); if (!best_match.has_value()) return false; - apply_association(best_match->decision, best_match->observation); + apply_match(*best_match); return true; } - auto is_converged() const -> bool { return initialized && spin.confirmed_sign.has_value(); } + auto is_converged() const -> bool { + if (!initialized || update_count < 5) return false; + auto const omega = std::abs(ekf.x[6]); + auto const p_omega = ekf.P()(6, 6); + // return true; + return p_omega < 4.0 && (omega < 0.3 || std::abs(omega - rmcs::kOutpostAngularSpeed) < 0.5); + } auto get_snapshot() const -> Snapshot { return detail::make_outpost_snapshot( - ekf.x, color, assigned_count(layout), time_stamp, spin.current_sign(), layout); + ekf.x, color, assigned_count(layout), time_stamp, layout); } - auto distance() const -> double { return std::sqrt(ekf.x[0] * ekf.x[0] + ekf.x[2] * ekf.x[2]); } + auto distance() const -> double { + return initialized ? std::sqrt(ekf.x[0] * ekf.x[0] + ekf.x[2] * ekf.x[2]) + : std::numeric_limits::infinity(); + } 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(); + color = CampColor::UNKNOWN; + ekf = EKF {}; + layout = OutpostArmorLayout {}; + time_stamp = t; + initialized = false; update_count = 0; } auto select_best_match(std::span armors) const -> std::optional { - auto best_match = std::optional {}; - auto matcher = - AssociationEngine { ekf.x, ekf.P(), layout, current_armor_id, spin, config.matching }; + auto best_match = std::optional {}; + auto const candidates = generate_slot_candidates(layout); for (auto const& armor : armors) { - auto observation = make_observation(armor); - auto decision = matcher.decide(observation); - if (!decision.is_valid) continue; - if (best_match.has_value() && decision.error >= best_match->decision.error) continue; - - best_match = BestMatch { observation, decision }; + auto const observation = make_observation(armor); + for (auto const& candidate : candidates) { + auto const decision = + evaluate_candidate(observation, ekf.x, ekf.P(), config.matching, candidate); + if (!decision.is_valid) continue; + if (best_match.has_value() && decision.error >= best_match->decision.error) + continue; + + best_match = BestMatch { observation, decision }; + } } return best_match; } - auto apply_association( - AssociationDecision const& decision, OutpostObservation const& observation) -> void { - auto const stay_ref_phase = [&]() -> std::optional { - if (decision.event != SwitchEvent::Stay) return std::nullopt; - - auto const slot_phase = - std::atan2(ekf.x[2] - observation.xyz[1], ekf.x[0] - observation.xyz[0]); - return rmcs::util::normalize_angle( - slot_phase - layout.slots[decision.armor_id].phase_offset); - }(); - - auto const next_layout = layout_after_association(layout, decision); + auto apply_match(BestMatch const& best_match) -> void { + auto const next_layout = layout_after_match(layout, best_match.decision); ekf.update( - observation.z, - [layout = next_layout, armor_id = decision.armor_id]( + best_match.observation.z, + [layout = next_layout, armor_id = best_match.decision.armor_id]( EKF::XVec const& x) { return OutpostEKFParameters::h(x, layout, armor_id); }, - [layout = next_layout, armor_id = decision.armor_id]( + [layout = next_layout, armor_id = best_match.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 (spin.confirmed_sign == 0) { - update_count++; - return; - } - - auto observe_pending = [&](int sign, int confirm_count) { - if (spin.pending_sign == sign) { - spin.pending_count++; - } else { - spin.pending_sign = sign; - spin.pending_count = 1; - } - - if (spin.pending_count >= confirm_count) { - spin.confirmed_sign = sign; - spin.pending_sign.reset(); - spin.pending_count = 0; - } - }; - - if (decision.event != SwitchEvent::Stay) { - spin.last_stay_sample.reset(); - spin.stop_pending_count = 0; - - if (spin.pending_sign == 0) { - spin.pending_sign.reset(); - spin.pending_count = 0; - } - - if (!spin.confirmed_sign.has_value()) { - observe_pending(decision.inferred_spin_sign, config.spin_confirm_switches); - } - } else { - if (spin.last_stay_sample.has_value()) { - auto const [last_ref_phase, last_stay_stamp] = *spin.last_stay_sample; - auto const dt = rmcs::util::delta_time(time_stamp, last_stay_stamp).count(); - if (dt > 0.0) { - auto const phase_rate = - std::abs(rmcs::util::normalize_angle(*stay_ref_phase - last_ref_phase)) - / dt; - if (phase_rate < config.stop_phase_rate_gate) { - spin.stop_pending_count++; - if (spin.stop_pending_count >= config.stop_confirm_stays) { - spin.confirmed_sign = 0; - spin.pending_sign.reset(); - spin.pending_count = 0; - } - } else { - spin.stop_pending_count = 0; - } - } - } - - spin.last_stay_sample = std::pair { *stay_ref_phase, time_stamp }; - } + best_match.observation.R, OutpostEKFParameters::x_add, + OutpostEKFParameters::z_subtract); + layout = next_layout; update_count++; } @@ -462,8 +304,6 @@ struct OutpostRobotState::Impl { TimePoint time_stamp; bool initialized { false }; - int current_armor_id { kUnknownArmorId }; - SpinTracker spin {}; int update_count { 0 }; TrackingConfig config {}; }; diff --git a/src/module/predictor/outpost/snapshot.cpp b/src/module/predictor/outpost/snapshot.cpp index a748ba42..a2679863 100644 --- a/src/module/predictor/outpost/snapshot.cpp +++ b/src/module/predictor/outpost/snapshot.cpp @@ -12,12 +12,6 @@ namespace rmcs::predictor { namespace { - auto normalize_spin_sign(int spin_sign) -> int { - if (spin_sign > 0) return +1; - if (spin_sign < 0) return -1; - return 0; - } - auto make_armor(DeviceId device, CampColor color, int id) -> Armor3D { auto armor = Armor3D { }; armor.genre = device; @@ -28,10 +22,9 @@ namespace { struct OutpostSnapshotBackend final : ISnapshotBackend { explicit OutpostSnapshotBackend(Snapshot::OutpostEKF::XVec x, CampColor color, - int armor_num, TimePoint stamp, int spin_sign, OutpostArmorLayout layout) noexcept + int armor_num, TimePoint stamp, 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 { @@ -40,13 +33,11 @@ namespace { [[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 armors = std::vector { }; - armors.reserve(max_armors); + armors.reserve(std::clamp(armor_num, 0, OutpostEKFParameters::kOutpostArmorCount)); - for (int id = 0; id < max_armors; ++id) { + for (int id = 0; id < OutpostEKFParameters::kOutpostArmorCount; ++id) { if (!layout.slots[id].assigned) continue; auto armor = make_armor(device, color, id); @@ -64,11 +55,9 @@ namespace { private: auto kinematics_of(Snapshot::OutpostEKF::XVec const& x) const -> Snapshot::Kinematics { - auto const max_armors = - std::clamp(armor_num, 0, OutpostEKFParameters::kOutpostArmorCount); double height_sum = 0.0; int assigned_count = 0; - for (int id = 0; id < max_armors; ++id) { + for (int id = 0; id < OutpostEKFParameters::kOutpostArmorCount; ++id) { if (!layout.slots[id].assigned) continue; height_sum += layout.slots[id].height_offset; assigned_count++; @@ -76,27 +65,24 @@ namespace { auto const center_z = x[4] + (assigned_count == 0 ? 0.0 : height_sum / static_cast(assigned_count)); - auto const angular_velocity = static_cast(spin_sign) * kOutpostAngularSpeed; - return { Eigen::Vector3d { x[0], x[2], center_z }, angular_velocity }; + return { Eigen::Vector3d { x[0], x[2], center_z }, x[6] }; } auto predict_state_at(TimePoint t) const -> Snapshot::OutpostEKF::XVec { auto const dt = util::delta_time(t, stamp).count(); - return OutpostEKFParameters::f(dt, spin_sign)(x); + return OutpostEKFParameters::f(dt)(x); } Snapshot::OutpostEKF::XVec x; - int spin_sign; OutpostArmorLayout layout; }; } // namespace auto detail::make_outpost_snapshot(Snapshot::OutpostEKF::XVec ekf_x, CampColor color, int armor_num, - TimePoint stamp, int outpost_spin_sign, OutpostArmorLayout outpost_layout) noexcept - -> Snapshot { + TimePoint stamp, 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)); + std::move(ekf_x), color, armor_num, stamp, outpost_layout)); } } // namespace rmcs::predictor diff --git a/src/module/predictor/outpost/snapshot.hpp b/src/module/predictor/outpost/snapshot.hpp index 7d4f84e9..8e53aea7 100644 --- a/src/module/predictor/outpost/snapshot.hpp +++ b/src/module/predictor/outpost/snapshot.hpp @@ -7,6 +7,6 @@ namespace rmcs::predictor::detail { auto make_outpost_snapshot(Snapshot::OutpostEKF::XVec ekf_x, CampColor color, int armor_num, - TimePoint stamp, int outpost_spin_sign, OutpostArmorLayout outpost_layout) noexcept -> Snapshot; + TimePoint stamp, OutpostArmorLayout outpost_layout) noexcept -> Snapshot; } // namespace rmcs::predictor::detail diff --git a/src/module/predictor/snapshot.hpp b/src/module/predictor/snapshot.hpp index 3c4c4140..6114e2c7 100644 --- a/src/module/predictor/snapshot.hpp +++ b/src/module/predictor/snapshot.hpp @@ -21,7 +21,7 @@ namespace detail { class Snapshot { public: using NormalEKF = util::EKF<11, 4>; - using OutpostEKF = util::EKF<6, 4>; + using OutpostEKF = util::EKF<7, 4>; struct Kinematics { Eigen::Vector3d center_position; From 9dd23a84a763d5b3da6778c52c8db98ce470ecf5 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 4 Jun 2026 01:09:31 +0800 Subject: [PATCH 02/21] refactor: consolidate structs, rewrite generate_slot_candidates --- src/module/predictor/outpost/robot_state.cpp | 145 ++++++++----------- 1 file changed, 57 insertions(+), 88 deletions(-) diff --git a/src/module/predictor/outpost/robot_state.cpp b/src/module/predictor/outpost/robot_state.cpp index a10c0f3e..26042aed 100644 --- a/src/module/predictor/outpost/robot_state.cpp +++ b/src/module/predictor/outpost/robot_state.cpp @@ -24,14 +24,7 @@ struct OutpostObservation { Eigen::Vector3d xyz; }; -struct SlotCandidate { - int armor_id { kUnknownArmorId }; - double phase_offset { 0.0 }; - double height_offset { 0.0 }; - bool extends_layout { false }; -}; - -struct MatchDecision { +struct MatchCandidate { int armor_id { kUnknownArmorId }; double phase_offset { 0.0 }; double height_offset { 0.0 }; @@ -40,20 +33,6 @@ struct MatchDecision { bool is_valid { false }; }; -struct BestMatch { - OutpostObservation observation; - MatchDecision decision; -}; - -struct MatchingConfig { - double mahalanobis_gate { 25.0 }; -}; - -struct TrackingConfig { - std::chrono::duration reset_interval { 1.5 }; - MatchingConfig matching {}; -}; - // 模板 A: slot1 = +H, slot2 = -H // 模板 B: slot1 = -H, slot2 = +H enum class HeightTemplate { Unknown, PositiveOnSlot1, NegativeOnSlot1 }; @@ -90,14 +69,14 @@ auto assigned_count(OutpostArmorLayout const& layout) -> int { return static_cast(std::ranges::count(layout.slots, true, &OutpostArmorSlot::assigned)); } -auto layout_after_match(OutpostArmorLayout const& layout, MatchDecision const& decision) +auto layout_after_match(OutpostArmorLayout const& layout, MatchCandidate const& candidate) -> OutpostArmorLayout { auto next_layout = layout; - if (!decision.extends_layout) return next_layout; + if (!candidate.extends_layout) return next_layout; - next_layout.slots[decision.armor_id].phase_offset = decision.phase_offset; - next_layout.slots[decision.armor_id].height_offset = decision.height_offset; - next_layout.slots[decision.armor_id].assigned = true; + next_layout.slots[candidate.armor_id].phase_offset = candidate.phase_offset; + next_layout.slots[candidate.armor_id].height_offset = candidate.height_offset; + next_layout.slots[candidate.armor_id].assigned = true; return next_layout; } @@ -114,69 +93,60 @@ auto slot_phase_offset(int armor_id) -> double { } } -auto generate_slot_candidates(OutpostArmorLayout const& layout) -> std::array { +auto generate_slot_candidates(OutpostArmorLayout const& layout) -> std::array { using rmcs::kOutpostArmorHeightStep; + constexpr auto kPhaseStep = OutpostEKFParameters::kPhaseStep; - auto candidates = std::array {}; - auto append_at = int { 0 }; - auto append = [&](int armor_id, double height_offset, bool extends_layout) { - candidates[append_at++] = { - armor_id, - slot_phase_offset(armor_id), - height_offset, - extends_layout, - }; - }; - - for (int armor_id = 0; armor_id < OutpostEKFParameters::kOutpostArmorCount; ++armor_id) { - if (!layout.slots[armor_id].assigned) continue; - append(armor_id, layout.slots[armor_id].height_offset, false); - } + auto candidates = std::array {}; + auto n = int { 0 }; - if (assigned_count(layout) == OutpostEKFParameters::kOutpostArmorCount) return candidates; + for (int id = 0; id < OutpostEKFParameters::kOutpostArmorCount; ++id) { + if (!layout.slots[id].assigned) continue; + candidates[n++] = MatchCandidate { id, slot_phase_offset(id), + layout.slots[id].height_offset, 0.0, false, false }; + } - auto const height_slot1 = [&](HeightTemplate t) -> double { - return t == HeightTemplate::PositiveOnSlot1 ? kOutpostArmorHeightStep - : -kOutpostArmorHeightStep; - }; + if (n == OutpostEKFParameters::kOutpostArmorCount) return candidates; auto const tmpl = resolve_template(layout); - if (!layout.slots[1].assigned) { - if (tmpl == HeightTemplate::Unknown) { - append(1, kOutpostArmorHeightStep, true); - append(1, -kOutpostArmorHeightStep, true); - } else { - append(1, height_slot1(tmpl), true); - } - } + for (int id : { 1, 2 }) { + if (layout.slots[id].assigned) continue; + + auto const phase = (id == 1) ? +kPhaseStep : -kPhaseStep; - if (!layout.slots[2].assigned) { if (tmpl == HeightTemplate::Unknown) { - append(2, kOutpostArmorHeightStep, true); - append(2, -kOutpostArmorHeightStep, true); - } else { - append(2, -height_slot1(tmpl), true); + candidates[n++] = + MatchCandidate { id, phase, +kOutpostArmorHeightStep, 0.0, true, false }; + candidates[n++] = + MatchCandidate { id, phase, -kOutpostArmorHeightStep, 0.0, true, false }; + continue; } + + auto const h1 = (tmpl == HeightTemplate::PositiveOnSlot1) ? +kOutpostArmorHeightStep + : -kOutpostArmorHeightStep; + candidates[n++] = MatchCandidate { id, phase, (id == 1) ? h1 : -h1, 0.0, true, false }; } return candidates; } auto evaluate_candidate(OutpostObservation const& observation, OutpostEKF::XVec const& x, - OutpostEKF::PMat const& P, MatchingConfig const& config, SlotCandidate const& candidate) - -> MatchDecision { - if (candidate.armor_id == kUnknownArmorId) return {}; + OutpostEKF::PMat const& P, double mahalanobis_gate, MatchCandidate const& candidate) + -> MatchCandidate { + auto result = candidate; + if (result.armor_id == kUnknownArmorId) return result; - auto const H = OutpostEKFParameters::H(x, candidate.phase_offset, candidate.height_offset); - auto const z_hat = OutpostEKFParameters::h(x, candidate.phase_offset, candidate.height_offset); + auto const H = OutpostEKFParameters::H(x, result.phase_offset, result.height_offset); + auto const z_hat = OutpostEKFParameters::h(x, result.phase_offset, result.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 {}; + if (!mahalanobis.has_value() || *mahalanobis > mahalanobis_gate) return result; - return { candidate.armor_id, candidate.phase_offset, candidate.height_offset, *mahalanobis, - candidate.extends_layout, true }; + result.error = *mahalanobis; + result.is_valid = true; + return result; } } // namespace @@ -204,7 +174,7 @@ struct OutpostRobotState::Impl { if (initialized) { auto dt = rmcs::util::delta_time(t, time_stamp); - if (dt > config.reset_interval) { + if (dt > reset_interval) { reset_runtime_state(t); return; } @@ -230,7 +200,7 @@ struct OutpostRobotState::Impl { auto best_match = select_best_match(armors); if (!best_match.has_value()) return false; - apply_match(*best_match); + apply_match(best_match->first, best_match->second); return true; } @@ -238,7 +208,6 @@ struct OutpostRobotState::Impl { if (!initialized || update_count < 5) return false; auto const omega = std::abs(ekf.x[6]); auto const p_omega = ekf.P()(6, 6); - // return true; return p_omega < 4.0 && (omega < 0.3 || std::abs(omega - rmcs::kOutpostAngularSpeed) < 0.5); } @@ -262,37 +231,36 @@ struct OutpostRobotState::Impl { update_count = 0; } - auto select_best_match(std::span armors) const -> std::optional { - auto best_match = std::optional {}; + auto select_best_match(std::span armors) const + -> std::optional> { + auto best_match = std::optional> {}; auto const candidates = generate_slot_candidates(layout); for (auto const& armor : armors) { auto const observation = make_observation(armor); for (auto const& candidate : candidates) { - auto const decision = - evaluate_candidate(observation, ekf.x, ekf.P(), config.matching, candidate); - if (!decision.is_valid) continue; - if (best_match.has_value() && decision.error >= best_match->decision.error) - continue; + auto match = + evaluate_candidate(observation, ekf.x, ekf.P(), mahalanobis_gate, candidate); + if (!match.is_valid) continue; + if (best_match.has_value() && match.error >= best_match->second.error) continue; - best_match = BestMatch { observation, decision }; + best_match = std::pair { observation, match }; } } return best_match; } - auto apply_match(BestMatch const& best_match) -> void { - auto const next_layout = layout_after_match(layout, best_match.decision); + auto apply_match(OutpostObservation const& observation, MatchCandidate const& match) -> void { + auto const next_layout = layout_after_match(layout, match); ekf.update( - best_match.observation.z, - [layout = next_layout, armor_id = best_match.decision.armor_id]( + observation.z, + [layout = next_layout, armor_id = match.armor_id]( EKF::XVec const& x) { return OutpostEKFParameters::h(x, layout, armor_id); }, - [layout = next_layout, armor_id = best_match.decision.armor_id]( + [layout = next_layout, armor_id = match.armor_id]( EKF::XVec const& x) { return OutpostEKFParameters::H(x, layout, armor_id); }, - best_match.observation.R, OutpostEKFParameters::x_add, - OutpostEKFParameters::z_subtract); + observation.R, OutpostEKFParameters::x_add, OutpostEKFParameters::z_subtract); layout = next_layout; update_count++; @@ -305,7 +273,8 @@ struct OutpostRobotState::Impl { bool initialized { false }; int update_count { 0 }; - TrackingConfig config {}; + std::chrono::duration reset_interval { 1.5 }; + double mahalanobis_gate { 25.0 }; }; OutpostRobotState::OutpostRobotState() noexcept From f07e18ac3ec3f069cf8a321230f6f94a9be5ea2b Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 4 Jun 2026 17:09:31 +0800 Subject: [PATCH 03/21] refactor: simplify snapshot, remove detail namespace and snapshot.hpp files --- src/module/predictor/outpost/robot_state.cpp | 13 ++++++++----- src/module/predictor/outpost/snapshot.cpp | 15 ++++++++------- src/module/predictor/outpost/snapshot.hpp | 12 ------------ src/module/predictor/regular/robot_state.cpp | 8 ++++++-- src/module/predictor/regular/snapshot.cpp | 10 ++++++---- src/module/predictor/regular/snapshot.hpp | 12 ------------ src/module/predictor/snapshot.cpp | 6 +----- src/module/predictor/snapshot.hpp | 17 ++++------------- 8 files changed, 33 insertions(+), 60 deletions(-) delete mode 100644 src/module/predictor/outpost/snapshot.hpp delete mode 100644 src/module/predictor/regular/snapshot.hpp diff --git a/src/module/predictor/outpost/robot_state.cpp b/src/module/predictor/outpost/robot_state.cpp index 26042aed..b7b9efc6 100644 --- a/src/module/predictor/outpost/robot_state.cpp +++ b/src/module/predictor/outpost/robot_state.cpp @@ -7,12 +7,16 @@ #include #include -#include "module/predictor/outpost/snapshot.hpp" #include "utility/math/mahalanobis.hpp" #include "utility/time.hpp" using namespace rmcs::predictor; +namespace rmcs::predictor { +auto make_outpost_snapshot(Snapshot::OutpostEKF::XVec ekf_x, CampColor color, int armor_num, + TimePoint stamp, OutpostArmorLayout outpost_layout) noexcept -> Snapshot; +} + namespace { constexpr int kUnknownArmorId = -1; @@ -205,15 +209,14 @@ struct OutpostRobotState::Impl { } auto is_converged() const -> bool { - if (!initialized || update_count < 5) return false; + if (!initialized) return false; auto const omega = std::abs(ekf.x[6]); auto const p_omega = ekf.P()(6, 6); - return p_omega < 4.0 && (omega < 0.3 || std::abs(omega - rmcs::kOutpostAngularSpeed) < 0.5); + return p_omega < 4.0 && (omega < 0.3 || std::abs(omega - rmcs::kOutpostAngularSpeed) < 1.3); } auto get_snapshot() const -> Snapshot { - return detail::make_outpost_snapshot( - ekf.x, color, assigned_count(layout), time_stamp, layout); + return make_outpost_snapshot(ekf.x, color, assigned_count(layout), time_stamp, layout); } auto distance() const -> double { diff --git a/src/module/predictor/outpost/snapshot.cpp b/src/module/predictor/outpost/snapshot.cpp index a2679863..a32306e3 100644 --- a/src/module/predictor/outpost/snapshot.cpp +++ b/src/module/predictor/outpost/snapshot.cpp @@ -1,4 +1,6 @@ -#include "module/predictor/outpost/snapshot.hpp" +#include "module/predictor/outpost/armor_layout.hpp" +#include "module/predictor/snapshot.hpp" +#include "utility/robot/color.hpp" #include #include @@ -11,9 +13,8 @@ namespace rmcs::predictor { namespace { - auto make_armor(DeviceId device, CampColor color, int id) -> Armor3D { - auto armor = Armor3D { }; + auto armor = Armor3D {}; armor.genre = device; armor.color = camp_color2armor_color(color); armor.id = id; @@ -34,7 +35,7 @@ namespace { [[nodiscard]] auto predicted_armors(TimePoint t) const -> std::vector override { auto const predicted_x = predict_state_at(t); - auto armors = std::vector { }; + auto armors = std::vector {}; armors.reserve(std::clamp(armor_num, 0, OutpostEKFParameters::kOutpostArmorCount)); for (int id = 0; id < OutpostEKFParameters::kOutpostArmorCount; ++id) { @@ -79,10 +80,10 @@ namespace { } // namespace -auto detail::make_outpost_snapshot(Snapshot::OutpostEKF::XVec ekf_x, CampColor color, int armor_num, +auto make_outpost_snapshot(Snapshot::OutpostEKF::XVec ekf_x, CampColor color, int armor_num, TimePoint stamp, OutpostArmorLayout outpost_layout) noexcept -> Snapshot { - return detail::make_snapshot(std::make_unique( - std::move(ekf_x), color, armor_num, stamp, outpost_layout)); + return Snapshot { std::make_unique( + std::move(ekf_x), color, armor_num, stamp, outpost_layout) }; } } // namespace rmcs::predictor diff --git a/src/module/predictor/outpost/snapshot.hpp b/src/module/predictor/outpost/snapshot.hpp deleted file mode 100644 index 8e53aea7..00000000 --- a/src/module/predictor/outpost/snapshot.hpp +++ /dev/null @@ -1,12 +0,0 @@ -#pragma once - -#include "module/predictor/outpost/armor_layout.hpp" -#include "module/predictor/snapshot.hpp" -#include "utility/robot/color.hpp" - -namespace rmcs::predictor::detail { - -auto make_outpost_snapshot(Snapshot::OutpostEKF::XVec ekf_x, CampColor color, int armor_num, - TimePoint stamp, OutpostArmorLayout outpost_layout) noexcept -> Snapshot; - -} // namespace rmcs::predictor::detail diff --git a/src/module/predictor/regular/robot_state.cpp b/src/module/predictor/regular/robot_state.cpp index d97df137..0b4a6d7c 100644 --- a/src/module/predictor/regular/robot_state.cpp +++ b/src/module/predictor/regular/robot_state.cpp @@ -6,11 +6,15 @@ #include #include -#include "module/predictor/regular/snapshot.hpp" #include "utility/time.hpp" using namespace rmcs::predictor; +namespace rmcs::predictor { +auto make_regular_snapshot(Snapshot::NormalEKF::XVec ekf_x, DeviceId device, CampColor color, + int armor_num, TimePoint stamp) noexcept -> Snapshot; +} + struct RegularRobotState::Impl { struct MatchDecision { int matched_armor_id { kUnknownMatchedArmorId }; @@ -109,7 +113,7 @@ struct RegularRobotState::Impl { 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); + return make_regular_snapshot(ekf.x, device, color, armor_num, time_stamp); } auto distance() const -> double { diff --git a/src/module/predictor/regular/snapshot.cpp b/src/module/predictor/regular/snapshot.cpp index 77207669..2eb9b606 100644 --- a/src/module/predictor/regular/snapshot.cpp +++ b/src/module/predictor/regular/snapshot.cpp @@ -1,4 +1,6 @@ -#include "module/predictor/regular/snapshot.hpp" +#include "module/predictor/snapshot.hpp" +#include "utility/robot/color.hpp" +#include "utility/robot/id.hpp" #include @@ -64,10 +66,10 @@ namespace { } // namespace -auto detail::make_regular_snapshot(Snapshot::NormalEKF::XVec ekf_x, DeviceId device, +auto make_regular_snapshot(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)); + return Snapshot { std::make_unique( + std::move(ekf_x), device, color, armor_num, stamp) }; } } // namespace rmcs::predictor diff --git a/src/module/predictor/regular/snapshot.hpp b/src/module/predictor/regular/snapshot.hpp deleted file mode 100644 index da77793d..00000000 --- a/src/module/predictor/regular/snapshot.hpp +++ /dev/null @@ -1,12 +0,0 @@ -#pragma once - -#include "module/predictor/snapshot.hpp" -#include "utility/robot/color.hpp" -#include "utility/robot/id.hpp" - -namespace rmcs::predictor::detail { - -auto make_regular_snapshot(Snapshot::NormalEKF::XVec ekf_x, DeviceId device, CampColor color, - int armor_num, TimePoint stamp) noexcept -> Snapshot; - -} // namespace rmcs::predictor::detail diff --git a/src/module/predictor/snapshot.cpp b/src/module/predictor/snapshot.cpp index b8217c9b..29416aa0 100644 --- a/src/module/predictor/snapshot.cpp +++ b/src/module/predictor/snapshot.cpp @@ -24,12 +24,8 @@ namespace { } // namespace -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)); + return Snapshot { std::make_unique(stamp) }; } Snapshot::Snapshot(std::unique_ptr backend) noexcept diff --git a/src/module/predictor/snapshot.hpp b/src/module/predictor/snapshot.hpp index 6114e2c7..b910bcfd 100644 --- a/src/module/predictor/snapshot.hpp +++ b/src/module/predictor/snapshot.hpp @@ -6,18 +6,14 @@ #include "utility/clock.hpp" #include "utility/math/kalman_filter/ekf.hpp" -#include "utility/robot/id.hpp" #include "utility/robot/armor.hpp" +#include "utility/robot/id.hpp" namespace rmcs::predictor { struct ISnapshotBackend; class Snapshot; -namespace detail { - auto make_snapshot(std::unique_ptr backend) noexcept -> Snapshot; -} - class Snapshot { public: using NormalEKF = util::EKF<11, 4>; @@ -28,30 +24,25 @@ class Snapshot { double angular_velocity; }; - static auto empty(TimePoint stamp) noexcept -> Snapshot; - + explicit Snapshot(std::unique_ptr backend) noexcept; Snapshot(Snapshot const&) = delete; Snapshot(Snapshot&&) noexcept; Snapshot& operator=(Snapshot const&) = delete; Snapshot& operator=(Snapshot&&) noexcept; ~Snapshot() noexcept; + static auto empty(TimePoint stamp) noexcept -> 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 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; }; } From a669f8d643584c53287a8e4b65e55f51161e3049 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 4 Jun 2026 18:01:18 +0800 Subject: [PATCH 04/21] refactor: clean up is_converged, tune mahalanobis gate to 10.0 --- src/module/predictor/outpost/robot_state.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/module/predictor/outpost/robot_state.cpp b/src/module/predictor/outpost/robot_state.cpp index b7b9efc6..05e7e5c8 100644 --- a/src/module/predictor/outpost/robot_state.cpp +++ b/src/module/predictor/outpost/robot_state.cpp @@ -210,9 +210,13 @@ struct OutpostRobotState::Impl { auto is_converged() const -> bool { if (!initialized) return false; - auto const omega = std::abs(ekf.x[6]); - auto const p_omega = ekf.P()(6, 6); - return p_omega < 4.0 && (omega < 0.3 || std::abs(omega - rmcs::kOutpostAngularSpeed) < 1.3); + + constexpr int kStateOmega = 6; // x vx y vy z a w + auto const angular_speed = std::abs(ekf.x[kStateOmega]); + auto const angular_speed_var = ekf.P()(kStateOmega, kStateOmega); + return angular_speed_var < 4.0 + && (angular_speed < rmcs::util::deg2rad(30) + || std::abs(angular_speed - rmcs::kOutpostAngularSpeed) < rmcs::util::deg2rad(30)); } auto get_snapshot() const -> Snapshot { @@ -277,7 +281,7 @@ struct OutpostRobotState::Impl { bool initialized { false }; int update_count { 0 }; std::chrono::duration reset_interval { 1.5 }; - double mahalanobis_gate { 25.0 }; + double mahalanobis_gate { 10.0 }; }; OutpostRobotState::OutpostRobotState() noexcept From d921a98e63e5df217b24d31140ea660daa9a90e0 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 4 Jun 2026 22:50:52 +0800 Subject: [PATCH 05/21] fix: use return value of into_odom_link and fix adapter include --- src/component.cpp | 4 ++-- src/kernel/pose_estimator.cpp | 34 +++++++++++++++++----------------- 2 files changed, 19 insertions(+), 19 deletions(-) diff --git a/src/component.cpp b/src/component.cpp index 10963809..8724164c 100644 --- a/src/component.cpp +++ b/src/component.cpp @@ -4,7 +4,7 @@ #pragma GCC diagnostic ignored "-Wdeprecated-declarations" #endif -#include "adapter/sentry.hpp" +#include "adapter/adapter.hpp" #include "kernel/feishu.hpp" #include "utility/rclcpp/node.hpp" #include "utility/shared/context.hpp" @@ -48,7 +48,7 @@ class AutoAimComponent final : public rmcs_executor::Component { InputInterface robot_id; auto make_context() const { - auto context = SystemContext { }; + auto context = SystemContext {}; context.timestamp = Clock::now(); diff --git a/src/kernel/pose_estimator.cpp b/src/kernel/pose_estimator.cpp index 951840ec..c1239d60 100644 --- a/src/kernel/pose_estimator.cpp +++ b/src/kernel/pose_estimator.cpp @@ -33,7 +33,7 @@ struct PoseEstimator::Impl { bool optimized_bar_is_right { false }; bool optimized_bar_is_upper { false }; - auto clear() -> void { *this = { }; } + auto clear() -> void { *this = {}; } }; struct Config : util::Serializable { @@ -50,15 +50,15 @@ struct PoseEstimator::Impl { }; }; - DebugState debug_state { }; + DebugState debug_state {}; Config config; CameraFeature camera_feature; - PnpSolution pnp_solution { }; - OutpostDistanceOptimizer outpost_distance_optimizer { }; - YawOptimizer yaw_optimizer { }; - AdjacencyLightbarFinder adjacency_finder { }; + PnpSolution pnp_solution {}; + OutpostDistanceOptimizer outpost_distance_optimizer {}; + YawOptimizer yaw_optimizer {}; + AdjacencyLightbarFinder adjacency_finder {}; RclcppNode visual_node { "PoseEstimatorVisual" }; std::unique_ptr pre_optimized_outpost; @@ -116,7 +116,7 @@ struct PoseEstimator::Impl { outpost_distance_optimizer.input.armor_thickness = config.outpost_armor_thickness; adjacency_finder.set_armor_thickness(config.outpost_armor_thickness); - return { }; + return {}; } catch (const std::exception& e) { return std::unexpected { e.what() }; } @@ -133,13 +133,13 @@ struct PoseEstimator::Impl { input.camera.camera_translation = Eigen::Vector3d { -(q_odom_to_camera * camera_translation).eval() }; - auto result = std::vector { }; + auto result = std::vector {}; for (auto&& [index, armor] : armors | std::views::enumerate) { const auto small = armor.shape == ArmorShape::SMALL; const auto shape = small ? kSmallArmorShapeOpenCV : kLargeArmorShapeOpenCV; - auto armor_3d = Armor3D { }; + auto armor_3d = Armor3D {}; armor_3d.id = static_cast(index); { // pnp @@ -157,7 +157,7 @@ struct PoseEstimator::Impl { armor_3d.translation = pnp_solution.result.translation; armor_3d.orientation = pnp_solution.result.orientation; } - into_odom_link(armor_3d); + armor_3d = into_odom_link(armor_3d); if (config.yaw_optimizer) { // yaw input.armor_shape = shape; @@ -231,11 +231,11 @@ struct PoseEstimator::Impl { auto into_odom_link(const Armor3D& armor) const -> Armor3D { auto transformed = armor; - auto position = Eigen::Vector3d { }; + auto position = Eigen::Vector3d {}; transformed.translation.copy_to(position); transformed.translation = camera_orientation * position + camera_translation; - auto quat = Eigen::Quaterniond { }; + auto quat = Eigen::Quaterniond {}; transformed.orientation.copy_to(quat); transformed.orientation = camera_orientation * quat; @@ -243,7 +243,7 @@ struct PoseEstimator::Impl { } auto into_odom_link(std::span armors) const { - auto result = std::vector { }; + auto result = std::vector {}; for (const auto& armor : armors) { result.emplace_back(into_odom_link(armor)); } @@ -254,7 +254,7 @@ struct PoseEstimator::Impl { if (debug_state.optimized_outpost.has_value()) { const auto& armor = *debug_state.optimized_outpost; - auto solution = NeighborBarSolution { }; + auto solution = NeighborBarSolution {}; solution.input.source = armor; solution.input.in_right = debug_state.optimized_bar_is_right; solution.input.armor_thickness = config.outpost_armor_thickness; @@ -265,7 +265,7 @@ struct PoseEstimator::Impl { const auto& away_bar = debug_state.optimized_bar_is_upper ? solution.result.upper_away : solution.result.lower_away; - auto object_points = std::vector { }; + auto object_points = std::vector {}; object_points.reserve(4); for (const auto& point : { near_bar.first, near_bar.second, away_bar.first, away_bar.second }) { @@ -274,7 +274,7 @@ struct PoseEstimator::Impl { static_cast(p.z())); } - auto projected = std::vector { }; + auto projected = std::vector {}; cv::projectPoints(object_points, cv::Vec3d { 0.0, 0.0, 0.0 }, cv::Vec3d { 0.0, 0.0, 0.0 }, camera_feature.intrinsic(), camera_feature.distortion(), projected); @@ -308,7 +308,7 @@ struct PoseEstimator::Impl { if (debug_state.optimized_outpost.has_value()) { const auto& armor = *debug_state.optimized_outpost; - auto solution = NeighborBarSolution { }; + auto solution = NeighborBarSolution {}; solution.input.source = armor; solution.input.in_right = debug_state.optimized_bar_is_right; solution.input.armor_thickness = config.outpost_armor_thickness; From 9112f67a7e2e23b87e9052737fd6fce74c9bb0f4 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 4 Jun 2026 23:50:36 +0800 Subject: [PATCH 06/21] fix: publish full camera_link TF including translation --- src/kernel/visualization.cpp | 10 ++++++---- src/kernel/visualization.hpp | 2 +- src/runtime.cpp | 23 ++++++++++++----------- 3 files changed, 19 insertions(+), 16 deletions(-) diff --git a/src/kernel/visualization.cpp b/src/kernel/visualization.cpp index 31f2d424..0094922a 100644 --- a/src/kernel/visualization.cpp +++ b/src/kernel/visualization.cpp @@ -188,10 +188,11 @@ struct Visualization::Impl { mpc_plan->publish_planned_pitch(pitch, pitch_rate, pitch_acc); } - auto update_camera_pose(const Orientation& orientation) const -> void { + auto update_camera_pose(const Translation& translation, const Orientation& orientation) const + -> void { if (!is_initialized) return; if (!config.camera_pose_enabled) return; - camera_transform->move(Translation::kZero(), orientation); + camera_transform->move(translation, orientation); camera_transform->update(); } }; @@ -224,8 +225,9 @@ auto Visualization::update_mpc_plan(double yaw, double pitch, double yaw_rate, d pimpl->update_mpc_plan(yaw, pitch, yaw_rate, pitch_rate, yaw_acc, pitch_acc); } -auto Visualization::update_camera_pose(const Orientation& orientation) const -> void { - pimpl->update_camera_pose(orientation); +auto Visualization::update_camera_pose( + const Translation& translation, const Orientation& orientation) const -> void { + pimpl->update_camera_pose(translation, orientation); } Visualization::Visualization() noexcept diff --git a/src/kernel/visualization.hpp b/src/kernel/visualization.hpp index c6550ba4..0ecb03f2 100644 --- a/src/kernel/visualization.hpp +++ b/src/kernel/visualization.hpp @@ -37,7 +37,7 @@ class Visualization { auto update_mpc_plan(double yaw, double pitch, double yaw_rate, double pitch_rate, double yaw_acc, double pitch_acc) const -> void; - auto update_camera_pose(const Orientation&) const -> void; + auto update_camera_pose(const Translation&, const Orientation&) const -> void; }; } // namespace rmcs::kernel diff --git a/src/runtime.cpp b/src/runtime.cpp index 398196c6..c1d6e539 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -37,13 +37,13 @@ auto main() -> int { node.set_pub_topic_prefix("/rmcs/auto_aim/"); /// Runtime - auto feishu = kernel::Feishu { }; - auto capturer = kernel::Capturer { }; - auto identifier = kernel::Identifier { }; - auto tracker = kernel::Tracker { }; - auto pose_estimator = kernel::PoseEstimator { }; - auto fire_control = kernel::FireControl { }; - auto visualization = kernel::Visualization { }; + auto feishu = kernel::Feishu {}; + auto capturer = kernel::Capturer {}; + auto identifier = kernel::Identifier {}; + auto tracker = kernel::Tracker {}; + auto pose_estimator = kernel::PoseEstimator {}; + auto fire_control = kernel::FireControl {}; + auto visualization = kernel::Visualization {}; /// Configure auto configs = util::configs(); @@ -102,7 +102,7 @@ auto main() -> int { handle_result("visualization", result); } - auto framerate = FramerateCounter { }; + auto framerate = FramerateCounter {}; framerate.set_interval(std::chrono::seconds { 5 }); while (util::get_running()) { @@ -121,7 +121,8 @@ auto main() -> int { context = *closest; } - visualization.update_camera_pose(context.camera_transform.orientation); + visualization.update_camera_pose( + context.camera_transform.translation, context.camera_transform.orientation); if (framerate.tick()) { node.info("Autoaim Framerate: {}", framerate.fps()); @@ -133,7 +134,7 @@ auto main() -> int { /// 1. Identify Armor /// - auto armors_2d = Armor2Ds { }; + auto armors_2d = Armor2Ds {}; { auto result = identifier.sync_identify(*image); if (!result.has_value()) continue; // 一般不会推理出错喵~ @@ -163,7 +164,7 @@ auto main() -> int { /// 2. Transform 2d to 3d /// - auto armors_3d = Armor3Ds { }; + auto armors_3d = Armor3Ds {}; { pose_estimator.update_camera_transform(context.camera_transform); From 4fb640d17532af16ece43985b58b74bd43983646 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Fri, 5 Jun 2026 01:11:46 +0800 Subject: [PATCH 07/21] fix: keep outpost optimization in camera frame, convert to odom after --- src/kernel/pose_estimator.cpp | 28 ++++++++++++++++++++++------ 1 file changed, 22 insertions(+), 6 deletions(-) diff --git a/src/kernel/pose_estimator.cpp b/src/kernel/pose_estimator.cpp index c1239d60..16f73580 100644 --- a/src/kernel/pose_estimator.cpp +++ b/src/kernel/pose_estimator.cpp @@ -184,12 +184,14 @@ struct PoseEstimator::Impl { armor3ds, [](const Armor3D& armor) { return armor.genre == DeviceId::OUTPOST; }); if (outpost3d != armor3ds.end() && outpost2d != armor2ds.end()) { - debug_state.pre_optimized_outpost = *outpost3d; + auto camera_frame_outpost = into_camera_link(*outpost3d); - if (auto lightbar = adjacency_finder.find(image, *outpost2d, *outpost3d)) { + debug_state.pre_optimized_outpost = camera_frame_outpost; + + if (auto lightbar = adjacency_finder.find(image, *outpost2d, camera_frame_outpost)) { if (config.distance_optimizer) { auto& input = outpost_distance_optimizer.input; - input.initial = *outpost3d; + input.initial = camera_frame_outpost; input.armor = *outpost2d; input.upper_point = lightbar->upper; @@ -199,11 +201,11 @@ struct PoseEstimator::Impl { input.is_upper = lightbar->is_upper; if (outpost_distance_optimizer.solve()) { - *outpost3d = outpost_distance_optimizer.result.armor; - - debug_state.optimized_outpost = *outpost3d; + debug_state.optimized_outpost = outpost_distance_optimizer.result.armor; debug_state.optimized_bar_is_right = lightbar->is_right; debug_state.optimized_bar_is_upper = lightbar->is_upper; + + *outpost3d = into_odom_link(outpost_distance_optimizer.result.armor); } } @@ -250,6 +252,20 @@ struct PoseEstimator::Impl { return result; } + auto into_camera_link(const Armor3D& armor) const -> Armor3D { + auto transformed = armor; + + auto position = Eigen::Vector3d {}; + transformed.translation.copy_to(position); + transformed.translation = camera_orientation.inverse() * (position - camera_translation); + + auto quat = Eigen::Quaterniond {}; + transformed.orientation.copy_to(quat); + transformed.orientation = camera_orientation.inverse() * quat; + + return transformed; + } + auto draw_debug(Image& image) -> void { if (debug_state.optimized_outpost.has_value()) { const auto& armor = *debug_state.optimized_outpost; From 02620b1fd80187fbcb5a02d12651b5f25f6ee1d7 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Fri, 5 Jun 2026 02:13:13 +0800 Subject: [PATCH 08/21] tune: adjust outpost angles, simplify convergence check, tighten mahalanobis gate --- config/config.yaml | 6 +++--- src/module/predictor/outpost/robot_state.cpp | 11 +++-------- 2 files changed, 6 insertions(+), 11 deletions(-) diff --git a/config/config.yaml b/config/config.yaml index e8ee0a3e..5b5337eb 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -31,7 +31,7 @@ capturer: # 替换为你具体的路径 location: "/path/to/your/video" # double 帧率 - frame_rate: 40 + frame_rate: 120 # bool 是否循环播放 loop_play: true # bool 是否允许跳帧以满足实时性 @@ -94,8 +94,8 @@ fire_control: aim_point_chooser: coming_angle: 70.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 mpc: mpc_max_yaw_acc: 50.0 diff --git a/src/module/predictor/outpost/robot_state.cpp b/src/module/predictor/outpost/robot_state.cpp index 05e7e5c8..030d8f91 100644 --- a/src/module/predictor/outpost/robot_state.cpp +++ b/src/module/predictor/outpost/robot_state.cpp @@ -208,15 +208,10 @@ struct OutpostRobotState::Impl { return true; } + // TODO:添加收敛条件 auto is_converged() const -> bool { if (!initialized) return false; - - constexpr int kStateOmega = 6; // x vx y vy z a w - auto const angular_speed = std::abs(ekf.x[kStateOmega]); - auto const angular_speed_var = ekf.P()(kStateOmega, kStateOmega); - return angular_speed_var < 4.0 - && (angular_speed < rmcs::util::deg2rad(30) - || std::abs(angular_speed - rmcs::kOutpostAngularSpeed) < rmcs::util::deg2rad(30)); + return true; } auto get_snapshot() const -> Snapshot { @@ -281,7 +276,7 @@ struct OutpostRobotState::Impl { bool initialized { false }; int update_count { 0 }; std::chrono::duration reset_interval { 1.5 }; - double mahalanobis_gate { 10.0 }; + double mahalanobis_gate { 5.0 }; }; OutpostRobotState::OutpostRobotState() noexcept From 7f062e44d5c438306db4216c22758b8a3c47318e Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sun, 7 Jun 2026 17:39:31 +0800 Subject: [PATCH 09/21] chore: rename Armor --- src/kernel/identifier.cpp | 6 ++-- src/kernel/identifier.hpp | 2 +- src/kernel/pose_estimator.cpp | 36 +++++++++---------- src/kernel/pose_estimator.hpp | 8 ++--- src/kernel/tracker.cpp | 8 ++--- src/kernel/tracker.hpp | 4 +-- src/kernel/visualization.cpp | 8 ++--- src/kernel/visualization.hpp | 4 +-- .../debug/visualization/armor_visualizer.cpp | 6 ++-- .../debug/visualization/armor_visualizer.hpp | 2 +- src/module/fire_control/aim_point_chooser.cpp | 8 ++--- src/module/fire_control/aim_point_chooser.hpp | 4 +-- src/module/identifier/adjacency_lightbar.cpp | 4 +-- src/module/identifier/adjacency_lightbar.hpp | 2 +- src/module/identifier/armor_detection.cpp | 16 ++++----- src/module/identifier/armor_detection.hpp | 2 +- src/module/identifier/green_light_locator.cpp | 6 ++-- src/module/identifier/green_light_locator.hpp | 2 +- .../predictor/backend/robot_state_backend.cpp | 4 +-- .../predictor/backend/robot_state_backend.hpp | 4 +-- .../predictor/backend/snapshot_backend.hpp | 2 +- .../predictor/outpost/ekf_parameter.hpp | 2 +- src/module/predictor/outpost/robot_state.cpp | 12 +++---- src/module/predictor/outpost/robot_state.hpp | 4 +-- src/module/predictor/outpost/snapshot.cpp | 8 ++--- .../predictor/regular/ekf_parameter.hpp | 2 +- src/module/predictor/regular/robot_state.cpp | 14 ++++---- src/module/predictor/regular/robot_state.hpp | 4 +-- src/module/predictor/regular/snapshot.cpp | 8 ++--- src/module/predictor/robot_state.cpp | 12 +++---- src/module/predictor/robot_state.hpp | 4 +-- src/module/predictor/snapshot.cpp | 4 +-- src/module/predictor/snapshot.hpp | 2 +- src/module/tracker/armor_filter.cpp | 6 ++-- src/module/tracker/armor_filter.hpp | 2 +- src/module/tracker/decider.cpp | 8 ++--- src/module/tracker/decider.hpp | 2 +- src/runtime.cpp | 4 +-- src/utility/image/armor.cpp | 2 +- src/utility/image/armor.hpp | 2 +- src/utility/math/outpost.hpp | 2 +- .../solve_pnp/outpost_distance_optimizer.hpp | 6 ++-- src/utility/robot/armor.hpp | 8 ++--- test/aim_point_chooser.cpp | 22 ++++++------ 44 files changed, 138 insertions(+), 140 deletions(-) diff --git a/src/kernel/identifier.cpp b/src/kernel/identifier.cpp index 3401d053..f4c54fd5 100644 --- a/src/kernel/identifier.cpp +++ b/src/kernel/identifier.cpp @@ -37,8 +37,8 @@ struct Identifier::Impl { auto detected_armors = armor_detection.sync_detect(src); if (!detected_armors.has_value()) return std::nullopt; - auto outpost_armors = std::vector { }; - auto base_armors = std::vector { }; + auto outpost_armors = std::vector { }; + auto base_armors = std::vector { }; outpost_armors.reserve(detected_armors->size()); base_armors.reserve(detected_armors->size()); for (const auto& armor : *detected_armors) { @@ -54,7 +54,7 @@ struct Identifier::Impl { outpost_green_light_roi = outpost_locator_result.detect_roi; base_green_light_roi = base_locator_result.detect_roi; - auto filtered = Armor2Ds { }; + auto filtered = Armor2ds { }; filtered.reserve(detected_armors->size()); if (!outpost_locator_result.green_light.has_value() diff --git a/src/kernel/identifier.hpp b/src/kernel/identifier.hpp index eee68804..cb7672de 100644 --- a/src/kernel/identifier.hpp +++ b/src/kernel/identifier.hpp @@ -17,7 +17,7 @@ class Identifier { public: struct Result { - Armor2Ds armors; + Armor2ds armors; }; auto initialize(const YAML::Node&) noexcept -> std::expected; diff --git a/src/kernel/pose_estimator.cpp b/src/kernel/pose_estimator.cpp index 16f73580..f1894589 100644 --- a/src/kernel/pose_estimator.cpp +++ b/src/kernel/pose_estimator.cpp @@ -26,8 +26,8 @@ struct PoseEstimator::Impl { static constexpr auto kCameraLink = "camera_link"; struct DebugState { - std::optional pre_optimized_outpost; - std::optional optimized_outpost; + std::optional pre_optimized_outpost; + std::optional optimized_outpost; bool has_detected_lightbar { false }; bool has_candidate_roi { false }; bool optimized_bar_is_right { false }; @@ -121,7 +121,7 @@ struct PoseEstimator::Impl { return std::unexpected { e.what() }; } - auto solve_armor(const std::vector& armors) { + auto solve_armor(const std::vector& armors) { const auto q_camera_to_odom = camera_orientation; const auto q_odom_to_camera = q_camera_to_odom.inverse(); @@ -133,13 +133,13 @@ struct PoseEstimator::Impl { input.camera.camera_translation = Eigen::Vector3d { -(q_odom_to_camera * camera_translation).eval() }; - auto result = std::vector {}; + auto result = std::vector {}; for (auto&& [index, armor] : armors | std::views::enumerate) { const auto small = armor.shape == ArmorShape::SMALL; const auto shape = small ? kSmallArmorShapeOpenCV : kLargeArmorShapeOpenCV; - auto armor_3d = Armor3D {}; + auto armor_3d = Armor3d {}; armor_3d.id = static_cast(index); { // pnp @@ -173,15 +173,15 @@ struct PoseEstimator::Impl { }; return result; } - auto solve_armor(const std::vector& armor2ds, Image& image) { + auto solve_armor(const std::vector& armor2ds, Image& image) { auto armor3ds = solve_armor(armor2ds); debug_state.clear(); // 前哨站专属优化 auto outpost2d = std::ranges::find_if( - armor2ds, [](const Armor2D& armor) { return armor.genre == DeviceId::OUTPOST; }); + armor2ds, [](const Armor2d& armor) { return armor.genre == DeviceId::OUTPOST; }); auto outpost3d = std::ranges::find_if( - armor3ds, [](const Armor3D& armor) { return armor.genre == DeviceId::OUTPOST; }); + armor3ds, [](const Armor3d& armor) { return armor.genre == DeviceId::OUTPOST; }); if (outpost3d != armor3ds.end() && outpost2d != armor2ds.end()) { auto camera_frame_outpost = into_camera_link(*outpost3d); @@ -230,7 +230,7 @@ struct PoseEstimator::Impl { adjacency_finder.set_camera_feature(camera_feature); } - auto into_odom_link(const Armor3D& armor) const -> Armor3D { + auto into_odom_link(const Armor3d& armor) const -> Armor3d { auto transformed = armor; auto position = Eigen::Vector3d {}; @@ -244,15 +244,15 @@ struct PoseEstimator::Impl { return transformed; } - auto into_odom_link(std::span armors) const { - auto result = std::vector {}; + auto into_odom_link(std::span armors) const { + auto result = std::vector {}; for (const auto& armor : armors) { result.emplace_back(into_odom_link(armor)); } return result; } - auto into_camera_link(const Armor3D& armor) const -> Armor3D { + auto into_camera_link(const Armor3d& armor) const -> Armor3d { auto transformed = armor; auto position = Eigen::Vector3d {}; @@ -355,13 +355,13 @@ auto PoseEstimator::initialize(const YAML::Node& yaml) noexcept return pimpl->initialize(yaml); } -auto PoseEstimator::estimate_armor(const std::vector& armors) const - -> std::vector { +auto PoseEstimator::estimate_armor(const std::vector& armors) const + -> std::vector { return pimpl->solve_armor(armors); } -auto PoseEstimator::estimate_armor(const std::vector& armors, Image& image) const - -> std::vector { +auto PoseEstimator::estimate_armor(const std::vector& armors, Image& image) const + -> std::vector { return pimpl->solve_armor(armors, image); } @@ -369,10 +369,10 @@ auto PoseEstimator::update_camera_transform(const Transform& transform) -> void return pimpl->update_camera_transform(transform); } -auto PoseEstimator::into_odom_link(std::span armors) const -> std::vector { +auto PoseEstimator::into_odom_link(std::span armors) const -> std::vector { return pimpl->into_odom_link(armors); } -auto PoseEstimator::into_odom_link(const Armor3D& armor) const -> Armor3D { +auto PoseEstimator::into_odom_link(const Armor3d& armor) const -> Armor3d { return pimpl->into_odom_link(armor); } diff --git a/src/kernel/pose_estimator.hpp b/src/kernel/pose_estimator.hpp index ac775144..062fa766 100644 --- a/src/kernel/pose_estimator.hpp +++ b/src/kernel/pose_estimator.hpp @@ -15,11 +15,11 @@ class PoseEstimator { auto initialize(const YAML::Node&) noexcept -> std::expected; auto update_camera_transform(const Transform& transform) -> void; - auto estimate_armor(const std::vector&) const -> std::vector; - auto estimate_armor(const std::vector&, Image&) const -> std::vector; + auto estimate_armor(const std::vector&) const -> std::vector; + auto estimate_armor(const std::vector&, Image&) const -> std::vector; - auto into_odom_link(std::span armors) const -> std::vector; - auto into_odom_link(const Armor3D& armor) const -> Armor3D; + auto into_odom_link(std::span armors) const -> std::vector; + auto into_odom_link(const Armor3d& armor) const -> Armor3d; auto draw_debug(Image&) -> void; auto publish_debug() -> void; diff --git a/src/kernel/tracker.cpp b/src/kernel/tracker.cpp index 73f8332b..49263856 100644 --- a/src/kernel/tracker.cpp +++ b/src/kernel/tracker.cpp @@ -44,12 +44,12 @@ struct Tracker::Impl { auto set_enemy_color(CampColor color) { filter.set_enemy_color(color); } - auto filter_armors(std::span const& armors) const -> std::vector { + auto filter_armors(std::span const& armors) const -> std::vector { auto result = filter.filter(armors); return result; } - auto decide(std::span armors, TimePoint t) -> Decider::Output { + auto decide(std::span armors, TimePoint t) -> Decider::Output { auto decider_output = decider.update(armors, t); return decider_output; } @@ -71,10 +71,10 @@ auto Tracker::set_invincible_armors(DeviceIds devices) -> void { auto Tracker::set_enemy_color(CampColor color) -> void { pimpl->set_enemy_color(color); } -auto Tracker::filter_armors(std::span armors) const -> std::vector { +auto Tracker::filter_armors(std::span armors) const -> std::vector { return pimpl->filter_armors(armors); } -auto Tracker::decide(std::span armors, TimePoint t) -> Decider::Output { +auto Tracker::decide(std::span armors, TimePoint t) -> Decider::Output { return pimpl->decide(armors, t); } diff --git a/src/kernel/tracker.hpp b/src/kernel/tracker.hpp index ce7c7319..dd176471 100644 --- a/src/kernel/tracker.hpp +++ b/src/kernel/tracker.hpp @@ -21,8 +21,8 @@ struct Tracker { auto set_enemy_color(CampColor color) -> void; - auto filter_armors(std::span armors) const -> std::vector; + auto filter_armors(std::span armors) const -> std::vector; - auto decide(std::span armors, TimePoint t) -> tracker::Decider::Output; + auto decide(std::span armors, TimePoint t) -> tracker::Decider::Output; }; } diff --git a/src/kernel/visualization.cpp b/src/kernel/visualization.cpp index 0094922a..0b3bc143 100644 --- a/src/kernel/visualization.cpp +++ b/src/kernel/visualization.cpp @@ -161,13 +161,13 @@ struct Visualization::Impl { return session->push_frame(mat); } - auto update_visible_armors(std::span armors) const -> bool { + auto update_visible_armors(std::span armors) const -> bool { if (!is_initialized) return false; if (!config.visible_armors_enabled) return false; return armors_detect->visualize(armors, "visible_armors", kOdomLink); } - auto update_visible_robot(std::span armors) const -> bool { + auto update_visible_robot(std::span armors) const -> bool { if (!is_initialized) return false; if (!config.visible_robot_enabled) return false; return armors_group->visualize(armors, "visible_robot", kOdomLink); @@ -208,11 +208,11 @@ auto Visualization::update_image(const Image& image) noexcept -> bool { return pimpl->send_image(image); } -auto Visualization::update_visible_armors(std::span armors) const -> bool { +auto Visualization::update_visible_armors(std::span armors) const -> bool { return pimpl->update_visible_armors(armors); } -auto Visualization::update_visible_robot(std::span armors) const -> bool { +auto Visualization::update_visible_robot(std::span armors) const -> bool { return pimpl->update_visible_robot(armors); } diff --git a/src/kernel/visualization.hpp b/src/kernel/visualization.hpp index 0ecb03f2..149672e0 100644 --- a/src/kernel/visualization.hpp +++ b/src/kernel/visualization.hpp @@ -27,9 +27,9 @@ class Visualization { auto update_image(const Image& image) noexcept -> bool; - auto update_visible_armors(std::span armors) const -> bool; + auto update_visible_armors(std::span armors) const -> bool; - auto update_visible_robot(std::span armors) const -> bool; + auto update_visible_robot(std::span armors) const -> bool; auto update_aiming_direction(double yaw, double pitch) const -> void; diff --git a/src/module/debug/visualization/armor_visualizer.cpp b/src/module/debug/visualization/armor_visualizer.cpp index 19803b61..fdce914d 100644 --- a/src/module/debug/visualization/armor_visualizer.cpp +++ b/src/module/debug/visualization/armor_visualizer.cpp @@ -27,7 +27,7 @@ auto make_unique_marker_id(rmcs::DeviceId device, int armor_index) -> int { } auto make_marker(std::string_view frame_id, std::string_view ns, int id, int type, int action, - rmcs::DeviceId device, rmcs::CampColor camp, const rmcs::Armor3D* armor, + rmcs::DeviceId device, rmcs::CampColor camp, const rmcs::Armor3d* armor, const rclcpp::Time& stamp) -> Marker { auto marker = Marker { }; marker.header.frame_id = frame_id; @@ -61,7 +61,7 @@ struct ArmorVisualizer::Impl final { node = std::ref(visual_node); } - auto visualize(std::span armors, std::string const& name, + auto visualize(std::span armors, std::string const& name, std::string const& link_name) -> bool { if (!node.has_value()) { return false; @@ -126,7 +126,7 @@ auto ArmorVisualizer::initialize(util::RclcppNode& visual_node) noexcept -> void return pimpl->initialize(visual_node); } -auto ArmorVisualizer::visualize(std::span armors, std::string const& name, +auto ArmorVisualizer::visualize(std::span armors, std::string const& name, std::string const& link_name) -> bool { return pimpl->visualize(armors, name, link_name); } diff --git a/src/module/debug/visualization/armor_visualizer.hpp b/src/module/debug/visualization/armor_visualizer.hpp index 48ffd0e8..f3e14e5c 100644 --- a/src/module/debug/visualization/armor_visualizer.hpp +++ b/src/module/debug/visualization/armor_visualizer.hpp @@ -12,7 +12,7 @@ class ArmorVisualizer { public: auto initialize(util::RclcppNode&) noexcept -> void; - auto visualize(std::span armors, std::string const& name, + auto visualize(std::span armors, std::string const& name, std::string const& link_name) -> bool; }; } diff --git a/src/module/fire_control/aim_point_chooser.cpp b/src/module/fire_control/aim_point_chooser.cpp index 237ca71a..07772cdf 100644 --- a/src/module/fire_control/aim_point_chooser.cpp +++ b/src/module/fire_control/aim_point_chooser.cpp @@ -65,8 +65,8 @@ struct AimPointChooser::Impl { return {}; } - auto choose_armor(std::span armors, Eigen::Vector3d const& center_position, - double angular_velocity) -> std::optional { + 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; @@ -165,7 +165,7 @@ auto AimPointChooser::configure_yaml(const YAML::Node& yaml) noexcept return pimpl->configure_yaml(yaml); } -auto AimPointChooser::choose_armor(std::span armors, - Eigen::Vector3d const& center_position, double angular_velocity) -> std::optional { +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); } diff --git a/src/module/fire_control/aim_point_chooser.hpp b/src/module/fire_control/aim_point_chooser.hpp index 3e774c7c..c287d234 100644 --- a/src/module/fire_control/aim_point_chooser.hpp +++ b/src/module/fire_control/aim_point_chooser.hpp @@ -29,8 +29,8 @@ class AimPointChooser { 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 choose_armor(std::span armors, Eigen::Vector3d const& center_position, + double angular_velocity) -> std::optional; }; } // namespace rmcs::fire_control diff --git a/src/module/identifier/adjacency_lightbar.cpp b/src/module/identifier/adjacency_lightbar.cpp index 04ee466f..9a79ef33 100644 --- a/src/module/identifier/adjacency_lightbar.cpp +++ b/src/module/identifier/adjacency_lightbar.cpp @@ -36,7 +36,7 @@ struct AdjacencyLightbarFinder::Impl { auto set_armor_thickness(double thickness) { armor_thickness = thickness; } - auto find(const Image& image, const Armor2D& armor2d, const Armor3D& armor3d) + auto find(const Image& image, const Armor2d& armor2d, const Armor3d& armor3d) -> std::optional { detected.reset(); @@ -254,7 +254,7 @@ auto AdjacencyLightbarFinder::set_armor_thickness(double thickness) -> void { } auto AdjacencyLightbarFinder::find( - const Image& image, const Armor2D& armor2d, const Armor3D& armor3d) -> std::optional { + const Image& image, const Armor2d& armor2d, const Armor3d& armor3d) -> std::optional { return pimpl->find(image, armor2d, armor3d); } diff --git a/src/module/identifier/adjacency_lightbar.hpp b/src/module/identifier/adjacency_lightbar.hpp index 2e91c8f8..58049bad 100644 --- a/src/module/identifier/adjacency_lightbar.hpp +++ b/src/module/identifier/adjacency_lightbar.hpp @@ -24,7 +24,7 @@ class AdjacencyLightbarFinder { auto set_armor_thickness(double thickness) -> void; - auto find(const Image& image, const Armor2D& armor2d, const Armor3D& armor3d) + auto find(const Image& image, const Armor2d& armor2d, const Armor3d& armor3d) -> std::optional; // 绘制上一次 find 使用的 roi diff --git a/src/module/identifier/armor_detection.cpp b/src/module/identifier/armor_detection.cpp index 639a0a16..b98b3260 100644 --- a/src/module/identifier/armor_detection.cpp +++ b/src/module/identifier/armor_detection.cpp @@ -25,7 +25,7 @@ struct ArmorDetection::Impl { struct ExplainInterface { virtual ~ExplainInterface() = default; - virtual auto explain(ov::InferRequest&) const noexcept -> Armor2Ds = 0; + virtual auto explain(ov::InferRequest&) const noexcept -> Armor2ds = 0; }; struct Config : util::Serializable { @@ -77,7 +77,7 @@ struct ArmorDetection::Impl { , adapt_scaling { scaling } , roi_offset { offset } { } - auto explain(ov::InferRequest& finished_request) const noexcept -> Armor2Ds override { + auto explain(ov::InferRequest& finished_request) const noexcept -> Armor2ds override { using result_type = typename model_type::Result; using precision_type = typename result_type::precision_type; @@ -110,7 +110,7 @@ struct ArmorDetection::Impl { auto kept_points = std::vector { }; cv::dnn::NMSBoxes(boxes, scores, score_threshold, nms_threshold, kept_points); - auto final_result = Armor2Ds { }; + auto final_result = Armor2ds { }; final_result.reserve(kept_points.size()); for (auto idx : kept_points) { @@ -255,8 +255,8 @@ struct ArmorDetection::Impl { } template - static auto cast_to_armor_result(raw_type raw) noexcept -> Armor2D { - auto armor = Armor2D { }; + static auto cast_to_armor_result(raw_type raw) noexcept -> Armor2d { + auto armor = Armor2d { }; armor.genre = raw.armor_genre(); armor.color = raw.armor_color(); @@ -275,14 +275,14 @@ struct ArmorDetection::Impl { } auto explain_infer_result(ov::InferRequest& finished_request) const noexcept - -> std::optional { + -> std::optional { if (!explain_infer_functor) { return std::nullopt; } return explain_infer_functor->explain(finished_request); } - auto sync_detect(const Image& image) noexcept -> std::optional { + auto sync_detect(const Image& image) noexcept -> std::optional { auto result = generate_openvino_request(image); if (!result.has_value()) { return std::nullopt; @@ -301,7 +301,7 @@ auto ArmorDetection::initialize(const YAML::Node& yaml) noexcept } auto ArmorDetection::sync_detect(const Image& image) noexcept - -> std::optional> { + -> std::optional> { return pimpl->sync_detect(image); } diff --git a/src/module/identifier/armor_detection.hpp b/src/module/identifier/armor_detection.hpp index efaa4d23..d63e1551 100644 --- a/src/module/identifier/armor_detection.hpp +++ b/src/module/identifier/armor_detection.hpp @@ -16,7 +16,7 @@ class ArmorDetection { public: auto initialize(const YAML::Node&) noexcept -> std::expected; - auto sync_detect(const Image&) noexcept -> std::optional>; + auto sync_detect(const Image&) noexcept -> std::optional>; }; } diff --git a/src/module/identifier/green_light_locator.cpp b/src/module/identifier/green_light_locator.cpp index 3826fb9d..488a6e88 100644 --- a/src/module/identifier/green_light_locator.cpp +++ b/src/module/identifier/green_light_locator.cpp @@ -16,7 +16,7 @@ struct GreenLightLocator::Impl { return green_light_detection.initialize(yaml); } - auto locate(const Image& image, std::span armors) noexcept + auto locate(const Image& image, std::span armors) noexcept -> GreenLightLocator::Result { auto result = GreenLightLocator::Result { .detect_roi = std::nullopt, @@ -33,7 +33,7 @@ struct GreenLightLocator::Impl { } private: - static auto compute_detect_roi(const Image& image, std::span armors) + static auto compute_detect_roi(const Image& image, std::span armors) -> std::optional { constexpr auto kExpandScale = 4.0; @@ -97,7 +97,7 @@ auto GreenLightLocator::initialize(const YAML::Node& yaml) noexcept return pimpl->initialize(yaml); } -auto GreenLightLocator::locate(const Image& image, std::span armors) noexcept +auto GreenLightLocator::locate(const Image& image, std::span armors) noexcept -> Result { return pimpl->locate(image, armors); } diff --git a/src/module/identifier/green_light_locator.hpp b/src/module/identifier/green_light_locator.hpp index 5b0f0b4d..a0cf8b87 100644 --- a/src/module/identifier/green_light_locator.hpp +++ b/src/module/identifier/green_light_locator.hpp @@ -24,7 +24,7 @@ class GreenLightLocator { }; auto initialize(const YAML::Node&) noexcept -> std::expected; - auto locate(const Image&, std::span) noexcept -> Result; + auto locate(const Image&, std::span) noexcept -> Result; }; } diff --git a/src/module/predictor/backend/robot_state_backend.cpp b/src/module/predictor/backend/robot_state_backend.cpp index cfdbe876..6b2d01ae 100644 --- a/src/module/predictor/backend/robot_state_backend.cpp +++ b/src/module/predictor/backend/robot_state_backend.cpp @@ -14,13 +14,13 @@ class RobotStateBackendAdapter final : public IRobotStateBackend { explicit RobotStateBackendAdapter(TimePoint stamp) noexcept : state { stamp } { } - auto initialize(Armor3D const& armor, TimePoint t) -> void override { + 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); } + auto update(std::span armors) -> bool override { return state.update(armors); } [[nodiscard]] auto is_converged() const -> bool override { return state.is_converged(); } diff --git a/src/module/predictor/backend/robot_state_backend.hpp b/src/module/predictor/backend/robot_state_backend.hpp index 6b6a3338..90ffa75b 100644 --- a/src/module/predictor/backend/robot_state_backend.hpp +++ b/src/module/predictor/backend/robot_state_backend.hpp @@ -17,10 +17,10 @@ class IRobotStateBackend { public: virtual ~IRobotStateBackend() noexcept = default; - virtual auto initialize(Armor3D const& armor, TimePoint t) -> void = 0; + 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; + virtual auto update(std::span armors) -> bool = 0; [[nodiscard]] virtual auto is_converged() const -> bool = 0; [[nodiscard]] virtual auto get_snapshot() const -> Snapshot = 0; diff --git a/src/module/predictor/backend/snapshot_backend.hpp b/src/module/predictor/backend/snapshot_backend.hpp index fac9b451..6702d8a7 100644 --- a/src/module/predictor/backend/snapshot_backend.hpp +++ b/src/module/predictor/backend/snapshot_backend.hpp @@ -21,7 +21,7 @@ struct ISnapshotBackend { 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; + [[nodiscard]] virtual auto predicted_armors(TimePoint t) const -> std::vector = 0; auto time_stamp() const -> TimePoint { return stamp; } }; diff --git a/src/module/predictor/outpost/ekf_parameter.hpp b/src/module/predictor/outpost/ekf_parameter.hpp index 68bb7ae2..cd844a28 100644 --- a/src/module/predictor/outpost/ekf_parameter.hpp +++ b/src/module/predictor/outpost/ekf_parameter.hpp @@ -25,7 +25,7 @@ struct OutpostEKFParameters { // z：参考装甲板(id 0)在世界坐标系下的 z 坐标 // a：参考装甲板(id 0)的 yaw 角 // w：参考装甲板(id 0)的角速度 - static auto x(Armor3D const& armor) -> EKF::XVec { + 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 }; diff --git a/src/module/predictor/outpost/robot_state.cpp b/src/module/predictor/outpost/robot_state.cpp index 030d8f91..7e94f914 100644 --- a/src/module/predictor/outpost/robot_state.cpp +++ b/src/module/predictor/outpost/robot_state.cpp @@ -53,7 +53,7 @@ auto resolve_template(OutpostArmorLayout const& layout) -> HeightTemplate { return HeightTemplate::Unknown; } -auto make_observation(rmcs::Armor3D const& armor) -> OutpostObservation { +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 }; @@ -159,7 +159,7 @@ struct OutpostRobotState::Impl { explicit Impl(TimePoint stamp) noexcept : time_stamp { stamp } { } - auto initialize(Armor3D const& armor, TimePoint t) -> void { + auto initialize(Armor3d const& armor, TimePoint t) -> void { color = armor_color2camp_color(armor.color); ekf = EKF { OutpostEKFParameters::x(armor), OutpostEKFParameters::P_initial_dig().asDiagonal() }; @@ -193,7 +193,7 @@ struct OutpostRobotState::Impl { time_stamp = t; } - auto update(std::span armors) -> bool { + auto update(std::span armors) -> bool { if (armors.empty()) return false; if (!initialized) { @@ -233,7 +233,7 @@ struct OutpostRobotState::Impl { update_count = 0; } - auto select_best_match(std::span armors) const + auto select_best_match(std::span armors) const -> std::optional> { auto best_match = std::optional> {}; auto const candidates = generate_slot_candidates(layout); @@ -287,13 +287,13 @@ OutpostRobotState::OutpostRobotState(TimePoint stamp) noexcept OutpostRobotState::~OutpostRobotState() noexcept = default; -auto OutpostRobotState::initialize(Armor3D const& armor, TimePoint t) -> void { +auto OutpostRobotState::initialize(Armor3d const& armor, TimePoint t) -> void { return pimpl->initialize(armor, t); } auto OutpostRobotState::predict(TimePoint t) -> void { return pimpl->predict(t); } -auto OutpostRobotState::update(std::span armors) -> bool { +auto OutpostRobotState::update(std::span armors) -> bool { return pimpl->update(armors); } diff --git a/src/module/predictor/outpost/robot_state.hpp b/src/module/predictor/outpost/robot_state.hpp index 3c0f1372..734bdd01 100644 --- a/src/module/predictor/outpost/robot_state.hpp +++ b/src/module/predictor/outpost/robot_state.hpp @@ -16,10 +16,10 @@ class OutpostRobotState { explicit OutpostRobotState(TimePoint stamp) noexcept; - auto initialize(Armor3D const& armor, TimePoint t) -> void; + auto initialize(Armor3d const& armor, TimePoint t) -> void; auto predict(TimePoint t) -> void; - auto update(std::span armors) -> bool; + auto update(std::span armors) -> bool; auto is_converged() const -> bool; auto get_snapshot() const -> Snapshot; diff --git a/src/module/predictor/outpost/snapshot.cpp b/src/module/predictor/outpost/snapshot.cpp index a32306e3..a6ac100e 100644 --- a/src/module/predictor/outpost/snapshot.cpp +++ b/src/module/predictor/outpost/snapshot.cpp @@ -13,8 +13,8 @@ namespace rmcs::predictor { namespace { - auto make_armor(DeviceId device, CampColor color, int id) -> Armor3D { - auto armor = Armor3D {}; + 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; @@ -32,10 +32,10 @@ namespace { return kinematics_of(predict_state_at(t)); } - [[nodiscard]] auto predicted_armors(TimePoint t) const -> std::vector override { + [[nodiscard]] auto predicted_armors(TimePoint t) const -> std::vector override { auto const predicted_x = predict_state_at(t); - auto armors = std::vector {}; + auto armors = std::vector {}; armors.reserve(std::clamp(armor_num, 0, OutpostEKFParameters::kOutpostArmorCount)); for (int id = 0; id < OutpostEKFParameters::kOutpostArmorCount; ++id) { diff --git a/src/module/predictor/regular/ekf_parameter.hpp b/src/module/predictor/regular/ekf_parameter.hpp index 494b288f..dc3745a7 100644 --- a/src/module/predictor/regular/ekf_parameter.hpp +++ b/src/module/predictor/regular/ekf_parameter.hpp @@ -33,7 +33,7 @@ struct EKFParameters { // r: 装甲板中心到旋转中心的半径 // l: 连续两次观测到的半径差 r2 - r1，用于描述装甲板切换时的半径变化 // h: 连续两次观测到的高度差 z2 - z1，反映不同装甲板之间的竖直偏移 - static auto x(Armor3D const& armor) -> EKF::XVec { + static auto x(Armor3d const& armor) -> EKF::XVec { const auto r = radius(armor.genre); const auto [trans_x, trans_y, trans_z] = armor.translation; diff --git a/src/module/predictor/regular/robot_state.cpp b/src/module/predictor/regular/robot_state.cpp index 0b4a6d7c..6998cae5 100644 --- a/src/module/predictor/regular/robot_state.cpp +++ b/src/module/predictor/regular/robot_state.cpp @@ -47,7 +47,7 @@ struct RegularRobotState::Impl { explicit Impl(TimePoint stamp) noexcept : time_stamp { stamp } { } - auto initialize(Armor3D const& armor, TimePoint t) -> void { + auto initialize(Armor3d const& armor, TimePoint t) -> void { device = armor.genre; color = armor_color2camp_color(armor.color); armor_num = EKFParameters::armor_num(armor.genre); @@ -80,7 +80,7 @@ struct RegularRobotState::Impl { time_stamp = t; } - auto update(std::span armors) -> bool { + auto update(std::span armors) -> bool { if (armors.empty()) return false; if (!initialized) { @@ -122,7 +122,7 @@ struct RegularRobotState::Impl { } private: - auto decide_match(Armor3D const& armor) const -> MatchDecision { + auto decide_match(Armor3d const& armor) const -> MatchDecision { if (!initialized || armor.genre != device) return {}; auto armors_xyza = calculate_armors(ekf.x); @@ -167,7 +167,7 @@ struct RegularRobotState::Impl { }; } - auto select_best_match(std::span armors) const -> std::optional { + auto select_best_match(std::span armors) const -> std::optional { auto best_match = std::optional {}; for (std::size_t observation_index = 0; observation_index < armors.size(); ++observation_index) { @@ -181,7 +181,7 @@ struct RegularRobotState::Impl { return best_match; } - auto apply_match(std::span armors, BestMatch const& best_match) -> void { + auto apply_match(std::span armors, BestMatch const& best_match) -> void { auto const& armor = armors[best_match.observation_index]; auto const& decision = best_match.decision; @@ -231,13 +231,13 @@ RegularRobotState::~RegularRobotState() noexcept RegularRobotState::RegularRobotState(RegularRobotState&&) noexcept = default; auto RegularRobotState::operator=(RegularRobotState&&) noexcept -> RegularRobotState& = default; -auto RegularRobotState::initialize(Armor3D const& armor, TimePoint t) -> void { +auto RegularRobotState::initialize(Armor3d const& armor, TimePoint t) -> void { return pimpl->initialize(armor, t); } auto RegularRobotState::predict(TimePoint t) -> void { return pimpl->predict(t); } -auto RegularRobotState::update(std::span armors) -> bool { +auto RegularRobotState::update(std::span armors) -> bool { return pimpl->update(armors); } diff --git a/src/module/predictor/regular/robot_state.hpp b/src/module/predictor/regular/robot_state.hpp index 45f9973b..ecbe8896 100644 --- a/src/module/predictor/regular/robot_state.hpp +++ b/src/module/predictor/regular/robot_state.hpp @@ -18,10 +18,10 @@ class RegularRobotState { auto operator=(RegularRobotState&&) noexcept -> RegularRobotState&; - auto initialize(Armor3D const& armor, TimePoint t) -> void; + auto initialize(Armor3d const& armor, TimePoint t) -> void; auto predict(TimePoint t) -> void; - auto update(std::span armors) -> bool; + auto update(std::span armors) -> bool; auto is_converged() const -> bool; auto get_snapshot() const -> Snapshot; diff --git a/src/module/predictor/regular/snapshot.cpp b/src/module/predictor/regular/snapshot.cpp index 2eb9b606..25f28cd5 100644 --- a/src/module/predictor/regular/snapshot.cpp +++ b/src/module/predictor/regular/snapshot.cpp @@ -13,8 +13,8 @@ namespace rmcs::predictor { namespace { - auto make_armor(DeviceId device, CampColor color, int id) -> Armor3D { - auto armor = Armor3D { }; + 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; @@ -31,10 +31,10 @@ namespace { return kinematics_of(predict_state_at(t)); } - [[nodiscard]] auto predicted_armors(TimePoint t) const -> std::vector override { + [[nodiscard]] auto predicted_armors(TimePoint t) const -> std::vector override { auto const predicted_x = predict_state_at(t); - auto armors = std::vector { }; + auto armors = std::vector { }; armors.reserve(armor_num); for (int id = 0; id < armor_num; ++id) { diff --git a/src/module/predictor/robot_state.cpp b/src/module/predictor/robot_state.cpp index 03e7be23..0ae235f8 100644 --- a/src/module/predictor/robot_state.cpp +++ b/src/module/predictor/robot_state.cpp @@ -13,17 +13,17 @@ struct RobotState::Impl { return make_robot_state_backend(kind, stamp); } - auto reset_backend(Armor3D const& armor, TimePoint stamp) -> void { + auto reset_backend(Armor3d const& armor, TimePoint stamp) -> void { backend = make_backend(armor.genre, stamp); pending_time_stamp = stamp; } - auto ensure_backend(Armor3D const& armor) -> void { + auto ensure_backend(Armor3d const& armor) -> void { if (backend) return; backend = make_backend(armor.genre, pending_time_stamp); } - auto initialize(Armor3D const& armor, TimePoint t) -> void { + auto initialize(Armor3d const& armor, TimePoint t) -> void { reset_backend(armor, t); backend->initialize(armor, t); } @@ -33,7 +33,7 @@ struct RobotState::Impl { if (backend) backend->predict(t); } - auto update(std::span armors) -> bool { + auto update(std::span armors) -> bool { if (armors.empty()) return false; ensure_backend(armors.front()); return backend ? backend->update(armors) : false; @@ -52,13 +52,13 @@ RobotState::RobotState() noexcept : pimpl { std::make_unique() } { } RobotState::~RobotState() noexcept = default; -auto RobotState::initialize(rmcs::Armor3D const& armor, TimePoint t) -> void { +auto RobotState::initialize(rmcs::Armor3d const& armor, TimePoint t) -> void { return pimpl->initialize(armor, t); } auto RobotState::predict(TimePoint t) -> void { return pimpl->predict(t); } -auto RobotState::update(std::span armors) -> bool { return pimpl->update(armors); } +auto RobotState::update(std::span armors) -> bool { return pimpl->update(armors); } auto RobotState::is_converged() const -> bool { return pimpl->is_converged(); } diff --git a/src/module/predictor/robot_state.hpp b/src/module/predictor/robot_state.hpp index f20ed0c6..d6376bb4 100644 --- a/src/module/predictor/robot_state.hpp +++ b/src/module/predictor/robot_state.hpp @@ -11,11 +11,11 @@ struct RobotState { RMCS_PIMPL_DEFINITION(RobotState) public: - auto initialize(Armor3D const&, TimePoint) -> void; + auto initialize(Armor3d const&, TimePoint) -> void; auto predict(TimePoint t) -> void; - auto update(std::span armors) -> bool; + auto update(std::span armors) -> bool; auto is_converged() const -> bool; diff --git a/src/module/predictor/snapshot.cpp b/src/module/predictor/snapshot.cpp index 29416aa0..1afb698e 100644 --- a/src/module/predictor/snapshot.cpp +++ b/src/module/predictor/snapshot.cpp @@ -17,7 +17,7 @@ namespace { return { Eigen::Vector3d::Zero(), 0.0 }; } - [[nodiscard]] auto predicted_armors(TimePoint) const -> std::vector override { + [[nodiscard]] auto predicted_armors(TimePoint) const -> std::vector override { return { }; } }; @@ -44,7 +44,7 @@ auto Snapshot::kinematics() const -> Kinematics { return backend->kinematics_at( auto Snapshot::kinematics_at(TimePoint t) const -> Kinematics { return backend->kinematics_at(t); } -auto Snapshot::predicted_armors(TimePoint t) const -> std::vector { +auto Snapshot::predicted_armors(TimePoint t) const -> std::vector { return backend->predicted_armors(t); } diff --git a/src/module/predictor/snapshot.hpp b/src/module/predictor/snapshot.hpp index b910bcfd..98c4d5e8 100644 --- a/src/module/predictor/snapshot.hpp +++ b/src/module/predictor/snapshot.hpp @@ -39,7 +39,7 @@ class Snapshot { auto kinematics_at(TimePoint t) const -> Kinematics; auto predicted_armors() const { return predicted_armors(Clock::now()); } - auto predicted_armors(TimePoint t) const -> std::vector; + auto predicted_armors(TimePoint t) const -> std::vector; private: std::unique_ptr backend; diff --git a/src/module/tracker/armor_filter.cpp b/src/module/tracker/armor_filter.cpp index bf26bc5b..d8f64ffb 100644 --- a/src/module/tracker/armor_filter.cpp +++ b/src/module/tracker/armor_filter.cpp @@ -8,8 +8,8 @@ struct ArmorFilter::Impl { auto set_invincible_armors(DeviceIds devices) -> void { invincible_armors = devices; } - auto filter(std::span const& armors) const -> std::vector { - auto filtered = armors | std::views::filter([&](Armor2D const& armor) { + auto filter(std::span const& armors) const -> std::vector { + auto filtered = armors | std::views::filter([&](Armor2d const& armor) { return (armor.genre != DeviceId::INFANTRY_5) && (armor.genre != DeviceId::UNKNOWN) && (armor_color2camp_color(armor.color) == enemy_color) && (!invincible_armors.contains(armor.genre)); @@ -31,6 +31,6 @@ auto ArmorFilter::set_invincible_armors(DeviceIds devices) -> void { return pimpl->set_invincible_armors(devices); } -auto ArmorFilter::filter(std::span const& armors) const -> std::vector { +auto ArmorFilter::filter(std::span const& armors) const -> std::vector { return pimpl->filter(armors); } diff --git a/src/module/tracker/armor_filter.hpp b/src/module/tracker/armor_filter.hpp index 40e46b9b..5534966e 100644 --- a/src/module/tracker/armor_filter.hpp +++ b/src/module/tracker/armor_filter.hpp @@ -11,6 +11,6 @@ class ArmorFilter { auto set_invincible_armors(DeviceIds devices) -> void; - auto filter(std::span const&) const -> std::vector; + auto filter(std::span const&) const -> std::vector; }; } diff --git a/src/module/tracker/decider.cpp b/src/module/tracker/decider.cpp index 76ab219c..4e6e53b5 100644 --- a/src/module/tracker/decider.cpp +++ b/src/module/tracker/decider.cpp @@ -36,14 +36,14 @@ struct Decider::Impl { auto set_priority_mode(PriorityMode const& mode) -> void { priority_mode = mode; } - auto update(std::span armors, TimePoint t) -> Output { + auto update(std::span armors, TimePoint t) -> Output { // 推进所有现有追踪器的时间轴 for (auto& [id, tracker] : trackers) { tracker->predict(t); } auto observed_ids = std::unordered_set { }; - auto grouped_armors = std::unordered_map> { }; + auto grouped_armors = std::unordered_map> { }; for (const auto& armor : armors) { grouped_armors[armor.genre].emplace_back(armor); @@ -56,7 +56,7 @@ struct Decider::Impl { last_seen_times[id] = t; } - auto grouped_span = std::span { grouped.data(), grouped.size() }; + auto grouped_span = std::span { grouped.data(), grouped.size() }; bool fused = trackers[id]->update(grouped_span); if (fused) { @@ -180,6 +180,6 @@ auto Decider::set_priority_mode(PriorityMode const& mode) -> void { return pimpl->set_priority_mode(mode); } -auto Decider::update(std::span armors, TimePoint t) -> Output { +auto Decider::update(std::span armors, TimePoint t) -> Output { return pimpl->update(armors, t); } diff --git a/src/module/tracker/decider.hpp b/src/module/tracker/decider.hpp index 16fdc976..e873aae1 100644 --- a/src/module/tracker/decider.hpp +++ b/src/module/tracker/decider.hpp @@ -28,6 +28,6 @@ struct Decider { auto set_priority_mode(PriorityMode const& mode) -> void; - auto update(std::span armors, TimePoint t) -> Output; + auto update(std::span armors, TimePoint t) -> Output; }; } diff --git a/src/runtime.cpp b/src/runtime.cpp index c1d6e539..bafaecd5 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -134,7 +134,7 @@ auto main() -> int { /// 1. Identify Armor /// - auto armors_2d = Armor2Ds {}; + auto armors_2d = Armor2ds {}; { auto result = identifier.sync_identify(*image); if (!result.has_value()) continue; // 一般不会推理出错喵~ @@ -164,7 +164,7 @@ auto main() -> int { /// 2. Transform 2d to 3d /// - auto armors_3d = Armor3Ds {}; + auto armors_3d = Armor3ds {}; { pose_estimator.update_camera_transform(context.camera_transform); diff --git a/src/utility/image/armor.cpp b/src/utility/image/armor.cpp index 3472f1de..af43c50b 100644 --- a/src/utility/image/armor.cpp +++ b/src/utility/image/armor.cpp @@ -4,7 +4,7 @@ namespace rmcs::util { -auto draw(Image& canvas, const Armor2D& armor) noexcept -> void { +auto draw(Image& canvas, const Armor2d& armor) noexcept -> void { auto& opencv_mat = canvas.details().mat; auto color = cv::Scalar { 0, 0, 0 }; diff --git a/src/utility/image/armor.hpp b/src/utility/image/armor.hpp index 49c29f6b..81c72692 100644 --- a/src/utility/image/armor.hpp +++ b/src/utility/image/armor.hpp @@ -4,6 +4,6 @@ namespace rmcs::util { -auto draw(Image&, const Armor2D&) noexcept -> void; +auto draw(Image&, const Armor2d&) noexcept -> void; } diff --git a/src/utility/math/outpost.hpp b/src/utility/math/outpost.hpp index ebcb0ca5..4be3bacd 100644 --- a/src/utility/math/outpost.hpp +++ b/src/utility/math/outpost.hpp @@ -7,7 +7,7 @@ namespace rmcs::util { class NeighborBarSolution { public: struct Input { - Armor3D source; + Armor3d source; bool in_right = false; double armor_thickness = 0; } input; diff --git a/src/utility/math/solve_pnp/outpost_distance_optimizer.hpp b/src/utility/math/solve_pnp/outpost_distance_optimizer.hpp index fab06cd0..adcb95cb 100644 --- a/src/utility/math/solve_pnp/outpost_distance_optimizer.hpp +++ b/src/utility/math/solve_pnp/outpost_distance_optimizer.hpp @@ -7,9 +7,9 @@ namespace rmcs::util { struct OutpostDistanceOptimizer { struct Input { - Armor3D initial; + Armor3d initial; - Armor2D armor; + Armor2d armor; Point2d upper_point; Point2d lower_point; @@ -22,7 +22,7 @@ struct OutpostDistanceOptimizer { } input; struct Result { - Armor3D armor; + Armor3d armor; } result; OutpostDistanceOptimizer() noexcept = default; diff --git a/src/utility/robot/armor.hpp b/src/utility/robot/armor.hpp index dfcb4bcd..d5bfb12e 100644 --- a/src/utility/robot/armor.hpp +++ b/src/utility/robot/armor.hpp @@ -34,7 +34,7 @@ constexpr auto get_enum_name(ArmorShape shape) noexcept { using ArmorGenre = DeviceId; constexpr auto get_enum_name(ArmorGenre genre) noexcept { return rmcs::to_string(genre); } -struct Armor2D { +struct Armor2d { ArmorGenre genre; ArmorColor color; ArmorShape shape; @@ -55,9 +55,9 @@ struct Armor2D { co_yield bl; } }; -using Armor2Ds = std::vector; +using Armor2ds = std::vector; -struct Armor3D { +struct Armor3d { ArmorGenre genre; ArmorColor color; int id; @@ -65,7 +65,7 @@ struct Armor3D { Translation translation; Orientation orientation; }; -using Armor3Ds = std::vector; +using Armor3ds = std::vector; struct ArmorVisualScale : public Scalar3d { using Scalar3d::Scalar3d; diff --git a/test/aim_point_chooser.cpp b/test/aim_point_chooser.cpp index 0501e88f..d78c0c4e 100644 --- a/test/aim_point_chooser.cpp +++ b/test/aim_point_chooser.cpp @@ -3,7 +3,6 @@ #include #include #include -#include #include #include #include @@ -14,7 +13,7 @@ #include "module/fire_control/aim_point_chooser.hpp" #include "utility/math/angle.hpp" -using rmcs::Armor3D; +using rmcs::Armor3d; using rmcs::ArmorColor; using rmcs::DeviceId; using rmcs::Orientation; @@ -44,8 +43,8 @@ auto make_center_position(double yaw_deg) -> Eigen::Vector3d { 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 {}; +auto make_armor(double yaw_deg, int id, DeviceId genre) -> Armor3d { + auto armor = Armor3d {}; armor.genre = genre; armor.color = ArmorColor::BLUE; armor.id = id; @@ -58,9 +57,9 @@ auto make_armor(double yaw_deg, int id, DeviceId genre) -> Armor3D { return armor; } -auto make_armors(std::initializer_list yaws_deg, - DeviceId genre = DeviceId::SENTRY) -> std::vector { - auto armors = std::vector {}; +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; @@ -71,7 +70,7 @@ auto make_armors(std::initializer_list yaws_deg, return armors; } -auto choose_id(AimPointChooser& chooser, std::span armors, double center_yaw_deg, +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); @@ -79,7 +78,7 @@ auto choose_id(AimPointChooser& chooser, std::span armors, double return chosen->id; } -auto choose_once(std::span armors, double center_yaw_deg, double angular_velocity) +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); @@ -94,9 +93,8 @@ TEST(AimPointChooser, SingleArmorHighSpeedScanBySpinDirection) { 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 expected = + (in_coming_window && in_leaving_window) ? std::optional { 0 } : std::nullopt; auto const actual = choose_once(armors, 0.0, speed); From 73234169afbd9862daef8966687b163afb69e518 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 8 Jun 2026 11:46:12 +0800 Subject: [PATCH 10/21] refactor(outpost): simplify layout model and match flow - Replace per-slot assigned/phase/height state with single optional HeightTemplate - Remove MatchCandidate/MatchResult, use MatchedArmor { Armor3d, height_offset, layout } - Move h/H measurement interface to Armor3d + height_offset, remove layout dependency - Move measurement creation into OutpostEKFParameters::measurement() - Simplify height_offset to lookup table, inline h_armor_z - Remove anonymous namespaces from outpost/regular snapshot - Consolidate factory functions as Snapshot::make_outpost/make_regular - Nest HeightTemplate into OutpostArmorLayout - Rename layout_after_match -> resolve_layout --- src/module/predictor/outpost/armor_layout.hpp | 11 +- .../predictor/outpost/ekf_parameter.hpp | 101 ++++--- src/module/predictor/outpost/robot_state.cpp | 272 ++++++------------ src/module/predictor/outpost/snapshot.cpp | 121 ++++---- src/module/predictor/regular/robot_state.cpp | 9 +- src/module/predictor/regular/snapshot.cpp | 87 +++--- src/module/predictor/snapshot.cpp | 28 +- src/module/predictor/snapshot.hpp | 7 +- 8 files changed, 279 insertions(+), 357 deletions(-) diff --git a/src/module/predictor/outpost/armor_layout.hpp b/src/module/predictor/outpost/armor_layout.hpp index 6ea7e931..7d33b608 100644 --- a/src/module/predictor/outpost/armor_layout.hpp +++ b/src/module/predictor/outpost/armor_layout.hpp @@ -1,17 +1,12 @@ #pragma once -#include +#include namespace rmcs::predictor { -struct OutpostArmorSlot { - double phase_offset { 0.0 }; - double height_offset { 0.0 }; - bool assigned { false }; -}; - struct OutpostArmorLayout { - std::array slots {}; + enum class HeightTemplate { PositiveOnSlot1, NegativeOnSlot1 }; + std::optional height_template; }; } // namespace rmcs::predictor diff --git a/src/module/predictor/outpost/ekf_parameter.hpp b/src/module/predictor/outpost/ekf_parameter.hpp index ac5be4d8..1a6287fa 100644 --- a/src/module/predictor/outpost/ekf_parameter.hpp +++ b/src/module/predictor/outpost/ekf_parameter.hpp @@ -1,21 +1,27 @@ #pragma once -#include -#include -#include - #include "module/predictor/outpost/armor_layout.hpp" #include "utility/math/angle.hpp" #include "utility/math/conversion.hpp" #include "utility/math/kalman_filter/ekf.hpp" +#include "utility/panic.hpp" #include "utility/robot/armor.hpp" #include "utility/robot/constant.hpp" +#include +#include +#include + namespace rmcs::predictor { struct OutpostEKFParameters { using EKF = util::EKF<7, 4>; + struct Measurement { + EKF::ZVec z; + EKF::RMat R; + }; + static constexpr int kOutpostArmorCount = 3; static constexpr double kPhaseStep = 2.0 * std::numbers::pi / kOutpostArmorCount; @@ -50,18 +56,38 @@ 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 phase_offset(int armor_id) -> double { + constexpr auto kPhaseOffsets = + std::array { 0.0, kPhaseStep, -kPhaseStep }; + if (armor_id < 0 || armor_id >= kOutpostArmorCount) { + util::panic("outpost armor id out of range"); + } + return kPhaseOffsets[static_cast(armor_id)]; + } + + static auto armor_yaw(EKF::XVec const& x, int armor_id) -> double { + return armor_yaw(x, phase_offset(armor_id)); } - static auto h_armor_z(EKF::XVec const& x, double height_offset) -> double { - return x[4] + height_offset; + static auto height_offset(OutpostArmorLayout::HeightTemplate height_template, int armor_id) + -> double { + using rmcs::kOutpostArmorHeightStep; + constexpr auto kHeightOffsets = std::array { 0.0, + +kOutpostArmorHeightStep, -kOutpostArmorHeightStep }; + if (armor_id < 0 || armor_id >= kOutpostArmorCount) { + util::panic("outpost armor id out of range"); + } + auto const sign = + (height_template == OutpostArmorLayout::HeightTemplate::PositiveOnSlot1) ? +1.0 : -1.0; + return sign * kHeightOffsets[static_cast(armor_id)]; } - 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 height_offset(OutpostArmorLayout const& layout, int armor_id) -> double { + if (armor_id == 0) return 0.0; + if (!layout.height_template.has_value()) { + util::panic("outpost side armor height template is unknown"); + } + return height_offset(*layout.height_template, armor_id); } static auto h_armor_xyz(EKF::XVec const& x, double phase_offset, double height_offset) @@ -69,31 +95,22 @@ 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 h(EKF::XVec const& x, Armor3d const& armor, double height_offset) -> EKF::ZVec { + return h(x, phase_offset(armor.id), height_offset); } static auto x_add(EKF::XVec const& a, EKF::XVec const& b) -> EKF::XVec { @@ -111,7 +128,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, 0, @@ -132,15 +149,15 @@ struct OutpostEKFParameters { constexpr double z_ref_rw_var = 1e-3; // 参考装甲板 yaw / 角速度的角加速度噪声 constexpr double angular_acc_var = 4.0; - const auto v1 = linear_acc_var; - const auto v2 = z_ref_rw_var; - const auto v3 = angular_acc_var; + const auto v1 = linear_acc_var; + const auto v2 = z_ref_rw_var; + const auto v3 = angular_acc_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, 0, b * v1, c * v1, 0, 0, 0, 0, 0, @@ -167,7 +184,7 @@ struct OutpostEKFParameters { const auto delta_yaw = util::normalize_angle(ypr[0] - center_yaw); const auto distance = ypd[2]; - auto R_dig = EKF::RDig { }; + 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; @@ -176,6 +193,22 @@ struct OutpostEKFParameters { return R_dig.asDiagonal(); } + static auto measurement(Armor3d const& armor) -> Measurement { + 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); + + auto z = EKF::ZVec {}; + z << ypd[0], ypd[1], ypd[2], ypr[0]; + + return { z, R(xyz, ypr, ypd) }; + } + static auto H(EKF::XVec const& x, double phase_offset, double height_offset) -> EKF::HMat { const auto phase = armor_yaw(x, phase_offset); const auto cos_phase = std::cos(phase); @@ -207,10 +240,8 @@ 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); + static auto H(EKF::XVec const& x, Armor3d const& armor, double height_offset) -> EKF::HMat { + return H(x, phase_offset(armor.id), height_offset); } }; diff --git a/src/module/predictor/outpost/robot_state.cpp b/src/module/predictor/outpost/robot_state.cpp index 7e94f914..731be608 100644 --- a/src/module/predictor/outpost/robot_state.cpp +++ b/src/module/predictor/outpost/robot_state.cpp @@ -1,159 +1,14 @@ #include "robot_state.hpp" -#include -#include +#include "utility/math/mahalanobis.hpp" +#include "utility/time.hpp" + #include #include #include #include -#include "utility/math/mahalanobis.hpp" -#include "utility/time.hpp" - -using namespace rmcs::predictor; - namespace rmcs::predictor { -auto make_outpost_snapshot(Snapshot::OutpostEKF::XVec ekf_x, CampColor color, int armor_num, - TimePoint stamp, OutpostArmorLayout outpost_layout) noexcept -> Snapshot; -} - -namespace { - -constexpr int kUnknownArmorId = -1; -using OutpostEKF = OutpostRobotState::EKF; - -struct OutpostObservation { - OutpostEKF::ZVec z; - OutpostEKF::RMat R; - Eigen::Vector3d xyz; -}; - -struct MatchCandidate { - int armor_id { kUnknownArmorId }; - double phase_offset { 0.0 }; - double height_offset { 0.0 }; - double error { std::numeric_limits::infinity() }; - bool extends_layout { false }; - bool is_valid { false }; -}; - -// 模板 A: slot1 = +H, slot2 = -H -// 模板 B: slot1 = -H, slot2 = +H -enum class HeightTemplate { Unknown, PositiveOnSlot1, NegativeOnSlot1 }; - -auto resolve_template(OutpostArmorLayout const& layout) -> HeightTemplate { - if (layout.slots[1].assigned) { - return layout.slots[1].height_offset > 0.0 ? HeightTemplate::PositiveOnSlot1 - : HeightTemplate::NegativeOnSlot1; - } - if (layout.slots[2].assigned) { - return layout.slots[2].height_offset > 0.0 ? HeightTemplate::NegativeOnSlot1 - : HeightTemplate::PositiveOnSlot1; - } - return HeightTemplate::Unknown; -} - -auto make_observation(rmcs::Armor3d const& armor) -> OutpostObservation { - auto const [pos_x, pos_y, pos_z] = armor.translation; - auto const xyz = Eigen::Vector3d { pos_x, pos_y, pos_z }; - - auto const [quat_x, quat_y, quat_z, quat_w] = armor.orientation; - auto const orientation = Eigen::Quaterniond { quat_w, quat_x, quat_y, quat_z }; - - auto const ypr = rmcs::util::eulers(orientation); - auto const ypd = rmcs::util::xyz2ypd(xyz); - - auto z = OutpostEKF::ZVec {}; - z << ypd[0], ypd[1], ypd[2], ypr[0]; - - return { z, OutpostEKFParameters::R(xyz, ypr, ypd), xyz }; -} - -auto assigned_count(OutpostArmorLayout const& layout) -> int { - return static_cast(std::ranges::count(layout.slots, true, &OutpostArmorSlot::assigned)); -} - -auto layout_after_match(OutpostArmorLayout const& layout, MatchCandidate const& candidate) - -> OutpostArmorLayout { - auto next_layout = layout; - if (!candidate.extends_layout) return next_layout; - - next_layout.slots[candidate.armor_id].phase_offset = candidate.phase_offset; - next_layout.slots[candidate.armor_id].height_offset = candidate.height_offset; - next_layout.slots[candidate.armor_id].assigned = true; - return next_layout; -} - -auto slot_phase_offset(int armor_id) -> double { - switch (armor_id) { - case 0: - return 0.0; - case 1: - return OutpostEKFParameters::kPhaseStep; - case 2: - return -OutpostEKFParameters::kPhaseStep; - default: - return 0.0; - } -} - -auto generate_slot_candidates(OutpostArmorLayout const& layout) -> std::array { - using rmcs::kOutpostArmorHeightStep; - constexpr auto kPhaseStep = OutpostEKFParameters::kPhaseStep; - - auto candidates = std::array {}; - auto n = int { 0 }; - - for (int id = 0; id < OutpostEKFParameters::kOutpostArmorCount; ++id) { - if (!layout.slots[id].assigned) continue; - candidates[n++] = MatchCandidate { id, slot_phase_offset(id), - layout.slots[id].height_offset, 0.0, false, false }; - } - - if (n == OutpostEKFParameters::kOutpostArmorCount) return candidates; - - auto const tmpl = resolve_template(layout); - - for (int id : { 1, 2 }) { - if (layout.slots[id].assigned) continue; - - auto const phase = (id == 1) ? +kPhaseStep : -kPhaseStep; - - if (tmpl == HeightTemplate::Unknown) { - candidates[n++] = - MatchCandidate { id, phase, +kOutpostArmorHeightStep, 0.0, true, false }; - candidates[n++] = - MatchCandidate { id, phase, -kOutpostArmorHeightStep, 0.0, true, false }; - continue; - } - - auto const h1 = (tmpl == HeightTemplate::PositiveOnSlot1) ? +kOutpostArmorHeightStep - : -kOutpostArmorHeightStep; - candidates[n++] = MatchCandidate { id, phase, (id == 1) ? h1 : -h1, 0.0, true, false }; - } - - return candidates; -} - -auto evaluate_candidate(OutpostObservation const& observation, OutpostEKF::XVec const& x, - OutpostEKF::PMat const& P, double mahalanobis_gate, MatchCandidate const& candidate) - -> MatchCandidate { - auto result = candidate; - if (result.armor_id == kUnknownArmorId) return result; - - auto const H = OutpostEKFParameters::H(x, result.phase_offset, result.height_offset); - auto const z_hat = OutpostEKFParameters::h(x, result.phase_offset, result.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 > mahalanobis_gate) return result; - - result.error = *mahalanobis; - result.is_valid = true; - return result; -} - -} // namespace struct OutpostRobotState::Impl { explicit Impl(TimePoint stamp) noexcept @@ -165,12 +20,9 @@ struct OutpostRobotState::Impl { OutpostEKFParameters::P_initial_dig().asDiagonal() }; time_stamp = t; - layout = OutpostArmorLayout {}; - layout.slots[0].phase_offset = 0.0; - layout.slots[0].height_offset = 0.0; - layout.slots[0].assigned = true; - update_count = 0; - initialized = true; + layout = OutpostArmorLayout {}; + update_count = 0; + initialized = true; } auto predict(TimePoint t) -> void { @@ -204,7 +56,7 @@ struct OutpostRobotState::Impl { auto best_match = select_best_match(armors); if (!best_match.has_value()) return false; - apply_match(best_match->first, best_match->second); + apply_match(*best_match); return true; } @@ -215,7 +67,8 @@ struct OutpostRobotState::Impl { } auto get_snapshot() const -> Snapshot { - return make_outpost_snapshot(ekf.x, color, assigned_count(layout), time_stamp, layout); + if (!initialized) return Snapshot::empty(time_stamp); + return Snapshot::make_outpost(ekf.x, color, time_stamp, layout); } auto distance() const -> double { @@ -224,6 +77,61 @@ struct OutpostRobotState::Impl { } private: + struct MatchedArmor { + Armor3d armor; + double height_offset; + OutpostArmorLayout layout; + }; + + auto resolve_layout(int armor_id, double height_offset) const -> OutpostArmorLayout { + auto next_layout = layout; + if (next_layout.height_template.has_value() || armor_id == 0) return next_layout; + + auto const positive_on_slot1 = (armor_id == 1) == (height_offset > 0.0); + next_layout.height_template = positive_on_slot1 + ? OutpostArmorLayout::HeightTemplate::PositiveOnSlot1 + : OutpostArmorLayout::HeightTemplate::NegativeOnSlot1; + return next_layout; + } + + auto visit_match_hypotheses(auto&& visit) const -> void { + using rmcs::kOutpostArmorHeightStep; + + auto const visit_hypothesis = [&](int armor_id, double height_offset = 0.0) { + visit(armor_id, height_offset, resolve_layout(armor_id, height_offset)); + }; + + visit_hypothesis(0); + + if (!layout.height_template.has_value()) { + auto const visit_side_hypotheses = [&](int armor_id) { + visit_hypothesis(armor_id, +kOutpostArmorHeightStep); + visit_hypothesis(armor_id, -kOutpostArmorHeightStep); + }; + + visit_side_hypotheses(1); + visit_side_hypotheses(2); + return; + } + + for (int armor_id : { 1, 2 }) { + visit_hypothesis( + armor_id, OutpostEKFParameters::height_offset(*layout.height_template, armor_id)); + } + } + + auto evaluate_match(OutpostEKFParameters::Measurement const& measurement, Armor3d const& armor, + double height_offset) const -> std::optional { + auto const H = OutpostEKFParameters::H(ekf.x, armor, height_offset); + auto const z_hat = OutpostEKFParameters::h(ekf.x, armor, height_offset); + auto const innovation = OutpostEKFParameters::z_subtract(measurement.z, z_hat); + auto const S = H * ekf.P() * H.transpose() + measurement.R; + auto const mahalanobis = rmcs::util::mahalanobis_distance(innovation, S); + if (!mahalanobis.has_value() || *mahalanobis > mahalanobis_gate) return std::nullopt; + + return *mahalanobis; + } + auto reset_runtime_state(TimePoint t) -> void { color = CampColor::UNKNOWN; ekf = EKF {}; @@ -233,38 +141,42 @@ struct OutpostRobotState::Impl { update_count = 0; } - auto select_best_match(std::span armors) const - -> std::optional> { - auto best_match = std::optional> {}; - auto const candidates = generate_slot_candidates(layout); + auto select_best_match(std::span armors) const -> std::optional { + auto best_match = std::optional {}; + auto best_mahalanobis = std::numeric_limits::infinity(); for (auto const& armor : armors) { - auto const observation = make_observation(armor); - for (auto const& candidate : candidates) { - auto match = - evaluate_candidate(observation, ekf.x, ekf.P(), mahalanobis_gate, candidate); - if (!match.is_valid) continue; - if (best_match.has_value() && match.error >= best_match->second.error) continue; - - best_match = std::pair { observation, match }; - } + auto const measurement = OutpostEKFParameters::measurement(armor); + + visit_match_hypotheses([&](int armor_id, double height_offset, + OutpostArmorLayout const& next_layout) { + auto matched_armor = armor; + matched_armor.id = armor_id; + + auto const mahalanobis = evaluate_match(measurement, matched_armor, height_offset); + if (!mahalanobis.has_value()) return; + if (*mahalanobis >= best_mahalanobis) return; + + best_mahalanobis = *mahalanobis; + best_match = MatchedArmor { matched_armor, height_offset, next_layout }; + }); } return best_match; } - auto apply_match(OutpostObservation const& observation, MatchCandidate const& match) -> void { - auto const next_layout = layout_after_match(layout, match); + auto apply_match(MatchedArmor const& match) -> void { + auto const measurement = OutpostEKFParameters::measurement(match.armor); ekf.update( - observation.z, - [layout = next_layout, armor_id = match.armor_id]( - EKF::XVec const& x) { return OutpostEKFParameters::h(x, layout, armor_id); }, - [layout = next_layout, armor_id = match.armor_id]( - EKF::XVec const& x) { return OutpostEKFParameters::H(x, layout, armor_id); }, - observation.R, OutpostEKFParameters::x_add, OutpostEKFParameters::z_subtract); - - layout = next_layout; + measurement.z, + [armor = match.armor, height_offset = match.height_offset]( + EKF::XVec const& x) { return OutpostEKFParameters::h(x, armor, height_offset); }, + [armor = match.armor, height_offset = match.height_offset]( + EKF::XVec const& x) { return OutpostEKFParameters::H(x, armor, height_offset); }, + measurement.R, OutpostEKFParameters::x_add, OutpostEKFParameters::z_subtract); + + layout = match.layout; update_count++; } @@ -302,3 +214,5 @@ auto OutpostRobotState::is_converged() const -> bool { return pimpl->is_converge auto OutpostRobotState::get_snapshot() const -> Snapshot { return pimpl->get_snapshot(); } auto OutpostRobotState::distance() const -> double { return pimpl->distance(); } + +} // namespace rmcs::predictor diff --git a/src/module/predictor/outpost/snapshot.cpp b/src/module/predictor/outpost/snapshot.cpp index 16c7706b..bdeae13a 100644 --- a/src/module/predictor/outpost/snapshot.cpp +++ b/src/module/predictor/outpost/snapshot.cpp @@ -1,19 +1,54 @@ #include "module/predictor/snapshot.hpp" +#include "module/predictor/backend/snapshot_backend.hpp" #include "module/predictor/outpost/armor_layout.hpp" +#include "module/predictor/outpost/ekf_parameter.hpp" +#include "utility/math/conversion.hpp" #include "utility/robot/color.hpp" +#include "utility/time.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 { - auto make_armor(DeviceId device, CampColor color, int id) -> Armor3d { +struct OutpostSnapshotBackend final : ISnapshotBackend { + explicit OutpostSnapshotBackend(Snapshot::OutpostEKF::XVec x, CampColor color, TimePoint stamp, + OutpostArmorLayout layout) noexcept + : ISnapshotBackend { DeviceId::OUTPOST, color, OutpostEKFParameters::kOutpostArmorCount, + stamp } + , x { std::move(x) } + , layout { layout } { } + + [[nodiscard]] auto kinematics_at(TimePoint t) const -> Snapshot::Kinematics override { + return kinematics_of(predict_state_at(t)); + } + + [[nodiscard]] auto predicted_armors(TimePoint t) const -> std::vector override { + auto const predicted_x = predict_state_at(t); + auto const predicted_armor_count = + layout.height_template.has_value() ? OutpostEKFParameters::kOutpostArmorCount : 1; + + auto armors = std::vector {}; + armors.reserve( + std::clamp(predicted_armor_count, 0, OutpostEKFParameters::kOutpostArmorCount)); + + for (int id = 0; id < predicted_armor_count; ++id) { + auto armor = make_armor(device, color, id); + auto const height = OutpostEKFParameters::height_offset(layout, armor.id); + auto const angle = OutpostEKFParameters::armor_yaw(predicted_x, id); + auto const position = OutpostEKFParameters::h_armor_xyz( + predicted_x, OutpostEKFParameters::phase_offset(armor.id), height); + + armor.translation = position; + armor.orientation = util::euler_to_quaternion(angle, kPredictedOutpostArmorPitch, 0); + armors.emplace_back(armor); + } + + return armors; + } + +private: + static auto make_armor(DeviceId device, CampColor color, int id) -> Armor3d { auto armor = Armor3d {}; armor.genre = device; armor.color = camp_color2armor_color(color); @@ -21,69 +56,23 @@ namespace { return armor; } - struct OutpostSnapshotBackend final : ISnapshotBackend { - explicit OutpostSnapshotBackend(Snapshot::OutpostEKF::XVec x, CampColor color, - int armor_num, TimePoint stamp, OutpostArmorLayout layout) noexcept - : ISnapshotBackend { DeviceId::OUTPOST, color, armor_num, stamp } - , x { std::move(x) } - , layout { layout } { } - - [[nodiscard]] auto kinematics_at(TimePoint t) const -> Snapshot::Kinematics override { - return kinematics_of(predict_state_at(t)); - } - - [[nodiscard]] auto predicted_armors(TimePoint t) const -> std::vector override { - auto const predicted_x = predict_state_at(t); - - auto armors = std::vector {}; - armors.reserve(std::clamp(armor_num, 0, OutpostEKFParameters::kOutpostArmorCount)); - - for (int id = 0; id < OutpostEKFParameters::kOutpostArmorCount; ++id) { - if (!layout.slots[id].assigned) continue; - - auto armor = make_armor(device, color, id); - auto const angle = OutpostEKFParameters::armor_yaw(predicted_x, layout, id); - auto const position = OutpostEKFParameters::h_armor_xyz(predicted_x, layout, id); - - armor.translation = position; - armor.orientation = - util::euler_to_quaternion(angle, kPredictedOutpostArmorPitch, 0); - armors.emplace_back(armor); - } - - return armors; - } - - private: - auto kinematics_of(Snapshot::OutpostEKF::XVec const& x) const -> Snapshot::Kinematics { - double height_sum = 0.0; - int assigned_count = 0; - for (int id = 0; id < OutpostEKFParameters::kOutpostArmorCount; ++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)); - return { Eigen::Vector3d { x[0], x[2], center_z }, x[6] }; - } - - auto predict_state_at(TimePoint t) const -> Snapshot::OutpostEKF::XVec { - auto const dt = util::delta_time(t, stamp).count(); - return OutpostEKFParameters::f(dt)(x); - } + static auto kinematics_of(Snapshot::OutpostEKF::XVec const& x) -> Snapshot::Kinematics { + return { Eigen::Vector3d { x[0], x[2], x[4] }, x[6] }; + } - Snapshot::OutpostEKF::XVec x; - OutpostArmorLayout layout; - }; + auto predict_state_at(TimePoint t) const -> Snapshot::OutpostEKF::XVec { + auto const dt = util::delta_time(t, stamp).count(); + return OutpostEKFParameters::f(dt)(x); + } -} // namespace + Snapshot::OutpostEKF::XVec x; + OutpostArmorLayout layout; +}; -auto make_outpost_snapshot(Snapshot::OutpostEKF::XVec ekf_x, CampColor color, int armor_num, - TimePoint stamp, OutpostArmorLayout outpost_layout) noexcept -> Snapshot { +auto Snapshot::make_outpost(OutpostEKF::XVec ekf_x, CampColor color, TimePoint stamp, + OutpostArmorLayout const& outpost_layout) noexcept -> Snapshot { return Snapshot { std::make_unique( - std::move(ekf_x), color, armor_num, stamp, outpost_layout) }; + std::move(ekf_x), color, stamp, outpost_layout) }; } } // namespace rmcs::predictor diff --git a/src/module/predictor/regular/robot_state.cpp b/src/module/predictor/regular/robot_state.cpp index 6998cae5..5ec09bd6 100644 --- a/src/module/predictor/regular/robot_state.cpp +++ b/src/module/predictor/regular/robot_state.cpp @@ -8,12 +8,7 @@ #include "utility/time.hpp" -using namespace rmcs::predictor; - namespace rmcs::predictor { -auto make_regular_snapshot(Snapshot::NormalEKF::XVec ekf_x, DeviceId device, CampColor color, - int armor_num, TimePoint stamp) noexcept -> Snapshot; -} struct RegularRobotState::Impl { struct MatchDecision { @@ -113,7 +108,7 @@ struct RegularRobotState::Impl { auto get_snapshot() const -> Snapshot { if (!initialized) return Snapshot::empty(time_stamp); - return make_regular_snapshot(ekf.x, device, color, armor_num, time_stamp); + return Snapshot::make_regular(ekf.x, device, color, armor_num, time_stamp); } auto distance() const -> double { @@ -246,3 +241,5 @@ auto RegularRobotState::is_converged() const -> bool { return pimpl->is_converge auto RegularRobotState::get_snapshot() const -> Snapshot { return pimpl->get_snapshot(); } auto RegularRobotState::distance() const -> double { return pimpl->distance(); } + +} // namespace rmcs::predictor diff --git a/src/module/predictor/regular/snapshot.cpp b/src/module/predictor/regular/snapshot.cpp index 25f28cd5..9b89d5ea 100644 --- a/src/module/predictor/regular/snapshot.cpp +++ b/src/module/predictor/regular/snapshot.cpp @@ -11,63 +11,58 @@ namespace rmcs::predictor { -namespace { - - auto make_armor(DeviceId device, CampColor color, int id) -> Armor3d { - auto armor = Armor3d { }; - armor.genre = device; - armor.color = camp_color2armor_color(color); - armor.id = id; - return armor; +struct RegularSnapshotBackend final : ISnapshotBackend { + explicit RegularSnapshotBackend(Snapshot::NormalEKF::XVec x, DeviceId device, CampColor color, + int armor_num, 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)); } - struct RegularSnapshotBackend final : ISnapshotBackend { - explicit RegularSnapshotBackend(Snapshot::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 predicted_armors(TimePoint t) const -> std::vector override { + auto const predicted_x = predict_state_at(t); - [[nodiscard]] auto kinematics_at(TimePoint t) const -> Snapshot::Kinematics override { - return kinematics_of(predict_state_at(t)); - } - - [[nodiscard]] auto predicted_armors(TimePoint t) const -> std::vector override { - auto const predicted_x = predict_state_at(t); - - auto armors = std::vector { }; - armors.reserve(armor_num); + auto armors = std::vector {}; + armors.reserve(armor_num); - for (int id = 0; id < armor_num; ++id) { - auto armor = make_armor(device, color, id); - auto const angle = EKFParameters::armor_yaw(device, predicted_x, id); - auto const position = - EKFParameters::h_armor_xyz(device, predicted_x, id, armor_num); + for (int id = 0; id < armor_num; ++id) { + auto armor = make_armor(device, color, id); + auto const angle = EKFParameters::armor_yaw(device, predicted_x, id); + auto const position = EKFParameters::h_armor_xyz(device, predicted_x, id, armor_num); - armor.translation = position; - armor.orientation = util::euler_to_quaternion(angle, kPredictedOtherArmorPitch, 0); - armors.emplace_back(armor); - } - - return armors; + armor.translation = position; + armor.orientation = util::euler_to_quaternion(angle, kPredictedOtherArmorPitch, 0); + armors.emplace_back(armor); } - private: - static auto kinematics_of(Snapshot::NormalEKF::XVec const& x) -> Snapshot::Kinematics { - return { Eigen::Vector3d { x[0], x[2], x[4] }, x[7] }; - } + return armors; + } - auto predict_state_at(TimePoint t) const -> Snapshot::NormalEKF::XVec { - auto const dt = util::delta_time(t, stamp).count(); - return EKFParameters::f(dt)(x); - } +private: + 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; + } - Snapshot::NormalEKF::XVec x; - }; + static auto kinematics_of(Snapshot::NormalEKF::XVec const& x) -> Snapshot::Kinematics { + return { Eigen::Vector3d { x[0], x[2], x[4] }, x[7] }; + } + + auto predict_state_at(TimePoint t) const -> Snapshot::NormalEKF::XVec { + auto const dt = util::delta_time(t, stamp).count(); + return EKFParameters::f(dt)(x); + } -} // namespace + Snapshot::NormalEKF::XVec x; +}; -auto make_regular_snapshot(Snapshot::NormalEKF::XVec ekf_x, DeviceId device, - CampColor color, int armor_num, TimePoint stamp) noexcept -> Snapshot { +auto Snapshot::make_regular(NormalEKF::XVec ekf_x, DeviceId device, CampColor color, int armor_num, + TimePoint stamp) noexcept -> Snapshot { return Snapshot { std::make_unique( std::move(ekf_x), device, color, armor_num, stamp) }; } diff --git a/src/module/predictor/snapshot.cpp b/src/module/predictor/snapshot.cpp index 1afb698e..6b468a9e 100644 --- a/src/module/predictor/snapshot.cpp +++ b/src/module/predictor/snapshot.cpp @@ -7,22 +7,18 @@ namespace rmcs::predictor { -namespace { - - struct EmptySnapshotBackend final : ISnapshotBackend { - explicit EmptySnapshotBackend(TimePoint stamp) noexcept - : ISnapshotBackend { DeviceId::UNKNOWN, CampColor::UNKNOWN, 0, stamp } { } - - [[nodiscard]] auto kinematics_at(TimePoint) const -> Snapshot::Kinematics override { - return { Eigen::Vector3d::Zero(), 0.0 }; - } - - [[nodiscard]] auto predicted_armors(TimePoint) const -> std::vector override { - return { }; - } - }; - -} // namespace +struct EmptySnapshotBackend final : ISnapshotBackend { + explicit EmptySnapshotBackend(TimePoint stamp) noexcept + : ISnapshotBackend { DeviceId::UNKNOWN, CampColor::UNKNOWN, 0, stamp } { } + + [[nodiscard]] auto kinematics_at(TimePoint) const -> Snapshot::Kinematics override { + return { Eigen::Vector3d::Zero(), 0.0 }; + } + + [[nodiscard]] auto predicted_armors(TimePoint) const -> std::vector override { + return {}; + } +}; auto Snapshot::empty(TimePoint stamp) noexcept -> Snapshot { return Snapshot { std::make_unique(stamp) }; diff --git a/src/module/predictor/snapshot.hpp b/src/module/predictor/snapshot.hpp index 2ce6bd40..a076c8ae 100644 --- a/src/module/predictor/snapshot.hpp +++ b/src/module/predictor/snapshot.hpp @@ -8,11 +8,11 @@ #include "utility/math/kalman_filter/ekf.hpp" #include "utility/robot/armor.hpp" #include "utility/robot/id.hpp" -#include "utility/robot/id.hpp" namespace rmcs::predictor { struct ISnapshotBackend; +struct OutpostArmorLayout; class Snapshot; class Snapshot { @@ -33,6 +33,11 @@ class Snapshot { ~Snapshot() noexcept; static auto empty(TimePoint stamp) noexcept -> Snapshot; + static auto make_outpost(OutpostEKF::XVec ekf_x, CampColor color, TimePoint stamp, + OutpostArmorLayout const& outpost_layout) noexcept -> Snapshot; + static auto make_regular(NormalEKF::XVec ekf_x, DeviceId device, CampColor color, int armor_num, + TimePoint stamp) noexcept -> Snapshot; + auto time_stamp() const -> TimePoint; auto device_id() const -> DeviceId; From 9a1a1222077e11d55b4eb34d2222d16863a5b4b5 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 8 Jun 2026 16:49:31 +0800 Subject: [PATCH 11/21] refactor predictor state and snapshot models --- config/config.yaml | 2 +- .../solver/aim_point_sampling.cpp | 8 +- .../solver/target_solution_solver.cpp | 10 +-- .../predictor/backend/robot_state_backend.cpp | 47 ---------- .../predictor/backend/robot_state_backend.hpp | 43 --------- .../predictor/backend/snapshot_backend.hpp | 29 ------- src/module/predictor/outpost/robot_state.cpp | 11 ++- src/module/predictor/outpost/robot_state.hpp | 12 +-- src/module/predictor/outpost/snapshot.cpp | 87 ++++++++++--------- src/module/predictor/outpost/snapshot.hpp | 34 ++++++++ src/module/predictor/regular/robot_state.cpp | 23 +++-- src/module/predictor/regular/robot_state.hpp | 3 +- src/module/predictor/regular/snapshot.cpp | 85 ++++++++++-------- src/module/predictor/regular/snapshot.hpp | 33 +++++++ src/module/predictor/robot_state.cpp | 63 ++++++++------ src/module/predictor/robot_state.hpp | 3 +- src/module/predictor/snapshot.cpp | 49 ++++++----- src/module/predictor/snapshot.hpp | 43 ++++----- 18 files changed, 287 insertions(+), 298 deletions(-) delete mode 100644 src/module/predictor/backend/robot_state_backend.cpp delete mode 100644 src/module/predictor/backend/robot_state_backend.hpp delete mode 100644 src/module/predictor/backend/snapshot_backend.hpp create mode 100644 src/module/predictor/outpost/snapshot.hpp create mode 100644 src/module/predictor/regular/snapshot.hpp diff --git a/config/config.yaml b/config/config.yaml index 30de6c19..bc19bc9a 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -42,7 +42,7 @@ capturer: # 替换为你具体的路径 location: "/path/to/your/video" # double 帧率 - frame_rate: 120 + frame_rate: 80 # bool 是否循环播放 loop_play: true # bool 是否允许跳帧以满足实时性 diff --git a/src/module/fire_control/solver/aim_point_sampling.cpp b/src/module/fire_control/solver/aim_point_sampling.cpp index 7bca483f..e72ec4fb 100644 --- a/src/module/fire_control/solver/aim_point_sampling.cpp +++ b/src/module/fire_control/solver/aim_point_sampling.cpp @@ -28,10 +28,10 @@ auto rmcs::fire_control::AimPointSampler::sample_at( 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, - predicted_kinematics.angular_velocity); + auto predicted_armors = snapshot.predicted_armors(t); + auto predicted_motion = snapshot.motion_at(t); + auto chosen_armor = chooser.choose_armor( + predicted_armors, predicted_motion.center_position, predicted_motion.angular_velocity); if (!chosen_armor.has_value()) return std::unexpected { "choose_armor returned nullopt" }; auto aim_point = Eigen::Vector3d {}; diff --git a/src/module/fire_control/solver/target_solution_solver.cpp b/src/module/fire_control/solver/target_solution_solver.cpp index 689b5aea..0cfe61bc 100644 --- a/src/module/fire_control/solver/target_solution_solver.cpp +++ b/src/module/fire_control/solver/target_solution_solver.cpp @@ -21,9 +21,9 @@ struct TargetSolutionSolver::Impl { Eigen::Vector3d aim_point; }; - auto target_kinematics = snapshot.kinematics(); - auto target_position = target_kinematics.center_position; - auto current_fly_time = target_position.norm() / bullet_speed; + auto target_motion = snapshot.motion(); + auto target_position = target_motion.center_position; + auto current_fly_time = target_position.norm() / bullet_speed; auto best_candidate = std::optional {}; @@ -33,7 +33,7 @@ struct TargetSolutionSolver::Impl { + std::chrono::duration_cast( std::chrono::duration(total_predict_time)); - auto predicted_kinematics = snapshot.kinematics_at(t_target); + auto predicted_motion = snapshot.motion_at(t_target); auto sample = AimPointSampler::sample_at(snapshot, chooser, t_target, bullet_speed); if (!sample.has_value()) continue; @@ -42,7 +42,7 @@ struct TargetSolutionSolver::Impl { best_candidate = BestCandidate { .attitude = sample->attitude, .impact_time = t_target, - .center_position = predicted_kinematics.center_position, + .center_position = predicted_motion.center_position, .aim_point = sample->aim_point, }; if (time_error < kMaxFlyTimeThreshold) break; 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/robot_state.cpp b/src/module/predictor/outpost/robot_state.cpp index 731be608..7db8c3cd 100644 --- a/src/module/predictor/outpost/robot_state.cpp +++ b/src/module/predictor/outpost/robot_state.cpp @@ -1,5 +1,6 @@ #include "robot_state.hpp" +#include "module/predictor/outpost/snapshot.hpp" #include "utility/math/mahalanobis.hpp" #include "utility/time.hpp" @@ -66,9 +67,9 @@ struct OutpostRobotState::Impl { return true; } - auto get_snapshot() const -> Snapshot { - if (!initialized) return Snapshot::empty(time_stamp); - return Snapshot::make_outpost(ekf.x, color, time_stamp, layout); + auto get_snapshot() const -> std::optional { + if (!initialized) return std::nullopt; + return Snapshot { OutpostSnapshot { ekf.x, color, time_stamp, layout } }; } auto distance() const -> double { @@ -211,7 +212,9 @@ 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(); } 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 bdeae13a..7b8b9a89 100644 --- a/src/module/predictor/outpost/snapshot.cpp +++ b/src/module/predictor/outpost/snapshot.cpp @@ -1,39 +1,47 @@ -#include "module/predictor/snapshot.hpp" -#include "module/predictor/backend/snapshot_backend.hpp" -#include "module/predictor/outpost/armor_layout.hpp" -#include "module/predictor/outpost/ekf_parameter.hpp" +#include "module/predictor/outpost/snapshot.hpp" + #include "utility/math/conversion.hpp" -#include "utility/robot/color.hpp" #include "utility/time.hpp" -#include +#include #include namespace rmcs::predictor { -struct OutpostSnapshotBackend final : ISnapshotBackend { - explicit OutpostSnapshotBackend(Snapshot::OutpostEKF::XVec x, CampColor color, TimePoint stamp, - OutpostArmorLayout layout) noexcept - : ISnapshotBackend { DeviceId::OUTPOST, color, OutpostEKFParameters::kOutpostArmorCount, - stamp } - , x { std::move(x) } +struct OutpostSnapshot::Impl { + explicit Impl(EKF::XVec x, CampColor color, TimePoint stamp, OutpostArmorLayout layout) + : x { std::move(x) } + , color { color } + , stamp { stamp } , layout { layout } { } - [[nodiscard]] auto kinematics_at(TimePoint t) const -> Snapshot::Kinematics override { - return kinematics_of(predict_state_at(t)); + 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; } - [[nodiscard]] auto predicted_armors(TimePoint t) const -> std::vector override { + static auto motion_of(EKF::XVec const& x) -> TargetMotion { + return { Eigen::Vector3d { x[0], x[2], x[4] }, x[6] }; + } + + auto predict_state_at(TimePoint t) const -> EKF::XVec { + auto const dt = util::delta_time(t, stamp).count(); + return OutpostEKFParameters::f(dt)(x); + } + + auto predicted_armors(TimePoint t) const -> std::vector { auto const predicted_x = predict_state_at(t); auto const predicted_armor_count = layout.height_template.has_value() ? OutpostEKFParameters::kOutpostArmorCount : 1; auto armors = std::vector {}; - armors.reserve( - std::clamp(predicted_armor_count, 0, OutpostEKFParameters::kOutpostArmorCount)); + armors.reserve(predicted_armor_count); for (int id = 0; id < predicted_armor_count; ++id) { - auto armor = make_armor(device, color, id); + auto armor = make_armor(DeviceId::OUTPOST, color, id); auto const height = OutpostEKFParameters::height_offset(layout, armor.id); auto const angle = OutpostEKFParameters::armor_yaw(predicted_x, id); auto const position = OutpostEKFParameters::h_armor_xyz( @@ -47,32 +55,31 @@ struct OutpostSnapshotBackend final : ISnapshotBackend { return armors; } -private: - 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; - } + EKF::XVec x; + CampColor color; + TimePoint stamp; + OutpostArmorLayout layout; +}; - static auto kinematics_of(Snapshot::OutpostEKF::XVec const& x) -> Snapshot::Kinematics { - return { Eigen::Vector3d { x[0], x[2], x[4] }, x[6] }; - } +OutpostSnapshot::OutpostSnapshot( + EKF::XVec x, CampColor color, TimePoint stamp, OutpostArmorLayout layout) + : pimpl { std::make_unique(std::move(x), color, stamp, layout) } { } - auto predict_state_at(TimePoint t) const -> Snapshot::OutpostEKF::XVec { - auto const dt = util::delta_time(t, stamp).count(); - return OutpostEKFParameters::f(dt)(x); - } +OutpostSnapshot::OutpostSnapshot(OutpostSnapshot&&) noexcept = default; +auto OutpostSnapshot::operator=(OutpostSnapshot&&) noexcept -> OutpostSnapshot& = default; - Snapshot::OutpostEKF::XVec x; - OutpostArmorLayout layout; -}; +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)); +} -auto Snapshot::make_outpost(OutpostEKF::XVec ekf_x, CampColor color, TimePoint stamp, - OutpostArmorLayout const& outpost_layout) noexcept -> Snapshot { - return Snapshot { std::make_unique( - std::move(ekf_x), color, stamp, 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 new file mode 100644 index 00000000..9d3a0460 --- /dev/null +++ b/src/module/predictor/outpost/snapshot.hpp @@ -0,0 +1,34 @@ +#pragma once + +#include "module/predictor/outpost/armor_layout.hpp" +#include "module/predictor/outpost/ekf_parameter.hpp" +#include "module/predictor/snapshot.hpp" + +#include +#include + +namespace rmcs::predictor { + +class OutpostSnapshot { +public: + using EKF = OutpostEKFParameters::EKF; + + explicit OutpostSnapshot(EKF::XVec x, CampColor color, TimePoint stamp, + OutpostArmorLayout layout); + 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 5ec09bd6..cc179bf4 100644 --- a/src/module/predictor/regular/robot_state.cpp +++ b/src/module/predictor/regular/robot_state.cpp @@ -1,13 +1,14 @@ #include "robot_state.hpp" +#include "module/predictor/regular/snapshot.hpp" +#include "utility/time.hpp" + #include #include #include #include #include -#include "utility/time.hpp" - namespace rmcs::predictor { struct RegularRobotState::Impl { @@ -106,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 Snapshot::make_regular(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 { @@ -138,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])); @@ -149,7 +152,7 @@ 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; } @@ -238,7 +241,9 @@ 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(); } 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 9b89d5ea..ecf65765 100644 --- a/src/module/predictor/regular/snapshot.cpp +++ b/src/module/predictor/regular/snapshot.cpp @@ -1,27 +1,39 @@ -#include "module/predictor/snapshot.hpp" -#include "utility/robot/color.hpp" -#include "utility/robot/id.hpp" +#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 { -struct RegularSnapshotBackend final : ISnapshotBackend { - explicit RegularSnapshotBackend(Snapshot::NormalEKF::XVec x, DeviceId device, CampColor color, - int armor_num, TimePoint stamp) noexcept - : ISnapshotBackend { device, color, armor_num, stamp } - , x { std::move(x) } { } +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 } { } + + 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; + } + + static auto motion_of(EKF::XVec const& x) -> TargetMotion { + return { Eigen::Vector3d { x[0], x[2], x[4] }, x[7] }; + } - [[nodiscard]] auto kinematics_at(TimePoint t) const -> Snapshot::Kinematics override { - return kinematics_of(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); } - [[nodiscard]] auto predicted_armors(TimePoint t) const -> std::vector override { + auto predicted_armors(TimePoint t) const -> std::vector { auto const predicted_x = predict_state_at(t); auto armors = std::vector {}; @@ -40,31 +52,32 @@ struct RegularSnapshotBackend final : ISnapshotBackend { return armors; } -private: - 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; - } + EKF::XVec x; + DeviceId device; + CampColor color; + int armor_num; + TimePoint stamp; +}; - static auto kinematics_of(Snapshot::NormalEKF::XVec const& x) -> Snapshot::Kinematics { - return { Eigen::Vector3d { x[0], x[2], x[4] }, x[7] }; - } +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) } { } - auto predict_state_at(TimePoint t) const -> Snapshot::NormalEKF::XVec { - auto const dt = util::delta_time(t, stamp).count(); - return EKFParameters::f(dt)(x); - } +RegularSnapshot::RegularSnapshot(RegularSnapshot&&) noexcept = default; +auto RegularSnapshot::operator=(RegularSnapshot&&) noexcept -> RegularSnapshot& = default; - Snapshot::NormalEKF::XVec x; -}; +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 Snapshot::make_regular(NormalEKF::XVec ekf_x, DeviceId device, CampColor color, int armor_num, - TimePoint stamp) noexcept -> Snapshot { - return 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 new file mode 100644 index 00000000..5a10fe8b --- /dev/null +++ b/src/module/predictor/regular/snapshot.hpp @@ -0,0 +1,33 @@ +#pragma once + +#include "module/predictor/regular/ekf_parameter.hpp" +#include "module/predictor/snapshot.hpp" + +#include +#include + +namespace rmcs::predictor { + +class RegularSnapshot { +public: + using EKF = EKFParameters::EKF; + + 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 6b468a9e..3b3a4d09 100644 --- a/src/module/predictor/snapshot.cpp +++ b/src/module/predictor/snapshot.cpp @@ -1,47 +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 { -struct EmptySnapshotBackend final : ISnapshotBackend { - explicit EmptySnapshotBackend(TimePoint stamp) noexcept - : ISnapshotBackend { DeviceId::UNKNOWN, CampColor::UNKNOWN, 0, stamp } { } +struct Snapshot::Impl { + using SnapshotVariant = std::variant; - [[nodiscard]] auto kinematics_at(TimePoint) const -> Snapshot::Kinematics override { - return { Eigen::Vector3d::Zero(), 0.0 }; - } + explicit Impl(RegularSnapshot snapshot) + : snapshot { std::move(snapshot) } { } - [[nodiscard]] auto predicted_armors(TimePoint) const -> std::vector override { - return {}; - } + explicit Impl(OutpostSnapshot snapshot) + : snapshot { std::move(snapshot) } { } + + SnapshotVariant snapshot; }; -auto Snapshot::empty(TimePoint stamp) noexcept -> Snapshot { - return Snapshot { std::make_unique(stamp) }; -} +Snapshot::Snapshot(RegularSnapshot snapshot) + : pimpl { std::make_unique(std::move(snapshot)) } { } -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 a076c8ae..0973312c 100644 --- a/src/module/predictor/snapshot.hpp +++ b/src/module/predictor/snapshot.hpp @@ -1,54 +1,47 @@ #pragma once -#include -#include -#include - #include "utility/clock.hpp" -#include "utility/math/kalman_filter/ekf.hpp" #include "utility/robot/armor.hpp" #include "utility/robot/id.hpp" +#include +#include + +#include + namespace rmcs::predictor { -struct ISnapshotBackend; -struct OutpostArmorLayout; +class RegularSnapshot; +class OutpostSnapshot; class Snapshot; +struct TargetMotion { + Eigen::Vector3d center_position; + double angular_velocity; +}; + class Snapshot { public: - using NormalEKF = util::EKF<11, 4>; - using OutpostEKF = util::EKF<7, 4>; - - struct Kinematics { - Eigen::Vector3d center_position; - double angular_velocity; - }; - - explicit Snapshot(std::unique_ptr backend) noexcept; + explicit Snapshot(RegularSnapshot snapshot); + explicit Snapshot(OutpostSnapshot snapshot); Snapshot(Snapshot const&) = delete; Snapshot(Snapshot&&) noexcept; Snapshot& operator=(Snapshot const&) = delete; Snapshot& operator=(Snapshot&&) noexcept; ~Snapshot() noexcept; - static auto empty(TimePoint stamp) noexcept -> Snapshot; - - static auto make_outpost(OutpostEKF::XVec ekf_x, CampColor color, TimePoint stamp, - OutpostArmorLayout const& outpost_layout) noexcept -> Snapshot; - static auto make_regular(NormalEKF::XVec ekf_x, DeviceId device, CampColor color, int armor_num, - TimePoint stamp) noexcept -> Snapshot; auto time_stamp() const -> TimePoint; auto device_id() const -> DeviceId; - auto kinematics() const -> Kinematics; - auto kinematics_at(TimePoint t) const -> Kinematics; + 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: - std::unique_ptr backend; + struct Impl; + std::unique_ptr pimpl; }; } From 02bc2da81131b22849cb70c89f9a1faf30ea276a Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Tue, 9 Jun 2026 13:20:27 +0800 Subject: [PATCH 12/21] fix: fix height template --- src/module/predictor/outpost/armor_layout.hpp | 6 +- .../predictor/outpost/ekf_parameter.hpp | 23 +++--- src/module/predictor/outpost/robot_state.cpp | 75 ++++++++++++------- src/module/predictor/outpost/snapshot.cpp | 8 +- 4 files changed, 63 insertions(+), 49 deletions(-) diff --git a/src/module/predictor/outpost/armor_layout.hpp b/src/module/predictor/outpost/armor_layout.hpp index 7d33b608..163bd7b8 100644 --- a/src/module/predictor/outpost/armor_layout.hpp +++ b/src/module/predictor/outpost/armor_layout.hpp @@ -1,12 +1,14 @@ #pragma once +#include #include namespace rmcs::predictor { struct OutpostArmorLayout { - enum class HeightTemplate { PositiveOnSlot1, NegativeOnSlot1 }; - std::optional height_template; + static constexpr int kSlotCount = 3; + + std::array, kSlotCount> height_levels {}; }; } // namespace rmcs::predictor diff --git a/src/module/predictor/outpost/ekf_parameter.hpp b/src/module/predictor/outpost/ekf_parameter.hpp index 1a6287fa..796b1fd9 100644 --- a/src/module/predictor/outpost/ekf_parameter.hpp +++ b/src/module/predictor/outpost/ekf_parameter.hpp @@ -69,25 +69,20 @@ struct OutpostEKFParameters { return armor_yaw(x, phase_offset(armor_id)); } - static auto height_offset(OutpostArmorLayout::HeightTemplate height_template, int armor_id) - -> double { + static auto height_offset(int height_level) -> double { using rmcs::kOutpostArmorHeightStep; - constexpr auto kHeightOffsets = std::array { 0.0, - +kOutpostArmorHeightStep, -kOutpostArmorHeightStep }; - if (armor_id < 0 || armor_id >= kOutpostArmorCount) { - util::panic("outpost armor id out of range"); - } - auto const sign = - (height_template == OutpostArmorLayout::HeightTemplate::PositiveOnSlot1) ? +1.0 : -1.0; - return sign * kHeightOffsets[static_cast(armor_id)]; + return height_level * kOutpostArmorHeightStep; } static auto height_offset(OutpostArmorLayout const& layout, int armor_id) -> double { - if (armor_id == 0) return 0.0; - if (!layout.height_template.has_value()) { - util::panic("outpost side armor height template is unknown"); + if (armor_id < 0 || armor_id >= kOutpostArmorCount) { + util::panic("outpost armor id out of range"); + } + auto const height_level = layout.height_levels[static_cast(armor_id)]; + if (!height_level.has_value()) { + util::panic("outpost armor height level is unknown"); } - return height_offset(*layout.height_template, armor_id); + return height_offset(*height_level); } static auto h_armor_xyz(EKF::XVec const& x, double phase_offset, double height_offset) diff --git a/src/module/predictor/outpost/robot_state.cpp b/src/module/predictor/outpost/robot_state.cpp index 7db8c3cd..c1128eff 100644 --- a/src/module/predictor/outpost/robot_state.cpp +++ b/src/module/predictor/outpost/robot_state.cpp @@ -21,9 +21,11 @@ struct OutpostRobotState::Impl { OutpostEKFParameters::P_initial_dig().asDiagonal() }; time_stamp = t; - layout = OutpostArmorLayout {}; - update_count = 0; - initialized = true; + layout = OutpostArmorLayout {}; + layout.height_levels[0] = 0; + last_match = LastMatch { 0, 0 }; + update_count = 0; + initialized = true; } auto predict(TimePoint t) -> void { @@ -80,45 +82,55 @@ struct OutpostRobotState::Impl { private: struct MatchedArmor { Armor3d armor; + int height_level; double height_offset; OutpostArmorLayout layout; }; - auto resolve_layout(int armor_id, double height_offset) const -> OutpostArmorLayout { - auto next_layout = layout; - if (next_layout.height_template.has_value() || armor_id == 0) return next_layout; + struct LastMatch { + int armor_id; + int height_level; + }; - auto const positive_on_slot1 = (armor_id == 1) == (height_offset > 0.0); - next_layout.height_template = positive_on_slot1 - ? OutpostArmorLayout::HeightTemplate::PositiveOnSlot1 - : OutpostArmorLayout::HeightTemplate::NegativeOnSlot1; - return next_layout; + static auto next_armor_id(int armor_id, int direction) -> int { + constexpr auto armor_count = OutpostEKFParameters::kOutpostArmorCount; + return (armor_id + direction + armor_count) % armor_count; } auto visit_match_hypotheses(auto&& visit) const -> void { - using rmcs::kOutpostArmorHeightStep; + if (!last_match.has_value()) return; + + auto const visit_hypothesis = [&](int armor_id, int height_level) { + auto next_layout = layout; + auto& known_level = next_layout.height_levels[static_cast(armor_id)]; + if (known_level.has_value() && *known_level != height_level) return; - auto const visit_hypothesis = [&](int armor_id, double height_offset = 0.0) { - visit(armor_id, height_offset, resolve_layout(armor_id, height_offset)); + known_level = height_level; + visit(armor_id, height_level, OutpostEKFParameters::height_offset(height_level), + next_layout); }; - visit_hypothesis(0); + auto const& last = *last_match; + visit_hypothesis(last.armor_id, last.height_level); - if (!layout.height_template.has_value()) { - auto const visit_side_hypotheses = [&](int armor_id) { - visit_hypothesis(armor_id, +kOutpostArmorHeightStep); - visit_hypothesis(armor_id, -kOutpostArmorHeightStep); - }; + auto const visit_next_hypotheses = [&](int direction, int first_delta, int second_delta) { + auto const armor_id = next_armor_id(last.armor_id, direction); + visit_hypothesis(armor_id, last.height_level + first_delta); + visit_hypothesis(armor_id, last.height_level + second_delta); + }; - visit_side_hypotheses(1); - visit_side_hypotheses(2); + auto const omega = ekf.x[6]; + if (omega > omega_deadzone) { + visit_next_hypotheses(-1, +1, -2); return; } - - for (int armor_id : { 1, 2 }) { - visit_hypothesis( - armor_id, OutpostEKFParameters::height_offset(*layout.height_template, armor_id)); + if (omega < -omega_deadzone) { + visit_next_hypotheses(+1, -1, +2); + return; } + + visit_next_hypotheses(-1, +1, -2); + visit_next_hypotheses(+1, -1, +2); } auto evaluate_match(OutpostEKFParameters::Measurement const& measurement, Armor3d const& armor, @@ -137,6 +149,7 @@ struct OutpostRobotState::Impl { color = CampColor::UNKNOWN; ekf = EKF {}; layout = OutpostArmorLayout {}; + last_match = std::nullopt; time_stamp = t; initialized = false; update_count = 0; @@ -149,7 +162,7 @@ struct OutpostRobotState::Impl { for (auto const& armor : armors) { auto const measurement = OutpostEKFParameters::measurement(armor); - visit_match_hypotheses([&](int armor_id, double height_offset, + visit_match_hypotheses([&](int armor_id, int height_level, double height_offset, OutpostArmorLayout const& next_layout) { auto matched_armor = armor; matched_armor.id = armor_id; @@ -159,7 +172,8 @@ struct OutpostRobotState::Impl { if (*mahalanobis >= best_mahalanobis) return; best_mahalanobis = *mahalanobis; - best_match = MatchedArmor { matched_armor, height_offset, next_layout }; + best_match = + MatchedArmor { matched_armor, height_level, height_offset, next_layout }; }); } @@ -177,19 +191,22 @@ struct OutpostRobotState::Impl { EKF::XVec const& x) { return OutpostEKFParameters::H(x, armor, height_offset); }, measurement.R, OutpostEKFParameters::x_add, OutpostEKFParameters::z_subtract); - layout = match.layout; + layout = match.layout; + last_match = LastMatch { match.armor.id, match.height_level }; update_count++; } CampColor color { CampColor::UNKNOWN }; EKF ekf { EKF {} }; OutpostArmorLayout layout {}; + std::optional last_match; TimePoint time_stamp; bool initialized { false }; int update_count { 0 }; std::chrono::duration reset_interval { 1.5 }; double mahalanobis_gate { 5.0 }; + double omega_deadzone { 0.2 }; }; OutpostRobotState::OutpostRobotState() noexcept diff --git a/src/module/predictor/outpost/snapshot.cpp b/src/module/predictor/outpost/snapshot.cpp index 7b8b9a89..17f708cf 100644 --- a/src/module/predictor/outpost/snapshot.cpp +++ b/src/module/predictor/outpost/snapshot.cpp @@ -34,13 +34,13 @@ struct OutpostSnapshot::Impl { auto predicted_armors(TimePoint t) const -> std::vector { auto const predicted_x = predict_state_at(t); - auto const predicted_armor_count = - layout.height_template.has_value() ? OutpostEKFParameters::kOutpostArmorCount : 1; auto armors = std::vector {}; - armors.reserve(predicted_armor_count); + armors.reserve(OutpostEKFParameters::kOutpostArmorCount); + + for (int id = 0; id < OutpostEKFParameters::kOutpostArmorCount; ++id) { + if (!layout.height_levels[static_cast(id)].has_value()) continue; - for (int id = 0; id < predicted_armor_count; ++id) { auto armor = make_armor(DeviceId::OUTPOST, color, id); auto const height = OutpostEKFParameters::height_offset(layout, armor.id); auto const angle = OutpostEKFParameters::armor_yaw(predicted_x, id); From 503a5fa59416127247847f9e25f2d3388be49237 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 11 Jun 2026 13:03:39 +0800 Subject: [PATCH 13/21] refactor: restructure fire_control module --- config/config.yaml | 12 +- src/component.cpp | 36 ++-- src/kernel/fire_control.cpp | 172 +++++++++-------- src/kernel/fire_control.hpp | 19 +- ...m_point_chooser.cpp => armor_selector.cpp} | 119 ++++++------ ...m_point_chooser.hpp => armor_selector.hpp} | 15 +- ...iny_mpc_axis_solver.cpp => mpc_solver.cpp} | 59 ++---- ...iny_mpc_axis_solver.hpp => mpc_solver.hpp} | 8 +- .../planner/mpc_trajectory_planner.cpp | 118 ------------ src/module/fire_control/planner/mpc_types.hpp | 7 +- .../planner/reference_builder.cpp | 65 +++++++ .../planner/reference_builder.hpp | 23 +++ .../planner/reference_trajectory_builder.cpp | 45 ----- .../planner/reference_trajectory_builder.hpp | 24 --- .../planner/trajectory_planner.cpp | 182 ++++++++++++++++++ ...ory_planner.hpp => trajectory_planner.hpp} | 12 +- .../solver/aim_point_sampling.cpp | 63 ------ .../solver/aim_point_sampling.hpp | 38 ---- .../solver/target_solution_solver.cpp | 73 ------- .../solver/target_solution_solver.hpp | 30 --- src/module/fire_control/target_solver.cpp | 141 ++++++++++++++ src/module/fire_control/target_solver.hpp | 24 +++ .../fire_control/trajectory_solution.hpp | 6 +- src/module/fire_control/types.hpp | 31 +++ src/runtime.cpp | 38 ++-- src/utility/math/tiny_mpc_solver.hpp | 42 ++-- src/utility/shared/context.hpp | 6 +- test/CMakeLists.txt | 22 --- test/aim_point_chooser.cpp | 124 ------------ test/tiny_mpc_axis_solver.cpp | 85 -------- 30 files changed, 743 insertions(+), 896 deletions(-) rename src/module/fire_control/{aim_point_chooser.cpp => armor_selector.cpp} (53%) rename src/module/fire_control/{aim_point_chooser.hpp => armor_selector.hpp} (55%) rename src/module/fire_control/planner/{tiny_mpc_axis_solver.cpp => mpc_solver.cpp} (60%) rename src/module/fire_control/planner/{tiny_mpc_axis_solver.hpp => mpc_solver.hpp} (73%) delete mode 100644 src/module/fire_control/planner/mpc_trajectory_planner.cpp create mode 100644 src/module/fire_control/planner/reference_builder.cpp create mode 100644 src/module/fire_control/planner/reference_builder.hpp delete mode 100644 src/module/fire_control/planner/reference_trajectory_builder.cpp delete mode 100644 src/module/fire_control/planner/reference_trajectory_builder.hpp create mode 100644 src/module/fire_control/planner/trajectory_planner.cpp rename src/module/fire_control/planner/{mpc_trajectory_planner.hpp => trajectory_planner.hpp} (58%) delete mode 100644 src/module/fire_control/solver/aim_point_sampling.cpp delete mode 100644 src/module/fire_control/solver/aim_point_sampling.hpp delete mode 100644 src/module/fire_control/solver/target_solution_solver.cpp delete mode 100644 src/module/fire_control/solver/target_solution_solver.hpp create mode 100644 src/module/fire_control/target_solver.cpp create mode 100644 src/module/fire_control/target_solver.hpp create mode 100644 src/module/fire_control/types.hpp delete mode 100644 test/aim_point_chooser.cpp delete mode 100644 test/tiny_mpc_axis_solver.cpp diff --git a/config/config.yaml b/config/config.yaml index bc19bc9a..99217d1c 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -85,17 +85,18 @@ pose_estimator: outpost_armor_thickness: 0.02 fire_control: - initial_bullet_speed: 21.4 # m/s + initial_bullet_speed: 22.5 # m/s shoot_delay: 0.07 # s mpc_enable: true - yaw_offset: 0.0 # degree - pitch_offset: 0.0 # degree + yaw_offset: +0.0 # degree + pitch_offset: +0.0 # degree - aim_point_chooser: + armor_target_selector: coming_angle: 70.0 # degree leaving_angle: 20.0 # degree - outpost_coming_angle: 60.0 # degree + outpost_coming_angle: 70.0 # degree outpost_leaving_angle: 30.0 # degree + switch_threshold: 8.0 # degree mpc: mpc_max_yaw_acc: 50.0 @@ -106,6 +107,7 @@ 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 diff --git a/src/component.cpp b/src/component.cpp index 8724164c..20b77b47 100644 --- a/src/component.cpp +++ b/src/component.cpp @@ -43,7 +43,6 @@ class AutoAimComponent final : public rmcs_executor::Component { OutputInterface pitch_rate; OutputInterface yaw_acc; OutputInterface pitch_acc; - OutputInterface feedforward_valid; InputInterface robot_id; @@ -81,7 +80,6 @@ class AutoAimComponent final : public rmcs_executor::Component { register_output("/auto_aim/pitch_rate", pitch_rate, 0.0); register_output("/auto_aim/yaw_acc", yaw_acc, 0.0); register_output("/auto_aim/pitch_acc", pitch_acc, 0.0); - register_output("/auto_aim/feedforward_valid", feedforward_valid, false); register_input("/referee/id", robot_id, true); } @@ -95,15 +93,14 @@ class AutoAimComponent final : public rmcs_executor::Component { // Reset all command { - *should_control = false; - *should_shoot = false; - *target_direction = kVectorNaN; - *robot_center = kVectorNaN; - *yaw_rate = kNaN; - *pitch_rate = kNaN; - *yaw_acc = kNaN; - *pitch_acc = kNaN; - *feedforward_valid = false; + *should_control = false; + *should_shoot = false; + *target_direction = kVectorNaN; + *robot_center = kVectorNaN; + *yaw_rate = kNaN; + *pitch_rate = kNaN; + *yaw_acc = kNaN; + *pitch_acc = kNaN; } feishu.heartbeat(); @@ -119,14 +116,13 @@ class AutoAimComponent final : public rmcs_executor::Component { } // 业务开关 - *should_control = command.should_control; - *should_shoot = command.should_shoot; - *yaw_rate = command.yaw_rate; - *pitch_rate = command.pitch_rate; - *yaw_acc = command.yaw_acc; - *pitch_acc = command.pitch_acc; - *feedforward_valid = command.feedforward_valid; - *robot_center = { + *should_control = command.should_control; + *should_shoot = command.should_shoot; + *yaw_rate = command.yaw_rate; + *pitch_rate = command.pitch_rate; + *yaw_acc = command.yaw_acc; + *pitch_acc = command.pitch_acc; + *robot_center = { command.robot_center.x, command.robot_center.y, command.robot_center.z, @@ -139,7 +135,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), }; } }; diff --git a/src/kernel/fire_control.cpp b/src/kernel/fire_control.cpp index 411a2cbe..6b24d2cb 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,112 @@ 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, + .center_position = center_position, + .aim_point_position = aim_point_position, + }; + shoot_permitted = shoot_evaluator.evaluate(shoot_command, gimbal_state.yaw); + } + // 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 = center_position, + .pitch = pitch, + .yaw = yaw, + .feedforward = feedforward, + .shoot_permitted = shoot_permitted, + .center_position = 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 +181,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 8130f889..b1ec6b6b 100644 --- a/src/kernel/fire_control.hpp +++ b/src/kernel/fire_control.hpp @@ -1,10 +1,12 @@ #pragma once #include - #include +#include + #include +#include "module/fire_control/types.hpp" #include "module/predictor/snapshot.hpp" #include "utility/pimpl.hpp" @@ -14,21 +16,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; Eigen::Vector3d 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/module/fire_control/aim_point_chooser.cpp b/src/module/fire_control/armor_selector.cpp similarity index 53% rename from src/module/fire_control/aim_point_chooser.cpp rename to src/module/fire_control/armor_selector.cpp index c4ca7564..cdd1f331 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 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 }; }; - 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 c287d234..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/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/solver/aim_point_sampling.cpp b/src/module/fire_control/solver/aim_point_sampling.cpp deleted file mode 100644 index e72ec4fb..00000000 --- a/src/module/fire_control/solver/aim_point_sampling.cpp +++ /dev/null @@ -1,63 +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_motion = snapshot.motion_at(t); - auto chosen_armor = chooser.choose_armor( - predicted_armors, predicted_motion.center_position, predicted_motion.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 c2cec115..00000000 --- a/src/module/fire_control/solver/aim_point_sampling.hpp +++ /dev/null @@ -1,38 +0,0 @@ -#pragma once - -#include -#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 0cfe61bc..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_motion = snapshot.motion(); - auto target_position = target_motion.center_position; - 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_motion = snapshot.motion_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_motion.center_position, - .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 0a981bc1..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 1c3335ee..12458778 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..bdd36273 --- /dev/null +++ b/src/module/fire_control/types.hpp @@ -0,0 +1,31 @@ +#pragma once + +#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 GimbalState { + TimePoint timestamp; + double yaw { 0.0 }; + double pitch { 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/runtime.cpp b/src/runtime.cpp index 692b9911..6c852c03 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -191,21 +191,29 @@ auto main() -> int { auto armors = snapshot->predicted_armors(Clock::now()); visualization.publish(armors, "visible_robot"); - const auto yaw = context.yaw; - if (auto result = fire_control.solve(*snapshot, yaw)) { - command.should_control = true; - command.target = target.target_id; - command.should_shoot = result->shoot_permitted; - command.yaw = result->yaw; - command.pitch = result->pitch; - command.yaw_rate = result->yaw_rate; - command.pitch_rate = result->pitch_rate; - command.yaw_acc = result->yaw_acc; - command.pitch_acc = result->pitch_acc; - command.feedforward_valid = result->feedforward_valid; - command.robot_center = result->center_position; - - visualization.update_aiming_direction(command.yaw, -command.pitch); + auto const gimbal_state = fire_control::GimbalState { + .timestamp = context.timestamp, + .yaw = context.yaw, + .pitch = context.pitch, + }; + if (auto result = fire_control.solve(*snapshot, gimbal_state)) { + command.should_control = true; + command.target = target.target_id; + command.should_shoot = result->shoot_permitted; + command.yaw = result->yaw; + command.pitch = result->pitch; + command.robot_center = result->center_position; + + if (result->feedforward.has_value()) { + command.yaw_rate = result->feedforward->yaw_rate; + command.pitch_rate = result->feedforward->pitch_rate; + command.yaw_acc = result->feedforward->yaw_acc; + command.pitch_acc = result->feedforward->pitch_acc; + } + + auto impact_armors = snapshot->predicted_armors(result->impact_time); + visualization.publish(impact_armors, "impact_robot"); + visualization.update_aiming_direction(command.yaw, command.pitch); // TODO: 将 MPC 的 target 曲线和 Yaw 置于一个统一的参考系 visualization.update_mpc_plan(command.yaw, command.pitch, command.yaw_rate, 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/shared/context.hpp b/src/utility/shared/context.hpp index ea3e2f34..3b6b6463 100644 --- a/src/utility/shared/context.hpp +++ b/src/utility/shared/context.hpp @@ -16,7 +16,7 @@ struct AutoAimState { static constexpr auto kLength = 512; static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); - TimePoint timestamp { }; + TimePoint timestamp {}; bool should_control { false }; bool should_shoot = { false }; @@ -27,8 +27,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 }; @@ -50,7 +48,7 @@ struct SystemContext { /// Dynamic Context /// - TimePoint timestamp { }; + TimePoint timestamp {}; double yaw { kNaN }; double pitch { kNaN }; 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 bbceb20d..00000000 --- a/test/aim_point_chooser.cpp +++ /dev/null @@ -1,124 +0,0 @@ -#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); -} From df221a5a909b7455af22273e1872b76992372051 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 15 Jun 2026 13:57:28 +0800 Subject: [PATCH 14/21] refactor: restructure outpost ekf --- src/module/predictor/outpost/armor_layout.hpp | 9 +- .../predictor/outpost/ekf_parameter.hpp | 149 +++---- src/module/predictor/outpost/robot_state.cpp | 363 ++++++++++++------ src/module/predictor/outpost/snapshot.cpp | 32 +- src/module/predictor/outpost/snapshot.hpp | 2 +- 5 files changed, 337 insertions(+), 218 deletions(-) diff --git a/src/module/predictor/outpost/armor_layout.hpp b/src/module/predictor/outpost/armor_layout.hpp index 163bd7b8..367e2fdc 100644 --- a/src/module/predictor/outpost/armor_layout.hpp +++ b/src/module/predictor/outpost/armor_layout.hpp @@ -1,14 +1,19 @@ #pragma once #include -#include namespace rmcs::predictor { +struct OutpostArmorSlot { + double phase_offset { 0.0 }; + double height_offset { 0.0 }; + bool assigned { false }; +}; + struct OutpostArmorLayout { static constexpr int kSlotCount = 3; - std::array, kSlotCount> height_levels {}; + 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 796b1fd9..0107fe88 100644 --- a/src/module/predictor/outpost/ekf_parameter.hpp +++ b/src/module/predictor/outpost/ekf_parameter.hpp @@ -4,7 +4,6 @@ #include "utility/math/angle.hpp" #include "utility/math/conversion.hpp" #include "utility/math/kalman_filter/ekf.hpp" -#include "utility/panic.hpp" #include "utility/robot/armor.hpp" #include "utility/robot/constant.hpp" @@ -15,22 +14,22 @@ namespace rmcs::predictor { struct OutpostEKFParameters { - using EKF = util::EKF<7, 4>; + using EKF = util::EKF<6, 4>; - struct Measurement { - EKF::ZVec z; - EKF::RMat R; + 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; - // x vx y vy z a w + // x vx y vy z a // x, y：前哨站旋转中心在世界坐标系下的位置 // vx, vy：前哨站旋转中心在世界坐标系下的线速度 // z：参考装甲板(id 0)在世界坐标系下的 z 坐标 // a：参考装甲板(id 0)的 yaw 角 - // w：参考装甲板(id 0)的角速度 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; @@ -42,13 +41,13 @@ struct OutpostEKFParameters { 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, 0.0; + x << center_x, 0.0, center_y, 0.0, trans_z, yaw; return x; } static auto P_initial_dig() -> EKF::PDig { auto P_dig = EKF::PDig {}; - P_dig << 1.0, 64.0, 1.0, 64.0, 1.0, 0.4, 16.0; + P_dig << 1.0, 64.0, 1.0, 64.0, 1.0, 0.4; return P_dig; } @@ -56,33 +55,9 @@ struct OutpostEKFParameters { return util::normalize_angle(x[5] + phase_offset); } - static auto phase_offset(int armor_id) -> double { - constexpr auto kPhaseOffsets = - std::array { 0.0, kPhaseStep, -kPhaseStep }; - if (armor_id < 0 || armor_id >= kOutpostArmorCount) { - util::panic("outpost armor id out of range"); - } - return kPhaseOffsets[static_cast(armor_id)]; - } - - static auto armor_yaw(EKF::XVec const& x, int armor_id) -> double { - return armor_yaw(x, phase_offset(armor_id)); - } - - static auto height_offset(int height_level) -> double { - using rmcs::kOutpostArmorHeightStep; - return height_level * kOutpostArmorHeightStep; - } - - static auto height_offset(OutpostArmorLayout const& layout, int armor_id) -> double { - if (armor_id < 0 || armor_id >= kOutpostArmorCount) { - util::panic("outpost armor id out of range"); - } - auto const height_level = layout.height_levels[static_cast(armor_id)]; - if (!height_level.has_value()) { - util::panic("outpost armor height level is unknown"); - } - return height_offset(*height_level); + 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) @@ -104,10 +79,6 @@ struct OutpostEKFParameters { return z; } - static auto h(EKF::XVec const& x, Armor3d const& armor, double height_offset) -> EKF::ZVec { - return h(x, phase_offset(armor.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]); @@ -126,13 +97,12 @@ struct OutpostEKFParameters { auto F = EKF::AMat {}; // clang-format off F << - 1, dt, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, 0, - 0, 0, 1, dt, 0, 0, 0, - 0, 0, 0, 1, 0, 0, 0, - 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 0, 1, dt, - 0, 0, 0, 0, 0, 0, 1; + 1, dt, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, + 0, 0, 1, dt, 0, 0, + 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1; // clang-format on return F; } @@ -140,13 +110,13 @@ 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 angular_acc_var = 4.0; - const auto v1 = linear_acc_var; - const auto v2 = z_ref_rw_var; - const auto v3 = angular_acc_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; @@ -154,41 +124,25 @@ struct OutpostEKFParameters { auto Q = EKF::QMat {}; // clang-format off - Q << a * v1, b * v1, 0, 0, 0, 0, 0, - b * v1, c * v1, 0, 0, 0, 0, 0, - 0, 0, a * v1, b * v1, 0, 0, 0, - 0, 0, b * v1, c * v1, 0, 0, 0, - 0, 0, 0, 0, v2 * dt, 0, 0, - 0, 0, 0, 0, 0, a * v3, b * v3, - 0, 0, 0, 0, 0, b * v3, c * v3; + Q << a * v1, b * v1, 0, 0, 0, 0, + b * v1, c * v1, 0, 0, 0, 0, + 0, 0, a * v1, b * v1, 0, 0, + 0, 0, b * v1, c * v1, 0, 0, + 0, 0, 0, 0, v2 * dt, 0, + 0, 0, 0, 0, 0, v3 * dt; // clang-format on return Q; } - static auto f(double dt) -> auto { - return [dt](EKF::XVec const& x) { + 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_prior[5]); + 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]; - - auto R_dig = EKF::RDig {}; - // clang-format off - R_dig << 4e-3, 4e-3, std::log(std::abs(delta_yaw) + 1.0) + 1.0, - std::log(std::abs(distance) + 1.0) / 200.0 + 9e-2; - // clang-format on - - return R_dig.asDiagonal(); - } - - static auto measurement(Armor3d const& armor) -> Measurement { + 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 }; @@ -198,10 +152,27 @@ struct OutpostEKFParameters { auto const ypr = util::eulers(orientation); auto const ypd = util::xyz2ypd(xyz); + return { xyz, ypr, ypd }; + } + + static auto z(ArmorObservation const& obs) -> EKF::ZVec { auto z = EKF::ZVec {}; - z << ypd[0], ypd[1], ypd[2], ypr[0]; + 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; + // clang-format on - return { z, R(xyz, ypr, ypd) }; + return R_dig.asDiagonal(); } static auto H(EKF::XVec const& x, double phase_offset, double height_offset) -> EKF::HMat { @@ -211,13 +182,13 @@ 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, 0, - 0, 0, 1, 0, 0, dy_da, 0, - 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 0, 1, 0; + 1, 0, 0, 0, 0, dx_da, + 0, 0, 1, 0, 0, dy_da, + 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1; // clang-format on const auto xyz = h_armor_xyz(x, phase_offset, height_offset); @@ -234,10 +205,6 @@ struct OutpostEKFParameters { return H_armor_ypda * H_armor_xyza; } - - static auto H(EKF::XVec const& x, Armor3d const& armor, double height_offset) -> EKF::HMat { - return H(x, phase_offset(armor.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 c1128eff..840b2428 100644 --- a/src/module/predictor/outpost/robot_state.cpp +++ b/src/module/predictor/outpost/robot_state.cpp @@ -1,46 +1,71 @@ #include "robot_state.hpp" #include "module/predictor/outpost/snapshot.hpp" -#include "utility/math/mahalanobis.hpp" +#include "utility/math/angle.hpp" #include "utility/time.hpp" +#include +#include #include #include #include -#include +#include namespace rmcs::predictor { struct OutpostRobotState::Impl { + 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 CandidateScore { + Candidate candidate; + double score { std::numeric_limits::infinity() }; + }; + + struct MatchResult { + OutpostEKFParameters::ArmorObservation observation; + Candidate candidate; + double reference_yaw { 0.0 }; + double score { std::numeric_limits::infinity() }; + }; + explicit Impl(TimePoint stamp) noexcept : time_stamp { stamp } { } auto initialize(Armor3d const& armor, TimePoint t) -> void { - color = armor_color2camp_color(armor.color); - ekf = EKF { OutpostEKFParameters::x(armor), + color = armor_color2camp_color(armor.color); + time_stamp = t; + update_count = 0; + motion_mode = std::nullopt; + + layout = OutpostArmorLayout {}; + assign_slot(0, 0.0, 0.0); + last_matched_slot_id = 0; + + ekf = EKF { OutpostEKFParameters::x(armor), OutpostEKFParameters::P_initial_dig().asDiagonal() }; - time_stamp = t; + initialized = true; - layout = OutpostArmorLayout {}; - layout.height_levels[0] = 0; - last_match = LastMatch { 0, 0 }; - update_count = 0; - initialized = true; + auto const observation = OutpostEKFParameters::observe(armor); + mode_reference_yaw = observation.ypr[0]; + mode_reference_stamp = t; } auto predict(TimePoint t) -> void { if (t <= time_stamp) return; if (initialized) { - auto dt = rmcs::util::delta_time(t, time_stamp); - if (dt > reset_interval) { - reset_runtime_state(t); - return; - } - + auto const dt = util::delta_time(t, time_stamp); auto const dt_s = dt.count(); ekf.predict( - OutpostEKFParameters::f(dt_s), + OutpostEKFParameters::f(dt_s, angular_velocity()), [dt_s](EKF::XVec const&) { return OutpostEKFParameters::F(dt_s); }, OutpostEKFParameters::Q(dt_s)); } @@ -53,160 +78,276 @@ struct OutpostRobotState::Impl { if (!initialized) { initialize(armors.front(), time_stamp); + ++update_count; return true; } - auto best_match = select_best_match(armors); - if (!best_match.has_value()) return false; + auto match = select_best_match(armors); + if (!match.has_value()) return false; - apply_match(*best_match); + if (apply_match(*match)) ++update_count; return true; } - // TODO:添加收敛条件 auto is_converged() const -> bool { if (!initialized) return false; - return true; + if (!std::isfinite(distance())) return false; + + constexpr int min_updates = 3; + return layout.slots[0].assigned && update_count >= min_updates; } auto get_snapshot() const -> std::optional { if (!initialized) return std::nullopt; - return Snapshot { OutpostSnapshot { ekf.x, color, time_stamp, layout } }; + return Snapshot { OutpostSnapshot { + ekf.x, color, time_stamp, layout, angular_velocity() } }; } auto distance() const -> double { - return initialized ? std::sqrt(ekf.x[0] * ekf.x[0] + ekf.x[2] * ekf.x[2]) - : std::numeric_limits::infinity(); + if (!initialized) return std::numeric_limits::infinity(); + return std::hypot(ekf.x[0], ekf.x[2]); } private: - struct MatchedArmor { - Armor3d armor; - int height_level; - double height_offset; - OutpostArmorLayout layout; + static constexpr auto kPhaseOffsets = std::array { + 0.0, + OutpostEKFParameters::kPhaseStep, + -OutpostEKFParameters::kPhaseStep, }; - struct LastMatch { - int armor_id; - int height_level; - }; + static constexpr auto kModeConfirmWindow = std::chrono::duration { 0.2 }; + static constexpr auto kStaticReferenceYawThreshold = 0.08; + static constexpr auto kRotationReferenceYawThreshold = 0.25; - static auto next_armor_id(int armor_id, int direction) -> int { - constexpr auto armor_count = OutpostEKFParameters::kOutpostArmorCount; - return (armor_id + direction + armor_count) % armor_count; + 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 visit_match_hypotheses(auto&& visit) const -> void { - if (!last_match.has_value()) return; + static constexpr auto mode_angular_velocity(MotionMode mode) -> double { + switch (mode) { + case MotionMode::STATIC: + return 0.0; + case MotionMode::CW: + return -kOutpostAngularSpeed; + case MotionMode::CCW: + return kOutpostAngularSpeed; + } - auto const visit_hypothesis = [&](int armor_id, int height_level) { - auto next_layout = layout; - auto& known_level = next_layout.height_levels[static_cast(armor_id)]; - if (known_level.has_value() && *known_level != height_level) return; + return 0.0; + } - known_level = height_level; - visit(armor_id, height_level, OutpostEKFParameters::height_offset(height_level), - next_layout); - }; + auto angular_velocity() const -> double { + if (!motion_mode.has_value()) return 0.0; + return mode_angular_velocity(*motion_mode); + } + + static constexpr auto next_slot_id(int slot_id, MotionMode mode) -> int { + constexpr int slot_count = OutpostEKFParameters::kOutpostArmorCount; + switch (mode) { + case MotionMode::CW: + return (slot_id + 1) % slot_count; + case MotionMode::CCW: + return (slot_id + slot_count - 1) % slot_count; + case MotionMode::STATIC: + return slot_id; + } - auto const& last = *last_match; - visit_hypothesis(last.armor_id, last.height_level); + return slot_id; + } - auto const visit_next_hypotheses = [&](int direction, int first_delta, int second_delta) { - auto const armor_id = next_armor_id(last.armor_id, direction); - visit_hypothesis(armor_id, last.height_level + first_delta); - visit_hypothesis(armor_id, last.height_level + second_delta); + static constexpr auto height_steps(MotionMode mode) { + switch (mode) { + case MotionMode::CW: + return std::array { -1.0, 2.0 }; + case MotionMode::CCW: + return std::array { 1.0, -2.0 }; + case MotionMode::STATIC: + return std::array { 0.0, 0.0 }; + } + + return std::array { 0.0, 0.0 }; + } + + auto candidate_for_assigned_slot(int slot_id) const -> Candidate { + auto const& slot = layout.slots.at(slot_id); + return { + .slot_id = slot_id, + .phase_offset = slot.phase_offset, + .height_offset = slot.height_offset, + .assigned = true, + .motion_mode = motion_mode, }; + } - auto const omega = ekf.x[6]; - if (omega > omega_deadzone) { - visit_next_hypotheses(-1, +1, -2); - return; + auto candidate_for_slot(int slot_id, double phase_offset, double height_offset, + MotionMode mode) const -> Candidate { + return Candidate { + .slot_id = slot_id, + .phase_offset = phase_offset, + .height_offset = height_offset, + .assigned = layout.slots.at(slot_id).assigned, + .motion_mode = mode, + }; + } + + auto make_candidates() const -> std::vector { + auto candidates = std::vector {}; + candidates.reserve(OutpostEKFParameters::kOutpostArmorCount * 3); + + for (int slot_id = 0; slot_id < OutpostEKFParameters::kOutpostArmorCount; ++slot_id) { + if (layout.slots.at(slot_id).assigned) { + candidates.emplace_back(candidate_for_assigned_slot(slot_id)); + } } - if (omega < -omega_deadzone) { - visit_next_hypotheses(+1, -1, +2); - return; + + if (motion_mode.has_value()) { + if (*motion_mode == MotionMode::STATIC) return candidates; + append_next_slot_candidates(candidates, *motion_mode); + return candidates; } - visit_next_hypotheses(-1, +1, -2); - visit_next_hypotheses(+1, -1, +2); + append_next_slot_candidates(candidates, MotionMode::CW); + append_next_slot_candidates(candidates, MotionMode::CCW); + return candidates; } - auto evaluate_match(OutpostEKFParameters::Measurement const& measurement, Armor3d const& armor, - double height_offset) const -> std::optional { - auto const H = OutpostEKFParameters::H(ekf.x, armor, height_offset); - auto const z_hat = OutpostEKFParameters::h(ekf.x, armor, height_offset); - auto const innovation = OutpostEKFParameters::z_subtract(measurement.z, z_hat); - auto const S = H * ekf.P() * H.transpose() + measurement.R; - auto const mahalanobis = rmcs::util::mahalanobis_distance(innovation, S); - if (!mahalanobis.has_value() || *mahalanobis > mahalanobis_gate) return std::nullopt; + auto append_next_slot_candidates(std::vector& candidates, MotionMode mode) const + -> void { + auto const source_slot_id = last_matched_slot_id; + auto const target_slot_id = next_slot_id(source_slot_id, mode); + auto const& source_slot = layout.slots.at(source_slot_id); + auto const& target_slot = layout.slots.at(target_slot_id); - return *mahalanobis; + if (target_slot.assigned) return; + + for (auto const height_step : height_steps(mode)) { + candidates.emplace_back( + candidate_for_slot(target_slot_id, kPhaseOffsets.at(target_slot_id), + source_slot.height_offset + height_step * kOutpostArmorHeightStep, mode)); + } } - auto reset_runtime_state(TimePoint t) -> void { - color = CampColor::UNKNOWN; - ekf = EKF {}; - layout = OutpostArmorLayout {}; - last_match = std::nullopt; - time_stamp = t; - initialized = false; - update_count = 0; + auto score_candidate(OutpostEKFParameters::ArmorObservation const& observation, + Candidate const& candidate) const -> double { + auto const predicted_xyz = OutpostEKFParameters::h_armor_xyz( + ekf.x, candidate.phase_offset, candidate.height_offset); + auto const predicted_yaw = OutpostEKFParameters::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)); + return position_error + kOutpostRadius * yaw_error; } - auto select_best_match(std::span armors) const -> std::optional { - auto best_match = std::optional {}; - auto best_mahalanobis = std::numeric_limits::infinity(); + auto best_candidate(OutpostEKFParameters::ArmorObservation const& observation) const + -> std::optional { + auto const candidates = make_candidates(); + if (candidates.empty()) return std::nullopt; - for (auto const& armor : armors) { - auto const measurement = OutpostEKFParameters::measurement(armor); + auto best = CandidateScore {}; + for (auto const& candidate : candidates) { + auto const score = score_candidate(observation, candidate); + if (score >= best.score) continue; - visit_match_hypotheses([&](int armor_id, int height_level, double height_offset, - OutpostArmorLayout const& next_layout) { - auto matched_armor = armor; - matched_armor.id = armor_id; + best = { + .candidate = candidate, + .score = score, + }; + } - auto const mahalanobis = evaluate_match(measurement, matched_armor, height_offset); - if (!mahalanobis.has_value()) return; - if (*mahalanobis >= best_mahalanobis) return; + return best; + } - best_mahalanobis = *mahalanobis; - best_match = - MatchedArmor { matched_armor, height_level, height_offset, next_layout }; - }); + auto select_best_match(std::span armors) const -> std::optional { + auto best_match = std::optional {}; + + for (std::size_t observation_index = 0; observation_index < armors.size(); + ++observation_index) { + auto const& armor = armors[observation_index]; + if (armor.genre != DeviceId::OUTPOST) continue; + + auto const observation = OutpostEKFParameters::observe(armor); + auto const candidate_score = best_candidate(observation); + if (!candidate_score.has_value()) continue; + if (best_match.has_value() && candidate_score->score >= best_match->score) continue; + + auto const reference_yaw = + util::normalize_angle(observation.ypr[0] - candidate_score->candidate.phase_offset); + best_match = MatchResult { + .observation = observation, + .candidate = candidate_score->candidate, + .reference_yaw = reference_yaw, + .score = candidate_score->score, + }; } return best_match; } - auto apply_match(MatchedArmor const& match) -> void { - auto const measurement = OutpostEKFParameters::measurement(match.armor); + auto apply_match(MatchResult const& match) -> bool { + if (!motion_mode.has_value() && !match.candidate.assigned + && match.candidate.motion_mode.has_value()) { + motion_mode = *match.candidate.motion_mode; + return false; + } + + update_motion_mode(match); + auto const& candidate = match.candidate; ekf.update( - measurement.z, - [armor = match.armor, height_offset = match.height_offset]( - EKF::XVec const& x) { return OutpostEKFParameters::h(x, armor, height_offset); }, - [armor = match.armor, height_offset = match.height_offset]( - EKF::XVec const& x) { return OutpostEKFParameters::H(x, armor, height_offset); }, - measurement.R, OutpostEKFParameters::x_add, OutpostEKFParameters::z_subtract); + OutpostEKFParameters::z(match.observation), + [candidate](EKF::XVec const& x) { + return OutpostEKFParameters::h(x, candidate.phase_offset, candidate.height_offset); + }, + [candidate](EKF::XVec const& x) { + return OutpostEKFParameters::H(x, candidate.phase_offset, candidate.height_offset); + }, + OutpostEKFParameters::R(match.observation), OutpostEKFParameters::x_add, + OutpostEKFParameters::z_subtract); + + if (!layout.slots.at(candidate.slot_id).assigned) { + assign_slot(candidate.slot_id, candidate.phase_offset, candidate.height_offset); + } + + last_matched_slot_id = candidate.slot_id; + return true; + } + + auto update_motion_mode(MatchResult const& match) -> void { + if (motion_mode.has_value()) return; + + if (match.candidate.motion_mode.has_value()) { + motion_mode = *match.candidate.motion_mode; + return; + } + + if (util::delta_time(time_stamp, mode_reference_stamp) < kModeConfirmWindow) return; + + auto const mode_delta = util::normalize_angle(match.reference_yaw - mode_reference_yaw); + auto const abs_delta = std::abs(mode_delta); + if (abs_delta < kStaticReferenceYawThreshold) motion_mode = MotionMode::STATIC; + else if (abs_delta > kRotationReferenceYawThreshold) { + motion_mode = mode_delta > 0.0 ? MotionMode::CCW : MotionMode::CW; + } - layout = match.layout; - last_match = LastMatch { match.armor.id, match.height_level }; - update_count++; + mode_reference_yaw = match.reference_yaw; + mode_reference_stamp = time_stamp; } CampColor color { CampColor::UNKNOWN }; - EKF ekf { EKF {} }; OutpostArmorLayout layout {}; - std::optional last_match; + EKF ekf { EKF {} }; TimePoint time_stamp; bool initialized { false }; int update_count { 0 }; - std::chrono::duration reset_interval { 1.5 }; - double mahalanobis_gate { 5.0 }; - double omega_deadzone { 0.2 }; + int last_matched_slot_id { 0 }; + + std::optional motion_mode; + double mode_reference_yaw { 0.0 }; + TimePoint mode_reference_stamp; }; OutpostRobotState::OutpostRobotState() noexcept diff --git a/src/module/predictor/outpost/snapshot.cpp b/src/module/predictor/outpost/snapshot.cpp index 17f708cf..a5e55552 100644 --- a/src/module/predictor/outpost/snapshot.cpp +++ b/src/module/predictor/outpost/snapshot.cpp @@ -9,11 +9,13 @@ namespace rmcs::predictor { struct OutpostSnapshot::Impl { - explicit Impl(EKF::XVec x, CampColor color, TimePoint stamp, OutpostArmorLayout layout) + explicit Impl(EKF::XVec x, CampColor color, TimePoint stamp, OutpostArmorLayout layout, + double angular_velocity) : x { std::move(x) } , color { color } , stamp { stamp } - , layout { layout } { } + , layout { layout } + , angular_velocity { angular_velocity } { } static auto make_armor(DeviceId device, CampColor color, int id) -> Armor3d { auto armor = Armor3d {}; @@ -23,13 +25,16 @@ struct OutpostSnapshot::Impl { return armor; } - static auto motion_of(EKF::XVec const& x) -> TargetMotion { - return { Eigen::Vector3d { x[0], x[2], x[4] }, x[6] }; + static auto motion_of(EKF::XVec const& x, double angular_velocity) -> TargetMotion { + return { + Eigen::Vector3d { x[0], x[2], x[4] }, + angular_velocity, + }; } auto predict_state_at(TimePoint t) const -> EKF::XVec { auto const dt = util::delta_time(t, stamp).count(); - return OutpostEKFParameters::f(dt)(x); + return OutpostEKFParameters::f(dt, angular_velocity)(x); } auto predicted_armors(TimePoint t) const -> std::vector { @@ -39,13 +44,13 @@ struct OutpostSnapshot::Impl { armors.reserve(OutpostEKFParameters::kOutpostArmorCount); for (int id = 0; id < OutpostEKFParameters::kOutpostArmorCount; ++id) { - if (!layout.height_levels[static_cast(id)].has_value()) continue; + auto const& slot = layout.slots[id]; + if (!slot.assigned) continue; auto armor = make_armor(DeviceId::OUTPOST, color, id); - auto const height = OutpostEKFParameters::height_offset(layout, armor.id); - auto const angle = OutpostEKFParameters::armor_yaw(predicted_x, id); + auto const angle = OutpostEKFParameters::armor_yaw(predicted_x, layout, id); auto const position = OutpostEKFParameters::h_armor_xyz( - predicted_x, OutpostEKFParameters::phase_offset(armor.id), height); + predicted_x, slot.phase_offset, slot.height_offset); armor.translation = position; armor.orientation = util::euler_to_quaternion(angle, kPredictedOutpostArmorPitch, 0); @@ -59,11 +64,12 @@ struct OutpostSnapshot::Impl { CampColor color; TimePoint stamp; OutpostArmorLayout layout; + double angular_velocity { 0.0 }; }; -OutpostSnapshot::OutpostSnapshot( - EKF::XVec x, CampColor color, TimePoint stamp, OutpostArmorLayout layout) - : pimpl { std::make_unique(std::move(x), color, stamp, layout) } { } +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) } { } OutpostSnapshot::OutpostSnapshot(OutpostSnapshot&&) noexcept = default; auto OutpostSnapshot::operator=(OutpostSnapshot&&) noexcept -> OutpostSnapshot& = default; @@ -75,7 +81,7 @@ 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)); + return Impl::motion_of(pimpl->predict_state_at(t), pimpl->angular_velocity); } auto OutpostSnapshot::predicted_armors(TimePoint t) const -> std::vector { diff --git a/src/module/predictor/outpost/snapshot.hpp b/src/module/predictor/outpost/snapshot.hpp index 9d3a0460..22f60e7d 100644 --- a/src/module/predictor/outpost/snapshot.hpp +++ b/src/module/predictor/outpost/snapshot.hpp @@ -14,7 +14,7 @@ class OutpostSnapshot { using EKF = OutpostEKFParameters::EKF; explicit OutpostSnapshot(EKF::XVec x, CampColor color, TimePoint stamp, - OutpostArmorLayout layout); + OutpostArmorLayout layout, double angular_velocity); OutpostSnapshot(OutpostSnapshot const&) = delete; OutpostSnapshot(OutpostSnapshot&&) noexcept; OutpostSnapshot& operator=(OutpostSnapshot const&) = delete; From f4bd2a87d52220651833186bc4e19b234cfd6b99 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 15 Jun 2026 21:51:33 +0800 Subject: [PATCH 15/21] feat:add outpost rotation direction confirm --- src/module/predictor/outpost/robot_state.cpp | 131 +++++++++++-------- 1 file changed, 78 insertions(+), 53 deletions(-) diff --git a/src/module/predictor/outpost/robot_state.cpp b/src/module/predictor/outpost/robot_state.cpp index 840b2428..75569c36 100644 --- a/src/module/predictor/outpost/robot_state.cpp +++ b/src/module/predictor/outpost/robot_state.cpp @@ -36,14 +36,27 @@ struct OutpostRobotState::Impl { 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 }; + }; + explicit Impl(TimePoint stamp) noexcept : time_stamp { stamp } { } auto initialize(Armor3d const& armor, TimePoint t) -> void { - color = armor_color2camp_color(armor.color); - time_stamp = t; - update_count = 0; - motion_mode = std::nullopt; + color = armor_color2camp_color(armor.color); + time_stamp = t; + update_count = 0; + motion_mode = std::nullopt; + rotation_evidence = {}; layout = OutpostArmorLayout {}; assign_slot(0, 0.0, 0.0); @@ -109,15 +122,10 @@ struct OutpostRobotState::Impl { } private: - static constexpr auto kPhaseOffsets = std::array { - 0.0, - OutpostEKFParameters::kPhaseStep, - -OutpostEKFParameters::kPhaseStep, - }; - - static constexpr auto kModeConfirmWindow = std::chrono::duration { 0.2 }; - static constexpr auto kStaticReferenceYawThreshold = 0.08; - static constexpr auto kRotationReferenceYawThreshold = 0.25; + static constexpr auto kModeConfirmWindow = std::chrono::duration { 0.2 }; + static constexpr auto kStaticYawDeltaThreshold = util::deg2rad(4.6); + static constexpr auto kRotationYawDeltaThreshold = util::deg2rad(14.3); + 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); @@ -126,49 +134,39 @@ struct OutpostRobotState::Impl { slot.assigned = true; } - static constexpr auto mode_angular_velocity(MotionMode mode) -> double { + static constexpr auto topology_of(MotionMode mode) -> std::optional { switch (mode) { - case MotionMode::STATIC: - return 0.0; case MotionMode::CW: - return -kOutpostAngularSpeed; + return RotationTopology { + .angular_velocity = -kOutpostAngularSpeed, + .slot_delta = 1, + .phase_delta = OutpostEKFParameters::kPhaseStep, + .height_steps = { -1.0, 2.0 }, + }; case MotionMode::CCW: - return kOutpostAngularSpeed; + return RotationTopology { + .angular_velocity = kOutpostAngularSpeed, + .slot_delta = -1, + .phase_delta = -OutpostEKFParameters::kPhaseStep, + .height_steps = { 1.0, -2.0 }, + }; + case MotionMode::STATIC: + return std::nullopt; } - return 0.0; + return std::nullopt; } auto angular_velocity() const -> double { if (!motion_mode.has_value()) return 0.0; - return mode_angular_velocity(*motion_mode); + auto const topology = topology_of(*motion_mode); + if (!topology.has_value()) return 0.0; + return topology->angular_velocity; } - static constexpr auto next_slot_id(int slot_id, MotionMode mode) -> int { + static constexpr auto wrap_slot_id(int slot_id) -> int { constexpr int slot_count = OutpostEKFParameters::kOutpostArmorCount; - switch (mode) { - case MotionMode::CW: - return (slot_id + 1) % slot_count; - case MotionMode::CCW: - return (slot_id + slot_count - 1) % slot_count; - case MotionMode::STATIC: - return slot_id; - } - - return slot_id; - } - - static constexpr auto height_steps(MotionMode mode) { - switch (mode) { - case MotionMode::CW: - return std::array { -1.0, 2.0 }; - case MotionMode::CCW: - return std::array { 1.0, -2.0 }; - case MotionMode::STATIC: - return std::array { 0.0, 0.0 }; - } - - return std::array { 0.0, 0.0 }; + return (slot_id + slot_count) % slot_count; } auto candidate_for_assigned_slot(int slot_id) const -> Candidate { @@ -216,17 +214,24 @@ struct OutpostRobotState::Impl { auto append_next_slot_candidates(std::vector& candidates, MotionMode mode) const -> void { + auto const topology = topology_of(mode); + if (!topology.has_value()) return; + auto const source_slot_id = last_matched_slot_id; - auto const target_slot_id = next_slot_id(source_slot_id, mode); + auto const target_slot_id = wrap_slot_id(source_slot_id + topology->slot_delta); auto const& source_slot = layout.slots.at(source_slot_id); auto const& target_slot = layout.slots.at(target_slot_id); if (target_slot.assigned) return; - for (auto const height_step : height_steps(mode)) { + 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_for_slot(target_slot_id, kPhaseOffsets.at(target_slot_id), - source_slot.height_offset + height_step * kOutpostArmorHeightStep, mode)); + candidate_for_slot(target_slot_id, phase_offset, height_offset, mode)); } } @@ -271,10 +276,11 @@ struct OutpostRobotState::Impl { auto const observation = OutpostEKFParameters::observe(armor); auto const candidate_score = best_candidate(observation); if (!candidate_score.has_value()) continue; - if (best_match.has_value() && candidate_score->score >= best_match->score) continue; auto const reference_yaw = util::normalize_angle(observation.ypr[0] - candidate_score->candidate.phase_offset); + if (best_match.has_value() && candidate_score->score >= best_match->score) continue; + best_match = MatchResult { .observation = observation, .candidate = candidate_score->candidate, @@ -289,10 +295,11 @@ struct OutpostRobotState::Impl { auto apply_match(MatchResult const& match) -> bool { if (!motion_mode.has_value() && !match.candidate.assigned && match.candidate.motion_mode.has_value()) { - motion_mode = *match.candidate.motion_mode; + update_rotation_evidence(*match.candidate.motion_mode); return false; } + rotation_evidence = {}; update_motion_mode(match); auto const& candidate = match.candidate; @@ -315,6 +322,22 @@ struct OutpostRobotState::Impl { return true; } + 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, + }; + } + + if (rotation_evidence.count >= kRotationConfirmCandidateCount) { + motion_mode = candidate_mode; + rotation_evidence = {}; + } + } + auto update_motion_mode(MatchResult const& match) -> void { if (motion_mode.has_value()) return; @@ -323,12 +346,13 @@ struct OutpostRobotState::Impl { return; } - if (util::delta_time(time_stamp, mode_reference_stamp) < kModeConfirmWindow) return; + auto const elapsed = util::delta_time(time_stamp, mode_reference_stamp); + if (elapsed < kModeConfirmWindow) return; auto const mode_delta = util::normalize_angle(match.reference_yaw - mode_reference_yaw); auto const abs_delta = std::abs(mode_delta); - if (abs_delta < kStaticReferenceYawThreshold) motion_mode = MotionMode::STATIC; - else if (abs_delta > kRotationReferenceYawThreshold) { + if (abs_delta < kStaticYawDeltaThreshold) motion_mode = MotionMode::STATIC; + else if (abs_delta > kRotationYawDeltaThreshold) { motion_mode = mode_delta > 0.0 ? MotionMode::CCW : MotionMode::CW; } @@ -346,6 +370,7 @@ struct OutpostRobotState::Impl { int last_matched_slot_id { 0 }; std::optional motion_mode; + RotationEvidence rotation_evidence; double mode_reference_yaw { 0.0 }; TimePoint mode_reference_stamp; }; From 5ce7a46c1105c7fcb49dd1d786c0d31178898223 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Wed, 17 Jun 2026 11:40:35 +0800 Subject: [PATCH 16/21] refactor(outpost): improve robot_state readability and reset logic --- .../predictor/outpost/ekf_parameter.hpp | 15 +- src/module/predictor/outpost/robot_state.cpp | 238 ++++++++++-------- 2 files changed, 132 insertions(+), 121 deletions(-) diff --git a/src/module/predictor/outpost/ekf_parameter.hpp b/src/module/predictor/outpost/ekf_parameter.hpp index 0107fe88..0b085f48 100644 --- a/src/module/predictor/outpost/ekf_parameter.hpp +++ b/src/module/predictor/outpost/ekf_parameter.hpp @@ -30,18 +30,13 @@ 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); + 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, trans_z, yaw; + x << center_x, 0.0, center_y, 0.0, obs.xyz[2], yaw; return x; } diff --git a/src/module/predictor/outpost/robot_state.cpp b/src/module/predictor/outpost/robot_state.cpp index 75569c36..a81afe99 100644 --- a/src/module/predictor/outpost/robot_state.cpp +++ b/src/module/predictor/outpost/robot_state.cpp @@ -14,6 +14,8 @@ namespace rmcs::predictor { struct OutpostRobotState::Impl { + using Params = OutpostEKFParameters; + enum class MotionMode { STATIC, CW, CCW }; struct Candidate { @@ -24,13 +26,8 @@ struct OutpostRobotState::Impl { std::optional motion_mode; }; - struct CandidateScore { - Candidate candidate; - double score { std::numeric_limits::infinity() }; - }; - struct MatchResult { - OutpostEKFParameters::ArmorObservation observation; + Params::ArmorObservation observation; Candidate candidate; double reference_yaw { 0.0 }; double score { std::numeric_limits::infinity() }; @@ -62,11 +59,10 @@ struct OutpostRobotState::Impl { assign_slot(0, 0.0, 0.0); last_matched_slot_id = 0; - ekf = EKF { OutpostEKFParameters::x(armor), - OutpostEKFParameters::P_initial_dig().asDiagonal() }; + ekf = EKF { Params::x(Params::observe(armor)), Params::P_initial_dig().asDiagonal() }; initialized = true; - auto const observation = OutpostEKFParameters::observe(armor); + auto const observation = Params::observe(armor); mode_reference_yaw = observation.ypr[0]; mode_reference_stamp = t; } @@ -78,9 +74,8 @@ struct OutpostRobotState::Impl { auto const dt = util::delta_time(t, time_stamp); auto const dt_s = dt.count(); ekf.predict( - OutpostEKFParameters::f(dt_s, angular_velocity()), - [dt_s](EKF::XVec const&) { return OutpostEKFParameters::F(dt_s); }, - OutpostEKFParameters::Q(dt_s)); + Params::f(dt_s, angular_velocity()), + [dt_s](EKF::XVec const&) { return Params::F(dt_s); }, Params::Q(dt_s)); } time_stamp = t; @@ -126,6 +121,7 @@ struct OutpostRobotState::Impl { static constexpr auto kStaticYawDeltaThreshold = util::deg2rad(4.6); static constexpr auto kRotationYawDeltaThreshold = util::deg2rad(14.3); static constexpr int kRotationConfirmCandidateCount = 3; + static constexpr double kHeightMismatchThreshold = kOutpostArmorHeightStep / 2.0; auto assign_slot(int slot_id, double phase_offset, double height_offset) -> void { auto& slot = layout.slots.at(slot_id); @@ -134,20 +130,51 @@ struct OutpostRobotState::Impl { slot.assigned = true; } + auto reset_state(int slot_id, Params::ArmorObservation const& obs) -> void { + ekf = EKF { Params::x(obs), Params::P_initial_dig().asDiagonal() }; + + layout = OutpostArmorLayout {}; + assign_slot(slot_id, 0.0, 0.0); + last_matched_slot_id = slot_id; + + motion_mode = std::nullopt; + rotation_evidence = {}; + update_count = 0; + + mode_reference_yaw = obs.ypr[0]; + mode_reference_stamp = time_stamp; + } + + auto is_layout_consistent() const -> bool { + if (!layout.slots[0].assigned || !layout.slots[1].assigned || !layout.slots[2].assigned) + return true; + + auto height_tier = [](double h) { + auto c = static_cast(std::round(h / kOutpostArmorHeightStep)) % 3; + return (c + 3) % 3; + }; + + auto c0 = height_tier(layout.slots[0].height_offset); + auto c1 = height_tier(layout.slots[1].height_offset); + auto c2 = height_tier(layout.slots[2].height_offset); + + return c0 != c1 && c1 != c2 && c0 != c2; + } + static constexpr auto topology_of(MotionMode mode) -> std::optional { switch (mode) { case MotionMode::CW: return RotationTopology { .angular_velocity = -kOutpostAngularSpeed, .slot_delta = 1, - .phase_delta = OutpostEKFParameters::kPhaseStep, + .phase_delta = Params::kPhaseStep, .height_steps = { -1.0, 2.0 }, }; case MotionMode::CCW: return RotationTopology { .angular_velocity = kOutpostAngularSpeed, .slot_delta = -1, - .phase_delta = -OutpostEKFParameters::kPhaseStep, + .phase_delta = -Params::kPhaseStep, .height_steps = { 1.0, -2.0 }, }; case MotionMode::STATIC: @@ -164,41 +191,26 @@ struct OutpostRobotState::Impl { return topology->angular_velocity; } - static constexpr auto wrap_slot_id(int slot_id) -> int { - constexpr int slot_count = OutpostEKFParameters::kOutpostArmorCount; + static constexpr auto normalize_slot_id(int slot_id) -> int { + constexpr int slot_count = Params::kOutpostArmorCount; return (slot_id + slot_count) % slot_count; } - auto candidate_for_assigned_slot(int slot_id) const -> Candidate { - auto const& slot = layout.slots.at(slot_id); - return { - .slot_id = slot_id, - .phase_offset = slot.phase_offset, - .height_offset = slot.height_offset, - .assigned = true, - .motion_mode = motion_mode, - }; - } - - auto candidate_for_slot(int slot_id, double phase_offset, double height_offset, - MotionMode mode) const -> Candidate { - return Candidate { - .slot_id = slot_id, - .phase_offset = phase_offset, - .height_offset = height_offset, - .assigned = layout.slots.at(slot_id).assigned, - .motion_mode = mode, - }; - } - auto make_candidates() const -> std::vector { auto candidates = std::vector {}; - candidates.reserve(OutpostEKFParameters::kOutpostArmorCount * 3); - - for (int slot_id = 0; slot_id < OutpostEKFParameters::kOutpostArmorCount; ++slot_id) { - if (layout.slots.at(slot_id).assigned) { - candidates.emplace_back(candidate_for_assigned_slot(slot_id)); - } + 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, + }); } if (motion_mode.has_value()) { @@ -218,7 +230,7 @@ struct OutpostRobotState::Impl { if (!topology.has_value()) return; auto const source_slot_id = last_matched_slot_id; - auto const target_slot_id = wrap_slot_id(source_slot_id + topology->slot_delta); + auto const target_slot_id = normalize_slot_id(source_slot_id + topology->slot_delta); auto const& source_slot = layout.slots.at(source_slot_id); auto const& target_slot = layout.slots.at(target_slot_id); @@ -230,95 +242,86 @@ struct OutpostRobotState::Impl { auto const height_offset = source_slot.height_offset + height_step * kOutpostArmorHeightStep; - candidates.emplace_back( - candidate_for_slot(target_slot_id, phase_offset, height_offset, mode)); + candidates.emplace_back(Candidate { + .slot_id = target_slot_id, + .phase_offset = phase_offset, + .height_offset = height_offset, + .assigned = false, + .motion_mode = mode, + }); } } - auto score_candidate(OutpostEKFParameters::ArmorObservation const& observation, - Candidate const& candidate) const -> double { - auto const predicted_xyz = OutpostEKFParameters::h_armor_xyz( - ekf.x, candidate.phase_offset, candidate.height_offset); - auto const predicted_yaw = OutpostEKFParameters::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)); - return position_error + kOutpostRadius * yaw_error; - } - - auto best_candidate(OutpostEKFParameters::ArmorObservation const& observation) const - -> std::optional { + auto select_best_match(std::span armors) const -> std::optional { auto const candidates = make_candidates(); if (candidates.empty()) return std::nullopt; - auto best = CandidateScore {}; - for (auto const& candidate : candidates) { - auto const score = score_candidate(observation, candidate); - if (score >= best.score) continue; - - best = { - .candidate = candidate, - .score = score, - }; - } - - return best; - } - - auto select_best_match(std::span armors) const -> std::optional { auto best_match = std::optional {}; - for (std::size_t observation_index = 0; observation_index < armors.size(); - ++observation_index) { - auto const& armor = armors[observation_index]; - if (armor.genre != DeviceId::OUTPOST) continue; - - auto const observation = OutpostEKFParameters::observe(armor); - auto const candidate_score = best_candidate(observation); - if (!candidate_score.has_value()) continue; - - auto const reference_yaw = - util::normalize_angle(observation.ypr[0] - candidate_score->candidate.phase_offset); - if (best_match.has_value() && candidate_score->score >= best_match->score) continue; - - best_match = MatchResult { - .observation = observation, - .candidate = candidate_score->candidate, - .reference_yaw = reference_yaw, - .score = candidate_score->score, - }; + 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, + }; + } } return best_match; } auto apply_match(MatchResult const& match) -> bool { - if (!motion_mode.has_value() && !match.candidate.assigned - && match.candidate.motion_mode.has_value()) { - update_rotation_evidence(*match.candidate.motion_mode); + auto const& candidate = match.candidate; + + auto const predicted_z = ekf.x[4] + candidate.height_offset; + auto const z_error = std::abs(match.observation.xyz[2] - predicted_z); + if (z_error > kHeightMismatchThreshold) { + reset_state(candidate.slot_id, match.observation); + return true; + } + + if (!motion_mode.has_value() && !candidate.assigned && candidate.motion_mode.has_value()) { + update_rotation_evidence(*candidate.motion_mode); return false; } rotation_evidence = {}; update_motion_mode(match); - auto const& candidate = match.candidate; ekf.update( - OutpostEKFParameters::z(match.observation), + Params::z(match.observation), [candidate](EKF::XVec const& x) { - return OutpostEKFParameters::h(x, candidate.phase_offset, candidate.height_offset); + return Params::h(x, candidate.phase_offset, candidate.height_offset); }, [candidate](EKF::XVec const& x) { - return OutpostEKFParameters::H(x, candidate.phase_offset, candidate.height_offset); + return Params::H(x, candidate.phase_offset, candidate.height_offset); }, - OutpostEKFParameters::R(match.observation), OutpostEKFParameters::x_add, - OutpostEKFParameters::z_subtract); + Params::R(match.observation), Params::x_add, Params::z_subtract); - if (!layout.slots.at(candidate.slot_id).assigned) { - assign_slot(candidate.slot_id, candidate.phase_offset, candidate.height_offset); + assign_slot(candidate.slot_id, candidate.phase_offset, candidate.height_offset); + last_matched_slot_id = candidate.slot_id; + + if (!is_layout_consistent()) { + reset_state(candidate.slot_id, match.observation); } - last_matched_slot_id = candidate.slot_id; return true; } @@ -339,21 +342,34 @@ struct OutpostRobotState::Impl { } auto update_motion_mode(MatchResult const& match) -> void { - if (motion_mode.has_value()) return; - - if (match.candidate.motion_mode.has_value()) { + // 使用拓扑候选直接确认方向 + if (!motion_mode.has_value() && match.candidate.motion_mode.has_value()) { motion_mode = *match.candidate.motion_mode; return; } + // 按时间窗口判断 yaw 变化 auto const elapsed = util::delta_time(time_stamp, mode_reference_stamp); if (elapsed < kModeConfirmWindow) return; auto const mode_delta = util::normalize_angle(match.reference_yaw - mode_reference_yaw); auto const abs_delta = std::abs(mode_delta); - if (abs_delta < kStaticYawDeltaThreshold) motion_mode = MotionMode::STATIC; + + // 根据 yaw 变化推断当前观测到的运动模式 + auto observed_motion_mode = std::optional {}; + if (abs_delta < kStaticYawDeltaThreshold) observed_motion_mode = MotionMode::STATIC; else if (abs_delta > kRotationYawDeltaThreshold) { - motion_mode = mode_delta > 0.0 ? MotionMode::CCW : MotionMode::CW; + observed_motion_mode = mode_delta > 0.0 ? MotionMode::CCW : MotionMode::CW; + } + + // 应用观测结果 + 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 = {}; + } } mode_reference_yaw = match.reference_yaw; From 950551103f1da6373eef43b54ed81a253bbfe076 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 18 Jun 2026 17:10:46 +0800 Subject: [PATCH 17/21] fix outpost tracker reset and cleanup logic --- src/module/predictor/outpost/robot_state.cpp | 31 +++++++---------- src/module/tracker/decider.cpp | 36 ++++++++++---------- 2 files changed, 31 insertions(+), 36 deletions(-) diff --git a/src/module/predictor/outpost/robot_state.cpp b/src/module/predictor/outpost/robot_state.cpp index a81afe99..b0a9513a 100644 --- a/src/module/predictor/outpost/robot_state.cpp +++ b/src/module/predictor/outpost/robot_state.cpp @@ -4,6 +4,7 @@ #include "utility/math/angle.hpp" #include "utility/time.hpp" +#include #include #include #include @@ -59,7 +60,7 @@ struct OutpostRobotState::Impl { assign_slot(0, 0.0, 0.0); last_matched_slot_id = 0; - ekf = EKF { Params::x(Params::observe(armor)), Params::P_initial_dig().asDiagonal() }; + ekf = EKF { Params::x(Params::observe(armor)), Params::P_initial_dig().asDiagonal() }; initialized = true; auto const observation = Params::observe(armor); @@ -118,10 +119,9 @@ struct OutpostRobotState::Impl { private: static constexpr auto kModeConfirmWindow = std::chrono::duration { 0.2 }; - static constexpr auto kStaticYawDeltaThreshold = util::deg2rad(4.6); - static constexpr auto kRotationYawDeltaThreshold = util::deg2rad(14.3); + static constexpr auto kStaticYawDeltaThreshold = util::deg2rad(5.0); + static constexpr auto kRotationYawDeltaThreshold = util::deg2rad(15.0); static constexpr int kRotationConfirmCandidateCount = 3; - static constexpr double kHeightMismatchThreshold = kOutpostArmorHeightStep / 2.0; auto assign_slot(int slot_id, double phase_offset, double height_offset) -> void { auto& slot = layout.slots.at(slot_id); @@ -149,16 +149,18 @@ struct OutpostRobotState::Impl { if (!layout.slots[0].assigned || !layout.slots[1].assigned || !layout.slots[2].assigned) return true; - auto height_tier = [](double h) { - auto c = static_cast(std::round(h / kOutpostArmorHeightStep)) % 3; - return (c + 3) % 3; + auto height_step = [](double height_offset) { + return static_cast(std::round(height_offset / kOutpostArmorHeightStep)); }; - auto c0 = height_tier(layout.slots[0].height_offset); - auto c1 = height_tier(layout.slots[1].height_offset); - auto c2 = height_tier(layout.slots[2].height_offset); + 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); - return c0 != c1 && c1 != c2 && c0 != c2; + auto min_step = std::min({ s0, s1, s2 }); + auto max_step = std::max({ s0, s1, s2 }); + + return max_step - min_step == 2 && s0 != s1 && s1 != s2 && s0 != s2; } static constexpr auto topology_of(MotionMode mode) -> std::optional { @@ -290,13 +292,6 @@ struct OutpostRobotState::Impl { auto apply_match(MatchResult const& match) -> bool { auto const& candidate = match.candidate; - auto const predicted_z = ekf.x[4] + candidate.height_offset; - auto const z_error = std::abs(match.observation.xyz[2] - predicted_z); - if (z_error > kHeightMismatchThreshold) { - reset_state(candidate.slot_id, match.observation); - return true; - } - if (!motion_mode.has_value() && !candidate.assigned && candidate.motion_mode.has_value()) { update_rotation_evidence(*candidate.motion_mode); return false; 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), // 完全相同时按固定顺序兜底，避免容器遍历顺序抖动。 }; }; From 7a7fe5094a7dc9aa2388fd88869ce82a5fff9e29 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Fri, 19 Jun 2026 22:47:38 +0800 Subject: [PATCH 18/21] refactor: simplify outpost slot candidate tracking --- src/module/predictor/outpost/robot_state.cpp | 75 ++++++++------------ 1 file changed, 31 insertions(+), 44 deletions(-) diff --git a/src/module/predictor/outpost/robot_state.cpp b/src/module/predictor/outpost/robot_state.cpp index b0a9513a..319d44a2 100644 --- a/src/module/predictor/outpost/robot_state.cpp +++ b/src/module/predictor/outpost/robot_state.cpp @@ -58,7 +58,6 @@ struct OutpostRobotState::Impl { layout = OutpostArmorLayout {}; assign_slot(0, 0.0, 0.0); - last_matched_slot_id = 0; ekf = EKF { Params::x(Params::observe(armor)), Params::P_initial_dig().asDiagonal() }; initialized = true; @@ -130,21 +129,6 @@ struct OutpostRobotState::Impl { slot.assigned = true; } - auto reset_state(int slot_id, Params::ArmorObservation const& obs) -> void { - ekf = EKF { Params::x(obs), Params::P_initial_dig().asDiagonal() }; - - layout = OutpostArmorLayout {}; - assign_slot(slot_id, 0.0, 0.0); - last_matched_slot_id = slot_id; - - motion_mode = std::nullopt; - rotation_evidence = {}; - update_count = 0; - - mode_reference_yaw = obs.ypr[0]; - mode_reference_stamp = time_stamp; - } - auto is_layout_consistent() const -> bool { if (!layout.slots[0].assigned || !layout.slots[1].assigned || !layout.slots[2].assigned) return true; @@ -231,26 +215,29 @@ struct OutpostRobotState::Impl { auto const topology = topology_of(mode); if (!topology.has_value()) return; - auto const source_slot_id = last_matched_slot_id; - auto const target_slot_id = normalize_slot_id(source_slot_id + topology->slot_delta); - auto const& source_slot = layout.slots.at(source_slot_id); - auto const& target_slot = layout.slots.at(target_slot_id); - - if (target_slot.assigned) return; - - 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, - }); + 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, + }); + } } } @@ -298,6 +285,14 @@ struct OutpostRobotState::Impl { } rotation_evidence = {}; + + assign_slot(candidate.slot_id, candidate.phase_offset, candidate.height_offset); + + if (!is_layout_consistent()) { + initialized = false; + return false; + } + update_motion_mode(match); ekf.update( @@ -310,13 +305,6 @@ struct OutpostRobotState::Impl { }, Params::R(match.observation), Params::x_add, Params::z_subtract); - assign_slot(candidate.slot_id, candidate.phase_offset, candidate.height_offset); - last_matched_slot_id = candidate.slot_id; - - if (!is_layout_consistent()) { - reset_state(candidate.slot_id, match.observation); - } - return true; } @@ -378,7 +366,6 @@ struct OutpostRobotState::Impl { bool initialized { false }; int update_count { 0 }; - int last_matched_slot_id { 0 }; std::optional motion_mode; RotationEvidence rotation_evidence; From c128703129b674d2ae440a6de0589af839877e33 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Fri, 19 Jun 2026 22:59:37 +0800 Subject: [PATCH 19/21] feat(fire_control): add pitch tolerance to shoot evaluator --- config/config.yaml | 6 ++- src/kernel/fire_control.cpp | 3 +- src/module/fire_control/gimbal_state.hpp | 13 +++++ src/module/fire_control/shoot_evaluator.cpp | 59 ++++++++++++++------- src/module/fire_control/shoot_evaluator.hpp | 4 +- src/module/fire_control/types.hpp | 7 +-- 6 files changed, 62 insertions(+), 30 deletions(-) create mode 100644 src/module/fire_control/gimbal_state.hpp diff --git a/config/config.yaml b/config/config.yaml index 45cccda1..e5def4f8 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -116,8 +116,10 @@ fire_control: shoot_evaluator: is_lazy_gimbal: false - near_angle_tolerance: 3 # degree - far_angle_tolerance: 2 # degree + near_yaw_tolerance: 2 # degree + far_yaw_tolerance: 0.7 # degree + near_pitch_tolerance: 2 # degree + far_pitch_tolerance: 1 # degree split_distance: 2 # m visualization: diff --git a/src/kernel/fire_control.cpp b/src/kernel/fire_control.cpp index dfcc5a9a..d12e8bca 100644 --- a/src/kernel/fire_control.cpp +++ b/src/kernel/fire_control.cpp @@ -148,10 +148,11 @@ struct FireControl::Impl { { 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.yaw); + shoot_permitted = shoot_evaluator.evaluate(shoot_command, gimbal_state); } // 6. 组装结果 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/shoot_evaluator.cpp b/src/module/fire_control/shoot_evaluator.cpp index 81cf4dba..5d3bbb78 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 { 4.0 }; + double far_yaw_tolerance { 2.0 }; + double near_pitch_tolerance { 4.0 }; + double far_pitch_tolerance { 2.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 * 2.0) && (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/types.hpp b/src/module/fire_control/types.hpp index bdd36273..d83643df 100644 --- a/src/module/fire_control/types.hpp +++ b/src/module/fire_control/types.hpp @@ -1,5 +1,6 @@ #pragma once +#include "module/fire_control/gimbal_state.hpp" #include "module/predictor/snapshot.hpp" #include "utility/robot/armor.hpp" @@ -11,12 +12,6 @@ struct AimAttitude { double fly_time { 0.0 }; }; -struct GimbalState { - TimePoint timestamp; - double yaw { 0.0 }; - double pitch { 0.0 }; -}; - struct ArmorCandidate { Armor3d armor; predictor::TargetMotion motion; From 7f2582b61e7724095c49d375327e47eb272e4f9c Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Fri, 19 Jun 2026 23:10:14 +0800 Subject: [PATCH 20/21] tune(fire_control): adjust shoot parameters and threshold --- config/config.yaml | 16 ++++++++-------- src/module/fire_control/shoot_evaluator.cpp | 8 ++++---- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/config/config.yaml b/config/config.yaml index e5def4f8..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: @@ -90,16 +90,16 @@ pose_estimator: outpost_armor_thickness: 0.022 fire_control: - initial_bullet_speed: 22.5 # 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 armor_target_selector: - coming_angle: 70.0 # degree + coming_angle: 60.0 # degree leaving_angle: 20.0 # degree - outpost_coming_angle: 70.0 # degree + outpost_coming_angle: 60.0 # degree outpost_leaving_angle: 30.0 # degree switch_threshold: 8.0 # degree @@ -117,7 +117,7 @@ fire_control: shoot_evaluator: is_lazy_gimbal: false near_yaw_tolerance: 2 # degree - far_yaw_tolerance: 0.7 # degree + far_yaw_tolerance: 1 # degree near_pitch_tolerance: 2 # degree far_pitch_tolerance: 1 # degree split_distance: 2 # m diff --git a/src/module/fire_control/shoot_evaluator.cpp b/src/module/fire_control/shoot_evaluator.cpp index 5d3bbb78..326426dd 100644 --- a/src/module/fire_control/shoot_evaluator.cpp +++ b/src/module/fire_control/shoot_evaluator.cpp @@ -10,10 +10,10 @@ using namespace rmcs::fire_control; struct ShootEvaluator::Impl { struct Config : util::Serializable { - double near_yaw_tolerance { 4.0 }; + double near_yaw_tolerance { 3.0 }; double far_yaw_tolerance { 2.0 }; - double near_pitch_tolerance { 4.0 }; - double far_pitch_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 }; @@ -94,7 +94,7 @@ struct ShootEvaluator::Impl { 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 < yaw_tolerance * 2.0) && (track_delta < yaw_tolerance) + should_fire = (yaw_delta < yaw_tolerance) && (track_delta < yaw_tolerance) && (pitch_delta < pitch_tolerance); } From de77add3c4361f28e4ff14475075f635b42855bb Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sat, 20 Jun 2026 00:14:43 +0800 Subject: [PATCH 21/21] fix(outpost): tighten fusion and convergence checks --- src/module/fire_control/armor_selector.cpp | 4 ++-- src/module/predictor/outpost/robot_state.cpp | 5 ++++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/module/fire_control/armor_selector.cpp b/src/module/fire_control/armor_selector.cpp index cdd1f331..71baa20b 100644 --- a/src/module/fire_control/armor_selector.cpp +++ b/src/module/fire_control/armor_selector.cpp @@ -99,7 +99,7 @@ struct ArmorSelector::Impl { } const auto priority_key = [&](size_t index) { - const auto preferred_incoming = [&] { + const auto leaving_penalty = [&] { const auto delta = candidate_evals[index].delta_yaw; 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; @@ -110,7 +110,7 @@ struct ArmorSelector::Impl { 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 {}; diff --git a/src/module/predictor/outpost/robot_state.cpp b/src/module/predictor/outpost/robot_state.cpp index 319d44a2..9a6da539 100644 --- a/src/module/predictor/outpost/robot_state.cpp +++ b/src/module/predictor/outpost/robot_state.cpp @@ -93,13 +93,16 @@ struct OutpostRobotState::Impl { auto match = select_best_match(armors); if (!match.has_value()) return false; - if (apply_match(*match)) ++update_count; + if (!apply_match(*match)) return false; + + ++update_count; return true; } auto is_converged() const -> bool { if (!initialized) return false; if (!std::isfinite(distance())) return false; + if (!motion_mode.has_value()) return false; constexpr int min_updates = 3; return layout.slots[0].assigned && update_count >= min_updates;