bf

src/rm_rune/README.md

@@ -0,0 +1,21 @@
+# rm_rune
+
+FYT视觉24赛季能量机关识别和预测算法ROS2功能包
+
+## 维护者及开源许可证
+
+Maintainer : Chengfu Zou,  chengfuzou@outlook.com
+
+```
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+```

src/rm_rune/rm_rune/CMakeLists.txt

@@ -0,0 +1,12 @@
+cmake_minimum_required(VERSION 3.8)
+project(rm_rune)
+
+find_package(ament_cmake_auto REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  set(ament_cmake_copyright_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package()

src/rm_rune/rm_rune/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rm_rune</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="chengfuzou@outlook.com">chengfuzou</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rm_interfaces</depend>
+  <depend>rune_detector</depend>
+  <depend>rune_solver</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/rm_rune/rune_detector/CMakeLists.txt

@@ -0,0 +1,64 @@
+cmake_minimum_required(VERSION 3.10)
+project(rune_detector)
+
+## Use C++14
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+## By adding -Wall and -Werror, the compiler does not ignore warnings anymore,
+## enforcing cleaner code.
+add_definitions(-Wall -Werror -O3)
+
+## Export compile commands for clangd
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+# find dependencies
+find_package(ament_cmake_auto REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(OpenVINO REQUIRED COMPONENTS Runtime ONNX)
+ament_auto_find_build_dependencies()
+
+###########
+## Build ##
+###########
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  DIRECTORY src
+)
+
+target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS} ${EIGEN3_LIBS}   openvino::frontend::onnx openvino::runtime)
+target_include_directories(${PROJECT_NAME} PUBLIC ${OpenCV_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS})
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN fyt::rune::RuneDetectorNode
+  EXECUTABLE rune_detector_node
+)
+
+#############
+## Testing ##
+#############
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  list(APPEND AMENT_LINT_AUTO_EXCLUDE
+    ament_cmake_copyright
+    ament_cmake_uncrustify
+    ament_cmake_cpplint
+  )
+  ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gtest)
+  ament_add_gtest(test_detector test/test_detector.cpp)
+  target_link_libraries(test_detector ${PROJECT_NAME})
+
+endif()
+
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  model
+  docs
+)
+

src/rm_rune/rune_detector/README.md

@@ -0,0 +1,26 @@
+# rune_detector
+
+FYT视觉24赛季能量机关识别ROS2包。从图像中识别出能量机关的5个关键点。
+
+OpenVINO推理部分参考了[rm_vision-OpenVINO](https://github.com/Ericsii/rm_vision-OpenVINO)
+
+## fyt::rune::RuneDetectorNode
+
+能量机关识别器节点，订阅图像话题，调用OpenVINO进行网络推理，定位出能量机关的五个关键点
+
+### 发布话题 
+
+* `rune_target` (`rm_interfaces/msg/RuneTarget`) - 识别到的待击打能量机关五个关键点
+*  `bebug_img` (`sensor_msgs/msg/Image`) - debug图像，绘制出识别到的目标和识别R标的二值化ROI
+
+### 订阅话题 
+
+* `image_raw` (`sensor_msgs/msg/Image`) - 工业相机采集的图像
+
+### 参数 
+
+* `detector.model` (string, default: "yolox_rune.onnx") - 训练好的网络权重文件.
+* `detector.device_type` (string, default: CPU) - 推理网络的设备，可选，CPU/GPU/AUTO 之一.
+* `debug` (bool, default: true) - 是否开启debug模式.
+* `requests_limit` (int, default: 5) - 推理请求队列的最大长度，大于0时意味着异步推理，会消耗更多的处理器资源换取推理速度.
+* `detect_r_tag` (bool, default: true) - 是否使用传统方法识别R标，相比网络预测，传统方法识别R标会更稳定.

src/rm_rune/rune_detector/include/rune_detector/rune_detector.hpp

@@ -0,0 +1,93 @@
+// Copyright 2023 Yunlong Feng
+//
+// Additional modifications and features by Chengfu Zou, 2024.
+//
+// Copyright (C) FYT Vision Group. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RUNE_DETECTOR_RUNE_DETECTOR_HPP_
+#define RUNE_DETECTOR_RUNE_DETECTOR_HPP_
+
+// std
+#include <filesystem>
+#include <functional>
+#include <future>
+#include <memory>
+#include <string>
+#include <vector>
+#include <tuple>
+// third party
+#include <Eigen/Dense>
+#include <opencv2/opencv.hpp>
+#include <openvino/openvino.hpp>
+// project
+#include "rune_detector/types.hpp"
+
+namespace fyt::rune {
+struct GridAndStride {
+  int grid0;
+  int grid1;
+  int stride;
+};
+
+class RuneDetector {
+public:
+  using CallbackType = std::function<void(std::vector<RuneObject> &, int64_t, const cv::Mat &)>;
+
+public:
+  // Construct a new OpenVINO Detector object
+  explicit RuneDetector(const std::filesystem::path &model_path,
+                        const std::string &device_name,
+                        float conf_threshold = 0.25,
+                        int top_k = 128,
+                        float nms_threshold = 0.3,
+                        bool auto_init = false);
+
+  void init();
+
+  // Push an inference request to the detector
+  std::future<bool> pushInput(const cv::Mat &rgb_img, int64_t timestamp_nanosec);
+
+  void setCallback(CallbackType callback);
+
+  // Detect R tag using traditional method
+  // Return the center of the R tag and binary roi image (for debug)
+  std::tuple<cv::Point2f, cv::Mat> detectRTag(const cv::Mat &img,
+                                              int binary_thresh,
+                                              const cv::Point2f &prior);
+
+private:
+  // Do inference and call the infer_callback_ after inference
+  bool processCallback(const cv::Mat resized_img,
+                       Eigen::Matrix3f transform_matrix,
+                       int64_t timestamp_nanosec,
+                       const cv::Mat &src_img);
+
+private:
+  std::string model_path_;
+  std::string device_name_;
+  float conf_threshold_;
+  int top_k_;
+  float nms_threshold_;
+  std::mutex mtx_;
+  std::vector<int> strides_;
+  std::vector<GridAndStride> grid_strides_;
+
+  CallbackType infer_callback_;
+
+  std::unique_ptr<ov::Core> ov_core_;
+  std::unique_ptr<ov::CompiledModel> compiled_model_;
+};
+}  // namespace fyt::rune
+#endif  // RUNE_DETECTOR_RUNE_DETECTOR_HPP_

src/rm_rune/rune_detector/include/rune_detector/rune_detector_node.hpp

@@ -0,0 +1,97 @@
+// Copyright (C) FYT Vision Group. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RUNE_DETECTOR_RUNE_DETECTOR_NODE_HPP_
+#define RUNE_DETECTOR_RUNE_DETECTOR_NODE_HPP_
+
+// std
+#include <algorithm>
+#include <array>
+#include <string>
+#include <vector>
+// ros2
+#include <image_transport/image_transport.hpp>
+#include <image_transport/publisher.hpp>
+#include <image_transport/subscriber_filter.hpp>
+#include <rcl_interfaces/msg/set_parameters_result.hpp>
+#include <rclcpp/publisher.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/image.hpp>
+// 3rd party
+#include <opencv2/core.hpp>
+#include <opencv2/opencv.hpp>
+// project
+#include "rm_interfaces/msg/rune_target.hpp"
+#include "rm_interfaces/msg/serial_receive_data.hpp"
+#include "rm_interfaces/srv/set_mode.hpp"
+#include "rm_utils/common.hpp"
+#include "rm_utils/heartbeat.hpp"
+#include "rune_detector/rune_detector.hpp"
+
+namespace fyt::rune {
+class RuneDetectorNode : public rclcpp::Node {
+public:
+  RuneDetectorNode(const rclcpp::NodeOptions &options);
+
+private:
+  std::unique_ptr<RuneDetector> initDetector();
+
+  void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr img_msg);
+  void inferResultCallback(std::vector<RuneObject> &rune_objects,
+                           int64_t timestamp_nanosec,
+                           const cv::Mat &img);
+
+  void createDebugPublishers();
+  void destroyDebugPublishers();
+
+  void setModeCallback(const std::shared_ptr<rm_interfaces::srv::SetMode::Request> request,
+                       std::shared_ptr<rm_interfaces::srv::SetMode::Response> response);
+  // Dynamic Parameter
+  rcl_interfaces::msg::SetParametersResult onSetParameters(
+    std::vector<rclcpp::Parameter> parameters);
+  rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_handle_;
+
+  // Heartbeat
+  HeartBeatPublisher::SharedPtr heartbeat_;
+
+  // Image subscription
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr img_sub_;
+
+  //Target publisher
+  std::string frame_id_;
+  rclcpp::Publisher<rm_interfaces::msg::RuneTarget>::SharedPtr rune_pub_;
+
+  // Enable/Disable Rune Detector
+  rclcpp::Service<rm_interfaces::srv::SetMode>::SharedPtr set_rune_mode_srv_;
+
+  // Rune detector
+  int requests_limit_;
+  std::queue<std::future<bool>> detect_requests_;
+  std::unique_ptr<RuneDetector> rune_detector_;
+
+  // Rune params
+  EnemyColor detect_color_;
+  bool is_rune_;
+  bool is_big_rune_;
+
+  // For R tag detection
+  bool detect_r_tag_;
+  int binary_thresh_;
+
+  // Debug infomation
+  bool debug_;
+  image_transport::Publisher result_img_pub_;
+};
+}  // namespace fyt::rune
+#endif  // RUNE_DETECTOR_RUNE_DETECTOR_NODE_HPP_

src/rm_rune/rune_detector/include/rune_detector/types.hpp

@@ -0,0 +1,89 @@
+// Copyright (C) FYT Vision Group. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RUNE_DETECTOR_TYPES_HPP_
+#define RUNE_DETECTOR_TYPES_HPP_
+
+// 3rd party
+#include <opencv2/opencv.hpp>
+// project
+#include "rm_utils/common.hpp"
+
+namespace fyt::rune {
+
+enum class RuneType { INACTIVATED = 0, ACTIVATED };
+
+struct FeaturePoints {
+  FeaturePoints() {
+    r_center = cv::Point2f(-1, -1);
+    bottom_right = cv::Point2f(-1, -1);
+    top_right = cv::Point2f(-1, -1);
+    top_left = cv::Point2f(-1, -1);
+    bottom_left = cv::Point2f(-1, -1);
+  }
+
+  void reset() {
+    r_center = cv::Point2f(-1, -1);
+    bottom_right = cv::Point2f(-1, -1);
+    top_right = cv::Point2f(-1, -1);
+    top_left = cv::Point2f(-1, -1);
+    bottom_left = cv::Point2f(-1, -1);
+  }
+
+  FeaturePoints operator+(const FeaturePoints &other) {
+    FeaturePoints res;
+    res.r_center = r_center + other.r_center;
+    res.bottom_right = bottom_right + other.bottom_right;
+    res.top_right = top_right + other.top_right;
+    res.top_left = top_left + other.top_left;
+    res.bottom_left = bottom_left + other.bottom_left;
+    return res;
+  }
+
+  FeaturePoints operator/(const float &other) {
+    FeaturePoints res;
+    res.r_center = r_center / other;
+    res.bottom_right = bottom_right / other;
+    res.top_right = top_right / other;
+    res.top_left = top_left / other;
+    res.bottom_left = bottom_left / other;
+    return res;
+  }
+
+  std::vector<cv::Point2f> toVector2f() const {
+    return {r_center, bottom_left, top_left, top_right, bottom_right};
+  }
+  std::vector<cv::Point> toVector2i() const {
+    return {r_center, bottom_left, top_left, top_right, bottom_right};
+  }
+
+  cv::Point2f r_center;
+  cv::Point2f bottom_right;
+  cv::Point2f top_right;
+  cv::Point2f top_left;
+  cv::Point2f bottom_left;
+
+  std::vector<FeaturePoints> children;
+};
+
+struct RuneObject {
+  EnemyColor color;
+  RuneType type;
+  float prob;
+  FeaturePoints pts;
+  cv::Rect box;
+};
+
+}  // namespace fyt::rune
+#endif  // RUNE_DETECTOR_TYPES_HPP_

src/rm_rune/rune_detector/package.xml

@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rune_detector</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="uu@todo.todo">uu</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <!-- depend: build, export, and execution dependency -->
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>eigen</depend>
+  <depend>rm_utils</depend>
+  <depend>visualization_msgs</depend>
+  <depend>message_filters</depend>
+  <depend>cv_bridge</depend>
+  <depend>image_transport</depend>
+  <depend>image_transport_plugins</depend>
+  <depend>rm_interfaces</depend>
+  <depend>vision_opencv</depend>
+  <depend>tf2</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/rm_rune/rune_detector/src/rune_detector.cpp

@@ -0,0 +1,426 @@
+// Copyright 2023 Yunlong Feng
+//
+// Additional modifications and features by Chengfu Zou, 2024.
+//
+// Copyright (C) FYT Vision Group. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rune_detector/rune_detector.hpp"
+// std
+#include <algorithm>
+#include <numeric>
+#include <unordered_map>
+// third party
+#include <opencv2/imgproc.hpp>
+// project
+#include "rm_utils/common.hpp"
+#include "rune_detector/types.hpp"
+
+namespace fyt::rune {
+
+static constexpr int INPUT_W = 480;   // Width of input
+static constexpr int INPUT_H = 480;   // Height of input
+static constexpr int NUM_CLASSES = 2; // Number of classes
+static constexpr int NUM_COLORS = 2;  // Number of color
+static constexpr int NUM_POINTS = 5;
+static constexpr int NUM_POINTS_2 = 2 * NUM_POINTS;
+static constexpr float MERGE_CONF_ERROR = 0.15;
+static constexpr float MERGE_MIN_IOU = 0.9;
+// 由于训练失误，网络的颜色是反的
+static std::unordered_map<int, EnemyColor> DNN_COLOR_TO_ENEMY_COLOR = {
+    {0, EnemyColor::BLUE}, {1, EnemyColor::RED}};
+
+static cv::Mat letterbox(const cv::Mat &img, Eigen::Matrix3f &transform_matrix,
+                         std::vector<int> new_shape = {INPUT_W, INPUT_H}) {
+  // Get current image shape [height, width]
+
+  int img_h = img.rows;
+  int img_w = img.cols;
+
+  // Compute scale ratio(new / old) and target resized shape
+  float scale =
+      std::min(new_shape[1] * 1.0 / img_h, new_shape[0] * 1.0 / img_w);
+  int resize_h = static_cast<int>(round(img_h * scale));
+  int resize_w = static_cast<int>(round(img_w * scale));
+
+  // Compute padding
+  int pad_h = new_shape[1] - resize_h;
+  int pad_w = new_shape[0] - resize_w;
+
+  // Resize and pad image while meeting stride-multiple constraints
+  cv::Mat resized_img;
+  cv::resize(img, resized_img, cv::Size(resize_w, resize_h));
+
+  // divide padding into 2 sides
+  float half_h = pad_h * 1.0 / 2;
+  float half_w = pad_w * 1.0 / 2;
+
+  // Compute padding boarder
+  int top = static_cast<int>(round(half_h - 0.1));
+  int bottom = static_cast<int>(round(half_h + 0.1));
+  int left = static_cast<int>(round(half_w - 0.1));
+  int right = static_cast<int>(round(half_w + 0.1));
+
+  /* clang-format off */
+  /* *INDENT-OFF* */
+
+  // Compute point transform_matrix
+  transform_matrix << 1.0 / scale, 0, -half_w / scale,
+                      0, 1.0 / scale, -half_h / scale,
+                      0, 0, 1;
+
+  /* *INDENT-ON* */
+  /* clang-format on */
+
+  // Add border
+  cv::copyMakeBorder(resized_img, resized_img, top, bottom, left, right,
+                     cv::BORDER_CONSTANT, cv::Scalar(114, 114, 114));
+
+  return resized_img;
+}
+
+// Generate grids and stride for post processing
+// target_w: Width of input.
+// target_h: Height of input.
+// strides A vector of stride.
+// grid_strides Grid stride generated in this function
+static void generateGridsAndStride(const int target_w, const int target_h,
+                                   std::vector<int> &strides,
+                                   std::vector<GridAndStride> &grid_strides) {
+  for (auto stride : strides) {
+    int num_grid_w = target_w / stride;
+    int num_grid_h = target_h / stride;
+
+    for (int g1 = 0; g1 < num_grid_h; g1++) {
+      for (int g0 = 0; g0 < num_grid_w; g0++) {
+        grid_strides.emplace_back(GridAndStride{g0, g1, stride});
+      }
+    }
+  }
+}
+
+// Decode output tensor
+static void generateProposals(
+    std::vector<RuneObject> &output_objs, std::vector<float> &scores,
+    std::vector<cv::Rect> &rects, const cv::Mat &output_buffer,
+    const Eigen::Matrix<float, 3, 3> &transform_matrix, float conf_threshold,
+    std::vector<GridAndStride> grid_strides) {
+  const int num_anchors = grid_strides.size();
+
+  for (int anchor_idx = 0; anchor_idx < num_anchors; anchor_idx++) {
+    float confidence = output_buffer.at<float>(anchor_idx, NUM_POINTS_2);
+    if (confidence < conf_threshold) {
+      continue;
+    }
+
+    const int grid0 = grid_strides[anchor_idx].grid0;
+    const int grid1 = grid_strides[anchor_idx].grid1;
+    const int stride = grid_strides[anchor_idx].stride;
+
+    double color_score, class_score;
+    cv::Point color_id, class_id;
+    cv::Mat color_scores =
+        output_buffer.row(anchor_idx)
+            .colRange(NUM_POINTS_2 + 1, NUM_POINTS_2 + 1 + NUM_COLORS);
+    cv::Mat num_scores =
+        output_buffer.row(anchor_idx)
+            .colRange(NUM_POINTS_2 + 1 + NUM_COLORS,
+                      NUM_POINTS_2 + 1 + NUM_COLORS + NUM_CLASSES);
+    // Argmax
+    cv::minMaxLoc(color_scores, NULL, &color_score, NULL, &color_id);
+    cv::minMaxLoc(num_scores, NULL, &class_score, NULL, &class_id);
+
+    float x_1 = (output_buffer.at<float>(anchor_idx, 0) + grid0) * stride;
+    float y_1 = (output_buffer.at<float>(anchor_idx, 1) + grid1) * stride;
+    float x_2 = (output_buffer.at<float>(anchor_idx, 2) + grid0) * stride;
+    float y_2 = (output_buffer.at<float>(anchor_idx, 3) + grid1) * stride;
+    float x_3 = (output_buffer.at<float>(anchor_idx, 4) + grid0) * stride;
+    float y_3 = (output_buffer.at<float>(anchor_idx, 5) + grid1) * stride;
+    float x_4 = (output_buffer.at<float>(anchor_idx, 6) + grid0) * stride;
+    float y_4 = (output_buffer.at<float>(anchor_idx, 7) + grid1) * stride;
+    float x_5 = (output_buffer.at<float>(anchor_idx, 8) + grid0) * stride;
+    float y_5 = (output_buffer.at<float>(anchor_idx, 9) + grid1) * stride;
+
+    Eigen::Matrix<float, 3, 5> apex_norm;
+    Eigen::Matrix<float, 3, 5> apex_dst;
+
+    /* clang-format off */
+    /* *INDENT-OFF* */
+    apex_norm << x_1, x_2, x_3, x_4, x_5,
+                y_1, y_2, y_3, y_4, y_5,
+                1,   1,   1,   1,   1;
+    /* *INDENT-ON* */
+    /* clang-format on */
+
+    apex_dst = transform_matrix * apex_norm;
+
+    RuneObject obj;
+
+    obj.pts.r_center = cv::Point2f(apex_dst(0, 0), apex_dst(1, 0));
+    obj.pts.bottom_left = cv::Point2f(apex_dst(0, 1), apex_dst(1, 1));
+    obj.pts.top_left = cv::Point2f(apex_dst(0, 2), apex_dst(1, 2));
+    obj.pts.top_right = cv::Point2f(apex_dst(0, 3), apex_dst(1, 3));
+    obj.pts.bottom_right = cv::Point2f(apex_dst(0, 4), apex_dst(1, 4));
+
+    auto rect = cv::boundingRect(obj.pts.toVector2f());
+
+    obj.box = rect;
+    obj.color = DNN_COLOR_TO_ENEMY_COLOR[color_id.x];
+    obj.type = static_cast<RuneType>(class_id.x);
+    obj.prob = confidence;
+
+    rects.push_back(rect);
+    scores.push_back(confidence);
+    output_objs.push_back(std::move(obj));
+  }
+}
+
+// Calculate intersection area between Object a and Object b.
+static inline float intersectionArea(const RuneObject &a, const RuneObject &b) {
+  cv::Rect_<float> inter = a.box & b.box;
+  return inter.area();
+}
+
+static void nmsMergeSortedBboxes(std::vector<RuneObject> &faceobjects,
+                                 std::vector<int> &indices,
+                                 float nms_threshold) {
+  indices.clear();
+
+  const int n = faceobjects.size();
+
+  std::vector<float> areas(n);
+  for (int i = 0; i < n; i++) {
+    areas[i] = faceobjects[i].box.area();
+  }
+
+  for (int i = 0; i < n; i++) {
+    RuneObject &a = faceobjects[i];
+
+    int keep = 1;
+    for (size_t j = 0; j < indices.size(); j++) {
+      RuneObject &b = faceobjects[indices[j]];
+
+      // intersection over union
+      float inter_area = intersectionArea(a, b);
+      float union_area = areas[i] + areas[indices[j]] - inter_area;
+      float iou = inter_area / union_area;
+      if (iou > nms_threshold || isnan(iou)) {
+        keep = 0;
+        // Stored for Merge
+        if (a.type == b.type && a.color == b.color && iou > MERGE_MIN_IOU &&
+            abs(a.prob - b.prob) < MERGE_CONF_ERROR) {
+          a.pts.children.push_back(b.pts);
+        }
+        // cout<<b.pts_x.size()<<endl;
+      }
+    }
+
+    if (keep) {
+      indices.push_back(i);
+    }
+  }
+}
+
+RuneDetector::RuneDetector(const std::filesystem::path &model_path,
+                           const std::string &device_name, float conf_threshold,
+                           int top_k, float nms_threshold, bool auto_init)
+    : model_path_(model_path), device_name_(device_name),
+      conf_threshold_(conf_threshold), top_k_(top_k),
+      nms_threshold_(nms_threshold) {
+  if (auto_init) {
+    init();
+  }
+}
+
+void RuneDetector::init() {
+  if (ov_core_ == nullptr) {
+    ov_core_ = std::make_unique<ov::Core>();
+  }
+
+  auto model = ov_core_->read_model(model_path_);
+
+  // Set infer type
+  ov::preprocess::PrePostProcessor ppp(model);
+  // Set input output precision
+  auto elem_type = device_name_ == "GPU" ? ov::element::f16 : ov::element::f32;
+  auto perf_mode =
+      device_name_ == "GPU"
+          ? ov::hint::performance_mode(ov::hint::PerformanceMode::THROUGHPUT)
+          : ov::hint::performance_mode(ov::hint::PerformanceMode::LATENCY);
+  ppp.input().tensor().set_element_type(elem_type);
+  ppp.output().tensor().set_element_type(elem_type);
+
+  // Compile model
+  compiled_model_ = std::make_unique<ov::CompiledModel>(
+      ov_core_->compile_model(model, device_name_, perf_mode));
+
+  strides_ = {8, 16, 32};
+  generateGridsAndStride(INPUT_W, INPUT_H, strides_, grid_strides_);
+}
+
+std::future<bool> RuneDetector::pushInput(const cv::Mat &rgb_img,
+                                          int64_t timestamp_nanosec) {
+  if (rgb_img.empty()) {
+    // return false when img is empty
+    return std::async([]() { return false; });
+  }
+
+  // Reprocess
+  Eigen::Matrix3f
+      transform_matrix; // transform matrix from resized image to source image.
+  cv::Mat resized_img = letterbox(rgb_img, transform_matrix);
+
+  // Start async detect
+  return std::async(std::launch::async, &RuneDetector::processCallback, this,
+                    resized_img, transform_matrix, timestamp_nanosec, rgb_img);
+}
+
+void RuneDetector::setCallback(CallbackType callback) {
+  infer_callback_ = callback;
+}
+
+bool RuneDetector::processCallback(const cv::Mat resized_img,
+                                   Eigen::Matrix3f transform_matrix,
+                                   int64_t timestamp_nanosec,
+                                   const cv::Mat &src_img) {
+  // BGR->RGB, u8(0-255)->f32(0.0-1.0), HWC->NCHW
+  // note: TUP's model no need to normalize
+  cv::Mat blob = cv::dnn::blobFromImage(
+      resized_img, 1., cv::Size(INPUT_W, INPUT_H), cv::Scalar(0, 0, 0), true);
+
+  // Feed blob into input
+  auto input_port = compiled_model_->input();
+  ov::Tensor input_tensor(
+      input_port.get_element_type(),
+      ov::Shape(std::vector<size_t>{1, 3, INPUT_W, INPUT_H}), blob.ptr(0));
+
+  // Start inference
+  // Lock because of the thread race condition within the openvino library
+  mtx_.lock();
+  auto infer_request = compiled_model_->create_infer_request();
+  infer_request.set_input_tensor(input_tensor);
+  infer_request.infer();
+  mtx_.unlock();
+
+  auto output = infer_request.get_output_tensor();
+
+  // Process output data
+  auto output_shape = output.get_shape();
+  // 3549 x 21 Matrix
+  cv::Mat output_buffer(output_shape[1], output_shape[2], CV_32F,
+                        output.data());
+
+  // Parsed variable
+  std::vector<RuneObject> objs_tmp, objs_result;
+  std::vector<cv::Rect> rects;
+  std::vector<float> scores;
+  std::vector<int> indices;
+
+  // Parse YOLO output
+  generateProposals(objs_tmp, scores, rects, output_buffer, transform_matrix,
+                    this->conf_threshold_, this->grid_strides_);
+
+  // TopK
+  std::sort(
+      objs_tmp.begin(), objs_tmp.end(),
+      [](const RuneObject &a, const RuneObject &b) { return a.prob > b.prob; });
+  if (objs_tmp.size() > static_cast<size_t>(this->top_k_)) {
+    objs_tmp.resize(this->top_k_);
+  }
+
+  nmsMergeSortedBboxes(objs_tmp, indices, this->nms_threshold_);
+
+  for (size_t i = 0; i < indices.size(); i++) {
+    objs_result.push_back(std::move(objs_tmp[indices[i]]));
+
+    if (objs_result[i].pts.children.size() > 0) {
+      const float N =
+          static_cast<float>(objs_result[i].pts.children.size() + 1);
+      FeaturePoints pts_final = std::accumulate(
+          objs_result[i].pts.children.begin(),
+          objs_result[i].pts.children.end(), objs_result[i].pts);
+      objs_result[i].pts = pts_final / N;
+    }
+  }
+
+  // NMS & TopK
+  // cv::dnn::NMSBoxes(
+  //   rects, scores, this->conf_threshold_, this->nms_threshold_, indices, 1.0,
+  //   this->top_k_);
+  // for (size_t i = 0; i < indices.size(); ++i) {
+  //   objs_result.push_back(std::move(objs_tmp[i]));
+  // }
+
+  // Call callback function
+  if (this->infer_callback_) {
+    this->infer_callback_(objs_result, timestamp_nanosec, src_img);
+    return true;
+  }
+
+  return false;
+}
+
+std::tuple<cv::Point2f, cv::Mat>
+RuneDetector::detectRTag(const cv::Mat &img, int binary_thresh,
+                         const cv::Point2f &prior) {
+  if (prior.x < 0 || prior.x > img.cols || prior.y < 0 || prior.y > img.rows) {
+    return {prior, cv::Mat::zeros(cv::Size(200, 200), CV_8UC3)};
+  }
+
+  // Create ROI
+  cv::Rect roi = cv::Rect(prior.x - 100, prior.y - 100, 200, 200) &
+                 cv::Rect(0, 0, img.cols, img.rows);
+  const cv::Point2f prior_in_roi = prior - cv::Point2f(roi.tl());
+
+  cv::Mat img_roi = img(roi);
+
+  // Gray -> Binary -> Dilate
+  cv::Mat gray_img;
+  cv::cvtColor(img_roi, gray_img, cv::COLOR_BGR2GRAY);
+  cv::Mat binary_img;
+  cv::threshold(gray_img, binary_img, 0, 255,
+                cv::THRESH_BINARY | cv::THRESH_OTSU);
+  cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
+  cv::dilate(binary_img, binary_img, kernel);
+
+  // Find contours
+  std::vector<std::vector<cv::Point>> contours;
+  cv::findContours(binary_img, contours, cv::RETR_EXTERNAL,
+                   cv::CHAIN_APPROX_NONE);
+
+  auto it = std::find_if(
+      contours.begin(), contours.end(),
+      [p = prior_in_roi](const std::vector<cv::Point> &contour) -> bool {
+        return cv::boundingRect(contour).contains(p);
+      });
+
+  // For visualization
+  cv::cvtColor(binary_img, binary_img, cv::COLOR_GRAY2BGR);
+
+  if (it == contours.end()) {
+    return {prior, binary_img};
+  }
+
+  cv::drawContours(binary_img, contours, it - contours.begin(),
+                   cv::Scalar(0, 255, 0), 2);
+
+  cv::Point2f center =
+      std::accumulate(it->begin(), it->end(), cv::Point(0, 0));
+  center /= static_cast<float>(it->size());
+  center += cv::Point2f(roi.tl());
+
+  return {center, binary_img};
+}
+
+} // namespace fyt::rune

src/rm_rune/rune_detector/src/rune_detector_node.cpp

@@ -0,0 +1,342 @@
+// Copyright (C) FYT Vision Group. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rune_detector/rune_detector_node.hpp"
+// ros2
+#include <cv_bridge/cv_bridge.h>
+#include <rmw/qos_profiles.h>
+
+#include <rclcpp/qos.hpp>
+// std
+#include <algorithm>
+#include <array>
+#include <filesystem>
+#include <numeric>
+#include <vector>
+// third party
+#include <opencv2/imgproc.hpp>
+// project
+#include "rm_utils/assert.hpp"
+#include "rm_utils/common.hpp"
+#include "rm_utils/logger/log.hpp"
+#include "rm_utils/url_resolver.hpp"
+#include "rune_detector/types.hpp"
+
+namespace fyt::rune {
+RuneDetectorNode::RuneDetectorNode(const rclcpp::NodeOptions &options)
+: Node("rune_detector", options), is_rune_(false) {
+  FYT_REGISTER_LOGGER("rune_detector", "~/fyt2024-log", INFO);
+  FYT_INFO("rune_detector", "Starting RuneDetectorNode!");
+
+  frame_id_ = declare_parameter("frame_id", "camera_optical_frame");
+  detect_r_tag_ = declare_parameter("detect_r_tag", true);
+  binary_thresh_ = declare_parameter("min_lightness", 100);
+  requests_limit_ = declare_parameter("requests_limit", 5);
+
+  // Detector
+  rune_detector_ = initDetector();
+  // Rune Publisher
+  rune_pub_ = this->create_publisher<rm_interfaces::msg::RuneTarget>("rune_detector/rune_target",
+                                                                     rclcpp::SensorDataQoS());
+
+  // Debug Publishers
+  this->debug_ = declare_parameter("debug", true);
+  if (this->debug_) {
+    createDebugPublishers();
+  }
+  auto qos = rclcpp::SensorDataQoS();
+  qos.keep_last(1);
+  img_sub_ = this->create_subscription<sensor_msgs::msg::Image>(
+    "image_raw", qos, std::bind(&RuneDetectorNode::imageCallback, this, std::placeholders::_1));
+  set_rune_mode_srv_ = this->create_service<rm_interfaces::srv::SetMode>(
+    "rune_detector/set_mode",
+    std::bind(
+      &RuneDetectorNode::setModeCallback, this, std::placeholders::_1, std::placeholders::_2));
+
+  // Heartbeat
+  heartbeat_ = HeartBeatPublisher::create(this);
+}
+
+std::unique_ptr<RuneDetector> RuneDetectorNode::initDetector() {
+  std::string model_path =
+    this->declare_parameter("detector.model", "package://rune_detector/model/yolox_rune_3.6m.onnx");
+  std::string device_type = this->declare_parameter("detector.device_type", "AUTO");
+  FYT_ASSERT(!model_path.empty());
+  FYT_INFO("rune_detector", "model : {}, device : {}", model_path, device_type);
+
+  float conf_threshold = this->declare_parameter("detector.confidence_threshold", 0.50);
+  int top_k = this->declare_parameter("detector.top_k", 128);
+  float nms_threshold = this->declare_parameter("detector.nms_threshold", 0.3);
+
+  namespace fs = std::filesystem;
+  fs::path resolved_path = utils::URLResolver::getResolvedPath(model_path);
+  FYT_ASSERT_MSG(fs::exists(resolved_path), resolved_path.string() + " Not Found");
+
+  // Set dynamic parameter callback
+  rcl_interfaces::msg::SetParametersResult onSetParameters(
+    std::vector<rclcpp::Parameter> parameters);
+  on_set_parameters_callback_handle_ = this->add_on_set_parameters_callback(
+    std::bind(&RuneDetectorNode::onSetParameters, this, std::placeholders::_1));
+
+  // Create detector
+  auto rune_detector = std::make_unique<RuneDetector>(
+    resolved_path, device_type, conf_threshold, top_k, nms_threshold);
+  // Set detect callback
+  rune_detector->setCallback(std::bind(&RuneDetectorNode::inferResultCallback,
+                                       this,
+                                       std::placeholders::_1,
+                                       std::placeholders::_2,
+                                       std::placeholders::_3));
+  // init detector
+  rune_detector->init();
+  return rune_detector;
+}
+
+void RuneDetectorNode::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr msg) {
+  if (is_rune_ == false) {
+    return;
+  }
+
+  // Limits request size
+  while (detect_requests_.size() > static_cast<size_t>(requests_limit_)) {
+    detect_requests_.front().get();
+    detect_requests_.pop();
+  }
+
+  auto timestamp = rclcpp::Time(msg->header.stamp);
+  frame_id_ = msg->header.frame_id;
+  auto img = cv_bridge::toCvCopy(msg, "rgb8")->image;
+
+  // Push image to detector
+  detect_requests_.push(rune_detector_->pushInput(img, timestamp.nanoseconds()));
+};
+
+rcl_interfaces::msg::SetParametersResult RuneDetectorNode::onSetParameters(
+  std::vector<rclcpp::Parameter> parameters) {
+  rcl_interfaces::msg::SetParametersResult result;
+  for (const auto &param : parameters) {
+    if (param.get_name() == "binary_thresh") {
+      binary_thresh_ = param.as_int();
+    }
+  }
+  result.successful = true;
+  return result;
+}
+
+void RuneDetectorNode::inferResultCallback(std::vector<RuneObject> &objs,
+                                           int64_t timestamp_nanosec,
+                                           const cv::Mat &src_img) {
+  auto timestamp = rclcpp::Time(timestamp_nanosec);
+  // Used to draw debug info
+  cv::Mat debug_img;
+  if (debug_) {
+    debug_img = src_img.clone();
+  }
+
+  rm_interfaces::msg::RuneTarget rune_msg;
+  rune_msg.header.frame_id = frame_id_;
+  rune_msg.header.stamp = timestamp;
+  rune_msg.is_big_rune = is_big_rune_;
+
+  // Erase all object that not match the color
+  objs.erase(
+    std::remove_if(objs.begin(),
+                   objs.end(),
+                   [c = detect_color_](const auto &obj) -> bool { return obj.color != c; }),
+    objs.end());
+
+  if (!objs.empty()) {
+    // Sort by probability
+    std::sort(objs.begin(), objs.end(), [](const RuneObject &a, const RuneObject &b) {
+      return a.prob > b.prob;
+    });
+
+    cv::Point2f r_tag;
+    cv::Mat binary_roi = cv::Mat::zeros(1, 1, CV_8UC3);
+    if (detect_r_tag_) {
+      // Detect R tag using traditional method
+      std::tie(r_tag, binary_roi) =
+        rune_detector_->detectRTag(src_img, binary_thresh_, objs.at(0).pts.r_center);
+    } else {
+      // Use the average center of all objects as the center of the R tag
+      r_tag = std::accumulate(objs.begin(),
+                              objs.end(),
+                              cv::Point2f(0, 0),
+                              [n = static_cast<float>(objs.size())](cv::Point2f p, auto &o) {
+                                return p + o.pts.r_center / n;
+                              });
+    }
+    // Assign the center of the R tag to all objects
+    std::for_each(objs.begin(), objs.end(), [r = r_tag](RuneObject &obj) { obj.pts.r_center = r; });
+
+    // Draw binary roi
+    if (debug_ && !debug_img.empty()) {
+      cv::Rect roi =
+        cv::Rect(debug_img.cols - binary_roi.cols, 0, binary_roi.cols, binary_roi.rows);
+      binary_roi.copyTo(debug_img(roi));
+      cv::rectangle(debug_img, roi, cv::Scalar(150, 150, 150), 2);
+    }
+
+    // The final target is the inactivated rune with the highest probability
+    auto result_it =
+      std::find_if(objs.begin(), objs.end(), [c = detect_color_](const auto &obj) -> bool {
+        return obj.type == RuneType::INACTIVATED && obj.color == c;
+      });
+
+    if (result_it != objs.end()) {
+      // FYT_DEBUG("rune_detector", "Detected!");
+      rune_msg.is_lost = false;
+      rune_msg.pts[0].x = result_it->pts.r_center.x;
+      rune_msg.pts[0].y = result_it->pts.r_center.y;
+      rune_msg.pts[1].x = result_it->pts.bottom_left.x;
+      rune_msg.pts[1].y = result_it->pts.bottom_left.y;
+      rune_msg.pts[2].x = result_it->pts.top_left.x;
+      rune_msg.pts[2].y = result_it->pts.top_left.y;
+      rune_msg.pts[3].x = result_it->pts.top_right.x;
+      rune_msg.pts[3].y = result_it->pts.top_right.y;
+      rune_msg.pts[4].x = result_it->pts.bottom_right.x;
+      rune_msg.pts[4].y = result_it->pts.bottom_right.y;
+    } else {
+      // All runes are activated
+      rune_msg.is_lost = true;
+    }
+  } else {
+    // All runes are not the target color
+    rune_msg.is_lost = true;
+  }
+
+  rune_pub_->publish(std::move(rune_msg));
+
+  if (debug_) {
+    if (debug_img.empty()) {
+      // Avoid debug_mode change in processing
+      return;
+    }
+
+    // Draw detection result
+    for (auto &obj : objs) {
+      auto pts = obj.pts.toVector2f();
+      cv::Point2f aim_point = std::accumulate(pts.begin() + 1, pts.end(), cv::Point2f(0, 0)) / 4;
+
+      cv::Scalar line_color =
+        obj.type == RuneType::INACTIVATED ? cv::Scalar(50, 255, 50) : cv::Scalar(255, 50, 255);
+      cv::putText(debug_img,
+                  fmt::format("{:.2f}", obj.prob),
+                  cv::Point2i(pts[1]),
+                  cv::FONT_HERSHEY_SIMPLEX,
+                  0.8,
+                  line_color,
+                  2);
+      cv::polylines(debug_img, obj.pts.toVector2i(), true, line_color, 2);
+      cv::circle(debug_img, aim_point, 5, line_color, -1);
+
+      std::string rune_type = obj.type == RuneType::INACTIVATED ? "_HIT" : "_OK";
+      std::string rune_color = enemyColorToString(obj.color);
+      cv::putText(debug_img,
+                  rune_color + rune_type,
+                  cv::Point2i(pts[2]),
+                  cv::FONT_HERSHEY_SIMPLEX,
+                  0.8,
+                  line_color,
+                  2);
+    }
+
+    auto end = this->get_clock()->now();
+    auto duration = end.seconds() - timestamp.seconds();
+    std::string letency = fmt::format("Latency: {:.3f}ms", duration * 1000);
+    cv::putText(debug_img,
+                letency,
+                cv::Point2i(10, 30),
+                cv::FONT_HERSHEY_SIMPLEX,
+                0.8,
+                cv::Scalar(0, 255, 255),
+                2);
+    result_img_pub_.publish(cv_bridge::CvImage(rune_msg.header, "rgb8", debug_img).toImageMsg());
+  }
+}
+
+void RuneDetectorNode::setModeCallback(
+  const std::shared_ptr<rm_interfaces::srv::SetMode::Request> request,
+  std::shared_ptr<rm_interfaces::srv::SetMode::Response> response) {
+  response->success = true;
+
+  VisionMode mode = static_cast<VisionMode>(request->mode);
+  std::string mode_name = visionModeToString(mode);
+  if (mode_name == "UNKNOWN") {
+    FYT_ERROR("rune_detector", "Invalid mode: {}", request->mode);
+    return;
+  }
+
+  auto createImageSub = [this]() {
+    if (img_sub_ == nullptr) {
+      img_sub_ = this->create_subscription<sensor_msgs::msg::Image>(
+        "image_raw",
+        rclcpp::SensorDataQoS(),
+        std::bind(&RuneDetectorNode::imageCallback, this, std::placeholders::_1));
+    }
+  };
+
+  switch (mode) {
+    case VisionMode::SMALL_RUNE_RED: {
+      is_rune_ = true;
+      is_big_rune_ = false;
+      detect_color_ = EnemyColor::RED;
+      createImageSub();
+      break;
+    }
+    case VisionMode::SMALL_RUNE_BLUE: {
+      is_rune_ = true;
+      is_big_rune_ = false;
+      detect_color_ = EnemyColor::BLUE;
+      createImageSub();
+      break;
+    }
+    case VisionMode::BIG_RUNE_RED: {
+      is_rune_ = true;
+      is_big_rune_ = true;
+      detect_color_ = EnemyColor::RED;
+      createImageSub();
+      break;
+    }
+    case VisionMode::BIG_RUNE_BLUE: {
+      is_rune_ = true;
+      is_big_rune_ = true;
+      detect_color_ = EnemyColor::BLUE;
+      createImageSub();
+      break;
+    }
+    default: {
+      is_rune_ = false;
+      is_big_rune_ = false;
+      img_sub_.reset();
+      break;
+    }
+  }
+
+  FYT_WARN("rune_detector", "Set Rune Mode: {}", visionModeToString(mode));
+}
+
+void RuneDetectorNode::createDebugPublishers() {
+  result_img_pub_ = image_transport::create_publisher(this, "rune_detector/result_img");
+}
+
+void RuneDetectorNode::destroyDebugPublishers() { result_img_pub_.shutdown(); }
+
+}  // namespace fyt::rune
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(fyt::rune::RuneDetectorNode)

src/rm_rune/rune_detector/test/test_detector.cpp

@@ -0,0 +1,75 @@
+// Copyright (C) FYT Vision Group. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// gest
+#include <gtest/gtest.h>
+// ros2
+#include <opencv2/imgproc.hpp>
+#include <rclcpp/executors.hpp>
+#include <rclcpp/node_options.hpp>
+#include <rclcpp/utilities.hpp>
+// std
+#include <memory>
+// opencv
+#include <opencv2/opencv.hpp>
+// project
+#include "rm_utils/common.hpp"
+#include "rm_utils/url_resolver.hpp"
+#include "rune_detector/rune_detector.hpp"
+#include "rune_detector/types.hpp"
+
+using namespace fyt;
+using namespace fyt::rune;
+TEST(RuneDetectorNodeTest, NodeStartupTest) {
+  // Init Params
+  std::string model_path = "package://rune_detector/model/yolox_rune_3.6m.onnx";
+  std::string device_type = "AUTO";
+
+  float conf_threshold = 0.5;
+  int top_k = 128;
+  float nms_threshold = 0.3;
+
+  namespace fs = std::filesystem;
+  fs::path resolved_path = utils::URLResolver::getResolvedPath(model_path);
+
+  // Create detector
+  auto rune_detector = std::make_unique<RuneDetector>(
+    resolved_path, device_type, conf_threshold, top_k, nms_threshold);
+
+  // Set detect callback
+  std::vector<RuneObject> runes;
+  rune_detector->setCallback([&runes](std::vector<RuneObject> &objs,
+                                      int64_t timestamp_nanosec,
+                                      const cv::Mat &src_img) { runes = objs; });
+
+  // init detector
+  rune_detector->init();
+
+  // Load test image
+  fs::path test_image_path =
+    utils::URLResolver::getResolvedPath("package://rune_detector/docs/test.png");
+  cv::Mat test_image = cv::imread(test_image_path.string(), cv::IMREAD_COLOR);
+  cv::cvtColor(test_image, test_image, cv::COLOR_BGR2RGB);
+
+  auto future = rune_detector->pushInput(test_image, 0);
+  future.get();
+
+  EXPECT_EQ(runes.size(), static_cast<size_t>(3));
+  std::sort(runes.begin(), runes.end(), [](const RuneObject &a, const RuneObject &b) {
+    return a.type < b.type;
+  });
+  EXPECT_EQ(runes[0].type, RuneType::INACTIVATED);
+  EXPECT_EQ(runes[1].type, RuneType::ACTIVATED);
+  EXPECT_EQ(runes[2].type, RuneType::ACTIVATED);
+}

src/rm_rune/rune_detector/test/test_node_startup.cpp

@@ -0,0 +1,28 @@
+// Copyright 2022 Chen Jun
+
+#include <gtest/gtest.h>
+
+#include <rclcpp/executors.hpp>
+#include <rclcpp/node_options.hpp>
+#include <rclcpp/utilities.hpp>
+
+// STD
+#include <memory>
+
+#include "rune_detector/rune_detector_node.hpp"
+
+TEST(RuneDetectorNodeTest, NodeStartupTest)
+{
+  rclcpp::NodeOptions options;
+  auto node = std::make_shared<fyt::rune::RuneDetectorNode>(options);
+  node.reset();
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  rclcpp::init(argc, argv);
+  auto result = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return result;
+}

src/rm_rune/rune_solver/CMakeLists.txt

@@ -0,0 +1,60 @@
+cmake_minimum_required(VERSION 3.10)
+project(rune_solver)
+
+## Use C++17
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+## By adding -Wall and -Werror, the compiler does not ignore warnings anymore,
+## enforcing cleaner code.
+add_definitions(-Wall -Werror -O3)
+
+## Export compile commands for clangd
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+#######################
+## Find dependencies ##
+#######################
+find_package(ament_cmake_auto REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(Ceres REQUIRED)
+ament_auto_find_build_dependencies()
+
+
+###########
+## Build ##
+###########
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  DIRECTORY src
+)
+
+
+target_include_directories(${PROJECT_NAME} PUBLIC ${OpenCV_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS})
+target_link_libraries(${PROJECT_NAME}
+  ${OpenCV_LIBS}
+  ${CERES_LIBRARIES}
+)
+
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN fyt::rune::RuneSolverNode
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  list(APPEND AMENT_LINT_AUTO_EXCLUDE
+    ament_cmake_copyright
+    ament_cmake_uncrustify
+    ament_cmake_cpplint
+  )
+  ament_lint_auto_find_test_dependencies()
+  find_package(ament_cmake_gtest)
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+)
+

src/rm_rune/rune_solver/README.md

@@ -0,0 +1,32 @@
+# rune_solver
+
+FYT视觉24赛季能量机关解算和预测ROS2包。
+
+## fyt::rune::RuneSolverNode
+
+1. 计算能量机关的三维坐标
+2. 对坐标进行卡尔曼滤波消除抖动
+3. 拟合角度变化曲线，预测$\Delta t$后的坐标
+4. 解算出云台的角度发送给电控
+
+### 发布话题 
+
+*  `cmd_gimbal` (`rm_interfaces/msg/GimbalCmd`) - 最终发送到下位机的角度信息
+*  `predict_target` (`geometry_msgs/msg/PointStamped`) - 预测的靶心坐标
+*  `observed_angle` (`rm_interfaces/msg/DebugRuneAngle`) - 当前识别到的能量机关角度
+*  `predict_angle` (`rm_interfaces/msg/DebugRuneAngle`) - 预测的能量机关角度
+*  `markers` (`visualization_markers/msg/MarkerArray`) - 用于Debug的可视化Marker，包括当前识别到的靶心坐标可视化、R标坐标可视化和预测的靶心坐标可视化
+
+### 订阅话题 
+
+* `rune_target` (`rm_interfaces/msg/RuneTarget`) - 识别到的待击打能量机关五个关键点
+
+### 参数 
+
+* `debug` (bool, default: true) - 是否开启debug模式.
+* `predict_time` (double, default: "0.0") - 预测时间补偿，最终预测时间为$\Delta t = t(子弹飞行) + t(传输延迟) + predict\_time$.
+* `compensator_type` (string, default: resistance) - 弹道补偿模型
+* `auto_type_determined` (bool, default: true) - 设为true后会自动判断大符还是小符，设为false则用串口设定的模式判断
+* `ekf.q` (double[]) - 卡尔曼滤波的状态转移噪声
+* `ekf.r` (double[]) - 卡尔曼滤波的观测噪声
+  

src/rm_rune/rune_solver/include/rune_solver/curve_fitter.hpp

@@ -0,0 +1,119 @@
+// Created by Labor 2024.1.28
+// Maintained by Chengfu Zou, Labor
+// Copyright (C) FYT Vision Group. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RUNE_SOLVER_CURVE_FITTER_HPP_
+#define RUNE_SOLVER_CURVE_FITTER_HPP_
+
+// std
+#include <future>
+// third party
+#include <ceres/ceres.h>
+// project
+#include "rune_solver/types.hpp"
+
+namespace fyt::rune {
+
+class CurveFitter {
+public:
+  explicit CurveFitter(const MotionType &t) : type_(t), fitting_future_(nullptr) {
+    // Init parameters to be fitted
+    fitting_param_ = {1.045, 0, 0, 0, 0};
+    direction_ = Direction::UNKNOWN;
+  };
+
+  // Perform angle predict
+  double predict(double time);
+
+  // Update data to be fitted
+  void update(double time, double angle);
+
+  // Reset fitter
+  void reset();
+
+  // Check the state of fitter
+  bool statusVerified();
+
+  // Set the type of fitter
+  void setType(const MotionType &t);
+
+  void setAutoTypeDetermined(bool auto_type_determined);
+
+  MotionType getType() const;
+
+  // Get the string of the fitting result
+  std::string getDebugText();
+
+private:
+  // Perform double curve fitting
+  // automated determination of the type of curve
+  void fitDoubleCurve();
+
+  // Perform curve fitting
+  void fitCurve();
+
+  // Status value
+  MotionType type_;
+  bool is_static_ = false;
+  bool auto_type_determined_ = false;
+  int direction_;
+
+  // Data to be fitted
+  static constexpr int QUEUE_UPPER_LIMIT = 500;
+  static constexpr int QUEUE_LOWER_LIMIT = 50;
+  struct Data {
+    double time;
+    double angle;
+  };
+  std::deque<Data> data_history_queue_;
+
+  // Parameters to be fitted
+  std::array<double, 5> fitting_param_;
+  std::unique_ptr<std::future<void>> fitting_future_;
+
+private:
+  // Fitting Curve
+#define BIG_RUNE_CURVE(x, a, omega, b, c, d, sign) \
+  ((-((a) / (omega) * ceres::cos((omega) * ((x) + (d)))) + (b) * ((x) + (d)) + (c)) * (sign))
+
+#define SMALL_RUNE_CURVE(x, a, b, c, sign) (((a) * ((x) + (b)) + (c)) * (sign))
+
+  // Fitting Cost
+  struct BigRuneFittingCost {
+    BigRuneFittingCost(double x, double y, int m) : x_(x), y_(y), mov_(static_cast<double>(m)) {}
+    template <typename T>
+    bool operator()(const T *const p, T *residual) const {
+      residual[0] = y_ - BIG_RUNE_CURVE(x_, p[0], p[1], p[2], p[3], p[4], mov_);
+      return true;
+    }
+
+    const double x_, y_;
+    const double mov_;
+  };
+
+  struct SmallRuneFittingCost {
+    SmallRuneFittingCost(double x, double y, int m) : x_(x), y_(y), mov_(static_cast<double>(m)) {}
+    template <typename T>
+    bool operator()(const T *const p, T *residual) const {
+      residual[0] = y_ - SMALL_RUNE_CURVE(x_, p[0], p[1], p[2], mov_);
+      return true;
+    }
+
+    const double x_, y_;
+    const double mov_;
+  };
+};
+}  //namespace fyt::rune
+#endif  // RUNE_SOLVER_CURVE_FITTER_HPP_

src/rm_rune/rune_solver/include/rune_solver/motion_model.hpp

@@ -0,0 +1,47 @@
+// Copyright (C) FYT Vision Group. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RUNE_SOLVER_EKF_FUNCTIONS_HPP_
+#define RUNE_SOLVER_EKF_FUNCTIONS_HPP_
+
+#include <ceres/ceres.h>
+
+#include "rm_utils/math/extended_kalman_filter.hpp"
+
+namespace fyt::rune {
+
+constexpr int X_N = 4, Z_N = 4;
+
+struct Predict {
+  template <typename T>
+  void operator()(const T x0[X_N], T x1[X_N]) {
+    for (int i = 0; i < X_N; ++i) {
+      x1[i] = x0[i];
+    }
+  }
+};
+
+struct Measure {
+  template <typename T>
+  void operator()(const T x[Z_N], T z[Z_N]) {
+    for (int i = 0; i < Z_N; ++i) {
+      z[i] = x[i];
+    }
+  }
+};
+
+using RuneCenterEKF = ExtendedKalmanFilter<X_N, Z_N, Predict, Measure>;
+
+}  // namespace fyt::rune
+#endif

src/rm_rune/rune_solver/include/rune_solver/rune_solver.hpp

@@ -0,0 +1,139 @@
+// Maintained by Chengfu Zou, Labor
+// Copyright (C) FYT Vision Group. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RUNE_SOLVER_RUNE_SOLVER_HPP_
+#define RUNE_SOLVER_RUNE_SOLVER_HPP_
+
+// std
+#include <algorithm>
+#include <array>
+#include <deque>
+#include <iostream>
+#include <memory>
+#include <tuple>
+#include <vector>
+// ros2
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/point_stamped.hpp>
+#include <rclcpp/clock.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/time.hpp>
+#include <std_msgs/msg/float32.hpp>
+// project
+#include "rm_interfaces/msg/gimbal_cmd.hpp"
+#include "rm_interfaces/msg/rune_target.hpp"
+#include "rm_utils/logger/log.hpp"
+#include "rm_utils/math/extended_kalman_filter.hpp"
+#include "rm_utils/math/pnp_solver.hpp"
+#include "rm_utils/math/trajectory_compensator.hpp"
+#include "rm_utils/math/manual_compensator.hpp"
+#include "rune_solver/curve_fitter.hpp"
+#include "rune_solver/motion_model.hpp"
+#include "rune_solver/motion_model.hpp"
+#include "rune_solver/types.hpp"
+
+namespace fyt::rune {
+
+// Usage: 
+//   1. init(msg), if tracker_state == LOST
+//   2. update(msg), if tracker_state == DETECTING or TRACKING
+//   3. p = predictTarget(timestamp), to get the predicted position
+//   4. cmd = solveGimbalCmd(p), to get the gimbal command
+class RuneSolver {
+public:
+  struct RuneSolverParams {
+    std::string compensator_type;
+    double gravity;
+    double bullet_speed;
+    double angle_offset_thres;
+    double lost_time_thres;
+    bool auto_type_determined;
+  };
+
+  enum State {
+    LOST,
+    DETECTING,
+    TRACKING,
+  } tracker_state;
+
+  RuneSolver(const RuneSolverParams &sr_params, std::shared_ptr<tf2_ros::Buffer> tf2_buffer);
+
+  // Return: initial angle
+  double init(const rm_interfaces::msg::RuneTarget::SharedPtr received_target);
+
+  // Return: normalized angle
+  double update(const rm_interfaces::msg::RuneTarget::SharedPtr receive_target);
+
+  // Return: normalized predicted angle
+  double predictTarget(Eigen::Vector3d &predicted_position, double timestamp);
+
+  // Return: transormation matrix from rune to odom
+  // Throws: tf2::TransformException or std::runtime_error
+  Eigen::Matrix4d solvePose(const rm_interfaces::msg::RuneTarget &target);
+
+  rm_interfaces::msg::GimbalCmd solveGimbalCmd(const Eigen::Vector3d &target);
+
+  // Return: 3d position of R tag
+  Eigen::Vector3d getCenterPosition() const;
+
+  // Param: angle_diff: how much the angle target should prerotate, 0 for no prediction
+  // Return: 3d position of target to be aimed at
+  Eigen::Vector3d getTargetPosition(double angle_diff) const;
+
+  double getCurAngle() const;
+
+  // Solvers
+  std::unique_ptr<PnPSolver> pnp_solver;
+  std::unique_ptr<TrajectoryCompensator> trajectory_compensator;
+  std::unique_ptr<CurveFitter> curve_fitter;
+  std::unique_ptr<RuneCenterEKF> ekf;
+  std::unique_ptr<ManualCompensator> manual_compensator;
+
+  RuneSolverParams rune_solver_params;
+
+private:
+  double getNormalAngle(const rm_interfaces::msg::RuneTarget::SharedPtr received_target);
+
+  double getObservedAngle(double normal_angle);
+
+  // Return the centroid of the input armor points
+  cv::Point2f getCenterPoint(const std::array<cv::Point2f, ARMOR_KEYPOINTS_NUM> &armor_points);
+
+  // Return ekf state
+  Eigen::Vector4d getStateFromTransform(const Eigen::Matrix4d &transform) const;
+
+  // Observation data
+
+  // last_observed_angle_ is continuously increasing (or decreasing)
+  // from the first detection (call init()) of the target without
+  // any abrupt change in between.
+  double last_observed_angle_;
+
+  // last_angle_ would change (N * DEG_72) when the target jumps
+  double last_angle_;
+  double start_time_;
+  double last_time_;
+
+  Eigen::Vector4d ekf_state_;
+
+  std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
+};
+
+}  // namespace fyt::rune
+#endif // RUNE_SOLVER_SOLVER_HPP_

src/rm_rune/rune_solver/include/rune_solver/rune_solver_node.hpp

@@ -0,0 +1,106 @@
+// Maintained by Chengfu Zou, Labor
+// Copyright (C) FYT Vision Group. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RUNE_SOLVER_RUNE_SOLVER_NODE_HPP_
+#define RUNE_SOLVER_RUNE_SOLVER_NODE_HPP_
+
+// std
+#include <algorithm>
+#include <deque>
+#include <iostream>
+#include <rm_interfaces/msg/detail/debug_rune_angle__struct.hpp>
+#include <rm_utils/heartbeat.hpp>
+#include <string>
+#include <tuple>
+#include <vector>
+// ros2
+#include <geometry_msgs/msg/point_stamped.hpp>
+#include <image_transport/image_transport.hpp>
+#include <image_transport/publisher.hpp>
+#include <rcl_interfaces/msg/set_parameters_result.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <std_msgs/msg/float32.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+// third party
+#include <opencv2/opencv.hpp>
+// project
+#include "rm_interfaces/msg/debug_rune_angle.hpp"
+#include "rm_interfaces/msg/gimbal_cmd.hpp"
+#include "rm_interfaces/msg/rune_target.hpp"
+#include "rm_interfaces/srv/set_mode.hpp"
+#include "rm_utils/heartbeat.hpp"
+#include "rune_solver/rune_solver.hpp"
+
+namespace fyt::rune {
+class RuneSolverNode : public rclcpp::Node {
+public:
+  RuneSolverNode(const rclcpp::NodeOptions &options);
+
+private:
+  void runeTargetCallback(const rm_interfaces::msg::RuneTarget::SharedPtr rune_target_msg);
+
+  void setModeCallback(const std::shared_ptr<rm_interfaces::srv::SetMode::Request> request,
+                       std::shared_ptr<rm_interfaces::srv::SetMode::Response> response);
+
+  // Heartbeat
+  HeartBeatPublisher::SharedPtr heartbeat_;
+
+  // Rune solver
+  std::unique_ptr<RuneSolver> rune_solver_;
+  double predict_offset_;
+
+  // Tf message
+  std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
+
+  // Target Subscriber
+  rclcpp::Subscription<rm_interfaces::msg::RuneTarget>::SharedPtr rune_target_sub_;
+  rm_interfaces::msg::RuneTarget last_rune_target_;
+
+  // Predict Target publisher
+  rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr target_pub_;
+  rclcpp::Publisher<rm_interfaces::msg::GimbalCmd>::SharedPtr gimbal_pub_;
+  rclcpp::TimerBase::SharedPtr pub_timer_;
+  void timerCallback();
+
+  // Enable/Disable Rune Solver
+  bool enable_;
+  rclcpp::Service<rm_interfaces::srv::SetMode>::SharedPtr set_mode_srv_;
+
+  // Dynamic Parameter
+  rcl_interfaces::msg::SetParametersResult onSetParameters(
+    std::vector<rclcpp::Parameter> parameters);
+  rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_handle_;
+
+  // Camera info part
+  std::shared_ptr<sensor_msgs::msg::CameraInfo> cam_info_;
+  rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr cam_info_sub_;
+
+  // Debug info
+  bool debug_;
+  rclcpp::Publisher<rm_interfaces::msg::DebugRuneAngle>::SharedPtr observed_angle_pub_;
+  rclcpp::Publisher<rm_interfaces::msg::DebugRuneAngle>::SharedPtr predicted_angle_pub_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr fitter_text_pub_;
+  visualization_msgs::msg::Marker obs_pos_marker_;
+  visualization_msgs::msg::Marker r_tag_pos_marker_;
+  visualization_msgs::msg::Marker pred_pos_marker_;
+  visualization_msgs::msg::Marker aimming_line_marker_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_pub_;
+};
+}  // namespace fyt::rune
+#endif

src/rm_rune/rune_solver/include/rune_solver/types.hpp

@@ -0,0 +1,62 @@
+// Created by Labor 2024.1.27
+// Maintained by Chengfu Zou, Labor
+// Copyright (C) FYT Vision Group. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RUNE_SOLVER_TYPES_HPP_
+#define RUNE_SOLVER_TYPES_HPP_
+
+// STD
+#include <array>
+
+// 3rd party
+#include <ceres/ceres.h>
+
+#include <opencv2/opencv.hpp>
+
+namespace fyt::rune {
+
+constexpr double DEG_72 = 0.4 * CV_PI;
+constexpr int ARMOR_KEYPOINTS_NUM = 4;
+constexpr int KEYPOINTS_NUM = 5;
+
+// Motion type
+enum class MotionType { SMALL, BIG, UNKNOWN };
+
+// Moving direction
+enum Direction { CLOCKWISE = -1, ANTI_CLOCKWISE = 1, UNKNOWN = 0 };
+
+// Rune arm length, Unit: m
+constexpr double ARM_LENGTH = 0.700;
+
+// Acceptable distance between robot and rune, Unit: m
+// True value = 6.436 m
+constexpr double MIN_RUNE_DISTANCE = 4.0;
+constexpr double MAX_RUNE_DISTANCE = 9.0;
+
+// Rune object points
+// r_tag, bottom_left, top_left, top_right, bottom_right
+const std::vector<cv::Point3f> RUNE_OBJECT_POINTS = {cv::Point3f(0, 0, 0) / 1000,
+                                                     cv::Point3f(0, -541.5, 186) / 1000,
+                                                     cv::Point3f(0, -858.5, 160) / 1000,
+                                                     cv::Point3f(0, -858.5, -160) / 1000,
+                                                     cv::Point3f(0, -541.5, -186) / 1000};
+
+#define BIG_RUNE_CURVE(x, a, omega, b, c, d, sign) \
+  ((-((a) / (omega) * ceres::cos((omega) * ((x) + (d)))) + (b) * ((x) + (d)) + (c)) * (sign))
+
+#define SMALL_RUNE_CURVE(x, a, b, c, sign) (((a) * ((x) + (b)) + (c)) * (sign))
+
+}  // namespace fyt::rune
+#endif  // RUNE_SOLVER_TYPES_HPP_

src/rm_rune/rune_solver/package.xml

@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rune_solver</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="uu@todo.todo">uu</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>eigen</depend>
+  <depend>angles</depend>
+  <depend>visualization_msgs</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>rm_interfaces</depend>
+  <depend>image_transport</depend>
+  <depend>image_transport_plugins</depend>
+  <depend>vision_opencv</depend>
+  <depend>cv_bridge</depend>
+  <depend>ceres</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>rm_utils</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/rm_rune/rune_solver/src/curve_fitter.cpp

@@ -0,0 +1,334 @@
+// Created by Labor 2024.1.28
+// Maintained by Chengfu Zou
+// Copyright (C) FYT Vision Group. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rune_solver/curve_fitter.hpp"
+// std
+#include <cmath>
+#include <string>
+#include <thread>
+// third party
+#include <fmt/format.h>
+// project
+#include "rm_utils/logger/log.hpp"
+#include "rune_solver/types.hpp"
+
+namespace fyt::rune {
+
+// Fit two curves simultaneously and choose the one with lower cost
+// This function will change the type_ automatically
+void CurveFitter::fitDoubleCurve() {
+  if (is_static_) {
+    // Treat the static target as a small rune
+    type_ = MotionType::SMALL;
+    return;
+  }
+
+  constexpr int PARALLEL_THRESHOLD = 300;
+  bool is_parallel = data_history_queue_.size() > PARALLEL_THRESHOLD;
+
+  auto t1 = std::chrono::high_resolution_clock::now();
+
+  ceres::Problem small_fitting_problem;
+  ceres::Problem big_fitting_problem;
+
+  std::array<double, 5> small_param;
+  std::array<double, 5> big_param;
+
+  // Set the initial parameters in different cases
+  switch (type_) {
+    case MotionType::UNKNOWN: {
+      small_param = {1.045, 0, 0, 0, 0};
+      big_param = {0.9125, 1.942, 2.090 - 0.9125, 0, 0};
+      break;
+    }
+    case MotionType::BIG: {
+      small_param = {1.045, 0, 0, 0, 0};
+      big_param = fitting_param_;
+      break;
+    }
+    case MotionType::SMALL: {
+      small_param = fitting_param_;
+      big_param = {0.9125, 1.942, 2.090 - 0.9125, 0, 0};
+      break;
+    }
+  }
+
+  // Add residuals to the problems
+  double *big_param_ptr = big_param.data();
+  double *small_param_ptr = small_param.data();
+  std::for_each(data_history_queue_.begin(), data_history_queue_.end(), [&](const auto &data) {
+    small_fitting_problem.AddResidualBlock(
+      new ceres::AutoDiffCostFunction<CurveFitter::SmallRuneFittingCost, 1, 3>(
+        new SmallRuneFittingCost(data.time, data.angle, direction_)),
+      new ceres::CauchyLoss(0.5),
+      small_param_ptr);
+
+    big_fitting_problem.AddResidualBlock(
+      new ceres::AutoDiffCostFunction<CurveFitter::BigRuneFittingCost, 1, 5>(
+        new BigRuneFittingCost(data.time, data.angle, direction_)),
+      new ceres::CauchyLoss(0.5),
+      big_param_ptr);
+  });
+
+  // Set the bounds of the parameters
+  big_fitting_problem.SetParameterLowerBound(big_param_ptr, 0, 0.780 * 0.5);
+  big_fitting_problem.SetParameterUpperBound(big_param_ptr, 0, 1.045 * 1.5);
+  big_fitting_problem.SetParameterLowerBound(big_param_ptr, 1, 1.884 * 0.5);
+  big_fitting_problem.SetParameterUpperBound(big_param_ptr, 1, 2.000 * 1.5);
+  big_fitting_problem.SetParameterLowerBound(big_param_ptr, 2, (2.090 - 1.045) * 0.5);
+  big_fitting_problem.SetParameterUpperBound(big_param_ptr, 2, (2.090 - 0.780) * 1.5);
+  small_fitting_problem.SetParameterLowerBound(small_param_ptr, 0, 1.045 * 0.5);
+  small_fitting_problem.SetParameterUpperBound(small_param_ptr, 0, 1.045 * 1.5);
+
+  ceres::Solver::Options options;
+  options.linear_solver_type = ceres::DENSE_QR;
+  ceres::Solver::Summary small_summary;
+  ceres::Solver::Summary big_summary;
+
+  // Start the optimization
+  if (is_parallel) {
+    auto f1 = std::async(std::launch::async,
+                         [&]() { ceres::Solve(options, &small_fitting_problem, &small_summary); });
+    auto f2 = std::async(std::launch::async,
+                         [&]() { ceres::Solve(options, &big_fitting_problem, &big_summary); });
+    f1.get();
+    f2.get();
+  } else {
+    ceres::Solve(options, &small_fitting_problem, &small_summary);
+    ceres::Solve(options, &big_fitting_problem, &big_summary);
+  }
+
+  double small_cost = small_summary.final_cost;
+  double big_cost = big_summary.final_cost;
+  // Choose the curve with lower cost
+  if (small_cost < big_cost) {
+    fitting_param_ = small_param;
+    type_ = MotionType::SMALL;
+  } else {
+    fitting_param_ = big_param;
+    type_ = MotionType::BIG;
+  }
+  auto t2 = std::chrono::high_resolution_clock::now();
+  FYT_DEBUG("rune_solver",
+            "Fitting time: {} ms",
+            std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count());
+}
+
+// Fit the curve with the determined type
+void CurveFitter::fitCurve() {
+  if (is_static_) {
+    return;
+  }
+
+  auto t1 = std::chrono::high_resolution_clock::now();
+  ceres::Problem problem;
+
+  // Copy the fitting parameters to a temporary array
+  // to avoid using the variables that are being optimized
+  std::array<double, 5> temp_param = fitting_param_;
+
+  // If the target is static, set the initial parameters, optimize the curve from scratch
+  if (type_ == MotionType::BIG) {
+    temp_param = {0.9125, 1.942, 2.090 - 0.9125, 0, 0};
+  } else if (type_ == MotionType::SMALL) {
+    temp_param = {1.045, 0, 0, 0, 0};
+  }
+
+  double *param_ptr = temp_param.data();
+
+  // Add residuals to the problem
+  std::for_each(data_history_queue_.begin(), data_history_queue_.end(), [&](const auto &data) {
+    if (type_ == MotionType::BIG) {
+      problem.AddResidualBlock(
+        new ceres::AutoDiffCostFunction<CurveFitter::BigRuneFittingCost, 1, 5>(
+          new BigRuneFittingCost(data.time, data.angle, direction_)),
+        new ceres::CauchyLoss(0.5),
+        param_ptr);
+    } else if (type_ == MotionType::SMALL) {
+      problem.AddResidualBlock(
+        new ceres::AutoDiffCostFunction<CurveFitter::SmallRuneFittingCost, 1, 3>(
+          new SmallRuneFittingCost(data.time, data.angle, direction_)),
+        new ceres::CauchyLoss(0.5),
+        param_ptr);
+    }
+  });
+
+  // Set the bounds of the parameters
+  if (type_ == MotionType::BIG) {
+    problem.SetParameterLowerBound(param_ptr, 0, 0.780 * 0.5);
+    problem.SetParameterUpperBound(param_ptr, 0, 1.045 * 1.5);
+    problem.SetParameterLowerBound(param_ptr, 1, 1.884 * 0.5);
+    problem.SetParameterUpperBound(param_ptr, 1, 2.000 * 1.5);
+    problem.SetParameterLowerBound(param_ptr, 2, (2.090 - 1.045) * 0.5);
+    problem.SetParameterUpperBound(param_ptr, 2, (2.090 - 0.780) * 1.5);
+  } else if (type_ == MotionType::SMALL) {
+    problem.SetParameterLowerBound(param_ptr, 0, 1.045 * 0.5);
+    problem.SetParameterUpperBound(param_ptr, 0, 1.045 * 1.5);
+  }
+
+  // Start the optimization
+  ceres::Solver::Options options;
+  options.linear_solver_type = ceres::DENSE_QR;
+  ceres::Solver::Summary summary;
+  ceres::Solve(options, &problem, &summary);
+  auto t2 = std::chrono::high_resolution_clock::now();
+  FYT_DEBUG("rune_solver",
+            "Fitting time: {} ms",
+            std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count());
+
+  // Save the optimized parameters
+  fitting_param_ = temp_param;
+}
+
+double CurveFitter::predict(double current_time) {
+  // If the target is static, return the last angle
+  if (is_static_) {
+    return data_history_queue_.back().angle;
+  }
+
+  double pred_angle = 0;
+  if (type_ == MotionType::BIG) {
+    pred_angle = BIG_RUNE_CURVE(current_time,
+                                fitting_param_[0],
+                                fitting_param_[1],
+                                fitting_param_[2],
+                                fitting_param_[3],
+                                fitting_param_[4],
+                                direction_);
+  } else if (type_ == MotionType::SMALL) {
+    pred_angle = SMALL_RUNE_CURVE(
+      current_time, fitting_param_[0], fitting_param_[1], fitting_param_[2], direction_);
+  }
+
+  return pred_angle;
+}
+
+void CurveFitter::reset() {
+  if (fitting_future_ != nullptr && fitting_future_->valid()) {
+    fitting_future_->wait();
+    fitting_future_.reset();
+  }
+  type_ = MotionType::UNKNOWN;
+  direction_ = Direction::UNKNOWN;
+  data_history_queue_.clear();
+}
+
+void CurveFitter::setType(const MotionType &t) {
+  // Only available when the auto_type_determined_ is false
+  if (type_ == t || auto_type_determined_) {
+    return;
+  }
+
+  if (fitting_future_ != nullptr && fitting_future_->valid()) {
+    fitting_future_->wait();
+  }
+
+  type_ = t;
+  if (t == MotionType::BIG) {
+    fitting_param_ = {0.9125, 1.942, 2.090 - 0.9125, 0, 0};
+  } else if (t == MotionType::SMALL) {
+    fitting_param_ = {1.045, 0, 0, 0, 0};
+  }
+}
+
+MotionType CurveFitter::getType() const { return type_; }
+
+void CurveFitter::setAutoTypeDetermined(bool auto_type_determined) {
+  auto_type_determined_ = auto_type_determined;
+}
+
+std::string CurveFitter::getDebugText() {
+  std::string t = "Unknown";
+
+  if (type_ == MotionType::BIG) {
+    double a = fitting_param_[0];
+    double omega = fitting_param_[1];
+    double b = fitting_param_[2];
+    // double c = fitting_param_[3];
+    double d = fitting_param_[4];
+    t = fmt::format("V: {}( {:.2f} sin( {:.2f} (x {} {:.2f}) ) {} {:.2f} )",
+                    direction_ == Direction::CLOCKWISE ? "-" : " ",
+                    a,
+                    omega,
+                    (d > 0 ? "+" : "-"),
+                    std::abs(d),
+                    (b > 0 ? "+" : "-"),
+                    std::abs(b));
+
+  } else if (type_ == MotionType::SMALL) {
+    double v = is_static_ ? 0 : fitting_param_[0];
+    t = fmt::format("V: {:.2f}", direction_ * v);
+  }
+  return t;
+}
+
+void CurveFitter::update(double time, double angle) {
+  data_history_queue_.emplace_back(Data{.time = time, .angle = angle});
+
+  // Start fitting when the queue size reaches the lower limit
+  if (data_history_queue_.size() < QUEUE_LOWER_LIMIT) {
+    return;
+  }
+
+  // Limit the size of the queue
+  if (data_history_queue_.size() > QUEUE_UPPER_LIMIT) {
+    data_history_queue_.pop_front();
+  }
+
+  // Check if the target is moving or static
+  double angle_diff = data_history_queue_.back().angle - data_history_queue_.front().angle;
+  if (std::abs(angle_diff) < 2 * CV_PI / 180) {
+    is_static_ = true;
+    data_history_queue_.pop_front();
+  } else {
+    is_static_ = false;
+  }
+
+  // Determine the direction of rotation
+  direction_ = static_cast<int>(angle_diff < 0 ? Direction::CLOCKWISE : Direction::ANTI_CLOCKWISE);
+
+  auto startFitting = [this]() {
+    if (auto_type_determined_) {
+      fitting_future_ = std::make_unique<std::future<void>>(
+        std::async(std::launch::async, &CurveFitter::fitDoubleCurve, this));
+    } else {
+      fitting_future_ = std::make_unique<std::future<void>>(
+        std::async(std::launch::async, &CurveFitter::fitCurve, this));
+    }
+  };
+
+  if (fitting_future_ == nullptr) {
+    // First fitting
+    startFitting();
+    // Wait for the first fitting to finish in case the fitting is not finished when the first prediction is needed
+    fitting_future_->wait();
+  } else if (fitting_future_->wait_for(std::chrono::seconds(0)) == std::future_status::ready) {
+    // Start a new fitting asynchronously
+    startFitting();
+  } else {
+    // If fitting is in progess or has been completed, do not start a new fitting
+    FYT_WARN("rune_solver", "Fitting is in progress, do not start a new fitting");
+  }
+}
+
+bool CurveFitter::statusVerified() {
+  if (type_ == MotionType::UNKNOWN || direction_ == Direction::UNKNOWN ||
+      fitting_future_ == nullptr) {
+    return false;
+  }
+  return true;
+}
+}  //namespace fyt::rune

src/rm_rune/rune_solver/src/rune_solver.cpp

@@ -0,0 +1,383 @@
+// Maintained by Chengfu Zou, Labor
+// Copyright (C) FYT Vision Group. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rune_solver/rune_solver.hpp"
+
+// ros2
+#include <cv_bridge/cv_bridge.h>
+#include <tf2/LinearMath/Matrix3x3.h>
+#include <tf2/LinearMath/Quaternion.h>
+
+#include <geometry_msgs/msg/point_stamped.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+// std
+#include <memory>
+// third party
+#include <angles/angles.h>
+
+#include <Eigen/Geometry>
+// project
+#include "rm_utils/logger/log.hpp"
+#include "rm_utils/math/utils.hpp"
+#include "rune_solver/types.hpp"
+
+namespace fyt::rune {
+RuneSolver::RuneSolver(const RuneSolverParams &rsp, std::shared_ptr<tf2_ros::Buffer> buffer)
+: rune_solver_params(rsp), tf2_buffer_(buffer) {
+  // Init
+  tracker_state = LOST;
+  curve_fitter = std::make_unique<CurveFitter>(MotionType::UNKNOWN);
+  curve_fitter->setAutoTypeDetermined(rsp.auto_type_determined);
+  trajectory_compensator = CompensatorFactory::createCompensator(rsp.compensator_type);
+  trajectory_compensator->gravity = rsp.gravity;
+  trajectory_compensator->velocity = rsp.bullet_speed;
+  trajectory_compensator->resistance = 0.01;
+  ekf_state_ = Eigen::Vector4d::Zero();
+  manual_compensator = std::make_unique<ManualCompensator>();
+}
+
+double RuneSolver::init(const rm_interfaces::msg::RuneTarget::SharedPtr received_target) {
+  if (received_target->is_lost) {
+    return 0;
+  }
+
+  FYT_INFO("rune_solver", "Init!");
+
+  // Init EKF
+  try {
+    Eigen::Matrix4d T_odom_2_rune = solvePose(*received_target);
+
+    // Filter out outliers
+    Eigen::Vector3d t = T_odom_2_rune.block(0, 3, 3, 1);
+    if (t.norm() < MIN_RUNE_DISTANCE || t.norm() > MAX_RUNE_DISTANCE) {
+      FYT_ERROR("rune_solver", "Rune position is out of range");
+      return 0;
+    }
+
+    ekf_state_ = getStateFromTransform(T_odom_2_rune);
+    ekf->setState(ekf_state_);
+  } catch (...) {
+    FYT_ERROR("rune_solver", "Init failed");
+    return 0;
+  }
+
+  // Init observation variables
+  tracker_state = DETECTING;
+  double observed_angle = getNormalAngle(received_target);
+  double observed_time = 0;
+  curve_fitter->update(observed_time, observed_angle);
+
+  last_observed_angle_ = observed_angle;
+  last_angle_ = last_observed_angle_;
+  start_time_ = rclcpp::Time(received_target->header.stamp).seconds();
+  last_time_ = start_time_;
+
+  return observed_angle;
+}
+
+double RuneSolver::update(const rm_interfaces::msg::RuneTarget::SharedPtr received_target) {
+  double now_time = rclcpp::Time(received_target->header.stamp).seconds();
+  double delta_time = now_time - last_time_;
+
+  if (received_target->is_big_rune) {
+    curve_fitter->setType(MotionType::BIG);
+  } else {
+    curve_fitter->setType(MotionType::SMALL);
+  }
+
+  if (!received_target->is_lost) {
+    // Update EKF
+    try {
+      Eigen::Matrix4d T_odom_2_rune = solvePose(*received_target);
+
+      // Filter out outliers
+      Eigen::Vector3d t = T_odom_2_rune.block(0, 3, 3, 1);
+      if (t.norm() < MIN_RUNE_DISTANCE || t.norm() > MAX_RUNE_DISTANCE) {
+        FYT_ERROR("rune_solver", "Rune position is out of range");
+        return 0;
+      }
+
+      Eigen::Vector4d measurement = getStateFromTransform(T_odom_2_rune);
+      ekf->predict();
+      ekf_state_ = ekf->update(measurement);
+    } catch (...) {
+      FYT_ERROR("rune_solver", "EKF update failed");
+      return 0;
+    }
+
+    // Get the data to be fitted
+    double observed_time = now_time - start_time_;
+    double normal_angle = getNormalAngle(received_target);
+    double observed_angle = getObservedAngle(normal_angle);
+
+    // Update fitter
+    curve_fitter->update(observed_time, observed_angle);
+
+    last_time_ = now_time;
+    last_angle_ = normal_angle;
+    last_observed_angle_ = observed_angle;
+  }
+
+  // Update tracker state
+  switch (tracker_state) {
+    case DETECTING: {
+      if (received_target->is_lost && delta_time > rune_solver_params.lost_time_thres) {
+        tracker_state = LOST;
+        curve_fitter->reset();
+      } else if (curve_fitter->statusVerified()) {
+        tracker_state = TRACKING;
+      }
+      break;
+    }
+    case TRACKING: {
+      if (received_target->is_lost && delta_time > rune_solver_params.lost_time_thres) {
+        tracker_state = LOST;
+        curve_fitter->reset();
+      }
+      break;
+    }
+    case LOST: {
+      if (!received_target->is_lost) {
+        tracker_state = DETECTING;
+      }
+      break;
+    }
+  }
+  return last_observed_angle_;
+}
+
+double RuneSolver::predictTarget(Eigen::Vector3d &predicted_position, double timestamp) {
+  double t1 = timestamp - start_time_;
+  double t0 = last_time_ - start_time_;
+  double predict_angle_diff = curve_fitter->predict(t1) - curve_fitter->predict(t0);
+
+  // Get the predicted position
+  predicted_position = getTargetPosition(predict_angle_diff);
+
+  return predict_angle_diff + last_observed_angle_;
+}
+
+Eigen::Matrix4d RuneSolver::solvePose(const rm_interfaces::msg::RuneTarget &predicted_target) {
+  Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
+  std::vector<cv::Point2f> image_points(predicted_target.pts.size());
+  std::transform(predicted_target.pts.begin(),
+                 predicted_target.pts.end(),
+                 image_points.begin(),
+                 [](const auto &pt) { return cv::Point2f(pt.x, pt.y); });
+
+  cv::Mat rvec(3, 1, CV_64F), tvec(3, 1, CV_64F);
+  if (pnp_solver && pnp_solver->solvePnP(image_points, rvec, tvec, "rune")) {
+    // Get the transformation matrix from rune to odom
+    try {
+      // Get rotation matrix from rvec
+      cv::Mat rmat;
+      cv::Rodrigues(rvec, rmat);
+      Eigen::Matrix3d rot;
+      // clang-format off
+      rot << rmat.at<double>(0, 0), rmat.at<double>(0, 1), rmat.at<double>(0, 2),
+             rmat.at<double>(1, 0), rmat.at<double>(1, 1), rmat.at<double>(1, 2), 
+             rmat.at<double>(2, 0), rmat.at<double>(2, 1), rmat.at<double>(2, 2);
+      // clang-format on
+      Eigen::Quaterniond quat(rot);
+
+      // Init pose msg
+      geometry_msgs::msg::PoseStamped ps;
+      ps.header.frame_id = "camera_optical_frame";
+      ps.header.stamp = predicted_target.header.stamp;
+
+      // Fill pose msg
+      ps.pose.orientation.x = quat.x();
+      ps.pose.orientation.y = quat.y();
+      ps.pose.orientation.z = quat.z();
+      ps.pose.orientation.w = quat.w();
+      ps.pose.position.x = tvec.at<double>(0);
+      ps.pose.position.y = tvec.at<double>(1);
+      ps.pose.position.z = tvec.at<double>(2);
+
+      // Transform to odom
+      ps = tf2_buffer_->transform(ps, "odom");
+
+      // Fill pose
+      pose(0, 3) = ps.pose.position.x;
+      pose(1, 3) = ps.pose.position.y;
+      pose(2, 3) = ps.pose.position.z;
+
+      Eigen::Quaterniond quat_odom;
+      quat_odom.x() = ps.pose.orientation.x;
+      quat_odom.y() = ps.pose.orientation.y;
+      quat_odom.z() = ps.pose.orientation.z;
+      quat_odom.w() = ps.pose.orientation.w;
+
+      Eigen::Matrix3d rot_odom = quat_odom.toRotationMatrix();
+      pose.block(0, 0, 3, 3) = rot_odom;
+
+    } catch (tf2::TransformException &ex) {
+      FYT_ERROR("rune_solver", "rune to odom error: {}", ex.what());
+      throw ex;
+    }
+  } else {
+    FYT_ERROR("rune_solver", "PnP failed");
+    throw std::runtime_error("PnP failed");
+  }
+  return pose;
+}
+
+rm_interfaces::msg::GimbalCmd RuneSolver::solveGimbalCmd(const Eigen::Vector3d &target) {
+  // Get current yaw and pitch of gimbal
+  double current_yaw = 0.0, current_pitch = 0.0;
+  try {
+    auto gimbal_tf = tf2_buffer_->lookupTransform("odom", "pitch_link", tf2::TimePointZero);
+    auto msg_q = gimbal_tf.transform.rotation;
+
+    tf2::Quaternion tf_q;
+    tf2::fromMsg(msg_q, tf_q);
+    double roll;
+    tf2::Matrix3x3(tf_q).getRPY(roll, current_pitch, current_yaw);
+    current_pitch = -current_pitch;
+  } catch (tf2::TransformException &ex) {
+    FYT_ERROR("rune_solver", "{}", ex.what());
+    throw ex;
+  }
+
+  // Calculate yaw and pitch
+  double yaw = atan2(target.y(), target.x());
+  double pitch = atan2(target.z(), target.head(2).norm());
+
+  // Set parameters of compensator
+  trajectory_compensator->velocity = rune_solver_params.bullet_speed;
+  trajectory_compensator->gravity = rune_solver_params.gravity;
+  trajectory_compensator->iteration_times = 30;
+
+  if (double temp_pitch = pitch; trajectory_compensator->compensate(target, temp_pitch)) {
+    pitch = temp_pitch;
+  }
+  double distance = target.norm();
+
+  // Compensate angle by angle_offset_map
+  auto angle_offset = manual_compensator->angleHardCorrect(target.head(2).norm(), target.z());
+  double pitch_offset = angle_offset[0] * M_PI / 180;
+  double yaw_offset = angle_offset[1] * M_PI / 180;
+  double cmd_pitch = pitch + pitch_offset;
+  double cmd_yaw = angles::normalize_angle(yaw + yaw_offset);
+
+  rm_interfaces::msg::GimbalCmd gimbal_cmd;
+  gimbal_cmd.yaw = cmd_yaw * 180 / M_PI;
+  gimbal_cmd.pitch = cmd_pitch * 180 / M_PI;
+  gimbal_cmd.yaw_diff = (cmd_yaw - current_yaw) * 180 / M_PI;
+  gimbal_cmd.pitch_diff = (cmd_pitch - current_pitch) * 180 / M_PI;
+  gimbal_cmd.distance = distance;
+
+  // Judge whether to shoot
+  constexpr double TARGET_RADIUS = 0.308;
+  double shooting_range_yaw = std::abs(atan2(TARGET_RADIUS / 2, distance)) * 180 / M_PI;
+  double shooting_range_pitch = std::abs(atan2(TARGET_RADIUS / 2, distance)) * 180 / M_PI;
+  // Limit the shooting area to 1 degree to avoid not shooting when distance is
+  // too large
+  shooting_range_yaw = std::max(shooting_range_yaw, 1.0);
+  shooting_range_pitch = std::max(shooting_range_pitch, 1.0);
+  if (std::abs(gimbal_cmd.yaw_diff) < shooting_range_yaw &&
+      std::abs(gimbal_cmd.pitch_diff) < shooting_range_pitch) {
+    gimbal_cmd.fire_advice = true;
+    FYT_DEBUG("rune_solver", "You Can Fire!");
+  } else {
+    gimbal_cmd.fire_advice = false;
+  }
+
+  return gimbal_cmd;
+}
+
+double RuneSolver::getNormalAngle(const rm_interfaces::msg::RuneTarget::SharedPtr received_target) {
+  auto center_point = cv::Point2f(received_target->pts[0].x, received_target->pts[0].y);
+  std::array<cv::Point2f, ARMOR_KEYPOINTS_NUM> armor_points;
+  std::transform(received_target->pts.begin() + 1,
+                 received_target->pts.end(),
+                 armor_points.begin(),
+                 [](const auto &pt) { return cv::Point2f(pt.x, pt.y); });
+
+  cv::Point2f armor_center = getCenterPoint(armor_points);
+  double x_diff = armor_center.x - center_point.x;
+  double y_diff = -(armor_center.y - center_point.y);
+  double normal_angle = std::atan2(y_diff, x_diff);
+  // Normalize angle
+  normal_angle = angles::normalize_angle_positive(normal_angle);
+
+  return normal_angle;
+}
+
+double RuneSolver::getObservedAngle(double normal_angle) {
+  double angle_diff = angles::shortest_angular_distance(last_angle_, normal_angle);
+  // Handle rune target switch
+  if (std::abs(angle_diff) > rune_solver_params.angle_offset_thres) {
+    angle_diff = normal_angle - last_angle_;
+    int offset = std::round(double(angle_diff / DEG_72));
+    angle_diff -= offset * DEG_72;
+  }
+
+  double observed_angle = last_observed_angle_ + angle_diff;
+
+  return observed_angle;
+}
+
+Eigen::Vector3d RuneSolver::getCenterPosition() const { return ekf_state_.head(3); }
+
+Eigen::Vector3d RuneSolver::getTargetPosition(double angle_diff) const {
+  Eigen::Vector3d t_odom_2_rune = ekf_state_.head(3);
+
+  // Considering the large error and jitter(抖动) in the orientation obtained from PnP,
+  // and the fact that the position of the Rune are static in the odom frame,
+  // it is advisable to reconstruct the rotation matrix using geometric information
+  double yaw = ekf_state_(3);
+  double pitch = 0;
+  double roll = -last_angle_;
+  Eigen::Matrix3d R_odom_2_rune =
+    utils::eulerToMatrix(Eigen::Vector3d{roll, pitch, yaw}, utils::EulerOrder::XYZ);
+
+  // Calculate the position of the armor in rune frame
+  Eigen::Vector3d p_rune = Eigen::AngleAxisd(-angle_diff, Eigen::Vector3d::UnitX()).matrix() *
+                           Eigen::Vector3d(0, -ARM_LENGTH, 0);
+
+  // Transform to odom frame
+  Eigen::Vector3d p_odom = R_odom_2_rune * p_rune + t_odom_2_rune;
+
+  return p_odom;
+}
+
+Eigen::Vector4d RuneSolver::getStateFromTransform(const Eigen::Matrix4d &transform) const {
+  // Get yaw
+  Eigen::Matrix3d R_odom_2_rune = transform.block(0, 0, 3, 3);
+  Eigen::Quaterniond q_eigen = Eigen::Quaterniond(R_odom_2_rune);
+  tf2::Quaternion q_tf = tf2::Quaternion(q_eigen.x(), q_eigen.y(), q_eigen.z(), q_eigen.w());
+  double roll, pitch, yaw;
+  tf2::Matrix3x3(q_tf).getRPY(roll, pitch, yaw);
+  yaw = angles::normalize_angle(yaw);
+
+  // Make yaw continuos
+  yaw = ekf_state_(3) + angles::shortest_angular_distance(ekf_state_(3), yaw);
+
+  Eigen::Vector4d state;
+  state << transform(0, 3), transform(1, 3), transform(2, 3), yaw;
+  return state;
+}
+
+double RuneSolver::getCurAngle() const { return last_angle_; }
+
+cv::Point2f RuneSolver::getCenterPoint(
+  const std::array<cv::Point2f, ARMOR_KEYPOINTS_NUM> &armor_points) {
+  return std::accumulate(armor_points.begin(), armor_points.end(), cv::Point2f(0, 0)) /
+         ARMOR_KEYPOINTS_NUM;
+}
+
+}  // namespace fyt::rune
+

src/rm_rune/rune_solver/src/rune_solver_node.cpp

@@ -0,0 +1,390 @@
+// Maintained by Chengfu Zou, Labor
+// Copyright (C) FYT Vision Group. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rune_solver/rune_solver_node.hpp"
+// ros2
+#include <cv_bridge/cv_bridge.h>
+#include <rmw/qos_profiles.h>
+
+#include <rclcpp/qos.hpp>
+// third party
+#include <opencv2/calib3d.hpp>
+#include <opencv2/core/types.hpp>
+#include <opencv2/imgproc.hpp>
+// project
+#include "rm_utils/common.hpp"
+#include "rm_utils/logger/log.hpp"
+#include "rm_utils/math/pnp_solver.hpp"
+#include "rune_solver/motion_model.hpp"
+
+namespace fyt::rune {
+RuneSolverNode::RuneSolverNode(const rclcpp::NodeOptions &options) : Node("rune_solver", options) {
+  FYT_REGISTER_LOGGER("rune_solver", "~/fyt2024-log", INFO);
+  FYT_INFO("rune_solver", "Starting RuneSolverNode!");
+
+  predict_offset_ = declare_parameter("predict_time", 0.1);
+
+  // Tf2 info
+  tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
+  tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);
+
+  // RuneSolver
+  auto rune_solver_params = RuneSolver::RuneSolverParams{
+    .compensator_type = declare_parameter("compensator_type", "ideal"),
+    .gravity = declare_parameter("gravity", 9.8),
+    .bullet_speed = declare_parameter("bullet_speet", 28.0),
+    .angle_offset_thres = declare_parameter("angle_offset_thres", 0.78),
+    .lost_time_thres = declare_parameter("lost_time_thres", 0.5),
+    .auto_type_determined = declare_parameter("auto_type_determined", true),
+  };
+  rune_solver_ = std::make_unique<RuneSolver>(rune_solver_params, tf2_buffer_);
+  
+  // Init manual compensator
+  auto angle_offset = declare_parameter("angle_offset", std::vector<std::string>{});
+  rune_solver_->manual_compensator->updateMapFlow(angle_offset);
+
+  // EKF for filtering the position of R tag
+  // state: x, y, z, yaw
+  // measurement: x, y, z, yaw
+  // f - Process function
+  auto f = Predict();
+  // h - Observation function
+  auto h = Measure();
+  // update_Q - process noise covariance matrix
+  std::vector<double> q_vec =
+    declare_parameter("ekf.q", std::vector<double>{0.001, 0.001, 0.001, 0.001});
+  auto u_q = [q_vec]() {
+    Eigen::Matrix<double, X_N, X_N> q = Eigen::MatrixXd::Zero(4, 4);
+    q.diagonal() << q_vec[0], q_vec[1], q_vec[2], q_vec[3];
+    return q;
+  };
+  // update_R - measurement noise covariance matrix
+  std::vector<double> r_vec = declare_parameter("ekf.r", std::vector<double>{0.1, 0.1, 0.1, 0.1});
+  auto u_r = [r_vec](const Eigen::Matrix<double, Z_N, 1> &z) {
+    Eigen::Matrix<double, Z_N, Z_N> r = Eigen::MatrixXd::Zero(4, 4);
+    r.diagonal() << r_vec[0], r_vec[1], r_vec[2], r_vec[3];
+    return r;
+  };
+  // P - error estimate covariance matrix
+  Eigen::MatrixXd p0 = Eigen::MatrixXd::Identity(4, 4);
+  rune_solver_->ekf = std::make_unique<RuneCenterEKF>(f, h, u_q, u_r, p0);
+
+  // Target subscriber
+  rune_target_sub_ = this->create_subscription<rm_interfaces::msg::RuneTarget>(
+    "rune_detector/rune_target",
+    rclcpp::SensorDataQoS(),
+    std::bind(&RuneSolverNode::runeTargetCallback, this, std::placeholders::_1));
+
+  // Publisher
+  gimbal_pub_ = this->create_publisher<rm_interfaces::msg::GimbalCmd>("rune_solver/cmd_gimbal",
+                                                                      rclcpp::SensorDataQoS());
+  target_pub_ = this->create_publisher<geometry_msgs::msg::PointStamped>(
+    "rune_solver/predict_target", rclcpp::SensorDataQoS());
+
+  // Set dynamic parameter callback
+  on_set_parameters_callback_handle_ = this->add_on_set_parameters_callback(
+    std::bind(&RuneSolverNode::onSetParameters, this, std::placeholders::_1));
+
+  // Camera info
+  cam_info_sub_ = this->create_subscription<sensor_msgs::msg::CameraInfo>(
+    "camera_info",
+    rclcpp::SensorDataQoS(),
+    [this](sensor_msgs::msg::CameraInfo::ConstSharedPtr camera_info) {
+      cam_info_ = std::make_shared<sensor_msgs::msg::CameraInfo>(*camera_info);
+      rune_solver_->pnp_solver = std::make_unique<PnPSolver>(camera_info->k, camera_info->d);
+      rune_solver_->pnp_solver->setObjectPoints("rune", RUNE_OBJECT_POINTS);
+      cam_info_sub_.reset();
+    });
+
+  // Enable/Disable Rune Solver
+  set_mode_srv_ = this->create_service<rm_interfaces::srv::SetMode>(
+    "rune_solver/set_mode",
+    std::bind(
+      &RuneSolverNode::setModeCallback, this, std::placeholders::_1, std::placeholders::_2));
+
+  // Debug info
+  debug_ = this->declare_parameter("debug", true);
+  if (debug_) {
+    observed_angle_pub_ = this->create_publisher<rm_interfaces::msg::DebugRuneAngle>(
+      "rune_solver/observed_angle", rclcpp::SensorDataQoS());
+    predicted_angle_pub_ = this->create_publisher<rm_interfaces::msg::DebugRuneAngle>(
+      "rune_solver/predicted_angle", rclcpp::SensorDataQoS());
+    fitter_text_pub_ = this->create_publisher<std_msgs::msg::String>("rune_solver/fitting_info",
+                                                                     rclcpp::SensorDataQoS());
+    // Marker
+    r_tag_pos_marker_.ns = "r_tag_position";
+    r_tag_pos_marker_.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
+    r_tag_pos_marker_.scale.x = r_tag_pos_marker_.scale.y = r_tag_pos_marker_.scale.z = 0.15;
+    r_tag_pos_marker_.text = "R";
+    r_tag_pos_marker_.color.a = 1.0;
+    r_tag_pos_marker_.color.r = 1.0;
+    r_tag_pos_marker_.color.g = 1.0;
+    obs_pos_marker_.ns = "observed_position";
+    obs_pos_marker_.type = visualization_msgs::msg::Marker::SPHERE;
+    obs_pos_marker_.scale.x = obs_pos_marker_.scale.y = obs_pos_marker_.scale.z = 0.308;
+    obs_pos_marker_.color.a = 1.0;
+    obs_pos_marker_.color.r = 1.0;
+    pred_pos_marker_.ns = "predicted_position";
+    pred_pos_marker_.type = visualization_msgs::msg::Marker::SPHERE;
+    pred_pos_marker_.scale.x = pred_pos_marker_.scale.y = pred_pos_marker_.scale.z = 0.308;
+    pred_pos_marker_.color.a = 1.0;
+    pred_pos_marker_.color.g = 1.0;
+    aimming_line_marker_.ns = "aimming_line";
+    aimming_line_marker_.type = visualization_msgs::msg::Marker::ARROW;
+    aimming_line_marker_.scale.x = 0.03;
+    aimming_line_marker_.scale.y = 0.05;
+    aimming_line_marker_.color.a = 0.5;
+    aimming_line_marker_.color.r = 1.0;
+    aimming_line_marker_.color.b = 1.0;
+    aimming_line_marker_.color.g = 1.0;
+    marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(
+      "rune_solver/marker", rclcpp::SensorDataQoS());
+  }
+  last_rune_target_.header.frame_id = "";
+  // Timer 250 Hz
+  pub_timer_ = this->create_wall_timer(std::chrono::milliseconds(4),
+                                       std::bind(&RuneSolverNode::timerCallback, this));
+
+  // Heartbeat
+  heartbeat_ = HeartBeatPublisher::create(this);
+}
+
+void RuneSolverNode::timerCallback() {
+  // rune_solver_->pnp_solver is nullptr when camera_info is not received
+  if (rune_solver_->pnp_solver == nullptr) {
+    return;
+  }
+
+  // Return if not enable
+  if (!enable_) {
+    return;
+  }
+
+  // Init message
+  rm_interfaces::msg::GimbalCmd control_msg;
+  geometry_msgs::msg::PointStamped target_msg;
+  target_msg.header.frame_id = "odom";
+
+  // If target never detected
+  if (last_rune_target_.header.frame_id.empty()) {
+    control_msg.yaw_diff = 0;
+    control_msg.pitch_diff = 0;
+    control_msg.distance = -1;
+    control_msg.pitch = 0;
+    control_msg.yaw = 0;
+    control_msg.fire_advice = false;
+    gimbal_pub_->publish(control_msg);
+    return;
+  }
+
+  double predict_angle = 0;
+
+  // Calculate predict time
+  Eigen::Vector3d cur_pos = rune_solver_->getTargetPosition(0);
+  double flying_time = rune_solver_->trajectory_compensator->getFlyingTime(cur_pos);
+  rclcpp::Time predict_timestamp = this->now() + rclcpp::Duration::from_seconds(predict_offset_) +
+                                   rclcpp::Duration::from_seconds(flying_time);
+
+  Eigen::Vector3d pred_pos = Eigen::Vector3d::Zero();
+
+  if (rune_solver_->tracker_state == RuneSolver::TRACKING) {
+    // Predict target
+    predict_angle = rune_solver_->predictTarget(pred_pos, predict_timestamp.seconds());
+
+    target_msg.header.stamp = predict_timestamp;
+    target_msg.point.x = pred_pos.x();
+    target_msg.point.y = pred_pos.y();
+    target_msg.point.z = pred_pos.z();
+    target_pub_->publish(target_msg);
+    try {
+      control_msg = rune_solver_->solveGimbalCmd(pred_pos);
+    } catch (...) {
+      FYT_ERROR("rune_solver", "solveGimbalCmd error");
+      control_msg.yaw_diff = 0;
+      control_msg.pitch_diff = 0;
+      control_msg.distance = -1;
+      control_msg.pitch = 0;
+      control_msg.yaw = 0;
+      control_msg.fire_advice = false;
+    }
+  } else {
+    control_msg.yaw_diff = 0;
+    control_msg.pitch_diff = 0;
+    control_msg.distance = -1;
+    control_msg.pitch = 0;
+    control_msg.yaw = 0;
+    control_msg.fire_advice = false;
+  }
+  gimbal_pub_->publish(control_msg);
+
+  if (debug_) {
+    // Publish fitting info
+    std_msgs::msg::String fitter_text_msg;
+    fitter_text_msg.data = fmt::format("{} | predict_time: {:.3f}s",
+                                       rune_solver_->curve_fitter->getDebugText(),
+                                       predict_offset_ + flying_time);
+    fitter_text_pub_->publish(fitter_text_msg);
+
+    // Publish visualization marker
+    visualization_msgs::msg::MarkerArray marker_array;
+    if (rune_solver_->tracker_state == RuneSolver::LOST) {
+      obs_pos_marker_.action = visualization_msgs::msg::Marker::DELETEALL;
+      pred_pos_marker_.action = visualization_msgs::msg::Marker::DELETEALL;
+      r_tag_pos_marker_.action = visualization_msgs::msg::Marker::DELETEALL;
+      aimming_line_marker_.action = visualization_msgs::msg::Marker::DELETEALL;
+      marker_array.markers.push_back(obs_pos_marker_);
+      marker_array.markers.push_back(pred_pos_marker_);
+      marker_array.markers.push_back(r_tag_pos_marker_);
+      marker_array.markers.push_back(aimming_line_marker_);
+      marker_pub_->publish(marker_array);
+    } else {
+      obs_pos_marker_.header.frame_id = "odom";
+      obs_pos_marker_.header.stamp = last_rune_target_.header.stamp;
+      obs_pos_marker_.action = visualization_msgs::msg::Marker::ADD;
+      obs_pos_marker_.lifetime = rclcpp::Duration::from_seconds(0.1);
+      obs_pos_marker_.pose.position.x = cur_pos.x();
+      obs_pos_marker_.pose.position.y = cur_pos.y();
+      obs_pos_marker_.pose.position.z = cur_pos.z();
+
+      Eigen::Vector3d r_tag_pos = rune_solver_->getCenterPosition();
+      r_tag_pos_marker_.header.frame_id = "odom";
+      r_tag_pos_marker_.header.stamp = last_rune_target_.header.stamp;
+      r_tag_pos_marker_.action = visualization_msgs::msg::Marker::ADD;
+      r_tag_pos_marker_.lifetime = rclcpp::Duration::from_seconds(0.1);
+      r_tag_pos_marker_.pose.position.x = r_tag_pos.x();
+      r_tag_pos_marker_.pose.position.y = r_tag_pos.y();
+      r_tag_pos_marker_.pose.position.z = r_tag_pos.z();
+
+      marker_array.markers.push_back(obs_pos_marker_);
+      marker_array.markers.push_back(r_tag_pos_marker_);
+      if (rune_solver_->tracker_state == RuneSolver::TRACKING) {
+        pred_pos_marker_.header.frame_id = "odom";
+        pred_pos_marker_.header.stamp = predict_timestamp;
+        pred_pos_marker_.action = visualization_msgs::msg::Marker::ADD;
+        pred_pos_marker_.lifetime = rclcpp::Duration::from_seconds(0.1);
+        pred_pos_marker_.pose.position.x = pred_pos.x();
+        pred_pos_marker_.pose.position.y = pred_pos.y();
+        pred_pos_marker_.pose.position.z = pred_pos.z();
+        marker_array.markers.push_back(pred_pos_marker_);
+
+        aimming_line_marker_.action = visualization_msgs::msg::Marker::ADD;
+        aimming_line_marker_.points.clear();
+        aimming_line_marker_.header.frame_id = "odom";
+        aimming_line_marker_.header.stamp = this->now();
+        aimming_line_marker_.lifetime = rclcpp::Duration::from_seconds(0.1);
+        geometry_msgs::msg::Point aimming_line_start, aimming_line_end;
+        aimming_line_marker_.points.emplace_back(aimming_line_start);
+        aimming_line_end.y = 15 * sin(control_msg.yaw * M_PI / 180);
+        aimming_line_end.x = 15 * cos(control_msg.yaw * M_PI / 180);
+        aimming_line_end.z = 15 * sin(control_msg.pitch * M_PI / 180);
+        aimming_line_marker_.points.emplace_back(aimming_line_end);
+        marker_array.markers.push_back(aimming_line_marker_);
+
+        rm_interfaces::msg::DebugRuneAngle predict_angle_msg;
+        predict_angle_msg.header = last_rune_target_.header;
+        predict_angle_msg.header.stamp = predict_timestamp;
+        predict_angle_msg.data = predict_angle;
+        predicted_angle_pub_->publish(predict_angle_msg);
+      }
+      marker_pub_->publish(marker_array);
+    }
+  }
+}
+
+void RuneSolverNode::runeTargetCallback(
+  const rm_interfaces::msg::RuneTarget::SharedPtr rune_target_msg) {
+  // rune_solver_->pnp_solver is nullptr when camera_info is not received
+  if (rune_solver_->pnp_solver == nullptr) {
+    return;
+  }
+
+  // Keep the last detected target
+  if (!rune_target_msg->is_lost) {
+    last_rune_target_ = *rune_target_msg;
+  }
+  double observed_angle = 0;
+  if (rune_solver_->tracker_state == RuneSolver::LOST) {
+    observed_angle = rune_solver_->init(rune_target_msg);
+  } else {
+    observed_angle = rune_solver_->update(rune_target_msg);
+
+    if (debug_) {
+      rm_interfaces::msg::DebugRuneAngle observed_angle_msg;
+      observed_angle_msg.header = rune_target_msg->header;
+      observed_angle_msg.data = observed_angle;
+      observed_angle_pub_->publish(observed_angle_msg);
+    }
+  }
+}
+
+rcl_interfaces::msg::SetParametersResult RuneSolverNode::onSetParameters(
+  std::vector<rclcpp::Parameter> parameters) {
+  rcl_interfaces::msg::SetParametersResult result;
+  result.successful = true;
+  for (const auto &param : parameters) {
+    if (param.get_name() == "predict_time") {
+      predict_offset_ = param.as_double();
+    } else if (param.get_name() == "debug") {
+      debug_ = param.as_bool();
+    } else if (param.get_name() == "gravity") {
+      rune_solver_->rune_solver_params.gravity = param.as_double();
+    } else if (param.get_name() == "bullet_speed") {
+      rune_solver_->rune_solver_params.bullet_speed = param.as_double();
+    } else if (param.get_name() == "angle_offset_thres") {
+      rune_solver_->rune_solver_params.angle_offset_thres = param.as_double();
+    } else if (param.get_name() == "lost_time_thres") {
+      rune_solver_->rune_solver_params.lost_time_thres = param.as_double();
+    }
+  }
+  return result;
+}
+
+void RuneSolverNode::setModeCallback(
+  const std::shared_ptr<rm_interfaces::srv::SetMode::Request> request,
+  std::shared_ptr<rm_interfaces::srv::SetMode::Response> response) {
+  response->success = true;
+
+  VisionMode mode = static_cast<VisionMode>(request->mode);
+  std::string mode_name = visionModeToString(mode);
+  if (mode_name == "UNKNOWN") {
+    FYT_ERROR("rune_solver", "Invalid mode: {}", request->mode);
+    return;
+  }
+
+  switch (mode) {
+    case VisionMode::SMALL_RUNE_RED:
+    case VisionMode::SMALL_RUNE_BLUE:
+    case VisionMode::BIG_RUNE_RED:
+    case VisionMode::BIG_RUNE_BLUE: {
+      enable_ = true;
+      break;
+    }
+    default: {
+      enable_ = false;
+      break;
+    }
+  }
+
+  FYT_WARN("rune_solver", "Set Rune Mode: {}", visionModeToString(mode));
+}
+
+}  // namespace fyt::rune
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(fyt::rune::RuneSolverNode)

src/rm_rune/rune_solver/test/test_node_startup.cpp

@@ -0,0 +1,40 @@
+// Copyright (C) FYT Vision Group. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// gest
+#include <gtest/gtest.h>
+// ros2
+#include <rclcpp/executors.hpp>
+#include <rclcpp/node_options.hpp>
+#include <rclcpp/utilities.hpp>
+// std
+#include <memory>
+// project
+#include "rune_solver/rune_solver_node.hpp"
+
+TEST(RuneSolverrNodeTest, NodeStartupTest)
+{
+  rclcpp::NodeOptions options;
+  auto node = std::make_shared<fyt::rune::RuneSolverNode>(options);
+  node.reset();
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  rclcpp::init(argc, argv);
+  auto result = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return result;
+}