From 18859637ba83f2f58341c04e3fbd8a352068ac3f Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sat, 25 Apr 2026 17:37:27 +0800 Subject: [PATCH 01/13] refactor: refactor armor selection logic for better target switching stability --- config/config.yaml | 8 +- src/module/fire_control/aim_point_chooser.cpp | 188 +++++--- test/CMakeLists.txt | 6 + test/aim_point_chooser.cpp | 411 ++++++++++++++++++ 4 files changed, 541 insertions(+), 72 deletions(-) create mode 100644 test/aim_point_chooser.cpp diff --git a/config/config.yaml b/config/config.yaml index a54280c3..a5fc9d77 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -94,10 +94,10 @@ fire_control: yaw_offset: 0.0 # degree pitch_offset: 0.0 # degree - coming_angle: 70.0 # degree - leaving_angle: 20.0 # degree - outpost_coming_angle: 70.0 # degree - outpost_leaving_angle: 30.0 # degree + coming_angle: 55.0 # degree + leaving_angle: 30.0 # degree + outpost_coming_angle: 50.0 # degree + outpost_leaving_angle: 30.0 # degree angular_velocity_threshold: 120 # degree/s first_tolerance: 3 # 近距离射击容差，degree diff --git a/src/module/fire_control/aim_point_chooser.cpp b/src/module/fire_control/aim_point_chooser.cpp index 62b13c8f..a1728776 100644 --- a/src/module/fire_control/aim_point_chooser.cpp +++ b/src/module/fire_control/aim_point_chooser.cpp @@ -1,5 +1,7 @@ #include "aim_point_chooser.hpp" +#include +#include #include #include "utility/math/conversion.hpp" @@ -7,23 +9,114 @@ using namespace rmcs::fire_control; struct AimPointChooser::Impl { + static constexpr int kNoArmor = -1; - double coming_angle { 60 / 57.3 }; // rad - double leaving_angle { 20 / 57.3 }; // rad + struct ArmorCandidate { + int index { kNoArmor }; + double delta_yaw { 0.0 }; + double phase { 0.0 }; + }; - double outpost_coming_angle { 70 / 57.3 }; // rad - double outpost_leaving_angle { 30 / 57.3 }; // rad + struct AngleWindow { + double coming { 0.0 }; + double leaving { 0.0 }; + }; + + AngleWindow normal_window { util::deg2rad(60.0), util::deg2rad(20.0) }; // rad + AngleWindow outpost_window { util::deg2rad(70.0), util::deg2rad(30.0) }; // rad + const double min_switch_improvement_angle { util::deg2rad(6.0) }; double angular_velocity_threshold { 2 }; // rad/s - int last_chosen_id { -1 }; + int last_chosen_id { kNoArmor }; - auto initialize(Config const& config) noexcept -> void { - coming_angle = config.coming_angle; - leaving_angle = config.leaving_angle; + auto should_switch_target(double current_abs_error, double candidate_abs_error) const noexcept + -> bool { + return candidate_abs_error + min_switch_improvement_angle < current_abs_error; + } + + static auto abs_error(ArmorCandidate const& candidate) noexcept -> double { + return std::abs(candidate.delta_yaw); + } + + auto angle_window(DeviceId genre) const noexcept -> AngleWindow const& { + return genre == DeviceId::OUTPOST ? outpost_window : normal_window; + } + + auto choose_low_speed( + std::span candidates, AngleWindow const& window) const -> int { + auto const in_window = [&](ArmorCandidate const& candidate) { + return abs_error(candidate) < window.coming; + }; + + auto best_in_window = std::optional {}; + for (auto const& candidate : candidates) { + if (!in_window(candidate)) continue; + if (!best_in_window.has_value() || abs_error(candidate) < abs_error(*best_in_window)) { + best_in_window = candidate; + } + } + + if (!best_in_window.has_value()) return kNoArmor; + + auto const has_last_candidate = + (last_chosen_id >= 0) && (static_cast(last_chosen_id) < candidates.size()); + if (!has_last_candidate) return best_in_window->index; - outpost_coming_angle = config.outpost_coming_angle; - outpost_leaving_angle = config.outpost_leaving_angle; + auto const& last_candidate = candidates[static_cast(last_chosen_id)]; + if (!in_window(last_candidate)) return best_in_window->index; + if (best_in_window->index == last_chosen_id) return last_chosen_id; + if (!should_switch_target(abs_error(last_candidate), abs_error(*best_in_window))) + return last_chosen_id; + + return best_in_window->index; + } + + auto choose_high_speed( + std::span candidates, AngleWindow const& window) const -> int { + auto const is_in_window = [&](ArmorCandidate const& candidate) { + return abs_error(candidate) < window.coming && candidate.phase <= window.leaving; + }; + + auto const is_incoming = [](ArmorCandidate const& candidate) { + return candidate.phase < 0.0; + }; + + auto const is_better_than = [&](ArmorCandidate const& candidate, + ArmorCandidate const& current_best) { + auto const candidate_incoming = is_incoming(candidate); + auto const best_incoming = is_incoming(current_best); + if (candidate_incoming != best_incoming) return candidate_incoming; + + return abs_error(candidate) < abs_error(current_best); + }; + + auto preferred = std::optional {}; + for (auto const& candidate : candidates) { + if (!is_in_window(candidate)) continue; + + if (!preferred.has_value() || is_better_than(candidate, *preferred)) { + preferred = candidate; + } + } + + if (!preferred) return kNoArmor; + + auto const has_last = + (last_chosen_id >= 0) && (static_cast(last_chosen_id) < candidates.size()); + if (!has_last) return preferred->index; + + auto const& last = candidates[static_cast(last_chosen_id)]; + if (!is_in_window(last)) return preferred->index; + if (preferred->index == last_chosen_id) return last_chosen_id; + + return should_switch_target(abs_error(last), abs_error(*preferred)) ? preferred->index + : last_chosen_id; + } + + auto initialize(Config const& config) noexcept -> void { + normal_window = { config.coming_angle, config.leaving_angle }; + outpost_window = { config.outpost_coming_angle, config.outpost_leaving_angle }; angular_velocity_threshold = config.angular_velocity_threshold; } @@ -31,20 +124,13 @@ struct AimPointChooser::Impl { auto choose_armor(std::span armors, Eigen::Vector3d const& center_position, double angular_velocity) -> std::optional { if (armors.empty()) { - last_chosen_id = -1; + last_chosen_id = kNoArmor; return std::nullopt; } const auto center_yaw = std::atan2(center_position.y(), center_position.x()); - - struct ArmorCandidate { - int index; - double delta_yaw; - double score; // 分数越低越好 - }; - - auto candidates = std::array {}; - const auto n = std::min(armors.size(), candidates.size()); + auto candidates = std::array {}; + const auto n = std::min(armors.size(), candidates.size()); for (size_t id = 0; id < n; ++id) { auto orientation = Eigen::Quaterniond {}; @@ -52,69 +138,35 @@ struct AimPointChooser::Impl { const auto ypr = util::eulers(orientation); const auto yaw_in_world = ypr[0]; + const auto delta_yaw = util::normalize_angle(yaw_in_world - center_yaw); + const auto phase = (angular_velocity > 0.0) ? delta_yaw + : (angular_velocity < 0.0) ? -delta_yaw + : 0.0; - candidates.at(id) = { static_cast(id), - util::normalize_angle(yaw_in_world - center_yaw), 0. }; + candidates[id] = { static_cast(id), delta_yaw, phase }; } - auto chosen_id = int { -1 }; + auto chosen_id = kNoArmor; + auto candidate_view = std::span { candidates }.first(n); + auto const genre = armors.front().genre; + auto const& window = angle_window(genre); // --- 非小陀螺模式 (低速旋转) --- if ((std::abs(angular_velocity) < angular_velocity_threshold) - && (armors.front().genre != DeviceId::OUTPOST)) { - for (auto& candidate : candidates) { - candidate.score = std::abs(candidate.delta_yaw); - - if (candidate.index == last_chosen_id) - candidate.score -= util::deg2rad(8); // 约 8 度的优先权，防止微小跳变导致换板 - } - - auto valid_it = std::ranges::min_element( - candidates | std::views::take(n), {}, [](auto const& candidate) { - return (std::abs(candidate.delta_yaw) > util::deg2rad(90)) ? 1e5 - : candidate.score; - }); - if (std::abs(valid_it->delta_yaw) < util::deg2rad(90)) chosen_id = valid_it->index; + && (genre != DeviceId::OUTPOST)) { + chosen_id = choose_low_speed(candidate_view, window); } // --- 小陀螺模式 (快速旋转) --- else { - auto genre = armors.front().genre; - auto _coming_angle = (genre == DeviceId::OUTPOST) ? outpost_coming_angle : coming_angle; - auto _leaving_angle = - (genre == DeviceId::OUTPOST) ? outpost_leaving_angle : leaving_angle; - - for (auto& candidate : candidates) { - candidate.score = std::abs(candidate.delta_yaw); - // 判断旋转方向，给予“顺势”补偿 - // 如果 omega > 0 (逆时针)，板从右侧 (delta > 0) 进入。 - // 我们给正处于“迎面而来”位置的板减分 - bool is_incoming = (angular_velocity > 0 && candidate.delta_yaw > 0) - || (angular_velocity < 0 && candidate.delta_yaw < 0); - - if (is_incoming) candidate.score -= 0.2; - - if (candidate.index == last_chosen_id) { - candidate.score -= util::deg2rad(17); // 约 17 度的优先权 - } - // 剔除已经快要转没的板 (Leaving Angle) - if ((angular_velocity > 0 && candidate.delta_yaw < -_leaving_angle) - || (angular_velocity < 0 && candidate.delta_yaw > _leaving_angle)) { - candidate.score += 10.0; - } - } - - auto it = std::ranges::min_element( - candidates | std::views::take(n), std::ranges::less {}, &ArmorCandidate::score); - - if (std::abs(it->delta_yaw) < _coming_angle) chosen_id = it->index; + chosen_id = choose_high_speed(candidate_view, window); } - if (chosen_id != -1) { + if (chosen_id != kNoArmor) { last_chosen_id = chosen_id; return { armors[chosen_id] }; } - last_chosen_id = -1; + last_chosen_id = kNoArmor; return std::nullopt; } }; diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index e114ac3e..dfb92d46 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -121,3 +121,9 @@ ament_add_gtest( ${TEST_DIR}/feishu_test.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 +) diff --git a/test/aim_point_chooser.cpp b/test/aim_point_chooser.cpp new file mode 100644 index 00000000..391e12b7 --- /dev/null +++ b/test/aim_point_chooser.cpp @@ -0,0 +1,411 @@ +#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 kLowSpeed = 1.0; +constexpr auto kFastPositive = 4.0; +constexpr auto kFastNegative = -4.0; +constexpr auto kSwitchSpeedEps = 0.01; + +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 make_armors(std::span yaws_deg, + DeviceId genre = DeviceId::SENTRY) -> std::vector { + auto armors = std::vector {}; + armors.reserve(yaws_deg.size()); + + for (size_t i = 0; i < yaws_deg.size(); ++i) { + armors.emplace_back(make_armor(yaws_deg[i], static_cast(i), genre)); + } + 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); +} + +auto expect_single_armor_result(double angle_deg, double angular_velocity, DeviceId genre, + std::optional expected) -> void { + auto const armors = make_armors({ angle_deg }, genre); + auto const actual = choose_once(armors, 0.0, angular_velocity); + EXPECT_EQ(actual, expected); +} + +} // namespace + +TEST(AimPointChooser, ScenarioTemplateSingleArmorLowSpeedAcquireScan) { + for (int angle_deg = -180; angle_deg <= 180; angle_deg += kAngleStepDeg) { + auto const armors = make_armors({ static_cast(angle_deg) }); + auto const actual = choose_once(armors, 0.0, kLowSpeed); + + auto const expected = (std::abs(angle_deg) < 60) ? std::optional { 0 } : std::nullopt; + + SCOPED_TRACE("angle_deg=" + std::to_string(angle_deg)); + EXPECT_EQ(actual, expected); + } +} + +TEST(AimPointChooser, ScenarioTemplateSingleArmorHighSpeedScanBySpinDirection) { + constexpr std::array speeds { 2.0, kFastPositive, -2.0, kFastNegative }; + + for (int angle_deg = -180; angle_deg <= 180; angle_deg += kAngleStepDeg) { + auto const armors = make_armors({ static_cast(angle_deg) }); + + for (auto const speed : speeds) { + 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, ScenarioTemplateSingleArmorOutpostScan) { + constexpr std::array speeds { 0.0, 1.0, -1.0, 1.99, -1.99 }; + + for (int angle_deg = -180; angle_deg <= 180; angle_deg += kAngleStepDeg) { + for (auto const speed : speeds) { + auto const in_coming_window = std::abs(angle_deg) < 70; + + auto in_leaving_window = false; + if (speed == 0.0) { + in_leaving_window = true; + } else if (speed > 0.0) { + in_leaving_window = angle_deg <= 30; + } else { + in_leaving_window = angle_deg >= -30; + } + + auto const expected = (in_coming_window && in_leaving_window) + ? std::optional { 0 } + : std::nullopt; + + auto const armors = make_armors({ static_cast(angle_deg) }, DeviceId::OUTPOST); + 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, BoundaryCasesCoverWindowsAndVelocityThreshold) { + struct TestCase { + std::string name; + double angle_deg; + double speed; + DeviceId genre; + std::optional expected; + }; + + constexpr auto kDegEps = 0.001; + + auto const cases = std::array { + TestCase { "low_speed_acquire_inside", 60.0 - kDegEps, kLowSpeed, DeviceId::SENTRY, 0 }, + TestCase { "low_speed_acquire_boundary", 60.0, kLowSpeed, DeviceId::SENTRY, std::nullopt }, + TestCase { "low_speed_acquire_outside", 60.0 + kDegEps, kLowSpeed, DeviceId::SENTRY, + std::nullopt }, + + TestCase { "high_positive_leaving_boundary", 20.0, kFastPositive, DeviceId::SENTRY, 0 }, + TestCase { "high_positive_leaving_outside", 20.0 + kDegEps, kFastPositive, + DeviceId::SENTRY, std::nullopt }, + TestCase { "high_positive_coming_inside", -60.0 + kDegEps, kFastPositive, + DeviceId::SENTRY, 0 }, + TestCase { "high_positive_coming_boundary", -60.0, kFastPositive, DeviceId::SENTRY, + std::nullopt }, + + TestCase { "high_negative_leaving_boundary", -20.0, kFastNegative, DeviceId::SENTRY, 0 }, + TestCase { "high_negative_leaving_outside", -20.0 - kDegEps, kFastNegative, + DeviceId::SENTRY, std::nullopt }, + TestCase { "high_negative_coming_inside", 60.0 - kDegEps, kFastNegative, DeviceId::SENTRY, + 0 }, + TestCase { "high_negative_coming_boundary", 60.0, kFastNegative, DeviceId::SENTRY, + std::nullopt }, + + TestCase { "outpost_positive_leaving_boundary", 30.0, 1.0, DeviceId::OUTPOST, 0 }, + TestCase { "outpost_positive_leaving_outside", 30.0 + kDegEps, 1.0, DeviceId::OUTPOST, + std::nullopt }, + TestCase { "outpost_positive_coming_inside", -70.0 + kDegEps, 1.0, DeviceId::OUTPOST, 0 }, + TestCase { "outpost_positive_coming_boundary", -70.0, 1.0, DeviceId::OUTPOST, + std::nullopt }, + + TestCase { "velocity_threshold_below", 50.0, 2.0 - kSwitchSpeedEps, DeviceId::SENTRY, 0 }, + TestCase { "velocity_threshold_equal", 50.0, 2.0, DeviceId::SENTRY, std::nullopt }, + TestCase { "velocity_threshold_above", 50.0, 2.0 + kSwitchSpeedEps, DeviceId::SENTRY, + std::nullopt }, + TestCase { + "velocity_threshold_negative_below", -50.0, -2.0 + kSwitchSpeedEps, DeviceId::SENTRY, 0 }, + TestCase { "velocity_threshold_negative_equal", -50.0, -2.0, DeviceId::SENTRY, + std::nullopt }, + TestCase { "velocity_threshold_negative_above", -50.0, -2.0 - kSwitchSpeedEps, + DeviceId::SENTRY, std::nullopt }, + }; + + for (auto const& test_case : cases) { + SCOPED_TRACE(test_case.name); + expect_single_armor_result( + test_case.angle_deg, test_case.speed, test_case.genre, test_case.expected); + } +} + +TEST(AimPointChooser, SymmetricDualArmorHighSpeedFollowsIncomingDirection) { + for (int theta_deg = 5; theta_deg <= 55; theta_deg += 5) { + auto const armors = make_armors({ -static_cast(theta_deg), static_cast(theta_deg) }); + + auto const positive_spin = choose_once(armors, 0.0, kFastPositive); + auto const negative_spin = choose_once(armors, 0.0, kFastNegative); + + SCOPED_TRACE("theta_deg=" + std::to_string(theta_deg)); + EXPECT_EQ(positive_spin, std::optional { 0 }); + EXPECT_EQ(negative_spin, std::optional { 1 }); + } +} + +TEST(AimPointChooser, SymmetricLowSpeedSwitchUsesMarginAndImmediateSwitch) { + auto chooser = make_chooser(); + + auto const frame_initial = make_armors({ -30.0, 30.0 }); + EXPECT_EQ(choose_id(*chooser, frame_initial, 0.0, kLowSpeed), std::optional { 0 }); + + auto const frame_margin = make_armors({ -30.0, 25.0 }); // improvement = 5 deg + EXPECT_EQ(choose_id(*chooser, frame_margin, 0.0, kLowSpeed), std::optional { 0 }); + + auto const frame_switch = make_armors({ -30.0, 22.0 }); // improvement = 8 deg + EXPECT_EQ(choose_id(*chooser, frame_switch, 0.0, kLowSpeed), std::optional { 1 }); +} + +TEST(AimPointChooser, LowSpeedSwitchesImmediatelyWhenPreferredTargetChanges) { + auto chooser = make_chooser(); + + auto const frame_initial = make_armors({ 10.0, 30.0, 40.0 }); + EXPECT_EQ(choose_id(*chooser, frame_initial, 0.0, kLowSpeed), std::optional { 0 }); + + auto const frame_target_2 = make_armors({ 20.0, 15.0, 8.0 }); + EXPECT_EQ(choose_id(*chooser, frame_target_2, 0.0, kLowSpeed), std::optional { 2 }); + + auto const frame_target_1 = make_armors({ 20.0, 8.0, 15.0 }); + EXPECT_EQ(choose_id(*chooser, frame_target_1, 0.0, kLowSpeed), std::optional { 1 }); +} + +TEST(AimPointChooser, HighSpeedSwitchesImmediatelyWhenImprovementExceedsMargin) { + auto chooser = make_chooser(); + + auto const frame_initial = make_armors({ 12.0, 18.0, 25.0 }); + EXPECT_EQ(choose_id(*chooser, frame_initial, 0.0, kFastPositive), std::optional { 0 }); + + auto const frame_switch = make_armors({ 20.0, 12.0, 25.0 }); // abs improvement = 8 deg + EXPECT_EQ(choose_id(*chooser, frame_switch, 0.0, kFastPositive), std::optional { 1 }); +} + +TEST(AimPointChooser, EmptyInputClearsStateAndAllowsImmediateReacquire) { + auto chooser = make_chooser(); + + auto const frame_a = make_armors({ 10.0, 20.0 }); + auto const frame_b = make_armors({ 20.0, 10.0 }); + auto const frame_no_acquire = make_armors({ 70.0, 65.0 }); + auto const empty = std::vector {}; + + EXPECT_EQ(choose_id(*chooser, frame_a, 0.0, kLowSpeed), std::optional { 0 }); + EXPECT_EQ(choose_id(*chooser, frame_b, 0.0, kLowSpeed), std::optional { 1 }); + EXPECT_EQ(choose_id(*chooser, frame_no_acquire, 0.0, kLowSpeed), std::nullopt); + + EXPECT_EQ(choose_id(*chooser, empty, 0.0, kLowSpeed), std::nullopt); + EXPECT_EQ(choose_id(*chooser, frame_no_acquire, 0.0, kLowSpeed), std::nullopt); +} + +TEST(AimPointChooser, YawWraparoundAndGlobalShiftAreInvariant) { + constexpr std::array speeds { kLowSpeed, kFastPositive, kFastNegative }; + + for (int angle_deg = -175; angle_deg <= 175; angle_deg += 35) { + for (auto const speed : speeds) { + auto const base_armors = make_armors({ static_cast(angle_deg) }); + auto const plus_armors = make_armors({ static_cast(angle_deg + 360) }); + auto const minus_armors = make_armors({ static_cast(angle_deg - 360) }); + + auto const base_result = choose_once(base_armors, 0.0, speed); + auto const plus_result = choose_once(plus_armors, 0.0, speed); + auto const minus_result = choose_once(minus_armors, 0.0, speed); + + SCOPED_TRACE("angle_deg=" + std::to_string(angle_deg)); + SCOPED_TRACE("speed=" + std::to_string(speed)); + + EXPECT_EQ(base_result, plus_result); + EXPECT_EQ(base_result, minus_result); + } + } + + constexpr std::array template_yaws { -35.0, 15.0, 80.0 }; + constexpr std::array genres { DeviceId::SENTRY, DeviceId::OUTPOST }; + + for (auto const genre : genres) { + for (auto const speed : speeds) { + auto const baseline_armors = make_armors(template_yaws, genre); + auto const baseline_result = choose_once(baseline_armors, 0.0, speed); + + for (int shift_deg = -150; shift_deg <= 150; shift_deg += 30) { + auto shifted_yaws = std::vector {}; + shifted_yaws.reserve(template_yaws.size()); + for (auto const yaw_deg : template_yaws) { + shifted_yaws.push_back(yaw_deg + static_cast(shift_deg)); + } + + auto const shifted_armors = make_armors(shifted_yaws, genre); + auto const shifted_result = + choose_once(shifted_armors, static_cast(shift_deg), speed); + + SCOPED_TRACE("genre=" + std::to_string(static_cast(genre))); + SCOPED_TRACE("speed=" + std::to_string(speed)); + SCOPED_TRACE("shift_deg=" + std::to_string(shift_deg)); + + EXPECT_EQ(shifted_result, baseline_result); + } + } + } +} + +TEST(AimPointChooser, RandomizedScenarioRegressionIsDeterministicAndValid) { + struct Frame { + std::vector armors; + double center_yaw_deg; + double angular_velocity; + }; + + auto rng = std::mt19937 { 0xA11C0DEu }; + + auto count_dist = std::uniform_int_distribution { 0, 4 }; + auto yaw_dist = std::uniform_real_distribution { -720.0, 720.0 }; + auto center_yaw_dist = std::uniform_real_distribution { -180.0, 180.0 }; + auto velocity_dist = std::uniform_real_distribution { -8.0, 8.0 }; + auto outpost_selector = std::bernoulli_distribution { 0.25 }; + + auto frames = std::vector {}; + frames.reserve(2000); + + for (int i = 0; i < 2000; ++i) { + auto const count = count_dist(rng); + auto const genre = outpost_selector(rng) ? DeviceId::OUTPOST : DeviceId::SENTRY; + + auto armors = std::vector {}; + armors.reserve(static_cast(count)); + + for (int id = 0; id < count; ++id) { + armors.emplace_back(make_armor(yaw_dist(rng), id, genre)); + } + + frames.emplace_back(Frame { + .armors = std::move(armors), + .center_yaw_deg = center_yaw_dist(rng), + .angular_velocity = velocity_dist(rng), + }); + } + + auto chooser_a = make_chooser(); + auto chooser_b = make_chooser(); + + for (size_t i = 0; i < frames.size(); ++i) { + auto const& frame = frames[i]; + + auto const result_a = choose_id( + *chooser_a, frame.armors, frame.center_yaw_deg, frame.angular_velocity); + auto const result_b = choose_id( + *chooser_b, frame.armors, frame.center_yaw_deg, frame.angular_velocity); + + SCOPED_TRACE("frame_index=" + std::to_string(i)); + EXPECT_EQ(result_a, result_b); + + if (frame.armors.empty()) { + EXPECT_EQ(result_a, std::nullopt); + } + + if (result_a.has_value()) { + EXPECT_GE(*result_a, 0); + EXPECT_LT(*result_a, static_cast(frame.armors.size())); + } + } +} From 2f833528a4f745be13109c15675a948153be76d9 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sun, 26 Apr 2026 11:27:45 +0800 Subject: [PATCH 02/13] refactor: optimize armor selection logic without cost function evaluation --- config/config.yaml | 8 +-- src/module/tracker/decider.cpp | 81 +++++++++++++------------- src/utility/math/kalman_filter/ekf.hpp | 3 +- src/utility/math/mahalanobis.hpp | 5 +- 4 files changed, 50 insertions(+), 47 deletions(-) diff --git a/config/config.yaml b/config/config.yaml index a5fc9d77..7f7ab9ac 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -19,9 +19,9 @@ capturer: gain: 16.9807 invert_image: false - software_sync: false + software_sync: true trigger_mode: false - fixed_framerate: false + fixed_framerate: true local_video: # 替换为你具体的路径 location: "/workspaces/alliance/test_videos/outpost.mp4" @@ -89,7 +89,7 @@ pose_estimator: q: [1., 0., 0., 0.] fire_control: - initial_bullet_speed: 26.6 # m/s + initial_bullet_speed: 21.0 # m/s shoot_delay: 0.1 # s yaw_offset: 0.0 # degree pitch_offset: 0.0 # degree @@ -97,7 +97,7 @@ fire_control: coming_angle: 55.0 # degree leaving_angle: 30.0 # degree outpost_coming_angle: 50.0 # degree - outpost_leaving_angle: 30.0 # degree + outpost_leaving_angle: 40.0 # degree angular_velocity_threshold: 120 # degree/s first_tolerance: 3 # 近距离射击容差，degree diff --git a/src/module/tracker/decider.cpp b/src/module/tracker/decider.cpp index d1daca5c..fd441c24 100644 --- a/src/module/tracker/decider.cpp +++ b/src/module/tracker/decider.cpp @@ -1,8 +1,8 @@ #include "decider.hpp" -#include #include #include +#include #include #include #include @@ -16,13 +16,8 @@ using namespace rmcs::predictor; using namespace std::chrono_literals; struct Decider::Impl { - static constexpr auto kDefaultCleanupInterval = 1s; - static constexpr auto kOutpostCleanupInterval = 1.5s; - static constexpr double kPriorityScoreBase = 10.0; - static constexpr double kDistanceScoreWeight = 5.0; - static constexpr double kDistanceScoreBias = 1.0; - static constexpr double kConvergedScoreBonus = 4.0; - static constexpr double kPrimaryTargetScoreBonus = 2.0; + static constexpr auto kDefaultCleanupInterval = 1s; + static constexpr auto kOutpostCleanupInterval = 1.5s; struct TargetMemory { std::optional last_seen_time { }; @@ -62,7 +57,9 @@ struct Decider::Impl { return std::unexpected { "tracker.tracking_confirm_frames must be > 0" }; } - return { }; + if (priority_mode.empty()) priority_mode = mode2; + + return {}; } auto set_priority_mode(PriorityMode const& mode) -> void { priority_mode = mode; } @@ -166,16 +163,18 @@ struct Decider::Impl { } auto arbitrate(const std::unordered_set& observed_ids) -> DeviceId { - auto candidates = trackers | std::views::filter([&](const auto& pair) { - return observed_ids.contains(pair.first); - }); + auto best_target_id = DeviceId::UNKNOWN; - if (std::ranges::empty(candidates)) return DeviceId::UNKNOWN; + for (const auto& [device_id, _] : trackers) { + if (!observed_ids.contains(device_id)) continue; - auto it = std::ranges::max_element(candidates, { }, - [&](const auto& pair) { return calculate_score(pair.first, *pair.second); }); + if (best_target_id == DeviceId::UNKNOWN + || is_better_target(device_id, best_target_id)) { + best_target_id = device_id; + } + } - return it->first; + return best_target_id; } auto tracking_confirmed(DeviceId device_id) const -> bool { @@ -229,28 +228,30 @@ struct Decider::Impl { return memory_it->second.consecutive_missing_frames <= max_missing_frames; } - // TODO:需要进一步确定 - // 评分函数：结合优先级模式、距离、收敛情况 - auto calculate_score(DeviceId device, RobotState const& tracker) const -> double { - double score = 0.0; - - // 基础优先级评分 - if (priority_mode.contains(device)) { - // RobotPriority 枚举值越小，优先级越高 - score += (kPriorityScoreBase - static_cast(priority_mode.at(device))); + auto priority_of(DeviceId device_id) const -> int { + if (auto it = priority_mode.find(device_id); it != priority_mode.end()) { + return it->second; } + return std::numeric_limits::max(); + } - // 距离加权：优先锁定近处的目标 (简单的 1/dist) - double dist = tracker.distance(); - score += kDistanceScoreWeight / (dist + kDistanceScoreBias); - - // 优先延续已经收敛的目标，避免频繁切到未收敛目标导致停留 Detecting。 - if (tracker.is_converged()) score += kConvergedScoreBonus; - - // 粘滞性：如果已经是主目标，额外加分防止“摇头” - if (device == primary_target_id) score += kPrimaryTargetScoreBonus; + auto is_better_target(DeviceId lhs, DeviceId rhs) const -> bool { + auto rank = [&](DeviceId device_id) { + auto const& tracker = *trackers.at(device_id); + auto distance = tracker.distance(); + auto safe_distance = + std::isfinite(distance) ? distance : std::numeric_limits::infinity(); + + // 比较顺序：优先级 -> 收敛状态 -> 距离 -> 固定 ID 兜底。 + return std::tuple { + priority_of(device_id), // 数值越小，优先级越高。 + !tracker.is_converged(), // 收敛目标映射为 0，未收敛目标映射为 1。 + safe_distance, // 非有限距离按无穷远处理，避免 NaN/Inf 干扰排序。 + rmcs::to_index(device_id), // 完全相同时按固定顺序兜底，避免容器遍历顺序抖动。 + }; + }; - return score; + return rank(lhs) < rank(rhs); } DeviceId primary_target_id { DeviceId::UNKNOWN }; @@ -265,9 +266,9 @@ struct Decider::Impl { { DeviceId::ENGINEER, 4 }, { DeviceId::INFANTRY_3, 1 }, { DeviceId::INFANTRY_4, 1 }, - { DeviceId::INFANTRY_5, 3 }, + { DeviceId::INFANTRY_5, 5 }, { DeviceId::SENTRY, 3 }, - { DeviceId::OUTPOST, 5 }, + { DeviceId::OUTPOST, 2 }, { DeviceId::BASE, 5 }, { DeviceId::UNKNOWN, 5 }, }; @@ -276,10 +277,10 @@ struct Decider::Impl { { DeviceId::HERO, 1 }, { DeviceId::ENGINEER, 2 }, { DeviceId::INFANTRY_3, 1 }, - { DeviceId::INFANTRY_4, 2 }, - { DeviceId::INFANTRY_5, 3 }, + { DeviceId::INFANTRY_4, 1 }, + { DeviceId::INFANTRY_5, 5 }, { DeviceId::SENTRY, 3 }, - { DeviceId::OUTPOST, 5 }, + { DeviceId::OUTPOST, 1 }, { DeviceId::BASE, 5 }, { DeviceId::UNKNOWN, 5 }, }; diff --git a/src/utility/math/kalman_filter/ekf.hpp b/src/utility/math/kalman_filter/ekf.hpp index 037b0864..3fc137e4 100644 --- a/src/utility/math/kalman_filter/ekf.hpp +++ b/src/utility/math/kalman_filter/ekf.hpp @@ -133,7 +133,8 @@ class EKF { // --- 3. 计算最优卡尔曼增益 (Optimal Kalman Gain) --- // K_k = P_{k|k-1} * H_k^T * S_k^-1 // 使用高效的 LDLT 分解求解线性方程组 S*K^T = H*P - auto K = P_ * H.transpose() * S.ldlt().solve(RMat::Identity()); + auto const ldlt = S.ldlt(); + auto K = P_ * H.transpose() * ldlt.solve(RMat::Identity()); // --- 4. 状态后验更新 (State Update) --- // x_{k|k} = x_{k|k-1} + K_k * y_k diff --git a/src/utility/math/mahalanobis.hpp b/src/utility/math/mahalanobis.hpp index 4c88510a..ab76f396 100644 --- a/src/utility/math/mahalanobis.hpp +++ b/src/utility/math/mahalanobis.hpp @@ -10,8 +10,9 @@ namespace rmcs::util { template inline auto mahalanobis_distance(TVec const& innovation, TMat const& covariance) -> std::optional { - auto solved = covariance.ldlt().solve(innovation); - auto distance = innovation.dot(solved); + auto const ldlt = covariance.ldlt(); + auto solved = ldlt.solve(innovation); + auto distance = innovation.dot(solved); if (!std::isfinite(distance)) return std::nullopt; return distance; } From 07c68b9917c04f932a760409bf86aef4e60c4279 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sun, 26 Apr 2026 16:49:12 +0800 Subject: [PATCH 03/13] wip: sentry adapter --- config/config.yaml | 6 +- src/adapter/adapter.hpp | 0 src/adapter/sentry.hpp | 31 +++++++++ src/component.cpp | 150 +++++++++++++++------------------------- src/runtime.cpp | 2 + 5 files changed, 92 insertions(+), 97 deletions(-) create mode 100644 src/adapter/adapter.hpp create mode 100644 src/adapter/sentry.hpp diff --git a/config/config.yaml b/config/config.yaml index a54280c3..4e18c955 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -1,6 +1,6 @@ use_visualization: true use_painted_image: true -is_local_runtime: true +is_local_runtime: false capturer: show_loss_framerate: false @@ -18,7 +18,7 @@ capturer: # float gain: 16.9807 - invert_image: false + invert_image: true software_sync: false trigger_mode: false fixed_framerate: false @@ -107,6 +107,6 @@ fire_control: visualization: framerate: 60 - monitor_host: "127.0.0.1" + monitor_host: "192.168.3.125" monitor_port: "5000" stream_type: "RTP_JEPG" diff --git a/src/adapter/adapter.hpp b/src/adapter/adapter.hpp new file mode 100644 index 00000000..e69de29b diff --git a/src/adapter/sentry.hpp b/src/adapter/sentry.hpp new file mode 100644 index 00000000..f1ad19f6 --- /dev/null +++ b/src/adapter/sentry.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include +#include +#include + +namespace rmcs { + +class Adapter { +public: + static constexpr const char* kParentFrame = "odom_imu_link"; + + explicit Adapter(rmcs_executor::Component& component) { component.register_input("/tf", tf_); } + + [[nodiscard]] auto ready() const -> bool { return tf_.ready(); } + + [[nodiscard]] auto camera_transform() const -> Eigen::Isometry3d { + return fast_tf::lookup_transform(*tf_); + } + + [[nodiscard]] auto barrel_direction() const -> Eigen::Vector3d { + return *fast_tf::cast( + rmcs_description::PitchLink::DirectionVector { Eigen::Vector3d::UnitX() }, *tf_); + } + +private: + rmcs_executor::Component::InputInterface tf_; +}; + +} // namespace rmcs::adapter diff --git a/src/component.cpp b/src/component.cpp index a1574386..19aed128 100644 --- a/src/component.cpp +++ b/src/component.cpp @@ -1,3 +1,4 @@ +#include "adapter/sentry.hpp" #include "kernel/feishu.hpp" #include "module/debug/action_throttler.hpp" #include "module/debug/framerate.hpp" @@ -9,20 +10,18 @@ #include #include -#include #include namespace rmcs { -using namespace rmcs::util; +using namespace util; using namespace kernel; class AutoAimComponent final : public rmcs_executor::Component { public: explicit AutoAimComponent() noexcept - : rclcpp { get_component_name() } { - - register_input("/tf", rmcs_tf); + : adapter { *this } + , rclcpp { get_component_name() } { register_output("/gimbal/auto_aim/auto_aim_enabled", gimbal_takeover, false); register_output( @@ -33,10 +32,10 @@ class AutoAimComponent final : public rmcs_executor::Component { framerate.set_interval(2s); const auto config = visual::Transform::Config { - .rclcpp = rclcpp, // 当前组件持有的 RclcppNode - .topic = "odom_to_camera_transform", // 发布的 topic 名 - .parent_frame = "odom_imu_link", // 父坐标系 - .child_frame = "camera_link", // 子坐标系 + .rclcpp = rclcpp, + .topic = "odom_to_camera_transform", + .parent_frame = Adapter::kParentFrame, + .child_frame = "camera_link", }; visual_odom_to_camera = std::make_unique(config); @@ -45,19 +44,42 @@ class AutoAimComponent final : public rmcs_executor::Component { } auto update() -> void override { - if (!rmcs_tf.ready()) [[unlikely]] { - handle_tf_not_ready(); + if (!adapter.ready()) [[unlikely]] { + action_throttler.dispatch("tf_not_ready", [&] { rclcpp.warn("adapter is not ready"); }); + command = ControlState::kInvalid(); + + const auto state = AutoAimState::kInvalid(); + *gimbal_takeover = state.gimbal_takeover; + *shoot_permitted = state.shoot_permitted; + *target_direction = compute_target_direction(state); return; } - publish_control_state(); - forward_auto_aim_outputs(); + update_control_state(); + feishu.send(command); + action_throttler.reset("commit_control_state_failed"); + + if (feishu.heartbeat()) { + if (auto latest = feishu.latest()) { + context = *latest; + } + auto_aim_state_received_ = true; + } + + const auto state = + auto_aim_state_received_ && Clock::now() - context.timestamp <= kAutoAimTimeout + ? context + : AutoAimState::kInvalid(); + + *gimbal_takeover = state.gimbal_takeover; + *shoot_permitted = state.shoot_permitted; + *target_direction = compute_target_direction(state); } private: static constexpr auto kAutoAimTimeout = std::chrono::milliseconds { 100 }; - InputInterface rmcs_tf; + Adapter adapter; double current_gimbal_yaw { std::numeric_limits::quiet_NaN() }; double current_gimbal_pitch { std::numeric_limits::quiet_NaN() }; @@ -66,8 +88,8 @@ class AutoAimComponent final : public rmcs_executor::Component { std::unique_ptr visual_odom_to_camera; Feishu feishu; - ControlState control_state; - AutoAimState auto_aim_state; + ControlState command; + AutoAimState context; bool auto_aim_state_received_ { false }; OutputInterface gimbal_takeover; @@ -77,13 +99,6 @@ class AutoAimComponent final : public rmcs_executor::Component { FramerateCounter framerate; ActionThrottler action_throttler { std::chrono::seconds(1), 233 }; - /// FIXME: - /// 很多细碎的辅助函数和逻辑 - /// 显然是不需要的，记得重构掉 - static auto make_invalid_auto_aim_state() -> AutoAimState { - return AutoAimState::kInvalid(); - } - static auto compute_target_direction(const AutoAimState& state) -> Eigen::Vector3d { if (!state.gimbal_takeover || !std::isfinite(state.yaw) || !std::isfinite(state.pitch)) { return Eigen::Vector3d::Zero(); @@ -98,84 +113,31 @@ class AutoAimComponent final : public rmcs_executor::Component { }; } - auto has_fresh_auto_aim_state() const -> bool { - return auto_aim_state_received_ - && Clock::now() - auto_aim_state.timestamp <= kAutoAimTimeout; - } - - auto resolve_auto_aim_state() -> AutoAimState { - if (feishu.heartbeat()) { - if (auto latest = feishu.latest()) { - auto_aim_state = *latest; - } - auto_aim_state_received_ = true; - } - - if (has_fresh_auto_aim_state()) { - return auto_aim_state; - } - - return make_invalid_auto_aim_state(); - } - - auto publish_auto_aim_outputs(const AutoAimState& state) -> void { - *gimbal_takeover = state.gimbal_takeover; - *shoot_permitted = state.shoot_permitted; - *target_direction = compute_target_direction(state); - } - - auto forward_auto_aim_outputs() -> void { publish_auto_aim_outputs(resolve_auto_aim_state()); } - - auto handle_tf_not_ready() -> void { - action_throttler.dispatch("tf_not_ready", [&] { rclcpp.warn("rmcs_tf is not ready"); }); - control_state = ControlState::kInvalid(); - publish_auto_aim_outputs(make_invalid_auto_aim_state()); - } - - auto publish_control_state() -> void { - update_gimbal_direction(); - update_control_state(); - - feishu.with_write([&](auto& data) { data = control_state; }); - action_throttler.reset("commit_control_state_failed"); - } - + std::uint8_t publish_count = 0; auto update_control_state() -> void { - control_state.timestamp = Clock::now(); - - auto odom_to_camera_transform = - fast_tf::lookup_transform( - *rmcs_tf); - - control_state.odom_to_camera_transform.position = odom_to_camera_transform.translation(); - control_state.odom_to_camera_transform.orientation = - Eigen::Quaterniond(odom_to_camera_transform.rotation()); + command.timestamp = Clock::now(); - visual_odom_to_camera->move(control_state.odom_to_camera_transform.position, - control_state.odom_to_camera_transform.orientation); - visual_odom_to_camera->update(); + auto dir = adapter.barrel_direction(); + current_gimbal_yaw = std::atan2(dir.y(), dir.x()); + current_gimbal_pitch = std::atan2(-dir.z(), std::hypot(dir.x(), dir.y())); - // TODO:无敌状态下的装甲板需要从裁判系统获取并在此更新 - control_state.invincible_devices = DeviceIds::None(); - - control_state.yaw = current_gimbal_yaw; - control_state.pitch = current_gimbal_pitch; - } + auto iso = adapter.camera_transform(); + command.odom_to_camera_transform.position = iso.translation(); + command.odom_to_camera_transform.orientation = Eigen::Quaterniond(iso.rotation()); - auto update_gimbal_direction() -> void { - using namespace rmcs_description; + visual_odom_to_camera->move(command.odom_to_camera_transform.position, + command.odom_to_camera_transform.orientation); - auto odom_to_pitch_transform = - fast_tf::lookup_transform( - *rmcs_tf); - - auto quat = Eigen::Quaterniond { odom_to_pitch_transform.toRotationMatrix() }; + if (publish_count++ > 100) { + publish_count = 0; + visual_odom_to_camera->update(); + } - auto current_pitch_direction = quat * Eigen::Vector3d::UnitX(); + // TODO:无敌状态下的装甲板需要从裁判系统获取并在此更新 + command.invincible_devices = DeviceIds::None(); - current_gimbal_yaw = std::atan2(current_pitch_direction.y(), current_pitch_direction.x()); - current_gimbal_pitch = std::atan2(current_pitch_direction.z(), - std::hypot(current_pitch_direction.x(), current_pitch_direction.y())); + command.yaw = current_gimbal_yaw; + command.pitch = current_gimbal_pitch; } }; diff --git a/src/runtime.cpp b/src/runtime.cpp index a455f0cf..e1721b29 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -186,6 +186,8 @@ auto main() -> int { /// feishu.send(command); + node.info("command: yaw({:.2}), pitch({:.2})", command.yaw, command.pitch); + } // runtime loop scope node.shutdown(); From 30d80d939e9ce34eb921bc5f2a6b1191283d8c34 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sun, 26 Apr 2026 17:03:56 +0800 Subject: [PATCH 04/13] chore: update model and remove debug logging --- config/config.yaml | 2 +- src/runtime.cpp | 2 -- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/config/config.yaml b/config/config.yaml index 4e18c955..8e4e77b5 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -40,7 +40,7 @@ identifier: - "tongji-yolov5.xml" - "shenzhen-0526.onnx" - "shenzhen-0708.onnx" - model_location: "tongji-yolov5.xml" + model_location: "shenzhen-0526.onnx" infer_device: "AUTO" use_roi_segment: false roi_rows: 640 diff --git a/src/runtime.cpp b/src/runtime.cpp index e1721b29..a455f0cf 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -186,8 +186,6 @@ auto main() -> int { /// feishu.send(command); - node.info("command: yaw({:.2}), pitch({:.2})", command.yaw, command.pitch); - } // runtime loop scope node.shutdown(); From c25073afcbb0975a5deaa3e662ea5e4d11e7472b Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sun, 26 Apr 2026 17:08:44 +0800 Subject: [PATCH 05/13] wip: add a framerate counter --- src/runtime.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/runtime.cpp b/src/runtime.cpp index a455f0cf..9ab04c0f 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -6,6 +6,7 @@ #include "kernel/tracker.hpp" #include "kernel/visualization.hpp" +#include "module/debug/framerate.hpp" #include "utility/image/armor.hpp" #include "utility/logging_util.hpp" #include "utility/panic.hpp" @@ -100,6 +101,9 @@ auto main() -> int { handle_result("visualization", result); } + auto framerate = FramerateCounter { }; + framerate.set_interval(std::chrono::seconds { 5 }); + while (util::get_running()) { node.spin_once(); @@ -108,6 +112,10 @@ auto main() -> int { auto image = capturer.fetch_image(); if (!image) continue; + if (framerate.tick()) { + node.info("Autoaim framerate: {}", framerate.fps()); + } + [[maybe_unused]] auto _ = std::experimental::scope_exit { [&] { if (visualization.initialized()) { visualization.send_image(*image); From 69c45d2c3854ef66b633736a74efe0c7beafe81e Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 27 Apr 2026 00:53:54 +0800 Subject: [PATCH 06/13] refactor: rewrite armor selection logic --- config/config.yaml | 2 +- src/module/fire_control/aim_point_chooser.cpp | 209 ++++----- src/module/predictor/outpost/robot_state.cpp | 32 +- test/aim_point_chooser.cpp | 411 ------------------ 4 files changed, 98 insertions(+), 556 deletions(-) delete mode 100644 test/aim_point_chooser.cpp diff --git a/config/config.yaml b/config/config.yaml index 7f7ab9ac..4ba90108 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -19,7 +19,7 @@ capturer: gain: 16.9807 invert_image: false - software_sync: true + software_sync: false trigger_mode: false fixed_framerate: true local_video: diff --git a/src/module/fire_control/aim_point_chooser.cpp b/src/module/fire_control/aim_point_chooser.cpp index a1728776..5d5d16db 100644 --- a/src/module/fire_control/aim_point_chooser.cpp +++ b/src/module/fire_control/aim_point_chooser.cpp @@ -1,20 +1,17 @@ #include "aim_point_chooser.hpp" -#include -#include #include +#include +#include #include "utility/math/conversion.hpp" using namespace rmcs::fire_control; struct AimPointChooser::Impl { - static constexpr int kNoArmor = -1; - - struct ArmorCandidate { - int index { kNoArmor }; + struct CandidateEval { double delta_yaw { 0.0 }; - double phase { 0.0 }; + bool in_window { false }; }; struct AngleWindow { @@ -22,152 +19,108 @@ struct AimPointChooser::Impl { double leaving { 0.0 }; }; - AngleWindow normal_window { util::deg2rad(60.0), util::deg2rad(20.0) }; // rad - AngleWindow outpost_window { util::deg2rad(70.0), util::deg2rad(30.0) }; // rad - const double min_switch_improvement_angle { util::deg2rad(6.0) }; - - double angular_velocity_threshold { 2 }; // rad/s - - int last_chosen_id { kNoArmor }; + AngleWindow normal_fast_window { util::deg2rad(70.0), util::deg2rad(20.0) }; // rad + AngleWindow outpost_window { util::deg2rad(70.0), util::deg2rad(30.0) }; // rad + const double min_switch_improvement_angle { util::deg2rad(7.0) }; - auto should_switch_target(double current_abs_error, double candidate_abs_error) const noexcept - -> bool { - return candidate_abs_error + min_switch_improvement_angle < current_abs_error; - } - - static auto abs_error(ArmorCandidate const& candidate) noexcept -> double { - return std::abs(candidate.delta_yaw); - } + std::optional last_chosen_armor_id {}; - auto angle_window(DeviceId genre) const noexcept -> AngleWindow const& { - return genre == DeviceId::OUTPOST ? outpost_window : normal_window; + auto initialize(Config const& config) noexcept -> void { + normal_fast_window = { config.coming_angle, config.leaving_angle }; + outpost_window = { config.outpost_coming_angle, config.outpost_leaving_angle }; } - auto choose_low_speed( - std::span candidates, AngleWindow const& window) const -> int { - auto const in_window = [&](ArmorCandidate const& candidate) { - return abs_error(candidate) < window.coming; - }; - - auto best_in_window = std::optional {}; - for (auto const& candidate : candidates) { - if (!in_window(candidate)) continue; - if (!best_in_window.has_value() || abs_error(candidate) < abs_error(*best_in_window)) { - best_in_window = candidate; - } + 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; } - if (!best_in_window.has_value()) return kNoArmor; + const auto center_yaw = std::atan2(center_position.y(), center_position.x()); + const auto is_outpost = armors.front().genre == DeviceId::OUTPOST; + auto const& active_window = is_outpost ? outpost_window : normal_fast_window; - auto const has_last_candidate = - (last_chosen_id >= 0) && (static_cast(last_chosen_id) < candidates.size()); - if (!has_last_candidate) return best_in_window->index; - - auto const& last_candidate = candidates[static_cast(last_chosen_id)]; - if (!in_window(last_candidate)) return best_in_window->index; - if (best_in_window->index == last_chosen_id) return last_chosen_id; - if (!should_switch_target(abs_error(last_candidate), abs_error(*best_in_window))) - return last_chosen_id; - - return best_in_window->index; - } + auto candidate_evals = std::vector(armors.size()); - auto choose_high_speed( - std::span candidates, AngleWindow const& window) const -> int { - auto const is_in_window = [&](ArmorCandidate const& candidate) { - return abs_error(candidate) < window.coming && candidate.phase <= window.leaving; + const auto yaw = [&](size_t index) { + auto orientation = Eigen::Quaterniond {}; + armors[index].orientation.copy_to(orientation); + return util::eulers(orientation)[0]; }; - auto const is_incoming = [](ArmorCandidate const& candidate) { - return candidate.phase < 0.0; + const auto in_window = [&](double delta_yaw) { + auto const abs_delta = std::abs(delta_yaw); + auto const in_coming = abs_delta <= active_window.coming; + auto const in_leaving = (angular_velocity > 0.0) ? (delta_yaw <= active_window.leaving) + : (angular_velocity < 0.0) ? (delta_yaw >= -active_window.leaving) + : true; + return in_coming && in_leaving; }; - auto const is_better_than = [&](ArmorCandidate const& candidate, - ArmorCandidate const& current_best) { - auto const candidate_incoming = is_incoming(candidate); - auto const best_incoming = is_incoming(current_best); - if (candidate_incoming != best_incoming) return candidate_incoming; - - return abs_error(candidate) < abs_error(current_best); - }; - - auto preferred = std::optional {}; - for (auto const& candidate : candidates) { - if (!is_in_window(candidate)) continue; - - if (!preferred.has_value() || is_better_than(candidate, *preferred)) { - preferred = candidate; + { // 1) 候选评估 + for (size_t index = 0; index < armors.size(); ++index) { + auto const delta_yaw = util::normalize_angle(yaw(index) - center_yaw); + candidate_evals[index] = { + .delta_yaw = delta_yaw, + .in_window = in_window(delta_yaw), + }; } } - if (!preferred) return kNoArmor; - - auto const has_last = - (last_chosen_id >= 0) && (static_cast(last_chosen_id) < candidates.size()); - if (!has_last) return preferred->index; - - auto const& last = candidates[static_cast(last_chosen_id)]; - if (!is_in_window(last)) return preferred->index; - if (preferred->index == last_chosen_id) return last_chosen_id; + const auto priority_key = [&](size_t index) { + // 优先级： + // 1) abs_delta：角误差更小优先 + // 2) last_penalty：上一帧目标优先（is_last -> 0，其它 -> 1） + // 3) id：稳定排序 + // 4) index：最终兜底，保证结果确定性 + auto const abs_delta = std::abs(candidate_evals[index].delta_yaw); + auto const id = armors[index].id; + auto const is_last = last_chosen_armor_id.has_value() && (id == *last_chosen_armor_id); + auto const last_penalty = is_last ? 0 : 1; + return std::tuple { abs_delta, last_penalty, id, index }; + }; - return should_switch_target(abs_error(last), abs_error(*preferred)) ? preferred->index - : last_chosen_id; - } + auto best_idx = std::optional {}; + auto last_idx = std::optional {}; - auto initialize(Config const& config) noexcept -> void { - normal_window = { config.coming_angle, config.leaving_angle }; - outpost_window = { config.outpost_coming_angle, config.outpost_leaving_angle }; + { + // 2) 最优筛选（仅角度窗口内）并定位上次目标 + for (size_t index = 0; index < armors.size(); ++index) { + if (last_chosen_armor_id.has_value() + && (armors[index].id == *last_chosen_armor_id)) { + last_idx = index; + } - angular_velocity_threshold = config.angular_velocity_threshold; - } + if (!candidate_evals[index].in_window) continue; - auto choose_armor(std::span armors, Eigen::Vector3d const& center_position, - double angular_velocity) -> std::optional { - if (armors.empty()) { - last_chosen_id = kNoArmor; - return std::nullopt; + if (!best_idx.has_value() || (priority_key(index) < priority_key(*best_idx))) { + best_idx = index; + } + } } - const auto center_yaw = std::atan2(center_position.y(), center_position.x()); - auto candidates = std::array {}; - const auto n = std::min(armors.size(), candidates.size()); - - for (size_t id = 0; id < n; ++id) { - auto orientation = Eigen::Quaterniond {}; - armors[id].orientation.copy_to(orientation); - - const auto ypr = util::eulers(orientation); - const auto yaw_in_world = ypr[0]; - const auto delta_yaw = util::normalize_angle(yaw_in_world - center_yaw); - const auto phase = (angular_velocity > 0.0) ? delta_yaw - : (angular_velocity < 0.0) ? -delta_yaw - : 0.0; - - candidates[id] = { static_cast(id), delta_yaw, phase }; + if (!best_idx.has_value()) { + last_chosen_armor_id.reset(); + return std::nullopt; } - auto chosen_id = kNoArmor; - auto candidate_view = std::span { candidates }.first(n); - auto const genre = armors.front().genre; - auto const& window = angle_window(genre); - - // --- 非小陀螺模式 (低速旋转) --- - if ((std::abs(angular_velocity) < angular_velocity_threshold) - && (genre != DeviceId::OUTPOST)) { - chosen_id = choose_low_speed(candidate_view, window); + { + // 3) 切换抖动抑制 + if (last_idx.has_value() && (*last_idx != *best_idx)) { + auto const last_abs = std::abs(candidate_evals[*last_idx].delta_yaw); + auto const best_abs = std::abs(candidate_evals[*best_idx].delta_yaw); + auto const improvement = last_abs - best_abs; + if (improvement < min_switch_improvement_angle) { + best_idx = last_idx; + } + } } - // --- 小陀螺模式 (快速旋转) --- - else { - chosen_id = choose_high_speed(candidate_view, window); + { + // 4) 状态更新并返回 + last_chosen_armor_id = armors[*best_idx].id; + return { armors[*best_idx] }; } - - if (chosen_id != kNoArmor) { - last_chosen_id = chosen_id; - return { armors[chosen_id] }; - } - - last_chosen_id = kNoArmor; - return std::nullopt; } }; diff --git a/src/module/predictor/outpost/robot_state.cpp b/src/module/predictor/outpost/robot_state.cpp index 19b634ed..3a158f4d 100644 --- a/src/module/predictor/outpost/robot_state.cpp +++ b/src/module/predictor/outpost/robot_state.cpp @@ -43,7 +43,7 @@ auto make_observation(rmcs::Armor3D const& armor) -> OutpostObservation { auto const ypr = rmcs::util::eulers(orientation); auto const ypd = rmcs::util::xyz2ypd(xyz); - auto z = OutpostEKF::ZVec { }; + auto z = OutpostEKF::ZVec {}; z << ypd[0], ypd[1], ypd[2], ypr[0]; return { z, OutpostEKFParameters::R(xyz, ypr, ypd), xyz, ypr, ypd }; @@ -86,7 +86,7 @@ struct TrackingConfig { std::chrono::duration reset_interval { 1.5 }; int spin_confirm_switches { 2 }; int min_converged_updates { 6 }; - MatchingConfig matching { }; + MatchingConfig matching {}; }; struct SpinTracker { @@ -95,7 +95,7 @@ struct SpinTracker { int candidate_count { 0 }; bool locked { false }; - auto reset() -> void { *this = { }; } + auto reset() -> void { *this = {}; } auto current_sign() const -> int { if (locked) return locked_sign; @@ -177,9 +177,9 @@ class AssociationEngine { , config_ { config } { } auto decide(OutpostObservation const& observation) const -> AssociationDecision { - if (!has_assigned_slot(layout_, current_armor_id_)) return { }; + if (!has_assigned_slot(layout_, current_armor_id_)) return {}; - auto best_decision = AssociationDecision { }; + 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; @@ -216,7 +216,7 @@ class AssociationEngine { // 这里没有加yaw约束，一是因为yaw的抖动太大，二是因为大部分图像中 一帧只有一块装甲板 if (azimuth_error > config_.azimuth_gate || z_error > config_.z_gate) { - return { }; + return {}; } auto const H = OutpostEKFParameters::H(x_, phase_offset, height_offset); @@ -225,7 +225,7 @@ class AssociationEngine { 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 {}; } auto error = *mahalanobis; @@ -302,10 +302,10 @@ struct OutpostRobotState::Impl { auto initialize(Armor3D const& armor, TimePoint t) -> void { color = armor_color2camp_color(armor.color); ekf = EKF { OutpostEKFParameters::x(armor), - OutpostEKFParameters::P_initial_dig().asDiagonal() }; + OutpostEKFParameters::P_initial_dig().asDiagonal() }; time_stamp = t; - layout = OutpostArmorLayout { }; + layout = OutpostArmorLayout {}; layout.slots[0].assigned = true; spin.reset(); @@ -363,8 +363,8 @@ struct OutpostRobotState::Impl { private: auto reset_runtime_state(TimePoint t) -> void { color = CampColor::UNKNOWN; - ekf = EKF { }; - layout = OutpostArmorLayout { }; + ekf = EKF {}; + layout = OutpostArmorLayout {}; time_stamp = t; initialized = false; current_armor_id = kUnknownArmorId; @@ -373,7 +373,7 @@ struct OutpostRobotState::Impl { } auto select_best_match(std::span armors) const -> std::optional { - auto best_match = std::optional { }; + auto best_match = std::optional {}; auto matcher = AssociationEngine { ekf.x, ekf.P(), layout, current_armor_id, spin, config.matching }; @@ -410,15 +410,15 @@ struct OutpostRobotState::Impl { } CampColor color { CampColor::UNKNOWN }; - EKF ekf { EKF { } }; - OutpostArmorLayout layout { }; + EKF ekf { EKF {} }; + OutpostArmorLayout layout {}; TimePoint time_stamp; bool initialized { false }; int current_armor_id { kUnknownArmorId }; - SpinTracker spin { }; + SpinTracker spin {}; int update_count { 0 }; - TrackingConfig config { }; + TrackingConfig config {}; }; OutpostRobotState::OutpostRobotState() noexcept diff --git a/test/aim_point_chooser.cpp b/test/aim_point_chooser.cpp deleted file mode 100644 index 391e12b7..00000000 --- a/test/aim_point_chooser.cpp +++ /dev/null @@ -1,411 +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 kLowSpeed = 1.0; -constexpr auto kFastPositive = 4.0; -constexpr auto kFastNegative = -4.0; -constexpr auto kSwitchSpeedEps = 0.01; - -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 make_armors(std::span yaws_deg, - DeviceId genre = DeviceId::SENTRY) -> std::vector { - auto armors = std::vector {}; - armors.reserve(yaws_deg.size()); - - for (size_t i = 0; i < yaws_deg.size(); ++i) { - armors.emplace_back(make_armor(yaws_deg[i], static_cast(i), genre)); - } - 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); -} - -auto expect_single_armor_result(double angle_deg, double angular_velocity, DeviceId genre, - std::optional expected) -> void { - auto const armors = make_armors({ angle_deg }, genre); - auto const actual = choose_once(armors, 0.0, angular_velocity); - EXPECT_EQ(actual, expected); -} - -} // namespace - -TEST(AimPointChooser, ScenarioTemplateSingleArmorLowSpeedAcquireScan) { - for (int angle_deg = -180; angle_deg <= 180; angle_deg += kAngleStepDeg) { - auto const armors = make_armors({ static_cast(angle_deg) }); - auto const actual = choose_once(armors, 0.0, kLowSpeed); - - auto const expected = (std::abs(angle_deg) < 60) ? std::optional { 0 } : std::nullopt; - - SCOPED_TRACE("angle_deg=" + std::to_string(angle_deg)); - EXPECT_EQ(actual, expected); - } -} - -TEST(AimPointChooser, ScenarioTemplateSingleArmorHighSpeedScanBySpinDirection) { - constexpr std::array speeds { 2.0, kFastPositive, -2.0, kFastNegative }; - - for (int angle_deg = -180; angle_deg <= 180; angle_deg += kAngleStepDeg) { - auto const armors = make_armors({ static_cast(angle_deg) }); - - for (auto const speed : speeds) { - 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, ScenarioTemplateSingleArmorOutpostScan) { - constexpr std::array speeds { 0.0, 1.0, -1.0, 1.99, -1.99 }; - - for (int angle_deg = -180; angle_deg <= 180; angle_deg += kAngleStepDeg) { - for (auto const speed : speeds) { - auto const in_coming_window = std::abs(angle_deg) < 70; - - auto in_leaving_window = false; - if (speed == 0.0) { - in_leaving_window = true; - } else if (speed > 0.0) { - in_leaving_window = angle_deg <= 30; - } else { - in_leaving_window = angle_deg >= -30; - } - - auto const expected = (in_coming_window && in_leaving_window) - ? std::optional { 0 } - : std::nullopt; - - auto const armors = make_armors({ static_cast(angle_deg) }, DeviceId::OUTPOST); - 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, BoundaryCasesCoverWindowsAndVelocityThreshold) { - struct TestCase { - std::string name; - double angle_deg; - double speed; - DeviceId genre; - std::optional expected; - }; - - constexpr auto kDegEps = 0.001; - - auto const cases = std::array { - TestCase { "low_speed_acquire_inside", 60.0 - kDegEps, kLowSpeed, DeviceId::SENTRY, 0 }, - TestCase { "low_speed_acquire_boundary", 60.0, kLowSpeed, DeviceId::SENTRY, std::nullopt }, - TestCase { "low_speed_acquire_outside", 60.0 + kDegEps, kLowSpeed, DeviceId::SENTRY, - std::nullopt }, - - TestCase { "high_positive_leaving_boundary", 20.0, kFastPositive, DeviceId::SENTRY, 0 }, - TestCase { "high_positive_leaving_outside", 20.0 + kDegEps, kFastPositive, - DeviceId::SENTRY, std::nullopt }, - TestCase { "high_positive_coming_inside", -60.0 + kDegEps, kFastPositive, - DeviceId::SENTRY, 0 }, - TestCase { "high_positive_coming_boundary", -60.0, kFastPositive, DeviceId::SENTRY, - std::nullopt }, - - TestCase { "high_negative_leaving_boundary", -20.0, kFastNegative, DeviceId::SENTRY, 0 }, - TestCase { "high_negative_leaving_outside", -20.0 - kDegEps, kFastNegative, - DeviceId::SENTRY, std::nullopt }, - TestCase { "high_negative_coming_inside", 60.0 - kDegEps, kFastNegative, DeviceId::SENTRY, - 0 }, - TestCase { "high_negative_coming_boundary", 60.0, kFastNegative, DeviceId::SENTRY, - std::nullopt }, - - TestCase { "outpost_positive_leaving_boundary", 30.0, 1.0, DeviceId::OUTPOST, 0 }, - TestCase { "outpost_positive_leaving_outside", 30.0 + kDegEps, 1.0, DeviceId::OUTPOST, - std::nullopt }, - TestCase { "outpost_positive_coming_inside", -70.0 + kDegEps, 1.0, DeviceId::OUTPOST, 0 }, - TestCase { "outpost_positive_coming_boundary", -70.0, 1.0, DeviceId::OUTPOST, - std::nullopt }, - - TestCase { "velocity_threshold_below", 50.0, 2.0 - kSwitchSpeedEps, DeviceId::SENTRY, 0 }, - TestCase { "velocity_threshold_equal", 50.0, 2.0, DeviceId::SENTRY, std::nullopt }, - TestCase { "velocity_threshold_above", 50.0, 2.0 + kSwitchSpeedEps, DeviceId::SENTRY, - std::nullopt }, - TestCase { - "velocity_threshold_negative_below", -50.0, -2.0 + kSwitchSpeedEps, DeviceId::SENTRY, 0 }, - TestCase { "velocity_threshold_negative_equal", -50.0, -2.0, DeviceId::SENTRY, - std::nullopt }, - TestCase { "velocity_threshold_negative_above", -50.0, -2.0 - kSwitchSpeedEps, - DeviceId::SENTRY, std::nullopt }, - }; - - for (auto const& test_case : cases) { - SCOPED_TRACE(test_case.name); - expect_single_armor_result( - test_case.angle_deg, test_case.speed, test_case.genre, test_case.expected); - } -} - -TEST(AimPointChooser, SymmetricDualArmorHighSpeedFollowsIncomingDirection) { - for (int theta_deg = 5; theta_deg <= 55; theta_deg += 5) { - auto const armors = make_armors({ -static_cast(theta_deg), static_cast(theta_deg) }); - - auto const positive_spin = choose_once(armors, 0.0, kFastPositive); - auto const negative_spin = choose_once(armors, 0.0, kFastNegative); - - SCOPED_TRACE("theta_deg=" + std::to_string(theta_deg)); - EXPECT_EQ(positive_spin, std::optional { 0 }); - EXPECT_EQ(negative_spin, std::optional { 1 }); - } -} - -TEST(AimPointChooser, SymmetricLowSpeedSwitchUsesMarginAndImmediateSwitch) { - auto chooser = make_chooser(); - - auto const frame_initial = make_armors({ -30.0, 30.0 }); - EXPECT_EQ(choose_id(*chooser, frame_initial, 0.0, kLowSpeed), std::optional { 0 }); - - auto const frame_margin = make_armors({ -30.0, 25.0 }); // improvement = 5 deg - EXPECT_EQ(choose_id(*chooser, frame_margin, 0.0, kLowSpeed), std::optional { 0 }); - - auto const frame_switch = make_armors({ -30.0, 22.0 }); // improvement = 8 deg - EXPECT_EQ(choose_id(*chooser, frame_switch, 0.0, kLowSpeed), std::optional { 1 }); -} - -TEST(AimPointChooser, LowSpeedSwitchesImmediatelyWhenPreferredTargetChanges) { - auto chooser = make_chooser(); - - auto const frame_initial = make_armors({ 10.0, 30.0, 40.0 }); - EXPECT_EQ(choose_id(*chooser, frame_initial, 0.0, kLowSpeed), std::optional { 0 }); - - auto const frame_target_2 = make_armors({ 20.0, 15.0, 8.0 }); - EXPECT_EQ(choose_id(*chooser, frame_target_2, 0.0, kLowSpeed), std::optional { 2 }); - - auto const frame_target_1 = make_armors({ 20.0, 8.0, 15.0 }); - EXPECT_EQ(choose_id(*chooser, frame_target_1, 0.0, kLowSpeed), std::optional { 1 }); -} - -TEST(AimPointChooser, HighSpeedSwitchesImmediatelyWhenImprovementExceedsMargin) { - auto chooser = make_chooser(); - - auto const frame_initial = make_armors({ 12.0, 18.0, 25.0 }); - EXPECT_EQ(choose_id(*chooser, frame_initial, 0.0, kFastPositive), std::optional { 0 }); - - auto const frame_switch = make_armors({ 20.0, 12.0, 25.0 }); // abs improvement = 8 deg - EXPECT_EQ(choose_id(*chooser, frame_switch, 0.0, kFastPositive), std::optional { 1 }); -} - -TEST(AimPointChooser, EmptyInputClearsStateAndAllowsImmediateReacquire) { - auto chooser = make_chooser(); - - auto const frame_a = make_armors({ 10.0, 20.0 }); - auto const frame_b = make_armors({ 20.0, 10.0 }); - auto const frame_no_acquire = make_armors({ 70.0, 65.0 }); - auto const empty = std::vector {}; - - EXPECT_EQ(choose_id(*chooser, frame_a, 0.0, kLowSpeed), std::optional { 0 }); - EXPECT_EQ(choose_id(*chooser, frame_b, 0.0, kLowSpeed), std::optional { 1 }); - EXPECT_EQ(choose_id(*chooser, frame_no_acquire, 0.0, kLowSpeed), std::nullopt); - - EXPECT_EQ(choose_id(*chooser, empty, 0.0, kLowSpeed), std::nullopt); - EXPECT_EQ(choose_id(*chooser, frame_no_acquire, 0.0, kLowSpeed), std::nullopt); -} - -TEST(AimPointChooser, YawWraparoundAndGlobalShiftAreInvariant) { - constexpr std::array speeds { kLowSpeed, kFastPositive, kFastNegative }; - - for (int angle_deg = -175; angle_deg <= 175; angle_deg += 35) { - for (auto const speed : speeds) { - auto const base_armors = make_armors({ static_cast(angle_deg) }); - auto const plus_armors = make_armors({ static_cast(angle_deg + 360) }); - auto const minus_armors = make_armors({ static_cast(angle_deg - 360) }); - - auto const base_result = choose_once(base_armors, 0.0, speed); - auto const plus_result = choose_once(plus_armors, 0.0, speed); - auto const minus_result = choose_once(minus_armors, 0.0, speed); - - SCOPED_TRACE("angle_deg=" + std::to_string(angle_deg)); - SCOPED_TRACE("speed=" + std::to_string(speed)); - - EXPECT_EQ(base_result, plus_result); - EXPECT_EQ(base_result, minus_result); - } - } - - constexpr std::array template_yaws { -35.0, 15.0, 80.0 }; - constexpr std::array genres { DeviceId::SENTRY, DeviceId::OUTPOST }; - - for (auto const genre : genres) { - for (auto const speed : speeds) { - auto const baseline_armors = make_armors(template_yaws, genre); - auto const baseline_result = choose_once(baseline_armors, 0.0, speed); - - for (int shift_deg = -150; shift_deg <= 150; shift_deg += 30) { - auto shifted_yaws = std::vector {}; - shifted_yaws.reserve(template_yaws.size()); - for (auto const yaw_deg : template_yaws) { - shifted_yaws.push_back(yaw_deg + static_cast(shift_deg)); - } - - auto const shifted_armors = make_armors(shifted_yaws, genre); - auto const shifted_result = - choose_once(shifted_armors, static_cast(shift_deg), speed); - - SCOPED_TRACE("genre=" + std::to_string(static_cast(genre))); - SCOPED_TRACE("speed=" + std::to_string(speed)); - SCOPED_TRACE("shift_deg=" + std::to_string(shift_deg)); - - EXPECT_EQ(shifted_result, baseline_result); - } - } - } -} - -TEST(AimPointChooser, RandomizedScenarioRegressionIsDeterministicAndValid) { - struct Frame { - std::vector armors; - double center_yaw_deg; - double angular_velocity; - }; - - auto rng = std::mt19937 { 0xA11C0DEu }; - - auto count_dist = std::uniform_int_distribution { 0, 4 }; - auto yaw_dist = std::uniform_real_distribution { -720.0, 720.0 }; - auto center_yaw_dist = std::uniform_real_distribution { -180.0, 180.0 }; - auto velocity_dist = std::uniform_real_distribution { -8.0, 8.0 }; - auto outpost_selector = std::bernoulli_distribution { 0.25 }; - - auto frames = std::vector {}; - frames.reserve(2000); - - for (int i = 0; i < 2000; ++i) { - auto const count = count_dist(rng); - auto const genre = outpost_selector(rng) ? DeviceId::OUTPOST : DeviceId::SENTRY; - - auto armors = std::vector {}; - armors.reserve(static_cast(count)); - - for (int id = 0; id < count; ++id) { - armors.emplace_back(make_armor(yaw_dist(rng), id, genre)); - } - - frames.emplace_back(Frame { - .armors = std::move(armors), - .center_yaw_deg = center_yaw_dist(rng), - .angular_velocity = velocity_dist(rng), - }); - } - - auto chooser_a = make_chooser(); - auto chooser_b = make_chooser(); - - for (size_t i = 0; i < frames.size(); ++i) { - auto const& frame = frames[i]; - - auto const result_a = choose_id( - *chooser_a, frame.armors, frame.center_yaw_deg, frame.angular_velocity); - auto const result_b = choose_id( - *chooser_b, frame.armors, frame.center_yaw_deg, frame.angular_velocity); - - SCOPED_TRACE("frame_index=" + std::to_string(i)); - EXPECT_EQ(result_a, result_b); - - if (frame.armors.empty()) { - EXPECT_EQ(result_a, std::nullopt); - } - - if (result_a.has_value()) { - EXPECT_GE(*result_a, 0); - EXPECT_LT(*result_a, static_cast(frame.armors.size())); - } - } -} From a89caf0f7747d4a0a95ef4d38cbace8b0225ba25 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 27 Apr 2026 01:23:28 +0800 Subject: [PATCH 07/13] chore: remove redundant parameters and deprecated configurations --- config/config.yaml | 1 - src/kernel/fire_control.cpp | 32 ++++++++----------- src/module/fire_control/aim_point_chooser.hpp | 9 +++--- test/CMakeLists.txt | 6 ---- 4 files changed, 18 insertions(+), 30 deletions(-) diff --git a/config/config.yaml b/config/config.yaml index 4ba90108..3aec952d 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -98,7 +98,6 @@ fire_control: leaving_angle: 30.0 # degree outpost_coming_angle: 50.0 # degree outpost_leaving_angle: 40.0 # degree - angular_velocity_threshold: 120 # degree/s first_tolerance: 3 # 近距离射击容差，degree second_tolerance: 2 # 远距离射击容差，degree diff --git a/src/kernel/fire_control.cpp b/src/kernel/fire_control.cpp index 0a0ef471..225a6ad2 100644 --- a/src/kernel/fire_control.cpp +++ b/src/kernel/fire_control.cpp @@ -22,11 +22,10 @@ struct FireControl::Impl { double yaw_offset; // rad (config in degree) double pitch_offset; // rad (config in degree) - double coming_angle; // rad - double leaving_angle; // rad - double outpost_coming_angle; // rad - double outpost_leaving_angle; // rad - double angular_velocity_threshold; // rad/s + double coming_angle; // rad + double leaving_angle; // rad + double outpost_coming_angle; // rad + double outpost_leaving_angle; // rad // clang-format off constexpr static std::tuple metas { @@ -39,7 +38,6 @@ struct FireControl::Impl { &Config::leaving_angle,"leaving_angle", &Config::outpost_coming_angle,"outpost_coming_angle", &Config::outpost_leaving_angle,"outpost_leaving_angle", - &Config::angular_velocity_threshold,"angular_velocity_threshold", }; // clang-format on }; @@ -61,20 +59,18 @@ struct FireControl::Impl { "Invalid initial_bullet_speed: {}", config.initial_bullet_speed) }; } - config.yaw_offset = util::deg2rad(config.yaw_offset); - config.pitch_offset = util::deg2rad(config.pitch_offset); - config.coming_angle = util::deg2rad(config.coming_angle); - config.leaving_angle = util::deg2rad(config.leaving_angle); - config.outpost_coming_angle = util::deg2rad(config.outpost_coming_angle); - config.outpost_leaving_angle = util::deg2rad(config.outpost_leaving_angle); - config.angular_velocity_threshold = util::deg2rad(config.angular_velocity_threshold); + config.yaw_offset = util::deg2rad(config.yaw_offset); + config.pitch_offset = util::deg2rad(config.pitch_offset); + config.coming_angle = util::deg2rad(config.coming_angle); + config.leaving_angle = util::deg2rad(config.leaving_angle); + config.outpost_coming_angle = util::deg2rad(config.outpost_coming_angle); + config.outpost_leaving_angle = util::deg2rad(config.outpost_leaving_angle); auto chooser_config = AimPointChooser::Config { - .coming_angle = config.coming_angle, - .leaving_angle = config.leaving_angle, - .angular_velocity_threshold = config.angular_velocity_threshold, - .outpost_coming_angle = config.outpost_coming_angle, - .outpost_leaving_angle = config.outpost_leaving_angle, + .coming_angle = config.coming_angle, + .leaving_angle = config.leaving_angle, + .outpost_coming_angle = config.outpost_coming_angle, + .outpost_leaving_angle = config.outpost_leaving_angle, }; aim_point_chooser.initialize(chooser_config); diff --git a/src/module/fire_control/aim_point_chooser.hpp b/src/module/fire_control/aim_point_chooser.hpp index 930d6f07..dfd26c53 100644 --- a/src/module/fire_control/aim_point_chooser.hpp +++ b/src/module/fire_control/aim_point_chooser.hpp @@ -12,11 +12,10 @@ namespace rmcs::fire_control { class AimPointChooser { public: struct Config { - double coming_angle; // rad - double leaving_angle; // rad - double angular_velocity_threshold; // rad/s - double outpost_coming_angle; // rad - double outpost_leaving_angle; // rad + double coming_angle; // rad + double leaving_angle; // rad + double outpost_coming_angle; // rad + double outpost_leaving_angle; // rad }; auto initialize(Config const& config) noexcept -> void; diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index dfb92d46..e114ac3e 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -121,9 +121,3 @@ ament_add_gtest( ${TEST_DIR}/feishu_test.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 -) From 6237f7a4a3fab51b362555cd705ae244d4de941a Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Mon, 27 Apr 2026 22:04:04 +0800 Subject: [PATCH 08/13] feat: add ImageRecorder utility with HFYU lossless recording - ImageRecorder: pure cv::Mat-based recorder with frame-rate control, duration validation, history cleanup, and auto-save on destruction - Uses HuffYUV (HFYU) lossless codec with AVI container for real-time recording at camera native frame rate - Tool: async recording in hikcamera tool with queue-backed worker thread to avoid blocking the capture pipeline - Recording stats printed every 2s: write fps, latency, queue depth - Test: verifies duration-based save/discard behavior --- src/utility/image/recorder.cpp | 234 +++++++++++++++++++++++++++ src/utility/image/recorder.hpp | 30 ++++ test/CMakeLists.txt | 12 +- test/image_recorder.cpp | 77 +++++++++ tool/CMakeLists.txt | 1 + tool/hikcamera.cpp | 278 ++++++++++++++++++++++++++++----- 6 files changed, 594 insertions(+), 38 deletions(-) create mode 100644 src/utility/image/recorder.cpp create mode 100644 src/utility/image/recorder.hpp create mode 100644 test/image_recorder.cpp diff --git a/src/utility/image/recorder.cpp b/src/utility/image/recorder.cpp new file mode 100644 index 00000000..d9dc69ba --- /dev/null +++ b/src/utility/image/recorder.cpp @@ -0,0 +1,234 @@ +#include "recorder.hpp" + +#include +#include +#include +#include +#include +#include + +#include + +using namespace rmcs; + +struct ImageRecorder::Impl { + using Clock = std::chrono::steady_clock; + using TimePoint = Clock::time_point; + + std::string location = "/tmp/auto_aim_recordings"; + + std::size_t framerate = 30; + bool auto_save = true; + std::size_t max_history = 5; + + std::chrono::seconds max_duration { 60 }; + std::chrono::seconds min_duration { 1 }; + + bool is_recording = false; + + TimePoint recording_start { }; + + int frame_count = 0; + int width = 0; + int height = 0; + + std::string current_file_path = { }; + std::string last_saved_path = { }; + + std::unique_ptr writer = { }; + + ~Impl() noexcept { + if (auto_save) { + stop(); + } else { + finalize_recording(false); + } + } + + auto generate_output_path() const -> std::string { + const auto now = std::chrono::system_clock::now(); + const auto timestamp = + std::chrono::duration_cast(now.time_since_epoch()).count(); + + return location + "/" + std::to_string(timestamp) + ".avi"; + } + + auto start_recording(const cv::Mat& mat) -> void { + if (mat.empty()) { + return; + } + + last_saved_path = { }; + + std::error_code error = { }; + std::filesystem::create_directories(location, error); + if (error) { + return; + } + + width = mat.cols; + height = mat.rows; + + current_file_path = generate_output_path(); + writer = std::make_unique(); + + if (!writer->open(current_file_path, cv::VideoWriter::fourcc('H', 'F', 'Y', 'U'), + static_cast(framerate), cv::Size { width, height })) { + writer.reset(); + current_file_path = { }; + width = 0; + height = 0; + frame_count = 0; + is_recording = false; + return; + } + + const auto now = Clock::now(); + recording_start = now; + frame_count = 0; + is_recording = true; + } + + auto is_valid_duration() const noexcept -> bool { + if (!is_recording && recording_start == TimePoint { }) { + return false; + } + + const auto duration = Clock::now() - recording_start; + return duration >= min_duration && duration <= max_duration; + } + + auto cleanup_old_recordings() noexcept -> void { + if (max_history == 0) { + return; + } + + std::error_code error = { }; + if (!std::filesystem::exists(location, error) || error) { + return; + } + + auto recordings = std::vector { }; + for (const auto& entry : std::filesystem::directory_iterator { location, error }) { + if (error) { + return; + } + + if (entry.is_regular_file(error) && !error && entry.path().extension() == ".avi") { + recordings.push_back(entry); + } + error.clear(); + } + + if (recordings.size() <= max_history) { + return; + } + + std::ranges::sort(recordings, [](const auto& lhs, const auto& rhs) { + std::error_code lhs_error = { }; + std::error_code rhs_error = { }; + const auto lhs_time = std::filesystem::last_write_time(lhs, lhs_error); + const auto rhs_time = std::filesystem::last_write_time(rhs, rhs_error); + + if (lhs_error || rhs_error) { + return lhs.path().filename().string() > rhs.path().filename().string(); + } + return lhs_time > rhs_time; + }); + + for (const auto& entry : recordings | std::views::drop(max_history)) { + std::filesystem::remove(entry.path(), error); + error.clear(); + } + } + + auto finalize_recording(bool keep) noexcept -> void { + if (writer) { + writer->release(); + writer.reset(); + } + + if (!current_file_path.empty()) { + std::error_code error = { }; + if (keep) { + last_saved_path = current_file_path; + cleanup_old_recordings(); + } else { + std::filesystem::remove(current_file_path, error); + last_saved_path = { }; + } + } + + current_file_path = { }; + recording_start = TimePoint { }; + frame_count = 0; + width = 0; + height = 0; + is_recording = false; + } + + auto write_frame(const cv::Mat& mat) -> void { + if (mat.empty()) { + return; + } + + if (!is_recording) { + start_recording(mat); + } + + if (!writer || !is_recording) { + return; + } + + if (mat.cols != width || mat.rows != height) { + finalize_recording(false); + start_recording(mat); + if (!writer || !is_recording) { + return; + } + } + + writer->write(mat.clone()); + frame_count += 1; + } + + auto stop() -> void { + if (!writer || !is_recording) { + return; + } + + const auto keep = auto_save && is_valid_duration() && frame_count > 0; + finalize_recording(keep); + } +}; + +auto ImageRecorder::set_saving_location(const std::string& path) -> void { pimpl->location = path; } + +auto ImageRecorder::set_framerate(std::size_t rate) -> void { pimpl->framerate = rate; } + +auto ImageRecorder::set_auto_save(bool enabled) -> void { pimpl->auto_save = enabled; } + +auto ImageRecorder::set_max_history_count(std::size_t count) -> void { pimpl->max_history = count; } + +auto ImageRecorder::set_max_recording_duration(std::chrono::seconds duration) -> void { + pimpl->max_duration = duration; +} + +auto ImageRecorder::set_min_recording_duration(std::chrono::seconds duration) -> void { + pimpl->min_duration = duration; +} + +auto ImageRecorder::write_frame(const cv::Mat& mat) -> void { pimpl->write_frame(mat); } + +auto ImageRecorder::stop() -> void { pimpl->stop(); } + +auto ImageRecorder::recording() const -> bool { return pimpl->is_recording; } + +auto ImageRecorder::current_file_path() const -> std::string { return pimpl->current_file_path; } + +auto ImageRecorder::last_saved_path() const -> std::string { return pimpl->last_saved_path; } + +ImageRecorder::ImageRecorder() noexcept + : pimpl { std::make_unique() } { } + +ImageRecorder::~ImageRecorder() noexcept = default; diff --git a/src/utility/image/recorder.hpp b/src/utility/image/recorder.hpp new file mode 100644 index 00000000..7f165612 --- /dev/null +++ b/src/utility/image/recorder.hpp @@ -0,0 +1,30 @@ +#pragma once +#include "utility/pimpl.hpp" + +#include +#include + +#include + +namespace rmcs { + +class ImageRecorder { + RMCS_PIMPL_DEFINITION(ImageRecorder) + +public: + auto set_saving_location(const std::string&) -> void; + auto set_framerate(std::size_t) -> void; + auto set_auto_save(bool) -> void; + auto set_max_history_count(std::size_t) -> void; + auto set_max_recording_duration(std::chrono::seconds) -> void; + auto set_min_recording_duration(std::chrono::seconds) -> void; + + auto write_frame(const cv::Mat&) -> void; + auto stop() -> void; + + [[nodiscard]] auto recording() const -> bool; + [[nodiscard]] auto current_file_path() const -> std::string; + [[nodiscard]] auto last_saved_path() const -> std::string; +}; + +} diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index e114ac3e..e87e9e4f 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -77,6 +77,17 @@ ament_add_gtest( ${TEST_DIR}/device_id.cpp ) +# Image Recorder +ament_add_gtest( + test_image_recorder + ${TEST_DIR}/image_recorder.cpp + ${RMCS_SRC_DIR}/utility/image/recorder.cpp +) +target_link_libraries( + test_image_recorder + ${OpenCV_LIBRARIES} +) + # Solve pnp ament_add_gtest( test_solve_pnp @@ -120,4 +131,3 @@ ament_add_gtest( test_feishu ${TEST_DIR}/feishu_test.cpp ) - diff --git a/test/image_recorder.cpp b/test/image_recorder.cpp new file mode 100644 index 00000000..5846d974 --- /dev/null +++ b/test/image_recorder.cpp @@ -0,0 +1,77 @@ +#include "utility/image/recorder.hpp" + +#include +#include +#include +#include +#include + +#include + +#include + +namespace { + +auto list_recordings(const std::filesystem::path& directory) -> std::vector { + auto recordings = std::vector { }; + + if (!std::filesystem::exists(directory)) { + return recordings; + } + + for (const auto& entry : std::filesystem::directory_iterator { directory }) { + if (entry.is_regular_file() && entry.path().extension() == ".avi") { + recordings.push_back(entry.path()); + } + } + + return recordings; +} + +class ImageRecorderTest : public ::testing::Test { +protected: + auto SetUp() -> void override { + base_directory = std::filesystem::temp_directory_path() / "rmcs_image_recorder_test"; + std::filesystem::remove_all(base_directory); + std::filesystem::create_directories(base_directory); + } + + auto TearDown() -> void override { std::filesystem::remove_all(base_directory); } + + std::filesystem::path base_directory = { }; +}; + +TEST_F(ImageRecorderTest, saves_recording_when_duration_is_valid) { + auto recorder = rmcs::ImageRecorder { }; + recorder.set_saving_location(base_directory.string()); + recorder.set_framerate(60); + recorder.set_min_recording_duration(std::chrono::seconds { 1 }); + recorder.set_max_recording_duration(std::chrono::seconds { 5 }); + + auto frame = cv::Mat { 32, 32, CV_8UC3, cv::Scalar { 10, 20, 30 } }; + + recorder.write_frame(frame); + std::this_thread::sleep_for(std::chrono::milliseconds { 1100 }); + recorder.stop(); + + const auto recordings = list_recordings(base_directory); + ASSERT_EQ(recordings.size(), 1); + EXPECT_GT(std::filesystem::file_size(recordings.front()), 0); +} + +TEST_F(ImageRecorderTest, discards_recording_when_duration_is_too_short) { + auto recorder = rmcs::ImageRecorder { }; + recorder.set_saving_location(base_directory.string()); + recorder.set_framerate(60); + recorder.set_min_recording_duration(std::chrono::seconds { 2 }); + recorder.set_max_recording_duration(std::chrono::seconds { 5 }); + + auto frame = cv::Mat { 32, 32, CV_8UC3, cv::Scalar { 10, 20, 30 } }; + + recorder.write_frame(frame); + recorder.stop(); + + EXPECT_TRUE(list_recordings(base_directory).empty()); +} + +} // namespace diff --git a/tool/CMakeLists.txt b/tool/CMakeLists.txt index 0ee2e03d..3806997b 100644 --- a/tool/CMakeLists.txt +++ b/tool/CMakeLists.txt @@ -83,6 +83,7 @@ if(HIKCAMERA_AVAILABLE) add_executable( hikcamera ${TOOL_DIR}/hikcamera.cpp + ${RMCS_SRC_DIR}/utility/image/recorder.cpp ) target_link_libraries( hikcamera diff --git a/tool/hikcamera.cpp b/tool/hikcamera.cpp index 7ba1985d..4e5cf95f 100644 --- a/tool/hikcamera.cpp +++ b/tool/hikcamera.cpp @@ -1,94 +1,298 @@ +#include "module/debug/framerate.hpp" #include "util/snapshot.hpp" #include "util/terminal.hpp" -#include +#include "utility/image/recorder.hpp" #include #include +#include +#include #include +#include #include -#include #include +#include #include +#include +#include + +using Clock = std::chrono::steady_clock; +using TimePoint = Clock::time_point; + +namespace { + +struct Recording { + static constexpr auto kQueueSize = std::size_t { 120 }; + static constexpr auto kStatInterval = std::chrono::seconds { 2 }; + + rmcs::ImageRecorder recorder; + + std::mutex mutex = { }; + std::condition_variable cv = { }; + std::deque queue = { }; + std::jthread worker = { }; + + std::uint64_t written = 0; + std::uint64_t dropped = 0; + + TimePoint started_at = { }; + std::chrono::nanoseconds total_write_time = { }; + TimePoint stat_last = { }; + std::uint64_t stat_written = 0; + std::chrono::nanoseconds stat_write_time = { }; + + bool enabled = false; + bool stop_requested = false; + bool stop_pending = false; + + auto start(std::size_t framerate) -> void { + recorder.set_saving_location("/tmp/hikcamera_recordings"); + recorder.set_framerate(framerate); + recorder.set_auto_save(true); + recorder.set_max_history_count(10); + recorder.set_min_recording_duration(std::chrono::seconds { 0 }); + recorder.set_max_recording_duration(std::chrono::hours { 24 }); + + auto lock = std::lock_guard { mutex }; + started_at = Clock::now(); + written = 0; + dropped = 0; + total_write_time = std::chrono::nanoseconds { 0 }; + stat_last = started_at; + stat_written = 0; + stat_write_time = std::chrono::nanoseconds { 0 }; + enabled = true; + stop_pending = false; + + worker = std::jthread { [this] { + for (;;) { + auto frame = cv::Mat { }; + auto do_stop = false; + auto do_exit = false; + + { + auto lock = std::unique_lock { mutex }; + cv.wait(lock, [this] { return !enabled || !queue.empty() || stop_requested; }); + + if (!queue.empty()) { + frame = std::move(queue.front()); + queue.pop_front(); + } else if (stop_requested) { + stop_requested = false; + do_stop = true; + } else if (!enabled) { + do_exit = true; + } + } + + if (!frame.empty()) { + const auto t_before = Clock::now(); + recorder.write_frame(frame); + const auto t_after = Clock::now(); + + auto lock = std::lock_guard { mutex }; + written += 1; + stat_written += 1; + const auto elapsed = t_after - t_before; + total_write_time += elapsed; + stat_write_time += elapsed; + + if (t_after - stat_last >= kStatInterval) { + const auto fps = static_cast(stat_written) + / std::chrono::duration(t_after - stat_last).count(); + const auto avg_ms = stat_written > 0 + ? std::chrono::duration_cast(stat_write_time) + .count() + / stat_written + : 0; + + std::println("[recording] write fps={:.1f}, avg write latency={}ms, q={}", + fps, avg_ms, queue.size()); + stat_last = t_after; + stat_written = 0; + stat_write_time = std::chrono::nanoseconds { 0 }; + } + continue; + } + + if (do_stop) { + recorder.stop(); + auto lock = std::lock_guard { mutex }; + stop_pending = false; + cv.notify_all(); + continue; + } + + if (do_exit) break; + } + } }; + } + + auto request_stop() -> void { + auto lock = std::lock_guard { mutex }; + stop_requested = true; + stop_pending = true; + cv.notify_one(); + } + + auto wait_stop(std::string& saved_path, std::uint64_t& out_written, std::uint64_t& out_dropped, + std::chrono::nanoseconds& out_duration) -> void { + auto lock = std::unique_lock { mutex }; + cv.wait(lock, [this] { return !stop_pending; }); + saved_path = recorder.last_saved_path(); + out_written = written; + out_dropped = dropped; + out_duration = Clock::now() - started_at; + } + + auto shutdown() -> void { + enabled = false; + cv.notify_one(); + if (worker.joinable()) worker.join(); + } + + auto push(const cv::Mat& mat) -> void { + auto lock = std::lock_guard { mutex }; + if (queue.size() >= kQueueSize) { + queue.pop_front(); + dropped += 1; + } + queue.push_back(mat.clone()); + cv.notify_one(); + } +}; + +auto save_snapshot(const cv::Mat& src) -> void { + const auto path = rmcs::tool::util::build_snapshot_path(); + if (cv::imwrite(path, src)) { + std::println("[main] Saved image to {}", path); + } else { + std::println("[main] Failed to save image to {}", path); + } +} + +} // namespace + std::atomic running = true; auto main() -> int { std::signal(SIGINT, [](auto) { running = false; }); + auto framerate = rmcs::FramerateCounter { }; + framerate.set_interval(std::chrono::seconds { 2 }); + auto config = hikcamera::Config { .timeout_ms = 2'000, .exposure_us = 1'500, - // ... }; auto camera = hikcamera::Camera { }; camera.configure(config); - - if (auto result = camera.connect()) { + if (auto r = camera.connect()) { std::println("[hikcamera] Camera connect successfully"); } else { - std::println("[hikcamera] {}", result.error()); + std::println("[hikcamera] {}", r.error()); } - auto latest_image = cv::Mat { }; - auto image_mutex = std::mutex { }; + auto const recording_fps = static_cast( + std::max(1, static_cast(std::llround(config.framerate)))); + + auto latest = cv::Mat { }; + auto latest_mtx = std::mutex { }; + Recording recording; + auto rec_enabled = std::atomic { false }; - std::jthread capture_thread([&camera, &latest_image, &image_mutex] { - std::size_t count = 0; + auto capture_thread = std::jthread { [&] { while (running.load(std::memory_order::relaxed)) { if (!camera.connected()) { - if (auto ret = camera.connect(); !ret) { - std::println("[capture] Connected failed: {}", ret.error()); + if (auto r = camera.connect(); !r) { + std::println("[capture] Connect failed: {}", r.error()); std::this_thread::sleep_for(std::chrono::milliseconds(100)); continue; } } - if (auto mat = camera.read_image()) { - std::lock_guard lock(image_mutex); - latest_image = mat->clone(); - std::println("[capture] Read image {} ({}x{})", count++, mat->cols, mat->rows); + if (rec_enabled.load(std::memory_order::relaxed)) recording.push(*mat); + + { + auto lk = std::lock_guard { latest_mtx }; + latest = mat->clone(); + } + if (framerate.tick()) + std::println("[capture] capture framerate {}hz", framerate.fps()); } else { std::println("[capture] Failed to read image: {}", mat.error()); } } - }); + } }; - auto raw_mode = rmcs::tool::util::TerminalRawMode { }; - std::println("[main] Press S to save current frame, Q to exit, Ctrl+C to exit"); + auto _raw = rmcs::tool::util::TerminalRawMode { }; + std::println("[main] Hikcamera tool is ready"); + std::println("[main] Controls:"); + std::println("[main] S/s : save current frame to /tmp as PNG"); + std::println("[main] R/r : start / stop raw video recording"); + std::println("[main] Q/q : quit"); + std::println("[main] Ctrl+C : quit"); + std::println("[main] Recording output: /tmp/hikcamera_recordings"); + std::println("[main] Recording framerate: {} fps", recording_fps); while (running.load(std::memory_order::relaxed)) { if (auto key = rmcs::tool::util::poll_key(std::chrono::milliseconds(100)); key.has_value()) { if (*key == 'q' || *key == 'Q') { - std::println("[main] Quit requested by keyboard"); + std::println("[main] Quit"); running.store(false, std::memory_order::relaxed); break; } - if (*key != 's' && *key != 'S') { - continue; - } + if (*key == 'r' || *key == 'R') { + if (!rec_enabled.load(std::memory_order::relaxed)) { + rec_enabled.store(true, std::memory_order::relaxed); + recording.start(recording_fps); + std::println("[main] Recording started"); + } else { + if (recording.stop_pending) { + std::println("[main] Stop in progress, please wait"); + continue; + } + rec_enabled.store(false, std::memory_order::relaxed); + recording.request_stop(); + std::println("[main] Stopping..."); - auto snapshot = cv::Mat { }; - { - std::lock_guard lock(image_mutex); - if (!latest_image.empty()) { - snapshot = latest_image.clone(); - } - } + auto saved = std::string { }; + auto w = std::uint64_t { }, d = std::uint64_t { }; + auto dur = std::chrono::nanoseconds { }; + recording.wait_stop(saved, w, d, dur); - if (snapshot.empty()) { - std::println("[main] No image available yet, skip saving"); + if (saved.empty()) { + std::println("[main] Recording stopped, no file saved"); + } else { + std::println("[main] Saved: {}", saved); + std::println("[main] Stats: {}ms, written={}, dropped={}", + std::chrono::duration_cast(dur).count(), w, + d); + } + } continue; } - const auto path = rmcs::tool::util::build_snapshot_path(); - if (cv::imwrite(path, snapshot)) { - std::println("[main] Saved image to {}", path); - } else { - std::println("[main] Failed to save image to {}", path); + if (*key == 's' || *key == 'S') { + auto snap = cv::Mat { }; + { + auto lk = std::lock_guard { latest_mtx }; + if (!latest.empty()) snap = latest.clone(); + } + if (snap.empty()) { + std::println("[main] No image available"); + } else { + save_snapshot(snap); + } + continue; } } } + + rec_enabled.store(false, std::memory_order::relaxed); + recording.shutdown(); capture_thread.join(); } From f47656d9802f95a5e2fa5d3599708ea4d158fb18 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Mon, 27 Apr 2026 22:04:31 +0800 Subject: [PATCH 09/13] refactor: merge armor chooser improvements and clean up - Replace old debug recorder module with ImageRecorder utility - Add Arrow visual utility for aiming direction visualization - Refactor fire control and aim point chooser logic - Clean up runtime loop flow and error handling - Tidy visualization, streaming, and config defaults - Update AGENTS.md with agent collaboration guidelines --- AGENTS.md | 24 +++--- config/config.yaml | 40 ++++------ src/kernel/capturer.cpp | 16 ++-- src/kernel/fire_control.cpp | 23 +++--- src/kernel/fire_control.hpp | 1 - src/kernel/pose_estimator.cpp | 38 ++++----- src/kernel/visualization.cpp | 49 ++++++++---- src/kernel/visualization.hpp | 3 + src/module/debug/recorder.cpp | 0 src/module/debug/recorder.hpp | 15 ---- .../debug/visualization/armor_visualizer.cpp | 22 +++--- .../debug/visualization/armor_visualizer.hpp | 1 - .../debug/visualization/stream_session.cpp | 17 ++-- src/module/fire_control/aim_point_chooser.cpp | 11 ++- src/module/fire_control/aim_point_chooser.hpp | 5 +- src/runtime.cpp | 30 +++---- src/utility/image/image.cpp | 9 +++ src/utility/image/image.hpp | 2 + src/utility/rclcpp/visual/arrow.cpp | 78 +++++++++++++++++++ src/utility/rclcpp/visual/arrow.hpp | 45 +++++++++++ src/utility/shared/context.hpp | 17 ++++ 21 files changed, 295 insertions(+), 151 deletions(-) delete mode 100644 src/module/debug/recorder.cpp delete mode 100644 src/module/debug/recorder.hpp create mode 100644 src/utility/rclcpp/visual/arrow.cpp create mode 100644 src/utility/rclcpp/visual/arrow.hpp diff --git a/AGENTS.md b/AGENTS.md index 54bb8280..15ec6718 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -1,3 +1,12 @@ +## SOP + +项目构建方法： + +如果该项目位于 RMCS 的工作区下，可以用如下方法构建 +```zsh +zsh -lc "build-rmcs --packages-up-to rmcs_auto_aim_v2" +``` + ## Agent 协作开发规范（Auto Aim） 本文件用于约束 Agent 在本仓库中的行为，目标是： @@ -69,16 +78,7 @@ Agent 不应主导大规模算法功能实现。面对“完整实现大需求 目标是在错误发生时暴露真实原因，降低后期调试成本。 -## 6. 标准响应模板（高风险请求） - -当判定为高风险请求时，按以下结构回复： - -1. 风险判断：说明命中的高风险条件 -2. 不直接实现原因：说明可维护性/可验证性风险 -3. 推荐最小下一步：给出 1 个可执行且可验证的下一步 - -示例： +## 6. 代码风格 -- 风险判断：当前需求缺少明确实现边界和验收标准 -- 不直接实现原因：直接大范围生成会提高回归风险，难以定位问题 -- 推荐最小下一步：先确定模块边界与接口草案，我基于该草案实现第一阶段并附带测试 +- 头文件遵循 “本地头文件” “标准库” “第三方库” 的顺序排列，并在这些头文件加上空行用以区分，头文件较少时可以合并为一大块 +- RMCS_PIMPL_DEFINITION 这个宏应该放在 class/struct 的最顶上 diff --git a/config/config.yaml b/config/config.yaml index 483cff9f..53114c3a 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -1,13 +1,14 @@ use_visualization: true use_painted_image: true -is_local_runtime: false +is_local_runtime: true capturer: show_loss_framerate: false show_loss_framerate_interval: 500 reconnect_wait_interval: 100 - # hikcamera or local_video - source: "hikcamera" + + source: "local_video" + # source: "hikcamera" hikcamera: # int timeout_ms: 500 @@ -18,13 +19,13 @@ capturer: # float gain: 16.9807 - invert_image: true + invert_image: false software_sync: false trigger_mode: false - fixed_framerate: true + fixed_framerate: false local_video: # 替换为你具体的路径 - location: "/workspaces/alliance/test_videos/outpost.mp4" + location: "/workspaces/data/autoaim/1777297317784.avi" # double 帧率 frame_rate: 60 # bool 是否循环播放 @@ -36,11 +37,10 @@ identifier: binarization_threshold: 0.5 # openvino infer - avaliable_models: - - "tongji-yolov5.xml" - - "shenzhen-0526.onnx" - - "shenzhen-0708.onnx" model_location: "shenzhen-0526.onnx" + # model_location: "shenzhen-0708.onnx" + # model_location: "tongji-yolov5.xml" + infer_device: "AUTO" use_roi_segment: false roi_rows: 640 @@ -52,8 +52,8 @@ identifier: nms_threshold: 0.3 tracker: - # blue or red enemy_color: red + # enemy_color: blue max_temporary_loss_frames: 4 max_unconfirmed_loss_frames: 2 tracking_confirm_frames: 2 @@ -74,20 +74,6 @@ pose_estimator: distort_coeff: [-0.064232403853946, -0.087667493884102, 0, 0, 0.792381808294582] - transforms: - - parent: "imu_link" - child: "pitch_link" - t: [0., 0., 0.] - q: [1., 0., 0., 0.] - - parent: "pitch_link" - child: "muzzle_link" - t: [0., 0., 0.] - q: [1., 0., 0., 0.] - - parent: "pitch_link" - child: "camera_link" - t: [0., 0., 0.] - q: [1., 0., 0., 0.] - fire_control: initial_bullet_speed: 21.0 # m/s shoot_delay: 0.1 # s @@ -105,7 +91,7 @@ fire_control: auto_fire: true # 是否由自瞄控制射击 visualization: - framerate: 60 - monitor_host: "192.168.3.125" + framerate: 50 + monitor_host: "127.0.0.1" monitor_port: "5000" stream_type: "RTP_JEPG" diff --git a/src/kernel/capturer.cpp b/src/kernel/capturer.cpp index b545914a..d483b950 100644 --- a/src/kernel/capturer.cpp +++ b/src/kernel/capturer.cpp @@ -20,7 +20,7 @@ struct Capturer::Impl { std::unique_ptr interface; Printer log { "Capturer" }; - FramerateCounter loss_image_framerate {}; + FramerateCounter loss_image_framerate { }; std::chrono::milliseconds reconnect_wait_interval { 500 }; @@ -45,7 +45,7 @@ struct Capturer::Impl { instantitation_result = std::unexpected { result.error() }; return; } - instantitation_result = {}; + instantitation_result = { }; interface = std::move(instance); }; @@ -74,7 +74,7 @@ struct Capturer::Impl { runtime_thread = std::jthread { [this](const auto& t) { runtime_task(t); }, }; - return {}; + return { }; } catch (const std::exception& e) { return std::unexpected { e.what() }; @@ -87,10 +87,14 @@ struct Capturer::Impl { } } + // 为了实时性，一般取最新的帧 auto fetch_image() noexcept -> ImageUnique { - auto raw = RawImage { nullptr }; - capture_queue.pop(raw); - return std::unique_ptr { raw }; + auto result = ImageUnique { nullptr }; + auto image = RawImage { }; + while (capture_queue.pop(image)) { + result = ImageUnique { image }; + } + return result; } auto runtime_task(const std::stop_token& token) noexcept -> void { diff --git a/src/kernel/fire_control.cpp b/src/kernel/fire_control.cpp index 225a6ad2..81e57cb6 100644 --- a/src/kernel/fire_control.cpp +++ b/src/kernel/fire_control.cpp @@ -1,10 +1,4 @@ #include "fire_control.hpp" - -#include -#include -#include -#include - #include "module/fire_control/aim_point_chooser.hpp" #include "module/fire_control/shoot_evaluator.hpp" #include "module/fire_control/trajectory_solution.hpp" @@ -12,6 +6,11 @@ #include "utility/math/angle.hpp" #include "utility/serializable.hpp" +#include +#include +#include +#include + using namespace rmcs::kernel; using namespace rmcs::fire_control; @@ -79,7 +78,7 @@ struct FireControl::Impl { return std::unexpected { std::format( "shoot_evaluator init failed: {}", evaluate_result.error()) }; } - return {}; + return { }; } const int kMaxIterateCount { 5 }; @@ -87,7 +86,7 @@ struct FireControl::Impl { auto make_result(const Armor3D& armor, bool control, double current_yaw) -> std::optional { - auto armor_position_in_world = Eigen::Vector3d {}; + auto armor_position_in_world = Eigen::Vector3d { }; armor.translation.copy_to(armor_position_in_world); auto target_d = std::sqrt(armor_position_in_world.x() * armor_position_in_world.x() @@ -97,7 +96,7 @@ struct FireControl::Impl { return std::nullopt; } - auto solution = TrajectorySolution {}; + auto solution = TrajectorySolution { }; solution.input.v0 = config.initial_bullet_speed; solution.input.target_d = target_d; solution.input.target_h = target_h; @@ -138,7 +137,7 @@ struct FireControl::Impl { const double bullet_speed = config.initial_bullet_speed; auto current_fly_time = target_position_in_world.norm() / bullet_speed; - auto best_armor_opt = std::optional {}; + auto best_armor_opt = std::optional { }; for (int i = 0; i < kMaxIterateCount; ++i) { // 计算预测的时间点 = 子弹飞行时间 + 系统响应延迟 @@ -157,7 +156,7 @@ struct FireControl::Impl { } best_armor_opt = chosen_armor_opt; - auto armor_position_in_world = Eigen::Vector3d {}; + auto armor_position_in_world = Eigen::Vector3d { }; best_armor_opt->translation.copy_to(armor_position_in_world); auto target_d = std::sqrt(armor_position_in_world.x() * armor_position_in_world.x() @@ -166,7 +165,7 @@ struct FireControl::Impl { continue; } - auto solution = TrajectorySolution {}; + auto solution = TrajectorySolution { }; solution.input.v0 = bullet_speed; solution.input.target_d = target_d; solution.input.target_h = armor_position_in_world.z(); diff --git a/src/kernel/fire_control.hpp b/src/kernel/fire_control.hpp index efe88147..93999de9 100644 --- a/src/kernel/fire_control.hpp +++ b/src/kernel/fire_control.hpp @@ -4,7 +4,6 @@ #include #include "module/predictor/snapshot.hpp" -#include "utility/clock.hpp" #include "utility/pimpl.hpp" namespace rmcs::kernel { diff --git a/src/kernel/pose_estimator.cpp b/src/kernel/pose_estimator.cpp index 7042a35c..4f390c01 100644 --- a/src/kernel/pose_estimator.cpp +++ b/src/kernel/pose_estimator.cpp @@ -1,11 +1,9 @@ #include "pose_estimator.hpp" -#include "kernel/transform_tree.hpp" #include "utility/logging/printer.hpp" #include "utility/math/solve_pnp/pnp_solution.hpp" #include "utility/math/solve_pnp/solve_pnp.hpp" #include "utility/serializable.hpp" -#include "utility/yaml/tf.hpp" using namespace rmcs::util; using namespace rmcs; @@ -27,7 +25,7 @@ struct PoseEstimator::Impl { }; Config config; - PnpSolution pnp_solution {}; + PnpSolution pnp_solution { }; Eigen::Vector3d odom_to_camera_translation { Eigen::Vector3d::Zero() }; Eigen::Quaterniond odom_to_camera_orientation { Eigen::Quaterniond::Identity() }; @@ -39,22 +37,12 @@ struct PoseEstimator::Impl { if (!result.has_value()) { return std::unexpected { result.error() }; } - { - auto result = serialize_from(yaml["transforms"]); - if (!result.has_value() - && result.error() != SerializeTfError::UNMATCHED_LINKS_IN_TREE) { - return std::unexpected { std::string { "Failed to parse transforms | " } - + util::to_string(result.error()) }; - } - } - { - pnp_solution.input.camera_matrix = - reshape_array(config.camera_matrix); - pnp_solution.input.distort_coeff = - reshape_array(config.distort_coeff); - } - return {}; + pnp_solution.input.camera_matrix = + reshape_array(config.camera_matrix); + pnp_solution.input.distort_coeff = reshape_array(config.distort_coeff); + + return { }; } catch (const std::exception& e) { return std::unexpected { e.what() }; } @@ -70,7 +58,7 @@ struct PoseEstimator::Impl { } }; - auto armors_in_camera = std::vector {}; + auto armors_in_camera = std::vector { }; // TODO: YAW 角优化 std::ranges::for_each(armors | std::views::enumerate, @@ -89,7 +77,7 @@ struct PoseEstimator::Impl { return; } - auto armor_3d = Armor3D {}; + auto armor_3d = Armor3D { }; armor_3d.genre = pnp_solution.result.genre; armor_3d.color = camp_color2armor_color(pnp_solution.result.color); armor_3d.id = i; @@ -110,12 +98,12 @@ struct PoseEstimator::Impl { auto odom_to_camera(Armor3D const& armor) const -> Armor3D { auto transformed = armor; - auto position = Eigen::Vector3d {}; + auto position = Eigen::Vector3d { }; transformed.translation.copy_to(position); transformed.translation = odom_to_camera_orientation * position + odom_to_camera_translation; - auto quat = Eigen::Quaterniond {}; + auto quat = Eigen::Quaterniond { }; transformed.orientation.copy_to(quat); transformed.orientation = odom_to_camera_orientation * quat; @@ -123,18 +111,18 @@ struct PoseEstimator::Impl { } auto odom_to_camera(std::span armors) const -> std::vector { - auto result = std::vector {}; + auto result = std::vector { }; result.reserve(armors.size()); for (const auto& armor : armors) { auto transformed = armor; - auto position = Eigen::Vector3d {}; + auto position = Eigen::Vector3d { }; transformed.translation.copy_to(position); transformed.translation = odom_to_camera_orientation * position + odom_to_camera_translation; - auto quat = Eigen::Quaterniond {}; + auto quat = Eigen::Quaterniond { }; transformed.orientation.copy_to(quat); transformed.orientation = odom_to_camera_orientation * quat; diff --git a/src/kernel/visualization.cpp b/src/kernel/visualization.cpp index 1eb8c9eb..16350b61 100644 --- a/src/kernel/visualization.cpp +++ b/src/kernel/visualization.cpp @@ -6,6 +6,8 @@ #include "module/debug/visualization/stream_session.hpp" #include "utility/image/image.details.hpp" #include "utility/logging/printer.hpp" +#include "utility/math/conversion.hpp" +#include "utility/rclcpp/visual/arrow.hpp" #include "utility/serializable.hpp" using namespace rmcs::kernel; @@ -17,6 +19,9 @@ constexpr std::array kVideoTypes { }; struct Visualization::Impl { + static constexpr auto kCameraLink = "camera_link"; + static constexpr auto kOdomLink = "odom_imu_link"; + using SessionConfig = debug::StreamSession::Config; using NormalResult = std::expected; @@ -41,25 +46,26 @@ struct Visualization::Impl { }; }; - Printer log { "visualization" }; + Printer log { "visual" }; std::unique_ptr session; SessionConfig session_config; + std::unique_ptr armors_detect; + std::unique_ptr armors_group; + std::unique_ptr aiming_direction; + bool is_initialized = false; bool size_determined = false; - std::unique_ptr solved_pnp_visualizer; - std::unique_ptr predicted_visualizer; - Impl() noexcept { - session = std::make_unique(); - solved_pnp_visualizer = std::make_unique(); - predicted_visualizer = std::make_unique(); + session = std::make_unique(); + armors_detect = std::make_unique(); + armors_group = std::make_unique(); } auto initialize(const YAML::Node& yaml, RclcppNode& visual_node) noexcept -> NormalResult { - auto config = Config {}; + auto config = Config { }; auto result = config.serialize(yaml); if (!result.has_value()) { return std::unexpected { result.error() }; @@ -77,11 +83,16 @@ struct Visualization::Impl { return std::unexpected { "Unknown video type: " + config.stream_type }; } - solved_pnp_visualizer->initialize(visual_node); - predicted_visualizer->initialize(visual_node); + armors_detect->initialize(visual_node); + armors_group->initialize(visual_node); + aiming_direction = std::make_unique(visual::Arrow::Config { + .rclcpp = visual_node, + .name = "aiming_direction", + .tf = kOdomLink, + }); is_initialized = true; - return {}; + return { }; } auto initialized() const noexcept { return is_initialized; } @@ -130,12 +141,19 @@ struct Visualization::Impl { auto solved_pnp_armors(std::span armors) const -> bool { if (!is_initialized) return false; - return solved_pnp_visualizer->visualize(armors, "solved_pnp_armors", "camera_link"); + return armors_detect->visualize(armors, "solved_pnp_armors", kCameraLink); } auto predicted_armors(std::span armors) const -> bool { if (!is_initialized) return false; - return predicted_visualizer->visualize(armors, "predicted_armors", "odom_imu_link"); + return armors_group->visualize(armors, "predicted_armors", kOdomLink); + } + + auto update_aiming_direction(double yaw, double pitch) const -> void { + if (!is_initialized) return; + + aiming_direction->move(Translation { }, euler_to_quaternion(yaw, pitch, 0.0)); + aiming_direction->update(); } }; @@ -156,6 +174,11 @@ auto Visualization::solved_pnp_armors(std::span armors) const -> auto Visualization::predicted_armors(std::span armors) const -> bool { return pimpl->predicted_armors(armors); } + +auto Visualization::update_aiming_direction(double yaw, double pitch) const -> void { + pimpl->update_aiming_direction(yaw, pitch); +} + Visualization::Visualization() noexcept : pimpl { std::make_unique() } { } diff --git a/src/kernel/visualization.hpp b/src/kernel/visualization.hpp index 17c85809..d241ae48 100644 --- a/src/kernel/visualization.hpp +++ b/src/kernel/visualization.hpp @@ -28,6 +28,9 @@ class Visualization { auto solved_pnp_armors(std::span armors) const -> bool; auto predicted_armors(std::span armors) const -> bool; + + // 自瞄方向，其坐标系为 OdomImu + auto update_aiming_direction(double yaw, double pitch) const -> void; }; } diff --git a/src/module/debug/recorder.cpp b/src/module/debug/recorder.cpp deleted file mode 100644 index e69de29b..00000000 diff --git a/src/module/debug/recorder.hpp b/src/module/debug/recorder.hpp deleted file mode 100644 index dcd31f06..00000000 --- a/src/module/debug/recorder.hpp +++ /dev/null @@ -1,15 +0,0 @@ -#pragma once -#include "utility/image/image.hpp" - -#include -#include - -namespace rmcs::debug { - -class Recorder { - auto set_save_location(const std::string&) noexcept -> void; - - auto save(const Image&) noexcept -> std::expected; -}; - -} // namespace rmcs::debug diff --git a/src/module/debug/visualization/armor_visualizer.cpp b/src/module/debug/visualization/armor_visualizer.cpp index af3044f8..e036ff93 100644 --- a/src/module/debug/visualization/armor_visualizer.cpp +++ b/src/module/debug/visualization/armor_visualizer.cpp @@ -65,9 +65,9 @@ auto set_marker_color(Marker& marker, rmcs::CampColor camp) -> void { } auto make_marker(std::string_view frame_id, std::string_view ns, int id, int type, int action, - rmcs::DeviceId device, rmcs::CampColor camp, const rmcs::Armor3D* armor, rclcpp::Time stamp) - -> Marker { - auto marker = Marker {}; + rmcs::DeviceId device, rmcs::CampColor camp, const rmcs::Armor3D* armor, + const rclcpp::Time& stamp) -> Marker { + auto marker = Marker { }; marker.header.frame_id = frame_id; marker.header.stamp = stamp; marker.ns = std::string { ns }; @@ -107,8 +107,7 @@ struct ArmorVisualizer::Impl final { if (!rmcs::util::prefix::check_naming(name) || !rmcs::util::prefix::check_naming(link_name)) { - util::panic(std::format( - "Not a valid naming for armor name or tf: {}", + util::panic(std::format("Not a valid naming for armor name or tf: {}", rmcs::util::prefix::naming_standard)); } @@ -120,14 +119,14 @@ struct ArmorVisualizer::Impl final { previous_ids.clear(); } - auto visual_marker = MarkerArray {}; + auto visual_marker = MarkerArray { }; const auto current_time = rclcpp_clock.now(); - auto current_ids = std::unordered_set {}; + auto current_ids = std::unordered_set { }; auto const arrow_name = std::format("{}_arrow", name); current_ids.reserve(armors.size()); for (auto const& armor : armors) { - auto const camp = armor_color2camp_color(armor.color); + auto const camp = armor_color2camp_color(armor.color); auto const marker_id = make_unique_marker_id(armor.genre, armor.id); current_ids.emplace(marker_id); @@ -143,10 +142,9 @@ struct ArmorVisualizer::Impl final { } visual_marker.markers.emplace_back(make_marker(link_name, name, id, Marker::CUBE, - Marker::DELETE, rmcs::DeviceId {}, rmcs::CampColor {}, nullptr, current_time)); - visual_marker.markers.emplace_back(make_marker(link_name, arrow_name, id, - Marker::ARROW, Marker::DELETE, rmcs::DeviceId {}, rmcs::CampColor {}, nullptr, - current_time)); + Marker::DELETE, rmcs::DeviceId { }, rmcs::CampColor { }, nullptr, current_time)); + visual_marker.markers.emplace_back(make_marker(link_name, arrow_name, id, Marker::ARROW, + Marker::DELETE, rmcs::DeviceId { }, rmcs::CampColor { }, nullptr, current_time)); } previous_ids = std::move(current_ids); diff --git a/src/module/debug/visualization/armor_visualizer.hpp b/src/module/debug/visualization/armor_visualizer.hpp index 47c26805..48ffd0e8 100644 --- a/src/module/debug/visualization/armor_visualizer.hpp +++ b/src/module/debug/visualization/armor_visualizer.hpp @@ -7,7 +7,6 @@ namespace rmcs::debug { class ArmorVisualizer { - RMCS_PIMPL_DEFINITION(ArmorVisualizer) public: diff --git a/src/module/debug/visualization/stream_session.cpp b/src/module/debug/visualization/stream_session.cpp index d781182a..6b0374ed 100644 --- a/src/module/debug/visualization/stream_session.cpp +++ b/src/module/debug/visualization/stream_session.cpp @@ -56,13 +56,20 @@ struct StreamSession::Impl final { auto streaming_thread(const std::stop_token& token) noexcept -> void { notifier("Streaming thread starts"); + // 按照帧率循环，每次只取最新的 Frame + auto interval = std::chrono::nanoseconds { + static_cast(std::round(1.0 / context->video_format().hz * 1e9)), + }; + while (!token.stop_requested()) { + auto now = std::chrono::steady_clock::now(); - auto current_frame = cv::Mat {}; - if (buffer.pop(current_frame)) { - context->write(current_frame); + auto latest = cv::Mat { }; + while (buffer.pop(latest)) { } + if (!latest.empty()) { + context->write(latest); } - std::this_thread::yield(); + std::this_thread::sleep_until(now + interval); } notifier("Streaming thread stops"); } @@ -92,7 +99,7 @@ struct StreamSession::Impl final { static auto get_same_subnet_ipv4(std::string_view target_ip_str) -> std::expected { - auto target_addr = in_addr {}; + auto target_addr = in_addr { }; if (inet_pton(AF_INET, target_ip_str.data(), &target_addr) != 1) { return std::unexpected(std::format("Invalid target IP address: {}", target_ip_str)); } diff --git a/src/module/fire_control/aim_point_chooser.cpp b/src/module/fire_control/aim_point_chooser.cpp index 5d5d16db..1a30cd48 100644 --- a/src/module/fire_control/aim_point_chooser.cpp +++ b/src/module/fire_control/aim_point_chooser.cpp @@ -1,11 +1,10 @@ #include "aim_point_chooser.hpp" +#include "utility/math/conversion.hpp" #include #include #include -#include "utility/math/conversion.hpp" - using namespace rmcs::fire_control; struct AimPointChooser::Impl { @@ -23,7 +22,7 @@ struct AimPointChooser::Impl { AngleWindow outpost_window { util::deg2rad(70.0), util::deg2rad(30.0) }; // rad const double min_switch_improvement_angle { util::deg2rad(7.0) }; - std::optional last_chosen_armor_id {}; + std::optional last_chosen_armor_id { }; auto initialize(Config const& config) noexcept -> void { normal_fast_window = { config.coming_angle, config.leaving_angle }; @@ -44,7 +43,7 @@ struct AimPointChooser::Impl { auto candidate_evals = std::vector(armors.size()); const auto yaw = [&](size_t index) { - auto orientation = Eigen::Quaterniond {}; + auto orientation = Eigen::Quaterniond { }; armors[index].orientation.copy_to(orientation); return util::eulers(orientation)[0]; }; @@ -81,8 +80,8 @@ struct AimPointChooser::Impl { return std::tuple { 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 { }; { // 2) 最优筛选（仅角度窗口内）并定位上次目标 diff --git a/src/module/fire_control/aim_point_chooser.hpp b/src/module/fire_control/aim_point_chooser.hpp index dfd26c53..9ad4abb8 100644 --- a/src/module/fire_control/aim_point_chooser.hpp +++ b/src/module/fire_control/aim_point_chooser.hpp @@ -9,7 +9,10 @@ #include "utility/robot/armor.hpp" namespace rmcs::fire_control { + class AimPointChooser { + RMCS_PIMPL_DEFINITION(AimPointChooser) + public: struct Config { double coming_angle; // rad @@ -21,8 +24,6 @@ class AimPointChooser { auto choose_armor(std::span armors, Eigen::Vector3d const& center_position, double angular_velocity) -> std::optional; - - RMCS_PIMPL_DEFINITION(AimPointChooser) }; } // namespace rmcs::fire_control diff --git a/src/runtime.cpp b/src/runtime.cpp index 9ab04c0f..66870c62 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -113,9 +113,10 @@ auto main() -> int { if (!image) continue; if (framerate.tick()) { - node.info("Autoaim framerate: {}", framerate.fps()); + node.info("Autoaim Framerate: {}", framerate.fps()); } + // 结束流程后发送串流帧 [[maybe_unused]] auto _ = std::experimental::scope_exit { [&] { if (visualization.initialized()) { visualization.send_image(*image); @@ -130,20 +131,20 @@ auto main() -> int { /// 1. Identify Armor /// auto armors_2d = Armor2Ds { }; - auto result = identifier.sync_identify(*image); - if (!result.has_value()) { - logging.error("detection", "Armor detection failed"); - } else { - logging.reset("detection", 5); - - tracker.set_invincible_armors(received.invincible_devices); - auto filtered = tracker.filter_armors(*result); + { + auto result = identifier.sync_identify(*image); + if (!result.has_value()) { + logging.error("detection", "Something wrong happend in identifier"); + continue; // 一般不会推理出错喵~ + } if (use_painted_image) { - for (const auto& armor_2d : filtered) - util::draw(*image, armor_2d); + for (const auto& armor : *result) + util::draw(*image, armor); } + logging.reset("detection", 5); - armors_2d = std::move(filtered); + tracker.set_invincible_armors(received.invincible_devices); + armors_2d = tracker.filter_armors(*result); } /// 2. Transform 2d to 3d @@ -177,9 +178,10 @@ auto main() -> int { command.target = target_id; } + // 火控 if (target.allow_takeover && snapshot) { - if (auto result = - fire_control.solve(*snapshot, target.tracking_confirmed, received.yaw)) { + auto result = fire_control.solve(*snapshot, target.tracking_confirmed, received.yaw); + if (result) { command.shoot_permitted = result->shoot_permitted; command.yaw = result->yaw; command.pitch = result->pitch; diff --git a/src/utility/image/image.cpp b/src/utility/image/image.cpp index 8e816754..4493b63e 100644 --- a/src/utility/image/image.cpp +++ b/src/utility/image/image.cpp @@ -19,6 +19,15 @@ auto Image::set_timestamp(TimePoint timestamp) noexcept -> void // pimpl->timestamp = timestamp; } +auto Image::clone() const noexcept -> std::unique_ptr { + auto result = std::make_unique(); + + result->details().mat = pimpl->details.mat.clone(); + result->set_timestamp(pimpl->timestamp); + + return result; +} + Image::Image() noexcept : pimpl { std::make_unique() } { } diff --git a/src/utility/image/image.hpp b/src/utility/image/image.hpp index 40e6a782..de6dde5a 100644 --- a/src/utility/image/image.hpp +++ b/src/utility/image/image.hpp @@ -14,6 +14,8 @@ class Image { auto get_timestamp() const noexcept -> TimePoint; auto set_timestamp(TimePoint) noexcept -> void; + + auto clone() const noexcept -> std::unique_ptr; }; } diff --git a/src/utility/rclcpp/visual/arrow.cpp b/src/utility/rclcpp/visual/arrow.cpp new file mode 100644 index 00000000..adedd90e --- /dev/null +++ b/src/utility/rclcpp/visual/arrow.cpp @@ -0,0 +1,78 @@ +#include "arrow.hpp" + +#include "utility/panic.hpp" +#include "utility/rclcpp/node.details.hpp" + +#include + +using namespace rmcs::util::visual; + +using Marker = visualization_msgs::msg::Marker; + +struct Arrow::Impl { + static inline rclcpp::Clock rclcpp_clock { RCL_STEADY_TIME }; + + Config config; + + Marker marker; + std::shared_ptr> rclcpp_pub; + + explicit Impl(Config config) noexcept + : config(std::move(config)) { + initialize(); + } + + static auto create_rclcpp_publisher(Config const& config) noexcept + -> std::shared_ptr> { + const auto topic_name { config.rclcpp.get_pub_topic_prefix() + config.name }; + return config.rclcpp.details->make_pub(topic_name, qos::debug); + } + + auto initialize() noexcept -> void { + if (!prefix::check_naming(config.name) || !prefix::check_naming(config.tf)) { + util::panic(std::format( + "Not a valid naming for arrow name or tf: {}", prefix::naming_standard)); + } + + marker.header.frame_id = config.tf; + marker.ns = config.name; + marker.id = config.id; + marker.type = Marker::ARROW; + marker.action = Marker::ADD; + marker.lifetime = rclcpp::Duration::from_seconds(0.1); + + marker.scale.x = config.length; + marker.scale.y = config.width; + marker.scale.z = config.height; + + marker.color.r = config.r; + marker.color.g = config.g; + marker.color.b = config.b; + marker.color.a = config.a; + } + + auto update() noexcept -> void { + if (!rclcpp_pub) { + rclcpp_pub = create_rclcpp_publisher(config); + } + + marker.header.stamp = rclcpp_clock.now(); + rclcpp_pub->publish(marker); + } + + auto move(const Translation& t, const Orientation& q) noexcept -> void { + t.copy_to(marker.pose.position); + q.copy_to(marker.pose.orientation); + } +}; + +auto Arrow::update() noexcept -> void { pimpl->update(); } + +auto Arrow::impl_move(const Translation& t, const Orientation& q) noexcept -> void { + pimpl->move(t, q); +} + +Arrow::Arrow(const Config& config) noexcept + : pimpl { std::make_unique(config) } { } + +Arrow::~Arrow() noexcept = default; diff --git a/src/utility/rclcpp/visual/arrow.hpp b/src/utility/rclcpp/visual/arrow.hpp new file mode 100644 index 00000000..4f4b1cfd --- /dev/null +++ b/src/utility/rclcpp/visual/arrow.hpp @@ -0,0 +1,45 @@ +#pragma once + +#include "utility/rclcpp/node.hpp" +#include "utility/rclcpp/visual/movable.hpp" + +namespace rmcs::util::visual { + +struct Arrow : public Movable { + friend Movable; + +public: + struct Config { + RclcppNode& rclcpp; + + int id { 0 }; + std::string name { "arrow" }; + std::string tf { "odom_link" }; + + float r { 0. }; + float g { 1. }; + float b { 0. }; + float a { 1. }; + + double length { 0.2 }; + double width { 0.01 }; + double height { 0.01 }; + }; + + explicit Arrow(const Config&) noexcept; + + ~Arrow() noexcept; + + Arrow(const Arrow&) = delete; + Arrow& operator=(const Arrow&) = delete; + + auto update() noexcept -> void; + +private: + auto impl_move(const Translation&, const Orientation&) noexcept -> void; + + struct Impl; + std::unique_ptr pimpl; +}; + +} diff --git a/src/utility/shared/context.hpp b/src/utility/shared/context.hpp index df9fe02f..ff3f53ad 100644 --- a/src/utility/shared/context.hpp +++ b/src/utility/shared/context.hpp @@ -38,6 +38,12 @@ struct Transform { }, }; }; + static constexpr auto kIdentity() { + return Transform { + Translation { 0, 0, 0 }, + Orientation { 0, 0, 0, 1 }, + }; + } }; struct AutoAimState { @@ -101,6 +107,17 @@ struct ControlState { .invincible_devices = DeviceIds::None(), }; } + static auto kIdentity() { + return ControlState { + .timestamp = Clock::now(), + .shoot_mode = ShootMode::BATTLE, + .yaw = 0, + .pitch = 0, + .odom_to_camera_transform = Transform::kIdentity(), + .capture_signals = { }, + .invincible_devices = DeviceIds::None(), + }; + } }; static_assert(context_trait); From 09d8b01e604bff58b7eebf4982fa224e0bda7787 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Tue, 28 Apr 2026 19:28:41 +0800 Subject: [PATCH 10/13] wip: add yaw optimizer, clean code and remove shit generated by ai --- AGENTS.md | 50 ++----- config/config.yaml | 10 +- src/kernel/pose_estimator.cpp | 78 ++++++---- src/kernel/pose_estimator.hpp | 9 +- src/kernel/visualization.cpp | 2 +- .../debug/visualization/armor_visualizer.cpp | 52 +------ src/runtime.cpp | 17 ++- src/utility/math/camera.cpp | 34 +++++ src/utility/math/camera.hpp | 26 ++++ src/utility/math/conversion.hpp | 4 +- src/utility/math/linear.hpp | 135 +++++++++++++----- src/utility/math/point.hpp | 132 ----------------- src/utility/math/solve_pnp/pnp_solution.cpp | 6 +- src/utility/math/solve_pnp/pnp_solution.hpp | 6 +- src/utility/math/solve_pnp/yaw_optimizer.cpp | 85 +++++++++++ src/utility/math/solve_pnp/yaw_optimizer.hpp | 30 ++++ src/utility/rclcpp/visual/armor.cpp | 28 +--- src/utility/rclcpp/visual/movable.hpp | 4 +- src/utility/robot/armor.hpp | 41 +++++- test/CMakeLists.txt | 2 + test/solve_pnp.cpp | 6 +- 21 files changed, 420 insertions(+), 337 deletions(-) create mode 100644 src/utility/math/camera.cpp create mode 100644 src/utility/math/camera.hpp delete mode 100644 src/utility/math/point.hpp create mode 100644 src/utility/math/solve_pnp/yaw_optimizer.cpp create mode 100644 src/utility/math/solve_pnp/yaw_optimizer.hpp diff --git a/AGENTS.md b/AGENTS.md index 15ec6718..63846a21 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -1,12 +1,19 @@ ## SOP -项目构建方法： +1. 项目构建方法： 如果该项目位于 RMCS 的工作区下，可以用如下方法构建 ```zsh zsh -lc "build-rmcs --packages-up-to rmcs_auto_aim_v2" ``` +## 代码风格 + +- 头文件遵循 “本地头文件” “标准库” “第三方库” 的顺序排列，并在这些头文件加上空行用以区分，头文件较少时可以合并为一大块 +- RMCS_PIMPL_DEFINITION 这个宏应该放在 class/struct 的最顶上 +- 在局部作用域，优先 using namespace xxx 以简化要大量调用的同一命名空间下的函数或类 +- Solution 表示一个纯洁的算法，比如 pnp，统一定义在 utility/math/ 下，命名类似 XxxxSolution，XxxxOptimizer + ## Agent 协作开发规范（Auto Aim） 本文件用于约束 Agent 在本仓库中的行为，目标是： @@ -15,49 +22,23 @@ zsh -lc "build-rmcs --packages-up-to rmcs_auto_aim_v2" - 降低无边界代码生成带来的维护风险 - 让 Agent 在可控范围内稳定提供辅助价值 -## 1. 指令优先级与冲突处理 - -当多条指令冲突时，按以下优先级执行（高到低）： - -1. System 指令 -2. Developer 指令 -3. 本文档（AGENTS.md） -4. 用户普通实现请求 - -如发生冲突，必须遵循更高优先级指令，并在回复中简要说明原因。 - -## 2. 核心原则：算法开发由人类主导 +## 核心原则：算法开发由人类主导 Agent 不应主导大规模算法功能实现。面对“完整实现大需求”时，先进行风险判断。 -### 2.1 高风险判定（命中任意两条即视为高风险） +### 高风险判定（命中任意两条即视为高风险） - 缺少充分且可执行的实现计划（Plan） - 缺少开发者提供的明确框架/边界/修改目标 - 缺少验收标准、测试标准或回归范围 -### 2.2 高风险场景下的行为 +### 高风险场景下的行为 - 不直接生成大规模 Feature 代码 - 输出结构化建议：风险点、拆解方案、最小可验证下一步 - 将主导权交还开发者，等待进一步明确指令 -## 3. 允许与禁止的工作边界 - -### 3.1 允许（默认可执行） - -- 小范围修复（bug fix） -- 已有方案下的局部实现 -- 测试补充、文档整理、日志与可观测性改进 -- 不改变核心算法路径的重构 - -### 3.2 禁止（默认不执行） - -- 在缺少明确方案时主导完整算法功能开发 -- 未经约束地跨模块大改 -- 以“兜底默认值”掩盖配置或调用错误 - -## 4. 慎重提取辅助函数 +## 慎重提取辅助函数 提取公共辅助函数前必须评估： @@ -67,7 +48,7 @@ Agent 不应主导大规模算法功能实现。面对“完整实现大需求 若仅为单点使用或短期逻辑，优先保持局部实现。 -## 5. 关于非法条件检查与失败策略 +## 关于非法条件检查与失败策略 默认策略：尽早失败（fail fast），避免 silent fallback。 @@ -77,8 +58,3 @@ Agent 不应主导大规模算法功能实现。面对“完整实现大需求 - 仅在越界、索引失效、外部系统不稳定等场景增加必要检查 目标是在错误发生时暴露真实原因，降低后期调试成本。 - -## 6. 代码风格 - -- 头文件遵循 “本地头文件” “标准库” “第三方库” 的顺序排列，并在这些头文件加上空行用以区分，头文件较少时可以合并为一大块 -- RMCS_PIMPL_DEFINITION 这个宏应该放在 class/struct 的最顶上 diff --git a/config/config.yaml b/config/config.yaml index 53114c3a..ccc2821c 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -1,14 +1,14 @@ use_visualization: true use_painted_image: true -is_local_runtime: true +is_local_runtime: false capturer: show_loss_framerate: false show_loss_framerate_interval: 500 reconnect_wait_interval: 100 - source: "local_video" - # source: "hikcamera" + # source: "local_video" + source: "hikcamera" hikcamera: # int timeout_ms: 500 @@ -19,7 +19,7 @@ capturer: # float gain: 16.9807 - invert_image: false + invert_image: true software_sync: false trigger_mode: false fixed_framerate: false @@ -92,6 +92,6 @@ fire_control: visualization: framerate: 50 - monitor_host: "127.0.0.1" + monitor_host: "192.168.3.125" monitor_port: "5000" stream_type: "RTP_JEPG" diff --git a/src/kernel/pose_estimator.cpp b/src/kernel/pose_estimator.cpp index 4f390c01..68695842 100644 --- a/src/kernel/pose_estimator.cpp +++ b/src/kernel/pose_estimator.cpp @@ -1,8 +1,10 @@ #include "pose_estimator.hpp" #include "utility/logging/printer.hpp" +#include "utility/math/conversion.hpp" #include "utility/math/solve_pnp/pnp_solution.hpp" #include "utility/math/solve_pnp/solve_pnp.hpp" +#include "utility/math/solve_pnp/yaw_optimizer.hpp" #include "utility/serializable.hpp" using namespace rmcs::util; @@ -26,6 +28,7 @@ struct PoseEstimator::Impl { Config config; PnpSolution pnp_solution { }; + YawOptimizer yaw_optimizer { }; Eigen::Vector3d odom_to_camera_translation { Eigen::Vector3d::Zero() }; Eigen::Quaterniond odom_to_camera_orientation { Eigen::Quaterniond::Identity() }; @@ -38,9 +41,10 @@ struct PoseEstimator::Impl { return std::unexpected { result.error() }; } - pnp_solution.input.camera_matrix = + pnp_solution.input.camera.camera_matrix = reshape_array(config.camera_matrix); - pnp_solution.input.distort_coeff = reshape_array(config.distort_coeff); + pnp_solution.input.camera.distort_coeff = + reshape_array(config.distort_coeff); return { }; } catch (const std::exception& e) { @@ -58,36 +62,56 @@ struct PoseEstimator::Impl { } }; - auto armors_in_camera = std::vector { }; + auto result = std::vector { }; + + auto q_camera_to_odom = odom_to_camera_orientation; + auto q_odom_to_camera = q_camera_to_odom.inverse(); + auto center_yaw = eulers(q_camera_to_odom, 2, 1, 0)[0]; + + auto& input = yaw_optimizer.input; - // TODO: YAW 角优化 - std::ranges::for_each(armors | std::views::enumerate, - [&armors_in_camera, &armor_shape, this](auto const& item) { - auto [i, armor] = item; + input.camera = pnp_solution.input.camera; + input.camera.world_to_camera_orientation = Orientation { q_odom_to_camera }; + input.camera.world_to_camera_translation = Translation { Eigen::Vector3d { + -(q_odom_to_camera * odom_to_camera_translation).eval() } }; - pnp_solution.input.armor_shape = armor_shape(armor.shape); - pnp_solution.input.genre = armor.genre; - pnp_solution.input.color = armor_color2camp_color(armor.color); - std::ranges::copy(armor.corners(), pnp_solution.input.armor_detection.begin()); + for (auto&& [index, armor] : armors | std::views::enumerate) { + pnp_solution.input.armor_shape = armor_shape(armor.shape); + pnp_solution.input.genre = armor.genre; + pnp_solution.input.color = armor_color2camp_color(armor.color); + std::ranges::copy(armor.corners(), pnp_solution.input.armor_detection.begin()); + + auto solved = pnp_solution.solve(); + if (!solved) { + log.warn("solvePnP failed for armor {} ({} {})", index, get_enum_name(armor.genre), + get_enum_name(armor.color)); + continue; + } - auto solved = pnp_solution.solve(); - if (!solved) { - log.warn("solvePnP failed for armor {} ({} {})", i, get_enum_name(armor.genre), - get_enum_name(armor.color)); - return; - } + auto armor_3d = Armor3D { }; + armor_3d.genre = pnp_solution.result.genre; + armor_3d.color = camp_color2armor_color(pnp_solution.result.color); + armor_3d.id = static_cast(index); - auto armor_3d = Armor3D { }; - armor_3d.genre = pnp_solution.result.genre; - armor_3d.color = camp_color2armor_color(pnp_solution.result.color); - armor_3d.id = i; - pnp_solution.result.translation.copy_to(armor_3d.translation); - pnp_solution.result.orientation.copy_to(armor_3d.orientation); + armor_3d.translation = pnp_solution.result.translation; + armor_3d.orientation = pnp_solution.result.orientation; - armors_in_camera.emplace_back(armor_3d); - }); + auto t_in_camera = pnp_solution.result.translation.make(); + auto t_in_world = Eigen::Vector3d { + (q_camera_to_odom * t_in_camera + odom_to_camera_translation).eval() + }; - return armors_in_camera; + input.armor_shape = armor_shape(armor.shape); + input.xyz_in_world = Translation { t_in_world }; + input.center_yaw = center_yaw; + input.genre = armor.genre; + std::ranges::copy(armor.corners(), input.detected_corners.begin()); + + armor_3d.orientation = yaw_optimizer.solve().orientation; + + result.emplace_back(armor_3d); + }; + return result; } auto set_odom_to_camera_transform(Transform const& transform) -> void { @@ -143,7 +167,7 @@ auto PoseEstimator::solve_pnp(std::vector const& armors) const return pimpl->solve_pnp(armors); } -auto PoseEstimator::set_odom_to_camera_transform(Transform const& transform) -> void { +auto PoseEstimator::update_camera_transform(Transform const& transform) -> void { return pimpl->set_odom_to_camera_transform(transform); } diff --git a/src/kernel/pose_estimator.hpp b/src/kernel/pose_estimator.hpp index b38a7cca..ff3b6773 100644 --- a/src/kernel/pose_estimator.hpp +++ b/src/kernel/pose_estimator.hpp @@ -3,7 +3,6 @@ #include #include -#include "utility/math/linear.hpp" #include "utility/pimpl.hpp" #include "utility/rclcpp/node.hpp" #include "utility/robot/armor.hpp" @@ -18,16 +17,12 @@ class PoseEstimator { using RclcppNode = util::RclcppNode; auto initialize(const YAML::Node&) noexcept -> std::expected; - - auto visualize(RclcppNode& visual_node) -> void; + auto update_camera_transform(Transform const& transform) -> void; auto solve_pnp(std::vector const&) const -> std::optional>; - auto set_odom_to_camera_transform(Transform const& transform) -> void; - auto odom_to_camera(std::span armors) const -> std::vector; auto odom_to_camera(Armor3D const& armor) const -> Armor3D; - - auto update_imu_link(const Orientation&) noexcept -> void; }; + } diff --git a/src/kernel/visualization.cpp b/src/kernel/visualization.cpp index 16350b61..5cb44d61 100644 --- a/src/kernel/visualization.cpp +++ b/src/kernel/visualization.cpp @@ -152,7 +152,7 @@ struct Visualization::Impl { auto update_aiming_direction(double yaw, double pitch) const -> void { if (!is_initialized) return; - aiming_direction->move(Translation { }, euler_to_quaternion(yaw, pitch, 0.0)); + aiming_direction->move(Translation::kZero(), euler_to_quaternion(yaw, pitch, 0.0)); aiming_direction->update(); } }; diff --git a/src/module/debug/visualization/armor_visualizer.cpp b/src/module/debug/visualization/armor_visualizer.cpp index e036ff93..19803b61 100644 --- a/src/module/debug/visualization/armor_visualizer.cpp +++ b/src/module/debug/visualization/armor_visualizer.cpp @@ -26,44 +26,6 @@ auto make_unique_marker_id(rmcs::DeviceId device, int armor_index) -> int { return (device_index << kArmorIndexBitWidth) | armor_index; } -auto set_marker_scale(Marker& marker, rmcs::DeviceId device, bool is_arrow) -> void { - if (is_arrow) { - marker.scale.x = 0.2; - marker.scale.y = 0.01; - marker.scale.z = 0.01; - return; - } - - if (rmcs::DeviceIds::kSmallArmor().contains(device)) { - marker.scale.x = 0.003; - marker.scale.y = 0.140; - marker.scale.z = 0.125; - } else if (rmcs::DeviceIds::kLargeArmor().contains(device)) { - marker.scale.x = 0.003; - marker.scale.y = 0.235; - marker.scale.z = 0.127; - } -} - -auto set_marker_color(Marker& marker, rmcs::CampColor camp) -> void { - if (camp == rmcs::CampColor::RED) { - marker.color.r = 1.; - marker.color.g = 0.; - marker.color.b = 0.; - marker.color.a = 1.; - } else if (camp == rmcs::CampColor::BLUE) { - marker.color.r = 0.; - marker.color.g = 0.; - marker.color.b = 1.; - marker.color.a = 1.; - } else { - marker.color.r = 1.; - marker.color.g = 0.; - marker.color.b = 1.; - marker.color.a = 1.; - } -} - auto make_marker(std::string_view frame_id, std::string_view ns, int id, int type, int action, rmcs::DeviceId device, rmcs::CampColor camp, const rmcs::Armor3D* armor, const rclcpp::Time& stamp) -> Marker { @@ -77,12 +39,12 @@ auto make_marker(std::string_view frame_id, std::string_view ns, int id, int typ marker.lifetime = rclcpp::Duration::from_seconds(0.1); if (type == Marker::ARROW) { - set_marker_scale(marker, device, true); + marker.scale.x = 0.2, marker.scale.y = 0.01, marker.scale.z = 0.01; } else { - set_marker_scale(marker, device, false); + rmcs::ArmorVisualScale { device }.to(marker.scale); } - set_marker_color(marker, camp); + rmcs::ArmorVisualColor { camp }.to(marker.color); if (armor) { armor->translation.copy_to(marker.pose.position); @@ -119,10 +81,10 @@ struct ArmorVisualizer::Impl final { previous_ids.clear(); } - auto visual_marker = MarkerArray { }; - const auto current_time = rclcpp_clock.now(); - auto current_ids = std::unordered_set { }; - auto const arrow_name = std::format("{}_arrow", name); + auto visual_marker = MarkerArray { }; + auto current_time = rclcpp_clock.now(); + auto current_ids = std::unordered_set { }; + auto arrow_name = std::format("{}_arrow", name); current_ids.reserve(armors.size()); for (auto const& armor : armors) { diff --git a/src/runtime.cpp b/src/runtime.cpp index 66870c62..3f3d6e58 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -123,7 +123,7 @@ auto main() -> int { } } }; - auto received = ControlState::kInvalid(); + auto received = ControlState::kIdentity(); if (!without_rmcs && updated) { received = *feishu.latest(); } @@ -145,20 +145,19 @@ auto main() -> int { tracker.set_invincible_armors(received.invincible_devices); armors_2d = tracker.filter_armors(*result); + + if (armors_2d.empty()) continue; } /// 2. Transform 2d to 3d /// auto armors_3d = Armor3Ds { }; - if (!armors_2d.empty()) { - auto solved_armors_3d = pose_estimator.solve_pnp(armors_2d); - if (solved_armors_3d && visualization.initialized()) { - std::ignore = visualization.solved_pnp_armors(*solved_armors_3d); - } + if (auto result = pose_estimator.solve_pnp(armors_2d)) { + pose_estimator.update_camera_transform(received.odom_to_camera_transform); + armors_3d = pose_estimator.odom_to_camera(*result); - if (solved_armors_3d) { - pose_estimator.set_odom_to_camera_transform(received.odom_to_camera_transform); - armors_3d = pose_estimator.odom_to_camera(*solved_armors_3d); + if (visualization.initialized()) { + visualization.solved_pnp_armors(*result); } } diff --git a/src/utility/math/camera.cpp b/src/utility/math/camera.cpp new file mode 100644 index 00000000..767e82c1 --- /dev/null +++ b/src/utility/math/camera.cpp @@ -0,0 +1,34 @@ +#define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT + +#include "camera.hpp" + +#include +#include + +#include "utility/math/conversion.hpp" + +using namespace rmcs::util; + +auto CameraFeature::intrinsic() const -> cv::Mat { + return cv::Mat(3, 3, CV_64F, const_cast(camera_matrix[0].data())).clone(); +} + +auto CameraFeature::distortion() const -> cv::Mat { + return cv::Mat(1, 5, CV_64F, const_cast(distort_coeff.data())).clone(); +} + +auto CameraFeature::orientation() const -> cv::Mat { + auto q_ros = world_to_camera_orientation.make(); + auto r_ros = q_ros.toRotationMatrix(); + Eigen::Matrix3d r_ocv = ros2opencv_rotation(r_ros); + + cv::Mat result(3, 3, CV_64F); + cv::eigen2cv(r_ocv, result); + return result; +} + +auto CameraFeature::translation() const -> cv::Vec3d { + auto t_ros = world_to_camera_translation.make(); + Eigen::Vector3d t_ocv = ros2opencv_position(t_ros); + return { t_ocv[0], t_ocv[1], t_ocv[2] }; +} diff --git a/src/utility/math/camera.hpp b/src/utility/math/camera.hpp new file mode 100644 index 00000000..ff79609c --- /dev/null +++ b/src/utility/math/camera.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include +#include + +#include "utility/math/linear.hpp" + +namespace rmcs::util { + +struct CameraFeature { + // Row Major — camera intrinsic matrix (3×3) + std::array, 3> camera_matrix; + // Distortion coefficients (k1, k2, p1, p2, k3) + std::array distort_coeff; + // World-to-camera extrinsic transform (ROS convention) + // p_camera = quaternion * p_world + translation + Orientation world_to_camera_orientation { Orientation::kIdentity() }; + Translation world_to_camera_translation { Translation::kZero() }; + + auto orientation() const -> cv::Mat; + auto translation() const -> cv::Vec3d; + auto intrinsic() const -> cv::Mat; + auto distortion() const -> cv::Mat; +}; + +} // namespace rmcs::util diff --git a/src/utility/math/conversion.hpp b/src/utility/math/conversion.hpp index 249e4cba..02e0b3c1 100644 --- a/src/utility/math/conversion.hpp +++ b/src/utility/math/conversion.hpp @@ -69,7 +69,7 @@ inline auto xyz2ypd_jacobian(Eigen::Vector3d const& xyz) -> Eigen::Matrix3d { const auto ddistance_dy = y / norm; const auto ddistance_dz = z / norm; - auto J = Eigen::Matrix {}; + auto J = Eigen::Matrix { }; // clang-format off J<< dyaw_dx, dyaw_dy, dyaw_dz, dpitch_dx, dpitch_dy, dpitch_dz, @@ -103,7 +103,7 @@ inline auto eulers(Eigen::Quaterniond const& q, int axis0 = 2, int axis1 = 1, in } const auto n2 = a * a + b * b + c * c + d * d; - auto eulers = Eigen::Vector3d {}; + auto eulers = Eigen::Vector3d { }; eulers[1] = std::acos(2 * (a * a + b * b) / n2 - 1); const auto half_sum = std::atan2(b, a); diff --git a/src/utility/math/linear.hpp b/src/utility/math/linear.hpp index a8663b07..ffd032d4 100644 --- a/src/utility/math/linear.hpp +++ b/src/utility/math/linear.hpp @@ -4,90 +4,150 @@ namespace rmcs { template -concept translation_struct_trait = requires(T t) { +concept scalar2d_struct_trait = requires(T t) { + { t.x } -> std::convertible_to; + { t.y } -> std::convertible_to; +}; +template +concept scalar2d_object_trait = requires(T t) { + { t.x() } -> std::convertible_to; + { t.y() } -> std::convertible_to; +}; +template +concept scalar2d_trait = scalar2d_struct_trait || scalar2d_object_trait; + +template +concept scalar3d_struct_trait = requires(T t) { { t.x } -> std::convertible_to; { t.y } -> std::convertible_to; { t.z } -> std::convertible_to; }; template -concept translation_object_trait = requires(T t) { +concept scalar3d_object_trait = requires(T t) { { t.x() } -> std::convertible_to; { t.y() } -> std::convertible_to; { t.z() } -> std::convertible_to; }; template -concept translation_trait = translation_struct_trait || translation_object_trait; +concept scalar3d_trait = scalar3d_struct_trait || scalar3d_object_trait; template -concept orientation_struct_trait = requires(T t) { +concept scalar4d_struct_trait = requires(T t) { { t.x } -> std::convertible_to; { t.y } -> std::convertible_to; { t.z } -> std::convertible_to; { t.w } -> std::convertible_to; }; template -concept orientation_object_trait = requires(T t) { +concept scalar4d_object_trait = requires(T t) { { t.x() } -> std::convertible_to; { t.y() } -> std::convertible_to; { t.z() } -> std::convertible_to; { t.w() } -> std::convertible_to; }; template -concept orientation_trait = orientation_struct_trait || orientation_object_trait; +concept scalar4d_trait = scalar4d_struct_trait || scalar4d_object_trait; namespace linear::details { template - inline auto clone_translation(const Src& src, Dst& dst) noexcept { - if constexpr (translation_object_trait && translation_object_trait) { + inline auto clone_scalar2d(const Src& src, Dst& dst) noexcept { + if constexpr (scalar2d_object_trait && scalar2d_object_trait) { + dst.x() = src.x(); + dst.y() = src.y(); + } else if constexpr (scalar2d_struct_trait && scalar2d_struct_trait) { + dst.x = src.x; + dst.y = src.y; + } else if constexpr (scalar2d_object_trait && scalar2d_struct_trait) { + dst.x = src.x(); + dst.y = src.y(); + } else if constexpr (scalar2d_struct_trait && scalar2d_object_trait) { + dst.x() = src.x; + dst.y() = src.y; + } else { + static_assert(false, "clone_scalar2d: unsupported trait combination"); + } + return dst; + } + template + inline auto clone_scalar3d(const Src& src, Dst& dst) noexcept { + if constexpr (scalar3d_object_trait && scalar3d_object_trait) { dst.x() = src.x(); dst.y() = src.y(); dst.z() = src.z(); - } else if constexpr (translation_struct_trait && translation_struct_trait) { + } else if constexpr (scalar3d_struct_trait && scalar3d_struct_trait) { dst.x = src.x; dst.y = src.y; dst.z = src.z; - } else if constexpr (translation_object_trait && translation_struct_trait) { + } else if constexpr (scalar3d_object_trait && scalar3d_struct_trait) { dst.x = src.x(); dst.y = src.y(); dst.z = src.z(); - } else if constexpr (translation_struct_trait && translation_object_trait) { + } else if constexpr (scalar3d_struct_trait && scalar3d_object_trait) { dst.x() = src.x; dst.y() = src.y; dst.z() = src.z; } else { - static_assert(false, "clone_translation: unsupported trait combination"); + static_assert(false, "clone_scalar3d: unsupported trait combination"); } return dst; } template - inline auto clone_orientation(const Src& src, Dst& dst) noexcept { - if constexpr (orientation_object_trait && orientation_object_trait) { + inline auto clone_scalar4d(const Src& src, Dst& dst) noexcept { + if constexpr (scalar4d_object_trait && scalar4d_object_trait) { dst.x() = src.x(); dst.y() = src.y(); dst.z() = src.z(); dst.w() = src.w(); - } else if constexpr (orientation_struct_trait && orientation_struct_trait) { + } else if constexpr (scalar4d_struct_trait && scalar4d_struct_trait) { dst.x = src.x; dst.y = src.y; dst.z = src.z; dst.w = src.w; - } else if constexpr (orientation_object_trait && orientation_struct_trait) { + } else if constexpr (scalar4d_object_trait && scalar4d_struct_trait) { dst.x = src.x(); dst.y = src.y(); dst.z = src.z(); dst.w = src.w(); - } else if constexpr (orientation_struct_trait && orientation_object_trait) { + } else if constexpr (scalar4d_struct_trait && scalar4d_object_trait) { dst.x() = src.x; dst.y() = src.y; dst.z() = src.z; dst.w() = src.w; } else { - static_assert(false, "clone_orientation: unsupported trait combination"); + static_assert(false, "clone_scalar4d: unsupported trait combination"); } return dst; } } +struct Scalar2d { + double x = 0; + double y = 0; + + constexpr Scalar2d() noexcept = default; + constexpr explicit Scalar2d(double x, double y) noexcept + : x { x } + , y { y } { } + constexpr explicit Scalar2d(const scalar2d_trait auto& t) noexcept { + linear::details::clone_scalar2d(t, *this); + } + auto operator=(const scalar2d_trait auto& t) noexcept -> Scalar2d& { + linear::details::clone_scalar2d(t, *this); + return *this; + } + auto copy_to(scalar2d_trait auto& target) const noexcept -> void { + linear::details::clone_scalar2d(*this, target); + } + template + auto make() const -> T { + auto result = T { }; + return linear::details::clone_scalar2d(*this, result); + } + + static constexpr auto kZero() { return Scalar2d { 0, 0 }; } +}; +using Point2d = Scalar2d; + struct Scalar3d { double x = 0; double y = 0; @@ -98,24 +158,27 @@ struct Scalar3d { : x { x } , y { y } , z { z } { } - constexpr explicit Scalar3d(const translation_trait auto& t) noexcept { - linear::details::clone_translation(t, *this); + constexpr explicit Scalar3d(const scalar3d_trait auto& t) noexcept { + linear::details::clone_scalar3d(t, *this); } - auto operator=(const translation_trait auto& t) noexcept -> Scalar3d& { - linear::details::clone_translation(t, *this); + auto operator=(const scalar3d_trait auto& t) noexcept -> Scalar3d& { + linear::details::clone_scalar3d(t, *this); return *this; } - auto copy_to(translation_trait auto& target) const noexcept -> void { - linear::details::clone_translation(*this, target); + auto copy_to(scalar3d_trait auto& target) const noexcept -> void { + linear::details::clone_scalar3d(*this, target); } template auto make() const -> T { - auto result = T {}; - return linear::details::clone_translation(*this, result); + auto result = T { }; + return linear::details::clone_scalar3d(*this, result); } + + static constexpr auto kZero() { return Scalar3d { 0, 0, 0 }; } }; -using Translation = Scalar3d; using Vector3d = Scalar3d; +using Point3d = Scalar3d; +using Translation = Scalar3d; using Direction3d = Scalar3d; struct Orientation { @@ -130,21 +193,23 @@ struct Orientation { , y { y } , z { z } , w { w } { } - constexpr explicit Orientation(const orientation_trait auto& q) noexcept { - linear::details::clone_orientation(q, *this); + constexpr explicit Orientation(const scalar4d_trait auto& q) noexcept { + linear::details::clone_scalar4d(q, *this); } - auto operator=(const orientation_trait auto& q) noexcept -> Orientation& { - linear::details::clone_orientation(q, *this); + auto operator=(const scalar4d_trait auto& q) noexcept -> Orientation& { + linear::details::clone_scalar4d(q, *this); return *this; } - auto copy_to(orientation_trait auto& target) const noexcept -> void { - linear::details::clone_orientation(*this, target); + auto copy_to(scalar4d_trait auto& target) const noexcept -> void { + linear::details::clone_scalar4d(*this, target); } template auto make() const -> T { - auto result = T {}; - return linear::details::clone_orientation(*this, result); + auto result = T { }; + return linear::details::clone_scalar4d(*this, result); } + + static constexpr auto kIdentity() { return Orientation { 0, 0, 0, 1 }; } }; } diff --git a/src/utility/math/point.hpp b/src/utility/math/point.hpp deleted file mode 100644 index 35b1c0d9..00000000 --- a/src/utility/math/point.hpp +++ /dev/null @@ -1,132 +0,0 @@ -#pragma once -#include - -namespace rmcs { - -template -concept point2d_struct_trait = requires(T t) { - { t.x } -> std::convertible_to; - { t.y } -> std::convertible_to; -}; -template -concept point2d_object_trait = requires(T t) { - { t.x() } -> std::convertible_to; - { t.y() } -> std::convertible_to; -}; -template -concept point2d_trait = point2d_struct_trait || point2d_object_trait; - -template -concept point3d_struct_trait = requires(T t) { - { t.x } -> std::convertible_to; - { t.y } -> std::convertible_to; - { t.z } -> std::convertible_to; -}; -template -concept point3d_object_trait = requires(T t) { - { t.x() } -> std::convertible_to; - { t.y() } -> std::convertible_to; - { t.z() } -> std::convertible_to; -}; -template -concept point3d_trait = point3d_struct_trait || point3d_object_trait; - -namespace point::details { - template - inline auto clone_point2d(const Src& src, Dst& dst) noexcept -> Dst { - if constexpr (point2d_object_trait && point2d_object_trait) { - dst.x() = src.x(); - dst.y() = src.y(); - } else if constexpr (point2d_struct_trait && point2d_struct_trait) { - dst.x = src.x; - dst.y = src.y; - } else if constexpr (point2d_object_trait && point2d_struct_trait) { - dst.x = src.x(); - dst.y = src.y(); - } else if constexpr (point2d_struct_trait && point2d_object_trait) { - dst.x() = src.x; - dst.y() = src.y; - } else { - static_assert(false, "clone_point2d: unsupported trait combination"); - } - return dst; - } - - template - inline auto clone_point3d(const Src& src, Dst& dst) noexcept -> Dst { - if constexpr (point3d_object_trait && point3d_object_trait) { - dst.x() = src.x(); - dst.y() = src.y(); - dst.z() = src.z(); - } else if constexpr (point3d_struct_trait && point3d_struct_trait) { - dst.x = src.x; - dst.y = src.y; - dst.z = src.z; - } else if constexpr (point3d_object_trait && point3d_struct_trait) { - dst.x = src.x(); - dst.y = src.y(); - dst.z = src.z(); - } else if constexpr (point3d_struct_trait && point3d_object_trait) { - dst.x() = src.x; - dst.y() = src.y; - dst.z() = src.z; - } else { - static_assert(false, "clone_point3d: unsupported trait combination"); - } - return dst; - } -} - -struct Point2d { - double x = 0; - double y = 0; - - constexpr Point2d() noexcept = default; - constexpr Point2d(double x, double y) noexcept - : x { x } - , y { y } { } - constexpr explicit Point2d(const point2d_trait auto& p) noexcept { - point::details::clone_point2d(p, *this); - } - auto operator=(const point2d_trait auto& p) noexcept -> Point2d& { - point::details::clone_point2d(p, *this); - return *this; - } - auto copy_to(point2d_trait auto& target) const noexcept -> void { - point::details::clone_point2d(*this, target); - } - template - auto make() const -> T { - auto result = T {}; - return point::details::clone_point2d(*this, result); - } -}; - -struct Point3d { - double x = 0; - double y = 0; - double z = 0; - - constexpr Point3d() noexcept = default; - constexpr Point3d(double x, double y, double z) noexcept - : x { x } - , y { y } - , z { z } { } - constexpr explicit Point3d(const point3d_trait auto& p) noexcept { - point::details::clone_point3d(p, *this); - } - auto operator=(const point3d_trait auto& p) noexcept -> Point3d& { - point::details::clone_point3d(p, *this); - return *this; - } - auto copy_to(point3d_trait auto& target) const noexcept -> void { - point::details::clone_point3d(*this, target); - } - template - auto make() const -> T { - auto result = T {}; - return point::details::clone_point3d(*this, result); - } -}; - -} diff --git a/src/utility/math/solve_pnp/pnp_solution.cpp b/src/utility/math/solve_pnp/pnp_solution.cpp index bdfc416e..14d6437f 100644 --- a/src/utility/math/solve_pnp/pnp_solution.cpp +++ b/src/utility/math/solve_pnp/pnp_solution.cpp @@ -2,9 +2,9 @@ #include "pnp_solution.hpp" #include "utility/math/conversion.hpp" -#include "utility/math/solve_pnp/solve_pnp.hpp" #include +#include #include #include @@ -12,8 +12,8 @@ using namespace rmcs::util; auto PnpSolution::solve() -> bool { try { - const auto camera_matrix = cast_opencv_matrix(input.camera_matrix); - const auto distort_coeff = cast_opencv_matrix(input.distort_coeff); + const auto camera_matrix = input.camera.intrinsic(); + const auto distort_coeff = input.camera.distortion(); const auto armor_shape = std::ranges::to(input.armor_shape | std::views::transform( diff --git a/src/utility/math/solve_pnp/pnp_solution.hpp b/src/utility/math/solve_pnp/pnp_solution.hpp index 7019c54b..35c90e51 100644 --- a/src/utility/math/solve_pnp/pnp_solution.hpp +++ b/src/utility/math/solve_pnp/pnp_solution.hpp @@ -2,8 +2,8 @@ #include +#include "utility/math/camera.hpp" #include "utility/math/linear.hpp" -#include "utility/math/point.hpp" #include "utility/robot/color.hpp" #include "utility/robot/id.hpp" @@ -11,9 +11,7 @@ namespace rmcs::util { struct PnpSolution { struct Input { - // Row Major - std::array, 3> camera_matrix; - std::array distort_coeff; + CameraFeature camera; std::array armor_shape; std::array armor_detection; DeviceId genre; diff --git a/src/utility/math/solve_pnp/yaw_optimizer.cpp b/src/utility/math/solve_pnp/yaw_optimizer.cpp new file mode 100644 index 00000000..2e8c6a67 --- /dev/null +++ b/src/utility/math/solve_pnp/yaw_optimizer.cpp @@ -0,0 +1,85 @@ +#define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT + +#include "yaw_optimizer.hpp" + +#include +#include +#include + +#include +#include +#include + +#include "utility/math/angle.hpp" +#include "utility/math/conversion.hpp" + +using namespace rmcs::util; + +auto YawOptimizer::solve() -> Output { + constexpr double kSearchRangeDeg { 140.0 }; + constexpr double kSearchStepDeg { 1.0 }; + constexpr double kDefaultPitchDeg { 15.0 }; + constexpr double kOutpostPitchDeg { -15.0 }; + + auto const pitch = double { + (input.genre == DeviceId::OUTPOST) ? deg2rad(kOutpostPitchDeg) : deg2rad(kDefaultPitchDeg) }; + + auto const yaw_start = double { input.center_yaw - deg2rad(kSearchRangeDeg / 2.0) }; + + auto camera_intrinsic = input.camera.intrinsic(); + auto camera_distortion = input.camera.distortion(); + + auto q_wc_ros = input.camera.world_to_camera_orientation.make(); + auto r_wc_ros = q_wc_ros.toRotationMatrix(); + auto t_wc_ros = input.camera.world_to_camera_translation.make(); + auto xyz_w = input.xyz_in_world.make(); + + auto armor_shape_ocv = std::vector {}; + armor_shape_ocv.reserve(4); + for (const auto& pt : input.armor_shape) + armor_shape_ocv.emplace_back(pt.x, pt.y, pt.z); + + auto detected_ocv = std::vector {}; + detected_ocv.reserve(4); + for (const auto& pt : input.detected_corners) + detected_ocv.emplace_back(pt.x, pt.y); + + auto best_error = double { std::numeric_limits::max() }; + auto best_yaw = double { input.center_yaw }; + + for (auto i = int {}; i < static_cast(kSearchRangeDeg); ++i) { + auto candidate_yaw = double { yaw_start + i * deg2rad(kSearchStepDeg) }; + + auto q_aw = Eigen::Quaterniond { euler_to_quaternion(candidate_yaw, pitch, 0.0) }; + auto r_aw_ros = Eigen::Matrix3d { q_aw.toRotationMatrix() }; + + auto r_ac_ocv = Eigen::Matrix3d { ros2opencv_rotation(r_wc_ros * r_aw_ros) }; + auto t_ac_ocv = Eigen::Vector3d { ros2opencv_position(r_wc_ros * xyz_w + t_wc_ros) }; + + auto r_ac_ocv_cv = cv::Mat {}; + cv::eigen2cv(r_ac_ocv, r_ac_ocv_cv); + + auto rvec = cv::Vec3d {}; + cv::Rodrigues(r_ac_ocv_cv, rvec); + auto tvec = cv::Vec3d { t_ac_ocv[0], t_ac_ocv[1], t_ac_ocv[2] }; + + auto projected = std::vector {}; + cv::projectPoints( + armor_shape_ocv, rvec, tvec, camera_intrinsic, camera_distortion, projected); + + auto error = double { 0.0 }; + for (auto j = int {}; j < 4; ++j) + error += cv::norm(detected_ocv[j] - projected[j]); + + if (error < best_error) { + best_error = error; + best_yaw = candidate_yaw; + } + } + + auto q_aw_best = euler_to_quaternion(best_yaw, pitch, 0.0); + auto r_aw_best_ros = q_aw_best.toRotationMatrix(); + auto r_ac_ros = r_wc_ros * r_aw_best_ros; + + return { Orientation { Eigen::Quaterniond(r_ac_ros).normalized() } }; +} diff --git a/src/utility/math/solve_pnp/yaw_optimizer.hpp b/src/utility/math/solve_pnp/yaw_optimizer.hpp new file mode 100644 index 00000000..d1d475ef --- /dev/null +++ b/src/utility/math/solve_pnp/yaw_optimizer.hpp @@ -0,0 +1,30 @@ +#pragma once + +#include + +#include "utility/math/camera.hpp" +#include "utility/math/linear.hpp" +#include "utility/robot/id.hpp" + +namespace rmcs::util { + +struct YawOptimizer { + struct Input { + CameraFeature camera; + std::array armor_shape; + std::array detected_corners; + Translation xyz_in_world; + double center_yaw { 0.0 }; + DeviceId genre { DeviceId::UNKNOWN }; + } input; + + struct Output { + Orientation orientation; + }; + + YawOptimizer() noexcept = default; + + auto solve() -> Output; +}; + +} // namespace rmcs::util diff --git a/src/utility/rclcpp/visual/armor.cpp b/src/utility/rclcpp/visual/armor.cpp index 9cba3d0c..cad57a82 100644 --- a/src/utility/rclcpp/visual/armor.cpp +++ b/src/utility/rclcpp/visual/armor.cpp @@ -1,6 +1,7 @@ #include "armor.hpp" #include "utility/panic.hpp" #include "utility/rclcpp/node.details.hpp" +#include "utility/robot/armor.hpp" #include @@ -42,20 +43,8 @@ struct Armor::Impl { marker.action = Marker::ADD; marker.lifetime = rclcpp::Duration::from_seconds(0.1); - // ref: "https://www.robomaster.com/zh-CN/products/components/detail/149" - /* */ if (DeviceIds::kSmallArmor().contains(config.device)) { - marker.scale.x = 0.003, marker.scale.y = 0.140, marker.scale.z = 0.125; - } else if (DeviceIds::kLargeArmor().contains(config.device)) { - marker.scale.x = 0.003, marker.scale.y = 0.235, marker.scale.z = 0.127; - }; - - /* */ if (config.camp == CampColor::RED) { - marker.color.r = 1., marker.color.g = 0., marker.color.b = 0., marker.color.a = 1.; - } else if (config.camp == CampColor::BLUE) { - marker.color.r = 0., marker.color.g = 0., marker.color.b = 1., marker.color.a = 1.; - } else { - marker.color.r = 1., marker.color.g = 0., marker.color.b = 1., marker.color.a = 1.; - } + ArmorVisualScale { config.device }.to(marker.scale); + ArmorVisualColor { config.camp }.to(marker.color); arrow_marker.header.frame_id = config.tf; arrow_marker.ns = config.name + std::string("_arrow"); @@ -68,16 +57,7 @@ struct Armor::Impl { arrow_marker.scale.y = 0.01; arrow_marker.scale.z = 0.01; - /* */ if (config.camp == CampColor::RED) { - arrow_marker.color.r = 1., arrow_marker.color.g = 0., arrow_marker.color.b = 0., - arrow_marker.color.a = 1.; - } else if (config.camp == CampColor::BLUE) { - arrow_marker.color.r = 0., arrow_marker.color.g = 0., arrow_marker.color.b = 1., - arrow_marker.color.a = 1.; - } else { - arrow_marker.color.r = 1., arrow_marker.color.g = 0., arrow_marker.color.b = 1., - arrow_marker.color.a = 1.; - } + ArmorVisualColor { config.camp }.to(arrow_marker.color); } auto update() noexcept -> void { diff --git a/src/utility/rclcpp/visual/movable.hpp b/src/utility/rclcpp/visual/movable.hpp index d5af29e8..0e66a6f7 100644 --- a/src/utility/rclcpp/visual/movable.hpp +++ b/src/utility/rclcpp/visual/movable.hpp @@ -11,8 +11,8 @@ struct Movable { auto move(this auto& self, const std::tuple& tuple) noexcept { self.impl_move(std::get<0>(tuple), std::get<1>(tuple)); } - auto move(this auto& self, const translation_trait auto& t, - const orientation_trait auto& q) noexcept { + auto move(this auto& self, const scalar3d_trait auto& t, + const scalar4d_trait auto& q) noexcept { self.impl_move(Translation { t }, Orientation { q }); } }; diff --git a/src/utility/robot/armor.hpp b/src/utility/robot/armor.hpp index dbe1f495..9cb33e1b 100644 --- a/src/utility/robot/armor.hpp +++ b/src/utility/robot/armor.hpp @@ -1,6 +1,5 @@ #pragma once #include "utility/math/linear.hpp" -#include "utility/math/point.hpp" #include "utility/robot/color.hpp" #include "utility/robot/id.hpp" #include @@ -68,6 +67,46 @@ struct Armor3D { }; using Armor3Ds = std::vector; +struct ArmorVisualScale : public Scalar3d { + using Scalar3d::Scalar3d; + + // ref: "https://www.robomaster.com/zh-CN/products/components/detail/149" + constexpr explicit ArmorVisualScale(DeviceId device) noexcept { + if (DeviceIds::kSmallArmor().contains(device)) { + x = 0.003, y = 0.140, z = 0.125; + } else if (DeviceIds::kLargeArmor().contains(device)) { + x = 0.003, y = 0.235, z = 0.127; + } + } + + template + auto to(T& target) const noexcept -> void { + copy_to(target); + } +}; + +struct ArmorVisualColor : public Scalar3d { + using Scalar3d::Scalar3d; + + constexpr explicit ArmorVisualColor(CampColor camp) noexcept { + if (camp == CampColor::RED) { + x = 1.0, y = 0.0, z = 0.0; + } else if (camp == CampColor::BLUE) { + x = 0.0, y = 0.0, z = 1.0; + } else { + x = 1.0, y = 0.0, z = 1.0; + } + } + + template + auto to(T& target) const noexcept -> void { + target.r = x; + target.g = y; + target.b = z; + target.a = 1.0; + } +}; + constexpr auto kLightBarHeight = 0.056; constexpr auto kLargeArmorWidth = 0.23; constexpr auto kSmallArmorWidth = 0.135; diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index e87e9e4f..27adf10d 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -93,6 +93,8 @@ ament_add_gtest( test_solve_pnp ${TEST_DIR}/solve_pnp.cpp ${RMCS_SRC_DIR}/utility/math/solve_pnp/pnp_solution.cpp + ${RMCS_SRC_DIR}/utility/math/solve_pnp/yaw_optimizer.cpp + ${RMCS_SRC_DIR}/utility/math/camera.cpp ${RMCS_SRC_DIR}/module/identifier/armor_detection.cpp ${RMCS_SRC_DIR}/utility/image/image.cpp ) diff --git a/test/solve_pnp.cpp b/test/solve_pnp.cpp index 464ddc1d..437ecc8a 100644 --- a/test/solve_pnp.cpp +++ b/test/solve_pnp.cpp @@ -15,7 +15,7 @@ #include "assets_manager.hpp" #include "module/identifier/armor_detection.hpp" #include "utility/image/image.details.hpp" -#include "utility/math/point.hpp" +#include "utility/math/linear.hpp" #include "utility/math/solve_pnp/pnp_solution.hpp" #include "utility/robot/armor.hpp" @@ -61,12 +61,12 @@ PnpSolution::Input create_test_input(double fx = 1.722231837421459e+03, auto distort_coeff = std::array { k1, k2, 0, 0, k3 }; PnpSolution::Input input {}; - input.camera_matrix = { { + input.camera.camera_matrix = { { { fx, 0.0, cx }, { 0.0, fy, cy }, { 0.0, 0.0, 1.0 }, } }; - input.distort_coeff = distort_coeff; + input.camera.distort_coeff = distort_coeff; return input; } From 480cb80ef41c8c50b3d04c37c11dd145984ca315 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Tue, 28 Apr 2026 23:12:31 +0800 Subject: [PATCH 11/13] refactor: stabilize auto-aim tracking chain MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Add yaw optimizer with parabolic interpolation for sub-degree stability - Add innovation gate and radius rollback to prevent EKF divergence - Add confirmed→unconfirmed hysteresis in decider state machine - Fix transform timing: update camera pose before PnP to eliminate 1-frame latency - Hold last valid command on heartbeat loss to prevent gimbal recoil - Rename gimbal_takeover/shoot_permitted to should_control/should_shoot --- config/config.yaml | 1 + src/component.cpp | 97 ++++++------- .../predictor/regular/ekf_parameter.hpp | 2 +- src/module/predictor/regular/robot_state.cpp | 127 +++++++++++------- src/module/predictor/regular/robot_state.hpp | 16 +-- src/module/predictor/robot_state.cpp | 1 - src/module/predictor/robot_state.hpp | 9 +- src/module/tracker/decider.cpp | 48 ++++--- src/module/tracker/decider.hpp | 2 +- src/runtime.cpp | 58 ++++---- src/utility/math/kalman_filter/ekf.hpp | 1 + src/utility/math/solve_pnp/yaw_optimizer.cpp | 78 ++++++----- src/utility/shared/context.hpp | 16 +-- test/feishu_test.cpp | 12 +- 14 files changed, 263 insertions(+), 205 deletions(-) diff --git a/config/config.yaml b/config/config.yaml index ccc2821c..ed530dad 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -92,6 +92,7 @@ fire_control: visualization: framerate: 50 + # monitor_host: "localhost" monitor_host: "192.168.3.125" monitor_port: "5000" stream_type: "RTP_JEPG" diff --git a/src/component.cpp b/src/component.cpp index 19aed128..1370d385 100644 --- a/src/component.cpp +++ b/src/component.cpp @@ -23,10 +23,10 @@ class AutoAimComponent final : public rmcs_executor::Component { : adapter { *this } , rclcpp { get_component_name() } { - register_output("/gimbal/auto_aim/auto_aim_enabled", gimbal_takeover, false); + register_output("/gimbal/auto_aim/auto_aim_enabled", should_control, false); register_output( "/gimbal/auto_aim/control_direction", target_direction, Eigen::Vector3d::Zero()); - register_output("/gimbal/auto_aim/shoot_enable", shoot_permitted, false); + register_output("/gimbal/auto_aim/shoot_enable", should_shoot, false); using namespace std::chrono_literals; framerate.set_interval(2s); @@ -39,41 +39,45 @@ class AutoAimComponent final : public rmcs_executor::Component { }; visual_odom_to_camera = std::make_unique(config); - action_throttler.register_action("tf_not_ready"); - action_throttler.register_action("commit_control_state_failed"); + action_throttler.register_action("adapter"); + action_throttler.register_action("feishu"); } auto update() -> void override { if (!adapter.ready()) [[unlikely]] { - action_throttler.dispatch("tf_not_ready", [&] { rclcpp.warn("adapter is not ready"); }); - command = ControlState::kInvalid(); + action_throttler.dispatch("adapter", [&] { rclcpp.warn("adapter is not ready"); }); - const auto state = AutoAimState::kInvalid(); - *gimbal_takeover = state.gimbal_takeover; - *shoot_permitted = state.shoot_permitted; - *target_direction = compute_target_direction(state); + *should_control = false; + *should_shoot = false; + + feishu.send(ControlState::kInvalid()); return; } + action_throttler.reset("adapter"); + + feishu.send(make_context()); + action_throttler.reset("feishu"); - update_control_state(); - feishu.send(command); - action_throttler.reset("commit_control_state_failed"); + if (!feishu.heartbeat()) return; - if (feishu.heartbeat()) { - if (auto latest = feishu.latest()) { - context = *latest; - } - auto_aim_state_received_ = true; + auto command = *feishu.latest(); + if (Clock::now() - command.timestamp > kAutoAimTimeout) { + *should_control = false; + *should_shoot = false; } - const auto state = - auto_aim_state_received_ && Clock::now() - context.timestamp <= kAutoAimTimeout - ? context - : AutoAimState::kInvalid(); + *should_control = command.should_control; + *should_shoot = command.should_shoot; + + if (!*should_control) return; - *gimbal_takeover = state.gimbal_takeover; - *shoot_permitted = state.shoot_permitted; - *target_direction = compute_target_direction(state); + const auto pitch = command.pitch; + const auto yaw = command.yaw; + *target_direction = Eigen::Vector3d { + std::cos(pitch) * std::cos(yaw), + std::cos(pitch) * std::sin(yaw), + std::sin(pitch), + }; } private: @@ -88,45 +92,30 @@ class AutoAimComponent final : public rmcs_executor::Component { std::unique_ptr visual_odom_to_camera; Feishu feishu; - ControlState command; - AutoAimState context; - bool auto_aim_state_received_ { false }; - OutputInterface gimbal_takeover; - OutputInterface shoot_permitted; + OutputInterface should_control; + OutputInterface should_shoot; OutputInterface target_direction; FramerateCounter framerate; ActionThrottler action_throttler { std::chrono::seconds(1), 233 }; - static auto compute_target_direction(const AutoAimState& state) -> Eigen::Vector3d { - if (!state.gimbal_takeover || !std::isfinite(state.yaw) || !std::isfinite(state.pitch)) { - return Eigen::Vector3d::Zero(); - } - - const auto& [yaw, pitch] = std::tie(state.yaw, state.pitch); - - return { - std::cos(pitch) * std::cos(yaw), - std::cos(pitch) * std::sin(yaw), - std::sin(pitch), - }; - } - std::uint8_t publish_count = 0; - auto update_control_state() -> void { - command.timestamp = Clock::now(); + auto make_context() -> ControlState { + auto context = ControlState { }; + + context.timestamp = Clock::now(); auto dir = adapter.barrel_direction(); current_gimbal_yaw = std::atan2(dir.y(), dir.x()); current_gimbal_pitch = std::atan2(-dir.z(), std::hypot(dir.x(), dir.y())); auto iso = adapter.camera_transform(); - command.odom_to_camera_transform.position = iso.translation(); - command.odom_to_camera_transform.orientation = Eigen::Quaterniond(iso.rotation()); + context.odom_to_camera_transform.position = iso.translation(); + context.odom_to_camera_transform.orientation = Eigen::Quaterniond(iso.rotation()); - visual_odom_to_camera->move(command.odom_to_camera_transform.position, - command.odom_to_camera_transform.orientation); + visual_odom_to_camera->move(context.odom_to_camera_transform.position, + context.odom_to_camera_transform.orientation); if (publish_count++ > 100) { publish_count = 0; @@ -134,10 +123,12 @@ class AutoAimComponent final : public rmcs_executor::Component { } // TODO:无敌状态下的装甲板需要从裁判系统获取并在此更新 - command.invincible_devices = DeviceIds::None(); + context.invincible_devices = DeviceIds::None(); + + context.yaw = current_gimbal_yaw; + context.pitch = current_gimbal_pitch; - command.yaw = current_gimbal_yaw; - command.pitch = current_gimbal_pitch; + return context; } }; diff --git a/src/module/predictor/regular/ekf_parameter.hpp b/src/module/predictor/regular/ekf_parameter.hpp index 9b714466..494b288f 100644 --- a/src/module/predictor/regular/ekf_parameter.hpp +++ b/src/module/predictor/regular/ekf_parameter.hpp @@ -54,7 +54,7 @@ struct EKFParameters { if (device == DeviceId::BASE) { P_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1e-4, 0, 0; } else { - P_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; + P_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1e-2, 1e-2, 1e-2; } return P_dig; diff --git a/src/module/predictor/regular/robot_state.cpp b/src/module/predictor/regular/robot_state.cpp index 868d8439..36a93025 100644 --- a/src/module/predictor/regular/robot_state.cpp +++ b/src/module/predictor/regular/robot_state.cpp @@ -6,26 +6,44 @@ #include #include "module/predictor/regular/snapshot.hpp" +#include "utility/math/mahalanobis.hpp" #include "utility/time.hpp" using namespace rmcs::predictor; struct RegularRobotState::Impl { + struct MatchResult { int armor_id; double error; bool is_valid; }; + static constexpr auto kResetInterval = std::chrono::duration { 1.0 }; + static constexpr auto kAngleErrorThreshold = double { 0.35 }; + static constexpr auto kChi2Gate = double { 13.277 }; + + DeviceId device { DeviceId::UNKNOWN }; + CampColor color { CampColor::UNKNOWN }; + int armor_num { 0 }; + + EKF ekf { EKF { } }; + TimePoint time_stamp; + + bool initialized { false }; + int update_count { 0 }; + int nis_fail_count { 0 }; + explicit Impl(TimePoint stamp) noexcept : time_stamp { stamp } { } auto initialize(Armor3D const& armor, TimePoint t) -> void { - device = armor.genre; - color = armor_color2camp_color(armor.color); - armor_num = EKFParameters::armor_num(armor.genre); - time_stamp = t; - update_count = 0; + device = armor.genre; + color = armor_color2camp_color(armor.color); + armor_num = EKFParameters::armor_num(armor.genre); + time_stamp = t; + update_count = 0; + nis_fail_count = 0; ekf = EKF { EKFParameters::x(armor), EKFParameters::P_initial_dig(device).asDiagonal() }; initialized = true; } @@ -33,10 +51,11 @@ struct RegularRobotState::Impl { auto predict(TimePoint t) -> void { if (initialized) { auto dt = util::delta_time(t, time_stamp); - if (dt > reset_interval) { - initialized = false; - update_count = 0; - time_stamp = t; + if (dt > kResetInterval) { + initialized = false; + update_count = 0; + nis_fail_count = 0; + time_stamp = t; return; } @@ -95,25 +114,32 @@ struct RegularRobotState::Impl { auto const ypr_in_world = util::eulers(orientation); auto const ypd_in_world = util::xyz2ypd(xyz); - auto it = - std::ranges::min_element(armors_xyza, [&](auto const& a_xyza, auto const& b_xyza) { - auto get_error = [&](auto const& pred) { - auto ypd_pred = util::xyz2ypd(pred.template head<3>()); - return std::abs(util::normalize_angle(ypr_in_world[0] - pred[3])) - + std::abs(util::normalize_angle(ypd_in_world[0] - ypd_pred[0])); - }; - - return get_error(a_xyza) < get_error(b_xyza); - }); - - auto const best_id = static_cast(std::distance(armors_xyza.begin(), it)); - auto const min_error = [&] { - auto ypd_pred = util::xyz2ypd(it->template head<3>()); - return std::abs(util::normalize_angle(ypr_in_world[0] - (*it)[3])) + auto get_error = [&](auto const& pred) { + auto ypd_pred = util::xyz2ypd(pred.template head<3>()); + return std::abs(util::normalize_angle(ypr_in_world[0] - pred[3])) + std::abs(util::normalize_angle(ypd_in_world[0] - ypd_pred[0])); - }(); + }; + + auto ranked = std::vector> { }; + ranked.reserve(armors_xyza.size()); + for (int id = 0; id < static_cast(armors_xyza.size()); ++id) + ranked.emplace_back(id, (armors_xyza[id].template head<3>() - xyz).norm()); + std::ranges::sort(ranked, { }, &std::pair::second); + + auto const kMaxCandidates { 3 }; + auto best_id = int { -1 }; + auto best_error = double { std::numeric_limits::max() }; + + for (auto id = 0; id < std::min(kMaxCandidates, static_cast(ranked.size())); ++id) { + auto candidate_id = ranked[id].first; + auto candidate_error = get_error(armors_xyza[candidate_id]); + if (candidate_error < best_error) { + best_error = candidate_error; + best_id = candidate_id; + } + } - return { best_id, min_error, min_error < angle_error_threshold }; + return { best_id, best_error, best_error < kAngleErrorThreshold }; } auto update_single(Armor3D const& armor) -> bool { @@ -134,17 +160,41 @@ struct RegularRobotState::Impl { auto const orientation = Eigen::Quaterniond { quat_w, quat_x, quat_y, quat_z }; auto const ypr = util::eulers(orientation); - auto z = EKF::ZVec { }; + auto id = match_result.armor_id; + auto z = EKF::ZVec { }; z << ypd[0], ypd[1], ypd[2], ypr[0]; + auto r = EKFParameters::R(xyz, ypr, ypd); + auto z_pred = EKFParameters::h(device, ekf.x, id, armor_num); + auto h_jac = EKFParameters::H(device, ekf.x, id, armor_num); + auto y = EKFParameters::z_subtract(z, z_pred); + auto s = h_jac * ekf.P() * h_jac.transpose() + r; + auto nis = util::mahalanobis_distance(y, s); + if (!nis.has_value() || *nis > kChi2Gate) { + ++nis_fail_count; + return false; + } + + auto x_pre = ekf.x; + auto P_pre = ekf.P(); + ekf.update( z, - [id = match_result.armor_id, this]( - EKF::XVec const& x) { return EKFParameters::h(device, x, id, armor_num); }, - [id = match_result.armor_id, this]( - EKF::XVec const& x) { return EKFParameters::H(device, x, id, armor_num); }, - EKFParameters::R(xyz, ypr, ypd), EKFParameters::x_add, EKFParameters::z_subtract); + [id, this](EKF::XVec const& x) { return EKFParameters::h(device, x, id, armor_num); }, + [id, this](EKF::XVec const& x) { return EKFParameters::H(device, x, id, armor_num); }, + r, EKFParameters::x_add, EKFParameters::z_subtract); + + auto const r_ok = ekf.x[8] > 0.05 && ekf.x[8] < 0.5; + auto const l_ok = + (ekf.x[8] + ekf.x[9]) > 0.05 && (ekf.x[8] + ekf.x[9]) < 0.5; + if (!r_ok || !l_ok) { + ekf.x = x_pre; + ekf.P() = P_pre; + ++nis_fail_count; + return false; + } + nis_fail_count = 0; return true; } @@ -158,19 +208,6 @@ struct RegularRobotState::Impl { } return armors; } - - DeviceId device { DeviceId::UNKNOWN }; - CampColor color { CampColor::UNKNOWN }; - int armor_num { 0 }; - - EKF ekf { EKF { } }; - TimePoint time_stamp; - - bool initialized { false }; - int update_count { 0 }; - - const std::chrono::duration reset_interval { 1.0 }; - const double angle_error_threshold { 0.65 }; }; RegularRobotState::RegularRobotState() noexcept diff --git a/src/module/predictor/regular/robot_state.hpp b/src/module/predictor/regular/robot_state.hpp index 8e48defe..45f9973b 100644 --- a/src/module/predictor/regular/robot_state.hpp +++ b/src/module/predictor/regular/robot_state.hpp @@ -1,19 +1,22 @@ #pragma once - -#include -#include - #include "module/predictor/regular/ekf_parameter.hpp" #include "module/predictor/snapshot.hpp" #include "utility/pimpl.hpp" +#include + namespace rmcs::predictor { class RegularRobotState { + RMCS_PIMPL_DEFINITION(RegularRobotState) + public: using EKF = EKFParameters::EKF; explicit RegularRobotState(TimePoint stamp) noexcept; + RegularRobotState(RegularRobotState&&) noexcept; + + auto operator=(RegularRobotState&&) noexcept -> RegularRobotState&; auto initialize(Armor3D const& armor, TimePoint t) -> void; auto predict(TimePoint t) -> void; @@ -23,11 +26,6 @@ class RegularRobotState { auto is_converged() const -> bool; auto get_snapshot() const -> Snapshot; auto distance() const -> double; - - RMCS_PIMPL_DEFINITION(RegularRobotState) -public: - RegularRobotState(RegularRobotState&&) noexcept; - auto operator=(RegularRobotState&&) noexcept -> RegularRobotState&; }; } // namespace rmcs::predictor diff --git a/src/module/predictor/robot_state.cpp b/src/module/predictor/robot_state.cpp index 3a38fc7f..03e7be23 100644 --- a/src/module/predictor/robot_state.cpp +++ b/src/module/predictor/robot_state.cpp @@ -1,5 +1,4 @@ #include "robot_state.hpp" - #include "module/predictor/backend/robot_state_backend.hpp" using namespace rmcs::predictor; diff --git a/src/module/predictor/robot_state.hpp b/src/module/predictor/robot_state.hpp index dd727a10..f20ed0c6 100644 --- a/src/module/predictor/robot_state.hpp +++ b/src/module/predictor/robot_state.hpp @@ -1,15 +1,15 @@ #pragma once - -#include -#include - #include "module/predictor/snapshot.hpp" #include "utility/clock.hpp" #include "utility/pimpl.hpp" +#include + namespace rmcs::predictor { + struct RobotState { RMCS_PIMPL_DEFINITION(RobotState) + public: auto initialize(Armor3D const&, TimePoint) -> void; @@ -23,4 +23,5 @@ struct RobotState { auto distance() const -> double; }; + } diff --git a/src/module/tracker/decider.cpp b/src/module/tracker/decider.cpp index fd441c24..c300f340 100644 --- a/src/module/tracker/decider.cpp +++ b/src/module/tracker/decider.cpp @@ -1,4 +1,7 @@ #include "decider.hpp" +#include "module/predictor/robot_state.hpp" +#include "utility/serializable.hpp" +#include "utility/time.hpp" #include #include @@ -7,22 +10,20 @@ #include #include -#include "module/predictor/robot_state.hpp" -#include "utility/serializable.hpp" -#include "utility/time.hpp" - using namespace rmcs::tracker; using namespace rmcs::predictor; using namespace std::chrono_literals; struct Decider::Impl { - static constexpr auto kDefaultCleanupInterval = 1s; + static constexpr auto kDefaultCleanupInterval = 1.0s; static constexpr auto kOutpostCleanupInterval = 1.5s; + static constexpr auto kReleaseAfterFrames = std::size_t { 3 }; struct TargetMemory { std::optional last_seen_time { }; std::size_t consecutive_missing_frames { 0 }; std::size_t consecutive_stable_frames { 0 }; + std::size_t consecutive_instable_frames { 0 }; bool temporary_lost_armed { false }; }; @@ -41,6 +42,15 @@ struct Decider::Impl { }; }; + static constexpr auto get_cleanup_interval(DeviceId device_id) { + switch (device_id) { + case DeviceId::OUTPOST: + return kOutpostCleanupInterval; + default: + return kDefaultCleanupInterval; + } + } + auto initialize(const YAML::Node& yaml) noexcept -> std::expected { auto result = config.serialize(yaml); if (!result.has_value()) { @@ -59,20 +69,11 @@ struct Decider::Impl { if (priority_mode.empty()) priority_mode = mode2; - return {}; + return { }; } auto set_priority_mode(PriorityMode const& mode) -> void { priority_mode = mode; } - static auto cleanup_interval_for(DeviceId device_id) -> std::chrono::duration { - switch (device_id) { - case DeviceId::OUTPOST: - return kOutpostCleanupInterval; - default: - return kDefaultCleanupInterval; - } - } - auto update(std::span armors, TimePoint t) -> Output { // 推进所有现有追踪器的时间轴 for (auto& [id, tracker] : trackers) { @@ -107,16 +108,25 @@ struct Decider::Impl { // 1. unconfirmed: 已有 tracker，但还没稳定到可接管； // 2. confirmed: 连续稳定若干帧后允许控制接管； // 3. temporary lost: confirmed 目标短暂丢失时，保留控制输出窗口。 + // 4. confirmed → unconfirmed: 需要连续不稳定 kReleaseAfterFrames 帧才释放 for (const auto& [id, tracker] : trackers) { auto& target_memory = target_memories[id]; auto was_tracking_confirmed = tracking_confirmed(id); if (observed_ids.contains(id) && tracker->is_converged()) { ++target_memory.consecutive_stable_frames; - target_memory.temporary_lost_armed = false; + target_memory.consecutive_instable_frames = 0; + target_memory.temporary_lost_armed = false; continue; } + if (observed_ids.contains(id)) { + ++target_memory.consecutive_instable_frames; + if (target_memory.consecutive_instable_frames < kReleaseAfterFrames) { + continue; + } + } + target_memory.consecutive_stable_frames = 0; if (!observed_ids.contains(id)) { if (was_tracking_confirmed) { @@ -130,7 +140,7 @@ struct Decider::Impl { std::erase_if(trackers, [&](const auto& item) { auto memory_it = target_memories.find(item.first); - auto cleanup_interval = cleanup_interval_for(item.first); + auto cleanup_interval = get_cleanup_interval(item.first); bool expired = memory_it == target_memories.end() || !memory_it->second.last_seen_time || util::delta_time(t, *memory_it->second.last_seen_time) > cleanup_interval; if (expired) { @@ -157,7 +167,7 @@ struct Decider::Impl { return Output { .target_id = DeviceId::UNKNOWN, .snapshot = std::nullopt, - .allow_takeover = false, + .allow_control = false, .tracking_confirmed = false, }; } @@ -190,7 +200,7 @@ struct Decider::Impl { return Output { .target_id = device_id, .snapshot = trackers.at(device_id)->get_snapshot(), - .allow_takeover = allow_takeover, + .allow_control = allow_takeover, .tracking_confirmed = confirmed, }; } diff --git a/src/module/tracker/decider.hpp b/src/module/tracker/decider.hpp index 23d6f846..02508cfd 100644 --- a/src/module/tracker/decider.hpp +++ b/src/module/tracker/decider.hpp @@ -21,7 +21,7 @@ struct Decider { struct Output { DeviceId target_id; std::optional snapshot; - bool allow_takeover { false }; + bool allow_control { false }; bool tracking_confirmed { false }; }; diff --git a/src/runtime.cpp b/src/runtime.cpp index 3f3d6e58..561be6a0 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -123,11 +123,14 @@ auto main() -> int { } } }; - auto received = ControlState::kIdentity(); - if (!without_rmcs && updated) { - received = *feishu.latest(); + auto received = ControlState::kInvalid(); + if (without_rmcs) { + received = ControlState::kIdentity(); } + if (!updated) continue; + received = *feishu.latest(); + /// 1. Identify Armor /// auto armors_2d = Armor2Ds { }; @@ -152,49 +155,50 @@ auto main() -> int { /// 2. Transform 2d to 3d /// auto armors_3d = Armor3Ds { }; + pose_estimator.update_camera_transform(received.odom_to_camera_transform); if (auto result = pose_estimator.solve_pnp(armors_2d)) { - pose_estimator.update_camera_transform(received.odom_to_camera_transform); armors_3d = pose_estimator.odom_to_camera(*result); if (visualization.initialized()) { visualization.solved_pnp_armors(*result); } + + if (armors_3d.empty()) continue; } /// 3. Apply Tracker /// auto target = tracker.decide(armors_3d, image->get_timestamp()); auto target_id = target.target_id; - auto snapshot = std::move(target.snapshot); auto command = AutoAimState::kInvalid(); - if (target.allow_takeover) { - command.timestamp = Clock::now(); - command.gimbal_takeover = true; - command.shoot_permitted = false; - command.yaw = received.yaw; - command.pitch = received.pitch; - command.target = target_id; - } + if (auto& snapshot = target.snapshot) { + command.timestamp = Clock::now(); + command.should_control = true; + command.should_shoot = false; + command.yaw = received.yaw; + command.pitch = received.pitch; + command.target = target_id; + + if (target.allow_control) { + const auto control = target.tracking_confirmed; + const auto yaw = received.yaw; + if (auto result = fire_control.solve(*snapshot, control, yaw)) { + command.should_shoot = result->shoot_permitted; + command.yaw = result->yaw; + command.pitch = result->pitch; + } + } - // 火控 - if (target.allow_takeover && snapshot) { - auto result = fire_control.solve(*snapshot, target.tracking_confirmed, received.yaw); - if (result) { - command.shoot_permitted = result->shoot_permitted; - command.yaw = result->yaw; - command.pitch = result->pitch; + if (visualization.initialized()) { + visualization.predicted_armors(snapshot->predicted_armors(Clock::now())); } - } - if (visualization.initialized() && snapshot) { - visualization.predicted_armors(snapshot->predicted_armors(Clock::now())); + /// 4. Transmit State + /// + feishu.send(command); } - /// 4. Transmit State - /// - feishu.send(command); - } // runtime loop scope node.shutdown(); diff --git a/src/utility/math/kalman_filter/ekf.hpp b/src/utility/math/kalman_filter/ekf.hpp index 3fc137e4..e182193e 100644 --- a/src/utility/math/kalman_filter/ekf.hpp +++ b/src/utility/math/kalman_filter/ekf.hpp @@ -49,6 +49,7 @@ class EKF { : x(initial_x) , P_(initial_P) { } + auto P() -> PMat& { return P_; } auto P() const -> PMat const& { return P_; } /** diff --git a/src/utility/math/solve_pnp/yaw_optimizer.cpp b/src/utility/math/solve_pnp/yaw_optimizer.cpp index 2e8c6a67..880b85fa 100644 --- a/src/utility/math/solve_pnp/yaw_optimizer.cpp +++ b/src/utility/math/solve_pnp/yaw_optimizer.cpp @@ -1,6 +1,8 @@ #define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT #include "yaw_optimizer.hpp" +#include "utility/math/angle.hpp" +#include "utility/math/conversion.hpp" #include #include @@ -10,21 +12,45 @@ #include #include -#include "utility/math/angle.hpp" -#include "utility/math/conversion.hpp" - using namespace rmcs::util; +auto compute_reprojection_error(double yaw, double pitch, Eigen::Matrix3d const& r_wc_ros, + Eigen::Vector3d const& t_wc_ros, Eigen::Vector3d const& xyz_w, cv::Mat const& camera_intrinsic, + cv::Mat const& camera_distortion, std::vector const& armor_shape_ocv, + std::vector const& detected_ocv) -> double { + auto q_aw = Eigen::Quaterniond { euler_to_quaternion(yaw, pitch, 0.0) }; + auto r_aw_ros = Eigen::Matrix3d { q_aw.toRotationMatrix() }; + + auto r_ac_ocv = Eigen::Matrix3d { ros2opencv_rotation(r_wc_ros * r_aw_ros) }; + auto t_ac_ocv = Eigen::Vector3d { ros2opencv_position(r_wc_ros * xyz_w + t_wc_ros) }; + + auto r_ac_ocv_cv = cv::Mat { }; + cv::eigen2cv(r_ac_ocv, r_ac_ocv_cv); + + auto rvec = cv::Vec3d { }; + cv::Rodrigues(r_ac_ocv_cv, rvec); + auto tvec = cv::Vec3d { t_ac_ocv[0], t_ac_ocv[1], t_ac_ocv[2] }; + + auto projected = std::vector { }; + cv::projectPoints(armor_shape_ocv, rvec, tvec, camera_intrinsic, camera_distortion, projected); + + auto error = double { 0.0 }; + for (auto j = int { }; j < 4; ++j) + error += cv::norm(detected_ocv[j] - projected[j]); + + return error; +} + auto YawOptimizer::solve() -> Output { constexpr double kSearchRangeDeg { 140.0 }; constexpr double kSearchStepDeg { 1.0 }; constexpr double kDefaultPitchDeg { 15.0 }; constexpr double kOutpostPitchDeg { -15.0 }; - auto const pitch = double { - (input.genre == DeviceId::OUTPOST) ? deg2rad(kOutpostPitchDeg) : deg2rad(kDefaultPitchDeg) }; + const auto pitch = double { (input.genre == DeviceId::OUTPOST) ? deg2rad(kOutpostPitchDeg) + : deg2rad(kDefaultPitchDeg) }; - auto const yaw_start = double { input.center_yaw - deg2rad(kSearchRangeDeg / 2.0) }; + const auto yaw_start = double { input.center_yaw - deg2rad(kSearchRangeDeg / 2.0) }; auto camera_intrinsic = input.camera.intrinsic(); auto camera_distortion = input.camera.distortion(); @@ -34,12 +60,12 @@ auto YawOptimizer::solve() -> Output { auto t_wc_ros = input.camera.world_to_camera_translation.make(); auto xyz_w = input.xyz_in_world.make(); - auto armor_shape_ocv = std::vector {}; + auto armor_shape_ocv = std::vector { }; armor_shape_ocv.reserve(4); for (const auto& pt : input.armor_shape) armor_shape_ocv.emplace_back(pt.x, pt.y, pt.z); - auto detected_ocv = std::vector {}; + auto detected_ocv = std::vector { }; detected_ocv.reserve(4); for (const auto& pt : input.detected_corners) detected_ocv.emplace_back(pt.x, pt.y); @@ -47,29 +73,10 @@ auto YawOptimizer::solve() -> Output { auto best_error = double { std::numeric_limits::max() }; auto best_yaw = double { input.center_yaw }; - for (auto i = int {}; i < static_cast(kSearchRangeDeg); ++i) { + for (auto i = int { }; i < static_cast(kSearchRangeDeg); ++i) { auto candidate_yaw = double { yaw_start + i * deg2rad(kSearchStepDeg) }; - - auto q_aw = Eigen::Quaterniond { euler_to_quaternion(candidate_yaw, pitch, 0.0) }; - auto r_aw_ros = Eigen::Matrix3d { q_aw.toRotationMatrix() }; - - auto r_ac_ocv = Eigen::Matrix3d { ros2opencv_rotation(r_wc_ros * r_aw_ros) }; - auto t_ac_ocv = Eigen::Vector3d { ros2opencv_position(r_wc_ros * xyz_w + t_wc_ros) }; - - auto r_ac_ocv_cv = cv::Mat {}; - cv::eigen2cv(r_ac_ocv, r_ac_ocv_cv); - - auto rvec = cv::Vec3d {}; - cv::Rodrigues(r_ac_ocv_cv, rvec); - auto tvec = cv::Vec3d { t_ac_ocv[0], t_ac_ocv[1], t_ac_ocv[2] }; - - auto projected = std::vector {}; - cv::projectPoints( - armor_shape_ocv, rvec, tvec, camera_intrinsic, camera_distortion, projected); - - auto error = double { 0.0 }; - for (auto j = int {}; j < 4; ++j) - error += cv::norm(detected_ocv[j] - projected[j]); + auto error = compute_reprojection_error(candidate_yaw, pitch, r_wc_ros, t_wc_ros, xyz_w, + camera_intrinsic, camera_distortion, armor_shape_ocv, detected_ocv); if (error < best_error) { best_error = error; @@ -77,7 +84,16 @@ auto YawOptimizer::solve() -> Output { } } - auto q_aw_best = euler_to_quaternion(best_yaw, pitch, 0.0); + // 三点二次插值，帧间连续稳定 + auto step = double { deg2rad(kSearchStepDeg) }; + auto e_lo = compute_reprojection_error(best_yaw - step, pitch, r_wc_ros, t_wc_ros, xyz_w, + camera_intrinsic, camera_distortion, armor_shape_ocv, detected_ocv); + auto e_hi = compute_reprojection_error(best_yaw + step, pitch, r_wc_ros, t_wc_ros, xyz_w, + camera_intrinsic, camera_distortion, armor_shape_ocv, detected_ocv); + auto denominator = e_lo - 2.0 * best_error + e_hi + 1e-9; + auto refined = best_yaw + step * 0.5 * (e_lo - e_hi) / denominator; + + auto q_aw_best = euler_to_quaternion(refined, pitch, 0.0); auto r_aw_best_ros = q_aw_best.toRotationMatrix(); auto r_ac_ros = r_wc_ros * r_aw_best_ros; diff --git a/src/utility/shared/context.hpp b/src/utility/shared/context.hpp index ff3f53ad..32da01db 100644 --- a/src/utility/shared/context.hpp +++ b/src/utility/shared/context.hpp @@ -52,8 +52,8 @@ struct AutoAimState { TimePoint timestamp { }; - bool gimbal_takeover { false }; - bool shoot_permitted = { false }; + bool should_control { false }; + bool should_shoot = { false }; double yaw { std::numeric_limits::quiet_NaN() }; double pitch { std::numeric_limits::quiet_NaN() }; @@ -62,12 +62,12 @@ struct AutoAimState { static auto kInvalid() { return AutoAimState { - .timestamp = Clock::now(), - .gimbal_takeover = false, - .shoot_permitted = false, - .yaw = std::numeric_limits::quiet_NaN(), - .pitch = std::numeric_limits::quiet_NaN(), - .target = DeviceId::UNKNOWN, + .timestamp = Clock::now(), + .should_control = false, + .should_shoot = false, + .yaw = std::numeric_limits::quiet_NaN(), + .pitch = std::numeric_limits::quiet_NaN(), + .target = DeviceId::UNKNOWN, }; } }; diff --git a/test/feishu_test.cpp b/test/feishu_test.cpp index 19bc430d..afaa766f 100644 --- a/test/feishu_test.cpp +++ b/test/feishu_test.cpp @@ -33,10 +33,10 @@ TEST(FeishuIntegration, BidirectionalCommunication) { } ASSERT_TRUE(ctrl.has_value()); - auto auto_state = AutoAimState { }; - auto_state.gimbal_takeover = true; - auto_state.shoot_permitted = true; - auto_state.yaw = 1.23; + auto auto_state = AutoAimState { }; + auto_state.should_control = true; + auto_state.should_shoot = true; + auto_state.yaw = 1.23; feishu_child.send(auto_state); exit(0); @@ -63,8 +63,8 @@ TEST(FeishuIntegration, BidirectionalCommunication) { } ASSERT_TRUE(auto_state.has_value()); - EXPECT_TRUE(auto_state->gimbal_takeover); - EXPECT_TRUE(auto_state->shoot_permitted); + EXPECT_TRUE(auto_state->should_control); + EXPECT_TRUE(auto_state->should_shoot); EXPECT_DOUBLE_EQ(auto_state->yaw, 1.23); int status = 0; From d1b4bb9d468c052336a56ced3347b3ff404f338b Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Wed, 29 Apr 2026 07:52:51 +0800 Subject: [PATCH 12/13] chore: remove unused namespace comment --- src/adapter/sentry.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/adapter/sentry.hpp b/src/adapter/sentry.hpp index f1ad19f6..447be280 100644 --- a/src/adapter/sentry.hpp +++ b/src/adapter/sentry.hpp @@ -28,4 +28,4 @@ class Adapter { rmcs_executor::Component::InputInterface tf_; }; -} // namespace rmcs::adapter +} From b5c9ca4b394e58755a7fe623d67affc643089b26 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Thu, 30 Apr 2026 09:52:03 +0800 Subject: [PATCH 13/13] refactor: streamline runtime loop, rename visuals and context fields --- src/adapter/sentry.hpp | 2 - src/component.cpp | 32 +++-------- src/kernel/visualization.cpp | 37 +++++++++---- src/kernel/visualization.hpp | 12 +++-- src/module/predictor/snapshot.hpp | 8 +-- src/runtime.cpp | 88 ++++++++++++++----------------- src/utility/shared/context.hpp | 30 +++++------ 7 files changed, 99 insertions(+), 110 deletions(-) diff --git a/src/adapter/sentry.hpp b/src/adapter/sentry.hpp index 447be280..f7263996 100644 --- a/src/adapter/sentry.hpp +++ b/src/adapter/sentry.hpp @@ -8,8 +8,6 @@ namespace rmcs { class Adapter { public: - static constexpr const char* kParentFrame = "odom_imu_link"; - explicit Adapter(rmcs_executor::Component& component) { component.register_input("/tf", tf_); } [[nodiscard]] auto ready() const -> bool { return tf_.ready(); } diff --git a/src/component.cpp b/src/component.cpp index 1370d385..4ca7f7ee 100644 --- a/src/component.cpp +++ b/src/component.cpp @@ -3,7 +3,6 @@ #include "module/debug/action_throttler.hpp" #include "module/debug/framerate.hpp" #include "utility/rclcpp/node.hpp" -#include "utility/rclcpp/visual/transform.hpp" #include "utility/shared/context.hpp" #include @@ -23,22 +22,13 @@ class AutoAimComponent final : public rmcs_executor::Component { : adapter { *this } , rclcpp { get_component_name() } { - register_output("/gimbal/auto_aim/auto_aim_enabled", should_control, false); - register_output( - "/gimbal/auto_aim/control_direction", target_direction, Eigen::Vector3d::Zero()); - register_output("/gimbal/auto_aim/shoot_enable", should_shoot, false); + register_output("/auto_aim/should_control", should_control, false); + register_output("/auto_aim/control_direction", target_direction, Eigen::Vector3d::Zero()); + register_output("/auto_aim/should_shoot", should_shoot, false); using namespace std::chrono_literals; framerate.set_interval(2s); - const auto config = visual::Transform::Config { - .rclcpp = rclcpp, - .topic = "odom_to_camera_transform", - .parent_frame = Adapter::kParentFrame, - .child_frame = "camera_link", - }; - visual_odom_to_camera = std::make_unique(config); - action_throttler.register_action("adapter"); action_throttler.register_action("feishu"); } @@ -89,7 +79,6 @@ class AutoAimComponent final : public rmcs_executor::Component { double current_gimbal_pitch { std::numeric_limits::quiet_NaN() }; RclcppNode rclcpp; - std::unique_ptr visual_odom_to_camera; Feishu feishu; @@ -100,7 +89,6 @@ class AutoAimComponent final : public rmcs_executor::Component { FramerateCounter framerate; ActionThrottler action_throttler { std::chrono::seconds(1), 233 }; - std::uint8_t publish_count = 0; auto make_context() -> ControlState { auto context = ControlState { }; @@ -110,17 +98,9 @@ class AutoAimComponent final : public rmcs_executor::Component { current_gimbal_yaw = std::atan2(dir.y(), dir.x()); current_gimbal_pitch = std::atan2(-dir.z(), std::hypot(dir.x(), dir.y())); - auto iso = adapter.camera_transform(); - context.odom_to_camera_transform.position = iso.translation(); - context.odom_to_camera_transform.orientation = Eigen::Quaterniond(iso.rotation()); - - visual_odom_to_camera->move(context.odom_to_camera_transform.position, - context.odom_to_camera_transform.orientation); - - if (publish_count++ > 100) { - publish_count = 0; - visual_odom_to_camera->update(); - } + auto iso = adapter.camera_transform(); + context.camera_transform.position = iso.translation(); + context.camera_transform.orientation = Eigen::Quaterniond(iso.rotation()); // TODO:无敌状态下的装甲板需要从裁判系统获取并在此更新 context.invincible_devices = DeviceIds::None(); diff --git a/src/kernel/visualization.cpp b/src/kernel/visualization.cpp index 5cb44d61..7c0aa0fe 100644 --- a/src/kernel/visualization.cpp +++ b/src/kernel/visualization.cpp @@ -8,6 +8,7 @@ #include "utility/logging/printer.hpp" #include "utility/math/conversion.hpp" #include "utility/rclcpp/visual/arrow.hpp" +#include "utility/rclcpp/visual/transform.hpp" #include "utility/serializable.hpp" using namespace rmcs::kernel; @@ -54,6 +55,7 @@ struct Visualization::Impl { std::unique_ptr armors_detect; std::unique_ptr armors_group; std::unique_ptr aiming_direction; + std::unique_ptr camera_transform; bool is_initialized = false; bool size_determined = false; @@ -90,6 +92,12 @@ struct Visualization::Impl { .name = "aiming_direction", .tf = kOdomLink, }); + camera_transform = std::make_unique(visual::Transform::Config { + .rclcpp = visual_node, + .topic = "odom_to_camera_transform", + .parent_frame = kOdomLink, + .child_frame = kCameraLink, + }); is_initialized = true; return { }; @@ -139,22 +147,27 @@ struct Visualization::Impl { return session->push_frame(mat); } - auto solved_pnp_armors(std::span armors) const -> bool { + auto update_visible_armors(std::span armors) const -> bool { if (!is_initialized) return false; - return armors_detect->visualize(armors, "solved_pnp_armors", kCameraLink); + return armors_detect->visualize(armors, "visible_armors", kCameraLink); } - auto predicted_armors(std::span armors) const -> bool { + auto update_visible_robot(std::span armors) const -> bool { if (!is_initialized) return false; - return armors_group->visualize(armors, "predicted_armors", kOdomLink); + return armors_group->visualize(armors, "visible_robot", kOdomLink); } auto update_aiming_direction(double yaw, double pitch) const -> void { if (!is_initialized) return; - aiming_direction->move(Translation::kZero(), euler_to_quaternion(yaw, pitch, 0.0)); aiming_direction->update(); } + + auto update_camera_pose(const Orientation& orientation) const -> void { + if (!is_initialized) return; + camera_transform->move(Translation::kZero(), orientation); + camera_transform->update(); + } }; auto Visualization::initialize(const YAML::Node& yaml, RclcppNode& visual_node) noexcept @@ -164,21 +177,25 @@ auto Visualization::initialize(const YAML::Node& yaml, RclcppNode& visual_node) auto Visualization::initialized() const noexcept -> bool { return pimpl->initialized(); } -auto Visualization::send_image(const Image& image) noexcept -> bool { +auto Visualization::update_image(const Image& image) noexcept -> bool { return pimpl->send_image(image); } -auto Visualization::solved_pnp_armors(std::span armors) const -> bool { - return pimpl->solved_pnp_armors(armors); +auto Visualization::update_visible_armors(std::span armors) const -> bool { + return pimpl->update_visible_armors(armors); } -auto Visualization::predicted_armors(std::span armors) const -> bool { - return pimpl->predicted_armors(armors); +auto Visualization::update_visible_robot(std::span armors) const -> bool { + return pimpl->update_visible_robot(armors); } auto Visualization::update_aiming_direction(double yaw, double pitch) const -> void { pimpl->update_aiming_direction(yaw, pitch); } +auto Visualization::update_camera_pose(const Orientation& orientation) const -> void { + pimpl->update_camera_pose(orientation); +} + Visualization::Visualization() noexcept : pimpl { std::make_unique() } { } diff --git a/src/kernel/visualization.hpp b/src/kernel/visualization.hpp index d241ae48..883111d5 100644 --- a/src/kernel/visualization.hpp +++ b/src/kernel/visualization.hpp @@ -15,7 +15,7 @@ class Visualization { static constexpr auto get_prefix() noexcept { return "visualization"; } auto operator<<(const Image& image) noexcept -> Visualization& { - return send_image(image), *this; + return update_image(image), *this; } public: @@ -24,13 +24,15 @@ class Visualization { auto initialized() const noexcept -> bool; - auto send_image(const Image& image) noexcept -> bool; + auto update_image(const Image& image) noexcept -> bool; - auto solved_pnp_armors(std::span armors) const -> bool; - auto predicted_armors(std::span armors) const -> bool; + auto update_visible_armors(std::span armors) const -> bool; + + auto update_visible_robot(std::span armors) const -> bool; - // 自瞄方向，其坐标系为 OdomImu auto update_aiming_direction(double yaw, double pitch) const -> void; + + auto update_camera_pose(const Orientation&) const -> void; }; } diff --git a/src/module/predictor/snapshot.hpp b/src/module/predictor/snapshot.hpp index 620be080..3e5206c1 100644 --- a/src/module/predictor/snapshot.hpp +++ b/src/module/predictor/snapshot.hpp @@ -14,10 +14,8 @@ struct ISnapshotBackend; class Snapshot; namespace detail { - auto make_snapshot(std::unique_ptr backend) noexcept -> Snapshot; - -} // namespace detail +} class Snapshot { public: @@ -41,6 +39,8 @@ class Snapshot { 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: @@ -52,4 +52,4 @@ class Snapshot { -> Snapshot; }; -} // namespace rmcs::predictor +} diff --git a/src/runtime.cpp b/src/runtime.cpp index 561be6a0..ea0ffc2c 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -107,29 +107,28 @@ auto main() -> int { while (util::get_running()) { node.spin_once(); - auto updated = feishu.heartbeat(); + if (!without_rmcs && !feishu.heartbeat()) continue; auto image = capturer.fetch_image(); if (!image) continue; + auto context = ControlState::kIdentity(); + if (!without_rmcs) { + using namespace std::chrono_literals; + auto closest_state = feishu.search(image->get_timestamp(), 50ms); + if (!closest_state) continue; + + context = *closest_state; + } + visualization.update_camera_pose(context.camera_transform.orientation); + if (framerate.tick()) { node.info("Autoaim Framerate: {}", framerate.fps()); } // 结束流程后发送串流帧 - [[maybe_unused]] auto _ = std::experimental::scope_exit { [&] { - if (visualization.initialized()) { - visualization.send_image(*image); - } - } }; - - auto received = ControlState::kInvalid(); - if (without_rmcs) { - received = ControlState::kIdentity(); - } - - if (!updated) continue; - received = *feishu.latest(); + [[maybe_unused]] auto _ = + std::experimental::scope_exit { [&] { visualization.update_image(*image); } }; /// 1. Identify Armor /// @@ -146,7 +145,7 @@ auto main() -> int { } logging.reset("detection", 5); - tracker.set_invincible_armors(received.invincible_devices); + tracker.set_invincible_armors(context.invincible_devices); armors_2d = tracker.filter_armors(*result); if (armors_2d.empty()) continue; @@ -155,49 +154,42 @@ auto main() -> int { /// 2. Transform 2d to 3d /// auto armors_3d = Armor3Ds { }; - pose_estimator.update_camera_transform(received.odom_to_camera_transform); - if (auto result = pose_estimator.solve_pnp(armors_2d)) { - armors_3d = pose_estimator.odom_to_camera(*result); + { + pose_estimator.update_camera_transform(context.camera_transform); + if (auto result = pose_estimator.solve_pnp(armors_2d)) { + armors_3d = pose_estimator.odom_to_camera(*result); - if (visualization.initialized()) { - visualization.solved_pnp_armors(*result); + visualization.update_visible_armors(*result); } - if (armors_3d.empty()) continue; } /// 3. Apply Tracker /// - auto target = tracker.decide(armors_3d, image->get_timestamp()); - auto target_id = target.target_id; - - auto command = AutoAimState::kInvalid(); - if (auto& snapshot = target.snapshot) { - command.timestamp = Clock::now(); - command.should_control = true; - command.should_shoot = false; - command.yaw = received.yaw; - command.pitch = received.pitch; - command.target = target_id; - - if (target.allow_control) { - const auto control = target.tracking_confirmed; - const auto yaw = received.yaw; - if (auto result = fire_control.solve(*snapshot, control, yaw)) { - command.should_shoot = result->shoot_permitted; - command.yaw = result->yaw; - command.pitch = result->pitch; - } + auto target = tracker.decide(armors_3d, image->get_timestamp()); + + if (!target.snapshot) continue; + + auto& snapshot = target.snapshot; + auto command = AutoAimState::kInvalid(); + if (target.allow_control) { + const auto control = target.tracking_confirmed; + const auto yaw = context.yaw; + if (auto result = fire_control.solve(*snapshot, control, yaw)) { + command.should_control = true; + command.target = target.target_id; + command.should_shoot = result->shoot_permitted; + command.yaw = result->yaw; + command.pitch = result->pitch; } + } - if (visualization.initialized()) { - visualization.predicted_armors(snapshot->predicted_armors(Clock::now())); - } + auto armors = snapshot->predicted_armors(Clock::now()); + visualization.update_visible_robot(armors); - /// 4. Transmit State - /// - feishu.send(command); - } + /// 4. Transmit State + /// + feishu.send(command); } // runtime loop scope diff --git a/src/utility/shared/context.hpp b/src/utility/shared/context.hpp index 32da01db..2d294958 100644 --- a/src/utility/shared/context.hpp +++ b/src/utility/shared/context.hpp @@ -85,7 +85,7 @@ struct ControlState { double yaw { std::numeric_limits::quiet_NaN() }; double pitch { std::numeric_limits::quiet_NaN() }; - Transform odom_to_camera_transform { }; + Transform camera_transform { }; // Imu Odom Link struct { TimePoint timestamp = Clock::now(); @@ -98,24 +98,24 @@ struct ControlState { static auto kInvalid() { return ControlState { - .timestamp = Clock::now(), - .shoot_mode = ShootMode::STOPPING, - .yaw = std::numeric_limits::quiet_NaN(), - .pitch = std::numeric_limits::quiet_NaN(), - .odom_to_camera_transform = Transform::kNaN(), - .capture_signals = { }, - .invincible_devices = DeviceIds::None(), + .timestamp = Clock::now(), + .shoot_mode = ShootMode::STOPPING, + .yaw = std::numeric_limits::quiet_NaN(), + .pitch = std::numeric_limits::quiet_NaN(), + .camera_transform = Transform::kNaN(), + .capture_signals = { }, + .invincible_devices = DeviceIds::None(), }; } static auto kIdentity() { return ControlState { - .timestamp = Clock::now(), - .shoot_mode = ShootMode::BATTLE, - .yaw = 0, - .pitch = 0, - .odom_to_camera_transform = Transform::kIdentity(), - .capture_signals = { }, - .invincible_devices = DeviceIds::None(), + .timestamp = Clock::now(), + .shoot_mode = ShootMode::BATTLE, + .yaw = 0, + .pitch = 0, + .camera_transform = Transform::kIdentity(), + .capture_signals = { }, + .invincible_devices = DeviceIds::None(), }; } };