Alliance-Algorithm
diff --git a/‎.github/workflows/gtest.yml‎
Lines changed: 11 additions & 5 deletions b/‎.github/workflows/gtest.yml‎
Lines changed: 11 additions & 5 deletions
diff --git a/‎README.md‎
Lines changed: 1 addition & 1 deletion b/‎README.md‎
Lines changed: 1 addition & 1 deletion
diff --git a/‎config/config.yaml‎
Lines changed: 12 additions & 6 deletions b/‎config/config.yaml‎
Lines changed: 12 additions & 6 deletions
diff --git a/‎package.xml‎
Lines changed: 3 additions & 2 deletions b/‎package.xml‎
Lines changed: 3 additions & 2 deletions
diff --git a/‎src/component.cpp‎
Lines changed: 21 additions & 2 deletions b/‎src/component.cpp‎
Lines changed: 21 additions & 2 deletions
diff --git a/‎src/kernel/pose_estimator.cpp‎
Lines changed: 61 additions & 9 deletions b/‎src/kernel/pose_estimator.cpp‎
Lines changed: 61 additions & 9 deletions
diff --git a/‎src/kernel/pose_estimator.hpp‎
Lines changed: 9 additions & 0 deletions b/‎src/kernel/pose_estimator.hpp‎
Lines changed: 9 additions & 0 deletions
diff --git a/‎src/kernel/visualization.cpp‎
Lines changed: 25 additions & 7 deletions b/‎src/kernel/visualization.cpp‎
Lines changed: 25 additions & 7 deletions
diff --git a/‎src/kernel/visualization.hpp‎
Lines changed: 6 additions & 1 deletion b/‎src/kernel/visualization.hpp‎
Lines changed: 6 additions & 1 deletion
@@ -16,8 +16,7 @@ jobs:
     env:
       WS_DIR: ${{ github.workspace }}/ws
       SRC_DIR: ${{ github.workspace }}/ws/src
-      IMAGE_URL: https://pub-997cd3005edc4b9db91df913907990bf.r2.dev/autoaim/model_infer_example.jpg
-      IMAGE_PATH: /tmp/model_infer_example.jpg
+      TEST_ASSETS_ROOT: /tmp/auto_aim
 
     steps:
       - name: Checkout repository into ROS2 workspace
@@ -26,9 +25,16 @@ jobs:
           repository: Alliance-Algorithm/rmcs_auto_aim_v2
           path: ws/src/rmcs_auto_aim_v2
 
-      - name: Download test frame
+      - name: Install yq dependency
         run: |
-          curl -fsSL "$IMAGE_URL" -o "$IMAGE_PATH"
+          apt-get update
+          apt-get install -y yq
+
+      - name: Download test assets
+        shell: bash
+        run: |
+          cd "$SRC_DIR/rmcs_auto_aim_v2/test"
+          TEST_ASSETS_ROOT="$TEST_ASSETS_ROOT" ./download_assets.sh
 
       - name: Configure test project
         shell: bash
@@ -49,4 +55,4 @@ jobs:
         run: |
           source /opt/ros/jazzy/setup.bash
           cd "$SRC_DIR/rmcs_auto_aim_v2/test"
-          IMAGE="$IMAGE_PATH" ctest --test-dir build --output-on-failure
+          ctest --test-dir build --output-on-failure
@@ -250,4 +250,4 @@ auto use_object(Object& object) -> void {
 
 ### 4. 自动化与测试：
 
-一个工程化项目，其测试代码应该占据**一半左右**的代码量，特别是对于 RM 这种对代码稳定性有高要求的场景，同时 `CI/CD` 的妥善使用，可以大大降低我们在更新，部署等场景所花费的精力
+一个工程化项目，其测试代码应该占据**一半左右**的代码量，特别是对于 RM 这种对代码稳定性有高要求的场景，同时 `CI/CD` 的妥善使用，可以大大降低我们在更新，部署等场景所花费的精力
@@ -5,14 +5,14 @@ capturer:
   show_loss_framerate: false
   show_loss_framerate_interval: 500
   reconnect_wait_interval: 100
-  source: "hikcamera"
+  source: "local_video"
   hikcamera:
     # int
     timeout_ms: 500
     # float
-    exposure_us: 3000.0
+    exposure_us: 2000.0
     # float
-    framerate: 120
+    framerate: 60
     # float
     gain: 16.9807
 
@@ -21,8 +21,14 @@ capturer:
     trigger_mode: false
     fixed_framerate: true
   local_video:
-    location: ""
+    # 替换为你具体的路径
+    location: "/workspaces/alliance/test_videos/solve_pnp_v2.mp4"
+    # double 帧率
+    frame_rate: 60
+    # bool 是否循环播放
     loop_play: true
+    # bool 是否允许跳帧以满足实时性
+    allow_skipping: false
 
 identifier:
   binarization_threshold: 0.5
@@ -41,8 +47,8 @@ identifier:
   nms_threshold: 0.3
 
 pose_estimator:
-  camera_matrix: [2414.9359264386621, 0, 717.26243105567414, 0, 2418.0489262208148, 582.68540529942845, 0, 0, 1]
-  distort_coeff: [-0.0209453389287673, 0.15028138841073832, -0.0006517722113234505, -0.0016861906197686788, 0]
+  camera_matrix: [1.722231837421459e+03, 0, 7.013056440882832e+02, 0, 1.724876404292754e+03,5.645821718351237e+02 , 0, 0, 1]
+  distort_coeff: [-0.064232403853946, -0.087667493884102, 0,0, 0.792381808294582]
 
   transforms:
     - parent: "imu_link"
 
@@ -12,10 +12,11 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 
-  <!-- 唯一添加的依赖，为了保证正常的依赖构建顺序 -->
+<!-- 添加的两个依赖，为了保证正常的依赖构建顺序 -->
   <depend>rmcs_executor</depend>
+  <depend>hikcamera</depend>
 
   <export>
     <build_type>ament_cmake</build_type>
   </export>
-</package>
+</package>
@@ -21,6 +21,21 @@ class AutoAimComponent final : public rmcs_executor::Component {
     }
 
     auto update() -> void override {
+        using namespace rmcs_description;
+        if (rmcs_tf.ready()) [[likely]] {
+            auto camera_odom =
+                fast_tf::lookup_transform<rmcs_description::CameraLink, rmcs_description::OdomImu>(
+                    *rmcs_tf);
+
+            control_state.timestamp = Clock::now();
+
+            control_state.camera_to_odom_transform.posture = camera_odom.translation();
+            control_state.camera_to_odom_transform.orientation =
+                Eigen::Quaterniond(camera_odom.rotation());
+
+            //...
+        }
+
         recv_state();
         send_state();
     }
@@ -33,6 +48,8 @@ class AutoAimComponent final : public rmcs_executor::Component {
     ControlClient::Send shm_send;
     ControlClient::Recv shm_recv;
 
+    ControlState control_state;
+
     FramerateCounter framerate;
 
 private:
@@ -46,6 +63,7 @@ class AutoAimComponent final : public rmcs_executor::Component {
 
         if (shm_recv.is_updated()) {
             auto timestamp = Stamp {};
+
             shm_recv.with_read([&](const auto& state) { timestamp = state.timestamp; });
 
             if (shm_recv.is_updated()) {
@@ -67,8 +85,9 @@ class AutoAimComponent final : public rmcs_executor::Component {
             return;
         }
 
-        shm_send.with_write([](ControlState& state) {
-            state.timestamp = Clock::now();
+        shm_send.with_write([&](ControlState& state) {
+            state = control_state;
+
             // ...
         });
     }
 
@@ -1,7 +1,9 @@
 #include "pose_estimator.hpp"
+
 #include "kernel/transform_tree.hpp"
 #include "utility/logging/printer.hpp"
-#include "utility/math/solve_pnp.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"
 
@@ -16,25 +18,24 @@ struct PoseEstimator::Impl {
         std::array<float, 9> camera_matrix;
         std::array<float, 5> distort_coeff;
 
-        // clang-format off
         constexpr static std::tuple metas {
-            &Config::camera_matrix, "camera_matrix",
-            &Config::distort_coeff, "distort_coeff",
+            &Config::camera_matrix,
+            "camera_matrix",
+            &Config::distort_coeff,
+            "distort_coeff",
         };
-        // clang-format on
     };
+
     Config config;
+    PnpSolution pnp_solution {};
 
     Printer log { "PoseEstimator" };
 
-    PnpSolution pnp_solution;
-
     auto initialize(const YAML::Node& yaml) noexcept -> std::expected<void, std::string> try {
         auto result = config.serialize(yaml);
         if (!result.has_value()) {
             return std::unexpected { result.error() };
         }
-
         {
             auto result = serialize_from<tf::AutoAim>(yaml["transforms"]);
             if (!result.has_value()
@@ -43,20 +44,71 @@ struct PoseEstimator::Impl {
                     + util::to_string(result.error()) };
             }
         }
+        {
+            pnp_solution.input.camera_matrix =
+                reshape_array<float, 9, double, 3, 3>(config.camera_matrix);
+            pnp_solution.input.distort_coeff =
+                reshape_array<float, 5, double>(config.distort_coeff);
+        }
 
         return {};
     } catch (const std::exception& e) {
         return std::unexpected { e.what() };
     }
 
-    auto transform() { }
+    auto solve_pnp(std::vector<Armor2D> const& armors) -> std::optional<std::vector<Armor3D>> {
+        if (armors.empty()) return std::nullopt;
+
+        auto armor_shape = [](ArmorShape shape) {
+            if (shape == ArmorShape::SMALL) {
+                return rmcs::kSmallArmorShapeOpenCV;
+            } else {
+                return rmcs::kLargeArmorShapeOpenCV;
+            }
+        };
+
+        auto armors_in_camera = std::vector<Armor3D> {};
+
+        std::ranges::for_each(armors | std::views::enumerate,
+            [&armors_in_camera, &armor_shape, this](auto const& item) {
+                auto [i, armor] = item;
+
+                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 {} ({} {})", 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    = i;
+                pnp_solution.result.translation.copy_to(armor_3d.translation);
+                pnp_solution.result.orientation.copy_to(armor_3d.orientation);
+
+                armors_in_camera.emplace_back(armor_3d);
+            });
+
+        return armors_in_camera;
+    }
 };
 
 auto PoseEstimator::initialize(const YAML::Node& yaml) noexcept
     -> std::expected<void, std::string> {
     return pimpl->initialize(yaml);
 }
 
+auto PoseEstimator::solve_pnp(std::vector<Armor2D> const& armors) const
+    -> std::optional<std::vector<Armor3D>> {
+    return pimpl->solve_pnp(armors);
+}
+
 PoseEstimator::PoseEstimator() noexcept
     : pimpl { std::make_unique<Impl>() } { }
 
 
@@ -1,6 +1,9 @@
 #pragma once
+
 #include "utility/math/linear.hpp"
 #include "utility/pimpl.hpp"
+#include "utility/rclcpp/node.hpp"
+#include "utility/robot/armor.hpp"
 #include <expected>
 #include <yaml-cpp/yaml.h>
 
@@ -10,8 +13,14 @@ class PoseEstimator {
     RMCS_PIMPL_DEFINITION(PoseEstimator)
 
 public:
+    using RclcppNode = util::RclcppNode;
+
     auto initialize(const YAML::Node&) noexcept -> std::expected<void, std::string>;
 
+    auto visualize(RclcppNode& visual_node) -> void;
+
+    auto solve_pnp(std::vector<Armor2D> const&) const -> std::optional<std::vector<Armor3D>>;
+
     auto update_imu_link(const Orientation&) noexcept -> void;
 };
 
 
@@ -1,13 +1,15 @@
 #include "visualization.hpp"
-#include "module/debug/visualization/stream_session.hpp"
 
+#include <fstream>
+
+#include "module/debug/visualization/armor_visualizer.hpp"
+#include "module/debug/visualization/stream_session.hpp"
 #include "utility/image/image.details.hpp"
 #include "utility/logging/printer.hpp"
 #include "utility/serializable.hpp"
 
-#include <fstream>
-
 using namespace rmcs::kernel;
+using namespace rmcs::util;
 
 constexpr std::array kVideoTypes {
     "RTP_JEPG",
@@ -47,9 +49,14 @@ struct Visualization::Impl {
     bool is_initialized  = false;
     bool size_determined = false;
 
-    Impl() noexcept { session = std::make_unique<debug::StreamSession>(); }
+    std::unique_ptr<debug::ArmorVisualizer> armor_visualizer;
+
+    Impl() noexcept {
+        session          = std::make_unique<debug::StreamSession>();
+        armor_visualizer = std::make_unique<debug::ArmorVisualizer>();
+    }
 
-    auto initialize(const YAML::Node& yaml) noexcept -> NormalResult {
+    auto initialize(const YAML::Node& yaml, RclcppNode& visual_node) noexcept -> NormalResult {
         auto config = Config {};
         auto result = config.serialize(yaml);
         if (!result.has_value()) {
@@ -67,6 +74,9 @@ struct Visualization::Impl {
         } else {
             return std::unexpected { "Unknown video type: " + config.stream_type };
         }
+
+        armor_visualizer->initialize(visual_node);
+
         is_initialized = true;
         return {};
     }
@@ -114,11 +124,15 @@ struct Visualization::Impl {
 
         return session->push_frame(mat);
     }
+    auto visualize_armors(std::span<Armor3D> const& armors) const -> bool {
+        if (!is_initialized) return false;
+        return armor_visualizer->visualize(armors);
+    }
 };
 
-auto Visualization::initialize(const YAML::Node& yaml) noexcept
+auto Visualization::initialize(const YAML::Node& yaml, RclcppNode& visual_node) noexcept
     -> std::expected<void, std::string> {
-    return pimpl->initialize(yaml);
+    return pimpl->initialize(yaml, visual_node);
 }
 
 auto Visualization::initialized() const noexcept -> bool { return pimpl->initialized(); }
@@ -127,6 +141,10 @@ auto Visualization::send_image(const Image& image) noexcept -> bool {
     return pimpl->send_image(image);
 }
 
+auto Visualization::visualize_armors(std::span<Armor3D> const& armors) const -> bool {
+    return pimpl->visualize_armors(armors);
+}
+
 Visualization::Visualization() noexcept
     : pimpl { std::make_unique<Impl>() } { }
 
 
@@ -1,5 +1,7 @@
 #pragma once
 #include "utility/image/image.hpp"
+#include "utility/rclcpp/node.hpp"
+#include "utility/robot/armor.hpp"
 
 #include <expected>
 #include <yaml-cpp/yaml.h>
@@ -17,11 +19,14 @@ class Visualization {
     }
 
 public:
-    auto initialize(const YAML::Node&) noexcept -> std::expected<void, std::string>;
+    auto initialize(const YAML::Node& yaml, util::RclcppNode& visual_node) noexcept
+        -> std::expected<void, std::string>;
 
     auto initialized() const noexcept -> bool;
 
     auto send_image(const Image&) noexcept -> bool;
+
+    auto visualize_armors(std::span<Armor3D> const& armors) const -> bool;
 };
 
 }
Original file line number	Diff line number	Diff line change
`@@ -250,4 +250,4 @@ auto use_object(Object& object) -> void {`
`250`	`250`
`251`	`251`	`### 4. 自动化与测试：`
`252`	`252`
`253`		-一个工程化项目，其测试代码应该占据一半左右的代码量，特别是对于 RM 这种对代码稳定性有高要求的场景，同时 `CI/CD` 的妥善使用，可以大大降低我们在更新，部署等场景所花费的精力
	`253`	+一个工程化项目，其测试代码应该占据一半左右的代码量，特别是对于 RM 这种对代码稳定性有高要求的场景，同时 `CI/CD` 的妥善使用，可以大大降低我们在更新，部署等场景所花费的精力