bf

src/rm_auto_aim/armor_solver/CMakeLists.txt

@@ -0,0 +1,56 @@
+cmake_minimum_required(VERSION 3.10)
+project(armor_solver)
+
+## 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)
+ament_auto_find_build_dependencies()
+
+###########
+## Build ##
+###########
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  DIRECTORY src
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN fyt::auto_aim::ArmorSolverNode
+  EXECUTABLE ${PROJECT_NAME}_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)
+
+endif()
+
+#############
+## Install ##
+#############
+
+ament_auto_package()

src/rm_auto_aim/armor_solver/README.md

@@ -0,0 +1,118 @@
+# armor_solver
+
+## fyt::ArmorSolverNode
+
+装甲板识别节点
+
+### 发布话题 
+
+* `armor_solver/target` (`rm_interfaces/msg/Target`) - 整车估计的状态
+* `armor_solver/measurement` (`rm_interfaces/msg/Measurement`) - EKF的输入观测量
+* `armor_solver/cmd_gimbal` (`rm_interfaces/msg/GimbalCmd`) - 云台控制指令
+
+### 订阅话题
+
+*  `armor_detector/armors` (`rm_interfaces/msg/Armors`) - 识别到的装甲板信息
+
+  
+### 参数 
+
+* `debug` (`bool`, default: false) - 是否开启调试模式
+* `target_frame` (`string`, default: "odom") - 目标坐标系
+* `ekf.sigma2_q_xyz` (`double`, default: 0.05) - 状态转移噪声方差 (x,y,z)
+* `ekf.sigma2_q_yaw` (`double`, default: 1.0) - 状态转移噪声方差 (yaw)
+* `ekf.sigma2_q_r` (`double`, default: 80.0) - 状态转移噪声方差 (r)
+* `r_xyz_factor` (`double`, default: 1.0) - 位置观测噪声方差系数 (x,y,z)
+* `r_yaw_factor` (`double`, default: 1.0) - 位置观测噪声方差系数 (yaw)
+* `tracker.max_match_distance` (`double`, default: 0.5) - 两帧间目标可匹配的最大距离
+* `tracker.max_match_yaw_diff` (`double`, default: 0.5) - 两帧间目标同一块装甲板可匹配的最大yaw角差（大于这个值则认为装甲板发生跳变）
+* `tracker.tracking_thres` (`int`, default: 2) - `DETECTING` 状态进入 `TRACKING` 状态需要连续识别到的帧数
+* `tracker.lost_thres` (`double`, default: 1.0) - `TRACKING` 状态进入 `LOST` 状态需要连续丢失的时间（s）
+* `solver.prediction_delay` (`double`, default: 0.0) - 预测延迟时间（s），会影响选版
+* `solver.controller_delay` (`double`, default: 0.0) - 控制延迟时间（s），不会影响选版
+* `solver.max_tracking_v_yaw` (`double`, default: 60.0) - 转速大于这个值时，瞄准中心
+* `solver.side_angle` (`double`, default: 15.0) - 跳转到下一装甲板的角度阈值
+* `solver.bullet_speed` (`double`, default: 25.0) - 子弹速度
+* `solver.gravity` (`double`, default: 9.8) - 重力加速度
+* `solver.compensator_type` (`string`, default: "ideal") - 补偿器类型
+* `solver.resistance` (`double`, default: 0.001) - 空气阻力
+
+
+## ArmorSolverNode
+装甲板处理节点
+
+订阅识别节点发布的装甲板三维位置及机器人的坐标转换信息，将装甲板三维位置变换到指定惯性系（一般是以云台中心为原点，IMU 上电时的 Yaw 朝向为 X 轴的惯性系）下，然后将装甲板目标送入跟踪器中，输出跟踪机器人在指定惯性系下的状态
+
+订阅：
+- 已识别到的装甲板 `/detector/armors`
+- 机器人的坐标转换信息 `/tf` `/tf_static`
+
+发布：
+- 最终锁定的目标 `/tracker/target`
+
+参数：
+- 跟踪器参数 tracker
+  - 两帧间目标可匹配的最大距离 max_match_distance
+  - `DETECTING` 状态进入 `TRACKING` 状态的阈值 tracking_threshold
+  - `TRACKING` 状态进入 `LOST` 状态的阈值 lost_threshold
+
+## ExtendedKalmanFilter
+
+$$ x_c = x_a + r * cos (\theta) $$
+$$ y_c = y_a + r * sin (\theta) $$
+
+$$ x = [x_c, y_c,z, yaw, v_{xc}, v_{yc},v_z, v_{yaw}, r]^T $$
+
+参考 OpenCV 中的卡尔曼滤波器使用 Eigen 进行了实现
+
+[卡尔曼滤波器](https://zh.wikipedia.org/wiki/%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2)
+
+![](docs/Kalman_filter_model.png)
+
+考虑到自瞄任务中对于目标只有观测没有控制，所以输入－控制模型 $B$ 和控制器向量 $u$ 可忽略。
+
+预测及更新的公式如下：
+
+预测：
+
+$$ x_{k|k-1} = F * x_{k-1|k-1} $$
+
+$$ P_{k|k-1} = F * P_{k-1|k-1}* F^T + Q $$
+
+更新:
+
+$$ K = P_{k|k-1} * H^T * (H * P_{k|k-1} * H^T + R)^{-1} $$
+
+$$ x_{k|k} = x_{k|k-1} + K * (z_k - H * x_{k|k-1}) $$
+
+$$ P_{k|k} = (I - K * H) * P_{k|k-1} $$
+
+## Tracker
+
+参考 [SORT(Simple online and realtime tracking)](https://ieeexplore.ieee.org/abstract/document/7533003/) 中对于目标匹配的方法，使用卡尔曼滤波器对单目标在三维空间中进行跟踪
+
+在此跟踪器中，卡尔曼滤波器观测量为目标在指定惯性系中的位置（xyz），状态量为目标位置及速度（xyz+vx vy vz）
+
+在对目标的运动模型建模为在指定惯性系中的匀速线性运动，即状态转移为 $x_{pre} = x_{post} + v_{post} * dt$
+
+目标关联的判断依据为三维位置的 L2 欧式距离
+
+跟踪器共有四个状态：
+- `DETECTING` ：短暂识别到目标，需要更多帧识别信息才能进入跟踪状态
+- `TRACKING` ：跟踪器正常跟踪目标中
+- `TEMP_LOST` ：跟踪器短暂丢失目标，通过卡尔曼滤波器预测目标
+- `LOST` ：跟踪器完全丢失目标
+
+工作流程：
+
+- init：
+
+  跟踪器默认选择离相机光心最近的目标作为跟踪对象，选择目标后初始化卡尔曼滤波器，初始状态设为当前目标位置，速度设为 0
+
+- update:
+
+  首先由卡尔曼滤波器得到目标在当前帧的预测位置，然后遍历当前帧中的目标位置与预测位置进行匹配，若当前帧不存在目标或所有目标位置与预测位置的偏差都过大则认为目标丢失，重置卡尔曼滤波器。
+  
+  最后选取位置相差最小的目标作为最佳匹配项，更新卡尔曼滤波器，将更新后的状态作为跟踪器的结果输出
+
+

src/rm_auto_aim/armor_solver/include/armor_solver/armor_solver.hpp

@@ -0,0 +1,122 @@
+// Created by Chengfu Zou
+// 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 ARMOR_SOLVER_SOLVER_HPP_
+#define ARMOR_SOLVER_SOLVER_HPP_
+
+// std
+#include <memory>
+// ros2
+#include <angles/angles.h>
+#include <tf2_ros/buffer.h>
+
+#include <rclcpp/time.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+// 3rd party
+#include <Eigen/Dense>
+// project
+#include "rm_interfaces/msg/gimbal_cmd.hpp"
+#include "rm_interfaces/msg/target.hpp"
+#include "rm_utils/math/manual_compensator.hpp"
+#include "rm_utils/math/trajectory_compensator.hpp"
+
+namespace fyt::auto_aim {
+// Solver class used to solve the gimbal command from tracked target
+class Solver {
+public:
+  explicit Solver(std::weak_ptr<rclcpp::Node> node);
+  // explicit Solver(std::string trajectory_compensator_type, float max_tracking_v_yaw);
+  ~Solver() = default;
+
+  // Solve the gimbal command from tracked target
+  // Throw: tf2::TransformException if the transform from "odom" to "pitch_link" is not available
+  rm_interfaces::msg::GimbalCmd solve(const rm_interfaces::msg::Target &target_msg,
+                                      const rclcpp::Time &current_time,
+                                      std::shared_ptr<tf2_ros::Buffer> tf2_buffer_);
+
+  enum State { TRACKING_ARMOR = 0, TRACKING_CENTER = 1 } state;
+
+  std::vector<std::pair<double, double>> getTrajectory() const noexcept;
+  void setBulletSpeed(double bullet_speed) noexcept;
+  void updateRuntimeParams(double max_tracking_v_yaw,
+                           double prediction_delay,
+                           double controller_delay,
+                           double side_angle,
+                           double min_switching_v_yaw,
+                           double shooting_range_w,
+                           double shooting_range_h) noexcept;
+
+private:
+  // Get the armor positions from the target robot
+  std::vector<Eigen::Vector3d> getArmorPositions(const Eigen::Vector3d &target_center,
+                                                 const double yaw,
+                                                 const double r1,
+                                                 const double r2,
+                                                 const double d_zc,
+                                                 const double d_za,
+                                                 const size_t armors_num) const noexcept;
+
+  // Select the best armor to shoot
+  // Return: selected idx in {0, 1, ..., armors_num - 1}
+  int selectBestArmor(const std::vector<Eigen::Vector3d> &armor_positions,
+                      const Eigen::Vector3d &target_center,
+                      const double target_yaw,
+                      const double target_v_yaw,
+                      const size_t armors_num) const noexcept;
+
+  void calcYawAndPitch(const Eigen::Vector3d &p,
+                       const Eigen::Vector3d &barrel_pos_in_odom,
+                       double &yaw,
+                       double &pitch) const noexcept;
+
+  // Transform target position from odom frame to barrel frame for trajectory calculation
+  Eigen::Vector3d transformToBarrelFrame(const Eigen::Vector3d &target_in_odom,
+                                         std::shared_ptr<tf2_ros::Buffer> tf2_buffer) const;
+
+  bool isOnTarget(const double cur_yaw,
+                  const double cur_pitch,
+                  const double target_yaw,
+                  const double target_pitch,
+                  const double distance) const noexcept;
+
+  std::unique_ptr<TrajectoryCompensator> trajectory_compensator_;
+  std::unique_ptr<ManualCompensator> manual_compensator_;
+
+  std::array<double, 3> rpy_;
+
+  double prediction_delay_;
+  double controller_delay_;
+
+  double shooting_range_w_;
+  double shooting_range_h_;
+
+  double max_tracking_v_yaw_;
+  int overflow_count_;
+  int transfer_thresh_;
+
+  double side_angle_;
+  double min_switching_v_yaw_;
+
+  // Barrel frame for trajectory calculation
+  bool use_barrel_frame_;
+  bool barrel_offset_initialized_;  // Whether barrel_offset has been initialized from TF
+  Eigen::Vector3d barrel_offset_;   // Barrel offset relative to pitch_link (from TF tree)
+
+  std::weak_ptr<rclcpp::Node> node_;
+};
+}  // namespace fyt::auto_aim
+#endif  // ARMOR_SOLVER_SOLVER_HPP_
+

src/rm_auto_aim/armor_solver/include/armor_solver/armor_solver_node.hpp

@@ -0,0 +1,134 @@
+// Copyright Chen Jun 2023. Licensed under the MIT License.
+//
+// Additional modifications and features by Chengfu Zou, Labor. Licensed under Apache License 2.0.
+//
+// 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 ARMOR_SOLVER_SOLVER_NODE_HPP_
+#define ARMOR_SOLVER_SOLVER_NODE_HPP_
+
+// ros2
+#include <message_filters/subscriber.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/create_timer_ros.h>
+#include <tf2_ros/message_filter.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <rcl_interfaces/msg/set_parameters_result.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+// std
+#include <atomic>
+#include <memory>
+#include <string>
+#include <vector>
+// project
+#include "armor_solver/armor_solver.hpp"
+#include "armor_solver/armor_tracker.hpp"
+#include "rm_interfaces/msg/armors.hpp"
+#include "rm_interfaces/msg/measurement.hpp"
+#include "rm_interfaces/msg/serial_receive_data.hpp"
+#include "rm_interfaces/msg/target.hpp"
+#include "rm_interfaces/srv/set_mode.hpp"
+#include "rm_utils/heartbeat.hpp"
+#include "rm_utils/logger/log.hpp"
+
+namespace fyt::auto_aim {
+using tf2_filter = tf2_ros::MessageFilter<rm_interfaces::msg::Armors>;
+class ArmorSolverNode : public rclcpp::Node {
+public:
+  explicit ArmorSolverNode(const rclcpp::NodeOptions &options);
+
+private:
+  rcl_interfaces::msg::SetParametersResult onSetParameters(
+    const std::vector<rclcpp::Parameter> &parameters);
+
+  void armorsCallback(const rm_interfaces::msg::Armors::SharedPtr armors_ptr);
+
+  void initMarkers() noexcept;
+
+  void publishMarkers(const rm_interfaces::msg::Target &target_msg,
+                      const rm_interfaces::msg::GimbalCmd &gimbal_cmd) noexcept;
+
+
+  void setModeCallback(const std::shared_ptr<rm_interfaces::srv::SetMode::Request> request,
+                       std::shared_ptr<rm_interfaces::srv::SetMode::Response> response);
+  
+  bool debug_mode_;
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr
+    on_set_parameters_callback_handle_;
+
+  // Heartbeat
+  HeartBeatPublisher::SharedPtr heartbeat_;
+
+  // The time when the last message was received
+  rclcpp::Time last_time_;
+  double dt_;
+
+  // Armor tracker
+  // Adaptive Q matrix parameters
+  double s2qxyz_max_, s2qxyz_min_;  // Position noise range
+  double s2qyaw_max_, s2qyaw_min_; // Yaw noise range
+  double s2qr_, s2qd_zc_;          // Radius and height offset noise
+  // R matrix parameters
+  double r_x_, r_y_, r_z_, r_yaw_;
+  double lost_time_thres_;
+  std::unique_ptr<Tracker> tracker_;
+
+  // Armor Solver
+  std::unique_ptr<Solver> solver_;
+
+  // Subscriber with tf2 message_filter
+  std::string target_frame_;
+  std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
+  message_filters::Subscriber<rm_interfaces::msg::Armors> armors_sub_;
+  rm_interfaces::msg::Target armor_target_;
+  std::shared_ptr<tf2_filter> tf2_filter_;
+
+  // Measurement publisher
+  rclcpp::Publisher<rm_interfaces::msg::Measurement>::SharedPtr measure_pub_;
+
+  // Publisher
+  rclcpp::Publisher<rm_interfaces::msg::Target>::SharedPtr target_pub_;
+  rclcpp::Publisher<rm_interfaces::msg::GimbalCmd>::SharedPtr gimbal_pub_;
+  rclcpp::Subscription<rm_interfaces::msg::SerialReceiveData>::SharedPtr serial_sub_;
+  rclcpp::TimerBase::SharedPtr pub_timer_;
+  void timerCallback();
+  std::atomic<double> bullet_speed_{0.0};
+  std::atomic<double> bullet_speed_param_{20.0};
+  std::atomic<bool> bullet_speed_debug_{false};
+  std::atomic<int> shoot_rate_{3};
+  
+  // Enable/Disable Armor Solver
+  bool enable_;
+  rclcpp::Service<rm_interfaces::srv::SetMode>::SharedPtr set_mode_srv_;
+
+  // Visualization marker publisher
+  visualization_msgs::msg::Marker position_marker_;
+  visualization_msgs::msg::Marker linear_v_marker_;
+  visualization_msgs::msg::Marker angular_v_marker_;
+  visualization_msgs::msg::Marker trajectory_marker_;
+  visualization_msgs::msg::Marker armors_marker_;
+  visualization_msgs::msg::Marker selection_marker_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_pub_;
+  rm_interfaces::msg::GimbalCmd control_msg_last; //保存上一次的角度
+};
+
+}  // namespace fyt::auto_aim
+
+#endif  // ARMOR_SOLVER_SOLVER_NODE_HPP_

src/rm_auto_aim/armor_solver/include/armor_solver/armor_tracker.hpp

@@ -0,0 +1,99 @@
+// Copyright Chen Jun 2023. Licensed under the MIT License.
+//
+// Additional modifications and features by Chengfu Zou, Labor. Licensed under Apache License 2.0.
+//
+// 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 ARMOR_SOLVER_TRACKER_HPP_
+#define ARMOR_SOLVER_TRACKER_HPP_
+
+// std
+#include <memory>
+#include <string>
+// ros2
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/quaternion.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
+// third party
+#include <Eigen/Eigen>
+// project
+#include "rm_interfaces/msg/armors.hpp"
+#include "rm_interfaces/msg/target.hpp"
+#include "rm_utils/math/extended_kalman_filter.hpp"
+#include "armor_solver/motion_model.hpp"
+
+namespace fyt::auto_aim {
+
+enum class ArmorsNum { NORMAL_4 = 4, BALANCE_2 = 2, OUTPOST_3 = 3 };
+
+class Tracker {
+public:
+  Tracker(double max_match_distance, double max_match_yaw);
+
+  using Armors = rm_interfaces::msg::Armors;
+  using Armor = rm_interfaces::msg::Armor;
+
+  void init(const Armors::SharedPtr &armors_msg) noexcept;
+
+  void update(const Armors::SharedPtr &armors_msg) noexcept;
+
+  void setMatchThreshold(double max_match_distance, double max_match_yaw_diff) noexcept;
+
+  enum State {
+    LOST,
+    DETECTING,
+    TRACKING,
+    TEMP_LOST,
+  } tracker_state;
+
+  std::unique_ptr<RobotStateEKF> ekf;
+
+  int tracking_thres;  // frame
+  int lost_thres;      // second
+
+  Armor tracked_armor;
+  std::string tracked_id;
+  ArmorsNum tracked_armors_num;
+  Eigen::VectorXd measurement;
+  Eigen::VectorXd target_state;
+
+  // To store another pair of armors message
+  double d_za, another_r;
+
+  // To store offset relative to the reference plane
+  double d_zc;
+
+private:
+  void initEKF(const Armor &a) noexcept;
+
+  void handleArmorJump(const Armor &a) noexcept;
+
+  double orientationToYaw(const geometry_msgs::msg::Quaternion &q) noexcept;
+
+  static Eigen::Vector3d getArmorPositionFromState(const Eigen::VectorXd &x) noexcept;
+
+  double max_match_distance_;
+  double max_match_yaw_diff_;
+
+  int detect_count_;
+  int lost_count_;
+
+  double last_yaw_;
+};
+
+}  // namespace fyt::auto_aim
+
+#endif  // ARMOR_SOLVER_ARMOR_TRACKER_HPP_

src/rm_auto_aim/armor_solver/include/armor_solver/motion_model.hpp

@@ -0,0 +1,84 @@
+// 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 ARMOR_SOLVER_MOTION_MODEL_HPP_
+#define ARMOR_SOLVER_MOTION_MODEL_HPP_
+
+// ceres
+#include <ceres/ceres.h>
+// project
+#include "rm_utils/math/extended_kalman_filter.hpp"
+
+namespace fyt::auto_aim {
+
+enum class MotionModel {
+  CONSTANT_VELOCITY = 0,  // Constant velocity
+  CONSTANT_ROTATION = 1,  // Constant rotation velocity
+  CONSTANT_VEL_ROT = 2    // Constant velocity and rotation velocity
+};
+
+// X_N: state dimension, Z_N: measurement dimension
+constexpr int X_N = 10, Z_N = 4;
+
+struct Predict {
+  explicit Predict(double dt, MotionModel model = MotionModel::CONSTANT_VEL_ROT)
+  : dt(dt), model(model) {}
+
+  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];
+    }
+
+    // v_xyz
+    if (model == MotionModel::CONSTANT_VEL_ROT || model == MotionModel::CONSTANT_VELOCITY) {
+      // linear velocity
+      x1[0] += x0[1] * dt;
+      x1[2] += x0[3] * dt;
+      x1[4] += x0[5] * dt;
+    } else {
+      // no velocity
+      x1[1] *= 0.;
+      x1[3] *= 0.;
+      x1[5] *= 0.;
+    }
+
+    // v_yaw
+    if (model == MotionModel::CONSTANT_VEL_ROT || model == MotionModel::CONSTANT_ROTATION) {
+      // angular velocity
+      x1[6] += x0[7] * dt;
+    } else {
+      // no rotation
+      x1[7] *= 0.;
+    }
+  }
+
+  double dt;
+  MotionModel model;
+};
+
+struct Measure {
+  template <typename T>
+  void operator()(const T x[Z_N], T z[Z_N]) {
+    z[0] = x[0] - ceres::cos(x[6]) * x[8];
+    z[1] = x[2] - ceres::sin(x[6]) * x[8];
+    z[2] = x[4] + x[9];
+    z[3] = x[6];
+  }
+};
+
+using RobotStateEKF = ExtendedKalmanFilter<X_N, Z_N, Predict, Measure>;
+
+}  // namespace fyt::auto_aim
+#endif

src/rm_auto_aim/armor_solver/package.xml

@@ -0,0 +1,37 @@
+<?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>armor_solver</name>
+  <version>0.1.0</version>
+  <description>A template for ROS packages.</description>
+  <maintainer email="chen.junn@outlook.com">Chen Jun</maintainer>
+  <license>BSD</license>
+  <url type="website">https://github.com/chenjunnn/rm_auto_aim</url>
+  <url type="bugtracker">https://github.com/chenjunnn/rm_auto_aim/issues</url>
+  <author email="chen.junn@outlook.com">Chen Jun</author>
+
+  <!-- buildtool_depend: dependencies of the build process -->
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <!-- depend: build, export, and execution dependency -->
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>eigen</depend>
+  <depend>angles</depend>
+  <depend>geometry_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>message_filters</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>rm_interfaces</depend>
+  <depend>rm_utils</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_clang_format</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/rm_auto_aim/armor_solver/src/armor_solver.cpp

@@ -0,0 +1,393 @@
+// Created by Chengfu Zou
+// 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 "armor_solver/armor_solver.hpp"
+// std
+#include <cmath>
+#include <cstddef>
+#include <stdexcept>
+// project
+#include "armor_solver/armor_solver_node.hpp"
+#include "rm_utils/logger/log.hpp"
+#include "rm_utils/math/utils.hpp"
+
+namespace fyt::auto_aim {
+Solver::Solver(std::weak_ptr<rclcpp::Node> n) : node_(n) {
+  auto node = node_.lock();
+
+  shooting_range_w_ = node->declare_parameter("solver.shooting_range_width", 0.135);
+  shooting_range_h_ = node->declare_parameter("solver.shooting_range_height", 0.135);
+  max_tracking_v_yaw_ = node->declare_parameter("solver.max_tracking_v_yaw", 6.0);
+  prediction_delay_ = node->declare_parameter("solver.prediction_delay", 0.0);
+  controller_delay_ = node->declare_parameter("solver.controller_delay", 0.0);
+  side_angle_ = node->declare_parameter("solver.side_angle", 15.0);
+  min_switching_v_yaw_ = node->declare_parameter("solver.min_switching_v_yaw", 1.0);
+
+  std::string compenstator_type = node->declare_parameter("solver.compensator_type", "ideal");
+  trajectory_compensator_ = CompensatorFactory::createCompensator(compenstator_type);
+  trajectory_compensator_->iteration_times = node->declare_parameter("solver.iteration_times", 20);
+  if (node->has_parameter("solver.bullet_speed")) {
+    trajectory_compensator_->velocity = node->get_parameter("solver.bullet_speed").as_double();
+  } else {
+    trajectory_compensator_->velocity = node->declare_parameter("solver.bullet_speed", 20.0);
+  }
+  trajectory_compensator_->gravity = node->declare_parameter("solver.gravity", 9.8);
+  trajectory_compensator_->resistance = node->declare_parameter("solver.resistance", 0.001);
+
+  manual_compensator_ = std::make_unique<ManualCompensator>();
+  auto angle_offset = node->declare_parameter("solver.angle_offset", std::vector<std::string>{});
+  if (!manual_compensator_->updateMapFlow(angle_offset)) {
+    FYT_WARN("armor_solver", "Manual compensator update failed!");
+  }
+
+  // Barrel frame parameters for trajectory calculation
+  // barrel_offset will be initialized from TF tree (barrel_link -> pitch_link)
+  use_barrel_frame_ = node->declare_parameter("solver.use_barrel_frame", true);
+  barrel_offset_ = Eigen::Vector3d(0.0, 0.0, 0.0);
+  barrel_offset_initialized_ = false;
+  FYT_INFO("armor_solver",
+           "Barrel frame enabled: {}, offset will be loaded from TF (barrel_link -> pitch_link)",
+           use_barrel_frame_);
+
+  state = State::TRACKING_ARMOR;
+  overflow_count_ = 0;
+  transfer_thresh_ = 5;
+
+  node.reset();
+}
+
+rm_interfaces::msg::GimbalCmd Solver::solve(const rm_interfaces::msg::Target &target,
+                                            const rclcpp::Time &current_time,
+                                            std::shared_ptr<tf2_ros::Buffer> tf2_buffer_) {
+  Eigen::Vector3d gimbal_pos_in_odom = Eigen::Vector3d::Zero();
+  Eigen::Vector3d barrel_pos_in_odom = Eigen::Vector3d::Zero();
+  Eigen::Matrix3d R_gimbal_to_odom = Eigen::Matrix3d::Identity();
+
+  // Get current roll, yaw and pitch of gimbal
+  try {
+    auto gimbal_tf =
+      tf2_buffer_->lookupTransform(target.header.frame_id, "pitch_link", tf2::TimePointZero);
+    auto msg_q = gimbal_tf.transform.rotation;
+
+    tf2::Quaternion tf_q;
+    tf2::fromMsg(msg_q, tf_q);
+    tf2::Matrix3x3 tf_mat(tf_q);
+    tf_mat.getRPY(rpy_[0], rpy_[1], rpy_[2]);
+    rpy_[1] = -rpy_[1];
+    R_gimbal_to_odom << tf_mat[0][0], tf_mat[0][1], tf_mat[0][2], tf_mat[1][0], tf_mat[1][1],
+      tf_mat[1][2], tf_mat[2][0], tf_mat[2][1], tf_mat[2][2];
+    gimbal_pos_in_odom = Eigen::Vector3d(gimbal_tf.transform.translation.x,
+                                         gimbal_tf.transform.translation.y,
+                                         gimbal_tf.transform.translation.z);
+  } catch (tf2::TransformException &ex) {
+    FYT_ERROR("armor_solver", "{}", ex.what());
+    throw ex;
+  }
+
+  // Initialize barrel_offset from TF tree (only once)
+  if (use_barrel_frame_ && !barrel_offset_initialized_) {
+    try {
+      auto barrel_tf =
+        tf2_buffer_->lookupTransform("pitch_link", "barrel_link", tf2::TimePointZero);
+      barrel_offset_ = Eigen::Vector3d(barrel_tf.transform.translation.x,
+                                       barrel_tf.transform.translation.y,
+                                       barrel_tf.transform.translation.z);
+      barrel_offset_initialized_ = true;
+      FYT_INFO("armor_solver",
+               "Barrel offset initialized from TF: [{:.3f}, {:.3f}, {:.3f}]",
+               barrel_offset_.x(),
+               barrel_offset_.y(),
+               barrel_offset_.z());
+    } catch (tf2::TransformException &ex) {
+      FYT_WARN(
+        "armor_solver", "Failed to get barrel_link transform: {}, using default offset", ex.what());
+      barrel_offset_ = Eigen::Vector3d(0.20, 0.0, -0.02);
+      barrel_offset_initialized_ = true;
+    }
+  }
+
+  // Use flying time to approximately predict the position of target
+  Eigen::Vector3d target_position(target.position.x, target.position.y, target.position.z);
+  double target_yaw = target.yaw;
+
+  // Calculate flying time considering barrel offset for more accurate prediction
+  Eigen::Vector3d target_for_flying_time = target_position;
+  if (use_barrel_frame_) {
+    // Approximate barrel position in odom frame for flying time calculation
+    barrel_pos_in_odom = gimbal_pos_in_odom + R_gimbal_to_odom * barrel_offset_;
+    target_for_flying_time = target_position - barrel_pos_in_odom;
+  }
+
+  double flying_time = trajectory_compensator_->getFlyingTime(target_for_flying_time);
+  double dt =
+    (current_time - rclcpp::Time(target.header.stamp)).seconds() + flying_time + prediction_delay_;
+  target_position.x() += dt * target.velocity.x;
+  target_position.y() += dt * target.velocity.y;
+  target_position.z() += dt * target.velocity.z;
+  target_yaw += dt * target.v_yaw;
+
+  // Choose the best armor to shoot
+  std::vector<Eigen::Vector3d> armor_positions = getArmorPositions(target_position,
+                                                                   target_yaw,
+                                                                   target.radius_1,
+                                                                   target.radius_2,
+                                                                   target.d_zc,
+                                                                   target.d_za,
+                                                                   target.armors_num);
+  int idx =
+    selectBestArmor(armor_positions, target_position, target_yaw, target.v_yaw, target.armors_num);
+  auto chosen_armor_position = armor_positions.at(idx);
+  if (chosen_armor_position.norm() < 0.1) {
+    throw std::runtime_error("No valid armor to shoot");
+  }
+
+  // Calculate yaw, pitch, distance
+  double yaw, pitch;
+  calcYawAndPitch(chosen_armor_position, barrel_pos_in_odom, yaw, pitch);
+  double distance = chosen_armor_position.norm();
+
+  // Initialize gimbal_cmd
+  rm_interfaces::msg::GimbalCmd gimbal_cmd;
+  gimbal_cmd.header = target.header;
+  gimbal_cmd.distance = distance;
+  gimbal_cmd.fire_advice = isOnTarget(rpy_[2], rpy_[1], yaw, pitch, distance);
+
+  switch (state) {
+    case TRACKING_ARMOR: {
+      if (std::abs(target.v_yaw) > max_tracking_v_yaw_) {
+        overflow_count_++;
+      } else {
+        overflow_count_ = 0;
+      }
+
+      if (overflow_count_ > transfer_thresh_) {
+        state = TRACKING_CENTER;
+      }
+
+      // If isOnTarget() never returns true, adjust controller_delay to force the gimbal to move
+      if (controller_delay_ != 0) {
+        target_position.x() += controller_delay_ * target.velocity.x;
+        target_position.y() += controller_delay_ * target.velocity.y;
+        target_position.z() += controller_delay_ * target.velocity.z;
+        target_yaw += controller_delay_ * target.v_yaw;
+        armor_positions = getArmorPositions(target_position,
+                                            target_yaw,
+                                            target.radius_1,
+                                            target.radius_2,
+                                            target.d_zc,
+                                            target.d_za,
+                                            target.armors_num);
+        chosen_armor_position = armor_positions.at(idx);
+        gimbal_cmd.distance = chosen_armor_position.norm();
+        if (chosen_armor_position.norm() < 0.1) {
+          throw std::runtime_error("No valid armor to shoot");
+        }
+        calcYawAndPitch(chosen_armor_position, barrel_pos_in_odom, yaw, pitch);
+      }
+      break;
+    }
+    case TRACKING_CENTER: {
+      if (std::abs(target.v_yaw) < max_tracking_v_yaw_) {
+        overflow_count_++;
+      } else {
+        overflow_count_ = 0;
+      }
+
+      if (overflow_count_ > transfer_thresh_) {
+        state = TRACKING_ARMOR;
+        overflow_count_ = 0;
+      }
+      gimbal_cmd.fire_advice = true;
+      calcYawAndPitch(target_position, barrel_pos_in_odom, yaw, pitch);
+      break;
+    }
+  }
+
+  // Compensate angle by angle_offset_map
+  auto angle_offset =
+    manual_compensator_->angleHardCorrect(target_position.head(2).norm(), target_position.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);
+
+  gimbal_cmd.yaw = cmd_yaw * 180 / M_PI;
+  gimbal_cmd.pitch = cmd_pitch * 180 / M_PI;
+  gimbal_cmd.yaw_diff = (cmd_yaw - rpy_[2]) * 180 / M_PI;
+  gimbal_cmd.pitch_diff = (cmd_pitch - rpy_[1]) * 180 / M_PI;
+
+  if (gimbal_cmd.fire_advice) {
+    FYT_DEBUG("armor_solver", "You Need Fire!");
+  }
+  return gimbal_cmd;
+}
+
+void Solver::updateRuntimeParams(double max_tracking_v_yaw,
+                                 double prediction_delay,
+                                 double controller_delay,
+                                 double side_angle,
+                                 double min_switching_v_yaw,
+                                 double shooting_range_w,
+                                 double shooting_range_h) noexcept {
+  max_tracking_v_yaw_ = max_tracking_v_yaw;
+  prediction_delay_ = prediction_delay;
+  controller_delay_ = controller_delay;
+  side_angle_ = side_angle;
+  min_switching_v_yaw_ = min_switching_v_yaw;
+  shooting_range_w_ = shooting_range_w;
+  shooting_range_h_ = shooting_range_h;
+}
+
+bool Solver::isOnTarget(const double cur_yaw,
+                        const double cur_pitch,
+                        const double target_yaw,
+                        const double target_pitch,
+                        const double distance) const noexcept {
+  // Judge whether to shoot
+  double shooting_range_yaw = std::abs(atan2(shooting_range_w_ / 2, distance));
+  double shooting_range_pitch = std::abs(atan2(shooting_range_h_ / 2, distance));
+  // 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 * M_PI / 180);
+  shooting_range_pitch = std::max(shooting_range_pitch, 1.0 * M_PI / 180);
+  if (std::abs(cur_yaw - target_yaw) < shooting_range_yaw &&
+      std::abs(cur_pitch - target_pitch) < shooting_range_pitch) {
+    return true;
+  }
+
+  return false;
+}
+
+std::vector<Eigen::Vector3d> Solver::getArmorPositions(const Eigen::Vector3d &target_center,
+                                                       const double target_yaw,
+                                                       const double r1,
+                                                       const double r2,
+                                                       const double d_zc,
+                                                       const double d_za,
+                                                       const size_t armors_num) const noexcept {
+  auto armor_positions = std::vector<Eigen::Vector3d>(armors_num, Eigen::Vector3d::Zero());
+  // Calculate the position of each armor
+  bool is_current_pair = true;
+  double r = 0., target_dz = 0.;
+  for (size_t i = 0; i < armors_num; i++) {
+    double temp_yaw = target_yaw + i * (2 * M_PI / armors_num);
+    if (armors_num == 4) {
+      r = is_current_pair ? r1 : r2;
+      target_dz = d_zc + (is_current_pair ? 0 : d_za);
+      is_current_pair = !is_current_pair;
+    } else {
+      r = r1;
+      target_dz = d_zc;
+    }
+    armor_positions[i] =
+      target_center + Eigen::Vector3d(-r * cos(temp_yaw), -r * sin(temp_yaw), target_dz);
+  }
+  return armor_positions;
+}
+
+int Solver::selectBestArmor(const std::vector<Eigen::Vector3d> &armor_positions,
+                            const Eigen::Vector3d &target_center,
+                            const double target_yaw,
+                            const double target_v_yaw,
+                            const size_t armors_num) const noexcept {
+  // Angle between the car's center and the X-axis
+  double alpha = std::atan2(target_center.y(), target_center.x());
+  // Angle between the front of observed armor and the X-axis
+  double beta = target_yaw;
+
+  // clang-format off
+  Eigen::Matrix2d R_odom2center;
+  Eigen::Matrix2d R_odom2armor;
+  R_odom2center << std::cos(alpha), std::sin(alpha), 
+                  -std::sin(alpha), std::cos(alpha);
+  R_odom2armor << std::cos(beta), std::sin(beta), 
+                 -std::sin(beta), std::cos(beta);
+  // clang-format on
+  Eigen::Matrix2d R_center2armor = R_odom2center.transpose() * R_odom2armor;
+
+  // Equal to (alpha - beta) in most cases
+  double decision_angle = -std::asin(R_center2armor(0, 1));
+
+  // Angle thresh of the armor jump
+  double theta = (target_v_yaw > 0 ? side_angle_ : -side_angle_) / 180.0 * M_PI;
+
+  // Avoid the frequent switch between two armor
+  if (std::abs(target_v_yaw) < min_switching_v_yaw_) {
+    theta = 0;
+  }
+
+  double temp_angle = decision_angle + M_PI / armors_num - theta;
+
+  if (temp_angle < 0) {
+    temp_angle += 2 * M_PI;
+  }
+
+  int selected_id = static_cast<int>(temp_angle / (2 * M_PI / armors_num));
+  return selected_id;
+}
+
+void Solver::calcYawAndPitch(const Eigen::Vector3d &p,
+                             const Eigen::Vector3d &barrel_pos_in_odom,
+                             double &yaw,
+                             double &pitch) const noexcept {
+  // Calculate yaw and pitch based on target position in odom frame
+  yaw = atan2(p.y(), p.x());
+  pitch = atan2(p.z(), p.head(2).norm());
+
+  // For trajectory compensation, we need to consider the barrel offset
+  // The trajectory compensator calculates based on the distance from barrel to target
+  Eigen::Vector3d target_in_barrel = p;
+  if (use_barrel_frame_) {
+    // Target position relative to barrel in odom frame
+    target_in_barrel = p - barrel_pos_in_odom;
+
+    FYT_DEBUG(
+      "armor_solver",
+      "Barrel compensation: target [{:.3f}, {:.3f}, {:.3f}] -> barrel_rel [{:.3f}, {:.3f}, {:.3f}]",
+      p.x(),
+      p.y(),
+      p.z(),
+      target_in_barrel.x(),
+      target_in_barrel.y(),
+      target_in_barrel.z());
+  }
+
+  if (double temp_pitch = pitch;
+      trajectory_compensator_->compensate(target_in_barrel, temp_pitch)) {
+    pitch = temp_pitch;
+  }
+}
+
+std::vector<std::pair<double, double>> Solver::getTrajectory() const noexcept {
+  auto trajectory = trajectory_compensator_->getTrajectory(15, rpy_[1]);
+  // Rotate
+  for (auto &p : trajectory) {
+    double x = p.first;
+    double y = p.second;
+    p.first = x * cos(rpy_[1]) + y * sin(rpy_[1]);
+    p.second = -x * sin(rpy_[1]) + y * cos(rpy_[1]);
+  }
+  return trajectory;
+}
+
+void Solver::setBulletSpeed(double bullet_speed) noexcept {
+  if (std::isfinite(bullet_speed) && bullet_speed > 0.0) {
+    trajectory_compensator_->velocity = bullet_speed;
+  }
+}
+
+}  // namespace fyt::auto_aim
+

src/rm_auto_aim/armor_solver/src/armor_solver_node.cpp

@@ -0,0 +1,604 @@
+// Copyright Chen Jun 2023. Licensed under the MIT License.
+//
+// Additional modifications and features by Chengfu Zou, Labor. Licensed under Apache License 2.0.
+//
+// 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 "armor_solver/armor_solver_node.hpp"
+
+// std
+#include <memory>
+#include <vector>
+// project
+#include "armor_solver/motion_model.hpp"
+#include "rm_utils/common.hpp"
+#include "rm_utils/heartbeat.hpp"
+
+namespace fyt::auto_aim {
+ArmorSolverNode::ArmorSolverNode(const rclcpp::NodeOptions &options)
+: Node("armor_solver", options), solver_(nullptr) {
+  // Register logger
+  FYT_REGISTER_LOGGER("armor_solver", "~/fyt2024-log", INFO);
+  FYT_INFO("armor_solver", "Starting ArmorSolverNode!");
+
+  debug_mode_ = this->declare_parameter("debug", true);
+  bullet_speed_debug_.store(this->declare_parameter("bullet_speed_debug", false),
+                            std::memory_order_relaxed);
+  shoot_rate_.store(this->declare_parameter("solver.shoot_rate", 3), std::memory_order_relaxed);
+  if (this->has_parameter("solver.bullet_speed")) {
+    bullet_speed_param_.store(this->get_parameter("solver.bullet_speed").as_double(),
+                              std::memory_order_relaxed);
+  } else {
+    bullet_speed_param_.store(this->declare_parameter("solver.bullet_speed", 20.0),
+                              std::memory_order_relaxed);
+  }
+
+  // Tracker
+  double max_match_distance = this->declare_parameter("tracker.max_match_distance", 0.2);
+  double max_match_yaw_diff = this->declare_parameter("tracker.max_match_yaw_diff", 1.0);
+  tracker_ = std::make_unique<Tracker>(max_match_distance, max_match_yaw_diff);
+  tracker_->tracking_thres = this->declare_parameter("tracker.tracking_thres", 5);
+  lost_time_thres_ = this->declare_parameter("tracker.lost_time_thres", 0.3);
+
+  // EKF
+  // xa = x_armor, xc = x_robot_center
+  // state: xc, v_xc, yc, v_yc, zc, v_zc, yaw, v_yaw, r, d_zc
+  // measurement: p, y, d, yaw
+  // f - Process function
+  auto f = Predict(0.005);
+  // h - Observation function
+  auto h = Measure();
+  // update_Q - process noise covariance matrix (Adaptive)
+  // Adaptive parameters: noise varies based on velocity and angular velocity
+  s2qxyz_max_ = declare_parameter("ekf.sigma2_q_xyz_max", 30.0);
+  s2qxyz_min_ = declare_parameter("ekf.sigma2_q_xyz_min", 10.0);
+  s2qyaw_max_ = declare_parameter("ekf.sigma2_q_yaw_max", 150.0);
+  s2qyaw_min_ = declare_parameter("ekf.sigma2_q_yaw_min", 50.0);
+  s2qr_ = declare_parameter("ekf.sigma2_q_r", 800.0);
+  s2qd_zc_ = declare_parameter("ekf.sigma2_q_d_zc", 800.0);
+
+  auto u_q = [this]() {
+    Eigen::Matrix<double, X_N, X_N> q;
+    double t = dt_;
+    
+    // Get current state for adaptive calculation
+    const auto &state = tracker_->target_state;
+    double vx = state(1), vy = state(3), v_yaw = state(7);
+    
+    // Adaptive noise calculation:
+    // - High angular velocity (spinning) -> lower position noise (trust position measurement more)
+    // - High linear velocity (moving fast) -> lower yaw noise (trust yaw measurement more)
+    double linear_vel = std::sqrt(vx * vx + vy * vy);
+    double angular_vel = std::abs(v_yaw);
+    
+    // Exponential decay: when velocity is high, noise approaches min value
+    double s2q_xyz = std::exp(-angular_vel) * (s2qxyz_max_ - s2qxyz_min_) + s2qxyz_min_;
+    double s2q_yaw = std::exp(-linear_vel) * (s2qyaw_max_ - s2qyaw_min_) + s2qyaw_min_;
+    
+    double r = s2qr_, d_zc = s2qd_zc_;
+    
+    // White noise integral model for position-velocity state
+    double q_x_x = pow(t, 4) / 4 * s2q_xyz, q_x_vx = pow(t, 3) / 2 * s2q_xyz, q_vx_vx = pow(t, 2) * s2q_xyz;
+    double q_y_y = pow(t, 4) / 4 * s2q_xyz, q_y_vy = pow(t, 3) / 2 * s2q_xyz, q_vy_vy = pow(t, 2) * s2q_xyz;
+    double q_z_z = pow(t, 4) / 4 * s2q_xyz, q_z_vz = pow(t, 3) / 2 * s2q_xyz, q_vz_vz = pow(t, 2) * s2q_xyz;
+    double q_yaw_yaw = pow(t, 4) / 4 * s2q_yaw, q_yaw_vyaw = pow(t, 3) / 2 * s2q_yaw,
+           q_vyaw_vyaw = pow(t, 2) * s2q_yaw;
+    double q_r = pow(t, 4) / 4 * r;
+    double q_d_zc = pow(t, 4) / 4 * d_zc;
+    // clang-format off
+    //    xc      v_xc    yc      v_yc    zc      v_zc    yaw         v_yaw       r       d_zc
+    q <<  q_x_x,  q_x_vx, 0,      0,      0,      0,      0,          0,          0,      0,
+          q_x_vx, q_vx_vx,0,      0,      0,      0,      0,          0,          0,      0,
+          0,      0,      q_y_y,  q_y_vy, 0,      0,      0,          0,          0,      0,
+          0,      0,      q_y_vy, q_vy_vy,0,      0,      0,          0,          0,      0,
+          0,      0,      0,      0,      q_z_z,  q_z_vz, 0,          0,          0,      0,
+          0,      0,      0,      0,      q_z_vz, q_vz_vz,0,          0,          0,      0,
+          0,      0,      0,      0,      0,      0,      q_yaw_yaw,  q_yaw_vyaw, 0,      0,
+          0,      0,      0,      0,      0,      0,      q_yaw_vyaw, q_vyaw_vyaw,0,      0,
+          0,      0,      0,      0,      0,      0,      0,          0,          q_r,    0,
+          0,      0,      0,      0,      0,      0,      0,          0,          0,      q_d_zc;
+
+    // clang-format on
+    return q;
+  };
+  // update_R - measurement noise covariance matrix
+  // R scales with distance: farther target -> larger measurement noise
+  r_x_ = declare_parameter("ekf.r_x", 0.05);
+  r_y_ = declare_parameter("ekf.r_y", 0.05);
+  r_z_ = declare_parameter("ekf.r_z", 0.05);
+  r_yaw_ = declare_parameter("ekf.r_yaw", 0.02);
+  auto u_r = [this](const Eigen::Matrix<double, Z_N, 1> &z) {
+    Eigen::Matrix<double, Z_N, Z_N> r;
+    // Calculate distance for better noise scaling
+    double dist = std::sqrt(z[0] * z[0] + z[1] * z[1] + z[2] * z[2]);
+    // Minimum distance to prevent numerical issues when target is very close
+    dist = std::max(dist, 1.0);
+    // clang-format off
+    r << r_x_ * dist, 0, 0, 0,
+         0, r_y_ * dist, 0, 0,
+         0, 0, r_z_ * dist, 0,
+         0, 0, 0, r_yaw_;
+    // clang-format on
+    return r;
+  };
+  // P - error estimate covariance matrix
+  Eigen::DiagonalMatrix<double, X_N> p0;
+  p0.setIdentity();
+  tracker_->ekf = std::make_unique<RobotStateEKF>(f, h, u_q, u_r, p0);
+
+  // Subscriber with tf2 message_filter
+  // tf2 relevant
+  tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
+  // Create the timer interface before call to waitForTransform,
+  // to avoid a tf2_ros::CreateTimerInterfaceException exception
+  auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
+    this->get_node_base_interface(), this->get_node_timers_interface());
+  tf2_buffer_->setCreateTimerInterface(timer_interface);
+  tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);
+  // subscriber and filter
+  armors_sub_.subscribe(this, "armor_detector/armors", rmw_qos_profile_sensor_data);
+  target_frame_ = this->declare_parameter("target_frame", "odom");
+  tf2_filter_ = std::make_shared<tf2_filter>(armors_sub_,
+                                             *tf2_buffer_,
+                                             target_frame_,
+                                             10,
+                                             this->get_node_logging_interface(),
+                                             this->get_node_clock_interface(),
+                                             std::chrono::duration<int>(1));
+  // Register a callback with tf2_ros::MessageFilter to be called when
+  // transforms are available
+  tf2_filter_->registerCallback(&ArmorSolverNode::armorsCallback, this);
+
+  // Measurement publisher (for debug usage)
+  measure_pub_ = this->create_publisher<rm_interfaces::msg::Measurement>("armor_solver/measurement",
+                                                                         rclcpp::SensorDataQoS());
+
+  // Publisher
+  target_pub_ = this->create_publisher<rm_interfaces::msg::Target>("armor_solver/target",
+                                                                   rclcpp::SensorDataQoS());
+  gimbal_pub_ = this->create_publisher<rm_interfaces::msg::GimbalCmd>("armor_solver/cmd_gimbal",
+                                                                      rclcpp::SensorDataQoS());
+  serial_sub_ = this->create_subscription<rm_interfaces::msg::SerialReceiveData>(
+    "serial/receive",
+    rclcpp::SensorDataQoS(),
+    [this](const rm_interfaces::msg::SerialReceiveData::SharedPtr msg) {
+      bullet_speed_.store(msg->bullet_speed, std::memory_order_relaxed);
+    });
+  // Timer 250 Hz
+  pub_timer_ = this->create_wall_timer(std::chrono::milliseconds(4),
+                                       std::bind(&ArmorSolverNode::timerCallback, this));
+  armor_target_.header.frame_id = "";
+
+  // Enable/Disable Armor Solver
+  enable_ = true;
+  set_mode_srv_ = this->create_service<rm_interfaces::srv::SetMode>(
+    "armor_solver/set_mode",
+    std::bind(
+      &ArmorSolverNode::setModeCallback, this, std::placeholders::_1, std::placeholders::_2));
+
+  if (debug_mode_) {
+    initMarkers();
+  }
+
+  on_set_parameters_callback_handle_ = this->add_on_set_parameters_callback(
+    std::bind(&ArmorSolverNode::onSetParameters, this, std::placeholders::_1));
+
+  // Heartbeat
+  heartbeat_ = HeartBeatPublisher::create(this);
+  //初始化control_msg_last
+  control_msg_last.yaw_diff = 0;
+  control_msg_last.pitch_diff = 0;
+  control_msg_last.distance = -1;
+  control_msg_last.pitch = 0;
+  control_msg_last.yaw = 0;
+  control_msg_last.fire_advice = false;
+  control_msg_last.shoot_rate =
+    static_cast<uint8_t>(shoot_rate_.load(std::memory_order_relaxed));
+
+}
+
+void ArmorSolverNode::timerCallback() {
+  if (solver_ == nullptr) {
+    return;
+  }
+
+  if (!enable_) {
+    return;
+  }
+
+  const bool bullet_speed_debug = bullet_speed_debug_.load(std::memory_order_relaxed);
+  const double serial_bullet_speed = bullet_speed_.load(std::memory_order_relaxed);
+  const double param_bullet_speed = bullet_speed_param_.load(std::memory_order_relaxed);
+  if (bullet_speed_debug) {
+    solver_->setBulletSpeed(param_bullet_speed);
+  } else if (serial_bullet_speed > 0.0) {
+    solver_->setBulletSpeed(serial_bullet_speed);
+  } else {
+    solver_->setBulletSpeed(param_bullet_speed);
+  }
+
+  // Init message
+  rm_interfaces::msg::GimbalCmd control_msg;
+  int shoot_rate = shoot_rate_.load(std::memory_order_relaxed);
+  if (shoot_rate < 0) 
+  {
+    shoot_rate = 0;
+  } 
+  else if (shoot_rate > 8) 
+  {
+    shoot_rate = 8;
+  }
+
+  // If target never detected
+  if (armor_target_.header.frame_id.empty()) {
+    control_msg = control_msg_last; //保持上一次的值
+    control_msg.distance = -1; // 默认发送-1
+    control_msg.fire_advice = false;//没识别不开火
+    gimbal_pub_->publish(control_msg);
+    return;
+  }
+
+  if (armor_target_.tracking) {
+    try {
+      control_msg = solver_->solve(armor_target_, this->now(), tf2_buffer_);
+    } catch (...) {
+      FYT_ERROR("armor_solver", "Something went wrong in solver!");
+          control_msg = control_msg_last; //保持上一次的值
+          control_msg.distance = -1; // 默认发送-1
+          control_msg.fire_advice = false;//没识别不开火
+    }
+  } else {
+    control_msg = control_msg_last; //保持上一次的值
+    control_msg.distance = -1; // 默认发送-1
+    control_msg.fire_advice = false;//没识别不开火
+  }
+  control_msg.shoot_rate = static_cast<uint8_t>(shoot_rate);
+  gimbal_pub_->publish(control_msg);
+  control_msg_last = control_msg; //保存上一次的值
+
+  if (debug_mode_) {
+    publishMarkers(armor_target_, control_msg);
+  }
+}
+
+void ArmorSolverNode::initMarkers() noexcept {
+  // Visualization Marker Publisher
+  // See http://wiki.ros.org/rviz/DisplayTypes/Marker
+  position_marker_.ns = "position";
+  position_marker_.type = visualization_msgs::msg::Marker::SPHERE;
+  position_marker_.scale.x = position_marker_.scale.y = position_marker_.scale.z = 0.1;
+  position_marker_.color.a = 1.0;
+  position_marker_.color.g = 1.0;
+  linear_v_marker_.type = visualization_msgs::msg::Marker::ARROW;
+  linear_v_marker_.ns = "linear_v";
+  linear_v_marker_.scale.x = 0.03;
+  linear_v_marker_.scale.y = 0.05;
+  linear_v_marker_.color.a = 1.0;
+  linear_v_marker_.color.r = 1.0;
+  linear_v_marker_.color.g = 1.0;
+  angular_v_marker_.type = visualization_msgs::msg::Marker::ARROW;
+  angular_v_marker_.ns = "angular_v";
+  angular_v_marker_.scale.x = 0.03;
+  angular_v_marker_.scale.y = 0.05;
+  angular_v_marker_.color.a = 1.0;
+  angular_v_marker_.color.b = 1.0;
+  angular_v_marker_.color.g = 1.0;
+  armors_marker_.ns = "filtered_armors";
+  armors_marker_.type = visualization_msgs::msg::Marker::CUBE;
+  armors_marker_.scale.x = 0.03;
+  armors_marker_.scale.z = 0.125;
+  armors_marker_.color.a = 1.0;
+  armors_marker_.color.b = 1.0;
+  selection_marker_.ns = "selection";
+  selection_marker_.type = visualization_msgs::msg::Marker::SPHERE;
+  selection_marker_.scale.x = selection_marker_.scale.y = selection_marker_.scale.z = 0.1;
+  selection_marker_.color.a = 1.0;
+  selection_marker_.color.g = 1.0;
+  selection_marker_.color.r = 1.0;
+  trajectory_marker_.ns = "trajectory";
+  trajectory_marker_.type = visualization_msgs::msg::Marker::POINTS;
+  trajectory_marker_.scale.x = 0.01;
+  trajectory_marker_.scale.y = 0.01;
+  trajectory_marker_.color.a = 1.0;
+  trajectory_marker_.color.r = 1.0;
+  trajectory_marker_.color.g = 0.75;
+  trajectory_marker_.color.b = 0.79;
+  trajectory_marker_.points.clear();
+
+  marker_pub_ =
+    this->create_publisher<visualization_msgs::msg::MarkerArray>("armor_solver/marker", 10);
+}
+
+void ArmorSolverNode::armorsCallback(const rm_interfaces::msg::Armors::SharedPtr armors_msg) {
+  // Lazy initialize solver owing to weak_from_this() can't be called in constructor
+  if (solver_ == nullptr) {
+    solver_ = std::make_unique<Solver>(weak_from_this());
+    solver_->updateRuntimeParams(this->get_parameter("solver.max_tracking_v_yaw").as_double(),
+                                 this->get_parameter("solver.prediction_delay").as_double(),
+                                 this->get_parameter("solver.controller_delay").as_double(),
+                                 this->get_parameter("solver.side_angle").as_double(),
+                                 this->get_parameter("solver.min_switching_v_yaw").as_double(),
+                                 this->get_parameter("solver.shooting_range_width").as_double(),
+                                 this->get_parameter("solver.shooting_range_height").as_double());
+    solver_->setBulletSpeed(bullet_speed_param_.load(std::memory_order_relaxed));
+  }
+
+  // Tranform armor position from image frame to world coordinate
+  for (auto &armor : armors_msg->armors) {
+    geometry_msgs::msg::PoseStamped ps;
+    ps.header = armors_msg->header;
+    ps.pose = armor.pose;
+    try {
+      armor.pose = tf2_buffer_->transform(ps, target_frame_).pose;
+    } catch (const tf2::TransformException &ex) {
+      FYT_ERROR("armor_solver", "Transform error: {}", ex.what());
+      return;
+    }
+  }
+
+  // Filter abnormal armors
+  armors_msg->armors.erase(std::remove_if(armors_msg->armors.begin(),
+                                          armors_msg->armors.end(),
+                                          [](const rm_interfaces::msg::Armor &armor) {
+                                            return abs(armor.pose.position.z) > 2;
+                                          }),
+                           armors_msg->armors.end());
+
+  // Init message
+  rm_interfaces::msg::Measurement measure_msg;
+  rm_interfaces::msg::Target target_msg;
+  rclcpp::Time time = armors_msg->header.stamp;
+  target_msg.header.stamp = time;
+  target_msg.header.frame_id = target_frame_;
+
+  // Update tracker
+  if (tracker_->tracker_state == Tracker::LOST) {
+    tracker_->init(armors_msg);
+    target_msg.tracking = false;
+  } else {
+    dt_ = (time - last_time_).seconds();
+    tracker_->lost_thres = std::abs(static_cast<int>(lost_time_thres_ / dt_));
+    if (tracker_->tracked_id == "outpost") {
+      tracker_->ekf->setPredictFunc(Predict{dt_, MotionModel::CONSTANT_ROTATION});
+    } else {
+      tracker_->ekf->setPredictFunc(Predict{dt_, MotionModel::CONSTANT_VEL_ROT});
+    }
+    tracker_->update(armors_msg);
+    // Publish measurement
+    measure_msg.x = tracker_->measurement(0);
+    measure_msg.y = tracker_->measurement(1);
+    measure_msg.z = tracker_->measurement(2);
+    measure_msg.yaw = tracker_->measurement(3);
+    if (measure_pub_->get_subscription_count() > 0) {
+      measure_pub_->publish(measure_msg);
+    }
+
+    if (tracker_->tracker_state == Tracker::DETECTING) {
+      target_msg.tracking = false;
+    } else if (tracker_->tracker_state == Tracker::TRACKING ||
+               tracker_->tracker_state == Tracker::TEMP_LOST) {
+      target_msg.tracking = true;
+      // Fill target message
+      const auto &state = tracker_->target_state;
+      target_msg.id = tracker_->tracked_id;
+      target_msg.armors_num = static_cast<int>(tracker_->tracked_armors_num);
+      target_msg.position.x = state(0);
+      target_msg.velocity.x = state(1);
+      target_msg.position.y = state(2);
+      target_msg.velocity.y = state(3);
+      target_msg.position.z = state(4);
+      target_msg.velocity.z = state(5);
+      target_msg.yaw = state(6);
+      target_msg.v_yaw = state(7);
+      target_msg.radius_1 = state(8);
+      target_msg.radius_2 = tracker_->another_r;
+      target_msg.d_zc = state(9);
+      target_msg.d_za = tracker_->d_za;
+    }
+  }
+
+  // Store and Publish the target_msg
+  armor_target_ = target_msg;
+  target_pub_->publish(target_msg);
+
+  last_time_ = time;
+}
+
+void ArmorSolverNode::publishMarkers(const rm_interfaces::msg::Target &target_msg,
+                                     const rm_interfaces::msg::GimbalCmd &gimbal_cmd) noexcept {
+  position_marker_.header = target_msg.header;
+  linear_v_marker_.header = target_msg.header;
+  angular_v_marker_.header = target_msg.header;
+  armors_marker_.header = target_msg.header;
+  selection_marker_.header = target_msg.header;
+  trajectory_marker_.header = target_msg.header;
+
+  visualization_msgs::msg::MarkerArray marker_array;
+
+  if (target_msg.tracking) {
+    double yaw = target_msg.yaw, r1 = target_msg.radius_1, r2 = target_msg.radius_2;
+    double xc = target_msg.position.x, yc = target_msg.position.y, zc = target_msg.position.z;
+    double vx = target_msg.velocity.x, vy = target_msg.velocity.y, vz = target_msg.velocity.z;
+    double d_za = target_msg.d_za, d_zc = target_msg.d_zc;
+    position_marker_.action = visualization_msgs::msg::Marker::ADD;
+    position_marker_.pose.position.x = xc;
+    position_marker_.pose.position.y = yc;
+    position_marker_.pose.position.z = zc;
+
+    linear_v_marker_.action = visualization_msgs::msg::Marker::ADD;
+    linear_v_marker_.points.clear();
+    linear_v_marker_.points.emplace_back(position_marker_.pose.position);
+    geometry_msgs::msg::Point arrow_end = position_marker_.pose.position;
+    arrow_end.x += vx;
+    arrow_end.y += vy;
+    arrow_end.z += vz;
+    linear_v_marker_.points.emplace_back(arrow_end);
+
+    angular_v_marker_.action = visualization_msgs::msg::Marker::ADD;
+    angular_v_marker_.points.clear();
+    angular_v_marker_.points.emplace_back(position_marker_.pose.position);
+    arrow_end = position_marker_.pose.position;
+    arrow_end.z += target_msg.v_yaw / M_PI;
+    angular_v_marker_.points.emplace_back(arrow_end);
+
+    armors_marker_.action = visualization_msgs::msg::Marker::ADD;
+    armors_marker_.scale.y = tracker_->tracked_armor.type == "small" ? 0.135 : 0.23;
+    // Draw armors
+    bool is_current_pair = true;
+    size_t a_n = target_msg.armors_num;
+    geometry_msgs::msg::Point p_a;
+    double r = 0;
+    for (size_t i = 0; i < a_n; i++) {
+      double tmp_yaw = yaw + i * (2 * M_PI / a_n);
+      // Only 4 armors has 2 radius and height
+      if (a_n == 4) {
+        r = is_current_pair ? r1 : r2;
+        p_a.z = zc + d_zc + (is_current_pair ? 0 : d_za);
+        is_current_pair = !is_current_pair;
+      } else {
+        r = r1;
+        p_a.z = zc;
+      }
+      p_a.x = xc - r * cos(tmp_yaw);
+      p_a.y = yc - r * sin(tmp_yaw);
+
+      armors_marker_.id = i;
+      armors_marker_.pose.position = p_a;
+      tf2::Quaternion q;
+      q.setRPY(0, target_msg.id == "outpost" ? -0.2618 : 0.2618, tmp_yaw);
+      armors_marker_.pose.orientation = tf2::toMsg(q);
+      marker_array.markers.emplace_back(armors_marker_);
+    }
+
+    selection_marker_.action = visualization_msgs::msg::Marker::ADD;
+    selection_marker_.points.clear();
+    selection_marker_.pose.position.y = gimbal_cmd.distance * sin(gimbal_cmd.yaw * M_PI / 180);
+    selection_marker_.pose.position.x = gimbal_cmd.distance * cos(gimbal_cmd.yaw * M_PI / 180);
+    selection_marker_.pose.position.z = gimbal_cmd.distance * sin(gimbal_cmd.pitch * M_PI / 180);
+
+    trajectory_marker_.action = visualization_msgs::msg::Marker::ADD;
+    trajectory_marker_.points.clear();
+    trajectory_marker_.header.frame_id = "pitch_link";
+    for (const auto &point : solver_->getTrajectory()) {
+      geometry_msgs::msg::Point p;
+      p.x = point.first;
+      p.z = point.second;
+      trajectory_marker_.points.emplace_back(p);
+    }
+    if (gimbal_cmd.fire_advice) {
+      trajectory_marker_.color.r = 0;
+      trajectory_marker_.color.g = 1;
+      trajectory_marker_.color.b = 0;
+    } else {
+      trajectory_marker_.color.r = 1;
+      trajectory_marker_.color.g = 1;
+      trajectory_marker_.color.b = 1;
+    }
+
+  } else {
+    position_marker_.action = visualization_msgs::msg::Marker::DELETE;
+    linear_v_marker_.action = visualization_msgs::msg::Marker::DELETE;
+    angular_v_marker_.action = visualization_msgs::msg::Marker::DELETE;
+    armors_marker_.action = visualization_msgs::msg::Marker::DELETE;
+    trajectory_marker_.action = visualization_msgs::msg::Marker::DELETE;
+    selection_marker_.action = visualization_msgs::msg::Marker::DELETE;
+  }
+
+  marker_array.markers.emplace_back(position_marker_);
+  marker_array.markers.emplace_back(trajectory_marker_);
+  marker_array.markers.emplace_back(linear_v_marker_);
+  marker_array.markers.emplace_back(angular_v_marker_);
+  marker_array.markers.emplace_back(armors_marker_);
+  marker_array.markers.emplace_back(selection_marker_);
+  marker_pub_->publish(marker_array);
+}
+
+void ArmorSolverNode::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("armor_solver", "Invalid mode: {}", request->mode);
+    return;
+  }
+
+  switch (mode) {
+    case VisionMode::AUTO_AIM_RED:
+    case VisionMode::AUTO_AIM_BLUE: {
+      enable_ = true;
+      break;
+    }
+    default: {
+      enable_ = false;
+      break;
+    }
+  }
+
+  FYT_WARN("armor_solver", "Set Mode to {}", visionModeToString(mode));
+}
+
+rcl_interfaces::msg::SetParametersResult ArmorSolverNode::onSetParameters(
+  const std::vector<rclcpp::Parameter> &parameters) {
+  rcl_interfaces::msg::SetParametersResult result;
+  result.successful = true;
+
+  for (const auto &param : parameters) {
+    if (param.get_name() == "debug") {
+      debug_mode_ = param.as_bool();
+    } else if (param.get_name() == "bullet_speed_debug") {
+      bullet_speed_debug_.store(param.as_bool(), std::memory_order_relaxed);
+    } else if (param.get_name() == "solver.bullet_speed") {
+      bullet_speed_param_.store(param.as_double(), std::memory_order_relaxed);
+    } else if (param.get_name() == "solver.shoot_rate") {
+      shoot_rate_.store(param.as_int(), std::memory_order_relaxed);
+    } else if (param.get_name() == "tracker.max_match_distance") {
+      const double max_match_distance = param.as_double();
+      tracker_->setMatchThreshold(max_match_distance, this->get_parameter("tracker.max_match_yaw_diff").as_double());
+    } else if (param.get_name() == "tracker.max_match_yaw_diff") {
+      const double max_match_yaw_diff = param.as_double();
+      tracker_->setMatchThreshold(this->get_parameter("tracker.max_match_distance").as_double(), max_match_yaw_diff);
+    } else if (param.get_name() == "tracker.tracking_thres") {
+      tracker_->tracking_thres = param.as_int();
+    } else if (param.get_name() == "tracker.lost_time_thres") {
+      lost_time_thres_ = param.as_double();
+    }
+  }
+
+  if (solver_ != nullptr) {
+    solver_->updateRuntimeParams(this->get_parameter("solver.max_tracking_v_yaw").as_double(),
+                                 this->get_parameter("solver.prediction_delay").as_double(),
+                                 this->get_parameter("solver.controller_delay").as_double(),
+                                 this->get_parameter("solver.side_angle").as_double(),
+                                 this->get_parameter("solver.min_switching_v_yaw").as_double(),
+                                 this->get_parameter("solver.shooting_range_width").as_double(),
+                                 this->get_parameter("solver.shooting_range_height").as_double());
+    solver_->setBulletSpeed(bullet_speed_param_.load(std::memory_order_relaxed));
+  }
+
+  return result;
+}
+
+}  // namespace fyt::auto_aim
+
+#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::auto_aim::ArmorSolverNode)
+

src/rm_auto_aim/armor_solver/src/armor_tracker.cpp

@@ -0,0 +1,272 @@
+// Copyright Chen Jun 2023. Licensed under the MIT License.
+//
+// Additional modifications and features by Chengfu Zou, Labor. Licensed under Apache License 2.0.
+//
+// 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 "armor_solver/armor_tracker.hpp"
+// std
+#include <cfloat>
+#include <memory>
+#include <string>
+// ros2
+#include <tf2/LinearMath/Matrix3x3.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/convert.h>
+
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+// third party
+#include <angles/angles.h>
+// project
+#include "rm_utils/logger/log.hpp"
+
+namespace fyt::auto_aim {
+Tracker::Tracker(double max_match_distance, double max_match_yaw_diff)
+: tracker_state(LOST)
+, tracked_id(std::string(""))
+, measurement(Eigen::VectorXd::Zero(4))
+, target_state(Eigen::VectorXd::Zero(9))
+, max_match_distance_(max_match_distance)
+, max_match_yaw_diff_(max_match_yaw_diff)
+, detect_count_(0)
+, lost_count_(0)
+, last_yaw_(0) {}
+
+void Tracker::setMatchThreshold(double max_match_distance, double max_match_yaw_diff) noexcept {
+  max_match_distance_ = max_match_distance;
+  max_match_yaw_diff_ = max_match_yaw_diff;
+}
+
+void Tracker::init(const Armors::SharedPtr &armors_msg) noexcept {
+  if (armors_msg->armors.empty()) {
+    return;
+  }
+
+  // Simply choose the armor that is closest to image center
+  double min_distance = DBL_MAX;
+  tracked_armor = armors_msg->armors[0];
+  for (const auto &armor : armors_msg->armors) {
+    if (armor.distance_to_image_center < min_distance) {
+      min_distance = armor.distance_to_image_center;
+      tracked_armor = armor;
+    }
+  }
+
+  initEKF(tracked_armor);
+  FYT_INFO("armor_solver", "Init EKF!");
+
+  tracked_id = tracked_armor.number;
+  tracker_state = DETECTING;
+
+  if (tracked_armor.type == "large" &&
+      (tracked_id == "3" || tracked_id == "4" || tracked_id == "5")) {
+    tracked_armors_num = ArmorsNum::BALANCE_2;
+  } else if (tracked_id == "outpost") {
+    tracked_armors_num = ArmorsNum::OUTPOST_3;
+  } else {
+    tracked_armors_num = ArmorsNum::NORMAL_4;
+  }
+}
+
+void Tracker::update(const Armors::SharedPtr &armors_msg) noexcept {
+  // KF predict
+  Eigen::VectorXd ekf_prediction = ekf->predict();
+
+  bool matched = false;
+  // Use KF prediction as default target state if no matched armor is found
+  target_state = ekf_prediction;
+
+  if (!armors_msg->armors.empty()) {
+    // Find the closest armor with the same id
+    Armor same_id_armor;
+    int same_id_armors_count = 0;
+    auto predicted_position = getArmorPositionFromState(ekf_prediction);
+    double min_position_diff = DBL_MAX;
+    double yaw_diff = DBL_MAX;
+    for (const auto &armor : armors_msg->armors) {
+      // Only consider armors with the same id
+      if (armor.number == tracked_id) {
+        same_id_armor = armor;
+        same_id_armors_count++;
+        // Calculate the difference between the predicted position and the
+        // current armor position
+        auto p = armor.pose.position;
+        Eigen::Vector3d position_vec(p.x, p.y, p.z);
+        double position_diff = (predicted_position - position_vec).norm();
+        if (position_diff < min_position_diff) {
+          // Find the closest armor
+          min_position_diff = position_diff;
+          yaw_diff = abs(orientationToYaw(armor.pose.orientation) - ekf_prediction(6));
+          tracked_armor = armor;
+          // Update tracked armor type
+          if (tracked_armor.type == "large" &&
+              (tracked_id == "3" || tracked_id == "4" || tracked_id == "5")) {
+            tracked_armors_num = ArmorsNum::BALANCE_2;
+          } else if (tracked_id == "outpost") {
+            tracked_armors_num = ArmorsNum::OUTPOST_3;
+          } else {
+            tracked_armors_num = ArmorsNum::NORMAL_4;
+          }
+        }
+      }
+    }
+
+    // Check if the distance and yaw difference of closest armor are within the
+    // threshold
+    if (min_position_diff < max_match_distance_ && yaw_diff < max_match_yaw_diff_) {
+      // Matched armor found
+      matched = true;
+      auto p = tracked_armor.pose.position;
+      // Update EKF
+      double measured_yaw = orientationToYaw(tracked_armor.pose.orientation);
+      measurement = Eigen::Vector4d(p.x, p.y, p.z, measured_yaw);
+      target_state = ekf->update(measurement);
+    } else if (same_id_armors_count == 1 && yaw_diff > max_match_yaw_diff_) {
+      // Matched armor not found, but there is only one armor with the same id
+      // and yaw has jumped, take this case as the target is spinning and armor
+      // jumped
+      handleArmorJump(same_id_armor);
+    } else {
+      // No matched armor found
+      FYT_WARN("armor_solver", "No matched armor found!");
+    }
+  }
+
+  // Prevent radius from spreading
+  if (target_state(8) < 0.12) {
+    target_state(8) = 0.12;
+    ekf->setState(target_state);
+  } else if (target_state(8) > 0.4) {
+    target_state(8) = 0.4;
+    ekf->setState(target_state);
+  }
+
+  // Tracking state machine
+  if (tracker_state == DETECTING) {
+    if (matched) {
+      detect_count_++;
+      if (detect_count_ > tracking_thres) {
+        detect_count_ = 0;
+        tracker_state = TRACKING;
+        FYT_DEBUG("armor_solver", "Tracker state: TRACKING {}", tracked_id);
+      }
+    } else {
+      detect_count_ = 0;
+      tracker_state = LOST;
+      FYT_DEBUG("armor_solver", "Tracker state: LOST {}", tracked_id);
+    }
+  } else if (tracker_state == TRACKING) {
+    if (!matched) {
+      tracker_state = TEMP_LOST;
+      lost_count_++;
+      FYT_DEBUG("armor_solver", "Tracker state: TEMP_LOST {}", tracked_id);
+    }
+  } else if (tracker_state == TEMP_LOST) {
+    if (!matched) {
+      lost_count_++;
+      if (lost_count_ > lost_thres) {
+        lost_count_ = 0;
+        tracker_state = LOST;
+        FYT_DEBUG("armor_solver", "Tracker state: LOST {}", tracked_id);
+      }
+    } else {
+      tracker_state = TRACKING;
+      lost_count_ = 0;
+      FYT_DEBUG("armor_solver", "Tracker state: TRACKING {}", tracked_id);
+    }
+  }
+}
+
+void Tracker::initEKF(const Armor &a) noexcept {
+  double xa = a.pose.position.x;
+  double ya = a.pose.position.y;
+  double za = a.pose.position.z;
+  last_yaw_ = 0;
+  double yaw = orientationToYaw(a.pose.orientation);
+
+  // Set initial position at 0.2m behind the target
+  target_state = Eigen::VectorXd::Zero(X_N);
+  double r = 0.26;
+  double xc = xa + r * cos(yaw);
+  double yc = ya + r * sin(yaw);
+  double zc = za;
+  d_za = 0, d_zc = 0, another_r = r;
+  target_state << xc, 0, yc, 0, zc, 0, yaw, 0, r, d_zc;
+
+  ekf->setState(target_state);
+}
+
+void Tracker::handleArmorJump(const Armor &current_armor) noexcept {
+  double last_yaw = target_state(6);
+  double yaw = orientationToYaw(current_armor.pose.orientation);
+
+  if (abs(yaw - last_yaw) > 0.4) {
+    // Armor angle also jumped, take this case as target spinning
+    target_state(6) = yaw;
+    // Only 4 armors has 2 radius and height
+    if (tracked_armors_num == ArmorsNum::NORMAL_4) {
+      d_za = target_state(4) + target_state(9) - current_armor.pose.position.z;
+      std::swap(target_state(8), another_r);
+      d_zc = d_zc == 0 ? -d_za : 0;
+      target_state(9) = d_zc;
+    }
+    FYT_DEBUG("armor_solver", "Armor Jump!");
+  }
+
+  auto p = current_armor.pose.position;
+  Eigen::Vector3d current_p(p.x, p.y, p.z);
+  Eigen::Vector3d infer_p = getArmorPositionFromState(target_state);
+
+  if ((current_p - infer_p).norm() > max_match_distance_) {
+    // If the distance between the current armor and the inferred armor is too
+    // large, the state is wrong, reset center position and velocity in the
+    // state
+    d_zc = 0;
+    double r = target_state(8);
+    target_state(0) = p.x + r * cos(yaw);  // xc
+    target_state(1) = 0;                   // vxc
+    target_state(2) = p.y + r * sin(yaw);  // yc
+    target_state(3) = 0;                   // vyc
+    target_state(4) = p.z;                 // zc
+    target_state(5) = 0;                   // vzc
+    target_state(9) = d_zc;                // d_zc
+    FYT_WARN("armor_solver", "State wrong!");
+  }
+
+  ekf->setState(target_state);
+}
+
+double Tracker::orientationToYaw(const geometry_msgs::msg::Quaternion &q) noexcept {
+  // Get armor yaw
+  tf2::Quaternion tf_q;
+  tf2::fromMsg(q, tf_q);
+  double roll, pitch, yaw;
+  tf2::Matrix3x3(tf_q).getRPY(roll, pitch, yaw);
+  // Make yaw change continuous (-pi~pi to -inf~inf)
+  yaw = last_yaw_ + angles::shortest_angular_distance(last_yaw_, yaw);
+  last_yaw_ = yaw;
+  return yaw;
+}
+
+Eigen::Vector3d Tracker::getArmorPositionFromState(const Eigen::VectorXd &x) noexcept {
+  // Calculate predicted position of the current armor
+  double xc = x(0), yc = x(2), za = x(4) + x(9);
+  double yaw = x(6), r = x(8);
+  double xa = xc - r * cos(yaw);
+  double ya = yc - r * sin(yaw);
+  return Eigen::Vector3d(xa, ya, za);
+}
+
+}  // namespace fyt::auto_aim