From c1dc963b0dd4951221a8e81828cd1de65a82c1da Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sun, 3 May 2026 23:27:10 +0800 Subject: [PATCH] refactor: update AutoAimState with center position, adapt Adapter and sync component.cpp --- config/config.yaml | 10 +++---- src/adapter/adapter.hpp | 29 ++++++++++++++++++++ src/component.cpp | 50 ++++++++++++++++++++++++---------- src/runtime.cpp | 25 ++++++++++------- src/utility/shared/context.hpp | 28 +++++++++---------- 5 files changed, 97 insertions(+), 45 deletions(-) diff --git a/config/config.yaml b/config/config.yaml index a1a470c..65b2d4f 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -19,7 +19,7 @@ capturer: # float gain: 16.9807 - invert_image: true + invert_image: false software_sync: false trigger_mode: false fixed_framerate: false @@ -76,17 +76,17 @@ pose_estimator: [-0.064232403853946, -0.087667493884102, 0, 0, 0.792381808294582] fire_control: - initial_bullet_speed: 21.0 # m/s + initial_bullet_speed: 24.2 # m/s shoot_delay: 0.1 # s mpc_enable: true - yaw_offset: +2.5 # degree + yaw_offset: 0.0 # degree pitch_offset: 0.0 # degree aim_point_chooser: coming_angle: 60.0 # degree leaving_angle: 20.0 # degree outpost_coming_angle: 70.0 # degree - outpost_leaving_angle: 50.0 # degree + outpost_leaving_angle: 30.0 # degree mpc: mpc_max_yaw_acc: 50.0 @@ -99,7 +99,7 @@ fire_control: mpc_pitch_r_acc: 1.0 shoot_evaluator: - is_lazy_gimbal: true + is_lazy_gimbal: false near_angle_tolerance: 3 # degree far_angle_tolerance: 2 # degree split_distance: 2 # m diff --git a/src/adapter/adapter.hpp b/src/adapter/adapter.hpp index e69de29..72dbadb 100644 --- a/src/adapter/adapter.hpp +++ b/src/adapter/adapter.hpp @@ -0,0 +1,29 @@ +#pragma once + +#include +#include +#include + +namespace rmcs { + +class Adapter { +public: + 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 diff --git a/src/component.cpp b/src/component.cpp index 2ff7074..1701113 100644 --- a/src/component.cpp +++ b/src/component.cpp @@ -1,4 +1,4 @@ -#include "adapter/sentry.hpp" +#include "adapter/adapter.hpp" #include "kernel/feishu.hpp" #include "module/debug/action_throttler.hpp" #include "utility/rclcpp/node.hpp" @@ -24,6 +24,7 @@ class AutoAimComponent final : public rmcs_executor::Component { register_output("/auto_aim/should_control", should_control, false); register_output("/auto_aim/control_direction", target_direction, Eigen::Vector3d::Zero()); + register_output("/auto_aim/robot_center", robot_center, kVectorNaN); register_output("/auto_aim/should_shoot", should_shoot, false); register_output("/auto_aim/yaw_rate", yaw_rate, 0.0); register_output("/auto_aim/pitch_rate", pitch_rate, 0.0); @@ -38,12 +39,11 @@ class AutoAimComponent final : public rmcs_executor::Component { } auto update() -> void override { + set_default_command(); + if (!adapter.ready()) [[unlikely]] { action_throttler.dispatch("adapter", [&] { rclcpp.warn("adapter is not ready"); }); - *should_control = false; - *should_shoot = false; - feishu.send(SystemContext::kInvalid()); return; } @@ -54,19 +54,29 @@ class AutoAimComponent final : public rmcs_executor::Component { if (!feishu.heartbeat()) return; - *target_direction = kVectorNaN; - // 超时检测 - auto command = *feishu.latest(); + const auto latest = feishu.latest(); + if (!latest.has_value()) return; + + const auto& command = *latest; if (Clock::now() - command.timestamp > kAutoAimTimeout) { - *should_control = false; - *should_shoot = false; return; } // 业务开关 *should_control = command.should_control; *should_shoot = command.should_shoot; + *robot_center = Eigen::Vector3d { + command.robot_center.x, + command.robot_center.y, + command.robot_center.z, + }; + + *yaw_rate = command.yaw_rate; + *pitch_rate = command.pitch_rate; + *yaw_acc = command.yaw_acc; + *pitch_acc = command.pitch_acc; + *feedforward_valid = command.feedforward_valid; if (!*should_control) return; @@ -81,11 +91,8 @@ class AutoAimComponent final : public rmcs_executor::Component { private: static constexpr auto kAutoAimTimeout = std::chrono::milliseconds { 100 }; - static inline const auto kVectorNaN = Eigen::Vector3d { - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), - }; + static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); + static inline const auto kVectorNaN = Eigen::Vector3d { kNaN, kNaN, kNaN }; Adapter adapter; RclcppNode rclcpp; @@ -95,6 +102,7 @@ class AutoAimComponent final : public rmcs_executor::Component { OutputInterface should_control; OutputInterface should_shoot; OutputInterface target_direction; + OutputInterface robot_center; OutputInterface yaw_rate; OutputInterface pitch_rate; OutputInterface yaw_acc; @@ -103,8 +111,20 @@ class AutoAimComponent final : public rmcs_executor::Component { InputInterface right_switch; + auto set_default_command() -> void { + *should_control = false; + *should_shoot = false; + *target_direction = kVectorNaN; + *robot_center = kVectorNaN; + *yaw_rate = kNaN; + *pitch_rate = kNaN; + *yaw_acc = kNaN; + *pitch_acc = kNaN; + *feedforward_valid = false; + }; + auto make_context() -> SystemContext { - auto context = SystemContext { }; + auto context = SystemContext {}; context.timestamp = Clock::now(); diff --git a/src/runtime.cpp b/src/runtime.cpp index 00b62f2..f40e314 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -9,6 +9,7 @@ #include "module/debug/framerate.hpp" #include "utility/image/armor.hpp" #include "utility/logging_util.hpp" +#include "utility/math/linear.hpp" #include "utility/panic.hpp" #include "utility/rclcpp/configuration.hpp" #include "utility/rclcpp/node.hpp" @@ -16,8 +17,10 @@ #include "utility/shared/context.hpp" #include "utility/singleton/running.hpp" +#include #include #include +#include #include #include @@ -37,13 +40,13 @@ auto main() -> int { logging.reset("detection", 5); /// Runtime - auto feishu = kernel::Feishu { }; - auto capturer = kernel::Capturer { }; - auto identifier = kernel::Identifier { }; - auto tracker = kernel::Tracker { }; - auto pose_estimator = kernel::PoseEstimator { }; - auto fire_control = kernel::FireControl { }; - auto visualization = kernel::Visualization { }; + auto feishu = kernel::Feishu {}; + auto capturer = kernel::Capturer {}; + auto identifier = kernel::Identifier {}; + auto tracker = kernel::Tracker {}; + auto pose_estimator = kernel::PoseEstimator {}; + auto fire_control = kernel::FireControl {}; + auto visualization = kernel::Visualization {}; /// Configure auto configuration = util::configuration(); @@ -101,7 +104,7 @@ auto main() -> int { handle_result("visualization", result); } - auto framerate = FramerateCounter { }; + auto framerate = FramerateCounter {}; framerate.set_interval(std::chrono::seconds { 5 }); while (util::get_running()) { @@ -132,7 +135,7 @@ auto main() -> int { /// 1. Identify Armor /// - auto armors_2d = Armor2Ds { }; + auto armors_2d = Armor2Ds {}; { auto result = identifier.sync_identify(*image); if (!result.has_value()) { @@ -153,7 +156,7 @@ auto main() -> int { /// 2. Transform 2d to 3d /// - auto armors_3d = Armor3Ds { }; + auto armors_3d = Armor3Ds {}; { pose_estimator.update_camera_transform(context.camera_transform); auto result = pose_estimator.estimate_armor(armors_2d); @@ -187,6 +190,8 @@ auto main() -> int { } auto armors = snapshot->predicted_armors(Clock::now()); visualization.update_visible_robot(armors); + visualization.update_mpc_plan(command.yaw, command.pitch, command.yaw_rate, + command.pitch_rate, command.yaw_acc, command.pitch_acc); } /// 4. Transmit State diff --git a/src/utility/shared/context.hpp b/src/utility/shared/context.hpp index 55bdfdd..cf4c60b 100644 --- a/src/utility/shared/context.hpp +++ b/src/utility/shared/context.hpp @@ -12,25 +12,22 @@ concept context_trait = std::is_trivially_copyable_v; struct AutoAimState { static constexpr auto kLabel = "/shm_autoaim_state"; static constexpr auto kLength = 512; + static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); - TimePoint timestamp { }; + TimePoint timestamp {}; bool should_control { false }; bool should_shoot = { false }; - double yaw { std::numeric_limits::quiet_NaN() }; - double pitch { std::numeric_limits::quiet_NaN() }; - double yaw_rate { std::numeric_limits::quiet_NaN() }; - double pitch_rate { std::numeric_limits::quiet_NaN() }; - double yaw_acc { std::numeric_limits::quiet_NaN() }; - double pitch_acc { std::numeric_limits::quiet_NaN() }; + double yaw { kNaN }; + double pitch { kNaN }; + double yaw_rate { kNaN }; + double pitch_rate { kNaN }; + double yaw_acc { kNaN }; + double pitch_acc { kNaN }; bool feedforward_valid { false }; - Translation robot_center { - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), - }; + Translation robot_center { kNaN, kNaN, kNaN }; DeviceId target { DeviceId::UNKNOWN }; @@ -45,15 +42,16 @@ static_assert(context_trait); struct SystemContext { static constexpr auto kLabel = "/shm_control_state"; static constexpr auto kLength = 512; + static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); /// Dynamic Context /// - TimePoint timestamp { }; + TimePoint timestamp {}; bool enable_autoaim = false; - double yaw { std::numeric_limits::quiet_NaN() }; - double pitch { std::numeric_limits::quiet_NaN() }; + double yaw { kNaN }; + double pitch { kNaN }; Transform camera_transform = Transform::kNaN(); // Imu Odom Link