fix: fix armor selection logic

.github/workflows/gtest.yml

@@ -2,10 +2,18 @@ name: Run Test
 
 on:
   push:
-    branches: [ main ]
   pull_request:
   workflow_dispatch:
 
+concurrency:
+  group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }}
+  cancel-in-progress: true
+
+env:
+  RMCS_REPO: https://github.com/Alliance-Algorithm/RMCS.git
+  RMCS_REF: main
+  HIKCAMERA_REPO: https://github.com/Alliance-Algorithm/ros2-hikcamera.git
+
 jobs:
   test:
     runs-on: ubuntu-latest
@@ -14,45 +22,79 @@ jobs:
       options: --user 0
 
     env:
-      WS_DIR: ${{ github.workspace }}/ws
-      SRC_DIR: ${{ github.workspace }}/ws/src
       TEST_ASSETS_ROOT: /tmp/auto_aim
 
     steps:
-      - name: Checkout repository into ROS2 workspace
+      - name: Checkout rmcs_auto_aim_v2
         uses: actions/checkout@v4
         with:
-          repository: Alliance-Algorithm/rmcs_auto_aim_v2
-          path: ws/src/rmcs_auto_aim_v2
+          path: rmcs_auto_aim_v2
+
+      - name: Clone RMCS workspace
+        shell: bash
+        run: |
+          set -euo pipefail
+          git clone --depth 1 --recurse-submodules --branch "${RMCS_REF}" "${RMCS_REPO}" /tmp/RMCS
+          git clone --depth 1 "${HIKCAMERA_REPO}" /tmp/RMCS/rmcs_ws/src/hikcamera
+          mkdir -p /tmp/RMCS/rmcs_ws/src/rmcs_auto_aim_v2
+          tar -C "${GITHUB_WORKSPACE}/rmcs_auto_aim_v2" --exclude=".git" -cf - . \
+            | tar -C /tmp/RMCS/rmcs_ws/src/rmcs_auto_aim_v2 -xf -
 
       - name: Install yq dependency
         run: |
           apt-get update
           apt-get install -y yq
 
+      - name: Build RMCS workspace
+        shell: bash
+        working-directory: /tmp/RMCS/rmcs_ws
+        run: |
+          set -euo pipefail
+          set +u
+          source /opt/ros/jazzy/setup.bash
+          set -u
+          colcon build --packages-up-to rmcs_auto_aim_v2
+
       - name: Download test assets
         shell: bash
         run: |
-          cd "$SRC_DIR/rmcs_auto_aim_v2/test"
+          set -euo pipefail
+          set +u
+          source /opt/ros/jazzy/setup.bash
+          source /tmp/RMCS/rmcs_ws/install/setup.bash
+          set -u
+          cd /tmp/RMCS/rmcs_ws/src/rmcs_auto_aim_v2/test
           TEST_ASSETS_ROOT="$TEST_ASSETS_ROOT" ./download_assets.sh
 
       - name: Configure test project
         shell: bash
         run: |
+          set -euo pipefail
+          set +u
           source /opt/ros/jazzy/setup.bash
-          cd "$SRC_DIR/rmcs_auto_aim_v2/test"
+          source /tmp/RMCS/rmcs_ws/install/setup.bash
+          set -u
+          cd /tmp/RMCS/rmcs_ws/src/rmcs_auto_aim_v2/test
           cmake -S . -B build -DCMAKE_BUILD_TYPE=Release
 
       - name: Build test project
         shell: bash
         run: |
+          set -euo pipefail
+          set +u
           source /opt/ros/jazzy/setup.bash
-          cd "$SRC_DIR/rmcs_auto_aim_v2/test"
+          source /tmp/RMCS/rmcs_ws/install/setup.bash
+          set -u
+          cd /tmp/RMCS/rmcs_ws/src/rmcs_auto_aim_v2/test
           cmake --build build --parallel
 
       - name: Run gtests
         shell: bash
         run: |
+          set -euo pipefail
+          set +u
           source /opt/ros/jazzy/setup.bash
-          cd "$SRC_DIR/rmcs_auto_aim_v2/test"
+          source /tmp/RMCS/rmcs_ws/install/setup.bash
+          set -u
+          cd /tmp/RMCS/rmcs_ws/src/rmcs_auto_aim_v2/test
           ctest --test-dir build --output-on-failure

CMakeLists.txt

@@ -32,6 +32,7 @@ find_package(ament_index_cpp REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(visualization_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(rmcs_msgs REQUIRED)
 
 find_package(rmcs_executor REQUIRED)
 find_package(rmcs_description REQUIRED)
@@ -47,10 +48,12 @@ include_directories(
     ${ament_index_cpp_INCLUDE_DIRS}
     ${visualization_msgs_INCLUDE_DIRS}
     ${geometry_msgs_INCLUDE_DIRS}
+    ${rmcs_msgs_INCLUDE_DIRS}
 
     ${rmcs_executor_INCLUDE_DIRS}
     ${rmcs_description_INCLUDE_DIRS}
     ${fast_tf_INCLUDE_DIRS}
+    ${hikcamera_INCLUDE_DIRS}
 )
 
 ## 代码资源搜索

config/config.yaml

@@ -76,14 +76,14 @@ pose_estimator:
     [-0.064232403853946, -0.087667493884102, 0, 0, 0.792381808294582]
 
 fire_control:
-  initial_bullet_speed: 24.2 # m/s
-  shoot_delay: 0.1 # s
+  initial_bullet_speed: 21.4 # m/s
+  shoot_delay: 0.07 # s
   mpc_enable: true
   yaw_offset: 0.0 # degree
   pitch_offset: 0.0 # degree
 
   aim_point_chooser:
-    coming_angle: 60.0 # degree
+    coming_angle: 70.0 # degree
     leaving_angle: 20.0 # degree
     outpost_coming_angle: 70.0 # degree
     outpost_leaving_angle: 30.0 # degree

package.xml

@@ -15,6 +15,9 @@
 
 <!-- 添加的两个依赖，为了保证正常的依赖构建顺序 -->
   <depend>rmcs_executor</depend>
+  <depend>rmcs_description</depend>
+  <depend>fast_tf</depend>
+  <depend>rmcs_msgs</depend>
   <depend>hikcamera</depend>
 
   <export>

src/component.cpp

@@ -4,7 +4,7 @@
 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
 #endif
 
-#include "adapter/sentry.hpp"
+#include "adapter/adapter.hpp"
 #include "kernel/feishu.hpp"
 #include "utility/rclcpp/node.hpp"
 #include "utility/shared/context.hpp"
@@ -41,7 +41,7 @@ class AutoAimComponent final : public rmcs_executor::Component {
     InputInterface<rmcs_msgs::RobotId> robot_id;
 
     auto make_context() const {
-        auto context = SystemContext { };
+        auto context = SystemContext {};
 
         context.timestamp = Clock::now();
 

src/kernel/fire_control.cpp

@@ -29,11 +29,16 @@ struct FireControl::Impl {
         double pitch_offset;
 
         constexpr static std::tuple metas {
-            &Config::initial_bullet_speed, "initial_bullet_speed",
-            &Config::shoot_delay,          "shoot_delay",
-            &Config::mpc_enable,           "mpc_enable",
-            &Config::yaw_offset,           "yaw_offset",
-            &Config::pitch_offset,         "pitch_offset",
+            &Config::initial_bullet_speed,
+            "initial_bullet_speed",
+            &Config::shoot_delay,
+            "shoot_delay",
+            &Config::mpc_enable,
+            "mpc_enable",
+            &Config::yaw_offset,
+            "yaw_offset",
+            &Config::pitch_offset,
+            "pitch_offset",
         };
     } config;
 
@@ -89,9 +94,8 @@ struct FireControl::Impl {
         auto yaw_acc           = std::numeric_limits<double>::quiet_NaN();
         auto feedforward_valid = false;
 
-        if (config.mpc_enable) {
-            auto sample_attitude =
-                [&](TimePoint t) -> std::expected<AimAttitude, std::string> {
+        if (config.mpc_enable && snapshot.device_id() != DeviceId::OUTPOST) {
+            auto sample_attitude = [&](TimePoint t) -> std::expected<AimAttitude, std::string> {
                 auto sample = AimPointSampler::sample_at(
                     snapshot, aim_point_chooser, t, config.initial_bullet_speed);
                 if (!sample.has_value()) return std::unexpected { sample.error() };
@@ -111,11 +115,11 @@ struct FireControl::Impl {
                     yaw_acc           = planned->yaw_acc;
                     feedforward_valid = true;
                 } else {
-                    constexpr int kYawRow   = 0;
-                    constexpr int kPitchRow = 2;
+                    constexpr int kYawRow    = 0;
+                    constexpr int kPitchRow  = 2;
                     constexpr int kCenterCol = kMpcAxisHorizon / 2;
-                    pitch = (*reference)(kPitchRow, kCenterCol);
-                    yaw   = util::normalize_angle((*reference)(kYawRow, kCenterCol));
+                    pitch                    = (*reference)(kPitchRow, kCenterCol);
+                    yaw = util::normalize_angle((*reference)(kYawRow, kCenterCol));
                 }
             }
         }

src/module/fire_control/aim_point_chooser.cpp

@@ -57,7 +57,8 @@ struct AimPointChooser::Impl {
         if (!(config.coming_angle > 0.0) || !(config.leaving_angle > 0.0)
             || !(config.outpost_coming_angle > 0.0) || !(config.outpost_leaving_angle > 0.0)) {
             return std::unexpected {
-                "coming_angle, leaving_angle, outpost_coming_angle and outpost_leaving_angle must be > 0",
+                "coming_angle, leaving_angle, outpost_coming_angle and outpost_leaving_angle must "
+                "be > 0",
             };
         }
 
@@ -95,19 +96,25 @@ struct AimPointChooser::Impl {
         };
 
         for (size_t index = 0; index < armors.size(); ++index) {
-            const auto delta_yaw  = util::normalize_angle(yaw(index) - center_yaw);
+            const auto delta_yaw   = util::normalize_angle(yaw(index) - center_yaw);
             candidate_evals[index] = {
                 .delta_yaw = delta_yaw,
                 .in_window = in_window(delta_yaw),
             };
         }
 
         const auto priority_key = [&](size_t index) {
-            const auto abs_delta    = std::abs(candidate_evals[index].delta_yaw);
-            const auto id           = armors[index].id;
-            const auto is_last      = last_chosen_armor_id.has_value() && (id == *last_chosen_armor_id);
+            const auto preferred_incoming = [&] {
+                const auto delta = candidate_evals[index].delta_yaw;
+                if (angular_velocity > 0.0) return (delta > 0.0) ? 1 : 0;
+                if (angular_velocity < 0.0) return (delta < 0.0) ? 1 : 0;
+                return 0;
+            }();
+            const auto abs_delta = std::abs(candidate_evals[index].delta_yaw);
+            const auto id        = armors[index].id;
+            const auto is_last = last_chosen_armor_id.has_value() && (id == *last_chosen_armor_id);
             const auto last_penalty = is_last ? 0 : 1;
-            return std::tuple { abs_delta, last_penalty, id, index };
+            return std::tuple { preferred_incoming, abs_delta, last_penalty, id, index };
         };
 
         auto best_idx = std::optional<size_t> {};

src/module/fire_control/solver/aim_point_sampling.cpp

@@ -12,27 +12,27 @@ auto rmcs::fire_control::AimPointSampler::sample_at(
     -> std::expected<AimSample, std::string> {
     auto aim_point = sample_aim_point_at(snapshot, chooser, t);
     if (!aim_point.has_value()) {
-        return std::unexpected { "aim point sample failed" };
+        return std::unexpected { aim_point.error() };
     }
 
-    auto attitude = solve_aim_attitude(*aim_point, bullet_speed);
+    auto attitude = solve_aim_attitude(aim_point.value(), bullet_speed);
     if (!attitude.has_value()) {
         return std::unexpected { attitude.error() };
     }
 
     return AimSample {
         .attitude  = *attitude,
-        .aim_point = *aim_point,
+        .aim_point = aim_point.value(),
     };
 }
 
 auto rmcs::fire_control::AimPointSampler::sample_aim_point_at(predictor::Snapshot const& snapshot,
-    AimPointChooser& chooser, TimePoint t) -> std::optional<Eigen::Vector3d> {
+    AimPointChooser& chooser, TimePoint t) -> std::expected<Eigen::Vector3d, std::string> {
     auto predicted_armors     = snapshot.predicted_armors(t);
     auto predicted_kinematics = snapshot.kinematics_at(t);
     auto chosen_armor = chooser.choose_armor(predicted_armors, predicted_kinematics.center_position,
         predicted_kinematics.angular_velocity);
-    if (!chosen_armor.has_value()) return std::nullopt;
+    if (!chosen_armor.has_value()) return std::unexpected { "choose_armor returned nullopt" };
 
     auto aim_point = Eigen::Vector3d {};
     chosen_armor->translation.copy_to(aim_point);

src/module/fire_control/solver/aim_point_sampling.hpp

@@ -29,7 +29,7 @@ class AimPointSampler {
 
 private:
     static auto sample_aim_point_at(predictor::Snapshot const& snapshot, AimPointChooser& chooser,
-        TimePoint t) -> std::optional<Eigen::Vector3d>;
+        TimePoint t) -> std::expected<Eigen::Vector3d, std::string>;
 
     static auto solve_aim_attitude(Eigen::Vector3d const& aim_point, double bullet_speed)
         -> std::expected<AimAttitude, std::string>;

test/CMakeLists.txt

@@ -27,6 +27,7 @@ find_package(ament_cmake_gtest REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(visualization_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(rmcs_msgs REQUIRED)
 
 find_package(yaml-cpp REQUIRED)
 find_package(OpenVINO REQUIRED)
@@ -38,6 +39,7 @@ include_directories(
     ${rclcpp_INCLUDE_DIRS}
     ${visualization_msgs_INCLUDE_DIRS}
     ${geometry_msgs_INCLUDE_DIRS}
+    ${rmcs_msgs_INCLUDE_DIRS}
     ${OpenCV_INCLUDE_DIRS}
 )
 
@@ -143,12 +145,6 @@ ament_add_gtest(
     ${TEST_DIR}/transform_communication.cpp
 )
 
-# Feishu IPC
-ament_add_gtest(
-    test_feishu
-    ${TEST_DIR}/feishu_test.cpp
-)
-
 # Aim point chooser
 ament_add_gtest(
     test_aim_point_chooser

test/aim_point_chooser.cpp

@@ -106,3 +106,21 @@ TEST(AimPointChooser, SingleArmorHighSpeedScanBySpinDirection) {
         }
     }
 }
+
+TEST(AimPointChooser, PreferIncomingArmorWhenSpinPositive) {
+    auto const armors = make_armors({ -30.0, 5.0 });
+    auto const chosen = choose_once(armors, 0.0, kFastPositive);
+    EXPECT_EQ(chosen, std::optional<int> { 0 });
+}
+
+TEST(AimPointChooser, PreferIncomingArmorWhenSpinNegative) {
+    auto const armors = make_armors({ -5.0, 30.0 });
+    auto const chosen = choose_once(armors, 0.0, kFastNegative);
+    EXPECT_EQ(chosen, std::optional<int> { 1 });
+}
+
+TEST(AimPointChooser, KeepAbsoluteDeltaPriorityWhenSpinZero) {
+    auto const armors = make_armors({ -15.0, 5.0 });
+    auto const chosen = choose_once(armors, 0.0, 0.0);
+    EXPECT_EQ(chosen, std::optional<int> { 1 });
+}