refactor: outpost ekf

README.md

@@ -108,7 +108,6 @@ auto barrel_direction = *fast_tf::cast<rmcs_description::OdomImu>(
 | `/auto_aim/pitch_rate` | `double` | pitch 角速度 |
 | `/auto_aim/yaw_acc` | `double` | yaw 角加速度 |
 | `/auto_aim/pitch_acc` | `double` | pitch 角加速度 |
-| `/auto_aim/feedforward_valid` | `bool` | 前馈数据是否有效 |
 
 ## 项目架构
 

config/config.yaml

@@ -93,13 +93,13 @@ fire_control:
   initial_bullet_speed: 23.5 # m/s
   shoot_delay: 0.04 # s
   mpc_enable: true
-  yaw_offset: -0.4 # degree
-  pitch_offset: +0.8 # degree
+  yaw_offset: -0.0 # degree
+  pitch_offset: +0.0 # degree
 
   armor_target_selector:
     coming_angle: 60.0 # degree
     leaving_angle: 20.0 # degree
-    outpost_coming_angle: 60.0 # degree
+    outpost_coming_angle: 40.0 # degree
     outpost_leaving_angle: 30.0 # degree
     switch_threshold: 8.0 # degree
 

src/kernel/auto_aim.cpp

@@ -18,6 +18,7 @@
 #include <csignal>
 #include <experimental/scope>
 #include <filesystem>
+#include <memory>
 #include <thread>
 
 using namespace rmcs;

src/kernel/fire_control.cpp

@@ -71,7 +71,7 @@ struct FireControl::Impl {
             };
         }
 
-        return { };
+        return {};
     }
 
     auto solve(predictor::Snapshot const& snapshot, GimbalState const& gimbal_state)
@@ -82,9 +82,9 @@ struct FireControl::Impl {
         auto aim_point_position = Eigen::Vector3d {};
         auto center_position    = Eigen::Vector3d {};
         {
-            auto const target_motion = snapshot.motion();
-            auto const coarse_fly_time =
-                target_motion.center_position.norm() / config.initial_bullet_speed;
+            auto const target_motion   = snapshot.motion();
+            auto target_center         = target_motion.center_position.make<Eigen::Vector3d>();
+            auto const coarse_fly_time = target_center.norm() / config.initial_bullet_speed;
             auto const selection_predict_time = config.shoot_delay + coarse_fly_time;
             auto const selection_time         = snapshot.time_stamp()
                 + std::chrono::duration_cast<Duration>(
@@ -100,7 +100,7 @@ struct FireControl::Impl {
 
             auto const& selected = target_candidates[*selected_index];
             selected_armor_id    = selected.armor.id;
-            center_position      = target_motion.center_position;
+            center_position      = target_center;
             selected.armor.translation.copy_to(aim_point_position);
         }
 
@@ -116,7 +116,7 @@ struct FireControl::Impl {
             }
 
             last_selected_armor_id = solution->candidate.armor.id;
-            target_solution        = std::move(*solution);
+            target_solution        = *solution;
         }
 
         /// 3. 轨迹规划
@@ -167,11 +167,11 @@ struct FireControl::Impl {
         };
     }
 
-    TrajectoryPlanner trajectory_planner { };
-    TargetSolver target_solver { };
-    ShootEvaluator shoot_evaluator { };
-    ArmorSelector armor_selector { };
-    std::optional<int> last_selected_armor_id { };
+    TrajectoryPlanner trajectory_planner {};
+    TargetSolver target_solver {};
+    ShootEvaluator shoot_evaluator {};
+    ArmorSelector armor_selector {};
+    std::optional<int> last_selected_armor_id {};
 };
 
 FireControl::FireControl() noexcept

src/module/fire_control/armor_selector.cpp

@@ -85,8 +85,8 @@ struct ArmorSelector::Impl {
             auto orientation = Eigen::Quaterniond {};
             candidate.armor.orientation.copy_to(orientation);
             const auto armor_yaw  = util::eulers(orientation)[0];
-            const auto center_yaw = std::atan2(
-                candidate.motion.center_position.y(), candidate.motion.center_position.x());
+            const auto& center    = candidate.motion.center_position;
+            const auto center_yaw = std::atan2(center.y, center.x);
             return util::normalize_angle(armor_yaw - center_yaw);
         };
 

src/module/fire_control/target_solver.cpp

@@ -40,7 +40,7 @@ struct TargetSolver::Impl {
     static auto solve(predictor::Snapshot const& snapshot, int armor_id, TimePoint command_time,
         double bullet_speed, double shoot_delay) -> std::expected<TargetSolution, std::string> {
         auto target_motion    = snapshot.motion_at(command_time);
-        auto target_position  = target_motion.center_position;
+        auto target_position  = target_motion.center_position.make<Eigen::Vector3d>();
         auto current_fly_time = target_position.norm() / bullet_speed;
 
         auto result = std::optional<TargetSolution> {};