add task

CMakeLists.txt

@@ -42,21 +42,21 @@ target_link_libraries(calibrate_robotworld_handeye ${OpenCV_LIBS} fmt::fmt yaml-
 target_link_libraries(split_video ${OpenCV_LIBS} fmt::fmt component)
 
 ##################tests##################
-add_executable(auto_aim_test src/task/auto_aim_test.cpp)
-add_executable(auto_buff_test src/task/auto_buff_test.cpp)
-add_executable(camera_detect_test src/task/camera_detect_test.cpp)
-add_executable(camera_test src/task/camera_test.cpp)
-add_executable(camera_thread_test src/task/camera_thread_test.cpp)
-add_executable(cboard_test src/task/cboard_test.cpp)
-add_executable(fire_test src/task/fire_test.cpp)
-add_executable(detector_video_test src/task/detector_video_test.cpp)
-add_executable(gimbal_response_test src/task/gimbal_response_test.cpp)
-add_executable(multi_usbcamera_test src/task/multi_usbcamera_test.cpp)
-add_executable(usbcamera_detect_test src/task/usbcamera_detect_test.cpp)
-add_executable(usbcamera_test src/task/usbcamera_test.cpp)
-add_executable(handeye_test src/task/handeye_test.cpp)
-add_executable(dm_test src/task/dm_test.cpp)
-add_executable(minimum_vision_system src/task/minimum_vision_system.cpp)
+add_executable(auto_aim_test src/task/test/auto_aim_test.cpp)
+add_executable(auto_buff_test src/task/test/auto_buff_test.cpp)
+add_executable(camera_detect_test src/task/test/camera_detect_test.cpp)
+add_executable(camera_test src/task/test/camera_test.cpp)
+add_executable(camera_thread_test src/task/test/camera_thread_test.cpp)
+add_executable(cboard_test src/task/test/cboard_test.cpp)
+add_executable(fire_test src/task/test/fire_test.cpp)
+add_executable(detector_video_test src/task/test/detector_video_test.cpp)
+add_executable(gimbal_response_test src/task/test/gimbal_response_test.cpp)
+add_executable(multi_usbcamera_test src/task/test/multi_usbcamera_test.cpp)
+add_executable(usbcamera_detect_test src/task/test/usbcamera_detect_test.cpp)
+add_executable(usbcamera_test src/task/test/usbcamera_test.cpp)
+add_executable(handeye_test src/task/test/handeye_test.cpp)
+add_executable(dm_test src/task/test/dm_test.cpp)
+add_executable(minimum_vision_system src/task/test/minimum_vision_system.cpp)
 
 target_link_libraries(auto_aim_test ${OpenCV_LIBS} fmt::fmt yaml-cpp component device auto_aim)
 target_link_libraries(auto_buff_test ${OpenCV_LIBS} fmt::fmt yaml-cpp auto_buff component device)
@@ -74,11 +74,41 @@ target_link_libraries(handeye_test ${OpenCV_LIBS} fmt::fmt yaml-cpp component de
 target_link_libraries(dm_test ${OpenCV_LIBS} fmt::fmt yaml-cpp component device)
 target_link_libraries(minimum_vision_system ${OpenCV_LIBS} fmt::fmt yaml-cpp component device auto_aim)
 
-add_executable(gimbal_test src/task/gimbal_test.cpp)
+add_executable(gimbal_test src/task/test/gimbal_test.cpp)
 target_link_libraries(gimbal_test ${OpenCV_LIBS} fmt::fmt yaml-cpp auto_aim component device)
 
-add_executable(planner_test src/task/planner_test.cpp)
+add_executable(planner_test src/task/test/planner_test.cpp)
 target_link_libraries(planner_test ${OpenCV_LIBS} fmt::fmt yaml-cpp auto_aim component device)
 
-add_executable(planner_test_offline src/task/planner_test_offline.cpp)
-target_link_libraries(planner_test_offline ${OpenCV_LIBS} fmt::fmt yaml-cpp auto_aim component device)
+add_executable(planner_test_offline src/task/test/planner_test_offline.cpp)
+target_link_libraries(planner_test_offline ${OpenCV_LIBS} fmt::fmt yaml-cpp auto_aim component device)
+
+################## new tasks (no ROS2) ##################
+add_executable(auto_aim_debug_mpc src/task/auto_aim_debug_mpc.cpp)
+target_link_libraries(auto_aim_debug_mpc ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann_json::nlohmann_json auto_aim component device)
+
+add_executable(auto_buff_debug src/task/auto_buff_debug.cpp)
+target_link_libraries(auto_buff_debug ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann_json::nlohmann_json auto_aim auto_buff component device)
+
+add_executable(auto_buff_debug_mpc src/task/auto_buff_debug_mpc.cpp)
+target_link_libraries(auto_buff_debug_mpc ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann_json::nlohmann_json auto_aim auto_buff component device)
+
+add_executable(mt_auto_aim_debug src/task/mt_auto_aim_debug.cpp)
+target_link_libraries(mt_auto_aim_debug ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann_json::nlohmann_json auto_aim component device)
+
+add_executable(mt_standard src/task/mt_standard.cpp)
+target_link_libraries(mt_standard ${OpenCV_LIBS} fmt::fmt yaml-cpp auto_aim auto_buff component device)
+
+add_executable(standard src/task/standard.cpp)
+target_link_libraries(standard ${OpenCV_LIBS} fmt::fmt yaml-cpp auto_aim component device)
+
+add_executable(standard_mpc src/task/standard_mpc.cpp)
+target_link_libraries(standard_mpc ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann_json::nlohmann_json auto_aim auto_buff component device)
+
+add_executable(uav src/task/uav.cpp)
+target_link_libraries(uav ${OpenCV_LIBS} fmt::fmt yaml-cpp auto_aim auto_buff component device)
+
+add_executable(uav_debug src/task/uav_debug.cpp)
+target_link_libraries(uav_debug ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann_json::nlohmann_json auto_aim auto_buff component device)
+
+

README.md

@@ -1,2 +1,137 @@
-# 通用自瞄
-适用于单一云台的自瞄
+# MOVE_AI
+
+适用于 RoboMaster 机器人的视觉自瞄系统，参考同济大学 Superpower 战队 25 年开源设计，适配 MOVE。
+
+## 项目结构
+
+```
+├── calibration/          # 标定工具
+├── configs/              # 配置文件（yaml）
+├── src/
+│   ├── component/        # 通用组件（EKF、弹道、日志、绘图等）
+│   ├── device/           # 设备驱动（相机、串口、CAN、IMU）
+│   ├── module/
+│   │   ├── auto_aim/     # 自瞄模块（检测、解算、跟踪、瞄准、规划）
+│   │   ├── auto_buff/    # 打符模块
+│   │   └── omniperception/  # 全向感知模块（哨兵用）
+│   └── task/
+│       ├── *.cpp         # 各兵种主程序
+│       └── test/         # 测试用例
+```
+
+## 环境要求
+
+- Ubuntu 22.04
+- 运算平台：Intel NUC（i7-1260P / i7-1165G7）
+- 相机：海康 MV-CS016-10UC + 6mm 镜头
+- 下位机：RoboMaster C 型开发板（STM32F407）/ 达妙 MC02（STM32H7）
+
+## 依赖安装
+
+1. SDK：
+   - [HikRobot MVS SDK](https://www.hikrobotics.com/cn2/source/support/software/MVS_STD_GML_V2.1.2_231116.zip)
+   - [MindVision SDK](https://mindvision.com.cn/category/software/sdk-installation-package/)（可选）
+   - [OpenVINO 2024](https://docs.openvino.ai/2024/get-started/install-openvino/install-openvino-archive-linux.html)
+   - [Ceres Solver](http://ceres-solver.org/installation.html)
+
+2. 系统依赖：
+   ```bash
+   sudo apt install -y \
+       git g++ cmake can-utils \
+       libopencv-dev libfmt-dev libeigen3-dev \
+       libspdlog-dev libyaml-cpp-dev libusb-1.0-0-dev \
+       nlohmann-json3-dev openssh-server screen
+   ```
+
+## 编译与运行
+
+```bash
+cmake -B build
+make -C build/ -j$(nproc)
+./build/auto_aim_test    # 运行测试
+```
+
+## 可执行目标
+
+### 主程序（task）
+
+| 目标 | 说明 | 配置文件 |
+|------|------|----------|
+| `standard` | 步兵自瞄 | `configs/standard3.yaml` |
+| `standard_mpc` | 步兵自瞄（MPC 规划） | 需指定 |
+| `uav` | 无人机自瞄 + 打符 | `configs/uav.yaml` |
+| `uav_debug` | 无人机调试（含可视化） | `configs/uav.yaml` |
+| `mt_standard` | 多线程步兵 | 需指定 |
+| `sentry` | 哨兵 | `configs/sentry.yaml` |
+| `sentry_multithread` | 哨兵多线程 | `configs/sentry.yaml` |
+| `sentry_debug` | 哨兵调试 | `configs/sentry.yaml` |
+| `sentry_bp` | 哨兵（带弹道预测） | `configs/sentry.yaml` |
+| `auto_aim_debug_mpc` | 自瞄 MPC 调试 | 需指定 |
+| `auto_buff_debug` | 打符调试 | 需指定 |
+| `auto_buff_debug_mpc` | 打符 MPC 调试 | 需指定 |
+| `mt_auto_aim_debug` | 多线程自瞄调试 | 需指定 |
+
+### 标定工具（calibration）
+
+| 目标 | 说明 |
+|------|------|
+| `capture` | 采集标定图像 |
+| `calibrate_camera` | 相机内参标定 |
+| `calibrate_handeye` | 手眼标定 |
+| `calibrate_robotworld_handeye` | 机器人-世界手眼标定 |
+| `split_video` | 视频拆帧 |
+
+### 测试用例（test）
+
+| 目标 | 说明 |
+|------|------|
+| `auto_aim_test` | 自瞄全流程测试 |
+| `auto_buff_test` | 打符全流程测试 |
+| `camera_test` | 相机基础测试 |
+| `camera_detect_test` | 相机 + 检测测试 |
+| `camera_thread_test` | 相机多线程测试 |
+| `cboard_test` | C 板通信测试 |
+| `gimbal_test` | 云台通信测试 |
+| `gimbal_response_test` | 云台响应测试 |
+| `fire_test` | 发射测试 |
+| `dm_test` | 达妙 IMU 测试 |
+| `handeye_test` | 手眼标定测试 |
+| `detector_video_test` | 离线视频检测测试 |
+| `planner_test` | MPC 规划器测试 |
+| `planner_test_offline` | MPC 规划器离线测试 |
+| `usbcamera_test` | USB 相机测试 |
+| `usbcamera_detect_test` | USB 相机 + 检测测试 |
+| `multi_usbcamera_test` | 多 USB 相机测试 |
+| `minimum_vision_system` | 最小视觉系统 |
+
+## 串口设置
+
+1. 授予权限：
+   ```bash
+   sudo usermod -a -G dialout $USER
+   ```
+
+2. 获取端口 ID（serial, idVendor, idProduct）：
+   ```bash
+   udevadm info -a -n /dev/ttyACM0 | grep -E '({serial}|{idVendor}|{idProduct})'
+   ```
+
+3. 创建 udev 规则：
+   ```bash
+   sudo touch /etc/udev/rules.d/99-usb-serial.rules
+   ```
+   写入（用实际 ID 替换）：
+   ```
+   SUBSYSTEM=="tty", ATTRS{idVendor}=="1234", ATTRS{idProduct}=="1234", ATTRS{serial}=="A1234567", SYMLINK+="gimbal"
+   ```
+
+4. 重新加载规则：
+   ```bash
+   sudo udevadm control --reload-rules
+   sudo udevadm trigger
+   ```
+
+5. 验证：
+   ```bash
+   ls -l /dev/gimbal
+   ```

src/task/auto_aim_debug_mpc.cpp

@@ -0,0 +1,150 @@
+#include <fmt/core.h>
+
+#include <atomic>
+#include <chrono>
+#include <nlohmann/json.hpp>
+#include <opencv2/opencv.hpp>
+#include <thread>
+
+#include "src/device/camera.hpp"
+#include "src/device/gimbal/gimbal.hpp"
+#include "src/module/auto_aim/planner/planner.hpp"
+#include "src/module/auto_aim/solver.hpp"
+#include "src/module/auto_aim/tracker.hpp"
+#include "src/module/auto_aim/yolo.hpp"
+#include "src/component/exiter.hpp"
+#include "src/component/img_tools.hpp"
+#include "src/component/logger.hpp"
+#include "src/component/math_tools.hpp"
+#include "src/component/plotter.hpp"
+#include "src/component/thread_safe_queue.hpp"
+
+using namespace std::chrono_literals;
+
+const std::string keys =
+  "{help h usage ? |                        | 输出命令行参数说明}"
+  "{@config-path   | configs/sentry.yaml | 位置参数，yaml配置文件路径 }";
+
+int main(int argc, char * argv[])
+{
+  component::Exiter exiter;
+  component::Plotter plotter;
+
+  cv::CommandLineParser cli(argc, argv, keys);
+  auto config_path = cli.get<std::string>(0);
+  if (cli.has("help") || config_path.empty()) {
+    cli.printMessage();
+    return 0;
+  }
+
+  device::Gimbal gimbal(config_path);
+  device::Camera camera(config_path);
+
+  auto_aim::YOLO yolo(config_path, true);
+  auto_aim::Solver solver(config_path);
+  auto_aim::Tracker tracker(config_path, solver);
+  auto_aim::Planner planner(config_path);
+
+  component::ThreadSafeQueue<std::optional<auto_aim::Target>, true> target_queue(1);
+  target_queue.push(std::nullopt);
+
+  std::atomic<bool> quit = false;
+  auto plan_thread = std::thread([&]() {
+    auto t0 = std::chrono::steady_clock::now();
+    uint16_t last_bullet_count = 0;
+
+    while (!quit) {
+      auto target = target_queue.front();
+      auto gs = gimbal.state();
+      auto plan = planner.plan(target, gs.bullet_speed);
+
+      gimbal.send(
+        plan.control, plan.fire, plan.yaw, plan.yaw_vel, plan.yaw_acc, plan.pitch, plan.pitch_vel,
+        plan.pitch_acc);
+
+      auto fired = gs.bullet_count > last_bullet_count;
+      last_bullet_count = gs.bullet_count;
+
+      nlohmann::json data;
+      data["t"] = component::delta_time(std::chrono::steady_clock::now(), t0);
+
+      data["gimbal_yaw"] = gs.yaw;
+      data["gimbal_yaw_vel"] = gs.yaw_vel;
+      data["gimbal_pitch"] = gs.pitch;
+      data["gimbal_pitch_vel"] = gs.pitch_vel;
+
+      data["target_yaw"] = plan.target_yaw;
+      data["target_pitch"] = plan.target_pitch;
+
+      data["plan_yaw"] = plan.yaw;
+      data["plan_yaw_vel"] = plan.yaw_vel;
+      data["plan_yaw_acc"] = plan.yaw_acc;
+
+      data["plan_pitch"] = plan.pitch;
+      data["plan_pitch_vel"] = plan.pitch_vel;
+      data["plan_pitch_acc"] = plan.pitch_acc;
+
+      data["fire"] = plan.fire ? 1 : 0;
+      data["fired"] = fired ? 1 : 0;
+
+      if (target.has_value()) {
+        data["target_z"] = target->ekf_x()[4];   //z
+        data["target_vz"] = target->ekf_x()[5];  //vz
+      }
+
+      if (target.has_value()) {
+        data["w"] = target->ekf_x()[7];
+      } else {
+        data["w"] = 0.0;
+      }
+
+      plotter.plot(data);
+
+      std::this_thread::sleep_for(10ms);
+    }
+  });
+
+  cv::Mat img;
+  std::chrono::steady_clock::time_point t;
+
+  while (!exiter.exit()) {
+    camera.read(img, t);
+    auto q = gimbal.q(t);
+
+    solver.set_R_gimbal2world(q);
+    auto armors = yolo.detect(img);
+    auto targets = tracker.track(armors, t);
+    if (!targets.empty())
+      target_queue.push(targets.front());
+    else
+      target_queue.push(std::nullopt);
+
+    if (!targets.empty()) {
+      auto target = targets.front();
+
+      // 当前帧target更新后
+      std::vector<Eigen::Vector4d> armor_xyza_list = target.armor_xyza_list();
+      for (const Eigen::Vector4d & xyza : armor_xyza_list) {
+        auto image_points =
+          solver.reproject_armor(xyza.head(3), xyza[3], target.armor_type, target.name);
+        component::draw_points(img, image_points, {0, 255, 0});
+      }
+
+      Eigen::Vector4d aim_xyza = planner.debug_xyza;
+      auto image_points =
+        solver.reproject_armor(aim_xyza.head(3), aim_xyza[3], target.armor_type, target.name);
+      component::draw_points(img, image_points, {0, 0, 255});
+    }
+
+    cv::resize(img, img, {}, 0.5, 0.5);  // 显示时缩小图片尺寸
+    cv::imshow("reprojection", img);
+    auto key = cv::waitKey(1);
+    if (key == 'q') break;
+  }
+
+  quit = true;
+  if (plan_thread.joinable()) plan_thread.join();
+  gimbal.send(false, false, 0, 0, 0, 0, 0, 0);
+
+  return 0;
+}

src/task/auto_buff_debug.cpp

@@ -0,0 +1,158 @@
+#include <fmt/format.h>
+
+#include <string>
+
+#include "src/device/camera.hpp"
+#include "src/device/cboard.hpp"
+#include "src/module/auto_buff/buff_aimer.hpp"
+#include "src/module/auto_buff/buff_detector.hpp"
+#include "src/module/auto_buff/buff_solver.hpp"
+#include "src/module/auto_buff/buff_target.hpp"
+#include "src/module/auto_buff/buff_type.hpp"
+#include "src/component/exiter.hpp"
+#include "src/component/img_tools.hpp"
+#include "src/component/logger.hpp"
+#include "src/component/math_tools.hpp"
+#include "src/component/plotter.hpp"
+#include "src/component/recorder.hpp"
+#include "src/component/trajectory.hpp"
+
+// 定义命令行参数
+const std::string keys =
+  "{help h usage ? | | 输出命令行参数说明}"
+  "{@config-path   | | yaml配置文件路径 }";
+
+int main(int argc, char * argv[])
+{
+  // 读取命令行参数
+  cv::CommandLineParser cli(argc, argv, keys);
+  auto config_path = cli.get<std::string>(0);
+  if (cli.has("help") || config_path.empty()) {
+    cli.printMessage();
+    return 0;
+  }
+
+  // 初始化绘图器、录制器、退出器
+  component::Plotter plotter;
+  component::Recorder recorder;
+  component::Exiter exiter;
+
+  // 初始化C板、相机
+  device::CBoard cboard(config_path);
+  device::Camera camera(config_path);
+
+  // 初始化识别器、解算器、追踪器、瞄准器
+  auto_buff::Buff_Detector detector(config_path);
+  auto_buff::Solver solver(config_path);
+  auto_buff::SmallTarget target;
+  // auto_buff::BigTarget target;
+  auto_buff::Aimer aimer(config_path);
+
+  cv::Mat img;
+  Eigen::Quaterniond q;
+  std::chrono::steady_clock::time_point t;
+
+  while (!exiter.exit()) {
+    camera.read(img, t);
+    q = cboard.imu_at(t);
+    // recorder.record(img, q, t);
+
+    // -------------- 打符核心逻辑 --------------
+
+    solver.set_R_gimbal2world(q);
+
+    auto power_runes = detector.detect(img);
+
+    solver.solve(power_runes);
+
+    target.get_target(power_runes, t);
+
+    auto target_copy = target;
+
+    auto command = aimer.aim(target_copy, t, cboard.bullet_speed, true);
+
+    cboard.send(command);
+
+    // -------------- 调试输出 --------------
+
+    nlohmann::json data;
+
+    // buff原始观测数据
+    if (power_runes.has_value()) {
+      const auto & p = power_runes.value();
+      data["buff_R_yaw"] = p.ypd_in_world[0];
+      data["buff_R_pitch"] = p.ypd_in_world[1];
+      data["buff_R_dis"] = p.ypd_in_world[2];
+      data["buff_yaw"] = p.ypr_in_world[0] * 57.3;
+      data["buff_pitch"] = p.ypr_in_world[1] * 57.3;
+      data["buff_roll"] = p.ypr_in_world[2] * 57.3;
+    }
+
+    if (!target.is_unsolve()) {
+      auto & p = power_runes.value();
+
+      // 显示
+      for (int i = 0; i < 4; i++) component::draw_point(img, p.target().points[i]);
+      component::draw_point(img, p.target().center, {0, 0, 255}, 3);
+      component::draw_point(img, p.r_center, {0, 0, 255}, 3);
+
+      // 当前帧target更新后buff
+      auto Rxyz_in_world_now = target.point_buff2world(Eigen::Vector3d(0.0, 0.0, 0.0));
+      auto image_points =
+        solver.reproject_buff(Rxyz_in_world_now, target.ekf_x()[4], target.ekf_x()[5]);
+      component::draw_points(
+        img, std::vector<cv::Point2f>(image_points.begin(), image_points.begin() + 4), {0, 255, 0});
+      component::draw_points(
+        img, std::vector<cv::Point2f>(image_points.begin() + 4, image_points.end()), {0, 255, 0});
+
+      // buff瞄准位置(预测)
+      double dangle = target.ekf_x()[5] - target_copy.ekf_x()[5];
+      auto Rxyz_in_world_pre = target.point_buff2world(Eigen::Vector3d(0.0, 0.0, 0.0));
+      image_points =
+        solver.reproject_buff(Rxyz_in_world_pre, target_copy.ekf_x()[4], target_copy.ekf_x()[5]);
+      component::draw_points(
+        img, std::vector<cv::Point2f>(image_points.begin(), image_points.begin() + 4), {255, 0, 0});
+      component::draw_points(
+        img, std::vector<cv::Point2f>(image_points.begin() + 4, image_points.end()), {255, 0, 0});
+
+      // 观测器内部数据
+      Eigen::VectorXd x = target.ekf_x();
+      data["R_yaw"] = x[0];
+      data["R_V_yaw"] = x[1];
+      data["R_pitch"] = x[2];
+      data["R_dis"] = x[3];
+      data["yaw"] = x[4] * 57.3;
+
+      data["angle"] = x[5] * 57.3;
+      data["spd"] = x[6] * 57.3;
+      if (x.size() >= 10) {
+        data["spd"] = x[6];
+        data["a"] = x[7];
+        data["w"] = x[8];
+        data["fi"] = x[9];
+        data["spd0"] = target.spd;
+      }
+    }
+
+    // 云台响应情况
+    Eigen::Vector3d ypr = component::eulers(solver.R_gimbal2world(), 2, 1, 0);
+    data["gimbal_yaw"] = ypr[0] * 57.3;
+    data["gimbal_pitch"] = ypr[1] * 57.3;
+
+    if (command.control) {
+      data["cmd_yaw"] = command.yaw * 57.3;
+      data["cmd_pitch"] = command.pitch * 57.3;
+      data["shoot"] = command.shoot ? 1 : 0;
+    }
+
+    plotter.plot(data);
+
+    cv::resize(img, img, {}, 0.5, 0.5);
+    cv::imshow("result", img);
+
+    auto key = cv::waitKey(1);
+    if (key == 'q') break;
+  }
+
+  return 0;
+}

src/task/auto_buff_debug_mpc.cpp

@@ -0,0 +1,165 @@
+#include <fmt/format.h>
+
+#include <string>
+
+#include "src/device/camera.hpp"
+#include "src/device/gimbal/gimbal.hpp"
+#include "src/module/auto_buff/buff_aimer.hpp"
+#include "src/module/auto_buff/buff_detector.hpp"
+#include "src/module/auto_buff/buff_solver.hpp"
+#include "src/module/auto_buff/buff_target.hpp"
+#include "src/module/auto_buff/buff_type.hpp"
+#include "src/component/exiter.hpp"
+#include "src/component/img_tools.hpp"
+#include "src/component/logger.hpp"
+#include "src/component/math_tools.hpp"
+#include "src/component/plotter.hpp"
+#include "src/component/recorder.hpp"
+#include "src/component/trajectory.hpp"
+
+// 定义命令行参数
+const std::string keys =
+  "{help h usage ? | | 输出命令行参数说明}"
+  "{@config-path   | | yaml配置文件路径 }";
+
+int main(int argc, char * argv[])
+{
+  // 读取命令行参数
+  cv::CommandLineParser cli(argc, argv, keys);
+  auto config_path = cli.get<std::string>(0);
+  if (cli.has("help") || config_path.empty()) {
+    cli.printMessage();
+    return 0;
+  }
+
+  // 初始化绘图器、录制器、退出器
+  component::Plotter plotter;
+  component::Recorder recorder;
+  component::Exiter exiter;
+
+  // 初始化云台、相机
+  device::Gimbal gimbal(config_path);
+  device::Camera camera(config_path);
+
+  // 初始化识别器、解算器、追踪器、瞄准器
+  auto_buff::Buff_Detector detector(config_path);
+  auto_buff::Solver solver(config_path);
+  auto_buff::SmallTarget target;
+  // auto_buff::BigTarget target;
+  auto_buff::Aimer aimer(config_path);
+
+  cv::Mat img;
+  Eigen::Quaterniond q;
+  std::chrono::steady_clock::time_point t;
+
+  while (!exiter.exit()) {
+    camera.read(img, t);
+    q = gimbal.q(t);
+    auto gs = gimbal.state();
+    // recorder.record(img, q, t);
+
+    // -------------- 打符核心逻辑 --------------
+
+    solver.set_R_gimbal2world(q);
+
+    auto power_runes = detector.detect(img);
+
+    solver.solve(power_runes);
+
+    target.get_target(power_runes, t);
+
+    auto target_copy = target;
+
+    auto plan = aimer.mpc_aim(target_copy, t, gs, true);
+
+    gimbal.send(
+      plan.control, plan.fire, plan.yaw, plan.yaw_vel, plan.yaw_acc, plan.pitch, plan.pitch_vel,
+      plan.pitch_acc);
+    // -------------- 调试输出 --------------
+
+    nlohmann::json data;
+
+    // buff原始观测数据
+    if (power_runes.has_value()) {
+      const auto & p = power_runes.value();
+      data["buff_R_yaw"] = p.ypd_in_world[0];
+      data["buff_R_pitch"] = p.ypd_in_world[1];
+      data["buff_R_dis"] = p.ypd_in_world[2];
+      data["buff_yaw"] = p.ypr_in_world[0] * 57.3;
+      data["buff_pitch"] = p.ypr_in_world[1] * 57.3;
+      data["buff_roll"] = p.ypr_in_world[2] * 57.3;
+    }
+
+    if (!target.is_unsolve()) {
+      auto & p = power_runes.value();
+
+      // 显示
+      for (int i = 0; i < 4; i++) component::draw_point(img, p.target().points[i]);
+      component::draw_point(img, p.target().center, {0, 0, 255}, 3);
+      component::draw_point(img, p.r_center, {0, 0, 255}, 3);
+
+      // 当前帧target更新后buff
+      auto Rxyz_in_world_now = target.point_buff2world(Eigen::Vector3d(0.0, 0.0, 0.0));
+      auto image_points =
+        solver.reproject_buff(Rxyz_in_world_now, target.ekf_x()[4], target.ekf_x()[5]);
+      component::draw_points(
+        img, std::vector<cv::Point2f>(image_points.begin(), image_points.begin() + 4), {0, 255, 0});
+      component::draw_points(
+        img, std::vector<cv::Point2f>(image_points.begin() + 4, image_points.end()), {0, 255, 0});
+
+      // buff瞄准位置(预测)
+      double dangle = target.ekf_x()[5] - target_copy.ekf_x()[5];
+      auto Rxyz_in_world_pre = target.point_buff2world(Eigen::Vector3d(0.0, 0.0, 0.0));
+      image_points =
+        solver.reproject_buff(Rxyz_in_world_pre, target_copy.ekf_x()[4], target_copy.ekf_x()[5]);
+      component::draw_points(
+        img, std::vector<cv::Point2f>(image_points.begin(), image_points.begin() + 4), {255, 0, 0});
+      component::draw_points(
+        img, std::vector<cv::Point2f>(image_points.begin() + 4, image_points.end()), {255, 0, 0});
+
+      // 观测器内部数据
+      Eigen::VectorXd x = target.ekf_x();
+      data["R_yaw"] = x[0];
+      data["R_V_yaw"] = x[1];
+      data["R_pitch"] = x[2];
+      data["R_dis"] = x[3];
+      data["yaw"] = x[4] * 57.3;
+
+      data["angle"] = x[5] * 57.3;
+      data["spd"] = x[6] * 57.3;
+      if (x.size() >= 10) {
+        data["spd"] = x[6];
+        data["a"] = x[7];
+        data["w"] = x[8];
+        data["fi"] = x[9];
+        data["spd0"] = target.spd;
+      }
+    }
+
+    // 云台响应情况
+    data["gimbal_yaw"] = gs.yaw * 57.3;
+    data["gimbal_pitch"] = gs.pitch * 57.3;
+    data["gimbal_yaw_vel"] = gs.yaw_vel * 57.3;
+    data["gimbal_pitch_vel"] = gs.pitch_vel * 57.3;
+
+    if (plan.control) {
+      data["plan_yaw"] = plan.yaw * 57.3;
+      data["plan_pitch"] = plan.pitch * 57.3;
+      data["plan_yaw_vel"] = plan.yaw_vel * 57.3;
+      data["plan_pitch_vel"] = plan.pitch_vel * 57.3;
+      data["plan_yaw_acc"] = plan.yaw_acc * 57.3;
+      data["plan_pitch_acc"] = plan.pitch_acc * 57.3;
+      data["shoot"] = plan.fire ? 1 : 0;
+    }
+
+    plotter.plot(data);
+
+    cv::resize(img, img, {}, 0.5, 0.5);
+    cv::imshow("result", img);
+
+    auto key = cv::waitKey(1);
+    if (key == 'q') break;
+  }
+
+  return 0;
+}

src/task/mt_auto_aim_debug.cpp

@@ -0,0 +1,173 @@
+#include <fmt/core.h>
+
+#include <chrono>
+#include <nlohmann/json.hpp>
+#include <opencv2/opencv.hpp>
+
+#include "src/device/camera.hpp"
+#include "src/device/cboard.hpp"
+#include "src/module/auto_aim/aimer.hpp"
+#include "src/module/auto_aim/multithread/commandgener.hpp"
+#include "src/module/auto_aim/multithread/mt_detector.hpp"
+#include "src/module/auto_aim/shooter.hpp"
+#include "src/module/auto_aim/solver.hpp"
+#include "src/module/auto_aim/tracker.hpp"
+#include "src/module/auto_aim/yolo.hpp"
+#include "src/component/exiter.hpp"
+#include "src/component/img_tools.hpp"
+#include "src/component/logger.hpp"
+#include "src/component/math_tools.hpp"
+#include "src/component/plotter.hpp"
+#include "src/component/recorder.hpp"
+
+const std::string keys =
+  "{help h usage ? |                        | 输出命令行参数说明}"
+  "{@config-path   | configs/sentry.yaml | 位置参数，yaml配置文件路径 }";
+
+using namespace std::chrono;
+
+int main(int argc, char * argv[])
+{
+  cv::CommandLineParser cli(argc, argv, keys);
+  auto config_path = cli.get<std::string>(0);
+  if (cli.has("help") || config_path.empty()) {
+    cli.printMessage();
+    return 0;
+  }
+
+  component::Exiter exiter;
+  component::Plotter plotter;
+  component::Recorder recorder(100);  //根据实际帧率调整
+
+  device::CBoard cboard(config_path);
+  device::Camera camera(config_path);
+
+  auto_aim::multithread::MultiThreadDetector detector(config_path, true);
+  auto_aim::Solver solver(config_path);
+  auto_aim::Tracker tracker(config_path, solver);
+  auto_aim::Aimer aimer(config_path);
+  auto_aim::Shooter shooter(config_path);
+  auto_aim::multithread::CommandGener commandgener(shooter, aimer, cboard, plotter, true);
+
+  auto detect_thread = std::thread([&]() {
+    cv::Mat img;
+    std::chrono::steady_clock::time_point t;
+
+    while (!exiter.exit()) {
+      camera.read(img, t);
+      detector.push(img, t);
+    }
+  });
+
+  auto mode = device::Mode::idle;
+  auto last_mode = device::Mode::idle;
+
+  while (!exiter.exit()) {
+    auto t0 = std::chrono::steady_clock::now();
+    /// 自瞄核心逻辑
+    auto [img, armors, t] = detector.debug_pop();
+    Eigen::Quaterniond q = cboard.imu_at(t - 1ms);
+    mode = cboard.mode;
+
+    if (last_mode != mode) {
+      component::logger()->info("Switch to {}", device::MODES[mode]);
+      last_mode = mode;
+    }
+
+    solver.set_R_gimbal2world(q);
+
+    Eigen::Vector3d ypr = component::eulers(solver.R_gimbal2world(), 2, 1, 0);
+
+    auto targets = tracker.track(armors, t);
+
+    commandgener.push(targets, t, cboard.bullet_speed, ypr);  // 发送给决策线程
+
+    /// debug
+    component::draw_text(img, fmt::format("[{}]", tracker.state()), {10, 30}, {255, 255, 255});
+
+    nlohmann::json data;
+    data["t"] = component::delta_time(std::chrono::steady_clock::now(), t0);
+
+    // 装甲板原始观测数据
+    data["armor_num"] = armors.size();
+    if (!armors.empty()) {
+      auto min_x = 1e10;
+      auto & armor = armors.front();
+      for (auto & a : armors) {
+        if (a.center.x < min_x) {
+          min_x = a.center.x;
+          armor = a;
+        }
+      }  //always left
+      solver.solve(armor);
+      data["armor_x"] = armor.xyz_in_world[0];
+      data["armor_y"] = armor.xyz_in_world[1];
+      data["armor_yaw"] = armor.ypr_in_world[0] * 57.3;
+      data["armor_yaw_raw"] = armor.yaw_raw * 57.3;
+    }
+
+    if (!targets.empty()) {
+      auto target = targets.front();
+
+      // 当前帧target更新后
+      std::vector<Eigen::Vector4d> armor_xyza_list = target.armor_xyza_list();
+      for (const Eigen::Vector4d & xyza : armor_xyza_list) {
+        auto image_points =
+          solver.reproject_armor(xyza.head(3), xyza[3], target.armor_type, target.name);
+        component::draw_points(img, image_points, {0, 255, 0});
+      }
+
+      // aimer瞄准位置
+      auto aim_point = aimer.debug_aim_point;
+      Eigen::Vector4d aim_xyza = aim_point.xyza;
+      auto image_points =
+        solver.reproject_armor(aim_xyza.head(3), aim_xyza[3], target.armor_type, target.name);
+      if (aim_point.valid)
+        component::draw_points(img, image_points, {0, 0, 255});
+      else
+        component::draw_points(img, image_points, {255, 0, 0});
+
+      // 观测器内部数据
+      Eigen::VectorXd x = target.ekf_x();
+      data["x"] = x[0];
+      data["vx"] = x[1];
+      data["y"] = x[2];
+      data["vy"] = x[3];
+      data["z"] = x[4];
+      data["vz"] = x[5];
+      data["a"] = x[6] * 57.3;
+      data["w"] = x[7];
+      data["r"] = x[8];
+      data["l"] = x[9];
+      data["h"] = x[10];
+      data["last_id"] = target.last_id;
+
+      // 卡方检验数据
+      data["residual_yaw"] = target.ekf().data.at("residual_yaw");
+      data["residual_pitch"] = target.ekf().data.at("residual_pitch");
+      data["residual_distance"] = target.ekf().data.at("residual_distance");
+      data["residual_angle"] = target.ekf().data.at("residual_angle");
+      data["nis"] = target.ekf().data.at("nis");
+      data["nees"] = target.ekf().data.at("nees");
+      data["nis_fail"] = target.ekf().data.at("nis_fail");
+      data["nees_fail"] = target.ekf().data.at("nees_fail");
+      data["recent_nis_failures"] = target.ekf().data.at("recent_nis_failures");
+    }
+
+    // 云台响应情况
+    data["gimbal_yaw"] = ypr[0] * 57.3;
+    data["gimbal_pitch"] = ypr[1] * 57.3;
+    data["bullet_speed"] = cboard.bullet_speed;
+
+    plotter.plot(data);
+
+    cv::resize(img, img, {}, 0.5, 0.5);  // 显示时缩小图片尺寸
+    cv::imshow("reprojection", img);
+    auto key = cv::waitKey(1);
+    if (key == 'q') break;
+  }
+
+  detect_thread.join();
+
+  return 0;
+}

src/task/mt_standard.cpp

@@ -0,0 +1,138 @@
+#include <chrono>
+#include <opencv2/opencv.hpp>
+#include <thread>
+
+#include "src/device/camera.hpp"
+#include "src/device/dm_imu/dm_imu.hpp"
+#include "src/module/auto_aim/aimer.hpp"
+#include "src/module/auto_aim/multithread/commandgener.hpp"
+#include "src/module/auto_aim/multithread/mt_detector.hpp"
+#include "src/module/auto_aim/shooter.hpp"
+#include "src/module/auto_aim/solver.hpp"
+#include "src/module/auto_aim/tracker.hpp"
+#include "src/module/auto_buff/buff_aimer.hpp"
+#include "src/module/auto_buff/buff_detector.hpp"
+#include "src/module/auto_buff/buff_solver.hpp"
+#include "src/module/auto_buff/buff_target.hpp"
+#include "src/module/auto_buff/buff_type.hpp"
+#include "src/component/exiter.hpp"
+#include "src/component/img_tools.hpp"
+#include "src/component/logger.hpp"
+#include "src/component/math_tools.hpp"
+#include "src/component/plotter.hpp"
+#include "src/component/recorder.hpp"
+
+const std::string keys =
+  "{help h usage ? | | 输出命令行参数说明}"
+  "{@config-path   | | yaml配置文件路径 }";
+
+using namespace std::chrono_literals;
+
+int main(int argc, char * argv[])
+{
+  cv::CommandLineParser cli(argc, argv, keys);
+  auto config_path = cli.get<std::string>("@config-path");
+  if (cli.has("help") || !cli.has("@config-path")) {
+    cli.printMessage();
+    return 0;
+  }
+
+  component::Exiter exiter;
+  component::Plotter plotter;
+  component::Recorder recorder;
+
+  device::Camera camera(config_path);
+  device::CBoard cboard(config_path);
+
+  auto_aim::multithread::MultiThreadDetector detector(config_path);
+  auto_aim::Solver solver(config_path);
+  auto_aim::Tracker tracker(config_path, solver);
+  auto_aim::Aimer aimer(config_path);
+  auto_aim::Shooter shooter(config_path);
+
+  auto_buff::Buff_Detector buff_detector(config_path);
+  auto_buff::Solver buff_solver(config_path);
+  auto_buff::SmallTarget buff_small_target;
+  auto_buff::BigTarget buff_big_target;
+  auto_buff::Aimer buff_aimer(config_path);
+
+  auto_aim::multithread::CommandGener commandgener(shooter, aimer, cboard, plotter);
+
+  std::atomic<device::Mode> mode{device::Mode::idle};
+  auto last_mode{device::Mode::idle};
+
+  auto detect_thread = std::thread([&]() {
+    cv::Mat img;
+    std::chrono::steady_clock::time_point t;
+
+    while (!exiter.exit()) {
+      if (mode.load() == device::Mode::auto_aim) {
+        camera.read(img, t);
+        detector.push(img, t);
+      } else
+        continue;
+    }
+  });
+
+  while (!exiter.exit()) {
+    mode = cboard.mode;
+
+    if (last_mode != mode) {
+      component::logger()->info("Switch to {}", device::MODES[mode]);
+      last_mode = mode.load();
+    }
+
+    /// 自瞄
+    if (mode.load() == device::Mode::auto_aim) {
+      auto [img, armors, t] = detector.debug_pop();
+      Eigen::Quaterniond q = cboard.imu_at(t - 1ms);
+
+      // recorder.record(img, q, t);
+
+      solver.set_R_gimbal2world(q);
+
+      Eigen::Vector3d ypr = component::eulers(solver.R_gimbal2world(), 2, 1, 0);
+
+      auto targets = tracker.track(armors, t);
+
+      commandgener.push(targets, t, cboard.bullet_speed, ypr);  // 发送给决策线程
+
+    }
+
+    /// 打符
+    else if (mode.load() == device::Mode::small_buff || mode.load() == device::Mode::big_buff) {
+      cv::Mat img;
+      Eigen::Quaterniond q;
+      std::chrono::steady_clock::time_point t;
+
+      camera.read(img, t);
+      q = cboard.imu_at(t - 1ms);
+
+      // recorder.record(img, q, t);
+
+      buff_solver.set_R_gimbal2world(q);
+
+      auto power_runes = buff_detector.detect(img);
+
+      buff_solver.solve(power_runes);
+
+      device::Command buff_command;
+      if (mode.load() == device::Mode::small_buff) {
+        buff_small_target.get_target(power_runes, t);
+        auto target_copy = buff_small_target;
+        buff_command = buff_aimer.aim(target_copy, t, cboard.bullet_speed, true);
+      } else if (mode.load() == device::Mode::big_buff) {
+        buff_big_target.get_target(power_runes, t);
+        auto target_copy = buff_big_target;
+        buff_command = buff_aimer.aim(target_copy, t, cboard.bullet_speed, true);
+      }
+      cboard.send(buff_command);
+
+    } else
+      continue;
+  }
+
+  detect_thread.join();
+
+  return 0;
+}

src/task/sentry.cpp

@@ -0,0 +1,106 @@
+#include <fmt/core.h>
+
+#include <chrono>
+#include <nlohmann/json.hpp>
+#include <opencv2/opencv.hpp>
+#include <thread>
+
+#include "src/device/camera.hpp"
+#include "src/device/cboard.hpp"
+#include "src/device/ros2/publish2nav.hpp"
+#include "src/device/ros2/ros2.hpp"
+#include "src/device/usbcamera/usbcamera.hpp"
+#include "src/module/auto_aim/aimer.hpp"
+#include "src/module/auto_aim/shooter.hpp"
+#include "src/module/auto_aim/solver.hpp"
+#include "src/module/auto_aim/tracker.hpp"
+#include "src/module/auto_aim/yolo.hpp"
+#include "src/module/omniperception/decider.hpp"
+#include "src/component/exiter.hpp"
+#include "src/component/img_tools.hpp"
+#include "src/component/logger.hpp"
+#include "src/component/math_tools.hpp"
+#include "src/component/plotter.hpp"
+#include "src/component/recorder.hpp"
+
+using namespace std::chrono;
+
+const std::string keys =
+  "{help h usage ? |                        | 输出命令行参数说明}"
+  "{@config-path   | configs/sentry.yaml | 位置参数，yaml配置文件路径 }";
+
+int main(int argc, char * argv[])
+{
+  component::Exiter exiter;
+  component::Plotter plotter;
+  component::Recorder recorder;
+
+  cv::CommandLineParser cli(argc, argv, keys);
+  if (cli.has("help")) {
+    cli.printMessage();
+    return 0;
+  }
+  auto config_path = cli.get<std::string>(0);
+
+  device::ROS2 ros2;
+  device::CBoard cboard(config_path);
+  device::Camera camera(config_path);
+  device::Camera back_camera("configs/camera.yaml");
+  device::USBCamera usbcam1("video0", config_path);
+  device::USBCamera usbcam2("video2", config_path);
+
+  auto_aim::YOLO yolo(config_path, false);
+  auto_aim::Solver solver(config_path);
+  auto_aim::Tracker tracker(config_path, solver);
+  auto_aim::Aimer aimer(config_path);
+  auto_aim::Shooter shooter(config_path);
+
+  omniperception::Decider decider(config_path);
+
+  cv::Mat img;
+
+  std::chrono::steady_clock::time_point timestamp;
+  device::Command last_command;
+
+  while (!exiter.exit()) {
+    camera.read(img, timestamp);
+    Eigen::Quaterniond q = cboard.imu_at(timestamp - 1ms);
+    // recorder.record(img, q, timestamp);
+
+    /// 自瞄核心逻辑
+    solver.set_R_gimbal2world(q);
+
+    Eigen::Vector3d gimbal_pos = component::eulers(solver.R_gimbal2world(), 2, 1, 0);
+
+    auto armors = yolo.detect(img);
+
+    decider.get_invincible_armor(ros2.subscribe_enemy_status());
+
+    decider.armor_filter(armors);
+
+    // decider.get_auto_aim_target(armors, ros2.subscribe_autoaim_target());
+
+    decider.set_priority(armors);
+
+    auto targets = tracker.track(armors, timestamp);
+
+    device::Command command{false, false, 0, 0};
+
+    /// 全向感知逻辑
+    if (tracker.state() == "lost")
+      command = decider.decide(yolo, gimbal_pos, usbcam1, usbcam2, back_camera);
+    else
+      command = aimer.aim(targets, timestamp, cboard.bullet_speed, cboard.shoot_mode);
+
+    /// 发射逻辑
+    command.shoot = shooter.shoot(command, aimer, targets, gimbal_pos);
+
+    cboard.send(command);
+
+    /// ROS2通信
+    Eigen::Vector4d target_info = decider.get_target_info(armors, targets);
+
+    ros2.publish(target_info);
+  }
+  return 0;
+}

src/task/sentry_bp.cpp

@@ -0,0 +1,104 @@
+#include <fmt/core.h>
+
+#include <chrono>
+#include <nlohmann/json.hpp>
+#include <opencv2/opencv.hpp>
+#include <thread>
+
+#include "src/device/camera.hpp"
+#include "src/device/cboard.hpp"
+#include "src/device/ros2/publish2nav.hpp"
+#include "src/device/ros2/ros2.hpp"
+#include "src/device/usbcamera/usbcamera.hpp"
+#include "src/module/auto_aim/aimer.hpp"
+#include "src/module/auto_aim/shooter.hpp"
+#include "src/module/auto_aim/solver.hpp"
+#include "src/module/auto_aim/tracker.hpp"
+#include "src/module/auto_aim/yolo.hpp"
+#include "src/module/omniperception/decider.hpp"
+#include "src/component/exiter.hpp"
+#include "src/component/img_tools.hpp"
+#include "src/component/logger.hpp"
+#include "src/component/math_tools.hpp"
+#include "src/component/plotter.hpp"
+#include "src/component/recorder.hpp"
+
+using namespace std::chrono;
+
+const std::string keys =
+  "{help h usage ? |                        | 输出命令行参数说明}"
+  "{@config-path   | configs/sentry.yaml | 位置参数，yaml配置文件路径 }";
+
+int main(int argc, char * argv[])
+{
+  component::Exiter exiter;
+  component::Plotter plotter;
+  component::Recorder recorder;
+
+  cv::CommandLineParser cli(argc, argv, keys);
+  if (cli.has("help")) {
+    cli.printMessage();
+    return 0;
+  }
+  auto config_path = cli.get<std::string>(0);
+
+  device::ROS2 ros2;
+  device::CBoard cboard(config_path);
+  device::Camera camera(config_path);
+  device::Camera back_camera("configs/camera.yaml");
+
+  auto_aim::YOLO yolo(config_path, false);
+  auto_aim::Solver solver(config_path);
+  auto_aim::Tracker tracker(config_path, solver);
+  auto_aim::Aimer aimer(config_path);
+  auto_aim::Shooter shooter(config_path);
+
+  omniperception::Decider decider(config_path);
+
+  cv::Mat img;
+
+  std::chrono::steady_clock::time_point timestamp;
+  device::Command last_command;
+
+  while (!exiter.exit()) {
+    camera.read(img, timestamp);
+    Eigen::Quaterniond q = cboard.imu_at(timestamp - 1ms);
+    // recorder.record(img, q, timestamp);
+
+    /// 自瞄核心逻辑
+    solver.set_R_gimbal2world(q);
+
+    Eigen::Vector3d gimbal_pos = component::eulers(solver.R_gimbal2world(), 2, 1, 0);
+
+    auto armors = yolo.detect(img);
+
+    decider.get_invincible_armor(ros2.subscribe_enemy_status());
+
+    decider.armor_filter(armors);
+
+    // decider.get_auto_aim_target(armors, ros2.subscribe_autoaim_target());
+
+    decider.set_priority(armors);
+
+    auto targets = tracker.track(armors, timestamp);
+
+    device::Command command{false, false, 0, 0};
+
+    /// 全向感知逻辑
+    if (tracker.state() == "lost")
+      command = decider.decide(yolo, gimbal_pos, back_camera);
+    else
+      command = aimer.aim(targets, timestamp, cboard.bullet_speed, cboard.shoot_mode);
+
+    /// 发射逻辑
+    command.shoot = shooter.shoot(command, aimer, targets, gimbal_pos);
+
+    cboard.send(command);
+
+    /// ROS2通信
+    Eigen::Vector4d target_info = decider.get_target_info(armors, targets);
+
+    ros2.publish(target_info);
+  }
+  return 0;
+}

src/task/sentry_debug.cpp

@@ -0,0 +1,196 @@
+#include <fmt/core.h>
+
+#include <chrono>
+#include <nlohmann/json.hpp>
+#include <opencv2/opencv.hpp>
+#include <thread>
+
+#include "src/device/camera.hpp"
+#include "src/device/cboard.hpp"
+#include "src/device/ros2/publish2nav.hpp"
+#include "src/device/ros2/ros2.hpp"
+#include "src/device/usbcamera/usbcamera.hpp"
+#include "src/module/auto_aim/aimer.hpp"
+#include "src/module/auto_aim/shooter.hpp"
+#include "src/module/auto_aim/solver.hpp"
+#include "src/module/auto_aim/tracker.hpp"
+#include "src/module/auto_aim/yolo.hpp"
+#include "src/module/omniperception/decider.hpp"
+#include "src/component/exiter.hpp"
+#include "src/component/img_tools.hpp"
+#include "src/component/logger.hpp"
+#include "src/component/math_tools.hpp"
+#include "src/component/plotter.hpp"
+#include "src/component/recorder.hpp"
+
+using namespace std::chrono;
+
+const std::string keys =
+  "{help h usage ? |                        | 输出命令行参数说明}"
+  "{@config-path   | configs/sentry.yaml | 位置参数，yaml配置文件路径 }";
+
+int main(int argc, char * argv[])
+{
+  component::Exiter exiter;
+  component::Plotter plotter;
+  component::Recorder recorder;
+
+  cv::CommandLineParser cli(argc, argv, keys);
+  if (cli.has("help")) {
+    cli.printMessage();
+    return 0;
+  }
+  auto config_path = cli.get<std::string>(0);
+
+  device::ROS2 ros2;
+  device::CBoard cboard(config_path);
+  device::Camera camera(config_path);
+  device::Camera back_camera("configs/camera.yaml");
+  device::USBCamera usbcam1("video0", config_path);
+  device::USBCamera usbcam2("video2", config_path);
+
+  auto_aim::YOLO yolo(config_path, false);
+  auto_aim::Solver solver(config_path);
+  auto_aim::Tracker tracker(config_path, solver);
+  auto_aim::Aimer aimer(config_path);
+  auto_aim::Shooter shooter(config_path);
+
+  omniperception::Decider decider(config_path);
+
+  cv::Mat img;
+
+  std::chrono::steady_clock::time_point timestamp;
+  device::Command last_command;
+
+  while (!exiter.exit()) {
+    camera.read(img, timestamp);
+    Eigen::Quaterniond q = cboard.imu_at(timestamp - 1ms);
+    // recorder.record(img, q, timestamp);
+
+    /// 自瞄核心逻辑
+    solver.set_R_gimbal2world(q);
+
+    Eigen::Vector3d gimbal_pos = component::eulers(solver.R_gimbal2world(), 2, 1, 0);
+
+    auto armors = yolo.detect(img);
+
+    decider.get_invincible_armor(ros2.subscribe_enemy_status());
+
+    decider.armor_filter(armors);
+
+    decider.get_auto_aim_target(armors, ros2.subscribe_autoaim_target());
+
+    decider.set_priority(armors);
+
+    auto targets = tracker.track(armors, timestamp);
+
+    device::Command command{false, false, 0, 0};
+
+    /// 全向感知逻辑
+    if (tracker.state() == "lost")
+      command = decider.decide(yolo, gimbal_pos, usbcam1, usbcam2, back_camera);
+    else
+      command = aimer.aim(targets, timestamp, cboard.bullet_speed, cboard.shoot_mode);
+
+    /// 发射逻辑
+    command.shoot = shooter.shoot(command, aimer, targets, gimbal_pos);
+
+    cboard.send(command);
+
+    /// ROS2通信
+    Eigen::Vector4d target_info = decider.get_target_info(armors, targets);
+
+    ros2.publish(target_info);
+
+    /// debug
+    component::draw_text(img, fmt::format("[{}]", tracker.state()), {10, 30}, {255, 255, 255});
+
+    nlohmann::json data;
+
+    // 装甲板原始观测数据
+    data["armor_num"] = armors.size();
+    if (!armors.empty()) {
+      auto min_x = 1e10;
+      auto & armor = armors.front();
+      for (auto & a : armors) {
+        if (a.center.x < min_x) {
+          min_x = a.center.x;
+          armor = a;
+        }
+      }  //always left
+      solver.solve(armor);
+      data["armor_x"] = armor.xyz_in_world[0];
+      data["armor_y"] = armor.xyz_in_world[1];
+      data["armor_yaw"] = armor.ypr_in_world[0] * 57.3;
+      data["armor_yaw_raw"] = armor.yaw_raw * 57.3;
+    }
+
+    if (!targets.empty()) {
+      auto target = targets.front();
+
+      // 当前帧target更新后
+      std::vector<Eigen::Vector4d> armor_xyza_list = target.armor_xyza_list();
+      for (const Eigen::Vector4d & xyza : armor_xyza_list) {
+        auto image_points =
+          solver.reproject_armor(xyza.head(3), xyza[3], target.armor_type, target.name);
+        component::draw_points(img, image_points, {0, 255, 0});
+      }
+
+      // aimer瞄准位置
+      auto aim_point = aimer.debug_aim_point;
+      Eigen::Vector4d aim_xyza = aim_point.xyza;
+      auto image_points =
+        solver.reproject_armor(aim_xyza.head(3), aim_xyza[3], target.armor_type, target.name);
+      if (aim_point.valid)
+        component::draw_points(img, image_points, {0, 0, 255});
+      else
+        component::draw_points(img, image_points, {255, 0, 0});
+
+      // 观测器内部数据
+      Eigen::VectorXd x = target.ekf_x();
+      data["x"] = x[0];
+      data["vx"] = x[1];
+      data["y"] = x[2];
+      data["vy"] = x[3];
+      data["z"] = x[4];
+      data["vz"] = x[5];
+      data["a"] = x[6] * 57.3;
+      data["w"] = x[7];
+      data["r"] = x[8];
+      data["l"] = x[9];
+      data["h"] = x[10];
+      data["last_id"] = target.last_id;
+
+      // 卡方检验数据
+      data["residual_yaw"] = target.ekf().data.at("residual_yaw");
+      data["residual_pitch"] = target.ekf().data.at("residual_pitch");
+      data["residual_distance"] = target.ekf().data.at("residual_distance");
+      data["residual_angle"] = target.ekf().data.at("residual_angle");
+      data["nis"] = target.ekf().data.at("nis");
+      data["nees"] = target.ekf().data.at("nees");
+      data["nis_fail"] = target.ekf().data.at("nis_fail");
+      data["nees_fail"] = target.ekf().data.at("nees_fail");
+      data["recent_nis_failures"] = target.ekf().data.at("recent_nis_failures");
+    }
+
+    // 云台响应情况
+    data["gimbal_yaw"] = gimbal_pos[0] * 57.3;
+    data["gimbal_pitch"] = -gimbal_pos[1] * 57.3;
+    data["shootmode"] = cboard.shoot_mode;
+    if (command.control) {
+      data["cmd_yaw"] = command.yaw * 57.3;
+      data["cmd_pitch"] = command.pitch * 57.3;
+      data["cmd_shoot"] = command.shoot;
+    }
+
+    data["bullet_speed"] = cboard.bullet_speed;
+
+    plotter.plot(data);
+
+    cv::resize(img, img, {}, 0.5, 0.5);  // 显示时缩小图片尺寸
+    cv::imshow("reprojection", img);
+    auto key = cv::waitKey(1);
+    if (key == 'q') break;
+  }
+  return 0;
+}

src/task/sentry_multithread.cpp

@@ -0,0 +1,125 @@
+#include <fmt/core.h>
+
+#include <chrono>
+#include <future>
+#include <memory>
+#include <mutex>
+#include <nlohmann/json.hpp>
+#include <opencv2/opencv.hpp>
+#include <thread>
+
+#include "src/device/camera.hpp"
+#include "src/device/cboard.hpp"
+#include "src/device/ros2/ros2.hpp"
+#include "src/device/usbcamera/usbcamera.hpp"
+#include "src/module/auto_aim/aimer.hpp"
+#include "src/module/auto_aim/shooter.hpp"
+#include "src/module/auto_aim/solver.hpp"
+#include "src/module/auto_aim/tracker.hpp"
+#include "src/module/auto_aim/yolo.hpp"
+#include "src/module/omniperception/decider.hpp"
+#include "src/module/omniperception/perceptron.hpp"
+#include "src/component/exiter.hpp"
+#include "src/component/img_tools.hpp"
+#include "src/component/logger.hpp"
+#include "src/component/math_tools.hpp"
+#include "src/component/plotter.hpp"
+#include "src/component/recorder.hpp"
+
+using namespace std::chrono;
+
+const std::string keys =
+  "{help h usage ? |                     | 输出命令行参数说明}"
+  "{@config-path   | configs/sentry.yaml | 位置参数，yaml配置文件路径 }";
+
+int main(int argc, char * argv[])
+{
+  component::Exiter exiter;
+  component::Plotter plotter;
+  component::Recorder recorder;
+
+  cv::CommandLineParser cli(argc, argv, keys);
+  if (cli.has("help")) {
+    cli.printMessage();
+    return 0;
+  }
+  auto config_path = cli.get<std::string>(0);
+
+  device::ROS2 ros2;
+  device::CBoard cboard(config_path);
+  device::Camera camera(config_path);
+  device::USBCamera usbcam1("video0", config_path);
+  device::USBCamera usbcam2("video2", config_path);
+  device::USBCamera usbcam3("video4", config_path);
+  device::USBCamera usbcam4("video6", config_path);
+
+  auto_aim::YOLO yolo(config_path, false);
+  auto_aim::Solver solver(config_path);
+  auto_aim::Tracker tracker(config_path, solver);
+  auto_aim::Aimer aimer(config_path);
+  auto_aim::Shooter shooter(config_path);
+
+  omniperception::Decider decider(config_path);
+  omniperception::Perceptron perceptron(&usbcam1, &usbcam2, &usbcam3, &usbcam4, config_path);
+
+  omniperception::DetectionResult switch_target;
+  cv::Mat img;
+  std::chrono::steady_clock::time_point timestamp;
+  device::Command last_command;
+
+  while (!exiter.exit()) {
+    camera.read(img, timestamp);
+    Eigen::Quaterniond q = cboard.imu_at(timestamp - 1ms);
+    recorder.record(img, q, timestamp);
+    /// 自瞄核心逻辑
+    solver.set_R_gimbal2world(q);
+
+    Eigen::Vector3d gimbal_pos = component::eulers(solver.R_gimbal2world(), 2, 1, 0);
+
+    auto armors = yolo.detect(img);
+
+    decider.get_invincible_armor(ros2.subscribe_enemy_status());
+
+    decider.armor_filter(armors);
+
+    decider.set_priority(armors);
+
+    auto detection_queue = perceptron.get_detection_queue();
+
+    decider.sort(detection_queue);
+
+    auto [switch_target, targets] = tracker.track(detection_queue, armors, timestamp);
+
+    device::Command command{false, false, 0, 0};
+
+    /// 全向感知逻辑
+    if (tracker.state() == "switching") {
+      command.control = switch_target.armors.empty() ? false : true;
+      command.shoot = false;
+      command.pitch = component::limit_rad(switch_target.delta_pitch);
+      command.yaw = component::limit_rad(switch_target.delta_yaw + gimbal_pos[0]);
+    }
+
+    else if (tracker.state() == "lost") {
+      command = decider.decide(detection_queue);
+      command.yaw = component::limit_rad(command.yaw + gimbal_pos[0]);
+    }
+
+    else {
+      command = aimer.aim(targets, timestamp, cboard.bullet_speed);
+    }
+
+    /// 发射逻辑
+    command.shoot = shooter.shoot(command, aimer, targets, gimbal_pos);
+    // command.shoot = false;
+
+    cboard.send(command);
+
+    /// ROS2通信
+    Eigen::Vector4d target_info = decider.get_target_info(armors, targets);
+
+    ros2.publish(target_info);
+  }
+
+  return 0;
+}

src/task/standard.cpp

@@ -0,0 +1,83 @@
+#include <fmt/core.h>
+
+#include <chrono>
+#include <nlohmann/json.hpp>
+#include <opencv2/opencv.hpp>
+
+#include "src/device/camera.hpp"
+#include "src/device/cboard.hpp"
+#include "src/module/auto_aim/aimer.hpp"
+#include "src/module/auto_aim/multithread/commandgener.hpp"
+#include "src/module/auto_aim/shooter.hpp"
+#include "src/module/auto_aim/solver.hpp"
+#include "src/module/auto_aim/tracker.hpp"
+#include "src/module/auto_aim/yolo.hpp"
+#include "src/component/exiter.hpp"
+#include "src/component/img_tools.hpp"
+#include "src/component/logger.hpp"
+#include "src/component/math_tools.hpp"
+#include "src/component/plotter.hpp"
+#include "src/component/recorder.hpp"
+
+using namespace std::chrono;
+
+const std::string keys =
+  "{help h usage ? |      | 输出命令行参数说明}"
+  "{@config-path   | configs/standard3.yaml | 位置参数，yaml配置文件路径 }";
+
+int main(int argc, char * argv[])
+{
+  cv::CommandLineParser cli(argc, argv, keys);
+  auto config_path = cli.get<std::string>(0);
+  if (cli.has("help") || config_path.empty()) {
+    cli.printMessage();
+    return 0;
+  }
+
+  component::Exiter exiter;
+  component::Plotter plotter;
+  component::Recorder recorder;
+
+  device::CBoard cboard(config_path);
+  device::Camera camera(config_path);
+
+  auto_aim::YOLO detector(config_path, false);
+  auto_aim::Solver solver(config_path);
+  auto_aim::Tracker tracker(config_path, solver);
+  auto_aim::Aimer aimer(config_path);
+  auto_aim::Shooter shooter(config_path);
+
+  cv::Mat img;
+  Eigen::Quaterniond q;
+  std::chrono::steady_clock::time_point t;
+
+  auto mode = device::Mode::idle;
+  auto last_mode = device::Mode::idle;
+
+  while (!exiter.exit()) {
+    camera.read(img, t);
+    q = cboard.imu_at(t - 1ms);
+    mode = cboard.mode;
+
+    if (last_mode != mode) {
+      component::logger()->info("Switch to {}", device::MODES[mode]);
+      last_mode = mode;
+    }
+
+    // recorder.record(img, q, t);
+
+    solver.set_R_gimbal2world(q);
+
+    Eigen::Vector3d ypr = component::eulers(solver.R_gimbal2world(), 2, 1, 0);
+
+    auto armors = detector.detect(img);
+
+    auto targets = tracker.track(armors, t);
+
+    auto command = aimer.aim(targets, t, cboard.bullet_speed);
+
+    cboard.send(command);
+  }
+
+  return 0;
+}

src/task/standard_mpc.cpp

@@ -0,0 +1,145 @@
+#include <chrono>
+#include <opencv2/opencv.hpp>
+#include <thread>
+
+#include "src/device/camera.hpp"
+#include "src/device/dm_imu/dm_imu.hpp"
+#include "src/module/auto_aim/aimer.hpp"
+#include "src/module/auto_aim/multithread/commandgener.hpp"
+#include "src/module/auto_aim/multithread/mt_detector.hpp"
+#include "src/module/auto_aim/shooter.hpp"
+#include "src/module/auto_aim/solver.hpp"
+#include "src/module/auto_aim/tracker.hpp"
+#include "src/module/auto_buff/buff_aimer.hpp"
+#include "src/module/auto_buff/buff_detector.hpp"
+#include "src/module/auto_buff/buff_solver.hpp"
+#include "src/module/auto_buff/buff_target.hpp"
+#include "src/module/auto_buff/buff_type.hpp"
+#include "src/component/exiter.hpp"
+#include "src/component/img_tools.hpp"
+#include "src/component/logger.hpp"
+#include "src/component/math_tools.hpp"
+#include "src/component/plotter.hpp"
+#include "src/component/recorder.hpp"
+
+const std::string keys =
+  "{help h usage ? | | 输出命令行参数说明}"
+  "{@config-path   | | yaml配置文件路径 }";
+
+using namespace std::chrono_literals;
+
+int main(int argc, char * argv[])
+{
+  cv::CommandLineParser cli(argc, argv, keys);
+  auto config_path = cli.get<std::string>("@config-path");
+  if (cli.has("help") || !cli.has("@config-path")) {
+    cli.printMessage();
+    return 0;
+  }
+
+  component::Exiter exiter;
+  component::Plotter plotter;
+  component::Recorder recorder;
+
+  device::Gimbal gimbal(config_path);
+  device::Camera camera(config_path);
+
+  auto_aim::YOLO yolo(config_path, true);
+  auto_aim::Solver solver(config_path);
+  auto_aim::Tracker tracker(config_path, solver);
+  auto_aim::Planner planner(config_path);
+
+  component::ThreadSafeQueue<std::optional<auto_aim::Target>, true> target_queue(1);
+  target_queue.push(std::nullopt);
+
+  auto_buff::Buff_Detector buff_detector(config_path);
+  auto_buff::Solver buff_solver(config_path);
+  auto_buff::SmallTarget buff_small_target;
+  auto_buff::BigTarget buff_big_target;
+  auto_buff::Aimer buff_aimer(config_path);
+
+  cv::Mat img;
+  Eigen::Quaterniond q;
+  std::chrono::steady_clock::time_point t;
+
+  std::atomic<bool> quit = false;
+
+  std::atomic<device::GimbalMode> mode{device::GimbalMode::IDLE};
+  auto last_mode{device::GimbalMode::IDLE};
+
+  auto plan_thread = std::thread([&]() {
+    auto t0 = std::chrono::steady_clock::now();
+    uint16_t last_bullet_count = 0;
+
+    while (!quit) {
+      if (!target_queue.empty() && mode == device::GimbalMode::AUTO_AIM) {
+        auto target = target_queue.front();
+        auto gs = gimbal.state();
+        auto plan = planner.plan(target, gs.bullet_speed);
+
+        gimbal.send(
+          plan.control, plan.fire, plan.yaw, plan.yaw_vel, plan.yaw_acc, plan.pitch, plan.pitch_vel,
+          plan.pitch_acc);
+
+        std::this_thread::sleep_for(10ms);
+      } else
+        std::this_thread::sleep_for(200ms);
+    }
+  });
+
+  while (!exiter.exit()) {
+    mode = gimbal.mode();
+
+    if (last_mode != mode) {
+      component::logger()->info("Switch to {}", gimbal.str(mode));
+      last_mode = mode.load();
+    }
+
+    camera.read(img, t);
+    auto q = gimbal.q(t);
+    auto gs = gimbal.state();
+    recorder.record(img, q, t);
+    solver.set_R_gimbal2world(q);
+
+    /// 自瞄
+    if (mode.load() == device::GimbalMode::AUTO_AIM) {
+      auto armors = yolo.detect(img);
+      auto targets = tracker.track(armors, t);
+      if (!targets.empty())
+        target_queue.push(targets.front());
+      else
+        target_queue.push(std::nullopt);
+    }
+
+    /// 打符
+    else if (mode.load() == device::GimbalMode::SMALL_BUFF || mode.load() == device::GimbalMode::BIG_BUFF) {
+      buff_solver.set_R_gimbal2world(q);
+
+      auto power_runes = buff_detector.detect(img);
+
+      buff_solver.solve(power_runes);
+
+      auto_aim::Plan buff_plan;
+      if (mode.load() == device::GimbalMode::SMALL_BUFF) {
+        buff_small_target.get_target(power_runes, t);
+        auto target_copy = buff_small_target;
+        buff_plan = buff_aimer.mpc_aim(target_copy, t, gs, true);
+      } else if (mode.load() == device::GimbalMode::BIG_BUFF) {
+        buff_big_target.get_target(power_runes, t);
+        auto target_copy = buff_big_target;
+        buff_plan = buff_aimer.mpc_aim(target_copy, t, gs, true);
+      }
+      gimbal.send(
+        buff_plan.control, buff_plan.fire, buff_plan.yaw, buff_plan.yaw_vel, buff_plan.yaw_acc,
+        buff_plan.pitch, buff_plan.pitch_vel, buff_plan.pitch_acc);
+
+    } else
+      gimbal.send(false, false, 0, 0, 0, 0, 0, 0);
+  }
+
+  quit = true;
+  if (plan_thread.joinable()) plan_thread.join();
+  gimbal.send(false, false, 0, 0, 0, 0, 0, 0);
+
+  return 0;
+}

src/task/uav.cpp

@@ -0,0 +1,120 @@
+#include <chrono>
+#include <opencv2/opencv.hpp>
+#include <thread>
+
+#include "src/device/camera.hpp"
+#include "src/device/dm_imu/dm_imu.hpp"
+#include "src/module/auto_aim/aimer.hpp"
+#include "src/module/auto_aim/detector.hpp"
+#include "src/module/auto_aim/shooter.hpp"
+#include "src/module/auto_aim/solver.hpp"
+#include "src/module/auto_aim/tracker.hpp"
+#include "src/module/auto_aim/yolo.hpp"
+#include "src/module/auto_buff/buff_aimer.hpp"
+#include "src/module/auto_buff/buff_detector.hpp"
+#include "src/module/auto_buff/buff_solver.hpp"
+#include "src/module/auto_buff/buff_target.hpp"
+#include "src/module/auto_buff/buff_type.hpp"
+#include "src/component/exiter.hpp"
+#include "src/component/img_tools.hpp"
+#include "src/component/logger.hpp"
+#include "src/component/math_tools.hpp"
+#include "src/component/plotter.hpp"
+#include "src/component/recorder.hpp"
+
+const std::string keys =
+  "{help h usage ? |                  | 输出命令行参数说明}"
+  "{@config-path   | configs/uav.yaml | yaml配置文件路径 }";
+
+using namespace std::chrono_literals;
+
+int main(int argc, char * argv[])
+{
+  cv::CommandLineParser cli(argc, argv, keys);
+  auto config_path = cli.get<std::string>("@config-path");
+  if (cli.has("help") || !cli.has("@config-path")) {
+    cli.printMessage();
+    return 0;
+  }
+
+  component::Exiter exiter;
+  component::Plotter plotter;
+  component::Recorder recorder;
+
+  device::Camera camera(config_path);
+  device::CBoard cboard(config_path);
+
+  auto_aim::Detector detector(config_path);
+  auto_aim::Solver solver(config_path);
+  // auto_aim::YOLO yolo(config_path);
+  auto_aim::Tracker tracker(config_path, solver);
+  auto_aim::Aimer aimer(config_path);
+  auto_aim::Shooter shooter(config_path);
+
+  auto_buff::Buff_Detector buff_detector(config_path);
+  auto_buff::Solver buff_solver(config_path);
+  auto_buff::SmallTarget buff_small_target;
+  auto_buff::BigTarget buff_big_target;
+  auto_buff::Aimer buff_aimer(config_path);
+
+  cv::Mat img;
+  Eigen::Quaterniond q;
+  std::chrono::steady_clock::time_point t;
+
+  auto mode = device::Mode::idle;
+  auto last_mode = device::Mode::idle;
+
+  while (!exiter.exit()) {
+    camera.read(img, t);
+    q = cboard.imu_at(t - 1ms);
+    mode = cboard.mode;
+    // recorder.record(img, q, t);
+    if (last_mode != mode) {
+      component::logger()->info("Switch to {}", device::MODES[mode]);
+      last_mode = mode;
+    }
+
+    /// 自瞄
+    if (mode == device::Mode::auto_aim || mode == device::Mode::outpost) {
+      solver.set_R_gimbal2world(q);
+
+      Eigen::Vector3d ypr = component::eulers(solver.R_gimbal2world(), 2, 1, 0);
+
+      auto armors = detector.detect(img);
+
+      auto targets = tracker.track(armors, t);
+
+      auto command = aimer.aim(targets, t, cboard.bullet_speed);
+
+      command.shoot = shooter.shoot(command, aimer, targets, ypr);
+
+      cboard.send(command);
+    }
+
+    /// 打符
+    else if (mode == device::Mode::small_buff || mode == device::Mode::big_buff) {
+      buff_solver.set_R_gimbal2world(q);
+
+      auto power_runes = buff_detector.detect(img);
+
+      buff_solver.solve(power_runes);
+
+      device::Command buff_command;
+      if (mode == device::Mode::small_buff) {
+        buff_small_target.get_target(power_runes, t);
+        auto target_copy = buff_small_target;
+        buff_command = buff_aimer.aim(target_copy, t, cboard.bullet_speed, true);
+      } else if (mode == device::Mode::big_buff) {
+        buff_big_target.get_target(power_runes, t);
+        auto target_copy = buff_big_target;
+        buff_command = buff_aimer.aim(target_copy, t, cboard.bullet_speed, true);
+      }
+      cboard.send(buff_command);
+    }
+
+    else
+      continue;
+  }
+
+  return 0;
+}

src/task/uav_debug.cpp

@@ -0,0 +1,173 @@
+#include <chrono>
+#include <opencv2/opencv.hpp>
+#include <thread>
+
+#include "src/device/camera.hpp"
+#include "src/device/dm_imu/dm_imu.hpp"
+#include "src/module/auto_aim/aimer.hpp"
+#include "src/module/auto_aim/detector.hpp"
+#include "src/module/auto_aim/shooter.hpp"
+#include "src/module/auto_aim/solver.hpp"
+#include "src/module/auto_aim/tracker.hpp"
+#include "src/module/auto_aim/yolo.hpp"
+#include "src/component/exiter.hpp"
+#include "src/component/img_tools.hpp"
+#include "src/component/logger.hpp"
+#include "src/component/math_tools.hpp"
+#include "src/component/plotter.hpp"
+#include "src/component/recorder.hpp"
+
+const std::string keys =
+  "{help h usage ? |                  | 输出命令行参数说明}"
+  "{@config-path   | configs/uav.yaml | yaml配置文件路径 }";
+
+using namespace std::chrono_literals;
+
+int main(int argc, char * argv[])
+{
+  cv::CommandLineParser cli(argc, argv, keys);
+  auto config_path = cli.get<std::string>("@config-path");
+  if (cli.has("help") || !cli.has("@config-path")) {
+    cli.printMessage();
+    return 0;
+  }
+
+  component::Exiter exiter;
+  component::Plotter plotter;
+  component::Recorder recorder;
+
+  device::Camera camera(config_path);
+  device::CBoard cboard(config_path);
+
+  auto_aim::Detector detector(config_path);
+  auto_aim::Solver solver(config_path);
+  auto_aim::YOLO yolo(config_path);
+  auto_aim::Tracker tracker(config_path, solver);
+  auto_aim::Aimer aimer(config_path);
+  auto_aim::Shooter shooter(config_path);
+
+  cv::Mat img;
+  Eigen::Quaterniond q;
+  std::chrono::steady_clock::time_point t;
+
+  auto mode = device::Mode::idle;
+  auto last_mode = device::Mode::idle;
+
+  auto t0 = std::chrono::steady_clock::now();
+
+  while (!exiter.exit()) {
+    camera.read(img, t);
+    q = cboard.imu_at(t - 1ms);
+    mode = cboard.mode;
+    // recorder.record(img, q, t);
+    if (last_mode != mode) {
+      component::logger()->info("Switch to {}", device::MODES[mode]);
+      last_mode = mode;
+    }
+
+    /// 自瞄
+    solver.set_R_gimbal2world(q);
+
+    Eigen::Vector3d ypr = component::eulers(solver.R_gimbal2world(), 2, 1, 0);
+
+    auto armors = detector.detect(img);
+
+    auto targets = tracker.track(armors, t);
+
+    auto command = aimer.aim(targets, t, cboard.bullet_speed);
+
+    command.shoot = shooter.shoot(command, aimer, targets, ypr);
+
+    cboard.send(command);
+
+    /// debug
+    component::draw_text(img, fmt::format("[{}]", tracker.state()), {10, 30}, {255, 255, 255});
+
+    nlohmann::json data;
+    data["t"] = component::delta_time(std::chrono::steady_clock::now(), t0);
+
+    // 装甲板原始观测数据
+    data["armor_num"] = armors.size();
+    if (!armors.empty()) {
+      auto min_x = 1e10;
+      auto & armor = armors.front();
+      for (auto & a : armors) {
+        if (a.center.x < min_x) {
+          min_x = a.center.x;
+          armor = a;
+        }
+      }  //always left
+      solver.solve(armor);
+      data["armor_x"] = armor.xyz_in_world[0];
+      data["armor_y"] = armor.xyz_in_world[1];
+      data["armor_yaw"] = armor.ypr_in_world[0] * 57.3;
+      data["armor_yaw_raw"] = armor.yaw_raw * 57.3;
+    }
+
+    if (!targets.empty()) {
+      auto target = targets.front();
+
+      // 当前帧target更新后
+      std::vector<Eigen::Vector4d> armor_xyza_list = target.armor_xyza_list();
+      for (const Eigen::Vector4d & xyza : armor_xyza_list) {
+        auto image_points =
+          solver.reproject_armor(xyza.head(3), xyza[3], target.armor_type, target.name);
+        component::draw_points(img, image_points, {0, 255, 0});
+      }
+
+      // aimer瞄准位置
+      auto aim_point = aimer.debug_aim_point;
+      Eigen::Vector4d aim_xyza = aim_point.xyza;
+      auto image_points =
+        solver.reproject_armor(aim_xyza.head(3), aim_xyza[3], target.armor_type, target.name);
+      if (aim_point.valid)
+        component::draw_points(img, image_points, {0, 0, 255});
+      else
+        component::draw_points(img, image_points, {255, 0, 0});
+
+      // 观测器内部数据
+      Eigen::VectorXd x = target.ekf_x();
+      data["x"] = x[0];
+      data["vx"] = x[1];
+      data["y"] = x[2];
+      data["vy"] = x[3];
+      data["z"] = x[4];
+      data["vz"] = x[5];
+      data["a"] = x[6] * 57.3;
+      data["w"] = x[7];
+      data["r"] = x[8];
+      data["l"] = x[9];
+      data["h"] = x[10];
+      data["last_id"] = target.last_id;
+
+      // 卡方检验数据
+      data["residual_yaw"] = target.ekf().data.at("residual_yaw");
+      data["residual_pitch"] = target.ekf().data.at("residual_pitch");
+      data["residual_distance"] = target.ekf().data.at("residual_distance");
+      data["residual_angle"] = target.ekf().data.at("residual_angle");
+      data["nis"] = target.ekf().data.at("nis");
+      data["nees"] = target.ekf().data.at("nees");
+      data["nis_fail"] = target.ekf().data.at("nis_fail");
+      data["nees_fail"] = target.ekf().data.at("nees_fail");
+      data["recent_nis_failures"] = target.ekf().data.at("recent_nis_failures");
+    }
+
+    // 云台响应情况
+    data["gimbal_yaw"] = ypr[0] * 57.3;
+    data["gimbal_pitch"] = ypr[1] * 57.3;
+    data["bullet_speed"] = cboard.bullet_speed;
+    if (command.control) {
+      data["cmd_yaw"] = command.yaw * 57.3;
+      data["cmd_pitch"] = command.pitch * 57.3;
+      data["cmd_shoot"] = command.shoot;
+    }
+    plotter.plot(data);
+
+    cv::resize(img, img, {}, 0.5, 0.5);  // 显示时缩小图片尺寸
+    cv::imshow("reprojection", img);
+    auto key = cv::waitKey(1);
+    if (key == 'q') break;
+  }
+
+  return 0;
+}