Merge/outpost robot state

config/config.yaml

@@ -25,7 +25,7 @@ capturer:
   record_fps: 24
   max_videos_size_gb: 50
 
-  # source: "local_video"
+  # source: "hikcamera" or "local_video"
   source: "hikcamera"
 
   hikcamera:
@@ -47,7 +47,7 @@ capturer:
     # 替换为你具体的路径
     location: "/path/to/your/video"
     # double 帧率
-    frame_rate: 40
+    frame_rate: 80
     # bool 是否循环播放
     loop_play: true
     # bool 是否允许跳帧以满足实时性
@@ -90,17 +90,18 @@ pose_estimator:
   outpost_armor_thickness: 0.022
 
 fire_control:
-  initial_bullet_speed: 21.4 # m/s
-  shoot_delay: 0.07 # s
+  initial_bullet_speed: 23.5 # m/s
+  shoot_delay: 0.04 # s
   mpc_enable: true
-  yaw_offset: 0.0 # degree
-  pitch_offset: 0.0 # degree
+  yaw_offset: -0.4 # degree
+  pitch_offset: +0.8 # degree
 
-  aim_point_chooser:
-    coming_angle: 70.0 # degree
+  armor_target_selector:
+    coming_angle: 60.0 # degree
     leaving_angle: 20.0 # degree
-    outpost_coming_angle: 70.0 # degree
-    outpost_leaving_angle: 20.0 # degree
+    outpost_coming_angle: 60.0 # degree
+    outpost_leaving_angle: 30.0 # degree
+    switch_threshold: 8.0 # degree
 
   mpc:
     mpc_max_yaw_acc: 50.0
@@ -111,11 +112,14 @@ fire_control:
     mpc_pitch_q_angle: 9000000.0
     mpc_pitch_q_rate: 0.0
     mpc_pitch_r_acc: 1.0
+    mpc_max_iter: 5
 
   shoot_evaluator:
     is_lazy_gimbal: false
-    near_angle_tolerance: 3 # degree
-    far_angle_tolerance: 2 # degree
+    near_yaw_tolerance: 2 # degree
+    far_yaw_tolerance: 1 # degree
+    near_pitch_tolerance: 2 # degree
+    far_pitch_tolerance: 1 # degree
     split_distance: 2 # m
 
 visualization:

src/component.cpp

@@ -10,7 +10,6 @@ namespace rmcs {
 class AutoAimComponent final : public rmcs_executor::Component {
     static inline const auto kNaN  = std::numeric_limits<double>::quiet_NaN();
     static inline const auto kTNaN = Eigen::Vector3d { kNaN, kNaN, kNaN };
-    static inline const auto kQNaN = Eigen::Quaterniond { kNaN, kNaN, kNaN, kNaN };
 
 private:
     AutoAim auto_aim { };
@@ -27,7 +26,6 @@ class AutoAimComponent final : public rmcs_executor::Component {
     OutputInterface<double> pitch_rate;
     OutputInterface<double> yaw_acc;
     OutputInterface<double> pitch_acc;
-    OutputInterface<bool> feedforward_valid;
 
     std::chrono::steady_clock::time_point last_command_timestamp;
 
@@ -45,7 +43,6 @@ class AutoAimComponent final : public rmcs_executor::Component {
         register_output("/auto_aim/pitch_rate", pitch_rate, kNaN);
         register_output("/auto_aim/yaw_acc", yaw_acc, kNaN);
         register_output("/auto_aim/pitch_acc", pitch_acc, kNaN);
-        register_output("/auto_aim/feedforward_valid", feedforward_valid, false);
     }
 
     auto before_updating() -> void override {
@@ -66,12 +63,10 @@ class AutoAimComponent final : public rmcs_executor::Component {
             ctx.timestamp = Clock::now();
 
             const auto& dir = *barrel_direction;
-
-            ctx.yaw   = std::atan2(dir.y(), dir.x());
-            ctx.pitch = std::atan2(-dir.z(), std::hypot(dir.x(), dir.y()));
+            ctx.yaw         = std::atan2(dir.y(), dir.x());
+            ctx.pitch       = std::atan2(-dir.z(), std::hypot(dir.x(), dir.y()));
 
             const auto& iso = *camera_transform;
-
             ctx.camera_transform.translation = Translation { iso.translation() };
             ctx.camera_transform.orientation = Orientation { Eigen::Quaterniond(iso.rotation()) };
 
@@ -84,14 +79,13 @@ class AutoAimComponent final : public rmcs_executor::Component {
                 using namespace std::chrono_literals;
                 if (Clock::now() - cmd.timestamp > 100ms) return;
 
-                *should_control    = cmd.should_control;
-                *should_shoot      = cmd.should_shoot;
-                *yaw_rate          = cmd.yaw_rate;
-                *pitch_rate        = cmd.pitch_rate;
-                *yaw_acc           = cmd.yaw_acc;
-                *pitch_acc         = cmd.pitch_acc;
-                *feedforward_valid = cmd.feedforward_valid;
-                *robot_center      = {
+                *should_control = cmd.should_control;
+                *should_shoot   = cmd.should_shoot;
+                *yaw_rate       = cmd.yaw_rate;
+                *pitch_rate     = cmd.pitch_rate;
+                *yaw_acc        = cmd.yaw_acc;
+                *pitch_acc      = cmd.pitch_acc;
+                *robot_center   = {
                     cmd.robot_center.x,
                     cmd.robot_center.y,
                     cmd.robot_center.z,
@@ -104,7 +98,7 @@ class AutoAimComponent final : public rmcs_executor::Component {
                 *target_direction = Eigen::Vector3d {
                     std::cos(pitch) * std::cos(yaw),
                     std::cos(pitch) * std::sin(yaw),
-                    std::sin(pitch),
+                    -std::sin(pitch),
                 };
             });
             last_command_timestamp = now;

src/kernel/auto_aim.cpp

@@ -129,23 +129,29 @@ struct AutoAim::Impl {
                 auto armors    = snapshot->predicted_armors(Clock::now());
                 visual.publish(armors, "visible_robot");
 
-                const auto yaw = context.yaw;
-                if (auto result = fire.solve(*snapshot, yaw)) {
-                    cmd.should_control    = true;
-                    cmd.target            = target.target_id;
-                    cmd.should_shoot      = result->shoot_permitted;
-                    cmd.yaw               = result->yaw;
-                    cmd.pitch             = result->pitch;
-                    cmd.yaw_rate          = result->yaw_rate;
-                    cmd.pitch_rate        = result->pitch_rate;
-                    cmd.yaw_acc           = result->yaw_acc;
-                    cmd.pitch_acc         = result->pitch_acc;
-                    cmd.feedforward_valid = result->feedforward_valid;
-                    cmd.robot_center      = result->center_position;
-
-                    /// TODO:
-                    /// 统一控制侧和自瞄侧的 Pitch 符号方向定义
-                    visual.update_aiming_direction(cmd.yaw, -cmd.pitch);
+                const auto gimbal_state = fire_control::GimbalState {
+                    .timestamp = context.timestamp,
+                    .yaw       = context.yaw,
+                    .pitch     = context.pitch,
+                };
+                if (auto result = fire.solve(*snapshot, gimbal_state)) {
+                    cmd.should_control = true;
+                    cmd.target         = target.target_id;
+                    cmd.should_shoot   = result->shoot_permitted;
+                    cmd.yaw            = result->yaw;
+                    cmd.pitch          = result->pitch;
+                    cmd.robot_center   = result->center_position;
+
+                    if (result->feedforward.has_value()) {
+                        cmd.yaw_rate   = result->feedforward->yaw_rate;
+                        cmd.pitch_rate = result->feedforward->pitch_rate;
+                        cmd.yaw_acc    = result->feedforward->yaw_acc;
+                        cmd.pitch_acc  = result->feedforward->pitch_acc;
+                    }
+
+                    auto impact_armors = snapshot->predicted_armors(result->impact_time);
+                    visual.publish(impact_armors, "impact_robot");
+                    visual.update_aiming_direction(cmd.yaw, cmd.pitch);
                     visual.update_mpc_plan(cmd.yaw, cmd.pitch, cmd.yaw_rate, cmd.pitch_rate,
                         cmd.yaw_acc, cmd.pitch_acc);
                 }