fix hero

build ros

.gitignore

@@ -25,8 +25,8 @@
 
 # Shared objects (inc. Windows DLLs)
 *.dll
-*.so
-*.so.*
+# *.so
+# *.so.*
 *.dylib
 
 # Executables
@@ -67,7 +67,7 @@ dkms.conf
 *.pch
 
 # Compiled Dynamic libraries
-*.so
+# *.so
 *.dylib
 *.dll
 
@@ -91,7 +91,7 @@ devel/
 logs/
 build/
 bin/
-lib/
+# lib/
 msg_gen/
 srv_gen/
 msg/*Action.msg
@@ -170,3 +170,6 @@ qtcreator-*
 COLCON_IGNORE
 AMENT_IGNORE
 
+
+
+.claude/

CMakeLists.txt

@@ -0,0 +1,153 @@
+cmake_minimum_required(VERSION 3.16.3)
+
+project(mr_vision)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_BUILD_TYPE Release)
+message(STATUS "--------------------CMAKE_BUILD_TYPE: ${CMAKE_BUILD_TYPE}--------------------")
+
+find_package(OpenCV REQUIRED)
+find_package(fmt REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(spdlog REQUIRED)
+find_package(yaml-cpp REQUIRED)
+find_package(nlohmann_json REQUIRED)
+set(OpenVINO_DIR "/opt/intel/openvino_2024.6.0/runtime/cmake/")
+find_package(OpenVINO REQUIRED)
+
+# 尝试查找ROS2
+find_package(ament_cmake QUIET)
+find_package(rclcpp QUIET)
+
+if(ament_cmake_FOUND AND rclcpp_FOUND)
+  message(STATUS "ROS2 found, enabling ROS2 support")
+  set(USE_ROS2 ON)
+  add_definitions(-DUSE_ROS2)
+
+  # 查找rm_msgs包
+  find_package(rm_msgs REQUIRED)
+else()
+  message(STATUS "ROS2 not found, building without ROS2 support")
+  set(USE_ROS2 OFF)
+endif()
+
+include_directories(${EIGEN3_INCLUDE_DIR})
+include_directories(${OpenCV_INCLUDE_DIRS})
+include_directories(${PROJECT_SOURCE_DIR})
+
+
+add_subdirectory(src/component)
+add_subdirectory(src/device)
+add_subdirectory(src/module/auto_aim)
+add_subdirectory(src/module/auto_buff)
+add_subdirectory(src/module/omniperception)
+
+
+
+##################calibration################
+add_executable(capture calibration/capture.cpp)
+add_executable(calibrate_camera calibration/calibrate_camera.cpp)
+add_executable(calibrate_handeye calibration/calibrate_handeye.cpp)
+add_executable(calibrate_robotworld_handeye calibration/calibrate_robotworld_handeye.cpp)
+add_executable(split_video calibration/split_video.cpp)
+
+target_link_libraries(capture ${OpenCV_LIBS} fmt::fmt yaml-cpp component device)
+target_link_libraries(calibrate_camera ${OpenCV_LIBS} fmt::fmt yaml-cpp component)
+target_link_libraries(calibrate_handeye ${OpenCV_LIBS} fmt::fmt yaml-cpp component)
+target_link_libraries(calibrate_robotworld_handeye ${OpenCV_LIBS} fmt::fmt yaml-cpp component)
+target_link_libraries(split_video ${OpenCV_LIBS} fmt::fmt component)
+
+# ROS2版本的capture
+if(USE_ROS2)
+  add_executable(capture_ros calibration/capture_ros.cpp)
+  target_link_libraries(capture_ros ${OpenCV_LIBS} fmt::fmt yaml-cpp component device rclcpp::rclcpp ${rm_msgs_TARGETS})
+  ament_target_dependencies(capture_ros rclcpp rm_msgs)
+endif()
+
+##################tests##################
+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)
+target_link_libraries(camera_detect_test ${OpenCV_LIBS} fmt::fmt yaml-cpp component auto_aim device)
+target_link_libraries(camera_test ${OpenCV_LIBS} fmt::fmt component device)
+target_link_libraries(camera_thread_test ${OpenCV_LIBS} fmt::fmt auto_aim component device)
+target_link_libraries(cboard_test ${OpenCV_LIBS} fmt::fmt component device)
+target_link_libraries(fire_test ${OpenCV_LIBS} fmt::fmt component device)
+target_link_libraries(detector_video_test ${OpenCV_LIBS} fmt::fmt yaml-cpp component auto_aim)
+target_link_libraries(gimbal_response_test ${OpenCV_LIBS} fmt::fmt yaml-cpp component device)
+target_link_libraries(multi_usbcamera_test ${OpenCV_LIBS} fmt::fmt component device)
+target_link_libraries(usbcamera_detect_test ${OpenCV_LIBS} fmt::fmt yaml-cpp component device auto_aim)
+target_link_libraries(usbcamera_test ${OpenCV_LIBS} fmt::fmt yaml-cpp component device)
+target_link_libraries(handeye_test ${OpenCV_LIBS} fmt::fmt yaml-cpp component device auto_aim)
+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/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/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/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)
+
+add_executable(balance_infantry src/task/balance_infantry.cpp)
+target_link_libraries(balance_infantry ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann_json::nlohmann_json auto_aim auto_buff component device)
+
+add_executable(balance_infantry_mpc src/task/balance_infantry_mpc.cpp)
+target_link_libraries(balance_infantry_mpc ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann_json::nlohmann_json auto_aim auto_buff component device)
+
+################## ROS2 tasks ##################
+if(USE_ROS2)
+  add_executable(sentry_mpc src/task/sentry_mpc.cpp)
+  target_link_libraries(sentry_mpc ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann_json::nlohmann_json auto_aim auto_buff component device rclcpp::rclcpp ${rm_msgs_TARGETS})
+  ament_target_dependencies(sentry_mpc rclcpp rm_msgs)
+
+  add_executable(auto_aim_debug_mpc_ros src/task/auto_aim_debug_mpc_ros.cpp)
+  target_link_libraries(auto_aim_debug_mpc_ros ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann_json::nlohmann_json auto_aim component device rclcpp::rclcpp ${rm_msgs_TARGETS})
+  ament_target_dependencies(auto_aim_debug_mpc_ros rclcpp rm_msgs)
+endif()
+

README.md

@@ -1,11 +1,346 @@
 # MOVE_AI
 
-青岛理工大学QUT
-MOVE战队
-RoboMaster比赛
-所有上位机代码仓库。具体请查看分支
-包含步兵/英雄/无人机，
-哨兵，
-雷达，
-等兵种
-支持导航，自瞄，决策等功能。
+适用于 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）
+- ROS2 Humble（可选，用于哨兵ROS2通信）
+
+## 依赖安装
+
+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
+   ```
+
+3. ROS2 依赖（可选，用于哨兵）：
+   ```bash
+   # 安装 ROS2 Humble
+   sudo apt install ros-humble-desktop
+
+   # 编译 rm_msgs 包
+   cd ~/rm_msgs
+   source /opt/ros/humble/setup.bash
+   colcon build
+   ```
+
+## 编译与运行
+
+### 标准编译（不含ROS2）
+
+```bash
+cmake -B build
+make -C build/ -j$(nproc)
+./build/auto_aim_test    # 运行测试
+```
+
+### ROS2编译（哨兵专用）
+
+```bash
+# 设置ROS2环境
+source /opt/ros/humble/setup.bash
+source ~/rm_msgs/install/setup.bash
+
+# 编译
+cmake -B build
+make -C build/ -j$(nproc)
+
+# 运行ROS2版本程序
+./build/sentry_mpc configs/sentry.yaml
+./build/auto_aim_debug_mpc_ros configs/standard3.yaml
+./build/capture_ros configs/calibration.yaml -o assets/img_with_q
+```
+
+**注意**：CMake会自动检测ROS2环境，如果检测到ROS2和rm_msgs包，会自动启用ROS2支持并编译相关程序。
+
+## 可执行目标
+
+### 主程序（task）
+
+| 目标 | 说明 | 配置文件 |
+|------|------|----------|
+| `standard` | 步兵自瞄 | `configs/standard3.yaml` |
+| `standard_mpc` | 步兵自瞄（MPC 规划） | 需指定 |
+| `sentry_mpc` | 哨兵自瞄（ROS2通信） | 需指定 |
+| `uav` | 无人机自瞄 + 打符 | `configs/uav.yaml` |
+| `uav_debug` | 无人机调试（含可视化） | `configs/uav.yaml` |
+| `mt_standard` | 多线程步兵 | 需指定 |
+| `balance_infantry` | 平衡步兵 | 需指定 |
+| `balance_infantry_mpc` | 平衡步兵（MPC规划） | 需指定 |
+| `auto_aim_debug_mpc` | 自瞄 MPC 调试 | 需指定 |
+| `auto_aim_debug_mpc_ros` | 自瞄 MPC 调试（ROS2） | 需指定 |
+| `auto_buff_debug` | 打符调试 | 需指定 |
+| `auto_buff_debug_mpc` | 打符 MPC 调试 | 需指定 |
+| `mt_auto_aim_debug` | 多线程自瞄调试 | 需指定 |
+
+**注意**：`sentry_mpc` 和 `auto_aim_debug_mpc_ros` 需要ROS2环境，只在检测到ROS2时才会编译。
+
+### 标定工具（calibration）
+
+| 目标 | 说明 | 通信方式 |
+|------|------|----------|
+| `capture` | 采集标定图像 | 串口 |
+| `capture_ros` | 采集标定图像（ROS2） | ROS2话题 |
+| `calibrate_camera` | 相机内参标定 | - |
+| `calibrate_handeye` | 手眼标定 | - |
+| `calibrate_robotworld_handeye` | 机器人-世界手眼标定 | - |
+| `split_video` | 视频拆帧 | - |
+
+**注意**：`capture_ros` 需要ROS2环境，只在检测到ROS2时才会编译。
+
+### 测试用例（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
+   ```
+
+## 通信协议
+
+### 1. CBoard 协议（CAN 总线）
+
+通过 SocketCAN 与 RoboMaster C 型开发板通信，CAN ID 由 yaml 配置。
+
+**发送帧 — 控制命令（CAN ID: 0xff）**
+
+```
+8 bytes, Big-Endian
+[0]   : control    (uint8)   0=不控制, 1=控制
+[1]   : shoot      (uint8)   0=不射击, 1=射击
+[2-3] : yaw        (int16)   缩放 1e4, 单位 rad
+[4-5] : pitch      (int16)   缩放 1e4, 单位 rad
+[6-7] : horizon_distance (int16) 缩放 1e4（无人机专有）
+```
+
+**接收帧1 — 四元数（CAN ID: 0x100 / 0x01）**
+
+```
+8 bytes, Big-Endian
+[0-1] : x (int16) 缩放 1e4
+[2-3] : y (int16) 缩放 1e4
+[4-5] : z (int16) 缩放 1e4
+[6-7] : w (int16) 缩放 1e4
+四元数顺序: wxyz，验证 x²+y²+z²+w² ≈ 1
+```
+
+**接收帧2 — 子弹速度和模式（CAN ID: 0x101 / 0x110）**
+
+```
+8 bytes, Big-Endian
+[0-1] : bullet_speed (int16) 缩放 1e2, 单位 m/s
+[2]   : mode (uint8)  0=idle, 1=auto_aim, 2=small_buff, 3=big_buff, 4=outpost
+[3]   : shoot_mode (uint8) 0=left, 1=right, 2=both（哨兵专有）
+[4-5] : ft_angle (int16) 缩放 1e4, 单位 rad（无人机专有）
+```
+
+### 2. Gimbal 协议（串口）
+
+通过 USB 串口与达妙 MC02 通信，帧头 `{'S', 'P'}`，CRC16 校验。
+
+**发送帧 — VisionToGimbal（29 bytes, packed）**
+
+```
+[0-1]   : head       (uint8[2])  = {'S', 'P'}
+[2]     : mode       (uint8)     0=不控制, 1=控制不开火, 2=控制且开火
+[3-6]   : yaw        (float)     rad
+[7-10]  : yaw_vel    (float)     rad/s
+[11-14] : yaw_acc    (float)     rad/s²
+[15-18] : pitch      (float)     rad
+[19-22] : pitch_vel  (float)     rad/s
+[23-26] : pitch_acc  (float)     rad/s²
+[27-28] : crc16      (uint16)    Little-Endian
+```
+
+**接收帧 — GimbalToVision（43 bytes, packed）**
+
+```
+[0-1]   : head         (uint8[2])  = {'S', 'P'}
+[2]     : mode         (uint8)     0=IDLE, 1=AUTO_AIM, 2=SMALL_BUFF, 3=BIG_BUFF
+[3-6]   : q[0]         (float)     四元数 w
+[7-10]  : q[1]         (float)     四元数 x
+[11-14] : q[2]         (float)     四元数 y
+[15-18] : q[3]         (float)     四元数 z
+[19-22] : yaw          (float)     rad
+[23-26] : yaw_vel      (float)     rad/s
+[27-30] : pitch        (float)     rad
+[31-34] : pitch_vel    (float)     rad/s
+[35-38] : bullet_speed (float)     m/s
+[39-40] : bullet_count (uint16)    子弹累计发射次数
+[41-42] : crc16        (uint16)    Little-Endian
+```
+
+### 3. DM IMU 协议（串口）
+
+达妙 IMU，串口 921600 bps，Modbus RTU 格式，57 bytes 三帧合一。
+
+```
+帧1 [0-18]  : 加速度 (accx, accy, accz)  float, CRC16
+帧2 [19-37] : 角速度 (gyrox, gyroy, gyroz) float, CRC16
+帧3 [38-56] : 欧拉角 (roll, pitch, yaw)    float, 单位°, CRC16
+
+每帧结构: 帧头(0x55 0xAA) + slave_id(0x01) + reg + 3×float(uint32) + crc16 + 帧尾
+四元数由 ZYX 欧拉角生成: q = Rz(yaw) * Ry(pitch) * Rx(roll)
+```
+
+### 4. ROS2 通信（哨兵专有）
+
+**rm_msgs 自定义消息**
+
+| 方向 | 话题 | 消息类型 | 内容 |
+|------|------|----------|------|
+| 发布 | `data_aim` | `rm_msgs/DataAim` | 视觉控制指令 |
+| 订阅 | `data_mcu` | `rm_msgs/DataMCU` | MCU状态数据 |
+
+**DataAim 消息定义（视觉 → MCU）**
+
+```
+uint8 mode          # 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火
+float32 yaw         # 目标偏航角 (rad)
+float32 yaw_vel     # 偏航角速度 (rad/s)
+float32 yaw_acc     # 偏航角加速度 (rad/s²)
+float32 pitch       # 目标俯仰角 (rad)
+float32 pitch_vel   # 俯仰角速度 (rad/s)
+float32 pitch_acc   # 俯仰角加速度 (rad/s²)
+```
+
+**DataMCU 消息定义（MCU → 视觉）**
+
+```
+uint8 mode              # 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符
+float32 q0              # 四元数 w
+float32 q1              # 四元数 x
+float32 q2              # 四元数 y
+float32 q3              # 四元数 z
+float32 yaw             # 偏航角 (rad)
+float32 yaw_vel         # 偏航角速度 (rad/s)
+float32 pitch           # 俯仰角 (rad)
+float32 pitch_vel       # 俯仰角速度 (rad/s)
+float32 bullet_speed    # 弹速 (m/s)
+uint16 bullet_count     # 子弹累计发送次数
+```
+
+**ROS2 程序使用说明**
+
+1. 编译 rm_msgs 包：
+   ```bash
+   cd ~/rm_msgs
+   source /opt/ros/humble/setup.bash
+   colcon build
+   ```
+
+2. 编译视觉程序：
+   ```bash
+   cd /home/robofish/MOVE_AI
+   source /opt/ros/humble/setup.bash
+   source ~/rm_msgs/install/setup.bash
+   cmake -B build
+   make -C build/ sentry_mpc capture_ros -j$(nproc)
+   ```
+
+3. 运行程序：
+   ```bash
+   # 运行哨兵自瞄
+   source /opt/ros/humble/setup.bash
+   source ~/rm_msgs/install/setup.bash
+   ./build/sentry_mpc configs/sentry.yaml
+
+   # 运行标定采集
+   ./build/capture_ros configs/calibration.yaml -o assets/img_with_q
+   ```
+
+**ROS2 兼容性说明**
+
+- 项目支持条件编译，自动检测ROS2环境
+- 如果未安装ROS2，只编译串口/CAN版本的程序
+- 如果安装了ROS2和rm_msgs，会额外编译ROS2版本：
+  - `sentry_mpc`: 使用ROS2通信的哨兵自瞄程序
+  - `capture_ros`: 使用ROS2通信的标定采集程序
+- ROS2版本与串口版本功能完全相同，只是通信方式不同
+
+### 5. 协议总览
+
+| 协议 | 接口 | 速率 | 帧长 | 校验 | 适用设备 |
+|------|------|------|------|------|----------|
+| CBoard | CAN | 1Mbps | 8B | — | C 板 (STM32F407) |
+| Gimbal | 串口 | 可配置 | 29/43B | CRC16 | 达妙 MC02 (STM32H7) |
+| DM IMU | 串口 | 921600 | 57B | CRC16 | 达妙 IMU |
+| ROS2 | DDS | — | 可变 | DDS | 哨兵导航系统 |

assets/test_demo/demo.txt

@@ -0,0 +1,620 @@
+0.752677887 0.5014435722929029 0.02327065116579723 0.0019472255628188646 0.8648751522116124
+25.824433869 0.4044806118882999 -0.019203333238947136 0.007353485765453223 0.9143153683746531
+25.897933452 0.3983196747926323 -0.0436570165419713 0.018327136925283163 0.9160238084416079
+25.937844162 0.4060992801021506 -0.060038428432863424 0.024035605927579317 0.911537740008345
+25.971199735 0.41094740236919536 -0.05302595072734453 0.022424116638938174 0.9098393484722648
+26.004610259 0.40968966344416635 -0.06222787384895668 0.026676178451042067 0.9097089935174283
+26.038064416 0.4052178756282984 -0.06004722598623817 0.025247798677895384 0.9118965690167756
+26.077887404 0.40238001786745814 -0.05878320483250864 0.02484137457076296 0.9132457293412537
+26.111296109 0.40071966107416385 -0.060104792050292295 0.025001993365346215 0.91388515007574
+26.14465991 0.40002642058153615 -0.05785093696523309 0.02402514483049273 0.9143603908442461
+26.178099861 0.39929477459454044 -0.057247371909738026 0.02370218810986068 0.9147265316310269
+26.218006534 0.39592077215767296 -0.05910142357447336 0.024000578101308977 0.916066447456922
+26.258174622 0.3914227776785776 -0.06137418865489941 0.024358413852926662 0.9178388125133461
+26.297970237 0.3855702178436289 -0.06842624749883933 0.02654238434945672 0.9197548355938607
+26.331391866 0.37864028600252797 -0.07377439499650759 0.027719254342318013 0.9221824740242731
+26.364852852 0.36916508135424514 -0.08236062699653302 0.030084141809927266 0.9252182522199894
+26.398257869 0.35781284168321736 -0.09010595648555406 0.031602689105865805 0.9288983566427934
+26.438065898 0.3423988381374929 -0.094062894882146 0.031306837908086714 0.9343099535746594
+26.471630757 0.3284589912585138 -0.100292321321941 0.03340331127965669 0.9385842317772286
+26.511411364 0.30648004432504633 -0.10372794354937309 0.02920252880446784 0.9457577430129557
+26.544782875 0.2877371342097568 -0.10816547583833148 0.03136957635674919 0.9510644148072316
+26.578234048 0.27056379547459847 -0.11476815782525768 0.028758032907769022 0.9554038298390122
+26.618249467 0.24702723299386736 -0.11930861794076049 0.029647629255281208 0.9611784526943998
+26.65819223 0.22474004532143046 -0.12747277314000643 0.02684990433599021 0.9656716247120788
+26.691595323 0.20478718291793538 -0.1354562932082495 0.026679200858173283 0.9690211672530101
+26.731533578 0.18464779929792222 -0.14656507828977644 0.02557516004343661 0.9714781414057131
+26.764973543 0.16962745445697788 -0.15571367025164942 0.024602979225598506 0.9728178005178726
+26.804876967 0.15668079501547813 -0.165082270044186 0.021902446350445876 0.9735087341335167
+26.838271933 0.14948108929914852 -0.17336450566714282 0.023865026107513124 0.973154978739571
+26.878294766 0.14670414846347568 -0.17732779299413426 0.022900647578824766 0.9728865848576116
+26.918471623 0.16793725762402759 -0.1556833711631501 0.024272873455547277 0.9731241406209225
+26.958307164 0.18634764447388283 -0.14828721348713966 0.02716398992035998 0.9708488941987118
+26.991752292 0.19135093840176348 -0.1409333380258021 0.02560205317865002 0.9710134641078398
+27.031645647 0.19548121140962427 -0.13017912492072262 0.024075762752732745 0.9717308521749091
+27.065002019 0.2030810496528304 -0.11954184165675422 0.022601603668929358 0.9715744968229708
+27.098378842 0.21533477553100352 -0.10884752288991607 0.02217154034270757 0.9702018212756423
+27.138566887 0.2309530705457854 -0.10363395684749488 0.023384977600967825 0.9671472612876528
+27.178444982 0.2610574165126007 -0.07534989163557612 0.01975464577855268 0.9621752299266685
+27.211783938 0.28189139816589925 -0.06455159511978166 0.01934054677327865 0.9570769428097602
+27.251917112 0.30344756555078894 -0.05920441836657583 0.018158406704229697 0.9508336784491486
+27.285291209 0.3197638072317074 -0.052305763630237506 0.0190128093844623 0.9458613681481295
+27.325135927 0.34050741054630956 -0.04356369533756827 0.01739493407681777 0.9390709899044084
+27.358476022 0.36074398179533307 -0.043177686734943606 0.017975353795504252 0.9314914672838361
+27.391868412 0.3787823021316463 -0.03624523535416737 0.01568693148279716 0.9246427259685901
+27.42522498 0.3984670274873839 -0.04007924788396457 0.017989448735969783 0.9161299370878566
+27.465429627 0.4195406147954505 -0.03692008972117208 0.01825845120939563 0.9068016367825271
+27.505361927 0.4428245058408616 -0.03847256797862454 0.01988862267960126 0.8955617015191278
+27.538757471 0.46230490758180315 -0.03792234735216942 0.02220157131743785 0.8856315025043535
+27.578597084 0.4838649748930099 -0.04076880357753215 0.02443333342290202 0.8738510187351668
+27.612003127 0.5029620477890874 -0.04332278759451557 0.027400941729415852 0.8627870553915313
+27.645609807 0.5164711925243503 -0.04852694340232439 0.03269867674780466 0.8543028968670572
+27.685514463 0.5346476087719997 -0.052552062589177925 0.03330828442595869 0.8427815691747212
+27.725445945 0.5483503022201482 -0.05677974866673668 0.04052650103918396 0.833334032012335
+27.758780308 0.5604878399036811 -0.058961518326285545 0.04247728364422499 0.824968242449958
+27.798875869 0.571417869922125 -0.06320457473969693 0.046910106558344385 0.8168759034078068
+27.83230702 0.5793812677133586 -0.06757245519538539 0.048605247049575986 0.8107951898482002
+27.87214098 0.5939104023850323 -0.06534585789781365 0.0508035152165781 0.800262054352252
+27.912176896 0.5854053622748845 -0.0647026264982299 0.04940118779555409 0.806643449479651
+27.945738619 0.5484893523181781 -0.06296952524452691 0.04326309904847135 0.8326599387173533
+27.985561072 0.5023457335699482 -0.06007586355282575 0.036143248490428076 0.861819772441438
+28.025565824 0.48265743343532164 -0.05297786338571968 0.02931557823091408 0.873713765951813
+28.058924678 0.47321784897275215 -0.04940209718149981 0.027201154723416898 0.87913844040025
+28.098993169 0.454826580318603 -0.04068404964729429 0.020584409203952665 0.8894120934851888
+28.138865086 0.43209180787629975 -0.03808694033648821 0.018506465746383264 0.9008349267582678
+28.172422693 0.4146567620523134 -0.028721650974283558 0.012880152825570333 0.9094333060278761
+28.212409768 0.4165125396419987 -0.04340323269539244 0.021428768549504835 0.9078404439057637
+28.245832927 0.4084996986948947 -0.04176063927302493 0.019259226597509577 0.9115992142189211
+28.285655953 0.3837377360348871 -0.04162778955717806 0.01723239944012399 0.9223424101107672
+28.319103435 0.34663020129947947 -0.05575000855977064 0.020189405315549035 0.9361259680222915
+28.359132216 0.3215898402058671 -0.06694038194132015 0.02121937972090072 0.9442715170257839
+28.39893612 0.3191246191429412 -0.0856146732825442 0.02549929462302405 0.9434931855341215
+28.432387406 0.311282249380152 -0.10399925402143416 0.03281288350827445 0.9440396342631439
+28.472413896 0.3027637644227617 -0.1442911954492042 0.042430382176946085 0.9411236988497933
+28.512438655 0.2792311437803485 -0.19292151186743026 0.0530538305086665 0.9391467136024387
+28.552487544 0.25399677541556676 -0.2227387876177815 0.054100275930032804 0.9396521860312725
+28.586039756 0.2565817998246983 -0.24347813752778957 0.06245154897650981 0.9332652252040423
+28.625817307 0.29769257489780704 -0.2354296157143852 0.072967502762912 0.9222948392113202
+28.65918369 0.31398004396575024 -0.23344648938751542 0.07823336000621821 0.9169508219997666
+28.692535692 0.31150059834170446 -0.23627797301834827 0.07734464473301211 0.9171466091254031
+28.725869117 0.30290876772937103 -0.22655739560738158 0.07226151040671558 0.922873934533864
+28.765799969 0.30123134447545497 -0.187413394701377 0.05712519180985434 0.9332055556267885
+28.799252042 0.3041879950337558 -0.1657474708904943 0.05170527857763309 0.9366557551939549
+28.83281661 0.31099686973283364 -0.12893354694262157 0.03941615610368397 0.9407993697527565
+28.872692948 0.315227209620526 -0.12035272852321002 0.037414039040739464 0.9406100237263759
+28.912720524 0.3154260116678618 -0.10679714618271541 0.032446845303020014 0.9423629889593582
+28.952760607 0.31613276095486886 -0.1045108304959943 0.03200331651551979 0.9423976610179468
+28.992661348 0.3156462155986657 -0.10589643353602368 0.03231755011857671 0.9423953458591169
+29.032576927 0.314815368087299 -0.10450510154105064 0.031801552430673785 0.9428460261521774
+29.066041179 0.3148639043329818 -0.10515409891927241 0.03220150564769405 0.9427440799404091
+29.105976656 0.3148248942489635 -0.10505843638464234 0.032002530546273285 0.9427745483116193
+29.139375245 0.31452325985513124 -0.10510777300723144 0.032002366662525275 0.942869727794651
+29.172731973 0.3148245305089792 -0.10500818203126688 0.03220250915625518 0.9427734590559551
+29.206294979 0.31525741283153325 -0.10510685476651474 0.032002087084000694 0.9426246226139687
+29.246181549 0.3146354063412599 -0.10531185088281841 0.03210361266228367 0.9428060952253837
+29.279588728 0.31492782510257605 -0.10460924263489824 0.032202845247071926 0.9427832985843075
+29.319473832 0.3292795253786904 -0.05527650227395744 0.01743693655655526 0.9424518320350724
+29.359443762 0.3567154861057565 -0.023190959454469157 0.008381725248960104 0.9338875671373755
+29.392987327 0.360946593189094 -0.04470723120881977 0.014463611798745599 0.9314019670778901
+29.432980661 0.34577297778749766 -0.11129112511418768 0.03910734338753012 0.9308737556704211
+29.472866367 0.3310136170867147 -0.13852117797464425 0.046104784748342106 0.932264027719077
+29.50637431 0.32634270221054634 -0.15144298392210845 0.0499821024659049 0.9317012679865658
+29.546290163 0.32651364207192 -0.17929210883548183 0.05979575993220456 0.9261034760459471
+29.586258229 0.3207882708899325 -0.2109358301925009 0.07033174620624963 0.920681490134361
+29.626251253 0.3173645078894013 -0.22428208807123948 0.07354740778710415 0.9184596305283756
+29.659858131 0.3149299011405167 -0.2271949905143246 0.07391106915404334 0.9185525284431624
+29.699635647 0.3159754663811358 -0.2325000272672274 0.07614490269132208 0.9166816218077525
+29.733141129 0.31843723826828896 -0.22883963567773133 0.07558358435495449 0.91679728850112
+29.773035006 0.32315472577674303 -0.1966484277758075 0.06518853581097142 0.9233909647924237
+29.806553861 0.3255257062845889 -0.18414720918973623 0.0617048727427316 0.925373075346022
+29.846479871 0.324835280905318 -0.15337750844106096 0.0502745022225885 0.9318958389272088
+29.886403411 0.3248880692958557 -0.12357078512198329 0.04067624659138025 0.9367622144689576
+29.926389194 0.3264593571862328 -0.08671084772300566 0.028153929331478582 0.9408043756572922
+29.959840492 0.3283675311496364 -0.05622802944971049 0.01805900866458588 0.9427019918282487
+29.999770493 0.3307567180131652 -0.010821135472014803 0.002527329810063534 0.9436506287394117
+30.033161415 0.3320567726442118 0.0025965095322532357 -0.0018482956263382029 0.9432540175809679
+30.066523885 0.3342815614753687 0.025411818591313442 -0.01012335377226029 0.942076214985748
+30.106499966 0.3382845206792486 0.044224933480299655 -0.0165699225687323 0.9398580616207644
+30.139866396 0.3401183879702809 0.05720737205044044 -0.021252432746559503 0.9384003052268968
+30.179989005 0.34256937750165134 0.06619532394759138 -0.02430110224744672 0.9368424932266702
+30.219842067 0.344623430236308 0.059878104556614964 -0.022311130314846956 0.9365636750327987
+30.253276402 0.34522447628295433 0.05522026732492536 -0.02065964087518999 0.9366664092891513
+30.286641332 0.3451187515838374 0.04860151789103361 -0.018292598243356377 0.9371212945042965
+30.326675126 0.3438457407107739 0.04832093668524092 -0.01830201934721479 0.9376034501646922
+30.366667361 0.3429318533799306 0.03413163782197401 -0.013270694643356435 0.9386461867501932
+30.400078661 0.34099087160867003 0.009880972929028852 -0.004427041687738833 0.9400042516688052
+30.439933789 0.3337466350949278 -0.04479772856893368 0.015465943953540087 0.9414707386082178
+30.473359852 0.327502001625859 -0.04822546446444369 0.01554501461772024 0.9434909093514253
+30.506713728 0.32742515100044156 -0.05396348614326533 0.017189703551363827 0.9431782582033048
+30.540067363 0.32762673246303753 -0.0692522807086646 0.022457088218536425 0.9419981555082719
+30.580183256 0.3211706030541114 -0.13150514868757188 0.04340804157037819 0.936841278728442
+30.620118924 0.3156623157618935 -0.15152403620692897 0.048945734663551704 0.935415460593658
+30.653556381 0.31251469315972596 -0.17233735877100592 0.055033408585554396 0.9325265279178772
+30.693612598 0.30707801890832764 -0.201591008905681 0.06279686470201684 0.9279658987351438
+30.733401342 0.3070144936742045 -0.2180656699052845 0.06916985484325372 0.9237992181549487
+30.766763122 0.307516455225731 -0.2285795236638387 0.07338757315311484 0.9207601724845129
+30.800130476 0.30685308901421826 -0.2363146033564864 0.07590469062529547 0.9188226531512906
+30.833570583 0.31304294798738397 -0.18654854721808672 0.059324835261395314 0.9293461767110238
+30.873680288 0.3179267616358679 -0.1587133597722939 0.051604343820103114 0.9333111139188353
+30.913521915 0.31398354851894233 -0.15394153937504743 0.04853342238545297 0.9356072042400321
+31.080372626 0.31321944996563716 -0.1506093523781129 0.047502949787253404 0.9364581511744019
+31.113758022 0.3088298470777448 -0.0974729465717457 0.028097697739777907 0.9456921643020432
+31.15368512 0.28208488208503557 -0.085771531000141 0.022564467961138535 0.9552812196178643
+31.18702301 0.2616628145118343 -0.08315458740030784 0.01818988565662334 0.9613984679391939
+31.220453011 0.247896361568307 -0.06999324306994446 0.014566461258688601 0.9661449984614866
+31.260330085 0.23110617691853164 -0.07257634513517928 0.013824733687868419 0.9701193152676527
+31.293733821 0.2202888951997949 -0.06696486874124245 0.012200800488341431 0.973056858294296
+31.327167668 0.2112520306738455 -0.06458384143320492 0.011464277138012064 0.9752282180657049
+31.36052945 0.19972929183072463 -0.058442444889792354 0.009600770030694117 0.9780595666087617
+31.393946942 0.195298987116921 -0.06071821612280596 0.008800591361365616 0.9788228406885789
+31.427284711 0.1884055201932333 -0.056530448392185614 0.007963294597824626 0.9804306473710642
+31.467105136 0.1809636017918513 -0.058018052413492005 0.0074003701882794995 0.9817491099775655
+31.507183596 0.17685620474810448 -0.05806968583402605 0.007400621854908224 0.9824942876299052
+31.547212613 0.16466045534051527 -0.059204781976887055 0.006700193527206321 0.9845490519241901
+57.646185138 0.3648277315793616 0.0025002234819031127 -0.001300098824158909 0.9310707733014958
+57.679691503 0.35939410032381164 0.003681914957301948 -0.002094031130928149 0.9331762637295851
+57.71305164 0.3601129890279416 -0.03938227313049235 0.014928919849971368 0.9319575092505827
+57.753067919 0.36876978940305033 -0.07447976382732267 0.028640860117576963 0.926089255058918
+57.793093463 0.3755303499851984 -0.06476356468277647 0.025402052968372942 0.924195527274703
+57.833076037 0.37505547533571193 -0.0693647513044173 0.027802568360908445 0.9239853564260185
+57.872958251 0.371648935723233 -0.0685273769739429 0.02712443229489106 0.9254433166615551
+57.906291651 0.3698817896467579 -0.06704118431847518 0.026201377701737485 0.926286364522019
+57.946298377 0.36883775196533913 -0.06956436196205014 0.026960001241541888 0.9264948303164049
+57.979743529 0.36771210408958105 -0.06789539441821965 0.02590238061688819 0.9270960525216282
+58.01307696 0.3660305216376218 -0.06936753172424923 0.026583477045683517 0.9276330748309578
+58.052974633 0.3643509402540849 -0.06872859753131956 0.02640199542584913 0.9283467600282103
+58.086505113 0.36332175280535794 -0.06870977841227453 0.02625440421377999 0.928755606469408
+58.126570832 0.3621776022579473 -0.06945023327718987 0.026402468538066267 0.9291431316947484
+58.166469941 0.3597280954018862 -0.07164803946981183 0.02704955467844082 0.9299089081250711
+58.199900426 0.35647340016840284 -0.07736889913395652 0.029023119575736042 0.9306440925237048
+58.239864087 0.3514784440798335 -0.08366090281104806 0.03073943108649393 0.9319435841640515
+58.273396205 0.34623189094108586 -0.09170006766039057 0.03309975448491024 0.933069655245244
+58.313314371 0.3374310922974114 -0.10038541388923851 0.03489764152998602 0.9353315889271071
+58.353324331 0.327247779651809 -0.10840353029644155 0.03620402773962069 0.9380015105039321
+58.386708159 0.32084967489258137 -0.11230649023244715 0.036469268088100394 0.9397407785438027
+58.420110256 0.31312522948248706 -0.11724194944659518 0.037205319884909466 0.9417126313922755
+58.459890547 0.2934158767140341 -0.1388722335495908 0.040950048927314096 0.9449575226082564
+58.493474575 0.28391013881347704 -0.1474430651071003 0.04211535504946051 0.946510365764577
+58.533395611 0.2799214687055404 -0.15989369219174132 0.044500901904946585 0.9455673684538147
+58.573331218 0.2763304265699522 -0.1738098073833949 0.047902215360459445 0.9440005423582
+58.613252003 0.274635833710138 -0.1851832836280359 0.05059342615452792 0.9421903287218684
+58.646809776 0.27439578896301897 -0.19183288427053158 0.05277666438157558 0.9408090769165474
+58.68670508 0.2734499045567866 -0.1939459675217642 0.05350264945124146 0.9406102157012908
+58.726659999 0.2703606687750818 -0.19308909508172478 0.05220484193570918 0.9417517531804647
+58.760194601 0.26848646144490124 -0.18647101365306806 0.04989948326164886 0.9437444689417859
+58.800091022 0.2672177509023071 -0.1748348539570433 0.04656774141441302 0.9464982265720799
+58.840027909 0.26672276279028617 -0.15613398075798654 0.041341451714856106 0.9501431640717569
+58.873411026 0.26730627161015935 -0.1377186299679909 0.03640504803532092 0.9530244533038811
+58.913491169 0.267306647262335 -0.11594112896747992 0.0303817542261305 0.9561285269006772
+58.953486966 0.26760993558545765 -0.09298748317700264 0.023696524552685096 0.9587370468864119
+58.986896014 0.2684987185719504 -0.07613944241148507 0.018971515054979834 0.9600788014798616
+59.026930521 0.27147298641603634 -0.0537368583379652 0.013143257657024324 0.9608548394427148
+59.066903569 0.2757755204858268 -0.034311853833894984 0.007947104326678526 0.9605765989862931
+59.100260082 0.2789962725356336 -0.02058850829599111 0.004222067172057051 0.9600621685006462
+59.133635337 0.280729665148298 -0.0076084474944423655 0.0006737538540854326 0.9597564861402046
+59.167015679 0.28310252320476503 0.0024970573964317837 -0.00216577863043965 0.9590839564200719
+59.206883378 0.28586696886309737 0.01334532404459 -0.005048480852492831 0.9581630817769238
+59.240381069 0.2874075173807805 0.022003848343924756 -0.007651018322476242 0.9575250448576805
+59.280263498 0.2883515447825343 0.025640446260136892 -0.008839736495476532 0.9571404354616921
+59.31368658 0.2890182194808034 0.02821762825440461 -0.009516449346823212 0.9568603197205283
+59.347298376 0.2889153881977819 0.026502940620754204 -0.009000426936712078 0.9569453928599765
+59.387020221 0.28713766647345373 0.023147914945962314 -0.008216010472606861 0.9575743478695261
+59.420461592 0.28566612440829653 0.017049436942301356 -0.006449856983485678 0.9581558231365015
+59.460346171 0.2847212276456365 0.007801813220703021 -0.0037008916980609337 0.9585714671525907
+59.493884887 0.28489390344878146 0.0004908958586388112 -0.001553619322662375 0.958557671225875
+59.533705066 0.2830206185816189 -0.019982702000005712 0.0040246109056272066 0.9588971913534396
+59.567245962 0.28142431079927405 -0.03430018739554989 0.008371431959058652 0.9589336648396168
+59.60708867 0.280308255851015 -0.049880761174041555 0.01276013983733152 0.9585282312993145
+59.640467299 0.2787649308114513 -0.06437196433450137 0.016457017644675297 0.958058208110367
+59.673882936 0.2771935994115264 -0.07916682342006238 0.020737205304306502 0.9573224591698588
+59.713885138 0.2746909690270775 -0.09632564400615011 0.02518388503298312 0.9563639546618281
+59.753815901 0.2741171465659534 -0.11080133999916107 0.029125424141915345 0.9548479264686516
+59.787474612 0.27420963270161186 -0.12284048621704814 0.03281320968003316 0.9532274574046325
+59.827247797 0.2746074941695586 -0.13359023495548616 0.0361976740755462 0.951543010936434
+59.860695477 0.2750936394584554 -0.1406197928985947 0.03807247857911732 0.9503157631805864
+59.90054314 0.2747201705490309 -0.14527095340460258 0.03940289304562001 0.9496697268050954
+59.934049841 0.2750193586155686 -0.148949280473019 0.040502850996111006 0.9489667977829562
+59.973959253 0.27412126356538025 -0.15448185688784744 0.042073137352098666 0.9482735575070906
+60.007318902 0.2740216010757454 -0.15461521604839573 0.042205345545121356 0.9482747523358459
+60.047302748 0.27431062504513654 -0.1480072023983457 0.040102043145115986 0.949336281388818
+60.087423516 0.27462680180856897 -0.13146965567843455 0.034409525993149566 0.9518990670680993
+60.120785661 0.27542878501884666 -0.13352188907119297 0.03580374184340854 0.9513301117866041
+60.154181941 0.2742236561334814 -0.13677340081460995 0.0367031662293901 0.9511820545169732
+60.1940437 0.2750566903523599 -0.10277152530977768 0.026483886164914996 0.9555524237065759
+60.227515539 0.27754067726615284 -0.062063430532637034 0.015683429985613236 0.9585788090068819
+60.267432144 0.2774233745832361 -0.0598201423342465 0.014705573070768592 0.9587708630984884
+60.307483302 0.2770237550885866 -0.04626981951180268 0.01089239699100195 0.9596865626894827
+60.347437713 0.27681844922520893 -0.029758710987930707 0.006486096043002889 0.9604394285154105
+60.381027699 0.277412988347121 -0.019566489315750134 0.0035415591196058336 0.960544972268961
+60.420908029 0.2785889332634186 0.01006048009946837 -0.005028237337328633 0.9603445786969375
+60.46097436 0.28091585121888696 0.017085858088799973 -0.007085293787664801 0.9595541446939199
+60.50093872 0.2822986613948781 0.021347961278119 -0.007982704177122624 0.9590557891791012
+60.534317552 0.28345973360606685 0.02364509443060165 -0.008600484591643576 0.9586540150638991
+60.567686078 0.28392580334411344 0.024721410965704366 -0.008800799821867556 0.9584871078724847
+60.601134895 0.28369488034782964 0.026191995964036263 -0.009400105294010322 0.9585107366286056
+60.64098427 0.2845070080118677 0.02567377340725567 -0.00941870582720424 0.958283834639669
+60.681035995 0.2852300783730423 0.012649299246872954 -0.005412466437362554 0.9583603199355323
+60.721056573 0.28443123442982315 0.007414061375186405 -0.004000247986348545 0.9586594299284287
+60.760950423 0.2845019932975103 -0.0008613114356811632 -0.001579706486831008 0.9586737601915314
+60.794346753 0.2848613354630318 -0.024229759872585188 0.005311685742634968 0.9582477363862157
+60.827683646 0.284014202386103 -0.032295726940009144 0.007338011729121775 0.9582479180505772
+60.861059918 0.2845095607832369 -0.045307837181211605 0.01156023760536584 0.9575321773284687
+60.894418265 0.2844159262635979 -0.06201152709322005 0.016353332129045144 0.9565535635412497
+60.927853232 0.28329829541775264 -0.07787573172405092 0.020758259805666403 0.9556393361889619
+60.967822384 0.28230882822833464 -0.09547560242921625 0.026050133702126322 0.954205179916366
+61.007852874 0.28180987820301423 -0.10726356102589757 0.029254292491293066 0.9530067719560443
+61.047793891 0.28092241133537277 -0.12055656628538143 0.033044621526746 0.9515549201823047
+61.081297135 0.28131808585239404 -0.13162044158457847 0.03630638562412605 0.9498515885612542
+61.121126465 0.2827108332999337 -0.14174546260265558 0.03957752898837435 0.9478483147453451
+61.154576236 0.283791753554629 -0.14830510399666721 0.0416675929157472 0.9464310056435885
+61.194722847 0.2854004613278448 -0.15143880308621713 0.04312906788465369 0.9453849740169706
+61.234615585 0.2865944179029341 -0.15324757293495053 0.04403714683332024 0.9446901876886443
+61.268036279 0.2866302970700691 -0.15064630796320014 0.04337631566709277 0.9451281701111915
+61.307955206 0.2860798252793477 -0.14874561154348634 0.04223665641173193 0.9456474720898108
+61.341304937 0.28563435663316866 -0.14678701439683384 0.0414955630347826 0.9461208722798826
+61.374700703 0.28531184809918114 -0.12376644738366559 0.03455950353679075 0.9497813730283127
+61.408128753 0.285867897914429 -0.11584382382246575 0.03246127675785646 0.9506871298883902
+61.441511647 0.28593200766477295 -0.10514477186184161 0.02907645558108238 0.9520199702074295
+61.481396216 0.28582490808609917 -0.0905523414362669 0.024889725185378877 0.9536691758456803
+61.514803789 0.2871992313567452 -0.07691973477045909 0.020965768695799105 0.9545472185564899
+61.554684279 0.28852221790137245 -0.05970025097906658 0.016157145254836726 0.9554735770640252
+61.588075195 0.28921573339151685 -0.05026073817264798 0.013231578080056611 0.9558519985325442
+61.621566899 0.28902458829576755 -0.03861862231306294 0.00967663376576401 0.956493466851743
+61.661394915 0.28920964147309014 -0.028077574915538837 0.006492429401688862 0.9568318982069594
+61.694815738 0.28931478822671375 -0.022769474428963054 0.005223037165444572 0.9569489141173259
+61.734797182 0.2886196224288099 -0.018122548407617643 0.0037215693259306628 0.9572650817354705
+61.774938242 0.2877273086742173 -0.020184227380927657 0.004100389174710778 0.9574908770409997
+61.814938254 0.28860733071930167 -0.020200513099549178 0.004400111764258237 0.95722431380636
+61.84847218 0.28875919295377667 -0.02466856135980777 0.0056226707181293 0.9570673832803606
+61.888382085 0.2868986877286922 -0.035891258963021734 0.008758331470854126 0.9572482708991699
+61.928269187 0.2848074621127248 -0.04726902322872832 0.01215780260011114 0.9573413898938286
+61.968318182 0.28381385518873375 -0.057337360004308904 0.014956490296768757 0.9570467209852649
+62.008424942 0.2834565449505991 -0.07891109639227538 0.021222548942302456 0.9554972681317461
+62.041827025 0.2827147524815781 -0.09533837404096494 0.026079160563548875 0.9540979197906292
+62.081712008 0.2835224224197247 -0.11310352924741736 0.03136085550925628 0.9517558113291231
+62.121640667 0.28516155946180044 -0.13185773876604248 0.03695805643671476 0.9486466801698306
+62.161637419 0.2885637616404501 -0.14642537987815493 0.0419411640547271 0.9452679526743653
+62.195031152 0.2918918358347289 -0.15595855502800993 0.04568280976347906 0.9425439863362987
+62.228512856 0.2947915046390717 -0.16447734235562703 0.049135778232572196 0.9400164083364405
+62.268405621 0.2973761095573238 -0.17183142899332057 0.05231330381434036 0.9377124973672327
+62.308522313 0.29812810348935065 -0.18173747143537178 0.05543336138117255 0.9354241112095116
+62.341934684 0.29801463716053067 -0.18514483088932002 0.056202760430945714 0.9347459105837181
+62.381822543 0.2979245286258951 -0.18514487503268026 0.05616388755648625 0.9347769617543609
+62.421928382 0.2995523883022208 -0.18661466857341408 0.056872414930442684 0.9339212282391112
+62.461770589 0.3007365446583887 -0.18039157304908446 0.05517915728209339 0.9348645204955892
+62.495282167 0.30022041504782776 -0.16600290707915755 0.049898563917465326 0.9379876707871769
+62.535280767 0.2978254459637583 -0.1628123460742277 0.048302961138273384 0.9393928718313593
+62.575271511 0.2927147726873752 -0.15690826899251742 0.045402425878007635 0.9421446155961101
+62.608663599 0.286404822110078 -0.1521919670442699 0.04302991809568021 0.9449647131972545
+62.648664754 0.2792963135003527 -0.14883539420674555 0.040629007611222547 0.9477293276231155
+62.688583807 0.27650900443530846 -0.14507150809035033 0.03900127006501638 0.949197518401677
+62.722013011 0.2757161863938473 -0.1437084366514177 0.03850226034154197 0.9496557511773573
+62.76204976 0.27562033538357833 -0.14227281164287617 0.03830282599851614 0.9499077698970579
+62.801974933 0.275125992387055 -0.14077764179702965 0.03790358092137181 0.9502897781395117
+62.835410443 0.2737120039082373 -0.14101231850801518 0.037403267462409705 0.9506830494590017
+62.875370115 0.27308301603726287 -0.13395448039718916 0.035370276453400545 0.9519615575626577
+62.915414692 0.2658904368289764 -0.12701364460125542 0.03250157604817458 0.9550463115694375
+62.948840307 0.26166074146074464 -0.12881301053463154 0.03180321222827083 0.9559965587737133
+62.98220057 0.2612686416605179 -0.1278545635588067 0.03105491294941914 0.9562570260362844
+63.022028478 0.26644026275902405 -0.12811085776695608 0.03264199356153755 0.9547416900709038
+63.055412647 0.26884135554383154 -0.1278137534498672 0.03280352983689863 0.9541026665976002
+63.095569362 0.27101884188485453 -0.12780888558259929 0.03340232221016288 0.9534662872805177
+63.135472554 0.2700953628187982 -0.12709006175872004 0.03308657714580149 0.9538353577000181
+63.175458795 0.26795909854581584 -0.129350491791601 0.03340080623526233 0.9541230308102929
+63.208911137 0.26435129431231846 -0.13191133229816376 0.03330286099718616 0.954782023844253
+63.248792856 0.2628103336504036 -0.13040512750385322 0.03262339091024838 0.9554375676164218
+63.282240414 0.26311020327320933 -0.13050506091658615 0.03275020240941291 0.9553370474606495
+63.315635148 0.26339183339590566 -0.13045815523136886 0.03287699510500992 0.9552614903924668
+63.355570487 0.26351772633217535 -0.13035070600370108 0.03282317673298574 0.9552432886035959
+63.389101183 0.25931271446282833 -0.14020103785137325 0.03480109946876105 0.954929038504476
+63.428970599 0.2551229596991654 -0.1395125553823347 0.03401113070096952 0.9561860516204315
+63.46898607 0.2560003571369712 -0.13890487248862282 0.0341925765899839 0.956033535630838
+63.502432967 0.2572087388153544 -0.13940473635637793 0.034301165401892125 0.9556324681646682
+63.536008704 0.2577122609524792 -0.13840658484991508 0.03420162718112064 0.9556454659145871
+63.575632014 0.25821791987023207 -0.1386262079074321 0.03450239440558873 0.9554663076840424
+63.60921073 0.25822738085573405 -0.13841467664769014 0.03420362674386562 0.955505159055915
+63.649086326 0.25810946063986767 -0.13840429321403241 0.03430125726442255 0.9555350237946283
+63.682468557 0.2581284147957332 -0.1385494421287691 0.034403787171535145 0.9555051820838569
+63.715828042 0.2580193803659286 -0.13833647818762831 0.034228658435329226 0.9555717749598635
+63.755707572 0.2578230534211806 -0.13858943437308835 0.034303067231755226 0.9555854443131813
+63.789079711 0.2569187840801675 -0.14606903584460204 0.036091826850644285 0.9546486029889636
+63.822464671 0.24362501333806216 -0.17087707394123441 0.040672210528031806 0.953831038375285
+63.855833243 0.23758004409366493 -0.16754055393694606 0.03816990524571669 0.9560486095217577
+63.895896852 0.23794496569337004 -0.17022896998692733 0.03940256580395416 0.9554327443030388
+63.929355883 0.23881305941020078 -0.17830975080753264 0.04149134794591114 0.9536521501405671
+63.9692235 0.23912270819126671 -0.17639606135460537 0.040795027248123984 0.9539604424281342
+64.009199932 0.2403130344044474 -0.1769099918860005 0.04130198195602294 0.9535442551621433
+64.042571409 0.2409098335335081 -0.17726412159570387 0.041701702193222445 0.9533104695407204
+64.082558493 0.24111473617741602 -0.1762107694502725 0.04160254261709045 0.9534582723830295
+64.115950647 0.24021800141042882 -0.1773132874690634 0.041803132635120414 0.9534714510603781
+64.155900702 0.24082688018823045 -0.1763196801378116 0.04170465491631732 0.9535064267917729
+64.18924529 0.2409076773069191 -0.1766128860660676 0.041901335322374056 0.9534231262079821
+64.222754713 0.2404164794523016 -0.1765327440663529 0.041823510363253796 0.9535653625531181
+64.262702332 0.24062498750884803 -0.17609446986537078 0.0417043307527804 0.9535990257260457
+64.30275806 0.24092503341152474 -0.17671836199176597 0.041904354088596454 0.9533990633092841
+64.342809314 0.24083320204607145 -0.1759146992861333 0.04170348470853757 0.953579680325913
+64.382700693 0.2405170352223008 -0.17641249485743807 0.04170295371629915 0.9535675388127394
+64.416184431 0.24056521230446204 -0.17590893625023213 0.04170211848570028 0.9536484457545273
+64.456125522 0.24061449264735138 -0.17631061950842913 0.041802517841476663 0.9535574344939715
+64.496032895 0.24062297337195407 -0.17611681467498388 0.04180399121757141 0.9535910436831182
+64.529717853 0.24070870538774838 -0.1763063762353969 0.041801511779010724 0.9535344851982471
+64.569622489 0.24071819416869888 -0.17611331114710377 0.041703152043351656 0.9535720737010984
+64.609475668 0.24070890132552572 -0.17630651974943992 0.04178206820719111 0.9535352613788483
+64.643018115 0.240713953388724 -0.17621021432111827 0.04170241735068463 0.9535552744335203
+237.224652599 -0.7398448860850116 0.02880199027778505 0.02600172727781124 0.6716575764989666
+237.26459653 -0.7372147911859329 0.029190583961903285 0.025722260357686447 0.6745373427666818
+237.304577638 -0.719774213481204 0.024585925767784924 0.021717065753797272 0.6934327529881326
+237.338274005 -0.7065618911623412 0.02311168683405692 0.021108277492017914 0.7069586865652762
+237.377909861 -0.7029157775106044 0.0202262565466694 0.017676291691649994 0.7107656836010039
+237.411334506 -0.7065108557314751 0.01991394273749358 0.019157448950281628 0.707162525709574
+237.444759512 -0.7066496268974564 0.021287525981304834 0.020379923502217845 0.7069496479674665
+237.484615802 -0.7085189490191697 0.02103443639616331 0.020916670756841563 0.7050680422843719
+237.518174235 -0.7032733386680614 0.019396886854743087 0.02049211851203453 0.7103593773421388
+237.558096176 -0.7011389792290881 0.022573860527453183 0.025281284609096007 0.7122186527148762
+237.591439633 -0.7116380918435294 0.020814186798997172 0.02342727837692802 0.7018469623023575
+237.624809877 -0.7142359766518366 0.0244222614330849 0.02459588541984571 0.6990461824680148
+237.664758513 -0.6883823330947716 0.02078155641383355 0.023101642546811355 0.7246821403261273
+237.698193782 -0.6418943843669256 0.018006441479894 0.02266905587123676 0.7662463580920589
+237.731617528 -0.6067771801183071 0.01863711239024718 0.023073335359475672 0.7943184077716052
+237.771560356 -0.5912876062541214 0.02099286459940562 0.022800858396953828 0.8058649931486939
+237.811430199 -0.6152911676118697 0.029743209425535877 0.027543034522966206 0.787256947763027
+237.844900693 -0.6516273881691241 0.02733493544917675 0.02600113227053243 0.757600481396709
+237.885046453 -0.6636923427193527 0.026877123905247095 0.027106730051557944 0.747031003113118
+237.924951961 -0.6568099397047276 0.02603639084756669 0.026168119409953988 0.7531520689630026
+237.965042646 -0.6534747112605533 0.02540290429448739 0.025666812022707472 0.7560864427808057
+237.998432626 -0.6509603119699333 0.023112637992068807 0.02398122870212651 0.75838076114508
+238.038373687 -0.6518610896148515 0.024282998066971213 0.026792872097090355 0.7574758067792048
+238.071807186 -0.6523312880969664 0.023697992628947867 0.02406036089311443 0.7571812165847778
+238.111745652 -0.6541333862571794 0.023704463170307794 0.026628799095168607 0.7555385618671443
+238.151660619 -0.6551582775796915 0.026902393019224084 0.025929784086878592 0.7545671201860434
+238.185055706 -0.6539363742338972 0.027039839756743497 0.028388807721200768 0.7555330178859974
+238.224994003 -0.652245341533551 0.02883335334408795 0.02866763586874644 0.7569166525030437
+238.258337377 -0.6492631197767772 0.027507326320962183 0.024420982651544156 0.7596738536390019
+238.291777781 -0.6463246800306472 0.030203130788683277 0.028002902717984494 0.7619501403065756
+238.331816336 -0.6417429834184132 0.02973726885534526 0.02591589327909385 0.7659046967800928
+238.371776423 -0.6364593181664243 0.02849356837761892 0.027223983453827823 0.7703028674522419
+238.411753748 -0.6290274181709601 0.0316022530027864 0.02614681523754289 0.776300295534697
+238.445094107 -0.621693604302577 0.031118363054238585 0.026102653773446564 0.7822067254352133
+238.478469514 -0.617013808729699 0.03326661036518229 0.025589466936078956 0.7858323432218917
+238.518569694 -0.6131338818246268 0.03307181434644744 0.026436454278204645 0.78884359155646
+238.558465278 -0.6292927738169665 0.030655855388338288 0.021713493844222557 0.776259845372176
+238.591893193 -0.6378941040430138 0.03199957415778331 0.021558830862241703 0.769156912530034
+238.631944176 -0.6271538701270035 0.03323997536894822 0.026351955926561813 0.7777394818581849
+238.665330098 -0.5905127439272005 0.03290102473176915 0.023453149952717144 0.8060162353131998
+238.70531927 -0.5733654448877479 0.03278302238490831 0.025601998148518296 0.8182431654115963
+238.858595665 -0.6130995875387844 0.03594102126302643 0.022020851374104526 0.7888803590248035
+238.891960809 -0.5880713019710265 0.03769197803191883 0.02311304540425028 0.8075996816011534
+238.925379419 -0.5734855512032803 0.03682319432298265 0.022278843954877052 0.818084364862791
+238.965553319 -0.5569114281884976 0.03934392474727085 0.02327021473008288 0.8293130975964054
+239.005490219 -0.5481361669675943 0.04184904788369592 0.025048255195668417 0.8349658583229344
+239.04536653 -0.5485901390509745 0.040902715418226786 0.023647850247542943 0.8347554171049852
+239.078977707 -0.5491597841097845 0.040132500891461004 0.0229317141831777 0.8344381644995109
+239.119176823 -0.5473186385746069 0.04160296761048178 0.022751179345660618 0.8355799691191871
+239.158778348 -0.5445705381193571 0.04157381085158515 0.022324953055749167 0.837386496030793
+239.192218698 -0.550261828806127 0.04060233999774576 0.022601435837753075 0.8336981137340519
+239.225675996 -0.5547888547499716 0.039830711121631945 0.020129149871999628 0.8307933909355507
+239.259023645 -0.5568821236861582 0.04090318897651194 0.02436319252774728 0.8292259428528524
+239.298827899 -0.5666330578628691 0.03909509744202021 0.020062966517381382 0.8227976838005171
+239.332263476 -0.5756114966686485 0.040498957890201776 0.02240038010407117 0.816412556422229
+239.372266675 -0.5810681018589907 0.034107145619721294 0.01710974908846781 0.8129599129759072
+239.405604515 -0.5936616049119721 0.04081131010694034 0.021719302649958704 0.8033857154026927
+239.438948668 -0.627638922925641 0.04232908794896426 0.020738562164078395 0.7770762786120272
+239.472315909 -0.6432891548836509 0.040580441231301535 0.020878436195524163 0.7642619851206658
+239.505813017 -0.6415426694839114 0.040304059466855356 0.021763165689755417 0.7657185844950543
+239.54560886 -0.6325430527964304 0.036403340116676834 0.019860130466871854 0.7734142863984094
+239.578950703 -0.6249371039304736 0.038236719767479194 0.02133236741719445 0.7794462774898561
+239.612357077 -0.6262389018208373 0.038079984561546563 0.021600614098840124 0.7784010316620714
+239.64572434 -0.6294433047169053 0.03891086160143383 0.020487595873406907 0.7758010888184054
+239.679065633 -0.6327580022048975 0.03972190072611971 0.02002299569503784 0.7730708640815328
+239.712583156 -0.635157093496259 0.04406319601429506 0.02342248780517333 0.7707692835105762
+239.752536358 -0.636784906242236 0.04345734208913995 0.020565576239353314 0.7695410968067642
+239.792381657 -0.639159534914996 0.04638397560371163 0.022496762458775955 0.7673444542142165
+239.825895897 -0.6423973545820866 0.04518489070093774 0.023649082539650847 0.7646729270566037
+239.865988228 -0.6435476610183528 0.046117123960465585 0.023415442678233086 0.7636565562604728
+239.905810086 -0.6479926071669341 0.04708001288789257 0.025347004697747352 0.7597674531041243
+239.939230309 -0.6538293515860074 0.04695267676296676 0.02326001572880971 0.7548255406500957
+239.972745715 -0.6557733073619862 0.04713364913710568 0.023501531331391216 0.7531184943258411
+240.012586389 -0.6582987223720739 0.04786685633500892 0.02444960263871173 0.7508353835020748
+240.05252711 -0.6700262578599798 0.04300530392524151 0.017322349472025912 0.7408881790265104
+240.085969655 -0.7001708375552325 0.044196381582831165 0.019186952256127054 0.7123477654596175
+240.119389433 -0.7210171826308943 0.043655057963665735 0.02040226519295533 0.6912396153579402
+240.159243469 -0.7292553754727438 0.04315211551348799 0.022799589687022344 0.682498843208486
+240.199351733 -0.7328958141178934 0.03984653537153811 0.020058966915709527 0.6788767318988252
+240.239255958 -0.7392580145346286 0.040533371904498364 0.01993244109385431 0.6719057459944888
+240.272685278 -0.7440371721386948 0.04237320511099688 0.02124227062326568 0.6664547725864403
+240.306045977 -0.7491917310420214 0.040609613752215425 0.017808660763861464 0.6608672037640793
+240.346166128 -0.7535554430214993 0.040696543331009145 0.01900139803239348 0.6558482541917708
+240.386025549 -0.7563236059040115 0.04090972361520709 0.01872382837338613 0.652648769183797
+240.419548684 -0.7572022343054041 0.03799379700621332 0.02113207278048279 0.6517320640047987
+240.459425203 -0.7588321903106341 0.03810428629021577 0.01760080560694561 0.6499322902862001
+240.492855212 -0.7606906238959252 0.03776501398651325 0.01996377356278154 0.6477075159210062
+240.532733447 -0.7616749154204283 0.03730218337940114 0.017870713111560747 0.6466378491453291
+240.566139425 -0.7619988154362971 0.03600061610258555 0.019400489625791807 0.646285836078893
+240.599512374 -0.7639172030025358 0.0369748529559444 0.016244060771824553 0.6440492975660405
+240.63955173 -0.7619946257186881 0.03763024234044501 0.018282317981459534 0.6462305409731581
+240.673226409 -0.7627946315747083 0.03747640716029538 0.018868154977860513 0.645278127379886
+240.712977483 -0.7874763771384581 0.036703365723037104 0.019842236049353017 0.6149309750387045
+240.752841427 -0.7932514643598876 0.03650291347851922 0.019138810554269938 0.6074976193608018
+240.786276718 -0.7873101904571339 0.039632257426235176 0.022001393176875588 0.6148885158074824
+240.826387453 -0.7807397286608538 0.03861306972099767 0.019150943750093352 0.6233680680711665
+240.866407073 -0.7780167852440415 0.04018876600093528 0.01977046881212255 0.6266449341762831
+240.90626368 -0.7770544034990937 0.040793166763709254 0.02110658808902222 0.6277554328616332
+240.939623332 -0.773756269311581 0.03873000636236827 0.022850093583271003 0.6318853499895617
+240.972990291 -0.7705633609395449 0.040243112752171024 0.021782805146814233 0.6357185761431742
+241.006524855 -0.7649289126254382 0.0406568845388112 0.024278660844678453 0.6423716393154758
+241.046515904 -0.7585653724527895 0.041864354220823456 0.02236362999526692 0.6498660012761625
+241.086410399 -0.7590491325849261 0.0423037432165785 0.025020245235346746 0.6491754731660002
+241.126376736 -0.7608761501913889 0.04051535304488624 0.02207799609793322 0.6472546271181681
+241.159723766 -0.763111740906569 0.042180996459945566 0.024841424643631663 0.6444099146117063
+241.193190873 -0.7657056982338764 0.0412532437792336 0.02205006662221007 0.6414879173700928
+241.226680258 -0.7800509523526011 0.04082885668824386 0.021754262779086094 0.6240034200597536
+241.266542382 -0.7878570149977546 0.04050294899629278 0.02490181308660963 0.6140201419712802
+241.306530322 -0.7937301074622842 0.040448216200342994 0.023779360466155745 0.6064577481819666
+241.34648746 -0.7953611626425902 0.04038263266062048 0.0270381090412026 0.6041844127405975
+241.379869979 -0.7979080359031728 0.04169295130797545 0.025582584389363262 0.6007911412698537
+241.419866053 -0.8020346459463255 0.04031729865444801 0.027801714424672327 0.5952663326663196
+241.453324024 -0.8021744889231099 0.040203733268556315 0.02767496999175672 0.595091459515404
+241.486664985 -0.8021993865822173 0.04015406314749203 0.027802583829647017 0.5950553013899271
+241.519999899 -0.8006753898767827 0.040994413663027834 0.027608897116518285 0.5970562175323998
+241.553356735 -0.800629081395388 0.04167112845056958 0.02740393024429167 0.5970809121758955
+241.593333504 -0.8000552981777835 0.039282366786158714 0.02970205294485021 0.5979014998878972
+241.626691213 -0.799600529840944 0.04018235251939279 0.025221509036599372 0.5986553655535093
+241.660145009 -0.8006425422975403 0.04079847331279591 0.02910947405960326 0.5970424126966609
+241.700003673 -0.8004875900298837 0.04099529348393601 0.026387724774763914 0.5973631157859731
+242.720558703 -0.794450343530368 0.021729453520980112 0.03624393879652909 0.6058571278896314
+242.760454466 -0.7950640377780425 0.024836050594316345 0.034944504321129055 0.6050084528671501
+242.794103122 -0.7819008218387371 0.02060918070083504 0.03545406344966234 0.6220525507251421
+242.83403055 -0.7629413999181803 0.028900595538859982 0.04050008098162014 0.6445501681862077
+242.873829245 -0.7580740197433834 0.02470217157341355 0.03871078036053453 0.6505498126919601
+242.907301163 -0.758829264307354 0.02860248727435542 0.03957688868944677 0.6494564767820422
+242.947372815 -0.7623713427443437 0.02649413019336018 0.03832578769088851 0.6454604022133013
+242.987461621 -0.7637815838681922 0.02730291638025618 0.03640388850700824 0.6438687752970303
+243.027253869 -0.7616177267712572 0.027439704419571852 0.036416624517971244 0.6464203975338854
+243.060659886 -0.7604374493482504 0.030908341136354635 0.03497066186737446 0.6477318989253837
+243.09401031 -0.7735665648355493 0.033803728624669155 0.0376081341625404 0.6316943136869367
+243.127388229 -0.791929356078706 0.031169605853374218 0.03485865020635539 0.6088195341457948
+243.16074622 -0.7959355246960755 0.030774438423373705 0.034264996897322 0.6036269414581621
+243.194248473 -0.7929810467862657 0.03296087825419788 0.035888063432670286 0.6072945634908344
+243.233985182 -0.78504977820596 0.034002198937717595 0.03440189434575279 0.617541258436711
+243.267587731 -0.7785228240679044 0.03312143077675334 0.033703783679774224 0.6258348329985337
+243.307450961 -0.7743803674115186 0.03242764746757066 0.037178671653781824 0.6307941348977342
+243.340870149 -0.7748086221488291 0.028920749638342725 0.03282099712563151 0.6306805621167253
+243.374236749 -0.7759793421386333 0.03359608267896157 0.03408935798259871 0.6289398059234736
+243.407665296 -0.7772879387144771 0.032521657399209696 0.03450307063968944 0.6273558322399183
+243.447583812 -0.7788086853502265 0.03432244926460159 0.03554459571356544 0.6253123881829706
+243.487506729 -0.7818954694066684 0.03255672619385175 0.0353467846351223 0.6215546149107273
+243.520864347 -0.784278968627682 0.032986754940516415 0.03357024721393656 0.6186205718118247
+243.554311235 -0.7868957125355791 0.03698578020564404 0.036168606278630576 0.6149138326428871
+243.594197731 -0.7902765231685532 0.035827896450185366 0.03318934555182752 0.6108009873161794
+243.627636256 -0.7934598347217816 0.035382648985657204 0.03566265088442944 0.606545739549736
+243.661027148 -0.8052147827536443 0.038174259153005104 0.03720337355739193 0.5905825840380581
+243.700991524 -0.8291901340181825 0.037995973170304405 0.03515571618560939 0.5565645544671588
+243.734370962 -0.83509730954713 0.04003248448780763 0.036922347441520044 0.5473998758058738
+243.774402322 -0.8198314283616239 0.03764953561266079 0.032976020427303285 0.5704134672458818
+243.814480121 -0.8054633799247986 0.038357166197690716 0.03364830924751279 0.5904449700744229
+243.847913394 -0.8028464662978672 0.03550362042066799 0.03260220003156211 0.5942340793350336
+243.887739277 -0.8105031136087074 0.04011207707689189 0.03300265080081505 0.5834265584829745
+243.92769101 -0.8185163084845994 0.037888173152325906 0.032675454248351755 0.5723004925470424
+243.961154419 -0.823418853455404 0.03692683561043423 0.03360259745697052 0.5652332846092247
+244.001043379 -0.8268167768205759 0.034092419986228 0.032927749161793134 0.5604707733706146
+244.03450406 -0.8301858796962555 0.03593063620207311 0.034086782132382455 0.5552823478366015
+244.074515133 -0.8322518736899639 0.03527525947168676 0.03269428096774672 0.5523074857366608
+244.114529925 -0.8349697708900148 0.03588213317955448 0.03596343772328037 0.5479457868854224
+244.154597694 -0.834974971113073 0.035574076353461645 0.034544866154355945 0.5480492084919919
+244.188142254 -0.8341544866192296 0.03522830925146127 0.03421536706050674 0.5493401199049707
+244.2278879 -0.8339667080706517 0.037112152092703826 0.03520268466225661 0.5494387945799042
+244.261326426 -0.8338501520547195 0.03582483636170743 0.03121369300777566 0.549942006385032
+244.294858025 -0.8336788367480741 0.03572444280238121 0.03482435768593056 0.5499914776217899
+244.33459512 -0.8333759764707798 0.032271374510706746 0.031734460715658185 0.5508502194169032
+244.374535448 -0.8338381907472637 0.03687101017793684 0.034501580212018 0.5496944980829462
+244.407988351 -0.8334485181807082 0.03500440691503399 0.030701787266795948 0.5506320543680081
+244.441348514 -0.8343814765829702 0.03518358394791308 0.03570219247145967 0.5489034709386829
+244.474893758 -0.8336025385320821 0.0354563778747961 0.03449439015292214 0.5501452445207959
+244.514606905 -0.8334164674511001 0.03513605823197383 0.033270832259868106 0.5505229340490589
+261.742808571 -0.492390337246879 0.02761474303904702 0.02918992189195911 0.8694464504572628
+261.776378882 -0.48682071379739567 0.029633345065584637 0.031439369275732446 0.8724328189250805
+261.816359221 -0.46278316432593464 0.042966379369456555 0.039929856322520406 0.8845288235181586
+261.856255245 -0.4364826657557189 0.039018122632218266 0.03889695019679967 0.8980242178618716
+261.889617012 -0.40534723562500025 0.05057737482168146 0.040945473304809334 0.9118437453550706
+261.92301596 -0.4110476949285202 0.05790648721927345 0.04770811563288436 0.9085210877765215
+261.963055751 -0.4528477920897629 0.053507990588702134 0.04520407847818043 0.888832022055644
+262.00313968 -0.4702518066407381 0.06310344281097734 0.05529647924120107 0.8785348560188441
+262.036483884 -0.4761414437583875 0.060736790013897105 0.05157378623604043 0.8757513987700879
+262.076405694 -0.4812953535919647 0.06598484871490432 0.05747814962520306 0.8721794796178821
+262.109740578 -0.4876601820802172 0.0667707860672644 0.058342405950877044 0.8685190686503734
+262.143299317 -0.4919022542838608 0.07434590779148013 0.0632902792169759 0.8651584818874252
+262.183013211 -0.4984287966872071 0.07269072338000834 0.06320437753256815 0.8615625340206912
+262.216442071 -0.5067808238927949 0.08285415137576507 0.0707274172226619 0.8551643225645895
+262.256639628 -0.5185293751509472 0.0817992261845819 0.07073329780268191 0.8481939485065483
+262.29649362 -0.5277778670740825 0.0909772739298654 0.0791509258262726 0.8407786805074605
+262.329980071 -0.5379426006928065 0.09243346559147012 0.08101851306186507 0.8339723096961043
+262.369948234 -0.5494691869768229 0.0989374779731067 0.08267584488145745 0.8255057193554068
+262.409808927 -0.5576234483276423 0.1040292906370292 0.08986680595659953 0.8186317571117704
+262.449800633 -0.5633853831281898 0.10419232251821237 0.0906687360471637 0.8145674007160939
+262.489861551 -0.5671783877908197 0.10166763029185129 0.090148851798643 0.8123087798942176
+262.529911177 -0.5756197916112045 0.10696082555957338 0.09720520317847695 0.8048430814612989
+262.56997375 -0.5687882709979508 0.09596812028204671 0.08556701867675583 0.8123720256012412
+262.609853522 -0.5615109406583646 0.08894432496118791 0.08335492231888024 0.8184414013864222
+262.643276813 -0.5621995622183242 0.09236672514071978 0.08203435753785487 0.8177226941400458
+262.68331509 -0.5652046223756785 0.09149095540931225 0.0792894395324198 0.8160185811010021
+262.716713959 -0.5792666486301181 0.07541814343364919 0.07346740403770038 0.8083098378529203
+262.756684576 -0.6327026201780289 0.07314176725312356 0.07318043642833083 0.7674518226098013
+262.796721622 -0.6867117743077357 0.07253510126562135 0.07413523648881502 0.7194925745428198
+262.836624835 -0.7026505013220113 0.07255152169512319 0.07050692745189623 0.7043062706471819
+262.869991225 -0.6961192155330221 0.07078602036596023 0.07135737268391958 0.7108554722656977
+262.903445924 -0.6896244783031741 0.06977794592203947 0.0636563069737576 0.7179811917943524
+262.936843485 -0.6974285771125298 0.06741535042046969 0.061791339797198495 0.7107955969760537
+262.976781499 -0.7175640009150057 0.07108412266598935 0.05977200750756993 0.6902725977569812
+263.01015257 -0.7339481096858896 0.0708928757550175 0.05636305072004498 0.6731400886659645
+263.050089341 -0.7488010480670951 0.07039084133393367 0.04925384282377272 0.6572033009936971
+263.083433029 -0.7650276565384505 0.06608428793880212 0.046159723792380945 0.6389325719653987
+263.123580477 -0.759769112365214 0.07120439637288271 0.051619252283097564 0.6442175739815563
+263.163652675 -0.7395908309264311 0.08969193491507335 0.03875318628266474 0.6659271357841506
+263.203570002 -0.7138426882345714 0.08041083482311241 0.03470125541199424 0.6948082735330102
+263.243520895 -0.6905059060909539 0.0680043831659756 0.033929833028546276 0.719323129027825
+263.276978085 -0.6735392545108225 0.07094116690038248 0.05053340002649777 0.7340017704022106
+263.310351264 -0.6535198118300644 0.08169482521562137 0.048827371442678846 0.7509019236069282
+263.350348329 -0.6285031016449124 0.06540252568789343 0.033638237266481574 0.7743221744532267
+263.390215901 -0.597597759975782 0.08041849222986115 0.045151379258440816 0.7964741906241932
+263.423631479 -0.5855794838506132 0.06666940243972289 0.03496655077367687 0.8071116398606649
+263.463657246 -0.5845601318882496 0.07226772360141404 0.034338932523008195 0.8073956069024912
+263.497184693 -0.589325291486152 0.06999637263091353 0.035476477527406215 0.8040756358549386
+263.537050353 -0.5931609242298242 0.07129197916553609 0.03660375961357539 0.8010853490456764
+263.577229306 -0.590704120917637 0.0705189697772078 0.036731944054572675 0.8029610704875143
+263.617137181 -0.5877951213932916 0.06688175186599517 0.03978050132459503 0.8052584915714612
+263.650505531 -0.5885396442081441 0.06680449997128976 0.03910127750133417 0.80475420848648
+263.690427283 -0.5860902186568708 0.06565280929277087 0.040128839421852636 0.8065839326898123
+263.730486075 -0.5916245608673145 0.06052850817983245 0.03809646185305764 0.8030350791032479
+263.764034955 -0.5954060180692134 0.06401254344918918 0.04038745934692126 0.7998518119349546
+263.803941131 -0.6009381353312887 0.06158764547490287 0.038086154078189126 0.7960086458682116
+263.843968779 -0.601188718705783 0.06170522943653155 0.03720362711233978 0.7958520461057882
+263.883795881 -0.6041257681638235 0.06243057055249998 0.03910166782851432 0.793476867762374
+263.917149503 -0.6057301358239046 0.0590029354690612 0.03650181601052091 0.7926394347928458
+263.950528031 -0.6080634013365945 0.06048437618307217 0.03908286242362899 0.7906156272535295
+263.98395776 -0.6115813643241973 0.0615029372596799 0.03869753532337942 0.7878376255800946
+264.02402125 -0.6096917676270661 0.06497343686839953 0.04135971642311708 0.7888876820224101
+264.057500051 -0.6052202428728259 0.06308817907674807 0.03760298037333147 0.7926628256357054
+264.097336955 -0.5926158386131634 0.06392423657699592 0.04070258960181896 0.80191237613755
+264.137350425 -0.5713310415499188 0.06501045153289282 0.03998309797578019 0.8171632848025134
+264.177556814 -0.5624531659050787 0.06614190727716705 0.0404084276423265 0.8231882185992262
+264.217318908 -0.5494980844410502 0.06670474225761372 0.03930034106594068 0.8319001236615261
+264.250654226 -0.5335587415490124 0.06444960489227361 0.040048110162840045 0.8423523411365634
+264.284025954 -0.5079092288151696 0.06365611557014426 0.035546367802650525 0.8583196199382481
+264.317373401 -0.48606852198966877 0.0629840680658517 0.03542041845945029 0.8709281216362305
+264.350790037 -0.4737681769746858 0.06021818663254122 0.030406992407089835 0.8780620133552792
+264.390799451 -0.46115125378317784 0.059743582610039564 0.029793664094503413 0.8848065116458765
+264.424157809 -0.45145793664761047 0.056941380067110955 0.026838732954425747 0.8900691507334337
+264.464123518 -0.4375945786764074 0.05598087255134386 0.02476207947184981 0.8970863760205243
+264.497628602 -0.4232633740128462 0.0557021561900418 0.02447444228530421 0.9039614967961107
+264.537516814 -0.40976446304231784 0.051697068436898794 0.020471949480633984 0.9104951384972542
+264.570953282 -0.394404762834615 0.05649619527227564 0.022199131234126868 0.9169298018635768
+347.143468116 0.4730519346282629 -0.06221664456394384 -0.021076123747344584 -0.8785822404826131
+347.18332169 0.4648592034940948 -0.06187319382945125 -0.013535347229064071 -0.8831163135100425
+347.216678041 0.44476200771752367 -0.0750702575895775 -0.02079571159351462 -0.8922548690233253
+347.250059703 0.4365460120904825 -0.04296919106067979 -0.015423656303356275 -0.8985228649142565
+347.28996064 0.43028404268218046 -0.06387543895312994 -0.04280942086964046 -0.899412544051077
+347.323446715 0.44809170396511877 -0.04104020887525186 -0.05126545982605537 -0.8915724192245604
+347.356899921 0.48925795075881234 -0.04803711142677635 -0.059733227690227084 -0.8687640848094301
+347.396893155 0.5239674490283242 -0.03998553782005044 -0.053927633452822386 -0.84908838142691
+347.43697453 0.5315155721034144 -0.021464538394438206 -0.058758469653814725 -0.8447354097272327
+347.470330691 0.5037041385344577 -0.030576215470486272 -0.06966944538685836 -0.8605192643109448
+347.510236723 0.4545578437293535 -0.029488655136588736 -0.0791907512372356 -0.8866997298075816
+347.543585399 0.43450533757488047 -0.034308086723568595 -0.08974978294096142 -0.8955294764921797
+347.583513418 0.393449902051833 -0.014749737676544522 -0.08293897946586651 -0.9154784243765862
+347.616963521 0.38354208486947444 -0.02777757660563286 -0.077284434012173 -0.919864659409912
+347.65041836 0.38698802213408134 -0.03715615252394913 -0.08295334299492432 -0.9175938284122966
+347.690161084 0.3983786712713217 -0.007501916893747665 -0.07418855541829539 -0.9141849997478422
+347.723539363 0.40259613132462235 -0.021110313238303816 -0.07665253807862202 -0.9119183615453379
+347.756932275 0.40113316061560916 -0.013092739112825628 -0.07180545208523943 -0.9131071923317052
+347.796937718 0.3819278328053421 -0.022587278562296493 -0.060477766673134876 -0.9219345883054795
+349.31776045 0.40910432265814306 0.01678086795843859 -0.005840281520681834 -0.9123146095315708
+349.357902582 0.40562481581795595 0.02008156672452735 -0.004100331383478491 -0.9138098416810159
+349.3977756 0.37812767583828616 0.020411226141690005 -0.002022521124874527 -0.9255262027737672
+349.431331935 0.3660248648532596 0.01859780583343839 -0.004765823635339921 -0.930407011394697
+349.470994107 0.3639449660241751 0.01633510706619165 -0.004409753837888614 -0.9312667609519237
+349.504374961 0.36648487325138274 0.008994882729107818 -0.006562673605771192 -0.9303573835240689
+349.537824176 0.36488204611016994 0.00864716930228317 -0.007275381984313243 -0.9309851705083685
+349.577759027 0.3635893202800476 0.004373257049036545 -0.008436823042478997 -0.9315108699408859
+349.61780063 0.3554795000826525 0.005240242412714433 -0.005117332223630488 -0.9346553791592689
+349.657798937 0.3473621577382386 0.0036648960411413793 -0.005554370365892558 -0.9377074431176715
+349.691153707 0.34494037906991837 0.004945084104685389 -0.002184370522349425 -0.9386090291253968

build.sh

@@ -0,0 +1,5 @@
+source /opt/ros/humble/setup.bash
+source ~/MOVE_AI/install/setup.bash
+
+cmake -B build
+make -C build/ -j $(nproc)

calibration/board_pattern.hpp

@@ -0,0 +1,108 @@
+#ifndef CALIBRATION__BOARD_PATTERN_HPP
+#define CALIBRATION__BOARD_PATTERN_HPP
+
+#include <algorithm>
+#include <cctype>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#include <opencv2/opencv.hpp>
+#include <yaml-cpp/yaml.h>
+
+namespace calibration
+{
+enum class PatternType
+{
+  circles_grid,
+  chessboard
+};
+
+struct BoardPattern
+{
+  cv::Size pattern_size;
+  double center_distance_mm;
+  PatternType pattern_type;
+};
+
+inline std::string normalize(std::string text)
+{
+  std::transform(
+    text.begin(), text.end(), text.begin(),
+    [](unsigned char c) { return static_cast<char>(std::tolower(c)); });
+  return text;
+}
+
+inline PatternType parse_pattern_type(const YAML::Node & yaml)
+{
+  if (!yaml["pattern_type"]) return PatternType::circles_grid;
+
+  auto raw = normalize(yaml["pattern_type"].as<std::string>());
+  if (raw == "circles_grid" || raw == "circle_grid" || raw == "circles") {
+    return PatternType::circles_grid;
+  }
+  if (raw == "chessboard" || raw == "checkerboard") {
+    return PatternType::chessboard;
+  }
+
+  throw std::runtime_error(
+    "Unsupported pattern_type: " + raw +
+    ". Supported values: circles_grid, chessboard.");
+}
+
+inline std::string pattern_name(PatternType pattern_type)
+{
+  return pattern_type == PatternType::chessboard ? "chessboard" : "circles_grid";
+}
+
+inline BoardPattern load_board_pattern(const YAML::Node & yaml)
+{
+  BoardPattern board_pattern;
+  board_pattern.pattern_size = cv::Size(yaml["pattern_cols"].as<int>(), yaml["pattern_rows"].as<int>());
+  board_pattern.center_distance_mm = yaml["center_distance_mm"].as<double>();
+  board_pattern.pattern_type = parse_pattern_type(yaml);
+  return board_pattern;
+}
+
+inline bool find_pattern_points(
+  const cv::Mat & img, const BoardPattern & board_pattern, std::vector<cv::Point2f> & points)
+{
+  if (board_pattern.pattern_type == PatternType::circles_grid) {
+    return cv::findCirclesGrid(
+      img, board_pattern.pattern_size, points, cv::CALIB_CB_SYMMETRIC_GRID);
+  }
+
+  cv::Mat gray;
+  if (img.channels() == 1)
+    gray = img;
+  else
+    cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
+
+  auto flags = cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_NORMALIZE_IMAGE | cv::CALIB_CB_FAST_CHECK;
+  auto success = cv::findChessboardCorners(gray, board_pattern.pattern_size, points, flags);
+  if (!success) return false;
+
+  cv::cornerSubPix(
+    gray, points, cv::Size(11, 11), cv::Size(-1, -1),
+    cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, 1e-3));
+  return true;
+}
+
+inline std::vector<cv::Point3f> planar_points(
+  const cv::Size & pattern_size, const double center_distance_mm)
+{
+  std::vector<cv::Point3f> points;
+  points.reserve(static_cast<size_t>(pattern_size.width * pattern_size.height));
+
+  for (int i = 0; i < pattern_size.height; i++) {
+    for (int j = 0; j < pattern_size.width; j++) {
+      points.push_back({static_cast<float>(j * center_distance_mm), static_cast<float>(i * center_distance_mm), 0.0f});
+    }
+  }
+
+  return points;
+}
+
+}  // namespace calibration
+
+#endif  // CALIBRATION__BOARD_PATTERN_HPP

calibration/calibrate_camera.cpp

@@ -0,0 +1,118 @@
+#include <fmt/core.h>
+#include <yaml-cpp/yaml.h>
+
+#include <fstream>
+#include <opencv2/opencv.hpp>
+
+#include "calibration/board_pattern.hpp"
+#include "src/component/img_tools.hpp"
+
+const std::string keys =
+  "{help h usage ? |                          | 输出命令行参数说明}"
+  "{config-path c  | configs/calibration.yaml | yaml配置文件路径 }"
+  "{@input-folder  | assets/img_with_q        | 输入文件夹路径   }";
+
+void load(
+  const std::string & input_folder, const std::string & config_path, cv::Size & img_size,
+  std::vector<std::vector<cv::Point3f>> & obj_points,
+  std::vector<std::vector<cv::Point2f>> & img_points)
+{
+  // 读取yaml参数
+  auto yaml = YAML::LoadFile(config_path);
+  auto board_pattern = calibration::load_board_pattern(yaml);
+
+  for (int i = 1; true; i++) {
+    // 读取图片
+    auto img_path = fmt::format("{}/{}.jpg", input_folder, i);
+    auto img = cv::imread(img_path);
+    if (img.empty()) break;
+
+    // 设置图片尺寸
+    img_size = img.size();
+
+    // 识别标定板
+    std::vector<cv::Point2f> centers_2d;
+    auto success = calibration::find_pattern_points(img, board_pattern, centers_2d);
+
+    // 显示识别结果
+    auto drawing = img.clone();
+    cv::drawChessboardCorners(drawing, board_pattern.pattern_size, centers_2d, success);
+    cv::resize(drawing, drawing, {}, 0.5, 0.5);  // 缩小图片尺寸便于显示完全
+    cv::imshow("Press any to continue", drawing);
+    cv::waitKey(0);
+
+    // 输出识别结果
+    fmt::print("[{}] {}\n", success ? "success" : "failure", img_path);
+    if (!success) continue;
+
+    // 记录所需的数据
+    img_points.emplace_back(centers_2d);
+    obj_points.emplace_back(
+      calibration::planar_points(board_pattern.pattern_size, board_pattern.center_distance_mm));
+  }
+}
+
+void print_yaml(const cv::Mat & camera_matrix, const cv::Mat & distort_coeffs, double error)
+{
+  YAML::Emitter result;
+  std::vector<double> camera_matrix_data(
+    camera_matrix.begin<double>(), camera_matrix.end<double>());
+  std::vector<double> distort_coeffs_data(
+    distort_coeffs.begin<double>(), distort_coeffs.end<double>());
+
+  result << YAML::BeginMap;
+  result << YAML::Comment(fmt::format("重投影误差: {:.4f}px", error));
+  result << YAML::Key << "camera_matrix";
+  result << YAML::Value << YAML::Flow << camera_matrix_data;
+  result << YAML::Key << "distort_coeffs";
+  result << YAML::Value << YAML::Flow << distort_coeffs_data;
+  result << YAML::Newline;
+  result << YAML::EndMap;
+
+  fmt::print("\n{}\n", result.c_str());
+}
+
+int main(int argc, char * argv[])
+{
+  // 读取命令行参数
+  cv::CommandLineParser cli(argc, argv, keys);
+  if (cli.has("help")) {
+    cli.printMessage();
+    return 0;
+  }
+  auto input_folder = cli.get<std::string>(0);
+  auto config_path = cli.get<std::string>("config-path");
+
+  // 从输入文件夹中加载标定所需的数据
+  cv::Size img_size;
+  std::vector<std::vector<cv::Point3f>> obj_points;
+  std::vector<std::vector<cv::Point2f>> img_points;
+  load(input_folder, config_path, img_size, obj_points, img_points);
+
+  // 相机标定
+  cv::Mat camera_matrix, distort_coeffs;
+  std::vector<cv::Mat> rvecs, tvecs;
+  auto criteria = cv::TermCriteria(
+    cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 100,
+    DBL_EPSILON);  // 默认迭代次数(30)有时会导致结果发散，故设为100
+  cv::calibrateCamera(
+    obj_points, img_points, img_size, camera_matrix, distort_coeffs, rvecs, tvecs, cv::CALIB_FIX_K3,
+    criteria);  // 由于视场角较小，不需要考虑k3
+
+  // 重投影误差
+  double error_sum = 0;
+  size_t total_points = 0;
+  for (size_t i = 0; i < obj_points.size(); i++) {
+    std::vector<cv::Point2f> reprojected_points;
+    cv::projectPoints(
+      obj_points[i], rvecs[i], tvecs[i], camera_matrix, distort_coeffs, reprojected_points);
+
+    total_points += reprojected_points.size();
+    for (size_t j = 0; j < reprojected_points.size(); j++)
+      error_sum += cv::norm(img_points[i][j] - reprojected_points[j]);
+  }
+  auto error = error_sum / total_points;
+
+  // 输出yaml
+  print_yaml(camera_matrix, distort_coeffs, error);
+}

calibration/calibrate_handeye.cpp

@@ -0,0 +1,156 @@
+#include <fmt/core.h>
+#include <yaml-cpp/yaml.h>
+
+#include <Eigen/Dense>  // 必须在opencv2/core/eigen.hpp上面
+#include <fstream>
+#include <opencv2/core/eigen.hpp>
+#include <opencv2/opencv.hpp>
+
+#include "calibration/board_pattern.hpp"
+#include "src/component/img_tools.hpp"
+#include "src/component/math_tools.hpp"
+
+const std::string keys =
+  "{help h usage ? |                          | 输出命令行参数说明}"
+  "{config-path c  | configs/calibration.yaml | yaml配置文件路径 }"
+  "{@input-folder  | assets/img_with_q        | 输入文件夹路径   }";
+
+Eigen::Quaterniond read_q(const std::string & q_path)
+{
+  std::ifstream q_file(q_path);
+  double w, x, y, z;
+  q_file >> w >> x >> y >> z;
+  return {w, x, y, z};
+}
+
+void load(
+  const std::string & input_folder, const std::string & config_path,
+  std::vector<double> & R_gimbal2imubody_data, std::vector<cv::Mat> & R_gimbal2world_list,
+  std::vector<cv::Mat> & t_gimbal2world_list, std::vector<cv::Mat> & rvecs,
+  std::vector<cv::Mat> & tvecs)
+{
+  // 读取yaml参数
+  auto yaml = YAML::LoadFile(config_path);
+  auto board_pattern = calibration::load_board_pattern(yaml);
+  R_gimbal2imubody_data = yaml["R_gimbal2imubody"].as<std::vector<double>>();
+  auto camera_matrix_data = yaml["camera_matrix"].as<std::vector<double>>();
+  auto distort_coeffs_data = yaml["distort_coeffs"].as<std::vector<double>>();
+
+  Eigen::Matrix<double, 3, 3, Eigen::RowMajor> R_gimbal2imubody(R_gimbal2imubody_data.data());
+  cv::Matx33d camera_matrix(camera_matrix_data.data());
+  cv::Mat distort_coeffs(distort_coeffs_data);
+
+  for (int i = 1; true; i++) {
+    // 读取图片和对应四元数
+    auto img_path = fmt::format("{}/{}.jpg", input_folder, i);
+    auto q_path = fmt::format("{}/{}.txt", input_folder, i);
+    auto img = cv::imread(img_path);
+    Eigen::Quaterniond q = read_q(q_path);
+    if (img.empty()) break;
+
+    // 计算云台的欧拉角
+    Eigen::Matrix3d R_imubody2imuabs = q.toRotationMatrix();
+    Eigen::Matrix3d R_gimbal2world =
+      R_gimbal2imubody.transpose() * R_imubody2imuabs * R_gimbal2imubody;
+    Eigen::Vector3d ypr = component::eulers(R_gimbal2world, 2, 1, 0) * 57.3;  // degree
+
+    // 在图片上显示云台的欧拉角，用来检验R_gimbal2imubody是否正确
+    auto drawing = img.clone();
+    component::draw_text(drawing, fmt::format("yaw   {:.2f}", ypr[0]), {40, 40}, {0, 0, 255});
+    component::draw_text(drawing, fmt::format("pitch {:.2f}", ypr[1]), {40, 80}, {0, 0, 255});
+    component::draw_text(drawing, fmt::format("roll  {:.2f}", ypr[2]), {40, 120}, {0, 0, 255});
+
+    // 识别标定板
+    std::vector<cv::Point2f> centers_2d;
+    auto success = calibration::find_pattern_points(img, board_pattern, centers_2d);
+
+    // 显示识别结果
+    cv::drawChessboardCorners(drawing, board_pattern.pattern_size, centers_2d, success);
+    cv::resize(drawing, drawing, {}, 0.5, 0.5);  // 显示时缩小图片尺寸
+    cv::imshow("Press any to continue", drawing);
+    cv::waitKey(0);
+
+    // 输出识别结果
+    fmt::print("[{}] {}\n", success ? "success" : "failure", img_path);
+    if (!success) continue;
+
+    // 计算所需的数据
+    cv::Mat t_gimbal2world = (cv::Mat_<double>(3, 1) << 0, 0, 0);
+    cv::Mat R_gimbal2world_cv;
+    cv::eigen2cv(R_gimbal2world, R_gimbal2world_cv);
+    cv::Mat rvec, tvec;
+    auto centers_3d_ =
+      calibration::planar_points(board_pattern.pattern_size, board_pattern.center_distance_mm);
+    cv::solvePnP(
+      centers_3d_, centers_2d, camera_matrix, distort_coeffs, rvec, tvec, false, cv::SOLVEPNP_IPPE);
+
+    // 记录所需的数据
+    R_gimbal2world_list.emplace_back(R_gimbal2world_cv);
+    t_gimbal2world_list.emplace_back(t_gimbal2world);
+    rvecs.emplace_back(rvec);
+    tvecs.emplace_back(tvec);
+  }
+}
+
+void print_yaml(
+  const std::vector<double> & R_gimbal2imubody_data, const cv::Mat & R_camera2gimbal,
+  const cv::Mat & t_camera2gimbal, const Eigen::Vector3d & ypr)
+{
+  YAML::Emitter result;
+  std::vector<double> R_camera2gimbal_data(
+    R_camera2gimbal.begin<double>(), R_camera2gimbal.end<double>());
+  std::vector<double> t_camera2gimbal_data(
+    t_camera2gimbal.begin<double>(), t_camera2gimbal.end<double>());
+
+  result << YAML::BeginMap;
+  result << YAML::Key << "R_gimbal2imubody";
+  result << YAML::Value << YAML::Flow << R_gimbal2imubody_data;
+  result << YAML::Newline;
+  result << YAML::Newline;
+  result << YAML::Comment(fmt::format(
+    "相机同理想情况的偏角: yaw{:.2f} pitch{:.2f} roll{:.2f} degree", ypr[0], ypr[1], ypr[2]));
+  result << YAML::Key << "R_camera2gimbal";
+  result << YAML::Value << YAML::Flow << R_camera2gimbal_data;
+  result << YAML::Key << "t_camera2gimbal";
+  result << YAML::Value << YAML::Flow << t_camera2gimbal_data;
+  result << YAML::Newline;
+  result << YAML::EndMap;
+
+  fmt::print("\n{}\n", result.c_str());
+}
+
+int main(int argc, char * argv[])
+{
+  // 读取命令行参数
+  cv::CommandLineParser cli(argc, argv, keys);
+  if (cli.has("help")) {
+    cli.printMessage();
+    return 0;
+  }
+  auto input_folder = cli.get<std::string>(0);
+  auto config_path = cli.get<std::string>("config-path");
+
+  // 从输入文件夹中加载标定所需的数据
+  std::vector<double> R_gimbal2imubody_data;
+  std::vector<cv::Mat> R_gimbal2world_list, t_gimbal2world_list;
+  std::vector<cv::Mat> rvecs, tvecs;
+  load(
+    input_folder, config_path, R_gimbal2imubody_data, R_gimbal2world_list, t_gimbal2world_list,
+    rvecs, tvecs);
+
+  // 手眼标定
+  cv::Mat R_camera2gimbal, t_camera2gimbal;
+  cv::calibrateHandEye(
+    R_gimbal2world_list, t_gimbal2world_list, rvecs, tvecs, R_camera2gimbal, t_camera2gimbal);
+  t_camera2gimbal /= 1e3;  // mm to m
+
+  // 计算相机同理想情况的偏角
+  Eigen::Matrix3d R_camera2gimbal_eigen;
+  cv::cv2eigen(R_camera2gimbal, R_camera2gimbal_eigen);
+  Eigen::Matrix3d R_gimbal2ideal{{0, -1, 0}, {0, 0, -1}, {1, 0, 0}};
+  Eigen::Matrix3d R_camera2ideal = R_gimbal2ideal * R_camera2gimbal_eigen;
+  Eigen::Vector3d ypr = component::eulers(R_camera2ideal, 1, 0, 2) * 57.3;  // degree
+
+  // 输出yaml
+  print_yaml(R_gimbal2imubody_data, R_camera2gimbal, t_camera2gimbal, ypr);
+}

calibration/calibrate_robotworld_handeye.cpp

@@ -0,0 +1,203 @@
+#include <fmt/core.h>
+#include <yaml-cpp/yaml.h>
+
+#include <Eigen/Dense>  // 必须在opencv2/core/eigen.hpp上面
+#include <fstream>
+#include <opencv2/core/eigen.hpp>
+#include <opencv2/opencv.hpp>
+
+#include "calibration/board_pattern.hpp"
+#include "src/component/img_tools.hpp"
+#include "src/component/math_tools.hpp"
+
+const std::string keys =
+  "{help h usage ? |                          | 输出命令行参数说明}"
+  "{config-path c  | configs/calibration.yaml | yaml配置文件路径 }"
+  "{@input-folder  | assets/img_with_q        | 输入文件夹路径   }";
+
+std::vector<cv::Point3f> centers_3d(const cv::Size & pattern_size, const float center_distance)
+{
+  std::vector<cv::Point3f> centers_3d;
+
+  for (int i = 0; i < pattern_size.height; i++) {
+    for (int j = 0; j < pattern_size.width; j++) {
+      float x = 0;
+      float y = (-j + 0.5 * pattern_size.width) * center_distance;
+      float z = (-i + 0.5 * pattern_size.height) * center_distance;
+      centers_3d.push_back({x, y, z});
+    }
+  }
+
+  return centers_3d;
+}
+
+Eigen::Quaterniond read_q(const std::string & q_path)
+{
+  std::ifstream q_file(q_path);
+  double w, x, y, z;
+  q_file >> w >> x >> y >> z;
+  return {w, x, y, z};
+}
+
+void load(
+  const std::string & input_folder, const std::string & config_path,
+  std::vector<double> & R_gimbal2imubody_data, std::vector<cv::Mat> & R_world2gimbal_list,
+  std::vector<cv::Mat> & t_world2gimbal_list, std::vector<cv::Mat> & rvecs,
+  std::vector<cv::Mat> & tvecs)
+{
+  // 读取yaml参数
+  auto yaml = YAML::LoadFile(config_path);
+  auto board_pattern = calibration::load_board_pattern(yaml);
+  R_gimbal2imubody_data = yaml["R_gimbal2imubody"].as<std::vector<double>>();
+  auto camera_matrix_data = yaml["camera_matrix"].as<std::vector<double>>();
+  auto distort_coeffs_data = yaml["distort_coeffs"].as<std::vector<double>>();
+
+  Eigen::Matrix<double, 3, 3, Eigen::RowMajor> R_gimbal2imubody(R_gimbal2imubody_data.data());
+  cv::Matx33d camera_matrix(camera_matrix_data.data());
+  cv::Mat distort_coeffs(distort_coeffs_data);
+
+  for (int i = 1; true; i++) {
+    // 读取图片和对应四元数
+    auto img_path = fmt::format("{}/{}.jpg", input_folder, i);
+    auto q_path = fmt::format("{}/{}.txt", input_folder, i);
+    auto img = cv::imread(img_path);
+    Eigen::Quaterniond q = read_q(q_path);
+    if (img.empty()) break;
+
+    // 计算云台的欧拉角
+    Eigen::Matrix3d R_imubody2imuabs = q.toRotationMatrix();
+    Eigen::Matrix3d R_gimbal2world =
+      R_gimbal2imubody.transpose() * R_imubody2imuabs * R_gimbal2imubody;
+    Eigen::Vector3d ypr = component::eulers(R_gimbal2world, 2, 1, 0) * 57.3;  // degree
+
+    // 在图片上显示云台的欧拉角，用来检验R_gimbal2imubody是否正确
+    auto drawing = img.clone();
+    component::draw_text(drawing, fmt::format("yaw   {:.2f}", ypr[0]), {40, 40}, {0, 0, 255});
+    component::draw_text(drawing, fmt::format("pitch {:.2f}", ypr[1]), {40, 80}, {0, 0, 255});
+    component::draw_text(drawing, fmt::format("roll  {:.2f}", ypr[2]), {40, 120}, {0, 0, 255});
+
+    // 识别标定板
+    std::vector<cv::Point2f> centers_2d;
+    auto success = calibration::find_pattern_points(img, board_pattern, centers_2d);
+
+    // 显示识别结果
+    cv::drawChessboardCorners(drawing, board_pattern.pattern_size, centers_2d, success);
+    cv::resize(drawing, drawing, {}, 0.5, 0.5);  // 显示时缩小图片尺寸
+    cv::imshow("Press any to continue", drawing);
+    cv::waitKey(0);
+
+    // 输出识别结果
+    fmt::print("[{}] {}\n", success ? "success" : "failure", img_path);
+    if (!success) continue;
+
+    // 计算所需的数据
+    Eigen::Matrix3d R_world2gimbal = R_gimbal2world.transpose();
+    cv::Mat t_world2gimbal = (cv::Mat_<double>(3, 1) << 0, 0, 0);
+    cv::Mat R_world2gimbal_cv;
+    cv::eigen2cv(R_world2gimbal, R_world2gimbal_cv);
+    cv::Mat rvec, tvec;
+    auto centers_3d_ =
+      centers_3d(board_pattern.pattern_size, board_pattern.center_distance_mm);
+    cv::solvePnP(
+      centers_3d_, centers_2d, camera_matrix, distort_coeffs, rvec, tvec, false, cv::SOLVEPNP_IPPE);
+
+    // 记录所需的数据
+    R_world2gimbal_list.emplace_back(R_world2gimbal_cv);
+    t_world2gimbal_list.emplace_back(t_world2gimbal);
+    rvecs.emplace_back(rvec);
+    tvecs.emplace_back(tvec);
+  }
+}
+
+void print_yaml(
+  const std::vector<double> & R_gimbal2imubody_data, const cv::Mat & R_camera2gimbal,
+  const cv::Mat & t_camera2gimbal, const Eigen::Vector3d & camera_ypr, double distance,
+  const Eigen::Vector3d & board_ypr)
+{
+  YAML::Emitter result;
+  std::vector<double> R_camera2gimbal_data(
+    R_camera2gimbal.begin<double>(), R_camera2gimbal.end<double>());
+  std::vector<double> t_camera2gimbal_data(
+    t_camera2gimbal.begin<double>(), t_camera2gimbal.end<double>());
+
+  result << YAML::BeginMap;
+  result << YAML::Key << "R_gimbal2imubody";
+  result << YAML::Value << YAML::Flow << R_gimbal2imubody_data;
+  result << YAML::Newline;
+  result << YAML::Newline;
+  result << YAML::Comment(fmt::format(
+    "相机同理想情况的偏角: yaw{:.2f} pitch{:.2f} roll{:.2f} degree", camera_ypr[0], camera_ypr[1],
+    camera_ypr[2]));
+  result << YAML::Newline;
+  result << YAML::Comment(fmt::format("标定板到世界坐标系原点的水平距离: {:.2f} m", distance));
+  result << YAML::Newline;
+  result << YAML::Comment(fmt::format(
+    "标定板同竖直摆放时的偏角: yaw{:.2f} pitch{:.2f} roll{:.2f} degree", board_ypr[0], board_ypr[1],
+    board_ypr[2]));
+  result << YAML::Key << "R_camera2gimbal";
+  result << YAML::Value << YAML::Flow << R_camera2gimbal_data;
+  result << YAML::Key << "t_camera2gimbal";
+  result << YAML::Value << YAML::Flow << t_camera2gimbal_data;
+  result << YAML::Newline;
+  result << YAML::EndMap;
+
+  fmt::print("\n{}\n", result.c_str());
+}
+
+int main(int argc, char * argv[])
+{
+  // 读取命令行参数
+  cv::CommandLineParser cli(argc, argv, keys);
+  if (cli.has("help")) {
+    cli.printMessage();
+    return 0;
+  }
+  auto input_folder = cli.get<std::string>(0);
+  auto config_path = cli.get<std::string>("config-path");
+
+  // 从输入文件夹中加载标定所需的数据
+  std::vector<double> R_gimbal2imubody_data;
+  std::vector<cv::Mat> R_world2gimbal_list, t_world2gimbal_list;
+  std::vector<cv::Mat> rvecs, tvecs;
+  load(
+    input_folder, config_path, R_gimbal2imubody_data, R_world2gimbal_list, t_world2gimbal_list,
+    rvecs, tvecs);
+
+  // 手眼标定
+  cv::Mat R_gimbal2camera, t_gimbal2camera;
+  cv::Mat R_world2board, t_world2board;
+  cv::calibrateRobotWorldHandEye(
+    rvecs, tvecs, R_world2gimbal_list, t_world2gimbal_list, R_world2board, t_world2board,
+    R_gimbal2camera, t_gimbal2camera);
+  t_gimbal2camera /= 1e3;  // mm to m
+  t_world2board /= 1e3;    // mm to m
+
+  // 计算所需的数据
+  cv::Mat R_camera2gimbal, t_camera2gimbal;
+  cv::Mat R_board2world, t_board2world;
+  cv::transpose(R_gimbal2camera, R_camera2gimbal);
+  cv::transpose(R_world2board, R_board2world);
+  t_camera2gimbal = -R_camera2gimbal * t_gimbal2camera;
+  t_board2world = -R_board2world * t_world2board;
+
+  // 计算相机同理想情况的偏角
+  Eigen::Matrix3d R_camera2gimbal_eigen;
+  cv::cv2eigen(R_camera2gimbal, R_camera2gimbal_eigen);
+  Eigen::Matrix3d R_gimbal2ideal{{0, -1, 0}, {0, 0, -1}, {1, 0, 0}};
+  Eigen::Matrix3d R_camera2ideal = R_gimbal2ideal * R_camera2gimbal_eigen;
+  Eigen::Vector3d camera_ypr = component::eulers(R_camera2ideal, 1, 0, 2) * 57.3;  // degree
+
+  // 计算标定板到世界坐标系原点的水平距离
+  auto x = t_board2world.at<double>(0);
+  auto y = t_board2world.at<double>(1);
+  auto distance = std::sqrt(x * x + y * y);
+
+  // 计算标定板同竖直摆放时的偏角
+  Eigen::Matrix3d R_board2world_eigen;
+  cv::cv2eigen(R_board2world, R_board2world_eigen);
+  Eigen::Vector3d board_ypr = component::eulers(R_board2world_eigen, 2, 1, 0) * 57.3;  // degree
+
+  // 输出yaml
+  print_yaml(
+    R_gimbal2imubody_data, R_camera2gimbal, t_camera2gimbal, camera_ypr, distance, board_ypr);
+}

calibration/capture.cpp

@@ -0,0 +1,125 @@
+#include <fmt/core.h>
+#include <yaml-cpp/yaml.h>
+
+#include <filesystem>
+#include <fstream>
+#include <opencv2/opencv.hpp>
+
+#include "calibration/board_pattern.hpp"
+#include "src/component/img_tools.hpp"
+#include "src/component/logger.hpp"
+#include "src/component/math_tools.hpp"
+#include "src/device/camera.hpp"
+#include "src/device/gimbal/gimbal.hpp"
+
+const std::string keys =
+  "{help h usage ?  |                          | 输出命令行参数说明}"
+  "{@config-path c  | configs/calibration.yaml | yaml配置文件路径 }"
+  "{output-folder o |      assets/img_with_q   | 输出文件夹路径   }";
+
+void write_q(const std::string q_path, const Eigen::Quaterniond & q)
+{
+  std::ofstream q_file(q_path);
+  Eigen::Vector4d xyzw = q.coeffs();
+  // 输出顺序为wxyz
+  q_file << fmt::format("{} {} {} {}", xyzw[3], xyzw[0], xyzw[1], xyzw[2]);
+  q_file.close();
+}
+
+void capture_loop(
+  const std::string & config_path, const std::string & output_folder)
+{
+  // 从配置文件加载标定板参数（支持 circles_grid 和 chessboard）
+  auto yaml = YAML::LoadFile(config_path);
+  auto board_pattern = calibration::load_board_pattern(yaml);
+
+  device::Gimbal gimbal(config_path);
+  device::Camera camera(config_path);
+  cv::Mat img;
+  std::chrono::steady_clock::time_point timestamp;
+
+  int count = 0;
+  while (true) {
+    camera.read(img, timestamp);
+    Eigen::Quaterniond q = gimbal.q(timestamp);
+
+    // 在图像上显示欧拉角，用来判断imuabs系的xyz正方向，同时判断imu是否存在零漂
+    auto img_with_ypr = img.clone();
+    Eigen::Vector3d zyx = component::eulers(q, 2, 1, 0) * 57.3;  // degree
+    component::draw_text(img_with_ypr, fmt::format("Z {:.2f}", zyx[0]), {40, 40}, {0, 0, 255});
+    component::draw_text(img_with_ypr, fmt::format("Y {:.2f}", zyx[1]), {40, 80}, {0, 0, 255});
+    component::draw_text(img_with_ypr, fmt::format("X {:.2f}", zyx[2]), {40, 120}, {0, 0, 255});
+
+    std::vector<cv::Point2f> centers_2d;
+    bool success;
+    if (board_pattern.pattern_type == calibration::PatternType::chessboard) {
+      // 棋盘格检测很慢，先在缩小图上快速检测，再映射回原图做亚像素精化
+      cv::Mat small;
+      double scale = 0.5;
+      cv::resize(img, small, {}, scale, scale);
+      std::vector<cv::Point2f> small_pts;
+      success = calibration::find_pattern_points(small, board_pattern, small_pts);
+      if (success) {
+        for (auto & p : small_pts) { p.x /= scale; p.y /= scale; }
+        cv::Mat gray;
+        cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
+        cv::cornerSubPix(
+          gray, small_pts, cv::Size(11, 11), cv::Size(-1, -1),
+          cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, 1e-3));
+        centers_2d = std::move(small_pts);
+      }
+    } else {
+      success = calibration::find_pattern_points(img, board_pattern, centers_2d);
+    }
+    cv::drawChessboardCorners(img_with_ypr, board_pattern.pattern_size, centers_2d, success);
+    cv::resize(img_with_ypr, img_with_ypr, {}, 0.5, 0.5);  // 显示时缩小图片尺寸
+
+    // 按“s”保存图片和对应四元数，按“q”退出程序
+    cv::imshow("Press s to save, q to quit", img_with_ypr);
+    auto key = cv::waitKey(1);
+    if (key == 'q')
+      break;
+    else if (key != 's')
+      continue;
+
+    // 保存图片和四元数
+    count++;
+    auto img_path = fmt::format("{}/{}.jpg", output_folder, count);
+    auto q_path = fmt::format("{}/{}.txt", output_folder, count);
+    cv::imwrite(img_path, img);
+    write_q(q_path, q);
+    component::logger()->info("[{}] Saved in {}", count, output_folder);
+  }
+
+  // 离开该作用域时，camera和gimbal会自动关闭
+}
+
+int main(int argc, char * argv[])
+{
+  // 读取命令行参数
+  cv::CommandLineParser cli(argc, argv, keys);
+  if (cli.has("help")) {
+    cli.printMessage();
+    return 0;
+  }
+  auto config_path = cli.get<std::string>(0);
+  auto output_folder = cli.get<std::string>("output-folder");
+
+  // 新建输出文件夹
+  std::filesystem::create_directory(output_folder);
+
+  // 从配置文件读取标定板类型和尺寸
+  auto yaml = YAML::LoadFile(config_path);
+  auto board_pattern = calibration::load_board_pattern(yaml);
+  component::logger()->info(
+    "标定板类型: {}, 尺寸: {}列{}行",
+    calibration::pattern_name(board_pattern.pattern_type),
+    board_pattern.pattern_size.width, board_pattern.pattern_size.height);
+
+  // 主循环，保存图片和对应四元数
+  capture_loop(config_path, output_folder);
+
+  component::logger()->warn("注意四元数输出顺序为wxyz");
+
+  return 0;
+}

calibration/capture_ros.cpp

@@ -0,0 +1,164 @@
+#ifdef USE_ROS2
+
+#include <fmt/core.h>
+#include <yaml-cpp/yaml.h>
+
+#include <filesystem>
+#include <fstream>
+#include <memory>
+#include <opencv2/opencv.hpp>
+#include <thread>
+
+#include "rclcpp/rclcpp.hpp"
+#include "calibration/board_pattern.hpp"
+#include "src/component/img_tools.hpp"
+#include "src/component/logger.hpp"
+#include "src/component/math_tools.hpp"
+#include "src/device/camera.hpp"
+#include "src/device/gimbal/gimbal_ros.hpp"
+
+const std::string keys =
+  "{help h usage ?  |                          | 输出命令行参数说明}"
+  "{@config-path c  | configs/calibration.yaml | yaml配置文件路径 }"
+  "{output-folder o |      assets/img_with_q   | 输出文件夹路径   }";
+
+void write_q(const std::string q_path, const Eigen::Quaterniond & q)
+{
+  std::ofstream q_file(q_path);
+  Eigen::Vector4d xyzw = q.coeffs();
+  // 输出顺序为wxyz
+  q_file << fmt::format("{} {} {} {}", xyzw[3], xyzw[0], xyzw[1], xyzw[2]);
+  q_file.close();
+}
+
+void capture_loop(
+  const std::string & config_path, const std::string & output_folder,
+  std::shared_ptr<rclcpp::Node> node)
+{
+  // 从配置文件加载标定板参数（支持 circles_grid 和 chessboard）
+  auto yaml = YAML::LoadFile(config_path);
+  auto board_pattern = calibration::load_board_pattern(yaml);
+
+  device::GimbalROS gimbal(config_path, node);
+  device::Camera camera(config_path);
+  cv::Mat img;
+  std::chrono::steady_clock::time_point timestamp;
+
+  // ROS2 spin线程
+  std::atomic<bool> quit = false;
+  auto ros_thread = std::thread([&]() {
+    while (!quit && rclcpp::ok()) {
+      rclcpp::spin_some(node);
+      std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+  });
+
+  int count = 0;
+  component::logger()->info("[CaptureROS] Started with ROS2 communication");
+  component::logger()->info("[CaptureROS] Press 's' to save, 'q' to quit");
+
+  while (rclcpp::ok()) {
+    camera.read(img, timestamp);
+    Eigen::Quaterniond q = gimbal.q(timestamp);
+
+    // 在图像上显示欧拉角，用来判断imuabs系的xyz正方向，同时判断imu是否存在零漂
+    auto img_with_ypr = img.clone();
+    Eigen::Vector3d zyx = component::eulers(q, 2, 1, 0) * 57.3;  // degree
+    component::draw_text(img_with_ypr, fmt::format("Z {:.2f}", zyx[0]), {40, 40}, {0, 0, 255});
+    component::draw_text(img_with_ypr, fmt::format("Y {:.2f}", zyx[1]), {40, 80}, {0, 0, 255});
+    component::draw_text(img_with_ypr, fmt::format("X {:.2f}", zyx[2]), {40, 120}, {0, 0, 255});
+
+    std::vector<cv::Point2f> centers_2d;
+    bool success;
+    if (board_pattern.pattern_type == calibration::PatternType::chessboard) {
+      // 棋盘格检测很慢，先在缩小图上快速检测，再映射回原图做亚像素精化
+      cv::Mat small;
+      double scale = 0.5;
+      cv::resize(img, small, {}, scale, scale);
+      std::vector<cv::Point2f> small_pts;
+      success = calibration::find_pattern_points(small, board_pattern, small_pts);
+      if (success) {
+        for (auto & p : small_pts) { p.x /= scale; p.y /= scale; }
+        cv::Mat gray;
+        cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
+        cv::cornerSubPix(
+          gray, small_pts, cv::Size(11, 11), cv::Size(-1, -1),
+          cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, 1e-3));
+        centers_2d = std::move(small_pts);
+      }
+    } else {
+      success = calibration::find_pattern_points(img, board_pattern, centers_2d);
+    }
+    cv::drawChessboardCorners(img_with_ypr, board_pattern.pattern_size, centers_2d, success);
+    cv::resize(img_with_ypr, img_with_ypr, {}, 0.5, 0.5);  // 显示时缩小图片尺寸
+
+    // 按"s"保存图片和对应四元数，按"q"退出程序
+    cv::imshow("Press s to save, q to quit", img_with_ypr);
+    auto key = cv::waitKey(1);
+    if (key == 'q')
+      break;
+    else if (key != 's')
+      continue;
+
+    // 保存图片和四元数
+    count++;
+    auto img_path = fmt::format("{}/{}.jpg", output_folder, count);
+    auto q_path = fmt::format("{}/{}.txt", output_folder, count);
+    cv::imwrite(img_path, img);
+    write_q(q_path, q);
+    component::logger()->info("[{}] Saved in {}", count, output_folder);
+  }
+
+  quit = true;
+  if (ros_thread.joinable()) ros_thread.join();
+
+  // 离开该作用域时，camera和gimbal会自动关闭
+}
+
+int main(int argc, char * argv[])
+{
+  // 读取命令行参数
+  cv::CommandLineParser cli(argc, argv, keys);
+  if (cli.has("help")) {
+    cli.printMessage();
+    return 0;
+  }
+  auto config_path = cli.get<std::string>(0);
+  auto output_folder = cli.get<std::string>("output-folder");
+
+  // 初始化ROS2
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<rclcpp::Node>("capture_ros");
+
+  // 新建输出文件夹
+  std::filesystem::create_directory(output_folder);
+
+  // 从配置文件读取标定板类型和尺寸
+  auto yaml = YAML::LoadFile(config_path);
+  auto board_pattern = calibration::load_board_pattern(yaml);
+  component::logger()->info(
+    "标定板类型: {}, 尺寸: {}列{}行",
+    calibration::pattern_name(board_pattern.pattern_type),
+    board_pattern.pattern_size.width, board_pattern.pattern_size.height);
+
+  // 主循环，保存图片和对应四元数
+  capture_loop(config_path, output_folder, node);
+
+  component::logger()->warn("注意四元数输出顺序为wxyz");
+
+  rclcpp::shutdown();
+
+  return 0;
+}
+
+#else
+
+#include <iostream>
+
+int main()
+{
+  std::cerr << "Error: This program requires ROS2 support. Please build with ROS2 enabled." << std::endl;
+  return 1;
+}
+
+#endif  // USE_ROS2

calibration/split_video.cpp

@@ -0,0 +1,89 @@
+#include <fmt/format.h>
+
+#include <chrono>
+#include <fstream>
+#include <iomanip>
+#include <sstream>
+#include <string>
+
+#include "src/component/exiter.hpp"
+#include "src/component/img_tools.hpp"
+#include "src/component/logger.hpp"
+#include "src/component/math_tools.hpp"
+
+// 定义命令行参数
+const std::string keys =
+  "{help h usage ? |                        | 输出命令行参数说明 }"
+  "{start-index s  |                        | 视频起始帧下标    }"
+  "{end-index e    |                        | 视频结束帧下标    }"
+  "{output-path p  | records/Big/2024-05-14_11-6-26     | avi和txt文件的路径}"
+  "{@input-path    |                        | avi和txt文件的路径}";
+
+int main(int argc, char * argv[])
+{
+  // 读取命令行参数
+  cv::CommandLineParser cli(argc, argv, keys);
+  if (cli.has("help")) {
+    cli.printMessage();
+    return 0;
+  }
+  auto input_path = cli.get<std::string>(0);
+  auto output_path = cli.get<std::string>("output-path");
+  auto start_index = cli.get<int>("start-index");
+  std::cout << start_index << std::endl;
+  auto end_index = cli.get<int>("end-index");
+
+  // 初始化绘图器和退出器
+  component::Exiter exiter;
+
+  // 构造视频和文本文件路径
+  auto video_path = fmt::format("{}.avi", input_path);
+  auto text_path = fmt::format("{}.txt", input_path);
+  cv::VideoCapture video(video_path);
+  std::ifstream text(text_path);
+
+  // 设置视频起始帧
+  video.set(cv::CAP_PROP_POS_FRAMES, start_index);
+  for (int i = 0; i < start_index; i++) {
+    double t, w, x, y, z;
+    text >> t >> w >> x >> y >> z;
+  }
+  // 获取原始视频的参数
+  int frameWidth = video.get(cv::CAP_PROP_FRAME_WIDTH);
+  int frameHeight = video.get(cv::CAP_PROP_FRAME_HEIGHT);
+  double fps = video.get(cv::CAP_PROP_FPS);
+  int fourcc = static_cast<int>(video.get(cv::CAP_PROP_FOURCC));
+
+  // 创建输出文件
+  auto outvideo_path = fmt::format("{}.avi", output_path);
+  auto outtext_path = fmt::format("{}.txt", output_path);
+  cv::VideoWriter outvideo(outvideo_path, fourcc, fps, cv::Size(frameWidth, frameHeight));
+  std::ofstream outtext(outtext_path);
+
+  std::string line;
+  cv::Mat img;
+
+  // 循环处理视频帧
+  for (int frame_count = start_index; !exiter.exit(); frame_count++) {
+    if (end_index > 0 && frame_count > end_index) break;
+
+    video.read(img);
+    if (img.empty()) break;
+
+    outvideo.write(img);
+
+    getline(text, line);
+    outtext << line << std::endl;
+
+    cv::resize(img, img, cv::Size(img.size().width * 0.8, img.size().height * 0.8));
+    cv::imshow("result", img);
+
+    int key = cv::waitKey(1);
+    if (key == 'q') break;
+  }
+  cv::destroyAllWindows();
+  text.close();  // 关闭文件
+  outtext.close();
+
+  return 0;
+}

calibration/test/camera_calibration.py

@@ -0,0 +1,184 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+相机校准应用程序
+使用检测到的棋盘格参数进行图像矫正和去畸变
+"""
+
+import cv2
+import numpy as np
+import json
+import os
+
+
+class CameraCalibration:
+    def __init__(self, calibration_file='chessboard_detection_output/calibration_result.json'):
+        """
+        加载校准参数
+
+        Args:
+            calibration_file: 校准结果JSON文件路径
+        """
+        self.calibration_file = calibration_file
+        self.camera_matrix = None
+        self.dist_coeffs = None
+        self.load_calibration()
+
+    def load_calibration(self):
+        """从JSON文件加载校准参数"""
+        if not os.path.exists(self.calibration_file):
+            raise FileNotFoundError(f"校准文件不存在: {self.calibration_file}")
+
+        with open(self.calibration_file, 'r', encoding='utf-8') as f:
+            data = json.load(f)
+
+        self.camera_matrix = np.array(data['camera_matrix'])
+        self.dist_coeffs = np.array(data['distortion_coefficients'])
+
+        print("✓ 校准参数加载成功")
+        print(f"  重投影误差: {data['reprojection_error']:.4f} 像素")
+        print(f"  使用图像数: {data['num_images']}")
+
+    def undistort_image(self, image):
+        """
+        对图像进行去畸变处理
+
+        Args:
+            image: 输入图像
+
+        Returns:
+            undistorted: 去畸变后的图像
+        """
+        h, w = image.shape[:2]
+        new_camera_matrix, roi = cv2.getOptimalNewCameraMatrix(
+            self.camera_matrix, self.dist_coeffs, (w, h), 1, (w, h)
+        )
+
+        # 去畸变
+        undistorted = cv2.undistort(image, self.camera_matrix, self.dist_coeffs, None, new_camera_matrix)
+
+        # 裁剪图像
+        x, y, w, h = roi
+        undistorted = undistorted[y:y+h, x:x+w]
+
+        return undistorted
+
+    def undistort_video(self, input_video, output_video='undistorted_video.avi'):
+        """
+        对视频进行去畸变处理
+
+        Args:
+            input_video: 输入视频路径
+            output_video: 输出视频路径
+        """
+        cap = cv2.VideoCapture(input_video)
+        if not cap.isOpened():
+            print(f"无法打开视频: {input_video}")
+            return
+
+        # 获取视频参数
+        fps = cap.get(cv2.CAP_PROP_FPS)
+        total_frames = int(cap.get(cv2.CAP_PROP_FRAME_COUNT))
+
+        # 读取第一帧获取尺寸
+        ret, frame = cap.read()
+        if not ret:
+            print("无法读取视频帧")
+            return
+
+        h, w = frame.shape[:2]
+        new_camera_matrix, roi = cv2.getOptimalNewCameraMatrix(
+            self.camera_matrix, self.dist_coeffs, (w, h), 1, (w, h)
+        )
+        x, y, w_roi, h_roi = roi
+
+        # 创建视频写入器
+        fourcc = cv2.VideoWriter_fourcc(*'XVID')
+        out = cv2.VideoWriter(output_video, fourcc, fps, (w_roi, h_roi))
+
+        print(f"开始处理视频 (共 {total_frames} 帧)...")
+
+        # 重置到开头
+        cap.set(cv2.CAP_PROP_POS_FRAMES, 0)
+        frame_count = 0
+
+        while True:
+            ret, frame = cap.read()
+            if not ret:
+                break
+
+            frame_count += 1
+
+            # 去畸变
+            undistorted = cv2.undistort(frame, self.camera_matrix, self.dist_coeffs, None, new_camera_matrix)
+            undistorted = undistorted[y:y+h_roi, x:x+w_roi]
+
+            out.write(undistorted)
+
+            if frame_count % 50 == 0:
+                print(f"  处理进度: {frame_count}/{total_frames} ({100*frame_count/total_frames:.1f}%)")
+
+        cap.release()
+        out.release()
+
+        print(f"\n✓ 视频处理完成，已保存到: {output_video}")
+
+    def compare_images(self, image_path, output_path='comparison.jpg'):
+        """
+        生成原始图像和去畸变图像的对比图
+
+        Args:
+            image_path: 输入图像路径
+            output_path: 输出对比图路径
+        """
+        image = cv2.imread(image_path)
+        if image is None:
+            print(f"无法读取图像: {image_path}")
+            return
+
+        undistorted = self.undistort_image(image)
+
+        # 调整尺寸以便并排显示
+        h1, w1 = image.shape[:2]
+        h2, w2 = undistorted.shape[:2]
+        h = min(h1, h2)
+        image_resized = cv2.resize(image, (int(w1 * h / h1), h))
+        undistorted_resized = cv2.resize(undistorted, (int(w2 * h / h2), h))
+
+        # 并排拼接
+        comparison = np.hstack([image_resized, undistorted_resized])
+
+        # 添加文字标注
+        cv2.putText(comparison, 'Original', (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 0, 255), 3)
+        cv2.putText(comparison, 'Undistorted', (w1 + 50, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 255, 0), 3)
+
+        cv2.imwrite(output_path, comparison)
+        print(f"✓ 对比图已保存到: {output_path}")
+
+
+def main():
+    """主函数 - 演示如何使用校准参数"""
+    print("=== 相机校准应用程序 ===\n")
+
+    # 加载校准参数
+    calib = CameraCalibration()
+
+    # 示例1: 对检测结果图像进行去畸变
+    output_dir = 'chessboard_detection_output'
+    detected_images = [f for f in os.listdir(output_dir) if f.startswith('detected_') and f.endswith('.jpg')]
+
+    if detected_images:
+        print(f"\n找到 {len(detected_images)} 张检测图像")
+        sample_image = os.path.join(output_dir, detected_images[0])
+        print(f"生成对比图: {sample_image}")
+        calib.compare_images(sample_image, os.path.join(output_dir, 'comparison.jpg'))
+
+    # 示例2: 对原始视频进行去畸变
+    print("\n是否要对原始视频进行去畸变处理？")
+    print("注意: 这将处理整个视频，可能需要一些时间")
+    print("如需处理，请取消注释下面的代码行：")
+    print("# calib.undistort_video('Video_20260303114232727.avi', 'undistorted_video.avi')")
+
+
+if __name__ == '__main__':
+    main()

calibration/test/chessboard_detector.py

@@ -0,0 +1,221 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+棋盘格检测程序
+用于检测视频中的11x8内角点棋盘格，并用于相机校准
+"""
+
+import cv2
+import numpy as np
+import os
+import json
+from datetime import datetime
+
+
+class ChessboardDetector:
+    def __init__(self, pattern_size=(11, 8), square_size=1.0):
+        """
+        初始化棋盘格检测器
+
+        Args:
+            pattern_size: 棋盘格内角点数量 (列, 行)
+            square_size: 棋盘格方格实际尺寸(单位:mm或其他)
+        """
+        self.pattern_size = pattern_size
+        self.square_size = square_size
+
+        # 准备棋盘格的3D坐标点
+        self.objp = np.zeros((pattern_size[0] * pattern_size[1], 3), np.float32)
+        self.objp[:, :2] = np.mgrid[0:pattern_size[0], 0:pattern_size[1]].T.reshape(-1, 2)
+        self.objp *= square_size
+
+        # 存储所有图像的角点
+        self.obj_points = []  # 3D点
+        self.img_points = []  # 2D点
+
+        # 角点检测参数
+        self.criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
+
+    def detect_chessboard(self, image):
+        """
+        检测单张图像中的棋盘格
+
+        Args:
+            image: 输入图像
+
+        Returns:
+            ret: 是否检测成功
+            corners: 角点坐标
+            gray: 灰度图
+        """
+        gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
+
+        # 查找棋盘格角点
+        ret, corners = cv2.findChessboardCorners(gray, self.pattern_size, None)
+
+        if ret:
+            # 亚像素精度优化
+            corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), self.criteria)
+
+        return ret, corners, gray
+
+    def process_video(self, video_path, output_dir='output', sample_interval=30):
+        """
+        处理视频文件，检测棋盘格
+
+        Args:
+            video_path: 视频文件路径
+            output_dir: 输出目录
+            sample_interval: 采样间隔(帧数)
+        """
+        if not os.path.exists(output_dir):
+            os.makedirs(output_dir)
+
+        cap = cv2.VideoCapture(video_path)
+        if not cap.isOpened():
+            print(f"无法打开视频: {video_path}")
+            return False
+
+        total_frames = int(cap.get(cv2.CAP_PROP_FRAME_COUNT))
+        fps = cap.get(cv2.CAP_PROP_FPS)
+
+        print(f"视频信息: 总帧数={total_frames}, FPS={fps}")
+        print(f"开始检测棋盘格 (内角点: {self.pattern_size[0]}x{self.pattern_size[1]})...")
+
+        frame_count = 0
+        detected_count = 0
+
+        while True:
+            ret, frame = cap.read()
+            if not ret:
+                break
+
+            frame_count += 1
+
+            # 按间隔采样
+            if frame_count % sample_interval != 0:
+                continue
+
+            # 检测棋盘格
+            success, corners, gray = self.detect_chessboard(frame)
+
+            if success:
+                detected_count += 1
+                self.obj_points.append(self.objp)
+                self.img_points.append(corners)
+
+                # 绘制角点
+                vis_img = frame.copy()
+                cv2.drawChessboardCorners(vis_img, self.pattern_size, corners, success)
+
+                # 保存结果图像
+                output_path = os.path.join(output_dir, f'detected_{detected_count:03d}_frame{frame_count}.jpg')
+                cv2.imwrite(output_path, vis_img)
+
+                print(f"✓ 帧 {frame_count}: 检测成功 (已保存 {detected_count} 张)")
+            else:
+                print(f"✗ 帧 {frame_count}: 未检测到棋盘格")
+
+        cap.release()
+
+        print(f"\n检测完成: 共处理 {frame_count} 帧, 成功检测 {detected_count} 张图像")
+        return detected_count > 0
+
+    def calibrate_camera(self, image_size):
+        """
+        执行相机校准
+
+        Args:
+            image_size: 图像尺寸 (width, height)
+
+        Returns:
+            ret: 标定误差
+            camera_matrix: 相机内参矩阵
+            dist_coeffs: 畸变系数
+            rvecs: 旋转向量
+            tvecs: 平移向量
+        """
+        if len(self.obj_points) < 3:
+            print("错误: 需要至少3张成功检测的图像进行校准")
+            return None
+
+        print(f"\n开始相机校准 (使用 {len(self.obj_points)} 张图像)...")
+
+        ret, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera(
+            self.obj_points, self.img_points, image_size, None, None
+        )
+
+        print(f"标定完成! 重投影误差: {ret:.4f} 像素")
+
+        return ret, camera_matrix, dist_coeffs, rvecs, tvecs
+
+    def save_calibration_results(self, camera_matrix, dist_coeffs, ret, output_path='calibration_result.json'):
+        """
+        保存校准结果到JSON文件
+        """
+        result = {
+            'timestamp': datetime.now().strftime('%Y-%m-%d %H:%M:%S'),
+            'pattern_size': self.pattern_size,
+            'square_size': self.square_size,
+            'num_images': len(self.obj_points),
+            'reprojection_error': float(ret),
+            'camera_matrix': camera_matrix.tolist(),
+            'distortion_coefficients': dist_coeffs.tolist()
+        }
+
+        with open(output_path, 'w', encoding='utf-8') as f:
+            json.dump(result, f, indent=4, ensure_ascii=False)
+
+        print(f"\n校准结果已保存到: {output_path}")
+
+        # 打印结果
+        print("\n=== 相机校准结果 ===")
+        print(f"重投影误差: {ret:.4f} 像素")
+        print(f"\n相机内参矩阵:")
+        print(camera_matrix)
+        print(f"\n畸变系数:")
+        print(dist_coeffs.ravel())
+
+
+def main():
+    # 配置参数
+    VIDEO_PATH = 'Video_20260303114232727.avi'
+    OUTPUT_DIR = 'chessboard_detection_output'
+    PATTERN_SIZE = (11, 8)  # 11x8 内角点
+    SQUARE_SIZE = 25.0  # 假设每个方格25mm，根据实际情况调整
+    SAMPLE_INTERVAL = 30  # 每30帧采样一次
+
+    # 创建检测器
+    detector = ChessboardDetector(pattern_size=PATTERN_SIZE, square_size=SQUARE_SIZE)
+
+    # 处理视频
+    success = detector.process_video(VIDEO_PATH, OUTPUT_DIR, SAMPLE_INTERVAL)
+
+    if not success:
+        print("未能检测到任何棋盘格，程序退出")
+        return
+
+    # 获取图像尺寸
+    cap = cv2.VideoCapture(VIDEO_PATH)
+    ret, frame = cap.read()
+    if ret:
+        image_size = (frame.shape[1], frame.shape[0])
+        cap.release()
+
+        # 执行相机校准
+        calib_result = detector.calibrate_camera(image_size)
+
+        if calib_result:
+            ret, camera_matrix, dist_coeffs, rvecs, tvecs = calib_result
+
+            # 保存校准结果
+            detector.save_calibration_results(
+                camera_matrix, dist_coeffs, ret,
+                os.path.join(OUTPUT_DIR, 'calibration_result.json')
+            )
+
+            print("\n✓ 校准完成！可以使用生成的校准参数进行图像矫正")
+
+
+if __name__ == '__main__':
+    main()

calibration/test/visualize_video.py

@@ -0,0 +1,127 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+可视化视频中的棋盘格检测
+实时显示每一帧的检测结果
+"""
+
+import cv2
+import numpy as np
+
+
+class ChessboardVisualizer:
+    def __init__(self, pattern_size=(11, 8)):
+        """
+        初始化可视化器
+
+        Args:
+            pattern_size: 棋盘格内角点数量 (列, 行)
+        """
+        self.pattern_size = pattern_size
+        self.criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
+
+    def visualize_video(self, video_path, output_path='visualized_video.avi', show_window=False):
+        """
+        可视化整个视频的棋盘格检测
+
+        Args:
+            video_path: 输入视频路径
+            output_path: 输出视频路径
+            show_window: 是否显示实时窗口
+        """
+        cap = cv2.VideoCapture(video_path)
+        if not cap.isOpened():
+            print(f"无法打开视频: {video_path}")
+            return
+
+        # 获取视频参数
+        fps = cap.get(cv2.CAP_PROP_FPS)
+        width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
+        height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
+        total_frames = int(cap.get(cv2.CAP_PROP_FRAME_COUNT))
+
+        print(f"视频信息: {width}x{height}, {fps:.2f} FPS, {total_frames} 帧")
+
+        # 创建视频写入器
+        fourcc = cv2.VideoWriter_fourcc(*'XVID')
+        out = cv2.VideoWriter(output_path, fourcc, fps, (width, height))
+
+        frame_count = 0
+        detected_count = 0
+
+        print(f"\n开始处理视频...")
+        print("绿色角点 = 检测成功, 红色文字 = 未检测到")
+
+        while True:
+            ret, frame = cap.read()
+            if not ret:
+                break
+
+            frame_count += 1
+
+            # 转换为灰度图
+            gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+
+            # 检测棋盘格
+            ret_detect, corners = cv2.findChessboardCorners(gray, self.pattern_size, None)
+
+            # 创建可视化图像
+            vis_frame = frame.copy()
+
+            if ret_detect:
+                detected_count += 1
+                # 亚像素精度优化
+                corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), self.criteria)
+
+                # 绘制角点
+                cv2.drawChessboardCorners(vis_frame, self.pattern_size, corners, ret_detect)
+
+                # 添加成功标记
+                cv2.putText(vis_frame, f'DETECTED #{detected_count}', (20, 40),
+                           cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
+            else:
+                # 添加未检测标记
+                cv2.putText(vis_frame, 'NOT DETECTED', (20, 40),
+                           cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
+
+            # 添加帧信息
+            cv2.putText(vis_frame, f'Frame: {frame_count}/{total_frames}', (20, height - 20),
+                       cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
+
+            # 写入输出视频
+            out.write(vis_frame)
+
+            # 显示窗口（可选）
+            if show_window:
+                cv2.imshow('Chessboard Detection', vis_frame)
+                if cv2.waitKey(1) & 0xFF == ord('q'):
+                    print("\n用户中断")
+                    break
+
+            # 进度显示
+            if frame_count % 50 == 0:
+                progress = 100 * frame_count / total_frames
+                print(f"  进度: {frame_count}/{total_frames} ({progress:.1f}%) - 已检测: {detected_count}")
+
+        cap.release()
+        out.release()
+        if show_window:
+            cv2.destroyAllWindows()
+
+        print(f"\n✓ 处理完成!")
+        print(f"  总帧数: {frame_count}")
+        print(f"  检测成功: {detected_count} 帧 ({100*detected_count/frame_count:.1f}%)")
+        print(f"  输出文件: {output_path}")
+
+
+def main():
+    VIDEO_PATH = 'Video_20260303114232727.avi'
+    OUTPUT_PATH = 'visualized_chessboard_detection.avi'
+    PATTERN_SIZE = (11, 8)
+
+    visualizer = ChessboardVisualizer(pattern_size=PATTERN_SIZE)
+    visualizer.visualize_video(VIDEO_PATH, OUTPUT_PATH, show_window=False)
+
+
+if __name__ == '__main__':
+    main()

configs/ascento.yaml

@@ -0,0 +1,86 @@
+# enemy_color: "red"
+enemy_color: "blue"
+
+#####-----神经网络参数-----#####
+yolo_name: yolov5
+classify_model: assets/models/tiny_resnet.onnx
+yolo11_model_path: assets/models/yolo11.xml
+yolov8_model_path: assets/models/yolov8.xml
+yolov5_model_path: assets/models/yolov5.xml
+device: GPU
+min_confidence: 0.8
+use_traditional: false
+
+#####-----ROI-----#####
+roi: 
+  x: 500
+  y: 400
+  width: 500
+  height: 500
+
+use_roi: false
+
+#####-----传统方法参数-----#####
+threshold: 150
+max_angle_error: 45 # degree
+min_lightbar_ratio: 1.5
+max_lightbar_ratio: 20
+min_lightbar_length: 8
+min_armor_ratio: 1
+max_armor_ratio: 5
+max_side_ratio: 1.5
+max_rectangular_error: 25 # degree
+min_confidence: 0.8
+
+#####-----tracker参数-----#####
+min_detect_count: 5
+max_temp_lost_count: 15
+outpost_max_temp_lost_count: 75
+
+#####-----aimer参数-----#####
+yaw_offset: 1.5 # degree
+pitch_offset: -0.5 # degree
+comming_angle: 60 # degree
+leaving_angle: 20 # degree
+decision_speed: 7 # rad/s
+high_speed_delay_time: 0.050 # s
+low_speed_delay_time: 0.015 # s
+
+#####-----shooter参数-----#####
+first_tolerance: 100 # 近距离射击容差，degree
+second_tolerance: 100 # 远距离射击容差，degree
+judge_distance: 2 #距离判断阈值
+auto_fire: true # 是否由自瞄控制射击
+
+rotate_180: false
+camera_name: "hikrobot"
+exposure_ms: 2
+gain: 16
+vid_pid: "2bdf:0001"
+
+R_gimbal2imubody: [1, 0, 0, 0, 1, 0,  0, 0, 1]
+
+# 重投影误差: 0.1488px
+camera_matrix: [1775.1300101814929, 0, 710.22041588791421, 0, 1777.9427175568535, 599.38373307949621, 0, 0, 1]
+distort_coeffs: [-0.081907189655237952, 0.14019999270205855, -0.0012264127665053185, 0.0014292255962000792, 0]
+
+# 相机同理想情况的偏角: yaw1.81 pitch0.16 roll-0.00 degree
+R_camera2gimbal: [-0.03152275773856239, 0.0027095830669845509, 0.99949936163269204, -0.99950303430107212, -9.8404785402239625e-05, -0.031522606799691932, 1.2942398579498703e-05, -0.99999632423129503, 0.0027113384873522772]
+t_camera2gimbal: [0.13895789074180098, 0.066620637390426771, 0.028079650211170731]
+
+
+#####-----cboard参数-----#####
+quaternion_canid: 0x100
+bullet_speed_canid: 0x101
+send_canid: 0xff
+can_interface: "can0"
+
+#####-----gimbal参数-----#####
+com_port: "/dev/gimbal"
+baudrate: 115200
+#####-----buff_detector参数-----#####
+model: "assets/yolo11_buff_int8.xml"
+
+#####-----buff_aimer参数-----#####
+fire_gap_time: 0.520   # s
+predict_time: 0.100     # s

configs/calibration.yaml

@@ -0,0 +1,25 @@
+pattern_cols: 11
+pattern_rows: 8
+center_distance_mm: 30
+pattern_type: chessboard   # circles_grid | chessboard
+
+R_gimbal2imubody: [1, 0, 0, 0, 1, 0,  0, 0, 1]
+
+rotate_180: false
+camera_name: "hikrobot"
+exposure_ms: 30
+gain: 16.0
+vid_pid: "2bdf:0001"
+
+#####-----cboard参数-----#####
+quaternion_canid: 0x01
+bullet_speed_canid: 0x110
+send_canid: 0xff
+can_interface: "can0"
+#####-----gimbal参数-----#####
+com_port: "/dev/ttyUSB0"
+baudrate: 115200
+
+# 重投影误差: 0.0654px
+camera_matrix: [1794.7763123680743, 0, 762.48990119722805, 0, 1794.6747019516008, 546.1981535799863, 0, 0, 1]
+distort_coeffs: [-0.07577568384219846, 0.14361612515698613, 0.00057329040489542368, 0.0001190829309863327, 0]

configs/camera.yaml

@@ -0,0 +1,10 @@
+# camera_name: "mindvision"
+# exposure_ms: 2
+# gamma: 0.5
+# vid_pid: "f622:d13a"
+
+rotate_180: false
+camera_name: "hikrobot"
+exposure_ms: 3
+gain: 10.0
+vid_pid: "2bdf:0001"

configs/demo.yaml

@@ -0,0 +1,122 @@
+# enemy_color: "red"
+enemy_color: "blue"
+
+#####-----神经网络参数-----#####
+yolo_name: yolov5
+classify_model: assets/models/tiny_resnet.onnx
+yolo11_model_path: assets/models/yolo11.xml
+yolov8_model_path: assets/models/yolov8.xml
+yolov5_model_path: assets/models/yolov5.xml
+device: CPU
+min_confidence: 0.8
+use_traditional: true
+
+#####-----ROI-----#####
+roi: 
+  x: 420
+  y: 50
+  width: 600
+  height: 600
+
+use_roi: false
+
+#####-----传统方法参数-----#####
+threshold: 150
+max_angle_error: 45 # degree
+min_lightbar_ratio: 1.5
+max_lightbar_ratio: 20
+min_lightbar_length: 8
+min_armor_ratio: 1
+max_armor_ratio: 5
+max_side_ratio: 1.5
+max_rectangular_error: 25 # degree
+min_confidence: 0.8
+
+#####-----tracker参数-----#####
+min_detect_count: 5
+max_temp_lost_count: 15
+outpost_max_temp_lost_count: 75
+
+#####-----aimer参数-----#####
+yaw_offset: -1 # degree -2.5
+pitch_offset: -1.4 # degree 2
+comming_angle: 60 # degree
+leaving_angle: 20 # degree
+decision_speed: 8 # rad/s
+high_speed_delay_time: 0.030 # s
+low_speed_delay_time: 0.015 # s
+
+#####-----shooter参数-----#####
+first_tolerance: 3 # 近距离射击容差，degree
+second_tolerance: 2 # 远距离射击容差，degree
+judge_distance: 2 #距离判断阈值
+auto_fire: true # 是否由自瞄控制射击
+
+rotate_180: false
+camera_name: "hikrobot"
+exposure_ms: 2
+gain: 16
+vid_pid: "2bdf:0001"
+
+# -1  0  0
+#  0 -1  0
+#  0  0  1
+R_gimbal2imubody: [-1, 0, 0, 0, -1, 0, 0, 0, 1]
+
+# 重投影误差: 0.2697px
+camera_matrix: [1818.3669452465165, 0, 751.06226574703498, 0, 1822.494494078506, 530.43671556112133, 0, 0, 1]
+distort_coeffs: [-0.077944626599568856, 0.15447826031486889, -0.0025714394278524674, 0.00083016311301273629, 0]
+
+# 相机同理想情况的偏角: yaw0.46 pitch0.61 roll-1.53 degree
+R_camera2gimbal: [-0.0083195760046954614, 0.010498791137270739, 0.99991027599468041, -0.99960756138647755, -0.026835747568381807, -0.0080352891314148939, 0.026748978935305992, -0.99958472279097077, 0.010717933047771133]
+t_camera2gimbal: [0.094969301833534511, 0.095006290298006682, 0.050987066291756609]
+
+#####-----cboard参数-----#####
+quaternion_canid: 0x100
+bullet_speed_canid: 0x101
+send_canid: 0xff
+can_interface: "can0"
+
+#####-----gimbal参数-----#####
+com_port: "/dev/gimbal"
+baudrate: 115200
+#####-----planner-----#####
+fire_thresh: 0.003
+
+max_yaw_acc: 50
+Q_yaw: [9e6, 0]
+R_yaw: [1]
+
+max_pitch_acc: 100
+Q_pitch: [9e6,0]
+R_pitch: [1]
+
+#####-----buff_detector参数-----#####
+detect:
+  contrast: 1
+  brightness: 
+    blue: -120
+    red: -100
+  brightness_threshold:
+    blue: 120
+    red: 40
+  morphology_size:
+    blue: 3
+    red: 3
+  dilate_size: 2
+  # canny_low_threshold: 100
+  # canny_high_threshold: 180
+  # approx_epsilon: 1.0
+  R_contours_min_area: 200
+  R_contours_max_area: 600
+  fanblades_head_contours_min_area: 3000
+  fanblades_head_contours_max_area: 8000
+  fanblades_body_contours_min_area: 1000
+  fanblades_body_contours_max_area: 4000
+  standard_fanblade_path: ./assets/standard_fanblade.jpg
+
+#####-----buff_aimer参数-----#####
+aim_time: 0.150         # s
+wait_time: 0.300        # s
+command_fire_gap: 0.080 # s
+predict_time: 0.150     # s

configs/example.yaml

@@ -0,0 +1,84 @@
+# enemy_color: "red"
+enemy_color: "blue"
+
+
+#####-----神经网络参数-----#####
+yolo_name: yolov5
+classify_model: assets/models/tiny_resnet.onnx
+yolo11_model_path: assets/models/yolo11.xml
+yolov8_model_path: assets/models/yolov8.xml
+yolov5_model_path: assets/models/yolov5.xml
+device: CPU
+min_confidence: 0.8
+use_traditional: true
+
+#####-----ROI-----#####
+roi: 
+  x: 420
+  y: 50
+  width: 600
+  height: 600
+  
+#####-----USB相机参数-----#####
+image_width: 1920
+image_height: 1080
+fov_h: 87.7
+fov_v: 56.7 
+usb_frame_rate: 60
+usb_exposure: 250 #1-80000______250
+usb_gamma: 160
+usb_gain: 10 #0-96
+
+#####-----工业相机参数-----#####
+rotate_180: false
+camera_name: "mindvision"
+exposure_ms: 2
+gamma: 0.5
+vid_pid: "f622:d13a"
+
+#####-----传统方法参数-----#####
+threshold: 150
+max_angle_error: 45 # degree
+min_lightbar_ratio: 1.5
+max_lightbar_ratio: 20
+min_lightbar_length: 8
+min_armor_ratio: 1
+max_armor_ratio: 5
+max_side_ratio: 1.5
+max_rectangular_error: 25 # degree
+min_confidence: 0.8
+
+#####-----cboard参数-----#####
+quaternion_canid: 0x01
+bullet_speed_canid: 0x101
+send_canid: 0xff
+can_interface: "can0"
+
+#####-----tracker参数-----#####
+min_detect_count: 5
+max_temp_lost_count: 15
+
+#####-----aimer参数-----#####
+yaw_offset: -2.5 # degree -2.5
+pitch_offset: 3 # degree 2
+comming_angle: 60 # degree
+leaving_angle: 20 # degree
+
+#####-----shooter参数-----#####
+first_tolerance: 3 # 近距离射击容差，degree
+second_tolerance: 2 # 远距离射击容差，degree
+judge_distance: 2 #距离判断阈值
+auto_fire: true # 是否由自瞄控制射击
+
+#####-----工业相机标定参数-----#####
+R_gimbal2imubody: [1, 0, 0, 0, 1, 0, 0, 0, 1]
+
+# 重投影误差: 0.1944px
+camera_matrix: [1295.4665175006589, 0, 655.71498478800618, 0, 1297.2591156991834, 506.21320670125334, 0, 0, 1]
+distort_coeffs: [-0.48599095566724387, 0.23999516531343185, -0.00029018466701866776, -0.00083639963030895752, 0]
+
+# 相机同理想情况的偏角: yaw-2.64 pitch-3.20 roll-0.77 degree
+# 标定板到世界坐标系原点的水平距离: 1.45 m
+# 标定板同竖直摆放时的偏角: yaw90.85 pitch8.54 roll-0.32 degree
+R_camera2gimbal: [0.046736929626455238, -0.055211941324114208, 0.99738021884550865, -0.99881598162461238, -0.016078770676278314, 0.0459141370909011, 0.013501639172863072, -0.99834518813322415, -0.055898041744621867]
+t_camera2gimbal: [0.27420934670256336, 0.0098763306562371159, 0.043798385984197351]

configs/hero.yaml

@@ -0,0 +1,105 @@
+# enemy_color: "red"
+enemy_color: "blue"
+
+#####-----神经网络参数-----#####
+yolo_name: yolov5
+classify_model: assets/models/tiny_resnet.onnx
+yolo11_model_path: assets/models/yolo11.xml
+yolov8_model_path: assets/models/yolov8.xml
+yolov5_model_path: assets/models/yolov5.xml
+device: CPU
+min_confidence: 0.8
+use_traditional: true
+
+#####-----ROI-----#####
+roi: 
+  x: 420
+  y: 50
+  width: 600
+  height: 600
+
+use_roi: false
+
+#####-----传统方法参数-----#####
+threshold: 150
+max_angle_error: 45 # degree
+min_lightbar_ratio: 1.5
+max_lightbar_ratio: 20
+min_lightbar_length: 8
+min_armor_ratio: 1
+max_armor_ratio: 5
+max_side_ratio: 1.5
+max_rectangular_error: 25 # degree
+
+#####-----tracker参数-----#####
+min_detect_count: 5
+max_temp_lost_count: 15
+outpost_max_temp_lost_count: 75
+
+#####-----aimer参数-----#####
+yaw_offset: -4.5 # degree -2.5
+pitch_offset: 15 # degree 2
+comming_angle: 55 # degree
+leaving_angle: 20 # degree
+decision_speed: 7 # rad/s
+high_speed_delay_time: 0.0 # s
+low_speed_delay_time: 0.0 # s planner use this value
+
+#####-----shooter参数-----#####
+first_tolerance: 3 # 近距离射击容差，degree
+second_tolerance: 2 # 远距离射击容差，degree
+judge_distance: 2 #距离判断阈值
+auto_fire: true # 是否由自瞄控制射击
+
+rotate_180: false
+camera_name: "hikrobot"
+exposure_ms: 3.5
+gain: 16.9
+vid_pid: "2bdf:0001"
+
+#  1  0  0
+#  0  1  0
+#  0  0  1
+R_gimbal2imubody: [1, 0, 0, 0, 1, 0, 0, 0, 1]
+
+# 重投影误差: 0.0654px
+camera_matrix: [1794.7763123680743, 0, 762.48990119722805, 0, 1794.6747019516008, 546.1981535799863, 0, 0, 1]
+distort_coeffs: [-0.07577568384219846, 0.14361612515698613, 0.00057329040489542368, 0.0001190829309863327, 0]
+
+# 相机同理想情况的偏角: yaw1.44 pitch-7.28 roll0.96 degree
+# 标定板到世界坐标系原点的水平距离: 1.13 m
+# 标定板同竖直摆放时的偏角: yaw7.61 pitch13.92 roll-0.46 degree
+R_camera2gimbal: [-0.027182119030230909, -0.12616154330853446, 0.99163723074269183, -0.99949106557517331, 0.019998323121329122, -0.024853106601381177, -0.016695575474690555, -0.99180811252093692, -0.12664093215554434]
+t_camera2gimbal: [0.13160669975045827, 0.10377721766577375, 0.024908271912914642]
+
+#####-----cboard参数-----#####
+quaternion_canid: 0x100
+bullet_speed_canid: 0x101
+send_canid: 0xff
+can_interface: "can0"
+
+#####-----gimbal参数-----#####
+com_port: "/dev/ttyUSB0"
+baudrate: 115200
+yaw_kp: 0
+yaw_kd: 0
+pitch_kp: 0
+pitch_kd: 0
+
+#####-----planner-----#####
+fire_thresh: 0.0035
+
+max_yaw_acc: 50
+Q_yaw: [9e6, 0]
+R_yaw: [1]
+
+max_pitch_acc: 100
+Q_pitch: [9e6, 0]
+R_pitch: [1]
+
+#####-----buff_detector参数-----#####
+model: "assets/models/yolo11_buff_int8.xml"
+
+#####-----buff_aimer参数-----#####
+fire_gap_time: 0.700   # s
+predict_time: 0.120     # s

configs/mvs.yaml

@@ -0,0 +1,125 @@
+# enemy_color: "red"
+enemy_color: "blue"
+
+
+#####-----神经网络参数-----#####
+yolo_name: yolov5
+classify_model: assets/models/tiny_resnet.onnx
+yolo11_model_path: assets/models/yolo11.xml
+yolov8_model_path: assets/models/yolov8.xml
+yolov5_model_path: assets/models/yolov5.xml
+device: GPU
+min_confidence: 0.8
+use_traditional: true
+
+#####-----ROI-----#####
+roi: 
+  x: 420
+  y: 50
+  width: 600
+  height: 600
+
+use_roi: false
+
+#####-----USB相机参数-----#####
+image_width: 640
+image_height: 360
+new_image_width: 1280
+new_image_height: 720
+fov_h: 57.7 #87.7
+fov_v: 56.7 
+new_fov_h: 37 #67
+new_fov_v: 40.9
+usb_frame_rate: 60
+usb_exposure: 305 #1-80000______250
+new_usb_exposure: 320
+usb_gamma: 160
+usb_gain: 10 #0-96
+
+#####-----工业相机参数-----#####
+rotate_180: false
+camera_name: "hikrobot"
+exposure_ms: 2
+gain: 16
+vid_pid: "2bdf:0001"
+
+#####-----传统方法参数-----#####
+threshold: 150
+max_angle_error: 45 # degree
+min_lightbar_ratio: 1.5
+max_lightbar_ratio: 20
+min_lightbar_length: 8
+min_armor_ratio: 1
+max_armor_ratio: 5
+max_side_ratio: 1.5
+max_rectangular_error: 25 # degree
+min_confidence: 0.8
+
+#####-----cboard参数-----#####
+quaternion_canid: 0x01
+bullet_speed_canid: 0x110
+send_canid: 0xff
+can_interface: "can0"
+
+#####-----tracker参数-----#####
+min_detect_count: 5
+max_temp_lost_count: 15
+outpost_max_temp_lost_count: 75
+
+#####-----aimer参数-----#####
+yaw_offset: -2.5 # degree -2.5
+pitch_offset: 0  # degree 2
+comming_angle: 70 # degree
+leaving_angle: 30 # degree
+decision_speed: 7 # rad/s
+high_speed_delay_time: 0.066 # s
+low_speed_delay_time: 0.015 # s
+
+#####-----shooter参数-----#####
+first_tolerance: 3 # 近距离射击容差，degree
+second_tolerance: 2 # 远距离射击容差，degree
+judge_distance: 2 #距离判断阈值
+auto_fire: true # 是否由自瞄控制射击
+
+#####-----decider参数-----#####
+mode: 1
+
+#####-----工业相机标定参数-----#####
+R_gimbal2imubody: [1, 0, 0, 0, 1, 0, 0, 0, 1]
+
+# 重投影误差: 0.1322px
+camera_matrix: [1776.9477196851155, 0, 756.31235265560952, 0, 1776.0591253569607, 566.16539069551641, 0, 0, 1]
+distort_coeffs: [-0.08382326954462313, 0.097449270330296239, -0.0012558283068959985, 0.0037372210254148081, 0]
+
+R_camera2gimbal: [0, 0, 1, -1, 0, 0, 0, -1, 0]
+t_camera2gimbal: [0.1, 0, 0.05]
+
+
+#####-----buff_detector参数-----#####
+detect:
+  contrast: 1
+  brightness: 
+    blue: -120
+    red: -100
+  brightness_threshold:
+    blue: 120
+    red: 80
+  morphology_size:
+    blue: 3
+    red: 2
+  dilate_size: 1
+  # canny_low_threshold: 100
+  # canny_high_threshold: 180
+  # approx_epsilon: 1.0
+  R_contours_min_area: 1
+  R_contours_max_area: 600
+  fanblades_head_contours_min_area: 300
+  fanblades_head_contours_max_area: 8000
+  fanblades_body_contours_min_area: 1000
+  fanblades_body_contours_max_area: 4000
+  standard_fanblade_path: ./assets/standard_fanblade.jpg
+
+#####-----buff_aimer参数-----#####
+aim_time: 0.200 # s
+wait_time: 0.050 # s
+predict_time: 0.300 # s

configs/sentry.yaml

@@ -0,0 +1,105 @@
+# enemy_color: "red"
+enemy_color: "blue"
+
+#####-----神经网络参数-----#####
+yolo_name: yolov5
+classify_model: assets/models/tiny_resnet.onnx
+yolo11_model_path: assets/models/yolo11.xml
+yolov8_model_path: assets/models/yolov8.xml
+yolov5_model_path: assets/models/yolov5.xml
+device: CPU
+min_confidence: 0.8
+use_traditional: true
+
+#####-----ROI-----#####
+roi: 
+  x: 420
+  y: 50
+  width: 600
+  height: 600
+
+use_roi: false
+
+#####-----传统方法参数-----#####
+threshold: 150
+max_angle_error: 45 # degree
+min_lightbar_ratio: 1.5
+max_lightbar_ratio: 20
+min_lightbar_length: 8
+min_armor_ratio: 1
+max_armor_ratio: 5
+max_side_ratio: 1.5
+max_rectangular_error: 25 # degree
+
+#####-----tracker参数-----#####
+min_detect_count: 5
+max_temp_lost_count: 15
+outpost_max_temp_lost_count: 75
+
+#####-----aimer参数-----#####
+yaw_offset: 2 # degree -2.5
+pitch_offset: 6.5 # degree 2
+comming_angle: 55 # degree
+leaving_angle: 20 # degree
+decision_speed: 7 # rad/s
+high_speed_delay_time: 0.0 # s
+low_speed_delay_time: 0.0 # s planner use this value
+
+#####-----shooter参数-----#####
+first_tolerance: 3 # 近距离射击容差，degree
+second_tolerance: 2 # 远距离射击容差，degree
+judge_distance: 2 #距离判断阈值
+auto_fire: true # 是否由自瞄控制射击
+
+rotate_180: false
+camera_name: "hikrobot"
+exposure_ms: 2.5
+gain: 16.9
+vid_pid: "2bdf:0001"
+
+#  1  0  0
+#  0  1  0
+#  0  0  1
+R_gimbal2imubody: [1, 0, 0, 0, 1, 0, 0, 0, 1]
+
+# 重投影误差: 0.1820px
+camera_matrix: [1785.4881526822305, 0, 672.4806478241826, 0, 1785.026019470562, 559.89603224794314, 0, 0, 1]
+distort_coeffs: [-0.076005079619881746, 0.11182817466388446, 0.0005362204787722057, -0.0027546300984895122, 0]
+
+# 相机同理想情况的偏角: yaw1.44 pitch-7.28 roll0.96 degree
+# 标定板到世界坐标系原点的水平距离: 1.13 m
+# 标定板同竖直摆放时的偏角: yaw7.61 pitch13.92 roll-0.46 degree
+R_camera2gimbal: [-0.027182119030230909, -0.12616154330853446, 0.99163723074269183, -0.99949106557517331, 0.019998323121329122, -0.024853106601381177, -0.016695575474690555, -0.99180811252093692, -0.12664093215554434]
+t_camera2gimbal: [0.13160669975045827, 0.10377721766577375, 0.024908271912914642]
+
+#####-----cboard参数-----#####
+quaternion_canid: 0x100
+bullet_speed_canid: 0x101
+send_canid: 0xff
+can_interface: "can0"
+
+#####-----gimbal参数-----#####
+com_port: "/dev/ttyUSB0"
+baudrate: 115200
+yaw_kp: 0
+yaw_kd: 0
+pitch_kp: 0
+pitch_kd: 0
+
+#####-----planner-----#####
+fire_thresh: 0.0035
+
+max_yaw_acc: 50
+Q_yaw: [9e6, 0]
+R_yaw: [1]
+
+max_pitch_acc: 100
+Q_pitch: [9e6, 0]
+R_pitch: [1]
+
+#####-----buff_detector参数-----#####
+model: "assets/models/yolo11_buff_int8.xml"
+
+#####-----buff_aimer参数-----#####
+fire_gap_time: 0.700   # s
+predict_time: 0.120     # s

configs/standard3.yaml

@@ -0,0 +1,105 @@
+# enemy_color: "red"
+enemy_color: "blue"
+
+#####-----神经网络参数-----#####
+yolo_name: yolov5
+classify_model: assets/models/tiny_resnet.onnx
+yolo11_model_path: assets/models/yolo11.xml
+yolov8_model_path: assets/models/yolov8.xml
+yolov5_model_path: assets/models/yolov5.xml
+device: CPU
+min_confidence: 0.8
+use_traditional: true
+
+#####-----ROI-----#####
+roi: 
+  x: 420
+  y: 50
+  width: 600
+  height: 600
+
+use_roi: false
+
+#####-----传统方法参数-----#####
+threshold: 150
+max_angle_error: 45 # degree
+min_lightbar_ratio: 1.5
+max_lightbar_ratio: 20
+min_lightbar_length: 8
+min_armor_ratio: 1
+max_armor_ratio: 5
+max_side_ratio: 1.5
+max_rectangular_error: 25 # degree
+
+#####-----tracker参数-----#####
+min_detect_count: 5
+max_temp_lost_count: 15
+outpost_max_temp_lost_count: 75
+
+#####-----aimer参数-----#####
+yaw_offset: 2 # degree -2.5
+pitch_offset: 6.5 # degree 2
+comming_angle: 55 # degree
+leaving_angle: 20 # degree
+decision_speed: 7 # rad/s
+high_speed_delay_time: 0.0 # s
+low_speed_delay_time: 0.0 # s planner use this value
+
+#####-----shooter参数-----#####
+first_tolerance: 3 # 近距离射击容差，degree
+second_tolerance: 2 # 远距离射击容差，degree
+judge_distance: 2 #距离判断阈值
+auto_fire: true # 是否由自瞄控制射击
+
+rotate_180: false
+camera_name: "hikrobot"
+exposure_ms: 2.5
+gain: 16.9
+vid_pid: "2bdf:0001"
+
+#  1  0  0
+#  0  1  0
+#  0  0  1
+R_gimbal2imubody: [1, 0, 0, 0, 1, 0, 0, 0, 1]
+
+# 重投影误差: 0.1820px
+camera_matrix: [1785.4881526822305, 0, 672.4806478241826, 0, 1785.026019470562, 559.89603224794314, 0, 0, 1]
+distort_coeffs: [-0.076005079619881746, 0.11182817466388446, 0.0005362204787722057, -0.0027546300984895122, 0]
+
+# 相机同理想情况的偏角: yaw1.44 pitch-7.28 roll0.96 degree
+# 标定板到世界坐标系原点的水平距离: 1.13 m
+# 标定板同竖直摆放时的偏角: yaw7.61 pitch13.92 roll-0.46 degree
+R_camera2gimbal: [-0.027182119030230909, -0.12616154330853446, 0.99163723074269183, -0.99949106557517331, 0.019998323121329122, -0.024853106601381177, -0.016695575474690555, -0.99180811252093692, -0.12664093215554434]
+t_camera2gimbal: [0.13160669975045827, 0.10377721766577375, 0.024908271912914642]
+
+#####-----cboard参数-----#####
+quaternion_canid: 0x100
+bullet_speed_canid: 0x101
+send_canid: 0xff
+can_interface: "can0"
+
+#####-----gimbal参数-----#####
+com_port: "/dev/ttyUSB0"
+baudrate: 115200
+yaw_kp: 0
+yaw_kd: 0
+pitch_kp: 0
+pitch_kd: 0
+
+#####-----planner-----#####
+fire_thresh: 0.0035
+
+max_yaw_acc: 50
+Q_yaw: [9e6, 0]
+R_yaw: [1]
+
+max_pitch_acc: 100
+Q_pitch: [9e6, 0]
+R_pitch: [1]
+
+#####-----buff_detector参数-----#####
+model: "assets/yolo11_buff_int8.xml"
+
+#####-----buff_aimer参数-----#####
+fire_gap_time: 0.700   # s
+predict_time: 0.120     # s

configs/standard4.yaml

@@ -0,0 +1,100 @@
+enemy_color: "red"
+# enemy_color: "blue"
+
+#####-----神经网络参数-----#####
+yolo_name: yolov5
+classify_model: assets/models/tiny_resnet.onnx
+yolo11_model_path: assets/models/yolo11.xml
+yolov8_model_path: assets/models/yolov8.xml
+yolov5_model_path: assets/models/yolov5.xml
+device: GPU
+min_confidence: 0.8
+use_traditional: true
+
+#####-----ROI-----#####
+roi: 
+  x: 420
+  y: 50
+  width: 600
+  height: 600
+
+use_roi: false
+
+#####-----传统方法参数-----#####
+threshold: 150
+max_angle_error: 45 # degree
+min_lightbar_ratio: 1.5
+max_lightbar_ratio: 20
+min_lightbar_length: 8
+min_armor_ratio: 1
+max_armor_ratio: 5
+max_side_ratio: 1.5
+max_rectangular_error: 25 # degree
+min_confidence: 0.8
+
+#####-----tracker参数-----#####
+min_detect_count: 5
+max_temp_lost_count: 15
+outpost_max_temp_lost_count: 75
+
+#####-----aimer参数-----#####
+yaw_offset: -2 # degree -2.5
+pitch_offset: 0 # degree 2
+comming_angle: 60 # degree
+leaving_angle: 20 # degree
+decision_speed: 8 # rad/s
+high_speed_delay_time: 0.015 # s
+low_speed_delay_time: 0.015 # s
+
+#####-----shooter参数-----#####
+first_tolerance: 3 # 近距离射击容差，degree
+second_tolerance: 2 # 远距离射击容差，degree
+judge_distance: 2 #距离判断阈值
+auto_fire: true # 是否由自瞄控制射击
+
+rotate_180: false
+camera_name: "hikrobot"
+exposure_ms: 2
+gain: 16
+vid_pid: "2bdf:0001"
+
+#  1  0  0
+#  0  1  0
+#  0  0  1
+
+R_gimbal2imubody: [1, 0, 0, 0, 1, 0, 0, 0, 1]
+
+# 重投影误差: 0.3145px
+camera_matrix: [1851.7070167840545, 0, 721.12585328714192, 0, 1851.8175594364079, 571.69879709276688, 0, 0, 1]
+distort_coeffs: [-0.093662536083526302, 0.18945726820633155, -0.00040424349861928674, -0.0040568403852123142, 0]
+
+# 相机同理想情况的偏角: yaw-1.61 pitch-0.82 roll-0.61 degree
+R_camera2gimbal: [0.02823004230930648, -0.014076983590428133, 0.99950232778328707, -0.99954530259354468, -0.010995644138609872, 0.028076393521179785, 0.010594940981141165, -0.99984045444409364, -0.014380990321749998]
+t_camera2gimbal: [0.045517325957791413, 0.10544338092767802, 0.034649710880185793]
+
+#####-----cboard参数-----#####
+quaternion_canid: 0x100
+bullet_speed_canid: 0x101
+send_canid: 0xff
+can_interface: "can0"
+
+#####-----gimbal参数-----#####
+com_port: "/dev/gimbal"
+baudrate: 115200
+#####-----planner-----#####
+fire_thresh: 0.003
+
+max_yaw_acc: 50
+Q_yaw: [9e6, 0]
+R_yaw: [1]
+
+max_pitch_acc: 100
+Q_pitch: [9e6,0]
+R_pitch: [1]
+
+#####-----buff_detector参数-----#####
+model: "assets/yolo11_buff_int8.xml"
+
+#####-----buff_aimer参数-----#####
+fire_gap_time: 0.520   # s
+predict_time: 0.100     # s

configs/uav.yaml

@@ -0,0 +1,106 @@
+enemy_color: "red"
+# enemy_color: "blue"
+
+
+#####-----神经网络参数-----#####
+yolo_name: yolov5
+classify_model: assets/models/tiny_resnet.onnx
+yolo11_model_path: assets/models/yolo11.xml
+yolov8_model_path: assets/models/yolov8.xml
+yolov5_model_path: assets/models/yolov5.xml
+device: CPU
+min_confidence: 0.8
+use_traditional: true
+
+#####-----ROI-----#####
+roi: 
+  x: 420
+  y: 50
+  width: 600
+  height: 600
+
+use_roi: false
+
+#####-----工业相机参数-----#####
+rotate_180: false
+camera_name: "mindvision"
+exposure_ms: 8
+gamma: 0.6
+vid_pid: "f622:d13a"
+
+#####-----USB相机参数-----#####
+image_width: 640
+image_height: 360
+new_image_width: 1280
+new_image_height: 720
+fov_h: 87.7
+fov_v: 56.7 
+new_fov_h: 67
+new_fov_v: 40.9
+usb_frame_rate: 100
+usb_exposure: 315 #1-80000______250
+usb_gamma: 160
+usb_gain: 10 #0-96
+
+
+#####-----传统方法参数-----#####
+threshold: 150
+max_angle_error: 45 # degree
+min_lightbar_ratio: 1.5
+max_lightbar_ratio: 20
+min_lightbar_length: 8
+min_armor_ratio: 1
+max_armor_ratio: 5
+max_side_ratio: 1.5
+max_rectangular_error: 25 # degree
+min_confidence: 0.8
+
+#####-----cboard参数-----#####
+quaternion_canid: 0x001
+bullet_speed_canid: 0x110
+send_canid: 0xff
+can_interface: "can0"
+
+#####-----tracker参数-----#####
+min_detect_count: 5
+max_temp_lost_count: 15
+outpost_max_temp_lost_count: 150
+
+#####-----aimer参数-----#####
+yaw_offset: 0 # degree -2.5
+pitch_offset: 1.5 # degree 2
+comming_angle: 70 # degree
+leaving_angle: 30 # degree
+min_spin_speed: 2 # rad/s
+decision_speed: 12 # rad/s
+high_speed_delay_time: 0.005 # s
+low_speed_delay_time: 0.005 # s
+
+#####-----decider参数-----#####
+mode: 1
+
+#####-----shooter参数-----#####
+first_tolerance: 2 # 近距离射击容差，degree
+second_tolerance: 0.8 # 远距离射击容差，degree
+judge_distance: 0.5 #距离判断阈值
+auto_fire: true # 是否由自瞄控制射击
+
+#####-----工业相机标定参数-----#####
+R_gimbal2imubody: [1, 0, 0, 0, 1, 0, 0, 0, 1]
+
+
+# 重投影误差: 0.0898px
+camera_matrix: [1798.7099895400202, 0, 684.19918992969656, 0, 1796.3922446630108, 518.69958730895587, 0, 0, 1]
+distort_coeffs: [-0.071257863313183595, 0.12680911204832926, -0.00040857684893118372, 0.00024631839497110973, 0]
+
+# 相机同理想情况的偏角: yaw178.69 pitch-0.94 roll0.93 degree
+# 标定板到世界坐标系原点的水平距离: 1.80 m
+# 标定板同竖直摆放时的偏角: yaw-149.70 pitch4.40 roll4.02 degree
+R_camera2gimbal: [-0.022834689599904982, 0.016744625574604668, -0.99959901683887342, 0.99960655377849394, -0.015908035926893712, -0.023101342732220788, -0.016288480406682074, -0.99973324037330835, -0.016374782402407043]
+t_camera2gimbal: [-0.14646198800830063, -0.05254199849465014, 0.047644612585921703]
+#####-----buff_detector参数-----#####
+model: "assets/yolo11_buff_int8.xml"
+
+#####-----buff_aimer参数-----#####
+fire_gap_time: 0.600   # s
+predict_time: 0.090     # s

readme.md

@@ -0,0 +1,346 @@
+# 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）
+- ROS2 Humble（可选，用于哨兵ROS2通信）
+
+## 依赖安装
+
+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
+   ```
+
+3. ROS2 依赖（可选，用于哨兵）：
+   ```bash
+   # 安装 ROS2 Humble
+   sudo apt install ros-humble-desktop
+
+   # 编译 rm_msgs 包
+   cd ~/rm_msgs
+   source /opt/ros/humble/setup.bash
+   colcon build
+   ```
+
+## 编译与运行
+
+### 标准编译（不含ROS2）
+
+```bash
+cmake -B build
+make -C build/ -j$(nproc)
+./build/auto_aim_test    # 运行测试
+```
+
+### ROS2编译（哨兵专用）
+
+```bash
+# 设置ROS2环境
+source /opt/ros/humble/setup.bash
+source ~/rm_msgs/install/setup.bash
+
+# 编译
+cmake -B build
+make -C build/ -j$(nproc)
+
+# 运行ROS2版本程序
+./build/sentry_mpc configs/sentry.yaml
+./build/auto_aim_debug_mpc_ros configs/standard3.yaml
+./build/capture_ros configs/calibration.yaml -o assets/img_with_q
+```
+
+**注意**：CMake会自动检测ROS2环境，如果检测到ROS2和rm_msgs包，会自动启用ROS2支持并编译相关程序。
+
+## 可执行目标
+
+### 主程序（task）
+
+| 目标 | 说明 | 配置文件 |
+|------|------|----------|
+| `standard` | 步兵自瞄 | `configs/standard3.yaml` |
+| `standard_mpc` | 步兵自瞄（MPC 规划） | 需指定 |
+| `sentry_mpc` | 哨兵自瞄（ROS2通信） | 需指定 |
+| `uav` | 无人机自瞄 + 打符 | `configs/uav.yaml` |
+| `uav_debug` | 无人机调试（含可视化） | `configs/uav.yaml` |
+| `mt_standard` | 多线程步兵 | 需指定 |
+| `balance_infantry` | 平衡步兵 | 需指定 |
+| `balance_infantry_mpc` | 平衡步兵（MPC规划） | 需指定 |
+| `auto_aim_debug_mpc` | 自瞄 MPC 调试 | 需指定 |
+| `auto_aim_debug_mpc_ros` | 自瞄 MPC 调试（ROS2） | 需指定 |
+| `auto_buff_debug` | 打符调试 | 需指定 |
+| `auto_buff_debug_mpc` | 打符 MPC 调试 | 需指定 |
+| `mt_auto_aim_debug` | 多线程自瞄调试 | 需指定 |
+
+**注意**：`sentry_mpc` 和 `auto_aim_debug_mpc_ros` 需要ROS2环境，只在检测到ROS2时才会编译。
+
+### 标定工具（calibration）
+
+| 目标 | 说明 | 通信方式 |
+|------|------|----------|
+| `capture` | 采集标定图像 | 串口 |
+| `capture_ros` | 采集标定图像（ROS2） | ROS2话题 |
+| `calibrate_camera` | 相机内参标定 | - |
+| `calibrate_handeye` | 手眼标定 | - |
+| `calibrate_robotworld_handeye` | 机器人-世界手眼标定 | - |
+| `split_video` | 视频拆帧 | - |
+
+**注意**：`capture_ros` 需要ROS2环境，只在检测到ROS2时才会编译。
+
+### 测试用例（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
+   ```
+
+## 通信协议
+
+### 1. CBoard 协议（CAN 总线）
+
+通过 SocketCAN 与 RoboMaster C 型开发板通信，CAN ID 由 yaml 配置。
+
+**发送帧 — 控制命令（CAN ID: 0xff）**
+
+```
+8 bytes, Big-Endian
+[0]   : control    (uint8)   0=不控制, 1=控制
+[1]   : shoot      (uint8)   0=不射击, 1=射击
+[2-3] : yaw        (int16)   缩放 1e4, 单位 rad
+[4-5] : pitch      (int16)   缩放 1e4, 单位 rad
+[6-7] : horizon_distance (int16) 缩放 1e4（无人机专有）
+```
+
+**接收帧1 — 四元数（CAN ID: 0x100 / 0x01）**
+
+```
+8 bytes, Big-Endian
+[0-1] : x (int16) 缩放 1e4
+[2-3] : y (int16) 缩放 1e4
+[4-5] : z (int16) 缩放 1e4
+[6-7] : w (int16) 缩放 1e4
+四元数顺序: wxyz，验证 x²+y²+z²+w² ≈ 1
+```
+
+**接收帧2 — 子弹速度和模式（CAN ID: 0x101 / 0x110）**
+
+```
+8 bytes, Big-Endian
+[0-1] : bullet_speed (int16) 缩放 1e2, 单位 m/s
+[2]   : mode (uint8)  0=idle, 1=auto_aim, 2=small_buff, 3=big_buff, 4=outpost
+[3]   : shoot_mode (uint8) 0=left, 1=right, 2=both（哨兵专有）
+[4-5] : ft_angle (int16) 缩放 1e4, 单位 rad（无人机专有）
+```
+
+### 2. Gimbal 协议（串口）
+
+通过 USB 串口与达妙 MC02 通信，帧头 `{'S', 'P'}`，CRC16 校验。
+
+**发送帧 — VisionToGimbal（29 bytes, packed）**
+
+```
+[0-1]   : head       (uint8[2])  = {'S', 'P'}
+[2]     : mode       (uint8)     0=不控制, 1=控制不开火, 2=控制且开火
+[3-6]   : yaw        (float)     rad
+[7-10]  : yaw_vel    (float)     rad/s
+[11-14] : yaw_acc    (float)     rad/s²
+[15-18] : pitch      (float)     rad
+[19-22] : pitch_vel  (float)     rad/s
+[23-26] : pitch_acc  (float)     rad/s²
+[27-28] : crc16      (uint16)    Little-Endian
+```
+
+**接收帧 — GimbalToVision（43 bytes, packed）**
+
+```
+[0-1]   : head         (uint8[2])  = {'S', 'P'}
+[2]     : mode         (uint8)     0=IDLE, 1=AUTO_AIM, 2=SMALL_BUFF, 3=BIG_BUFF
+[3-6]   : q[0]         (float)     四元数 w
+[7-10]  : q[1]         (float)     四元数 x
+[11-14] : q[2]         (float)     四元数 y
+[15-18] : q[3]         (float)     四元数 z
+[19-22] : yaw          (float)     rad
+[23-26] : yaw_vel      (float)     rad/s
+[27-30] : pitch        (float)     rad
+[31-34] : pitch_vel    (float)     rad/s
+[35-38] : bullet_speed (float)     m/s
+[39-40] : bullet_count (uint16)    子弹累计发射次数
+[41-42] : crc16        (uint16)    Little-Endian
+```
+
+### 3. DM IMU 协议（串口）
+
+达妙 IMU，串口 921600 bps，Modbus RTU 格式，57 bytes 三帧合一。
+
+```
+帧1 [0-18]  : 加速度 (accx, accy, accz)  float, CRC16
+帧2 [19-37] : 角速度 (gyrox, gyroy, gyroz) float, CRC16
+帧3 [38-56] : 欧拉角 (roll, pitch, yaw)    float, 单位°, CRC16
+
+每帧结构: 帧头(0x55 0xAA) + slave_id(0x01) + reg + 3×float(uint32) + crc16 + 帧尾
+四元数由 ZYX 欧拉角生成: q = Rz(yaw) * Ry(pitch) * Rx(roll)
+```
+
+### 4. ROS2 通信（哨兵专有）
+
+**rm_msgs 自定义消息**
+
+| 方向 | 话题 | 消息类型 | 内容 |
+|------|------|----------|------|
+| 发布 | `data_aim` | `rm_msgs/DataAim` | 视觉控制指令 |
+| 订阅 | `data_mcu` | `rm_msgs/DataMCU` | MCU状态数据 |
+
+**DataAim 消息定义（视觉 → MCU）**
+
+```
+uint8 mode          # 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火
+float32 yaw         # 目标偏航角 (rad)
+float32 yaw_vel     # 偏航角速度 (rad/s)
+float32 yaw_acc     # 偏航角加速度 (rad/s²)
+float32 pitch       # 目标俯仰角 (rad)
+float32 pitch_vel   # 俯仰角速度 (rad/s)
+float32 pitch_acc   # 俯仰角加速度 (rad/s²)
+```
+
+**DataMCU 消息定义（MCU → 视觉）**
+
+```
+uint8 mode              # 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符
+float32 q0              # 四元数 w
+float32 q1              # 四元数 x
+float32 q2              # 四元数 y
+float32 q3              # 四元数 z
+float32 yaw             # 偏航角 (rad)
+float32 yaw_vel         # 偏航角速度 (rad/s)
+float32 pitch           # 俯仰角 (rad)
+float32 pitch_vel       # 俯仰角速度 (rad/s)
+float32 bullet_speed    # 弹速 (m/s)
+uint16 bullet_count     # 子弹累计发送次数
+```
+
+**ROS2 程序使用说明**
+
+1. 编译 rm_msgs 包：
+   ```bash
+   cd ~/rm_msgs
+   source /opt/ros/humble/setup.bash
+   colcon build
+   ```
+
+2. 编译视觉程序：
+   ```bash
+   cd /home/robofish/MOVE_AI
+   source /opt/ros/humble/setup.bash
+   source ~/rm_msgs/install/setup.bash
+   cmake -B build
+   make -C build/ sentry_mpc capture_ros -j$(nproc)
+   ```
+
+3. 运行程序：
+   ```bash
+   # 运行哨兵自瞄
+   source /opt/ros/humble/setup.bash
+   source ~/rm_msgs/install/setup.bash
+   ./build/sentry_mpc configs/sentry.yaml
+
+   # 运行标定采集
+   ./build/capture_ros configs/calibration.yaml -o assets/img_with_q
+   ```
+
+**ROS2 兼容性说明**
+
+- 项目支持条件编译，自动检测ROS2环境
+- 如果未安装ROS2，只编译串口/CAN版本的程序
+- 如果安装了ROS2和rm_msgs，会额外编译ROS2版本：
+  - `sentry_mpc`: 使用ROS2通信的哨兵自瞄程序
+  - `capture_ros`: 使用ROS2通信的标定采集程序
+- ROS2版本与串口版本功能完全相同，只是通信方式不同
+
+### 5. 协议总览
+
+| 协议 | 接口 | 速率 | 帧长 | 校验 | 适用设备 |
+|------|------|------|------|------|----------|
+| CBoard | CAN | 1Mbps | 8B | — | C 板 (STM32F407) |
+| Gimbal | 串口 | 可配置 | 29/43B | CRC16 | 达妙 MC02 (STM32H7) |
+| DM IMU | 串口 | 921600 | 57B | CRC16 | 达妙 IMU |
+| ROS2 | DDS | — | 可变 | DDS | 哨兵导航系统 |

src/component/CMakeLists.txt

@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.16)
+
+add_library(component OBJECT 
+    exiter.cpp
+    extended_kalman_filter.cpp
+    ransac_sine_fitter.cpp
+    img_tools.cpp
+    math_tools.cpp
+    plotter.cpp
+    trajectory.cpp
+    recorder.cpp
+    logger.cpp
+    pid.cpp
+    crc.cpp
+)
+target_link_libraries(component fmt::fmt spdlog::spdlog)

src/component/crc.cpp

@@ -0,0 +1,91 @@
+#include "crc.hpp"
+
+constexpr uint8_t CRC8_INIT = 0xff;
+const uint8_t CRC8_TABLE[256] = {
+  0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20, 0xa3, 0xfd, 0x1f, 0x41,
+  0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e, 0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc,
+  0x23, 0x7d, 0x9f, 0xc1, 0x42, 0x1c, 0xfe, 0xa0, 0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62,
+  0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d, 0x7c, 0x22, 0xc0, 0x9e, 0x1d, 0x43, 0xa1, 0xff,
+  0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5, 0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07,
+  0xdb, 0x85, 0x67, 0x39, 0xba, 0xe4, 0x06, 0x58, 0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a,
+  0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6, 0xa7, 0xf9, 0x1b, 0x45, 0xc6, 0x98, 0x7a, 0x24,
+  0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b, 0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9,
+  0x8c, 0xd2, 0x30, 0x6e, 0xed, 0xb3, 0x51, 0x0f, 0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd,
+  0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92, 0xd3, 0x8d, 0x6f, 0x31, 0xb2, 0xec, 0x0e, 0x50,
+  0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c, 0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee,
+  0x32, 0x6c, 0x8e, 0xd0, 0x53, 0x0d, 0xef, 0xb1, 0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73,
+  0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49, 0x08, 0x56, 0xb4, 0xea, 0x69, 0x37, 0xd5, 0x8b,
+  0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4, 0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16,
+  0xe9, 0xb7, 0x55, 0x0b, 0x88, 0xd6, 0x34, 0x6a, 0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8,
+  0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7, 0xb6, 0xe8, 0x0a, 0x54, 0xd7, 0x89, 0x6b, 0x35,
+};
+
+constexpr uint16_t CRC16_INIT = 0xffff;
+const uint16_t CRC16_TABLE[256] = {
+  0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48, 0x9dc1, 0xaf5a, 0xbed3,
+  0xca6c, 0xdbe5, 0xe97e, 0xf8f7, 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e,
+  0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, 0x2102, 0x308b, 0x0210, 0x1399,
+  0x6726, 0x76af, 0x4434, 0x55bd, 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5,
+  0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, 0xbdcb, 0xac42, 0x9ed9, 0x8f50,
+  0xfbef, 0xea66, 0xd8fd, 0xc974, 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
+  0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, 0x5285, 0x430c, 0x7197, 0x601e,
+  0x14a1, 0x0528, 0x37b3, 0x263a, 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72,
+  0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, 0xef4e, 0xfec7, 0xcc5c, 0xddd5,
+  0xa96a, 0xb8e3, 0x8a78, 0x9bf1, 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738,
+  0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, 0x8408, 0x9581, 0xa71a, 0xb693,
+  0xc22c, 0xd3a5, 0xe13e, 0xf0b7, 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
+  0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, 0x18c1, 0x0948, 0x3bd3, 0x2a5a,
+  0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5,
+  0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, 0xb58b, 0xa402, 0x9699, 0x8710,
+  0xf3af, 0xe226, 0xd0bd, 0xc134, 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c,
+  0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, 0x4a44, 0x5bcd, 0x6956, 0x78df,
+  0x0c60, 0x1de9, 0x2f72, 0x3efb, 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
+  0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, 0xe70e, 0xf687, 0xc41c, 0xd595,
+  0xa12a, 0xb0a3, 0x8238, 0x93b1, 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9,
+  0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c,
+  0x3de3, 0x2c6a, 0x1ef1, 0x0f78};
+
+namespace component
+{
+uint8_t get_crc8(const uint8_t * data, uint16_t len)
+{
+  uint8_t crc8 = CRC8_INIT;
+  uint8_t byte;
+  uint8_t i;
+
+  while (len--) {
+    byte = *data++;
+    i = crc8 ^ byte;
+    crc8 = CRC8_TABLE[i];
+  }
+
+  return crc8;
+}
+
+bool check_crc8(const uint8_t * data, uint16_t len)
+{
+  return get_crc8(data, len - 1) == data[len - 1];
+}
+
+uint16_t get_crc16(const uint8_t * data, uint32_t len)
+{
+  uint16_t crc16 = CRC16_INIT;
+  uint8_t byte;
+  uint8_t i;
+
+  while (len--) {
+    byte = *data++;
+    i = (crc16 ^ byte) & 0x00ff;
+    crc16 = (crc16 >> 8) ^ CRC16_TABLE[i];
+  }
+
+  return crc16;
+}
+
+bool check_crc16(const uint8_t * data, uint32_t len)
+{
+  uint16_t crc16 = (data[len - 1] << 8) | data[len - 2];
+  return get_crc16(data, len - 2) == crc16;
+}
+
+}  // namespace component

src/component/crc.hpp

@@ -0,0 +1,22 @@
+#ifndef COMPONENT__CRC_HPP
+#define COMPONENT__CRC_HPP
+
+#include <cstdint>
+
+namespace component
+{
+// len不包括crc8
+uint8_t get_crc8(const uint8_t * data, uint16_t len);
+
+// len包括crc8
+bool check_crc8(const uint8_t * data, uint16_t len);
+
+// len不包括crc16
+uint16_t get_crc16(const uint8_t * data, uint32_t len);
+
+// len包括crc16
+bool check_crc16(const uint8_t * data, uint32_t len);
+
+}  // namespace component
+
+#endif  // COMPONENT__CRC_HPP

src/component/exiter.cpp

@@ -0,0 +1,20 @@
+#include "exiter.hpp"
+
+#include <csignal>
+#include <stdexcept>
+
+namespace component
+{
+bool exit_ = false;
+bool exiter_inited_ = false;
+
+Exiter::Exiter()
+{
+  if (exiter_inited_) throw std::runtime_error("Multiple Exiter instances!");
+  std::signal(SIGINT, [](int) { exit_ = true; });
+  exiter_inited_ = true;
+}
+
+bool Exiter::exit() const { return exit_; }
+
+}  // namespace component

src/component/exiter.hpp

@@ -0,0 +1,16 @@
+#ifndef COMPONENT__EXITER_HPP
+#define COMPONENT__EXITER_HPP
+
+namespace component
+{
+class Exiter
+{
+public:
+  Exiter();
+
+  bool exit() const;
+};
+
+}  // namespace component
+
+#endif  // COMPONENT__EXITER_HPP

src/component/extended_kalman_filter.cpp

@@ -0,0 +1,94 @@
+#include "extended_kalman_filter.hpp"
+
+#include <numeric>
+
+namespace component
+{
+ExtendedKalmanFilter::ExtendedKalmanFilter(
+  const Eigen::VectorXd & x0, const Eigen::MatrixXd & P0,
+  std::function<Eigen::VectorXd(const Eigen::VectorXd &, const Eigen::VectorXd &)> x_add)
+: x(x0), P(P0), I(Eigen::MatrixXd::Identity(x0.rows(), x0.rows())), x_add(x_add)
+{
+  data["residual_yaw"] = 0.0;
+  data["residual_pitch"] = 0.0;
+  data["residual_distance"] = 0.0;
+  data["residual_angle"] = 0.0;
+  data["nis"] = 0.0;
+  data["nees"] = 0.0;
+  data["nis_fail"] = 0.0;
+  data["nees_fail"] = 0.0;
+  data["recent_nis_failures"] = 0.0;
+}
+
+Eigen::VectorXd ExtendedKalmanFilter::predict(const Eigen::MatrixXd & F, const Eigen::MatrixXd & Q)
+{
+  return predict(F, Q, [&](const Eigen::VectorXd & x) { return F * x; });
+}
+
+Eigen::VectorXd ExtendedKalmanFilter::predict(
+  const Eigen::MatrixXd & F, const Eigen::MatrixXd & Q,
+  std::function<Eigen::VectorXd(const Eigen::VectorXd &)> f)
+{
+  P = F * P * F.transpose() + Q;
+  x = f(x);
+  return x;
+}
+
+Eigen::VectorXd ExtendedKalmanFilter::update(
+  const Eigen::VectorXd & z, const Eigen::MatrixXd & H, const Eigen::MatrixXd & R,
+  std::function<Eigen::VectorXd(const Eigen::VectorXd &, const Eigen::VectorXd &)> z_subtract)
+{
+  return update(z, H, R, [&](const Eigen::VectorXd & x) { return H * x; }, z_subtract);
+}
+
+Eigen::VectorXd ExtendedKalmanFilter::update(
+  const Eigen::VectorXd & z, const Eigen::MatrixXd & H, const Eigen::MatrixXd & R,
+  std::function<Eigen::VectorXd(const Eigen::VectorXd &)> h,
+  std::function<Eigen::VectorXd(const Eigen::VectorXd &, const Eigen::VectorXd &)> z_subtract)
+{
+  Eigen::VectorXd x_prior = x;
+  Eigen::MatrixXd K = P * H.transpose() * (H * P * H.transpose() + R).inverse();
+
+  // Stable Compution of the Posterior Covariance
+  // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb
+  P = (I - K * H) * P * (I - K * H).transpose() + K * R * K.transpose();
+
+  x = x_add(x, K * z_subtract(z, h(x)));
+
+  /// 卡方检验
+  Eigen::VectorXd residual = z_subtract(z, h(x));
+  // 新增检验
+  Eigen::MatrixXd S = H * P * H.transpose() + R;
+  double nis = residual.transpose() * S.inverse() * residual;
+  double nees = (x - x_prior).transpose() * P.inverse() * (x - x_prior);
+
+  // 卡方检验阈值（自由度=4，取置信水平95%）
+  constexpr double nis_threshold = 0.711;
+  constexpr double nees_threshold = 0.711;
+
+  if (nis > nis_threshold) nis_count_++, data["nis_fail"] = 1;
+  if (nees > nees_threshold) nees_count_++, data["nees_fail"] = 1;
+  total_count_++;
+  last_nis = nis;
+
+  recent_nis_failures.push_back(nis > nis_threshold ? 1 : 0);
+
+  if (recent_nis_failures.size() > window_size) {
+    recent_nis_failures.pop_front();
+  }
+
+  int recent_failures = std::accumulate(recent_nis_failures.begin(), recent_nis_failures.end(), 0);
+  double recent_rate = static_cast<double>(recent_failures) / recent_nis_failures.size();
+
+  data["residual_yaw"] = residual[0];
+  data["residual_pitch"] = residual[1];
+  data["residual_distance"] = residual[2];
+  data["residual_angle"] = residual[3];
+  data["nis"] = nis;
+  data["nees"] = nees;
+  data["recent_nis_failures"] = recent_rate;
+
+  return x;
+}
+
+}  // namespace component

src/component/extended_kalman_filter.hpp

@@ -0,0 +1,57 @@
+#ifndef COMPONENT__EXTENDED_KALMAN_FILTER_HPP
+#define COMPONENT__EXTENDED_KALMAN_FILTER_HPP
+
+#include <Eigen/Dense>
+#include <deque>
+#include <functional>
+#include <map>
+
+namespace component
+{
+class ExtendedKalmanFilter
+{
+public:
+  Eigen::VectorXd x;
+  Eigen::MatrixXd P;
+
+  ExtendedKalmanFilter() = default;
+
+  ExtendedKalmanFilter(
+    const Eigen::VectorXd & x0, const Eigen::MatrixXd & P0,
+    std::function<Eigen::VectorXd(const Eigen::VectorXd &, const Eigen::VectorXd &)> x_add =
+      [](const Eigen::VectorXd & a, const Eigen::VectorXd & b) { return a + b; });
+
+  Eigen::VectorXd predict(const Eigen::MatrixXd & F, const Eigen::MatrixXd & Q);
+
+  Eigen::VectorXd predict(
+    const Eigen::MatrixXd & F, const Eigen::MatrixXd & Q,
+    std::function<Eigen::VectorXd(const Eigen::VectorXd &)> f);
+
+  Eigen::VectorXd update(
+    const Eigen::VectorXd & z, const Eigen::MatrixXd & H, const Eigen::MatrixXd & R,
+    std::function<Eigen::VectorXd(const Eigen::VectorXd &, const Eigen::VectorXd &)> z_subtract =
+      [](const Eigen::VectorXd & a, const Eigen::VectorXd & b) { return a - b; });
+
+  Eigen::VectorXd update(
+    const Eigen::VectorXd & z, const Eigen::MatrixXd & H, const Eigen::MatrixXd & R,
+    std::function<Eigen::VectorXd(const Eigen::VectorXd &)> h,
+    std::function<Eigen::VectorXd(const Eigen::VectorXd &, const Eigen::VectorXd &)> z_subtract =
+      [](const Eigen::VectorXd & a, const Eigen::VectorXd & b) { return a - b; });
+
+  std::map<std::string, double> data;  //卡方检验数据
+  std::deque<int> recent_nis_failures{0};
+  size_t window_size = 100;
+  double last_nis;
+
+private:
+  Eigen::MatrixXd I;
+  std::function<Eigen::VectorXd(const Eigen::VectorXd &, const Eigen::VectorXd &)> x_add;
+
+  int nees_count_ = 0;
+  int nis_count_ = 0;
+  int total_count_ = 0;
+};
+
+}  // namespace component
+
+#endif  // COMPONENT__EXTENDED_KALMAN_FILTER_HPP

src/component/img_tools.cpp

@@ -0,0 +1,31 @@
+#include "img_tools.hpp"
+
+namespace component
+{
+void draw_point(cv::Mat & img, const cv::Point & point, const cv::Scalar & color, int radius)
+{
+  cv::circle(img, point, radius, color, -1);
+}
+
+void draw_points(
+  cv::Mat & img, const std::vector<cv::Point> & points, const cv::Scalar & color, int thickness)
+{
+  std::vector<std::vector<cv::Point>> contours = {points};
+  cv::drawContours(img, contours, -1, color, thickness);
+}
+
+void draw_points(
+  cv::Mat & img, const std::vector<cv::Point2f> & points, const cv::Scalar & color, int thickness)
+{
+  std::vector<cv::Point> int_points(points.begin(), points.end());
+  draw_points(img, int_points, color, thickness);
+}
+
+void draw_text(
+  cv::Mat & img, const std::string & text, const cv::Point & point, const cv::Scalar & color,
+  double font_scale, int thickness)
+{
+  cv::putText(img, text, point, cv::FONT_HERSHEY_SIMPLEX, font_scale, color, thickness);
+}
+
+}  // namespace component

src/component/img_tools.hpp

@@ -0,0 +1,27 @@
+#ifndef COMPONENT__IMG_TOOLS_HPP
+#define COMPONENT__IMG_TOOLS_HPP
+
+#include <opencv2/opencv.hpp>
+#include <string>
+#include <vector>
+
+namespace component
+{
+void draw_point(
+  cv::Mat & img, const cv::Point & point, const cv::Scalar & color = {0, 0, 255}, int radius = 3);
+
+void draw_points(
+  cv::Mat & img, const std::vector<cv::Point> & points, const cv::Scalar & color = {0, 0, 255},
+  int thickness = 2);
+
+void draw_points(
+  cv::Mat & img, const std::vector<cv::Point2f> & points, const cv::Scalar & color = {0, 0, 255},
+  int thickness = 2);
+
+void draw_text(
+  cv::Mat & img, const std::string & text, const cv::Point & point,
+  const cv::Scalar & color = {0, 255, 255}, double font_scale = 1.0, int thickness = 2);
+
+}  // namespace component
+
+#endif  // COMPONENT__IMG_TOOLS_HPP

src/component/logger.cpp

@@ -0,0 +1,35 @@
+#include "logger.hpp"
+
+#include <fmt/chrono.h>
+#include <spdlog/sinks/basic_file_sink.h>
+#include <spdlog/sinks/stdout_color_sinks.h>
+#include <spdlog/spdlog.h>
+
+#include <chrono>
+#include <string>
+
+namespace component
+{
+std::shared_ptr<spdlog::logger> logger_ = nullptr;
+
+void set_logger()
+{
+  auto file_name = fmt::format("logs/{:%Y-%m-%d_%H-%M-%S}.log", std::chrono::system_clock::now());
+  auto file_sink = std::make_shared<spdlog::sinks::basic_file_sink_mt>(file_name, true);
+  file_sink->set_level(spdlog::level::debug);
+
+  auto console_sink = std::make_shared<spdlog::sinks::stdout_color_sink_mt>();
+  console_sink->set_level(spdlog::level::debug);
+
+  logger_ = std::make_shared<spdlog::logger>("", spdlog::sinks_init_list{file_sink, console_sink});
+  logger_->set_level(spdlog::level::debug);
+  logger_->flush_on(spdlog::level::info);
+}
+
+std::shared_ptr<spdlog::logger> logger()
+{
+  if (!logger_) set_logger();
+  return logger_;
+}
+
+}  // namespace component

src/component/logger.hpp

@@ -0,0 +1,12 @@
+#ifndef COMPONENT__LOGGER_HPP
+#define COMPONENT__LOGGER_HPP
+
+#include <spdlog/spdlog.h>
+
+namespace component
+{
+std::shared_ptr<spdlog::logger> logger();
+
+}  // namespace component
+
+#endif  // COMPONENT__LOGGER_HPP

src/component/math_tools.cpp

@@ -0,0 +1,202 @@
+#include "math_tools.hpp"
+
+#include <cmath>
+#include <opencv2/core.hpp>  // CV_PI
+
+namespace component
+{
+double limit_rad(double angle)
+{
+  while (angle > CV_PI) angle -= 2 * CV_PI;
+  while (angle <= -CV_PI) angle += 2 * CV_PI;
+  return angle;
+}
+
+Eigen::Vector3d eulers(Eigen::Quaterniond q, int axis0, int axis1, int axis2, bool extrinsic)
+{
+  if (!extrinsic) std::swap(axis0, axis2);
+
+  auto i = axis0, j = axis1, k = axis2;
+  auto is_proper = (i == k);
+  if (is_proper) k = 3 - i - j;
+  auto sign = (i - j) * (j - k) * (k - i) / 2;
+
+  double a, b, c, d;
+  Eigen::Vector4d xyzw = q.coeffs();
+  if (is_proper) {
+    a = xyzw[3];
+    b = xyzw[i];
+    c = xyzw[j];
+    d = xyzw[k] * sign;
+  } else {
+    a = xyzw[3] - xyzw[j];
+    b = xyzw[i] + xyzw[k] * sign;
+    c = xyzw[j] + xyzw[3];
+    d = xyzw[k] * sign - xyzw[i];
+  }
+
+  Eigen::Vector3d eulers;
+  auto n2 = a * a + b * b + c * c + d * d;
+  eulers[1] = std::acos(2 * (a * a + b * b) / n2 - 1);
+
+  auto half_sum = std::atan2(b, a);
+  auto half_diff = std::atan2(-d, c);
+
+  auto eps = 1e-7;
+  auto safe1 = std::abs(eulers[1]) >= eps;
+  auto safe2 = std::abs(eulers[1] - CV_PI) >= eps;
+  auto safe = safe1 && safe2;
+  if (safe) {
+    eulers[0] = half_sum + half_diff;
+    eulers[2] = half_sum - half_diff;
+  } else {
+    if (!extrinsic) {
+      eulers[0] = 0;
+      if (!safe1) eulers[2] = 2 * half_sum;
+      if (!safe2) eulers[2] = -2 * half_diff;
+    } else {
+      eulers[2] = 0;
+      if (!safe1) eulers[0] = 2 * half_sum;
+      if (!safe2) eulers[0] = 2 * half_diff;
+    }
+  }
+
+  for (int i = 0; i < 3; i++) eulers[i] = limit_rad(eulers[i]);
+
+  if (!is_proper) {
+    eulers[2] *= sign;
+    eulers[1] -= CV_PI / 2;
+  }
+
+  if (!extrinsic) std::swap(eulers[0], eulers[2]);
+
+  return eulers;
+}
+
+Eigen::Vector3d eulers(Eigen::Matrix3d R, int axis0, int axis1, int axis2, bool extrinsic)
+{
+  Eigen::Quaterniond q(R);
+  return eulers(q, axis0, axis1, axis2, extrinsic);
+}
+
+Eigen::Matrix3d rotation_matrix(const Eigen::Vector3d & ypr)
+{
+  double roll = ypr[2];
+  double pitch = ypr[1];
+  double yaw = ypr[0];
+  double cos_yaw = cos(yaw);
+  double sin_yaw = sin(yaw);
+  double cos_pitch = cos(pitch);
+  double sin_pitch = sin(pitch);
+  double cos_roll = cos(roll);
+  double sin_roll = sin(roll);
+  // clang-format off
+    Eigen::Matrix3d R{
+      {cos_yaw * cos_pitch, cos_yaw * sin_pitch * sin_roll - sin_yaw * cos_roll, cos_yaw * sin_pitch * cos_roll + sin_yaw * sin_roll},
+      {sin_yaw * cos_pitch, sin_yaw * sin_pitch * sin_roll + cos_yaw * cos_roll, sin_yaw * sin_pitch * cos_roll - cos_yaw * sin_roll},
+      {         -sin_pitch,                                cos_pitch * sin_roll,                                cos_pitch * cos_roll}
+    };
+  // clang-format on
+  return R;
+}
+
+Eigen::Vector3d xyz2ypd(const Eigen::Vector3d & xyz)
+{
+  auto x = xyz[0], y = xyz[1], z = xyz[2];
+  auto yaw = std::atan2(y, x);
+  auto pitch = std::atan2(z, std::sqrt(x * x + y * y));
+  auto distance = std::sqrt(x * x + y * y + z * z);
+  return {yaw, pitch, distance};
+}
+
+Eigen::MatrixXd xyz2ypd_jacobian(const Eigen::Vector3d & xyz)
+{
+  auto x = xyz[0], y = xyz[1], z = xyz[2];
+
+  auto dyaw_dx = -y / (x * x + y * y);
+  auto dyaw_dy = x / (x * x + y * y);
+  auto dyaw_dz = 0.0;
+
+  auto dpitch_dx = -(x * z) / ((z * z / (x * x + y * y) + 1) * std::pow((x * x + y * y), 1.5));
+  auto dpitch_dy = -(y * z) / ((z * z / (x * x + y * y) + 1) * std::pow((x * x + y * y), 1.5));
+  auto dpitch_dz = 1 / ((z * z / (x * x + y * y) + 1) * std::pow((x * x + y * y), 0.5));
+
+  auto ddistance_dx = x / std::pow((x * x + y * y + z * z), 0.5);
+  auto ddistance_dy = y / std::pow((x * x + y * y + z * z), 0.5);
+  auto ddistance_dz = z / std::pow((x * x + y * y + z * z), 0.5);
+
+  // clang-format off
+  Eigen::MatrixXd J{
+    {dyaw_dx, dyaw_dy, dyaw_dz},
+    {dpitch_dx, dpitch_dy, dpitch_dz},
+    {ddistance_dx, ddistance_dy, ddistance_dz}
+  };
+  // clang-format on
+
+  return J;
+}
+
+Eigen::Vector3d ypd2xyz(const Eigen::Vector3d & ypd)
+{
+  auto yaw = ypd[0], pitch = ypd[1], distance = ypd[2];
+  auto x = distance * std::cos(pitch) * std::cos(yaw);
+  auto y = distance * std::cos(pitch) * std::sin(yaw);
+  auto z = distance * std::sin(pitch);
+  return {x, y, z};
+}
+
+Eigen::MatrixXd ypd2xyz_jacobian(const Eigen::Vector3d & ypd)
+{
+  auto yaw = ypd[0], pitch = ypd[1], distance = ypd[2];
+  double cos_yaw = std::cos(yaw);
+  double sin_yaw = std::sin(yaw);
+  double cos_pitch = std::cos(pitch);
+  double sin_pitch = std::sin(pitch);
+
+  auto dx_dyaw = distance * cos_pitch * -sin_yaw;
+  auto dy_dyaw = distance * cos_pitch * cos_yaw;
+  auto dz_dyaw = 0.0;
+
+  auto dx_dpitch = distance * -sin_pitch * cos_yaw;
+  auto dy_dpitch = distance * -sin_pitch * sin_yaw;
+  auto dz_dpitch = distance * cos_pitch;
+
+  auto dx_ddistance = cos_pitch * cos_yaw;
+  auto dy_ddistance = cos_pitch * sin_yaw;
+  auto dz_ddistance = sin_pitch;
+
+  // clang-format off
+  Eigen::MatrixXd J{
+    {dx_dyaw, dx_dpitch, dx_ddistance},
+    {dy_dyaw, dy_dpitch, dy_ddistance},
+    {dz_dyaw, dz_dpitch, dz_ddistance}
+  };
+  // clang-format on
+
+  return J;
+}
+
+double delta_time(
+  const std::chrono::steady_clock::time_point & a, const std::chrono::steady_clock::time_point & b)
+{
+  std::chrono::duration<double> c = a - b;
+  return c.count();
+}
+
+double get_abs_angle(const Eigen::Vector2d & vec1, const Eigen::Vector2d & vec2)
+{
+  if (vec1.norm() == 0. || vec2.norm() == 0.) {
+    return 0.;
+  }
+  return std::acos(vec1.dot(vec2) / (vec1.norm() * vec2.norm()));
+}
+
+double limit_min_max(double input, double min, double max)
+{
+  if (input > max)
+    return max;
+  else if (input < min)
+    return min;
+  return input;
+}
+}  // namespace component

src/component/math_tools.hpp

@@ -0,0 +1,58 @@
+#ifndef COMPONENT__MATH_TOOLS_HPP
+#define COMPONENT__MATH_TOOLS_HPP
+
+#include <Eigen/Geometry>
+#include <chrono>
+
+namespace component
+{
+// 将弧度值限制在(-pi, pi]
+double limit_rad(double angle);
+
+// 四元数转欧拉角
+// x = 0, y = 1, z = 2
+// e.g. 先绕z轴旋转，再绕y轴旋转，最后绕x轴旋转：axis0=2, axis1=1, axis2=0
+// 参考：https://github.com/evbernardes/quaternion_to_euler
+Eigen::Vector3d eulers(
+  Eigen::Quaterniond q, int axis0, int axis1, int axis2, bool extrinsic = false);
+
+// 旋转矩阵转欧拉角
+// x = 0, y = 1, z = 2
+// e.g. 先绕z轴旋转，再绕y轴旋转，最后绕x轴旋转：axis0=2, axis1=1, axis2=0
+Eigen::Vector3d eulers(Eigen::Matrix3d R, int axis0, int axis1, int axis2, bool extrinsic = false);
+
+// 欧拉角转旋转矩阵
+// zyx:先绕z轴旋转，再绕y轴旋转，最后绕x轴旋转
+Eigen::Matrix3d rotation_matrix(const Eigen::Vector3d & ypr);
+
+// 直角坐标系转球坐标系
+// ypd为yaw、pitch、distance的缩写
+Eigen::Vector3d xyz2ypd(const Eigen::Vector3d & xyz);
+
+// 直角坐标系转球坐标系转换函数对xyz的雅可比矩阵
+Eigen::MatrixXd xyz2ypd_jacobian(const Eigen::Vector3d & xyz);
+
+// 球坐标系转直角坐标系
+Eigen::Vector3d ypd2xyz(const Eigen::Vector3d & ypd);
+
+// 球坐标系转直角坐标系转换函数对xyz的雅可比矩阵
+Eigen::MatrixXd ypd2xyz_jacobian(const Eigen::Vector3d & ypd);
+
+// 计算时间差a - b，单位：s
+double delta_time(
+  const std::chrono::steady_clock::time_point & a, const std::chrono::steady_clock::time_point & b);
+
+// 向量夹角 总是返回 0 ~ pi 来自SJTU
+double get_abs_angle(const Eigen::Vector2d & vec1, const Eigen::Vector2d & vec2);
+
+// 返回输入值的平方
+template <typename T>
+T square(T const & a)
+{
+  return a * a;
+};
+
+double limit_min_max(double input, double min, double max);
+}  // namespace component
+
+#endif  // COMPONENT__MATH_TOOLS_HPP

src/component/pid.cpp

@@ -0,0 +1,27 @@
+#include "pid.hpp"
+
+#include "math_tools.hpp"
+
+float clip(float value, float min, float max) { return std::max(min, std::min(max, value)); }
+
+namespace component
+{
+PID::PID(float dt, float kp, float ki, float kd, float max_out, float max_iout, bool angular)
+: dt_(dt), kp_(kp), ki_(ki), kd_(kd), max_out_(max_out), max_iout_(max_iout), angular_(angular)
+{
+}
+
+float PID::calc(float set, float fdb)
+{
+  float e = angular_ ? limit_rad(set - fdb) : (set - fdb);
+  float de = angular_ ? limit_rad(last_fdb_ - fdb) : (last_fdb_ - fdb);
+  last_fdb_ = fdb;
+
+  this->pout = e * kp_;
+  this->iout = clip(this->iout + e * dt_ * ki_, -max_iout_, max_iout_);
+  this->dout = de / dt_ * kd_;
+
+  return clip(this->pout + this->iout + this->dout, -max_out_, max_out_);
+}
+
+}  // namespace component

src/component/pid.hpp

@@ -0,0 +1,37 @@
+#ifndef COMPONENT__PID_HPP
+#define COMPONENT__PID_HPP
+
+namespace component
+{
+class PID
+{
+public:
+  // dt: 控制周期, 单位: s
+  // kp: P项系数
+  // ki: I项系数
+  // kd: D项系数
+  // max_out: PID最大输出值
+  // max_iout I项最大输出值
+  PID(float dt, float kp, float ki, float kd, float max_out, float max_iout, bool angular = false);
+
+  float pout = 0.0f;  // P项输出, 用于调试
+  float iout = 0.0f;  // I项输出, 用于调试
+  float dout = 0.0f;  // D项输出, 用于调试
+
+  // 计算PID输出值
+  // set: 目标值
+  // fdb: 反馈值(feedback)
+  float calc(float set, float fdb);
+
+private:
+  const float dt_;
+  const float kp_, ki_, kd_;
+  const float max_out_, max_iout_;
+  const bool angular_;
+
+  float last_fdb_ = 0.0f;  // 上次反馈值
+};
+
+}  // namespace component
+
+#endif  // COMPONENT__PID_HPP

src/component/plotter.cpp

@@ -0,0 +1,29 @@
+#include "plotter.hpp"
+
+#include <arpa/inet.h>   // htons, inet_addr
+#include <sys/socket.h>  // socket, sendto
+#include <unistd.h>      // close
+
+namespace component
+{
+Plotter::Plotter(std::string host, uint16_t port)
+{
+  socket_ = ::socket(AF_INET, SOCK_DGRAM, 0);
+
+  destination_.sin_family = AF_INET;
+  destination_.sin_port = ::htons(port);
+  destination_.sin_addr.s_addr = ::inet_addr(host.c_str());
+}
+
+Plotter::~Plotter() { ::close(socket_); }
+
+void Plotter::plot(const nlohmann::json & json)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  auto data = json.dump();
+  ::sendto(
+    socket_, data.c_str(), data.length(), 0, reinterpret_cast<sockaddr *>(&destination_),
+    sizeof(destination_));
+}
+
+}  // namespace component

src/component/plotter.hpp

@@ -0,0 +1,29 @@
+#ifndef COMPONENT__PLOTTER_HPP
+#define COMPONENT__PLOTTER_HPP
+
+#include <netinet/in.h>  // sockaddr_in
+
+#include <mutex>
+#include <nlohmann/json.hpp>
+#include <string>
+
+namespace component
+{
+class Plotter
+{
+public:
+  Plotter(std::string host = "127.0.0.1", uint16_t port = 9870);
+
+  ~Plotter();
+
+  void plot(const nlohmann::json & json);
+
+private:
+  int socket_;
+  sockaddr_in destination_;
+  std::mutex mutex_;
+};
+
+}  // namespace component
+
+#endif  // COMPONENT__PLOTTER_HPP

src/component/ransac_sine_fitter.cpp

@@ -0,0 +1,105 @@
+#include "ransac_sine_fitter.hpp"
+
+#include <Eigen/Dense>
+#include <algorithm>
+#include <cmath>
+#include <random>
+
+namespace component
+{
+
+RansacSineFitter::RansacSineFitter(
+  int max_iterations, double threshold, double min_omega, double max_omega)
+: max_iterations_(max_iterations),
+  threshold_(threshold),
+  min_omega_(min_omega),
+  max_omega_(max_omega),
+  gen_(std::random_device{}())
+{
+}
+
+void RansacSineFitter::add_data(double t, double v)
+{
+  if (fit_data_.size() > 0 && (t - fit_data_.back().first > 5)) fit_data_.clear();
+  fit_data_.emplace_back(std::make_pair(t, v));
+}
+
+void RansacSineFitter::fit()
+{
+  if (fit_data_.size() < 3) return;
+
+  std::uniform_real_distribution<double> omega_dist(min_omega_, max_omega_);
+  std::vector<size_t> indices(fit_data_.size());
+  std::iota(indices.begin(), indices.end(), 0);
+
+  for (int iter = 0; iter < max_iterations_; ++iter) {
+    std::shuffle(indices.begin(), indices.end(), gen_);
+
+    std::vector<std::pair<double, double>> sample;
+    for (int i = 0; i < 3; ++i) {
+      sample.push_back(fit_data_[indices[i]]);
+    }
+
+    double omega = omega_dist(gen_);
+    Eigen::Vector3d params;
+    if (!fit_partial_model(sample, omega, params)) continue;
+
+    double A1 = params(0);
+    double A2 = params(1);
+    double C = params(2);
+
+    double A = std::sqrt(A1 * A1 + A2 * A2);
+    double phi = std::atan2(A2, A1);
+
+    int inlier_count = evaluate_inliers(A, omega, phi, C);
+
+    if (inlier_count > best_result_.inliers) {
+      best_result_.A = A;
+      best_result_.omega = omega;
+      best_result_.phi = phi;
+      best_result_.C = C;
+      best_result_.inliers = inlier_count;
+    }
+  }
+
+  if (fit_data_.size() > 150) fit_data_.pop_front();
+}
+
+bool RansacSineFitter::fit_partial_model(
+  const std::vector<std::pair<double, double>> & sample, double omega, Eigen::Vector3d & params)
+{
+  Eigen::MatrixXd X(sample.size(), 3);
+  Eigen::VectorXd Y(sample.size());
+
+  for (size_t i = 0; i < sample.size(); ++i) {
+    double t = sample[i].first;
+    double y = sample[i].second;
+    X(i, 0) = std::sin(omega * t);
+    X(i, 1) = std::cos(omega * t);
+    X(i, 2) = 1.0;
+    Y(i) = y;
+  }
+
+  try {
+    params = X.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(Y);
+    return true;
+  } catch (...) {
+    return false;
+  }
+}
+
+int RansacSineFitter::evaluate_inliers(double A, double omega, double phi, double C)
+{
+  int count = 0;
+  for (const auto & p : fit_data_) {
+    double t = p.first;
+    double y = p.second;
+    double pred = A * std::sin(omega * t + phi) + C;
+    if (std::abs(y - pred) < threshold_) {
+      ++count;
+    }
+  }
+  return count;
+}
+
+}  // namespace component

src/component/ransac_sine_fitter.hpp

@@ -0,0 +1,50 @@
+#pragma once
+
+#include <Eigen/Dense>
+#include <deque>
+#include <iostream>
+#include <random>
+#include <vector>
+
+namespace component
+{
+
+class RansacSineFitter
+{
+public:
+  struct Result
+  {
+    double A = 0.0;
+    double omega = 0.0;
+    double phi = 0.0;
+    double C = 0.0;
+    int inliers = 0;
+  };
+  Result best_result_;
+
+  RansacSineFitter(int max_iterations, double threshold, double min_omega, double max_omega);
+
+  void add_data(double t, double v);
+
+  void fit();
+
+  double sine_function(double t, double A, double omega, double phi, double C)
+  {
+    return A * std::sin(omega * t + phi) + C;
+  }
+
+private:
+  int max_iterations_;
+  double threshold_;
+  double min_omega_;
+  double max_omega_;
+  std::mt19937 gen_;
+  std::deque<std::pair<double, double>> fit_data_;
+
+  bool fit_partial_model(
+    const std::vector<std::pair<double, double>> & sample, double omega, Eigen::Vector3d & params);
+
+  int evaluate_inliers(double A, double omega, double phi, double C);
+};
+
+}  // namespace component

src/component/recorder.cpp

@@ -0,0 +1,81 @@
+#include "recorder.hpp"
+
+#include <fmt/chrono.h>
+
+#include <filesystem>
+#include <string>
+
+#include "math_tools.hpp"
+#include "src/component/logger.hpp"
+
+namespace component
+{
+Recorder::Recorder(double fps) : init_(false), fps_(fps), queue_(1), stop_thread_(false)
+{
+  start_time_ = std::chrono::steady_clock::now();
+  last_time_ = start_time_;
+
+  auto folder_path = "records";
+  auto file_name = fmt::format("{:%Y-%m-%d_%H-%M-%S}", std::chrono::system_clock::now());
+  text_path_ = fmt::format("{}/{}.txt", folder_path, file_name);
+  video_path_ = fmt::format("{}/{}.avi", folder_path, file_name);
+
+  std::filesystem::create_directory(folder_path);
+}
+
+Recorder::~Recorder()
+{
+  stop_thread_ = true;
+  // 退出时给队列中额外推入一个空帧，避免pop一直等待
+  queue_.push({cv::Mat::zeros(0, 0, 0), {0, 0, 0, 0}, std::chrono::steady_clock::now()});
+  if (saving_thread_.joinable()) saving_thread_.join();  // 等待视频保存线程结束
+
+  if (!init_) return;
+  text_writer_.close();
+  video_writer_.release();
+}
+
+void Recorder::save_to_file()
+{
+  while (!stop_thread_) {
+    FrameData frame;
+    queue_.pop(frame);  // 从队列中取出帧数据
+    if (frame.img.empty()) {
+      component::logger()->debug("Recorder received empty img. Skip this frame.");
+      continue;
+    }
+    // 写入视频文件
+    video_writer_.write(frame.img);
+
+    // 写入文本文件（输出顺序为wxyz）
+    Eigen::Vector4d xyzw = frame.q.coeffs();
+    auto since_begin = component::delta_time(frame.timestamp, start_time_);
+    text_writer_ << fmt::format(
+      "{} {} {} {} {}\n", since_begin, xyzw[3], xyzw[0], xyzw[1], xyzw[2]);
+  }
+}
+
+void Recorder::record(
+  const cv::Mat & img, const Eigen::Quaterniond & q,
+  const std::chrono::steady_clock::time_point & timestamp)
+{
+  if (img.empty()) return;
+  if (!init_) init(img);
+
+  auto since_last = component::delta_time(timestamp, last_time_);
+  if (since_last < 1.0 / fps_) return;
+
+  last_time_ = timestamp;
+  queue_.push({img, q, timestamp});
+}
+
+void Recorder::init(const cv::Mat & img)
+{
+  text_writer_.open(text_path_);
+  auto fourcc = cv::VideoWriter::fourcc('M', 'J', 'P', 'G');
+  video_writer_ = cv::VideoWriter(video_path_, fourcc, fps_, img.size());
+  saving_thread_ = std::thread(&Recorder::save_to_file, this);  // 启动保存线程
+  init_ = true;
+}
+
+}  // namespace component

src/component/recorder.hpp

@@ -0,0 +1,46 @@
+#ifndef COMPONENT__RECORDER_HPP
+#define COMPONENT__RECORDER_HPP
+
+#include <Eigen/Geometry>
+#include <chrono>
+#include <fstream>
+#include <opencv2/opencv.hpp>
+#include <thread>
+
+#include "src/component/thread_safe_queue.hpp"
+namespace component
+{
+class Recorder
+{
+public:
+  Recorder(double fps = 30);
+  ~Recorder();
+  void record(
+    const cv::Mat & img, const Eigen::Quaterniond & q,
+    const std::chrono::steady_clock::time_point & timestamp);
+
+private:
+  struct FrameData
+  {
+    cv::Mat img;
+    Eigen::Quaterniond q;
+    std::chrono::steady_clock::time_point timestamp;
+  };
+  bool init_;
+  std::atomic<bool> stop_thread_;
+  double fps_;
+  std::string text_path_;
+  std::string video_path_;
+  std::ofstream text_writer_;
+  cv::VideoWriter video_writer_;
+  std::chrono::steady_clock::time_point start_time_;
+  std::chrono::steady_clock::time_point last_time_;
+  component::ThreadSafeQueue<FrameData> queue_;
+  std::thread saving_thread_;  // 负责保存帧数据的线程
+  void init(const cv::Mat & img);
+  void save_to_file();
+};
+
+}  // namespace component
+
+#endif  // COMPONENT__RECORDER_HPP

src/component/thread_pool.hpp

@@ -0,0 +1,185 @@
+#ifndef COMPONENT__THREAD_POOL_HPP
+#define COMPONENT__THREAD_POOL_HPP
+
+#include <condition_variable>
+#include <functional>
+#include <mutex>
+#include <queue>
+#include <thread>
+#include <vector>
+
+#include "src/module/auto_aim/yolo.hpp"
+#include "src/component/logger.hpp"
+
+namespace component
+{
+struct Frame
+{
+  int id;
+  cv::Mat img;
+  std::chrono::steady_clock::time_point t;
+  Eigen::Quaterniond q;
+  std::list<auto_aim::Armor> armors;
+};
+
+inline std::vector<auto_aim::YOLO> create_yolo11s(
+  const std::string & config_path, int numebr, bool debug)
+{
+  std::vector<auto_aim::YOLO> yolo11s;
+  for (int i = 0; i < numebr; i++) {
+    yolo11s.push_back(auto_aim::YOLO(config_path, debug));
+  }
+  return yolo11s;
+}
+
+inline std::vector<auto_aim::YOLO> create_yolov8s(
+  const std::string & config_path, int numebr, bool debug)
+{
+  std::vector<auto_aim::YOLO> yolov8s;
+  for (int i = 0; i < numebr; i++) {
+    yolov8s.push_back(auto_aim::YOLO(config_path, debug));
+  }
+  return yolov8s;
+}
+
+class OrderedQueue
+{
+public:
+  OrderedQueue() : current_id_(1) {}
+  ~OrderedQueue()
+  {
+    {
+      std::lock_guard<std::mutex> lock(mutex_);
+
+      main_queue_ = std::queue<component::Frame>();
+      buffer_.clear();
+      current_id_ = 0;
+    }
+    component::logger()->info("OrderedQueue destroyed, queue and buffer cleared.");
+  }
+
+  void enqueue(const component::Frame & item)
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+
+    if (item.id < current_id_) {
+      component::logger()->warn("small id");
+      return;
+    }
+
+    if (item.id == current_id_) {
+      main_queue_.push(item);
+      current_id_++;
+
+      auto it = buffer_.find(current_id_);
+      while (it != buffer_.end()) {
+        main_queue_.push(it->second);
+        buffer_.erase(it);
+        current_id_++;
+        it = buffer_.find(current_id_);
+      }
+
+      if (main_queue_.size() >= 1) {
+        cond_var_.notify_one();
+      }
+    } else {
+      buffer_[item.id] = item;
+    }
+  }
+
+  component::Frame dequeue()
+  {
+    std::unique_lock<std::mutex> lock(mutex_);
+
+    cond_var_.wait(lock, [this]() { return !main_queue_.empty(); });
+
+    component::Frame item = main_queue_.front();
+    main_queue_.pop();
+    return item;
+  }
+
+  // 不会阻塞队列
+  bool try_dequeue(component::Frame & item)
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (main_queue_.empty()) {
+      return false;
+    }
+    item = main_queue_.front();
+    main_queue_.pop();
+    return true;
+  }
+
+  size_t get_size() { return main_queue_.size() + buffer_.size(); }
+
+private:
+  std::queue<component::Frame> main_queue_;
+  std::unordered_map<int, component::Frame> buffer_;
+  int current_id_;
+  std::mutex mutex_;
+  std::condition_variable cond_var_;
+};
+
+class ThreadPool
+{
+public:
+  ThreadPool(size_t num_threads) : stop(false)
+  {
+    for (size_t i = 0; i < num_threads; ++i) {
+      workers.emplace_back([this] {
+        while (true) {
+          std::function<void()> task;
+          {
+            std::unique_lock<std::mutex> lock(queue_mutex);
+            condition.wait(lock, [this] { return stop || !tasks.empty(); });
+            if (stop && tasks.empty()) {
+              return;
+            }
+            task = std::move(tasks.front());
+            tasks.pop();
+          }
+          task();
+        }
+      });
+    }
+  }
+
+  ~ThreadPool()
+  {
+    {
+      std::unique_lock<std::mutex> lock(queue_mutex);
+      stop = true;
+      tasks = std::queue<std::function<void()>>();
+    }
+    condition.notify_all();
+    for (std::thread & worker : workers) {
+      if (worker.joinable()) {
+        worker.join();
+      }
+    }
+  }
+
+  // 添加任务到任务队列
+  template <class F>
+  void enqueue(F && f)
+  {
+    {
+      std::unique_lock<std::mutex> lock(queue_mutex);
+      if (stop) {
+        throw std::runtime_error("enqueue on stopped ThreadPool");
+      }
+      tasks.emplace(std::forward<F>(f));
+    }
+    condition.notify_one();
+  }
+
+private:
+  std::vector<std::thread> workers;         // 工作线程
+  std::queue<std::function<void()>> tasks;  // 任务队列
+  std::mutex queue_mutex;                   // 任务队列互斥锁
+  std::condition_variable condition;        // 条件变量，用于等待任务
+  bool stop;                                // 是否停止线程池
+};
+}  // namespace component
+
+#endif  // COMPONENT__THREAD_POOL_HPP

src/component/thread_safe_queue.hpp

@@ -0,0 +1,111 @@
+#ifndef COMPONENT__THREAD_SAFE_QUEUE_HPP
+#define COMPONENT__THREAD_SAFE_QUEUE_HPP
+
+#include <condition_variable>
+#include <functional>
+#include <iostream>
+#include <mutex>
+#include <queue>
+
+namespace component
+{
+template <typename T, bool PopWhenFull = false>
+class ThreadSafeQueue
+{
+public:
+  ThreadSafeQueue(
+    size_t max_size, std::function<void(void)> full_handler = [] {})
+  : max_size_(max_size), full_handler_(full_handler)
+  {
+  }
+
+  void push(const T & value)
+  {
+    std::unique_lock<std::mutex> lock(mutex_);
+
+    if (queue_.size() >= max_size_) {
+      if (PopWhenFull) {
+        queue_.pop();
+      } else {
+        full_handler_();
+        return;
+      }
+    }
+
+    queue_.push(value);
+    not_empty_condition_.notify_all();
+  }
+
+  void pop(T & value)
+  {
+    std::unique_lock<std::mutex> lock(mutex_);
+
+    not_empty_condition_.wait(lock, [this] { return !queue_.empty(); });
+
+    if (queue_.empty()) {
+      std::cerr << "Error: Attempt to pop from an empty queue." << std::endl;
+      return;
+    }
+
+    value = queue_.front();
+    queue_.pop();
+  }
+
+  T pop()
+  {
+    std::unique_lock<std::mutex> lock(mutex_);
+
+    not_empty_condition_.wait(lock, [this] { return !queue_.empty(); });
+
+    T value = std::move(queue_.front());
+    queue_.pop();
+    return std::move(value);
+  }
+
+  T front()
+  {
+    std::unique_lock<std::mutex> lock(mutex_);
+
+    not_empty_condition_.wait(lock, [this] { return !queue_.empty(); });
+
+    return queue_.front();
+  }
+
+  void back(T & value)
+  {
+    std::unique_lock<std::mutex> lock(mutex_);
+
+    if (queue_.empty()) {
+      std::cerr << "Error: Attempt to access the back of an empty queue." << std::endl;
+      return;
+    }
+
+    value = queue_.back();
+  }
+
+  bool empty()
+  {
+    std::unique_lock<std::mutex> lock(mutex_);
+    return queue_.empty();
+  }
+
+  void clear()
+  {
+    std::unique_lock<std::mutex> lock(mutex_);
+    while (!queue_.empty()) {
+      queue_.pop();
+    }
+    not_empty_condition_.notify_all();  // 如果其他线程正在等待队列不为空，这样可以唤醒它们
+  }
+
+private:
+  std::queue<T> queue_;
+  size_t max_size_;
+  mutable std::mutex mutex_;
+  std::condition_variable not_empty_condition_;
+  std::function<void(void)> full_handler_;
+};
+
+}  // namespace component
+
+#endif  // COMPONENT__THREAD_SAFE_QUEUE_HPP

src/component/trajectory.cpp

@@ -0,0 +1,33 @@
+#include "trajectory.hpp"
+
+#include <cmath>
+
+namespace component
+{
+constexpr double g = 9.7833;
+
+Trajectory::Trajectory(const double v0, const double d, const double h)
+{
+  auto a = g * d * d / (2 * v0 * v0);
+  auto b = -d;
+  auto c = a + h;
+  auto delta = b * b - 4 * a * c;
+
+  if (delta < 0) {
+    unsolvable = true;
+    return;
+  }
+
+  unsolvable = false;
+  auto tan_pitch_1 = (-b + std::sqrt(delta)) / (2 * a);
+  auto tan_pitch_2 = (-b - std::sqrt(delta)) / (2 * a);
+  auto pitch_1 = std::atan(tan_pitch_1);
+  auto pitch_2 = std::atan(tan_pitch_2);
+  auto t_1 = d / (v0 * std::cos(pitch_1));
+  auto t_2 = d / (v0 * std::cos(pitch_2));
+
+  pitch = (t_1 < t_2) ? pitch_1 : pitch_2;
+  fly_time = (t_1 < t_2) ? t_1 : t_2;
+}
+
+}  // namespace component

src/component/trajectory.hpp

@@ -0,0 +1,21 @@
+#ifndef COMPONENT__TRAJECTORY_HPP
+#define COMPONENT__TRAJECTORY_HPP
+
+namespace component
+{
+struct Trajectory
+{
+  bool unsolvable;
+  double fly_time;
+  double pitch;  // 抬头为正
+
+  // 不考虑空气阻力
+  // v0 子弹初速度大小，单位：m/s
+  // d 目标水平距离，单位：m
+  // h 目标竖直高度，单位：m
+  Trajectory(const double v0, const double d, const double h);
+};
+
+}  // namespace component
+
+#endif  // COMPONENT__TRAJECTORY_HPP

src/component/yaml.hpp

@@ -0,0 +1,33 @@
+#ifndef COMPONENT__YAML_HPP
+#define COMPONENT__YAML_HPP
+
+#include <yaml-cpp/yaml.h>
+
+#include "src/component/logger.hpp"
+
+namespace component
+{
+inline YAML::Node load(const std::string & path)
+{
+  try {
+    return YAML::LoadFile(path);
+  } catch (const YAML::BadFile & e) {
+    logger()->error("[YAML] Failed to load file: {}", e.what());
+    exit(1);
+  } catch (const YAML::ParserException & e) {
+    logger()->error("[YAML] Parser error: {}", e.what());
+    exit(1);
+  }
+}
+
+template <typename T>
+inline T read(const YAML::Node & yaml, const std::string & key)
+{
+  if (yaml[key]) return yaml[key].as<T>();
+  logger()->error("[YAML] {} not found!", key);
+  exit(1);
+}
+
+}  // namespace component
+
+#endif  // COMPONENT__YAML_HPP

src/device/CMakeLists.txt

@@ -0,0 +1,51 @@
+cmake_minimum_required(VERSION 3.16)
+
+find_package(yaml-cpp REQUIRED)
+
+add_subdirectory(serial)
+
+# 创建目标 device
+set(DEVICE_SOURCES
+    hikrobot/hikrobot.cpp
+    mindvision/mindvision.cpp
+    usbcamera/usbcamera.cpp
+    camera.cpp
+    cboard.cpp
+    dm_imu/dm_imu.cpp
+    gimbal/gimbal.cpp
+)
+
+# 如果启用ROS2，添加ROS2版本的gimbal
+if(USE_ROS2)
+    list(APPEND DEVICE_SOURCES gimbal/gimbal_ros.cpp)
+endif()
+
+add_library(device STATIC ${DEVICE_SOURCES})
+
+# hikrobot
+target_include_directories(device PUBLIC hikrobot/include)
+if(CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64")
+  target_link_directories(device PUBLIC hikrobot/lib/amd64)
+elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64")
+  target_link_directories(device PUBLIC hikrobot/lib/arm64)
+else()
+  message(FATAL_ERROR "Unsupported architecture: ${CMAKE_HOST_SYSTEM_PROCESSOR}!")
+endif()
+
+# mindvision
+target_include_directories(device PUBLIC mindvision/include)
+if(CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64")
+  target_link_directories(device PUBLIC mindvision/lib/amd64)
+elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64")
+  target_link_directories(device PUBLIC mindvision/lib/arm64)
+else()
+  message(FATAL_ERROR "Unsupported architecture: ${CMAKE_HOST_SYSTEM_PROCESSOR}!")
+endif()
+
+target_link_libraries(device MvCameraControl MVSDK usb-1.0 yaml-cpp serial)
+
+# 如果启用ROS2，添加ROS2依赖
+if(USE_ROS2)
+    target_include_directories(device PUBLIC ${rclcpp_INCLUDE_DIRS} ${rm_msgs_INCLUDE_DIRS})
+    target_link_libraries(device ${rclcpp_LIBRARIES})
+endif()

src/device/camera.cpp

@@ -0,0 +1,44 @@
+#include "camera.hpp"
+
+#include <stdexcept>
+
+#include "hikrobot/hikrobot.hpp"
+#include "mindvision/mindvision.hpp"
+#include "src/component/yaml.hpp"
+
+namespace device
+{
+Camera::Camera(const std::string & config_path)
+{
+  auto yaml = component::load(config_path);
+  auto camera_name = component::read<std::string>(yaml, "camera_name");
+  auto exposure_ms = component::read<double>(yaml, "exposure_ms");
+
+  if (camera_name == "mindvision") {
+    auto gamma = component::read<double>(yaml, "gamma");
+    auto vid_pid = component::read<std::string>(yaml, "vid_pid");
+    camera_ = std::make_unique<MindVision>(exposure_ms, gamma, vid_pid);
+  }
+
+  else if (camera_name == "hikrobot") {
+    auto gain = component::read<double>(yaml, "gain");
+    auto vid_pid = component::read<std::string>(yaml, "vid_pid");
+    camera_ = std::make_unique<HikRobot>(exposure_ms, gain, vid_pid);
+  }
+
+  else {
+    throw std::runtime_error("Unknow camera_name: " + camera_name + "!");
+  }
+
+  rotate_180_ = yaml["rotate_180"] ? yaml["rotate_180"].as<bool>() : false;
+}
+
+void Camera::read(cv::Mat & img, std::chrono::steady_clock::time_point & timestamp)
+{
+  camera_->read(img, timestamp);
+  if (rotate_180_) {
+    cv::rotate(img, img, cv::ROTATE_180);
+  }
+}
+
+}  // namespace device

src/device/camera.hpp

@@ -0,0 +1,31 @@
+#ifndef DEVICE__CAMERA_HPP
+#define DEVICE__CAMERA_HPP
+
+#include <chrono>
+#include <memory>
+#include <opencv2/opencv.hpp>
+#include <string>
+
+namespace device
+{
+class CameraBase
+{
+public:
+  virtual ~CameraBase() = default;
+  virtual void read(cv::Mat & img, std::chrono::steady_clock::time_point & timestamp) = 0;
+};
+
+class Camera
+{
+public:
+  Camera(const std::string & config_path);
+  void read(cv::Mat & img, std::chrono::steady_clock::time_point & timestamp);
+
+private:
+  std::unique_ptr<CameraBase> camera_;
+  bool rotate_180_ = false;
+};
+
+}  // namespace device
+
+#endif  // DEVICE__CAMERA_HPP

src/device/cboard.cpp

@@ -0,0 +1,121 @@
+#include "cboard.hpp"
+
+#include "src/component/math_tools.hpp"
+#include "src/component/yaml.hpp"
+
+namespace device
+{
+CBoard::CBoard(const std::string & config_path)
+: mode(Mode::idle),
+  shoot_mode(ShootMode::left_shoot),
+  bullet_speed(0),
+  queue_(5000),
+  can_(read_yaml(config_path), std::bind(&CBoard::callback, this, std::placeholders::_1))
+// 注意: callback的运行会早于Cboard构造函数的完成
+{
+  component::logger()->info("[Cboard] Waiting for q...");
+  queue_.pop(data_ahead_);
+  queue_.pop(data_behind_);
+  component::logger()->info("[Cboard] Opened.");
+}
+
+Eigen::Quaterniond CBoard::imu_at(std::chrono::steady_clock::time_point timestamp)
+{
+  if (data_behind_.timestamp < timestamp) data_ahead_ = data_behind_;
+
+  while (true) {
+    queue_.pop(data_behind_);
+    if (data_behind_.timestamp > timestamp) break;
+    data_ahead_ = data_behind_;
+  }
+
+  Eigen::Quaterniond q_a = data_ahead_.q.normalized();
+  Eigen::Quaterniond q_b = data_behind_.q.normalized();
+  auto t_a = data_ahead_.timestamp;
+  auto t_b = data_behind_.timestamp;
+  auto t_c = timestamp;
+  std::chrono::duration<double> t_ab = t_b - t_a;
+  std::chrono::duration<double> t_ac = t_c - t_a;
+
+  // 四元数插值
+  auto k = t_ac / t_ab;
+  Eigen::Quaterniond q_c = q_a.slerp(k, q_b).normalized();
+
+  return q_c;
+}
+
+void CBoard::send(Command command) const
+{
+  can_frame frame;
+  frame.can_id = send_canid_;
+  frame.can_dlc = 8;
+  frame.data[0] = (command.control) ? 1 : 0;
+  frame.data[1] = (command.shoot) ? 1 : 0;
+  frame.data[2] = (int16_t)(command.yaw * 1e4) >> 8;
+  frame.data[3] = (int16_t)(command.yaw * 1e4);
+  frame.data[4] = (int16_t)(command.pitch * 1e4) >> 8;
+  frame.data[5] = (int16_t)(command.pitch * 1e4);
+  frame.data[6] = (int16_t)(command.horizon_distance * 1e4) >> 8;
+  frame.data[7] = (int16_t)(command.horizon_distance * 1e4);
+
+  try {
+    can_.write(&frame);
+  } catch (const std::exception & e) {
+    component::logger()->warn("{}", e.what());
+  }
+}
+
+void CBoard::callback(const can_frame & frame)
+{
+  auto timestamp = std::chrono::steady_clock::now();
+
+  if (frame.can_id == quaternion_canid_) {
+    auto x = (int16_t)(frame.data[0] << 8 | frame.data[1]) / 1e4;
+    auto y = (int16_t)(frame.data[2] << 8 | frame.data[3]) / 1e4;
+    auto z = (int16_t)(frame.data[4] << 8 | frame.data[5]) / 1e4;
+    auto w = (int16_t)(frame.data[6] << 8 | frame.data[7]) / 1e4;
+
+    if (std::abs(x * x + y * y + z * z + w * w - 1) > 1e-2) {
+      component::logger()->warn("Invalid q: {} {} {} {}", w, x, y, z);
+      return;
+    }
+
+    queue_.push({{w, x, y, z}, timestamp});
+  }
+
+  else if (frame.can_id == bullet_speed_canid_) {
+    bullet_speed = (int16_t)(frame.data[0] << 8 | frame.data[1]) / 1e2;
+    mode = Mode(frame.data[2]);
+    shoot_mode = ShootMode(frame.data[3]);
+    ft_angle = (int16_t)(frame.data[4] << 8 | frame.data[5]) / 1e4;
+
+    // 限制日志输出频率为1Hz
+    static auto last_log_time = std::chrono::steady_clock::time_point::min();
+    auto now = std::chrono::steady_clock::now();
+
+    if (bullet_speed > 0 && component::delta_time(now, last_log_time) >= 1.0) {
+      component::logger()->info(
+        "[CBoard] Bullet speed: {:.2f} m/s, Mode: {}, Shoot mode: {}, FT angle: {:.2f} rad",
+        bullet_speed, MODES[mode], SHOOT_MODES[shoot_mode], ft_angle);
+      last_log_time = now;
+    }
+  }
+}
+
+// 实现方式有待改进
+std::string CBoard::read_yaml(const std::string & config_path)
+{
+  auto yaml = component::load(config_path);
+
+  quaternion_canid_ = component::read<int>(yaml, "quaternion_canid");
+  bullet_speed_canid_ = component::read<int>(yaml, "bullet_speed_canid");
+  send_canid_ = component::read<int>(yaml, "send_canid");
+
+  if (!yaml["can_interface"]) {
+    throw std::runtime_error("Missing 'can_interface' in YAML configuration.");
+  }
+
+  return yaml["can_interface"].as<std::string>();
+}
+
+}  // namespace device

src/device/cboard.hpp

@@ -0,0 +1,72 @@
+#ifndef DEVICE__CBOARD_HPP
+#define DEVICE__CBOARD_HPP
+
+#include <Eigen/Geometry>
+#include <chrono>
+#include <cmath>
+#include <functional>
+#include <string>
+#include <vector>
+
+#include "src/device/command.hpp"
+#include "src/device/socketcan.hpp"
+#include "src/component/logger.hpp"
+#include "src/component/thread_safe_queue.hpp"
+
+namespace device
+{
+enum Mode
+{
+  idle,
+  auto_aim,
+  small_buff,
+  big_buff,
+  outpost
+};
+const std::vector<std::string> MODES = {"idle", "auto_aim", "small_buff", "big_buff", "outpost"};
+
+// 哨兵专有
+enum ShootMode
+{
+  left_shoot,
+  right_shoot,
+  both_shoot
+};
+const std::vector<std::string> SHOOT_MODES = {"left_shoot", "right_shoot", "both_shoot"};
+
+class CBoard
+{
+public:
+  double bullet_speed;
+  Mode mode;
+  ShootMode shoot_mode;
+  double ft_angle;  //无人机专有
+
+  CBoard(const std::string & config_path);
+
+  Eigen::Quaterniond imu_at(std::chrono::steady_clock::time_point timestamp);
+
+  void send(Command command) const;
+
+private:
+  struct IMUData
+  {
+    Eigen::Quaterniond q;
+    std::chrono::steady_clock::time_point timestamp;
+  };
+
+  component::ThreadSafeQueue<IMUData> queue_;  // 必须在can_之前初始化，否则存在死锁的可能
+  SocketCAN can_;
+  IMUData data_ahead_;
+  IMUData data_behind_;
+
+  int quaternion_canid_, bullet_speed_canid_, send_canid_;
+
+  void callback(const can_frame & frame);
+
+  std::string read_yaml(const std::string & config_path);
+};
+
+}  // namespace device
+
+#endif  // DEVICE__CBOARD_HPP

src/device/command.hpp

@@ -0,0 +1,17 @@
+#ifndef DEVICE__COMMAND_HPP
+#define DEVICE__COMMAND_HPP
+
+namespace device
+{
+struct Command
+{
+  bool control;
+  bool shoot;
+  double yaw;
+  double pitch;
+  double horizon_distance = 0;  //无人机专有
+};
+
+}  // namespace device
+
+#endif  // DEVICE__COMMAND_HPP

src/device/dm_imu/dm_imu.cpp

@@ -0,0 +1,131 @@
+#include "dm_imu.hpp"
+
+#include <atomic>
+#include <chrono>
+#include <cstdint>
+#include <iostream>
+#include <mutex>
+#include <queue>
+#include <thread>
+
+#include "src/component/crc.hpp"
+#include "src/component/logger.hpp"
+#include "src/component/math_tools.hpp"
+
+namespace device
+{
+DM_IMU::DM_IMU() : queue_(5000)
+{
+  init_serial();
+  rec_thread_ = std::thread(&DM_IMU::get_imu_data_thread, this);
+  queue_.pop(data_ahead_);
+  queue_.pop(data_behind_);
+  component::logger()->info("[DM_IMU] initialized");
+}
+
+DM_IMU::~DM_IMU()
+{
+  stop_thread_ = true;
+  if (rec_thread_.joinable()) {
+    rec_thread_.join();
+  }
+  if (serial_.isOpen()) {
+    serial_.close();
+  }
+}
+
+void DM_IMU::init_serial()
+{
+  try {
+    serial_.setPort("/dev/ttyACM0");
+    serial_.setBaudrate(921600);
+    serial_.setFlowcontrol(serial::flowcontrol_none);
+    serial_.setParity(serial::parity_none);  //default is parity_none
+    serial_.setStopbits(serial::stopbits_one);
+    serial_.setBytesize(serial::eightbits);
+    serial::Timeout time_out = serial::Timeout::simpleTimeout(20);
+    serial_.setTimeout(time_out);
+    serial_.open();
+    usleep(1000000);  //1s
+
+    component::logger()->info("[DM_IMU] serial port opened");
+  }
+
+  catch (serial::IOException & e) {
+    component::logger()->warn("[DM_IMU] failed to open serial port ");
+    exit(0);
+  }
+}
+
+void DM_IMU::get_imu_data_thread()
+{
+  while (!stop_thread_) {
+    if (!serial_.isOpen()) {
+      component::logger()->warn("In get_imu_data_thread,imu serial port unopen");
+    }
+
+    serial_.read((uint8_t *)(&receive_data.FrameHeader1), 4);
+
+    if (
+      receive_data.FrameHeader1 == 0x55 && receive_data.flag1 == 0xAA &&
+      receive_data.slave_id1 == 0x01 && receive_data.reg_acc == 0x01)
+
+    {
+      serial_.read((uint8_t *)(&receive_data.accx_u32), 57 - 4);
+
+      if (component::get_crc16((uint8_t *)(&receive_data.FrameHeader1), 16) == receive_data.crc1) {
+        data.accx = *((float *)(&receive_data.accx_u32));
+        data.accy = *((float *)(&receive_data.accy_u32));
+        data.accz = *((float *)(&receive_data.accz_u32));
+      }
+      if (component::get_crc16((uint8_t *)(&receive_data.FrameHeader2), 16) == receive_data.crc2) {
+        data.gyrox = *((float *)(&receive_data.gyrox_u32));
+        data.gyroy = *((float *)(&receive_data.gyroy_u32));
+        data.gyroz = *((float *)(&receive_data.gyroz_u32));
+      }
+      if (component::get_crc16((uint8_t *)(&receive_data.FrameHeader3), 16) == receive_data.crc3) {
+        data.roll = *((float *)(&receive_data.roll_u32));
+        data.pitch = *((float *)(&receive_data.pitch_u32));
+        data.yaw = *((float *)(&receive_data.yaw_u32));
+        // component::logger()->debug(
+        //   "yaw: {:.2f}, pitch: {:.2f}, roll: {:.2f}", static_cast<double>(data.yaw),
+        //   static_cast<double>(data.pitch), static_cast<double>(data.roll));
+      }
+      auto timestamp = std::chrono::steady_clock::now();
+      Eigen::Quaterniond q = Eigen::AngleAxisd(data.yaw * M_PI / 180, Eigen::Vector3d::UnitZ()) *
+                             Eigen::AngleAxisd(data.pitch * M_PI / 180, Eigen::Vector3d::UnitY()) *
+                             Eigen::AngleAxisd(data.roll * M_PI / 180, Eigen::Vector3d::UnitX());
+      q.normalize();
+      queue_.push({q, timestamp});
+    } else {
+      component::logger()->info("[DM_IMU] failed to get correct data");
+    }
+  }
+}
+
+Eigen::Quaterniond DM_IMU::imu_at(std::chrono::steady_clock::time_point timestamp)
+{
+  if (data_behind_.timestamp < timestamp) data_ahead_ = data_behind_;
+
+  while (true) {
+    queue_.pop(data_behind_);
+    if (data_behind_.timestamp > timestamp) break;
+    data_ahead_ = data_behind_;
+  }
+
+  Eigen::Quaterniond q_a = data_ahead_.q.normalized();
+  Eigen::Quaterniond q_b = data_behind_.q.normalized();
+  auto t_a = data_ahead_.timestamp;
+  auto t_b = data_behind_.timestamp;
+  auto t_c = timestamp;
+  std::chrono::duration<double> t_ab = t_b - t_a;
+  std::chrono::duration<double> t_ac = t_c - t_a;
+
+  // 四元数插值
+  auto k = t_ac / t_ab;
+  Eigen::Quaterniond q_c = q_a.slerp(k, q_b).normalized();
+
+  return q_c;
+}
+
+}  // namespace device

src/device/dm_imu/dm_imu.hpp

@@ -0,0 +1,96 @@
+#ifndef DEVICE__Dm_Imu_HPP
+#define DEVICE__Dm_Imu_HPP
+
+#include <math.h>
+#include <serial/serial.h>
+
+#include <Eigen/Geometry>
+#include <array>
+#include <fstream>
+#include <initializer_list>
+#include <iostream>
+#include <thread>
+
+#include "src/component/thread_safe_queue.hpp"
+
+namespace device
+{
+
+struct __attribute__((packed)) IMU_Receive_Frame
+{
+  uint8_t FrameHeader1;
+  uint8_t flag1;
+  uint8_t slave_id1;
+  uint8_t reg_acc;
+  uint32_t accx_u32;
+  uint32_t accy_u32;
+  uint32_t accz_u32;
+  uint16_t crc1;
+  uint8_t FrameEnd1;
+
+  uint8_t FrameHeader2;
+  uint8_t flag2;
+  uint8_t slave_id2;
+  uint8_t reg_gyro;
+  uint32_t gyrox_u32;
+  uint32_t gyroy_u32;
+  uint32_t gyroz_u32;
+  uint16_t crc2;
+  uint8_t FrameEnd2;
+
+  uint8_t FrameHeader3;
+  uint8_t flag3;
+  uint8_t slave_id3;
+  uint8_t reg_euler;  //r-p-y
+  uint32_t roll_u32;
+  uint32_t pitch_u32;
+  uint32_t yaw_u32;
+  uint16_t crc3;
+  uint8_t FrameEnd3;
+};
+
+typedef struct
+{
+  float accx;
+  float accy;
+  float accz;
+  float gyrox;
+  float gyroy;
+  float gyroz;
+  float roll;
+  float pitch;
+  float yaw;
+} IMU_Data;
+
+class DM_IMU
+{
+public:
+  DM_IMU();
+  ~DM_IMU();
+
+  Eigen::Quaterniond imu_at(std::chrono::steady_clock::time_point timestamp);
+
+private:
+  struct IMUData
+  {
+    Eigen::Quaterniond q;
+    std::chrono::steady_clock::time_point timestamp;
+  };
+
+  void init_serial();
+  void get_imu_data_thread();
+
+  serial::Serial serial_;
+  std::thread rec_thread_;
+
+  component::ThreadSafeQueue<IMUData> queue_;
+  IMUData data_ahead_, data_behind_;
+
+  std::atomic<bool> stop_thread_{false};
+  IMU_Receive_Frame receive_data{};  //receive data frame
+  IMU_Data data{};
+};
+
+}  // namespace device
+
+#endif

src/device/gimbal/gimbal.cpp

@@ -0,0 +1,290 @@
+#include "gimbal.hpp"
+
+#include "src/component/crc.hpp"
+#include "src/component/logger.hpp"
+#include "src/component/math_tools.hpp"
+#include "src/component/yaml.hpp"
+
+namespace device
+{
+Gimbal::Gimbal(const std::string & config_path)
+{
+  auto yaml = component::load(config_path);
+  auto com_port = component::read<std::string>(yaml, "com_port");
+  auto baudrate = component::read<int>(yaml, "baudrate");
+
+  try {
+    serial_.setPort(com_port);
+    serial_.setBaudrate(baudrate);
+    serial_.setTimeout(serial::Timeout::max(), 100, 0, 100, 0);
+    serial_.open();
+    component::logger()->info("[Gimbal] Serial port {} opened at {} baud", com_port, baudrate);
+  } catch (const std::exception & e) {
+    component::logger()->error("[Gimbal] Failed to open serial: {}", e.what());
+    exit(1);
+  }
+
+  thread_ = std::thread(&Gimbal::read_thread, this);
+
+  queue_.pop();
+  component::logger()->info("[Gimbal] First q received.");
+}
+
+Gimbal::~Gimbal()
+{
+  quit_ = true;
+  if (thread_.joinable()) thread_.join();
+  serial_.close();
+}
+
+GimbalMode Gimbal::mode() const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  return mode_;
+}
+
+GimbalState Gimbal::state() const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  return state_;
+}
+
+std::string Gimbal::str(GimbalMode mode) const
+{
+  switch (mode) {
+    case GimbalMode::IDLE:
+      return "IDLE";
+    case GimbalMode::AUTO_AIM:
+      return "AUTO_AIM";
+    case GimbalMode::SMALL_BUFF:
+      return "SMALL_BUFF";
+    case GimbalMode::BIG_BUFF:
+      return "BIG_BUFF";
+    default:
+      return "INVALID";
+  }
+}
+
+Eigen::Quaterniond Gimbal::q(std::chrono::steady_clock::time_point t)
+{
+  while (true) {
+    auto [q_a, t_a] = queue_.pop();
+    auto [q_b, t_b] = queue_.front();
+    auto t_ab = component::delta_time(t_a, t_b);
+    auto t_ac = component::delta_time(t_a, t);
+    auto k = t_ac / t_ab;
+    Eigen::Quaterniond q_c = q_a.slerp(k, q_b).normalized();
+    if (t < t_a) return q_c;
+    if (!(t_a < t && t <= t_b)) continue;
+
+    return q_c;
+  }
+}
+
+void Gimbal::send(device::VisionToGimbal VisionToGimbal)
+{
+  tx_data_.mode = VisionToGimbal.mode;
+  tx_data_.yaw = VisionToGimbal.yaw;
+  tx_data_.yaw_vel = VisionToGimbal.yaw_vel;
+  tx_data_.yaw_acc = VisionToGimbal.yaw_acc;
+  tx_data_.pitch = VisionToGimbal.pitch;
+  tx_data_.pitch_vel = VisionToGimbal.pitch_vel;
+  tx_data_.pitch_acc = VisionToGimbal.pitch_acc;
+  tx_data_.crc16 = component::get_crc16(
+    reinterpret_cast<uint8_t *>(&tx_data_), sizeof(tx_data_) - sizeof(tx_data_.crc16));
+
+  try {
+    serial_.write(reinterpret_cast<uint8_t *>(&tx_data_), sizeof(tx_data_));
+  } catch (const std::exception & e) {
+    component::logger()->warn("[Gimbal] Failed to write serial: {}", e.what());
+  }
+}
+
+void Gimbal::send(
+  bool control, bool fire, float yaw, float yaw_vel, float yaw_acc, float pitch, float pitch_vel,
+  float pitch_acc)
+{
+  tx_data_.mode = control ? (fire ? 2 : 1) : 0;
+  tx_data_.yaw = yaw;
+  tx_data_.yaw_vel = yaw_vel;
+  tx_data_.yaw_acc = yaw_acc;
+  tx_data_.pitch = pitch;
+  tx_data_.pitch_vel = pitch_vel;
+  tx_data_.pitch_acc = pitch_acc;
+  tx_data_.crc16 = component::get_crc16(
+    reinterpret_cast<uint8_t *>(&tx_data_), sizeof(tx_data_) - sizeof(tx_data_.crc16));
+
+  try {
+    serial_.write(reinterpret_cast<uint8_t *>(&tx_data_), sizeof(tx_data_));
+  } catch (const std::exception & e) {
+    component::logger()->warn("[Gimbal] Failed to write serial: {}", e.what());
+  }
+}
+
+bool Gimbal::read(uint8_t * buffer, size_t size)
+{
+  try {
+    return serial_.read(buffer, size) == size;
+  } catch (const std::exception & e) {
+    // component::logger()->warn("[Gimbal] Failed to read serial: {}", e.what());
+    return false;
+  }
+}
+
+void Gimbal::read_thread()
+{
+  component::logger()->info("[Gimbal] read_thread started.");
+  int error_count = 0;
+  uint8_t byte;
+  int total_bytes_read = 0;
+  int valid_packets = 0;
+
+  while (!quit_) {
+    if (error_count > 5000) {
+      error_count = 0;
+      component::logger()->warn("[Gimbal] Too many errors (read {} bytes, {} valid packets), attempting to reconnect...",
+                                total_bytes_read, valid_packets);
+      reconnect();
+      continue;
+    }
+
+    // 逐字节查找包头第一个字节 'M'
+    if (!read(&byte, 1)) {
+      error_count++;
+      continue;
+    }
+
+    // 读取成功，重置错误计数
+    error_count = 0;
+    total_bytes_read++;
+
+    if (byte != 'M') continue;
+
+    // 读取第二个字节检查是否为 'R'
+    if (!read(&byte, 1)) {
+      error_count++;
+      continue;
+    }
+
+    total_bytes_read++;
+
+    if (byte != 'R') {
+      if (valid_packets < 3) {
+        component::logger()->debug("[Gimbal] Found 'M' but next byte is 0x{:02X}, not 'R'", byte);
+      }
+      continue;
+    }
+
+    // 找到包头，记录时间戳
+    rx_data_.head[0] = 'M';
+    rx_data_.head[1] = 'R';
+    auto t = std::chrono::steady_clock::now();
+
+    // 读取剩余数据
+    if (!read(
+          reinterpret_cast<uint8_t *>(&rx_data_) + sizeof(rx_data_.head),
+          sizeof(rx_data_) - sizeof(rx_data_.head))) {
+      error_count++;
+      component::logger()->warn("[Gimbal] Failed to read packet body");
+      continue;
+    }
+
+    // 验证数据合理性
+    if (rx_data_.mode > 3) {
+      // mode 应该在 0-3 范围内
+      if (valid_packets < 10) {
+        component::logger()->warn("[Gimbal] Invalid mode {}, skipping packet (possible misalignment)", rx_data_.mode);
+      }
+      continue;
+    }
+
+    // 验证四元数范数是否接近1
+    float q_norm = rx_data_.q[0] * rx_data_.q[0] +
+                   rx_data_.q[1] * rx_data_.q[1] +
+                   rx_data_.q[2] * rx_data_.q[2] +
+                   rx_data_.q[3] * rx_data_.q[3];
+    if (q_norm < 0.9f || q_norm > 1.1f) {
+      if (valid_packets < 10) {
+        component::logger()->warn("[Gimbal] Invalid quaternion norm {:.3f}, skipping packet", q_norm);
+      }
+      continue;
+    }
+
+    total_bytes_read += sizeof(rx_data_) - sizeof(rx_data_.head);
+    valid_packets++;
+
+    if (valid_packets <= 5) {
+      component::logger()->info("[Gimbal] Packet #{}: mode={}, q=[{:.3f},{:.3f},{:.3f},{:.3f}], yaw={:.3f}",
+                                valid_packets, (int)rx_data_.mode,
+                                (float)rx_data_.q[0], (float)rx_data_.q[1], (float)rx_data_.q[2], (float)rx_data_.q[3],
+                                (float)rx_data_.yaw);
+    } else if (valid_packets % 100 == 0) {
+      // 每100个包打印一次状态
+      component::logger()->info("[Gimbal] Received {} packets, total {} bytes", valid_packets, total_bytes_read);
+    }
+
+    // if (!component::check_crc16(reinterpret_cast<uint8_t *>(&rx_data_), sizeof(rx_data_))) {
+    //   component::logger()->debug("[Gimbal] CRC16 check failed.");
+    //   continue;
+    // }
+
+    error_count = 0;
+    Eigen::Quaterniond q(rx_data_.q[0], rx_data_.q[1], rx_data_.q[2], rx_data_.q[3]);
+    queue_.push({q, t});
+
+    std::lock_guard<std::mutex> lock(mutex_);
+
+    state_.yaw = rx_data_.yaw;
+    state_.yaw_vel = rx_data_.yaw_vel;
+    state_.pitch = rx_data_.pitch;
+    state_.pitch_vel = rx_data_.pitch_vel;
+    state_.bullet_speed = rx_data_.bullet_speed;
+    state_.bullet_count = rx_data_.bullet_count;
+
+    switch (rx_data_.mode) {
+      case 0:
+        mode_ = GimbalMode::IDLE;
+        break;
+      case 1:
+        mode_ = GimbalMode::AUTO_AIM;
+        break;
+      case 2:
+        mode_ = GimbalMode::SMALL_BUFF;
+        break;
+      case 3:
+        mode_ = GimbalMode::BIG_BUFF;
+        break;
+      default:
+        mode_ = GimbalMode::IDLE;
+        component::logger()->warn("[Gimbal] Invalid mode: {}", rx_data_.mode);
+        break;
+    }
+  }
+
+  component::logger()->info("[Gimbal] read_thread stopped.");
+}
+
+void Gimbal::reconnect()
+{
+  int max_retry_count = 10;
+  for (int i = 0; i < max_retry_count && !quit_; ++i) {
+    component::logger()->warn("[Gimbal] Reconnecting serial, attempt {}/{}...", i + 1, max_retry_count);
+    try {
+      serial_.close();
+      std::this_thread::sleep_for(std::chrono::seconds(1));
+    } catch (...) {
+    }
+
+    try {
+      serial_.open();  // 尝试重新打开
+      queue_.clear();
+      component::logger()->info("[Gimbal] Reconnected serial successfully.");
+      break;
+    } catch (const std::exception & e) {
+      component::logger()->warn("[Gimbal] Reconnect failed: {}", e.what());
+      std::this_thread::sleep_for(std::chrono::seconds(1));
+    }
+  }
+}
+
+}  // namespace device

src/device/gimbal/gimbal.hpp

@@ -0,0 +1,106 @@
+#ifndef DEVICE__GIMBAL_HPP
+#define DEVICE__GIMBAL_HPP
+
+#include <Eigen/Geometry>
+#include <atomic>
+#include <chrono>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <tuple>
+
+#include "serial/serial.h"
+#include "src/component/thread_safe_queue.hpp"
+
+namespace device
+{
+struct __attribute__((packed)) GimbalToVision
+{
+  uint8_t head[2] = {'M', 'R'};
+  uint8_t mode;  // 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符
+  float q[4];    // wxyz顺序
+  float yaw;
+  float yaw_vel;
+  float pitch;
+  float pitch_vel;
+  float bullet_speed;
+  uint16_t bullet_count;  // 子弹累计发送次数
+  uint16_t crc16;
+};
+
+static_assert(sizeof(GimbalToVision) <= 64);
+
+struct __attribute__((packed)) VisionToGimbal
+{
+  uint8_t head[2] = {'M', 'R'};
+  uint8_t mode;  // 0: 不控制, 1: 控制云台但不开火，2: 控制云台且开火
+  float yaw;
+  float yaw_vel;
+  float yaw_acc;
+  float pitch;
+  float pitch_vel;
+  float pitch_acc;
+  uint16_t crc16;
+};
+
+static_assert(sizeof(VisionToGimbal) <= 64);
+
+enum class GimbalMode
+{
+  IDLE,        // 空闲
+  AUTO_AIM,    // 自瞄
+  SMALL_BUFF,  // 小符
+  BIG_BUFF     // 大符
+};
+
+struct GimbalState
+{
+  float yaw;
+  float yaw_vel;
+  float pitch;
+  float pitch_vel;
+  float bullet_speed;
+  uint16_t bullet_count;
+};
+
+class Gimbal
+{
+public:
+  Gimbal(const std::string & config_path);
+
+  ~Gimbal();
+
+  GimbalMode mode() const;
+  GimbalState state() const;
+  std::string str(GimbalMode mode) const;
+  Eigen::Quaterniond q(std::chrono::steady_clock::time_point t);
+
+  void send(
+    bool control, bool fire, float yaw, float yaw_vel, float yaw_acc, float pitch, float pitch_vel,
+    float pitch_acc);
+
+  void send(device::VisionToGimbal VisionToGimbal);
+
+private:
+  serial::Serial serial_;
+
+  std::thread thread_;
+  std::atomic<bool> quit_ = false;
+  mutable std::mutex mutex_;
+
+  GimbalToVision rx_data_;
+  VisionToGimbal tx_data_;
+
+  GimbalMode mode_ = GimbalMode::IDLE;
+  GimbalState state_;
+  component::ThreadSafeQueue<std::tuple<Eigen::Quaterniond, std::chrono::steady_clock::time_point>>
+    queue_{1000};
+
+  bool read(uint8_t * buffer, size_t size);
+  void read_thread();
+  void reconnect();
+};
+
+}  // namespace device
+
+#endif  // DEVICE__GIMBAL_HPP

src/device/gimbal/gimbal_ros.cpp

@@ -0,0 +1,144 @@
+#ifdef USE_ROS2
+
+#include "gimbal_ros.hpp"
+
+#include "src/component/logger.hpp"
+#include "src/component/math_tools.hpp"
+#include "src/component/yaml.hpp"
+
+namespace device
+{
+
+GimbalROS::GimbalROS(const std::string & config_path, std::shared_ptr<rclcpp::Node> node)
+: node_(node)
+{
+  // 创建发布者和订阅者
+  aim_pub_ = node_->create_publisher<rm_msgs::msg::DataAim>("data_aim", 10);
+  mcu_sub_ = node_->create_subscription<rm_msgs::msg::DataMCU>(
+    "data_mcu", 10, std::bind(&GimbalROS::mcu_callback, this, std::placeholders::_1));
+
+  component::logger()->info("[GimbalROS] ROS2 communication initialized");
+  component::logger()->info("[GimbalROS] Publishing to: data_aim");
+  component::logger()->info("[GimbalROS] Subscribing to: data_mcu");
+}
+
+GimbalROS::~GimbalROS()
+{
+  component::logger()->info("[GimbalROS] Shutting down");
+}
+
+GimbalMode GimbalROS::mode() const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  return mode_;
+}
+
+GimbalState GimbalROS::state() const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  return state_;
+}
+
+std::string GimbalROS::str(GimbalMode mode) const
+{
+  switch (mode) {
+    case GimbalMode::IDLE:
+      return "IDLE";
+    case GimbalMode::AUTO_AIM:
+      return "AUTO_AIM";
+    case GimbalMode::SMALL_BUFF:
+      return "SMALL_BUFF";
+    case GimbalMode::BIG_BUFF:
+      return "BIG_BUFF";
+    default:
+      return "INVALID";
+  }
+}
+
+Eigen::Quaterniond GimbalROS::q(std::chrono::steady_clock::time_point t)
+{
+  while (true) {
+    auto [q_a, t_a] = queue_.pop();
+    auto [q_b, t_b] = queue_.front();
+    auto t_ab = component::delta_time(t_a, t_b);
+    auto t_ac = component::delta_time(t_a, t);
+    auto k = t_ac / t_ab;
+    Eigen::Quaterniond q_c = q_a.slerp(k, q_b).normalized();
+    if (t < t_a) return q_c;
+    if (!(t_a < t && t <= t_b)) continue;
+
+    return q_c;
+  }
+}
+
+void GimbalROS::send(
+  bool control, bool fire, float yaw, float yaw_vel, float yaw_acc, float pitch, float pitch_vel,
+  float pitch_acc)
+{
+  auto msg = rm_msgs::msg::DataAim();
+  msg.mode = control ? (fire ? 2 : 1) : 0;
+  msg.yaw = yaw;
+  msg.yaw_vel = yaw_vel;
+  msg.yaw_acc = yaw_acc;
+  msg.pitch = pitch;
+  msg.pitch_vel = pitch_vel;
+  msg.pitch_acc = pitch_acc;
+
+  aim_pub_->publish(msg);
+}
+
+void GimbalROS::mcu_callback(const rm_msgs::msg::DataMCU::SharedPtr msg)
+{
+  auto t = std::chrono::steady_clock::now();
+
+  // 验证四元数范数
+  float q_norm = msg->q0 * msg->q0 + msg->q1 * msg->q1 + msg->q2 * msg->q2 + msg->q3 * msg->q3;
+  if (q_norm < 0.9f || q_norm > 1.1f) {
+    component::logger()->warn("[GimbalROS] Invalid quaternion norm {:.3f}, skipping", q_norm);
+    return;
+  }
+
+  // 验证模式
+  if (msg->mode > 3) {
+    component::logger()->warn("[GimbalROS] Invalid mode {}, skipping", msg->mode);
+    return;
+  }
+
+  // 存储四元数到队列
+  Eigen::Quaterniond q(msg->q0, msg->q1, msg->q2, msg->q3);
+  queue_.push({q, t});
+
+  // 更新状态
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  state_.yaw = msg->yaw;
+  state_.yaw_vel = msg->yaw_vel;
+  state_.pitch = msg->pitch;
+  state_.pitch_vel = msg->pitch_vel;
+  state_.bullet_speed = msg->bullet_speed;
+  state_.bullet_count = msg->bullet_count;
+
+  // 更新模式
+  switch (msg->mode) {
+    case 0:
+      mode_ = GimbalMode::IDLE;
+      break;
+    case 1:
+      mode_ = GimbalMode::AUTO_AIM;
+      break;
+    case 2:
+      mode_ = GimbalMode::SMALL_BUFF;
+      break;
+    case 3:
+      mode_ = GimbalMode::BIG_BUFF;
+      break;
+    default:
+      mode_ = GimbalMode::IDLE;
+      component::logger()->warn("[GimbalROS] Invalid mode: {}", msg->mode);
+      break;
+  }
+}
+
+}  // namespace device
+
+#endif  // USE_ROS2

src/device/gimbal/gimbal_ros.hpp

@@ -0,0 +1,58 @@
+#ifndef DEVICE__GIMBAL_ROS_HPP
+#define DEVICE__GIMBAL_ROS_HPP
+
+#ifdef USE_ROS2
+
+#include <Eigen/Geometry>
+#include <atomic>
+#include <chrono>
+#include <memory>
+#include <mutex>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rm_msgs/msg/data_aim.hpp"
+#include "rm_msgs/msg/data_mcu.hpp"
+#include "src/component/thread_safe_queue.hpp"
+#include "src/device/gimbal/gimbal.hpp"
+
+namespace device
+{
+
+class GimbalROS
+{
+public:
+  GimbalROS(const std::string & config_path, std::shared_ptr<rclcpp::Node> node);
+
+  ~GimbalROS();
+
+  GimbalMode mode() const;
+  GimbalState state() const;
+  std::string str(GimbalMode mode) const;
+  Eigen::Quaterniond q(std::chrono::steady_clock::time_point t);
+
+  void send(
+    bool control, bool fire, float yaw, float yaw_vel, float yaw_acc, float pitch, float pitch_vel,
+    float pitch_acc);
+
+private:
+  std::shared_ptr<rclcpp::Node> node_;
+
+  rclcpp::Publisher<rm_msgs::msg::DataAim>::SharedPtr aim_pub_;
+  rclcpp::Subscription<rm_msgs::msg::DataMCU>::SharedPtr mcu_sub_;
+
+  mutable std::mutex mutex_;
+
+  GimbalMode mode_ = GimbalMode::IDLE;
+  GimbalState state_;
+  component::ThreadSafeQueue<std::tuple<Eigen::Quaterniond, std::chrono::steady_clock::time_point>>
+    queue_{1000};
+
+  void mcu_callback(const rm_msgs::msg::DataMCU::SharedPtr msg);
+};
+
+}  // namespace device
+
+#endif  // USE_ROS2
+
+#endif  // DEVICE__GIMBAL_ROS_HPP

src/device/hikrobot/hikrobot.cpp

@@ -0,0 +1,273 @@
+#include "hikrobot.hpp"
+
+#include <libusb-1.0/libusb.h>
+
+#include "src/component/logger.hpp"
+
+using namespace std::chrono_literals;
+
+namespace device
+{
+HikRobot::HikRobot(double exposure_ms, double gain, const std::string & vid_pid)
+: exposure_us_(exposure_ms * 1e3), gain_(gain), queue_(1), daemon_quit_(false), vid_(-1), pid_(-1)
+{
+  set_vid_pid(vid_pid);
+  if (libusb_init(NULL)) component::logger()->warn("Unable to init libusb!");
+
+  daemon_thread_ = std::thread{[this] {
+    component::logger()->info("HikRobot's daemon thread started.");
+
+    capture_start();
+
+    while (!daemon_quit_) {
+      std::this_thread::sleep_for(100ms);
+
+      if (capturing_) continue;
+
+      capture_stop();
+      reset_usb();
+      capture_start();
+    }
+
+    capture_stop();
+
+    component::logger()->info("HikRobot's daemon thread stopped.");
+  }};
+}
+
+HikRobot::~HikRobot()
+{
+  daemon_quit_ = true;
+  if (daemon_thread_.joinable()) daemon_thread_.join();
+  component::logger()->info("HikRobot destructed.");
+}
+
+void HikRobot::read(cv::Mat & img, std::chrono::steady_clock::time_point & timestamp)
+{
+  CameraData data;
+  queue_.pop(data);
+
+  img = data.img;
+  timestamp = data.timestamp;
+}
+
+void HikRobot::capture_start()
+{
+  capturing_ = false;
+  capture_quit_ = false;
+
+  unsigned int ret;
+
+  MV_CC_DEVICE_INFO_LIST device_list;
+  ret = MV_CC_EnumDevices(MV_USB_DEVICE, &device_list);
+  if (ret != MV_OK) {
+    component::logger()->warn("MV_CC_EnumDevices failed: {:#x}", ret);
+    return;
+  }
+
+  if (device_list.nDeviceNum == 0) {
+    component::logger()->warn("Not found camera!");
+    return;
+  }
+
+  ret = MV_CC_CreateHandle(&handle_, device_list.pDeviceInfo[0]);
+  if (ret != MV_OK) {
+    component::logger()->warn("MV_CC_CreateHandle failed: {:#x}", ret);
+    return;
+  }
+
+  ret = MV_CC_OpenDevice(handle_);
+  if (ret != MV_OK) {
+    component::logger()->warn("MV_CC_OpenDevice failed: {:#x}", ret);
+    return;
+  }
+
+  set_enum_value("BalanceWhiteAuto", MV_BALANCEWHITE_AUTO_CONTINUOUS);
+  set_enum_value("ExposureAuto", MV_EXPOSURE_AUTO_MODE_OFF);
+  set_enum_value("GainAuto", MV_GAIN_MODE_OFF);
+  set_float_value("ExposureTime", exposure_us_);
+  set_float_value("Gain", gain_);
+  MV_CC_SetFrameRate(handle_, 150);
+
+  ret = MV_CC_StartGrabbing(handle_);
+  if (ret != MV_OK) {
+    component::logger()->warn("MV_CC_StartGrabbing failed: {:#x}", ret);
+    return;
+  }
+
+  capture_thread_ = std::thread{[this] {
+    component::logger()->info("HikRobot's capture thread started.");
+
+    capturing_ = true;
+
+    MV_FRAME_OUT raw;
+    MV_CC_PIXEL_CONVERT_PARAM cvt_param;
+
+    while (!capture_quit_) {
+      std::this_thread::sleep_for(1ms);
+
+      unsigned int ret;
+      unsigned int nMsec = 100;
+
+      ret = MV_CC_GetImageBuffer(handle_, &raw, nMsec);
+      if (ret != MV_OK) {
+        component::logger()->warn("MV_CC_GetImageBuffer failed: {:#x}", ret);
+        break;
+      }
+
+      auto timestamp = std::chrono::steady_clock::now();
+      cv::Mat img(cv::Size(raw.stFrameInfo.nWidth, raw.stFrameInfo.nHeight), CV_8U, raw.pBufAddr);
+
+      cvt_param.nWidth = raw.stFrameInfo.nWidth;
+      cvt_param.nHeight = raw.stFrameInfo.nHeight;
+
+      cvt_param.pSrcData = raw.pBufAddr;
+      cvt_param.nSrcDataLen = raw.stFrameInfo.nFrameLen;
+      cvt_param.enSrcPixelType = raw.stFrameInfo.enPixelType;
+
+      cvt_param.pDstBuffer = img.data;
+      cvt_param.nDstBufferSize = img.total() * img.elemSize();
+      cvt_param.enDstPixelType = PixelType_Gvsp_BGR8_Packed;
+
+      // ret = MV_CC_ConvertPixelType(handle_, &cvt_param);
+      const auto & frame_info = raw.stFrameInfo;
+      auto pixel_type = frame_info.enPixelType;
+      cv::Mat dst_image;
+      const static std::unordered_map<MvGvspPixelType, cv::ColorConversionCodes> type_map = {
+        {PixelType_Gvsp_BayerGR8, cv::COLOR_BayerGR2RGB},
+        {PixelType_Gvsp_BayerRG8, cv::COLOR_BayerRG2RGB},
+        {PixelType_Gvsp_BayerGB8, cv::COLOR_BayerGB2RGB},
+        {PixelType_Gvsp_BayerBG8, cv::COLOR_BayerBG2RGB}};
+
+      auto it = type_map.find(pixel_type);
+      if (it != type_map.end()) {
+        cv::cvtColor(img, dst_image, it->second);
+        img = dst_image;
+      } else {
+        // 像素格式不在 map 中，尝试使用 SDK 转换
+        static bool warned = false;
+        if (!warned) {
+          component::logger()->warn("Unknown pixel type: {:#x}, using SDK conversion", pixel_type);
+          warned = true;
+        }
+        cv::Mat bgr_img(cv::Size(raw.stFrameInfo.nWidth, raw.stFrameInfo.nHeight), CV_8UC3);
+        cvt_param.pDstBuffer = bgr_img.data;
+        cvt_param.nDstBufferSize = bgr_img.total() * bgr_img.elemSize();
+        cvt_param.enDstPixelType = PixelType_Gvsp_BGR8_Packed;
+
+        ret = MV_CC_ConvertPixelType(handle_, &cvt_param);
+        if (ret == MV_OK) {
+          img = bgr_img;
+        } else {
+          component::logger()->warn("MV_CC_ConvertPixelType failed: {:#x}, using raw image", ret);
+          // 如果转换失败，尝试直接使用原始图像
+          if (img.channels() == 1) {
+            cv::cvtColor(img, dst_image, cv::COLOR_GRAY2BGR);
+            img = dst_image;
+          }
+        }
+      }
+
+      queue_.push({img, timestamp});
+
+      ret = MV_CC_FreeImageBuffer(handle_, &raw);
+      if (ret != MV_OK) {
+        component::logger()->warn("MV_CC_FreeImageBuffer failed: {:#x}", ret);
+        break;
+      }
+    }
+
+    capturing_ = false;
+    component::logger()->info("HikRobot's capture thread stopped.");
+  }};
+}
+
+void HikRobot::capture_stop()
+{
+  capture_quit_ = true;
+  if (capture_thread_.joinable()) capture_thread_.join();
+
+  unsigned int ret;
+
+  ret = MV_CC_StopGrabbing(handle_);
+  if (ret != MV_OK) {
+    component::logger()->warn("MV_CC_StopGrabbing failed: {:#x}", ret);
+    return;
+  }
+
+  ret = MV_CC_CloseDevice(handle_);
+  if (ret != MV_OK) {
+    component::logger()->warn("MV_CC_CloseDevice failed: {:#x}", ret);
+    return;
+  }
+
+  ret = MV_CC_DestroyHandle(handle_);
+  if (ret != MV_OK) {
+    component::logger()->warn("MV_CC_DestroyHandle failed: {:#x}", ret);
+    return;
+  }
+}
+
+void HikRobot::set_float_value(const std::string & name, double value)
+{
+  unsigned int ret;
+
+  ret = MV_CC_SetFloatValue(handle_, name.c_str(), value);
+
+  if (ret != MV_OK) {
+    component::logger()->warn("MV_CC_SetFloatValue(\"{}\", {}) failed: {:#x}", name, value, ret);
+    return;
+  }
+}
+
+void HikRobot::set_enum_value(const std::string & name, unsigned int value)
+{
+  unsigned int ret;
+
+  ret = MV_CC_SetEnumValue(handle_, name.c_str(), value);
+
+  if (ret != MV_OK) {
+    component::logger()->warn("MV_CC_SetEnumValue(\"{}\", {}) failed: {:#x}", name, value, ret);
+    return;
+  }
+}
+
+void HikRobot::set_vid_pid(const std::string & vid_pid)
+{
+  auto index = vid_pid.find(':');
+  if (index == std::string::npos) {
+    component::logger()->warn("Invalid vid_pid: \"{}\"", vid_pid);
+    return;
+  }
+
+  auto vid_str = vid_pid.substr(0, index);
+  auto pid_str = vid_pid.substr(index + 1);
+
+  try {
+    vid_ = std::stoi(vid_str, 0, 16);
+    pid_ = std::stoi(pid_str, 0, 16);
+  } catch (const std::exception &) {
+    component::logger()->warn("Invalid vid_pid: \"{}\"", vid_pid);
+  }
+}
+
+void HikRobot::reset_usb() const
+{
+  if (vid_ == -1 || pid_ == -1) return;
+
+  // https://github.com/ralight/usb-reset/blob/master/usb-reset.c
+  auto handle = libusb_open_device_with_vid_pid(NULL, vid_, pid_);
+  if (!handle) {
+    component::logger()->warn("Unable to open usb!");
+    return;
+  }
+
+  if (libusb_reset_device(handle))
+    component::logger()->warn("Unable to reset usb!");
+  else
+    component::logger()->info("Reset usb successfully :)");
+
+  libusb_close(handle);
+}
+
+}  // namespace device

src/device/hikrobot/hikrobot.hpp

@@ -0,0 +1,56 @@
+#ifndef DEVICE__HIKROBOT_HPP
+#define DEVICE__HIKROBOT_HPP
+
+#include <atomic>
+#include <chrono>
+#include <opencv2/opencv.hpp>
+#include <string>
+#include <thread>
+
+#include "MvCameraControl.h"
+#include "src/device/camera.hpp"
+#include "src/component/thread_safe_queue.hpp"
+
+namespace device
+{
+class HikRobot : public CameraBase
+{
+public:
+  HikRobot(double exposure_ms, double gain, const std::string & vid_pid);
+  ~HikRobot() override;
+  void read(cv::Mat & img, std::chrono::steady_clock::time_point & timestamp) override;
+
+private:
+  struct CameraData
+  {
+    cv::Mat img;
+    std::chrono::steady_clock::time_point timestamp;
+  };
+
+  double exposure_us_;
+  double gain_;
+
+  std::thread daemon_thread_;
+  std::atomic<bool> daemon_quit_;
+
+  void * handle_;
+  std::thread capture_thread_;
+  std::atomic<bool> capturing_;
+  std::atomic<bool> capture_quit_;
+  component::ThreadSafeQueue<CameraData> queue_;
+
+  int vid_, pid_;
+
+  void capture_start();
+  void capture_stop();
+
+  void set_float_value(const std::string & name, double value);
+  void set_enum_value(const std::string & name, unsigned int value);
+
+  void set_vid_pid(const std::string & vid_pid);
+  void reset_usb() const;
+};
+
+}  // namespace device
+
+#endif  // DEVICE__HIKROBOT_HPP

src/device/hikrobot/include/MvErrorDefine.h

@@ -0,0 +1,143 @@
+
+#ifndef _MV_ERROR_DEFINE_H_
+#define _MV_ERROR_DEFINE_H_
+
+#include "MvISPErrorDefine.h"
+
+/********************************************************************/
+///  \~chinese
+///  \name 正确码定义
+///  @{
+///  \~english
+///  \name Definition of correct code
+///  @{
+#define MV_OK 0x00000000  ///< \~chinese 成功，无错误             \~english Successed, no error
+/// @}
+
+/********************************************************************/
+///  \~chinese
+///  \name 通用错误码定义:范围0x80000000-0x800000FF
+///  @{
+///  \~english
+///  \name Definition of General error code
+///  @{
+#define MV_E_HANDLE \
+  0x80000000  ///< \~chinese 错误或无效的句柄         \~english Error or invalid handle
+#define MV_E_SUPPORT \
+  0x80000001  ///< \~chinese 不支持的功能             \~english Not supported function
+#define MV_E_BUFOVER 0x80000002  ///< \~chinese 缓存已满                 \~english Buffer overflow
+#define MV_E_CALLORDER \
+  0x80000003  ///< \~chinese 函数调用顺序错误         \~english Function calling order error
+#define MV_E_PARAMETER \
+  0x80000004  ///< \~chinese 错误的参数               \~english Incorrect parameter
+#define MV_E_RESOURCE \
+  0x80000006  ///< \~chinese 资源申请失败             \~english Applying resource failed
+#define MV_E_NODATA 0x80000007  ///< \~chinese 无数据                   \~english No data
+#define MV_E_PRECONDITION \
+  0x80000008  ///< \~chinese 前置条件有误，或运行环境已发生变化   \~english Precondition error, or running environment changed
+#define MV_E_VERSION \
+  0x80000009  ///< \~chinese 版本不匹配               \~english Version mismatches
+#define MV_E_NOENOUGH_BUF \
+  0x8000000A  ///< \~chinese 传入的内存空间不足       \~english Insufficient memory
+#define MV_E_ABNORMAL_IMAGE \
+  0x8000000B  ///< \~chinese 异常图像，可能是丢包导致图像不完整   \~english Abnormal image, maybe incomplete image because of lost packet
+#define MV_E_LOAD_LIBRARY \
+  0x8000000C  ///< \~chinese 动态导入DLL失败          \~english Load library failed
+#define MV_E_NOOUTBUF \
+  0x8000000D  ///< \~chinese 没有可输出的缓存         \~english No Avaliable Buffer
+#define MV_E_ENCRYPT 0x8000000E   ///< \~chinese 加密错误                 \~english Encryption error
+#define MV_E_OPENFILE 0x8000000F  ///< \~chinese 打开文件出现错误         \~english open file error
+#define MV_E_UNKNOW 0x800000FF    ///< \~chinese 未知的错误               \~english Unknown error
+/// @}
+
+/********************************************************************/
+///  \~chinese
+///  \name GenICam系列错误:范围0x80000100-0x800001FF
+///  @{
+///  \~english
+///  \name GenICam Series Error Codes: Range from 0x80000100 to 0x800001FF
+///  @{
+#define MV_E_GC_GENERIC 0x80000100  ///< \~chinese 通用错误                 \~english General error
+#define MV_E_GC_ARGUMENT \
+  0x80000101  ///< \~chinese 参数非法                 \~english Illegal parameters
+#define MV_E_GC_RANGE \
+  0x80000102  ///< \~chinese 值超出范围               \~english The value is out of range
+#define MV_E_GC_PROPERTY 0x80000103  ///< \~chinese 属性                     \~english Property
+#define MV_E_GC_RUNTIME \
+  0x80000104  ///< \~chinese 运行环境有问题           \~english Running environment error
+#define MV_E_GC_LOGICAL 0x80000105  ///< \~chinese 逻辑错误                 \~english Logical error
+#define MV_E_GC_ACCESS \
+  0x80000106  ///< \~chinese 节点访问条件有误         \~english Node accessing condition error
+#define MV_E_GC_TIMEOUT 0x80000107  ///< \~chinese 超时                     \~english Timeout
+#define MV_E_GC_DYNAMICCAST \
+  0x80000108  ///< \~chinese 转换异常                 \~english Transformation exception
+#define MV_E_GC_UNKNOW \
+  0x800001FF  ///< \~chinese GenICam未知错误          \~english GenICam unknown error
+/// @}
+
+/********************************************************************/
+///  \~chinese
+///  \name GigE_STATUS对应的错误码:范围0x80000200-0x800002FF
+///  @{
+///  \~english
+///  \name GigE_STATUS Error Codes: Range from 0x80000200 to 0x800002FF
+///  @{
+#define MV_E_NOT_IMPLEMENTED \
+  0x80000200  ///< \~chinese 命令不被设备支持         \~english The command is not supported by device
+#define MV_E_INVALID_ADDRESS \
+  0x80000201  ///< \~chinese 访问的目标地址不存在     \~english The target address being accessed does not exist
+#define MV_E_WRITE_PROTECT \
+  0x80000202  ///< \~chinese 目标地址不可写           \~english The target address is not writable
+#define MV_E_ACCESS_DENIED \
+  0x80000203  ///< \~chinese 设备无访问权限           \~english No permission
+#define MV_E_BUSY \
+  0x80000204  ///< \~chinese 设备忙，或网络断开       \~english Device is busy, or network disconnected
+#define MV_E_PACKET \
+  0x80000205  ///< \~chinese 网络包数据错误           \~english Network data packet error
+#define MV_E_NETER 0x80000206  ///< \~chinese 网络相关错误             \~english Network error
+#define MV_E_IP_CONFLICT \
+  0x80000221  ///< \~chinese 设备IP冲突               \~english Device IP conflict
+/// @}
+
+/********************************************************************/
+///  \~chinese
+///  \name USB_STATUS对应的错误码:范围0x80000300-0x800003FF
+///  @{
+///  \~english
+///  \name USB_STATUS Error Codes: Range from 0x80000300 to 0x800003FF
+///  @{
+#define MV_E_USB_READ \
+  0x80000300  ///< \~chinese 读usb出错                \~english Reading USB error
+#define MV_E_USB_WRITE \
+  0x80000301  ///< \~chinese 写usb出错                \~english Writing USB error
+#define MV_E_USB_DEVICE \
+  0x80000302  ///< \~chinese 设备异常                 \~english Device exception
+#define MV_E_USB_GENICAM 0x80000303  ///< \~chinese GenICam相关错误          \~english GenICam error
+#define MV_E_USB_BANDWIDTH \
+  0x80000304  ///< \~chinese 带宽不足                 \~english Insufficient bandwidth
+#define MV_E_USB_DRIVER \
+  0x80000305  ///< \~chinese 驱动不匹配或者未装驱动   \~english Driver mismatch or unmounted drive
+#define MV_E_USB_UNKNOW \
+  0x800003FF  ///< \~chinese USB未知的错误            \~english USB unknown error
+/// @}
+
+/********************************************************************/
+///  \~chinese
+///  \name 升级时对应的错误码:范围0x80000400-0x800004FF
+///  @{
+///  \~english
+///  \name Upgrade Error Codes: Range from 0x80000400 to 0x800004FF
+///  @{
+#define MV_E_UPG_FILE_MISMATCH \
+  0x80000400  ///< \~chinese 升级固件不匹配           \~english Firmware mismatches
+#define MV_E_UPG_LANGUSGE_MISMATCH \
+  0x80000401  ///< \~chinese 升级固件语言不匹配       \~english Firmware language mismatches
+#define MV_E_UPG_CONFLICT \
+  0x80000402  ///< \~chinese 升级冲突（设备已经在升级了再次请求升级即返回此错误） \~english Upgrading conflicted (repeated upgrading requests during device upgrade)
+#define MV_E_UPG_INNER_ERR \
+  0x80000403  ///< \~chinese 升级时设备内部出现错误   \~english Camera internal error during upgrade
+#define MV_E_UPG_UNKNOW \
+  0x800004FF  ///< \~chinese 升级时未知错误           \~english Unknown error during upgrade
+/// @}
+
+#endif  //_MV_ERROR_DEFINE_H_

src/device/hikrobot/include/MvISPErrorDefine.h

@@ -0,0 +1,93 @@
+
+#ifndef _MV_ISP_ERROR_DEFINE_H_
+#define _MV_ISP_ERROR_DEFINE_H_
+
+/************************************************************************
+* 来自ISP算法库的错误码
+************************************************************************/
+// 通用类型
+#define MV_ALG_OK 0x00000000   //处理正确
+#define MV_ALG_ERR 0x10000000  //不确定类型错误
+
+// 能力检查
+#define MV_ALG_E_ABILITY_ARG 0x10000001  //能力集中存在无效参数
+
+// 内存检查
+#define MV_ALG_E_MEM_NULL 0x10000002        //内存地址为空
+#define MV_ALG_E_MEM_ALIGN 0x10000003       //内存对齐不满足要求
+#define MV_ALG_E_MEM_LACK 0x10000004        //内存空间大小不够
+#define MV_ALG_E_MEM_SIZE_ALIGN 0x10000005  //内存空间大小不满足对齐要求
+#define MV_ALG_E_MEM_ADDR_ALIGN 0x10000006  //内存地址不满足对齐要求
+
+// 图像检查
+#define MV_ALG_E_IMG_FORMAT 0x10000007     //图像格式不正确或者不支持
+#define MV_ALG_E_IMG_SIZE 0x10000008       //图像宽高不正确或者超出范围
+#define MV_ALG_E_IMG_STEP 0x10000009       //图像宽高与step参数不匹配
+#define MV_ALG_E_IMG_DATA_NULL 0x1000000A  //图像数据存储地址为空
+
+// 输入输出参数检查
+#define MV_ALG_E_CFG_TYPE 0x1000000B   //设置或者获取参数类型不正确
+#define MV_ALG_E_CFG_SIZE 0x1000000C   //设置或者获取参数的输入、输出结构体大小不正确
+#define MV_ALG_E_PRC_TYPE 0x1000000D   //处理类型不正确
+#define MV_ALG_E_PRC_SIZE 0x1000000E   //处理时输入、输出参数大小不正确
+#define MV_ALG_E_FUNC_TYPE 0x1000000F  //子处理类型不正确
+#define MV_ALG_E_FUNC_SIZE 0x10000010  //子处理时输入、输出参数大小不正确
+
+// 运行参数检查
+#define MV_ALG_E_PARAM_INDEX 0x10000011  //index参数不正确
+#define MV_ALG_E_PARAM_VALUE 0x10000012  //value参数不正确或者超出范围
+#define MV_ALG_E_PARAM_NUM 0x10000013    //param_num参数不正确
+
+// 接口调用检查
+#define MV_ALG_E_NULL_PTR 0x10000014      //函数参数指针为空
+#define MV_ALG_E_OVER_MAX_MEM 0x10000015  //超过限定的最大内存
+#define MV_ALG_E_CALL_BACK 0x10000016     //回调函数出错
+
+// 算法库加密相关检查
+#define MV_ALG_E_ENCRYPT 0x10000017  //加密错误
+#define MV_ALG_E_EXPIRE 0x10000018   //算法库使用期限错误
+
+// 内部模块返回的基本错误类型
+#define MV_ALG_E_BAD_ARG 0x10000019    //参数范围不正确
+#define MV_ALG_E_DATA_SIZE 0x1000001A  //数据大小不正确
+#define MV_ALG_E_STEP 0x1000001B       //数据step不正确
+
+// cpu指令集支持错误码
+#define MV_ALG_E_CPUID 0x1000001C  //cpu不支持优化代码中的指令集
+
+#define MV_ALG_WARNING 0x1000001D  //警告
+
+#define MV_ALG_E_TIME_OUT 0x1000001E          //算法库超时
+#define MV_ALG_E_LIB_VERSION 0x1000001F       //算法版本号出错
+#define MV_ALG_E_MODEL_VERSION 0x10000020     //模型版本号出错
+#define MV_ALG_E_GPU_MEM_ALLOC 0x10000021     //GPU内存分配错误
+#define MV_ALG_E_FILE_NON_EXIST 0x10000022    //文件不存在
+#define MV_ALG_E_NONE_STRING 0x10000023       //字符串为空
+#define MV_ALG_E_IMAGE_CODEC 0x10000024       //图像解码器错误
+#define MV_ALG_E_FILE_OPEN 0x10000025         //打开文件错误
+#define MV_ALG_E_FILE_READ 0x10000026         //文件读取错误
+#define MV_ALG_E_FILE_WRITE 0x10000027        //文件写错误
+#define MV_ALG_E_FILE_READ_SIZE 0x10000028    //文件读取大小错误
+#define MV_ALG_E_FILE_TYPE 0x10000029         //文件类型错误
+#define MV_ALG_E_MODEL_TYPE 0x1000002A        //模型类型错误
+#define MV_ALG_E_MALLOC_MEM 0x1000002B        //分配内存错误
+#define MV_ALG_E_BIND_CORE_FAILED 0x1000002C  //线程绑核失败
+
+// 降噪特有错误码
+#define MV_ALG_E_DENOISE_NE_IMG_FORMAT 0x10402001      //噪声特性图像格式错误
+#define MV_ALG_E_DENOISE_NE_FEATURE_TYPE 0x10402002    //噪声特性类型错误
+#define MV_ALG_E_DENOISE_NE_PROFILE_NUM 0x10402003     //噪声特性个数错误
+#define MV_ALG_E_DENOISE_NE_GAIN_NUM 0x10402004        //噪声特性增益个数错误
+#define MV_ALG_E_DENOISE_NE_GAIN_VAL 0x10402005        //噪声曲线增益值输入错误
+#define MV_ALG_E_DENOISE_NE_BIN_NUM 0x10402006         //噪声曲线柱数错误
+#define MV_ALG_E_DENOISE_NE_INIT_GAIN 0x10402007       //噪声估计初始化增益设置错误
+#define MV_ALG_E_DENOISE_NE_NOT_INIT 0x10402008        //噪声估计未初始化
+#define MV_ALG_E_DENOISE_COLOR_MODE 0x10402009         //颜色空间模式错误
+#define MV_ALG_E_DENOISE_ROI_NUM 0x1040200a            //图像ROI个数错误
+#define MV_ALG_E_DENOISE_ROI_ORI_PT 0x1040200b         //图像ROI原点错误
+#define MV_ALG_E_DENOISE_ROI_SIZE 0x1040200c           //图像ROI大小错误
+#define MV_ALG_E_DENOISE_GAIN_NOT_EXIST 0x1040200d     //输入的相机增益不存在(增益个数已达上限)
+#define MV_ALG_E_DENOISE_GAIN_BEYOND_RANGE 0x1040200e  //输入的相机增益不在范围内
+#define MV_ALG_E_DENOISE_NP_BUF_SIZE 0x1040200f        //输入的噪声特性内存大小错误
+
+#endif  //_MV_ISP_ERROR_DEFINE_H_

src/device/hikrobot/include/ObsoleteCamParams.h

@@ -0,0 +1,714 @@
+
+#ifndef _MV_OBSOLETE_CAM_PARAMS_H_
+#define _MV_OBSOLETE_CAM_PARAMS_H_
+
+#include "PixelType.h"
+
+/// \~chinese 输出帧的信息             \~english Output Frame Information
+typedef struct _MV_FRAME_OUT_INFO_
+{
+  unsigned short nWidth;             ///< [OUT]  \~chinese 图像宽            \~english Image Width
+  unsigned short nHeight;            ///< [OUT]  \~chinese 图像高            \~english Image Height
+  enum MvGvspPixelType enPixelType;  ///< [OUT]  \~chinese 像素格式          \~english Pixel Type
+
+  unsigned int nFrameNum;  ///< [OUT]  \~chinese 帧号              \~english Frame Number
+  unsigned int
+    nDevTimeStampHigh;  ///< [OUT]  \~chinese 时间戳高32位      \~english Timestamp high 32 bits
+  unsigned int
+    nDevTimeStampLow;  ///< [OUT]  \~chinese 时间戳低32位      \~english Timestamp low 32 bits
+  unsigned int
+    nReserved0;  ///< [OUT]  \~chinese 保留，8字节对齐   \~english Reserved, 8-byte aligned
+  int64_t
+    nHostTimeStamp;  ///< [OUT]  \~chinese 主机生成的时间戳  \~english Host-generated timestamp
+
+  unsigned int nFrameLen;
+
+  unsigned int nLostPacket;  // 本帧丢包数
+  unsigned int nReserved[2];
+} MV_FRAME_OUT_INFO;
+
+/// \~chinese 保存图片参数            \~english Save image type
+typedef struct _MV_SAVE_IMAGE_PARAM_T_
+{
+  unsigned char * pData;  ///< [IN]   \~chinese 输入数据缓存      \~english Input Data Buffer
+  unsigned int nDataLen;  ///< [IN]   \~chinese 输入数据大小      \~english Input Data Size
+  enum MvGvspPixelType
+    enPixelType;           ///< [IN]   \~chinese 输入像素格式      \~english Input Data Pixel Format
+  unsigned short nWidth;   ///< [IN]   \~chinese 图像宽            \~english Image Width
+  unsigned short nHeight;  ///< [IN]   \~chinese 图像高            \~english Image Height
+
+  unsigned char *
+    pImageBuffer;          ///< [OUT]  \~chinese 输出图片缓存      \~english Output Image Buffer
+  unsigned int nImageLen;  ///< [OUT]  \~chinese 输出图片大小      \~english Output Image Size
+  unsigned int
+    nBufferSize;  ///< [IN]   \~chinese 提供的输出缓冲区大小  \~english Output buffer size provided
+  enum MV_SAVE_IAMGE_TYPE
+    enImageType;  ///< [IN]   \~chinese 输出图片格式      \~english Output Image Format
+
+} MV_SAVE_IMAGE_PARAM;
+
+typedef struct _MV_IMAGE_BASIC_INFO_
+{
+  unsigned short nWidthValue;
+  unsigned short nWidthMin;
+  unsigned int nWidthMax;
+  unsigned int nWidthInc;
+
+  unsigned int nHeightValue;
+  unsigned int nHeightMin;
+  unsigned int nHeightMax;
+  unsigned int nHeightInc;
+
+  float fFrameRateValue;
+  float fFrameRateMin;
+  float fFrameRateMax;
+
+  unsigned int enPixelType;  ///< [OUT]  \~chinese 当前的像素格式    \~english Current pixel format
+  unsigned int
+    nSupportedPixelFmtNum;  ///< [OUT]  \~chinese 支持的像素格式种类    \~english Support pixel format
+  unsigned int enPixelList[MV_MAX_XML_SYMBOLIC_NUM];
+  unsigned int nReserved[8];
+
+} MV_IMAGE_BASIC_INFO;
+
+/// \~chinese 噪声特性类型    \~english Noise feature type
+typedef enum _MV_CC_BAYER_NOISE_FEATURE_TYPE
+{
+  MV_CC_BAYER_NOISE_FEATURE_TYPE_INVALID =
+    0,  ///<        \~chinese 无效值                        \~english Invalid
+  MV_CC_BAYER_NOISE_FEATURE_TYPE_PROFILE =
+    1,  ///<        \~chinese 噪声曲线                      \~english Noise curve
+  MV_CC_BAYER_NOISE_FEATURE_TYPE_LEVEL =
+    2,  ///<        \~chinese 噪声水平                      \~english Noise level
+  MV_CC_BAYER_NOISE_FEATURE_TYPE_DEFAULT =
+    1,  ///<        \~chinese 默认值                        \~english Default
+
+} MV_CC_BAYER_NOISE_FEATURE_TYPE;
+
+/// \~chinese Bayer格式降噪特性信息    \~english Denoise profile info
+typedef struct _MV_CC_BAYER_NOISE_PROFILE_INFO_T_
+{
+  unsigned int nVersion;  ///<  \~chinese 版本                           \~english version
+  MV_CC_BAYER_NOISE_FEATURE_TYPE
+    enNoiseFeatureType;  ///<  \~chinese 噪声特性类型       \~english noise feature type
+  enum MvGvspPixelType
+    enPixelType;    ///<  \~chinese 图像格式                       \~english image format
+  int nNoiseLevel;  ///<  \~chinese 平均噪声水平                   \~english noise level
+  unsigned int
+    nCurvePointNum;   ///<  \~chinese 曲线点数                       \~english curve point number
+  int * nNoiseCurve;  ///<  \~chinese 噪声曲线                       \~english noise curve
+  int * nLumCurve;    ///<  \~chinese 亮度曲线                       \~english luminance curve
+
+  unsigned int nRes[8];  ///<       \~chinese 预留                           \~english Reserved
+
+} MV_CC_BAYER_NOISE_PROFILE_INFO;
+
+/// \~chinese Bayer格式噪声估计参数    \~english Bayer noise estimate param
+typedef struct _MV_CC_BAYER_NOISE_ESTIMATE_PARAM_T_
+{
+  unsigned int nWidth;   ///< [IN]  \~chinese 图像宽(大于等于8)              \~english Width
+  unsigned int nHeight;  ///< [IN]  \~chinese 图像高(大于等于8)              \~english Height
+  enum MvGvspPixelType
+    enPixelType;  ///< [IN]  \~chinese 像素格式                       \~english Pixel format
+
+  unsigned char *
+    pSrcData;  ///< [IN]  \~chinese 输入数据缓存                   \~english Input data buffer
+  unsigned int
+    nSrcDataLen;  ///< [IN]  \~chinese 输入数据大小                   \~english Input data size
+
+  unsigned int
+    nNoiseThreshold;  ///< [IN]  \~chinese 噪声阈值(0-4095)               \~english Noise Threshold
+
+  unsigned char *
+    pCurveBuf;  ///< [IN]  \~chinese 用于存储噪声曲线和亮度曲线（需要外部分配，缓存大小：4096 * sizeof(int) * 2）     \~english Buffer used to store noise and brightness curves, size:4096 * sizeof(int) * 2)
+  MV_CC_BAYER_NOISE_PROFILE_INFO
+    stNoiseProfile;  ///< [OUT]  \~chinese 降噪特性信息               \~english Denoise profile
+
+  unsigned int
+    nThreadNum;  ///< [IN]  \~chinese 线程数量，0表示算法库根据硬件自适应；1表示单线程（默认）；大于1表示线程数目      \~english Thread number, 0 means that the library is adaptive to the hardware, 1 means single thread(Default value), Greater than 1 indicates the number of threads
+
+  unsigned int nRes[8];  ///<       \~chinese 预留                           \~english Reserved
+
+} MV_CC_BAYER_NOISE_ESTIMATE_PARAM;
+
+/// \~chinese Bayer格式空域降噪参数    \~english Bayer spatial Denoise param
+typedef struct _MV_CC_BAYER_SPATIAL_DENOISE_PARAM_T_
+{
+  unsigned int nWidth;   ///< [IN]  \~chinese 图像宽(大于等于8)              \~english Width
+  unsigned int nHeight;  ///< [IN]  \~chinese 图像高(大于等于8)              \~english Height
+  enum MvGvspPixelType
+    enPixelType;  ///< [IN]  \~chinese 像素格式                       \~english Pixel format
+
+  unsigned char *
+    pSrcData;  ///< [IN]  \~chinese 输入数据缓存                   \~english Input data buffer
+  unsigned int
+    nSrcDataLen;  ///< [IN]  \~chinese 输入数据大小                   \~english Input data size
+
+  unsigned char *
+    pDstBuf;  ///< [OUT] \~chinese 输出降噪后的数据               \~english Output data buffer
+  unsigned int
+    nDstBufSize;  ///< [IN]  \~chinese 提供的输出缓冲区大小           \~english Provided output buffer size
+  unsigned int
+    nDstBufLen;  ///< [OUT] \~chinese 输出降噪后的数据长度           \~english Output data length
+
+  MV_CC_BAYER_NOISE_PROFILE_INFO
+    stNoiseProfile;  ///< [IN]  \~chinese 降噪特性信息(来源于噪声估计)   \~english Denoise profile
+  unsigned int
+    nDenoiseStrength;  ///< [IN]  \~chinese 降噪强度(0-100)                \~english nDenoise Strength
+  unsigned int
+    nSharpenStrength;  ///< [IN]  \~chinese 锐化强度(0-32)                 \~english Sharpen Strength
+  unsigned int
+    nNoiseCorrect;  ///< [IN]  \~chinese 噪声校正系数(0-1280)           \~english Noise Correct
+
+  unsigned int
+    nThreadNum;  ///< [IN]  \~chinese 线程数量，0表示算法库根据硬件自适应；1表示单线程（默认）；大于1表示线程数目         \~english Thread number, 0 means that the library is adaptive to the hardware, 1 means single thread(Default value), Greater than 1 indicates the number of threads
+
+  unsigned int nRes[8];  ///<       \~chinese 预留                           \~english Reserved
+
+} MV_CC_BAYER_SPATIAL_DENOISE_PARAM;
+
+/// \~chinese CLUT参数                  \~english CLUT param
+typedef struct _MV_CC_CLUT_PARAM_T_
+{
+  bool bCLUTEnable;  ///< [IN]  \~chinese 是否启用CLUT           \~english CLUT enable
+  unsigned int
+    nCLUTScale;  ///< [IN]  \~chinese 量化系数(2的整数幂,最大65536)  \~english Quantitative scale(Integer power of 2, <= 65536)
+  unsigned int
+    nCLUTSize;  ///< [IN]  \~chinese CLUT大小,[17,33]（建议值17）     \~english CLUT size[17,33](Recommended values of 17)
+  unsigned char * pCLUTBuf;  ///< [IN]  \~chinese 量化CLUT表             \~english CLUT buffer
+  unsigned int
+    nCLUTBufLen;  ///< [IN]  \~chinese 量化CLUT缓存大小(nCLUTSize*nCLUTSize*nCLUTSize*sizeof(int)*3)  \~english CLUT buffer length(nCLUTSize*nCLUTSize*nCLUTSize*sizeof(int)*3)
+
+  unsigned int nRes[8];  ///<       \~chinese 预留                   \~english Reserved
+
+} MV_CC_CLUT_PARAM;
+
+/// \~chinese 锐化结构体                \~english Sharpen structure
+typedef struct _MV_CC_SHARPEN_PARAM_T_
+{
+  unsigned int nWidth;      ///< [IN]  \~chinese 图像宽度(最小8)        \~english Image Width
+  unsigned int nHeight;     ///< [IN]  \~chinese 图像高度(最小8)        \~english Image Height
+  unsigned char * pSrcBuf;  ///< [IN]  \~chinese 输入数据缓存           \~english Input data buffer
+  unsigned int nSrcBufLen;  ///< [IN]  \~chinese 输入数据大小           \~english Input data length
+  enum MvGvspPixelType
+    enPixelType;  ///< [IN]  \~chinese 像素格式               \~english Pixel format
+
+  unsigned char * pDstBuf;  ///< [OUT] \~chinese 输出数据缓存           \~english Output data buffer
+  unsigned int
+    nDstBufSize;  ///< [IN]  \~chinese 提供的输出缓冲区大小   \~english Provided output buffer size
+  unsigned int nDstBufLen;  ///< [OUT] \~chinese 输出数据长度           \~english Output data length
+
+  unsigned int
+    nSharpenAmount;  ///< [IN]  \~chinese 锐度调节强度，[0,500]  \~english Sharpen amount,[0,500]  //   [nSharpenAmount 作废， 使用 nSharpenPosAmount & nSharpenNegAmount 替代 ]
+  unsigned int
+    nSharpenRadius;  ///< [IN]  \~chinese 锐度调节半径(半径越大，耗时越长)，[1,21]   \~english Sharpen radius(The larger the radius, the longer it takes),[1,21]
+  unsigned int
+    nSharpenThreshold;  ///< [IN]  \~chinese 锐度调节阈值，[0,255]  \~english Sharpen threshold,[0,255]
+
+  unsigned int nSharpenPosAmount;  // [IN]     锐度调节正向强度，范围:[0, 500]
+  unsigned int nSharpenNegAmount;  // [IN]     锐度调节负向强度，范围:[0, 500]
+
+  unsigned int nRes[6];  ///<       \~chinese 预留                   \~english Reserved
+
+} MV_CC_SHARPEN_PARAM;
+
+/// \~chinese 色彩校正结构体            \~english Color correct structure
+typedef struct _MV_CC_COLOR_CORRECT_PARAM_T_
+{
+  unsigned int nWidth;      ///< [IN]  \~chinese 图像宽度               \~english Image Width
+  unsigned int nHeight;     ///< [IN]  \~chinese 图像高度               \~english Image Height
+  unsigned char * pSrcBuf;  ///< [IN]  \~chinese 输入数据缓存           \~english Input data buffer
+  unsigned int nSrcBufLen;  ///< [IN]  \~chinese 输入数据大小           \~english Input data length
+  enum MvGvspPixelType
+    enPixelType;  ///< [IN]  \~chinese 像素格式               \~english Pixel format
+
+  unsigned char * pDstBuf;  ///< [OUT] \~chinese 输出数据缓存           \~english Output data buffer
+  unsigned int
+    nDstBufSize;  ///< [IN]  \~chinese 提供的输出缓冲区大小   \~english Provided output buffer size
+  unsigned int nDstBufLen;  ///< [OUT] \~chinese 输出数据长度           \~english Output data length
+
+  unsigned int
+    nImageBit;  ///< [IN]  \~chinese 有效图像位数(8,10,12,16)   \~english Image bit(8 or 10 or 12 or 16)
+  MV_CC_GAMMA_PARAM stGammaParam;  ///< [IN]  \~chinese Gamma信息              \~english Gamma info
+  MV_CC_CCM_PARAM_EX stCCMParam;   ///< [IN]  \~chinese CCM信息                \~english CCM info
+  MV_CC_CLUT_PARAM stCLUTParam;    ///< [IN]  \~chinese CLUT信息               \~english CLUT info
+
+  unsigned int nRes[8];  ///<       \~chinese 预留                   \~english Reserved
+
+} MV_CC_COLOR_CORRECT_PARAM;
+
+/// \~chinese 矩形ROI结构体             \~english Rect ROI structure
+typedef struct _MV_CC_RECT_I_
+{
+  unsigned int nX;       ///< \~chinese 矩形左上角X轴坐标            \~english X Position
+  unsigned int nY;       ///< \~chinese 矩形左上角Y轴坐标            \~english Y Position
+  unsigned int nWidth;   ///< \~chinese 矩形宽度                     \~english Rect Width
+  unsigned int nHeight;  ///< \~chinese 矩形高度                     \~english Rect Height
+
+} MV_CC_RECT_I;
+
+/// \~chinese 噪声估计结构体            \~english Noise estimate structure
+typedef struct _MV_CC_NOISE_ESTIMATE_PARAM_T_
+{
+  unsigned int nWidth;   ///< [IN]  \~chinese 图像宽度(最小8)        \~english Image Width
+  unsigned int nHeight;  ///< [IN]  \~chinese 图像高度(最小8)        \~english Image Height
+  enum MvGvspPixelType
+    enPixelType;            ///< [IN]  \~chinese 像素格式               \~english Pixel format
+  unsigned char * pSrcBuf;  ///< [IN]  \~chinese 输入数据缓存           \~english Input data buffer
+  unsigned int nSrcBufLen;  ///< [IN]  \~chinese 输入数据大小           \~english Input data length
+
+  MV_CC_RECT_I * pstROIRect;  ///< [IN]  \~chinese 图像ROI                \~english Image ROI
+  unsigned int nROINum;       ///< [IN]  \~chinese ROI个数                \~english ROI number
+
+  ///< \~chinese Bayer域噪声估计参数，Mono8/RGB域无效     \~english Bayer Noise estimate param,Mono8/RGB formats are invalid
+  unsigned int
+    nNoiseThreshold;  ///< [IN]  \~chinese 噪声阈值[0,4095]       \~english Noise threshold[0,4095]
+  ///< \~chinese 建议值:8bit,0xE0;10bit,0x380;12bit,0xE00     \~english Suggestive value:8bit,0xE0;10bit,0x380;12bit,0xE00
+
+  unsigned char *
+    pNoiseProfile;  ///< [OUT] \~chinese 输出噪声特性           \~english Output Noise Profile
+  unsigned int
+    nNoiseProfileSize;  ///< [IN]  \~chinese 提供的输出缓冲区大小   \~english Provided output buffer size
+  unsigned int
+    nNoiseProfileLen;  ///< [OUT] \~chinese 输出噪声特性长度       \~english Output Noise Profile length
+
+  unsigned int nRes[8];  ///<       \~chinese 预留                   \~english Reserved
+
+} MV_CC_NOISE_ESTIMATE_PARAM;
+
+/// \~chinese 空域降噪结构体            \~english Spatial denoise structure
+typedef struct _MV_CC_SPATIAL_DENOISE_PARAM_T_
+{
+  unsigned int nWidth;   ///< [IN]  \~chinese 图像宽度(最小8)        \~english Image Width
+  unsigned int nHeight;  ///< [IN]  \~chinese 图像高度(最小8)        \~english Image Height
+  enum MvGvspPixelType
+    enPixelType;            ///< [IN]  \~chinese 像素格式               \~english Pixel format
+  unsigned char * pSrcBuf;  ///< [IN]  \~chinese 输入数据缓存           \~english Input data buffer
+  unsigned int nSrcBufLen;  ///< [IN]  \~chinese 输入数据大小           \~english Input data length
+
+  unsigned char * pDstBuf;  ///< [OUT] \~chinese 输出降噪后的数据       \~english Output data buffer
+  unsigned int
+    nDstBufSize;  ///< [IN]  \~chinese 提供的输出缓冲区大小   \~english Provided output buffer size
+  unsigned int nDstBufLen;  ///< [OUT] \~chinese 输出降噪后的数据长度   \~english Output data length
+
+  unsigned char *
+    pNoiseProfile;  ///< [IN]  \~chinese 输入噪声特性           \~english Input Noise Profile
+  unsigned int
+    nNoiseProfileLen;  ///< [IN]  \~chinese 输入噪声特性长度       \~english Input Noise Profile length
+
+  ///< \~chinese Bayer域空域降噪参数，Mono8/RGB域无效     \~english Bayer Spatial denoise param,Mono8/RGB formats are invalid
+  unsigned int
+    nBayerDenoiseStrength;  ///< [IN]  \~chinese 降噪强度[0,100]        \~english Denoise Strength[0,100]
+  unsigned int
+    nBayerSharpenStrength;  ///< [IN]  \~chinese 锐化强度[0,32]         \~english Sharpen Strength[0,32]
+  unsigned int
+    nBayerNoiseCorrect;  ///< [IN]  \~chinese 噪声校正系数[0,1280]   \~english Noise Correct[0,1280]
+
+  ///< \~chinese Mono8/RGB域空域降噪参数，Bayer域无效     \~english Mono8/RGB Spatial denoise param,Bayer formats are invalid
+  unsigned int
+    nNoiseCorrectLum;  ///< [IN]  \~chinese 亮度校正系数[1,2000]   \~english Noise Correct Lum[1,2000]
+  unsigned int
+    nNoiseCorrectChrom;  ///< [IN]  \~chinese 色调校正系数[1,2000]   \~english Noise Correct Chrom[1,2000]
+  unsigned int
+    nStrengthLum;  ///< [IN]  \~chinese 亮度降噪强度[0,100]    \~english Strength Lum[0,100]
+  unsigned int
+    nStrengthChrom;  ///< [IN]  \~chinese 色调降噪强度[0,100]    \~english Strength Chrom[0,100]
+  unsigned int
+    nStrengthSharpen;  ///< [IN]  \~chinese 锐化强度[1,1000]       \~english Strength Sharpen[1,1000]
+
+  unsigned int nRes[8];  ///<       \~chinese 预留                   \~english Reserved
+
+} MV_CC_SPATIAL_DENOISE_PARAM;
+
+/// \~chinese LSC标定结构体             \~english LSC calib structure
+typedef struct _MV_CC_LSC_CALIB_PARAM_T_
+{
+  unsigned int nWidth;   ///< [IN]  \~chinese 图像宽度[16,65535]     \~english Image Width
+  unsigned int nHeight;  ///< [IN]  \~chinese 图像高度[16-65535]     \~english Image Height
+  enum MvGvspPixelType
+    enPixelType;            ///< [IN]  \~chinese 像素格式               \~english Pixel format
+  unsigned char * pSrcBuf;  ///< [IN]  \~chinese 输入数据缓存           \~english Input data buffer
+  unsigned int nSrcBufLen;  ///< [IN]  \~chinese 输入数据长度           \~english Input data length
+
+  unsigned char *
+    pCalibBuf;  ///< [OUT] \~chinese 输出标定表缓存         \~english Output calib buffer
+  unsigned int
+    nCalibBufSize;  ///< [IN]  \~chinese 提供的标定表缓冲大小(nWidth*nHeight*sizeof(unsigned short))    \~english Provided output buffer size
+  unsigned int
+    nCalibBufLen;  ///< [OUT] \~chinese 输出标定表缓存长度     \~english Output calib buffer length
+
+  unsigned int nSecNumW;  ///< [IN]  \~chinese 宽度分块数             \~english Width Sec num
+  unsigned int nSecNumH;  ///< [IN]  \~chinese 高度分块数             \~english Height Sec num
+  unsigned int nPadCoef;  ///< [IN]  \~chinese 边缘填充系数[1,5]      \~english Pad Coef[1,5]
+  unsigned int
+    nCalibMethod;  ///< [IN]  \~chinese 标定方式(0-中心为基准;1-最亮区域为基准;2-目标亮度为基准) \~english Calib method
+  unsigned int
+    nTargetGray;  ///< [IN]  \~chinese 目标亮度(标定方式为2时有效)    \~english Target Gray
+  ///< \~chinese 8位,范围：[0,255]            \~english 8bit,range:[0,255]
+  ///< \~chinese 10位,范围：[0,1023]          \~english 10bit,range:[0,1023]
+  ///< \~chinese 12位,范围：[0,4095]          \~english 12bit,range:[0,4095]
+
+  unsigned int nRes[8];  ///<       \~chinese 预留                   \~english Reserved
+
+} MV_CC_LSC_CALIB_PARAM;
+
+/// \~chinese LSC校正结构体             \~english LSC correct structure
+typedef struct _MV_CC_LSC_CORRECT_PARAM_T_
+{
+  unsigned int nWidth;   ///< [IN]  \~chinese 图像宽度[16,65535]     \~english Image Width
+  unsigned int nHeight;  ///< [IN]  \~chinese 图像高度[16,65535]     \~english Image Height
+  enum MvGvspPixelType
+    enPixelType;            ///< [IN]  \~chinese 像素格式               \~english Pixel format
+  unsigned char * pSrcBuf;  ///< [IN]  \~chinese 输入数据缓存           \~english Input data buffer
+  unsigned int nSrcBufLen;  ///< [IN]  \~chinese 输入数据长度           \~english Input data length
+
+  unsigned char * pDstBuf;  ///< [OUT] \~chinese 输出数据缓存           \~english Output data buffer
+  unsigned int
+    nDstBufSize;  ///< [IN]  \~chinese 提供的输出缓冲区大小   \~english Provided output buffer size
+  unsigned int nDstBufLen;  ///< [OUT] \~chinese 输出数据长度           \~english Output data length
+
+  unsigned char *
+    pCalibBuf;  ///< [IN]  \~chinese 输入标定表缓存         \~english Input calib buffer
+  unsigned int
+    nCalibBufLen;  ///< [IN]  \~chinese 输入标定表缓存长度     \~english Input calib buffer length
+
+  unsigned int nRes[8];  ///<       \~chinese 预留                   \~english Reserved
+
+} MV_CC_LSC_CORRECT_PARAM;
+
+/// \~chinese 某个节点对应的子节点个数最大值    \~english The maximum number of child nodes corresponding to a node
+#define MV_MAX_XML_NODE_NUM_C 128
+
+/// \~chinese 节点名称字符串最大长度            \~english The maximum length of node name string
+#define MV_MAX_XML_NODE_STRLEN_C 64
+
+/// \~chinese 节点String值最大长度              \~english The maximum length of Node String
+#define MV_MAX_XML_STRVALUE_STRLEN_C 64
+
+/// \~chinese 节点描述字段最大长度              \~english The maximum length of the node description field
+#define MV_MAX_XML_DISC_STRLEN_C 512
+
+/// \~chinese 最多的单元数                      \~english The maximum number of units
+#define MV_MAX_XML_ENTRY_NUM 10
+
+/// \~chinese 父节点个数上限                    \~english The maximum number of parent nodes
+#define MV_MAX_XML_PARENTS_NUM 8
+
+/// \~chinese 每个已经实现单元的名称长度        \~english The length of the name of each unit that has been implemented
+#define MV_MAX_XML_SYMBOLIC_STRLEN_C 64
+
+enum MV_XML_Visibility
+{
+  V_Beginner = 0,   ///< Always visible
+  V_Expert = 1,     ///< Visible for experts or Gurus
+  V_Guru = 2,       ///< Visible for Gurus
+  V_Invisible = 3,  ///< Not Visible
+  V_Undefined = 99  ///< Object is not yet initialized
+};
+
+/// \~chinese 单个节点基本属性 | en:Single Node Basic Attributes
+typedef struct _MV_XML_NODE_FEATURE_
+{
+  enum MV_XML_InterfaceType enType;     ///< \~chinese 节点类型         \~english Node Type
+  enum MV_XML_Visibility enVisivility;  ///< \~chinese 是否可见         \~english Is visibility
+  char strDescription
+    [MV_MAX_XML_DISC_STRLEN_C];  ///< \~chinese 节点描述,目前暂不支持   \~english Node Description, NOT SUPPORT NOW
+  char strDisplayName
+    [MV_MAX_XML_NODE_STRLEN_C];               ///< \~chinese 显示名称         \~english Display Name
+  char strName[MV_MAX_XML_NODE_STRLEN_C];     ///< \~chinese 节点名           \~english Node Name
+  char strToolTip[MV_MAX_XML_DISC_STRLEN_C];  ///< \~chinese 提示             \~english Notice
+
+  unsigned int nReserved[4];
+} MV_XML_NODE_FEATURE;
+
+/// \~chinese 节点列表 | en:Node List
+typedef struct _MV_XML_NODES_LIST_
+{
+  unsigned int nNodeNum;  ///< \~chinese 节点个数             \~english Node Number
+  MV_XML_NODE_FEATURE stNodes[MV_MAX_XML_NODE_NUM_C];
+} MV_XML_NODES_LIST;
+
+typedef struct _MV_XML_FEATURE_Value_
+{
+  enum MV_XML_InterfaceType enType;  ///< \~chinese 节点类型         \~english Node Type
+  char strDescription
+    [MV_MAX_XML_DISC_STRLEN_C];  ///< \~chinese 节点描述,目前暂不支持   \~english Node Description, NOT SUPPORT NOW
+  char strDisplayName
+    [MV_MAX_XML_NODE_STRLEN_C];               ///< \~chinese 显示名称         \~english Display Name
+  char strName[MV_MAX_XML_NODE_STRLEN_C];     ///< \~chinese 节点名           \~english Node Name
+  char strToolTip[MV_MAX_XML_DISC_STRLEN_C];  ///< \~chinese 提示             \~english Notice
+  unsigned int nReserved[4];
+} MV_XML_FEATURE_Value;
+
+typedef struct _MV_XML_FEATURE_Base_
+{
+  enum MV_XML_AccessMode enAccessMode;  ///< \~chinese 访问模式         \~english Access Mode
+} MV_XML_FEATURE_Base;
+
+typedef struct _MV_XML_FEATURE_Integer_
+{
+  char strName[MV_MAX_XML_NODE_STRLEN_C];
+  char strDisplayName[MV_MAX_XML_NODE_STRLEN_C];
+  char strDescription
+    [MV_MAX_XML_DISC_STRLEN_C];  ///< \~chinese 目前暂不支持     \~english NOT SUPPORT NOW
+  char strToolTip[MV_MAX_XML_DISC_STRLEN_C];
+
+  enum MV_XML_Visibility enVisivility;  ///< \~chinese 是否可见         \~english Visible
+  enum MV_XML_AccessMode enAccessMode;  ///< \~chinese 访问模式         \~english Access Mode
+  int
+    bIsLocked;  ///< \~chinese 是否锁定。0-否；1-是,目前暂不支持    \~english Locked. 0-NO; 1-YES, NOT SUPPORT NOW
+  int64_t nValue;      ///< \~chinese 当前值           \~english Current Value
+  int64_t nMinValue;   ///< \~chinese 最小值           \~english Min Value
+  int64_t nMaxValue;   ///< \~chinese 最大值           \~english Max Value
+  int64_t nIncrement;  ///< \~chinese 增量             \~english Increment
+
+  unsigned int nReserved[4];
+
+} MV_XML_FEATURE_Integer;
+
+typedef struct _MV_XML_FEATURE_Boolean_
+{
+  char strName[MV_MAX_XML_NODE_STRLEN_C];
+  char strDisplayName[MV_MAX_XML_NODE_STRLEN_C];
+  char strDescription
+    [MV_MAX_XML_DISC_STRLEN_C];  ///< \~chinese 目前暂不支持     \~english NOT SUPPORT NOW
+  char strToolTip[MV_MAX_XML_DISC_STRLEN_C];
+
+  enum MV_XML_Visibility enVisivility;  ///< \~chinese 是否可见         \~english Visible
+  enum MV_XML_AccessMode enAccessMode;  ///< \~chinese 访问模式         \~english Access Mode
+  int
+    bIsLocked;  ///< \~chinese 是否锁定。0-否；1-是,目前暂不支持    \~english Locked. 0-NO; 1-YES, NOT SUPPORT NOW
+  bool bValue;  ///< \~chinese 当前值           \~english Current Value
+
+  unsigned int nReserved[4];
+} MV_XML_FEATURE_Boolean;
+
+typedef struct _MV_XML_FEATURE_Command_
+{
+  char strName[MV_MAX_XML_NODE_STRLEN_C];
+  char strDisplayName[MV_MAX_XML_NODE_STRLEN_C];
+  char strDescription
+    [MV_MAX_XML_DISC_STRLEN_C];  ///< \~chinese 目前暂不支持     \~english NOT SUPPORT NOW
+  char strToolTip[MV_MAX_XML_DISC_STRLEN_C];
+
+  enum MV_XML_Visibility enVisivility;  ///< \~chinese 是否可见         \~english Visible
+  enum MV_XML_AccessMode enAccessMode;  ///< \~chinese 访问模式         \~english Access Mode
+  int
+    bIsLocked;  ///< \~chinese 是否锁定。0-否；1-是,目前暂不支持    \~english Locked. 0-NO; 1-YES, NOT SUPPORT NOW
+
+  unsigned int nReserved[4];
+} MV_XML_FEATURE_Command;
+
+typedef struct _MV_XML_FEATURE_Float_
+{
+  char strName[MV_MAX_XML_NODE_STRLEN_C];
+  char strDisplayName[MV_MAX_XML_NODE_STRLEN_C];
+  char strDescription
+    [MV_MAX_XML_DISC_STRLEN_C];  ///< \~chinese 目前暂不支持     \~english NOT SUPPORT NOW
+  char strToolTip[MV_MAX_XML_DISC_STRLEN_C];
+
+  enum MV_XML_Visibility enVisivility;  ///< \~chinese 是否可见         \~english Visible
+  enum MV_XML_AccessMode enAccessMode;  ///< \~chinese 访问模式         \~english Access Mode
+  int
+    bIsLocked;  ///< \~chinese 是否锁定。0-否；1-是,目前暂不支持    \~english Locked. 0-NO; 1-YES, NOT SUPPORT NOW
+  double dfValue;      ///< \~chinese 当前值           \~english Current Value
+  double dfMinValue;   ///< \~chinese 最小值           \~english Min Value
+  double dfMaxValue;   ///< \~chinese 最大值           \~english Max Value
+  double dfIncrement;  ///< \~chinese 增量             \~english Increment
+
+  unsigned int nReserved[4];
+} MV_XML_FEATURE_Float;
+
+typedef struct _MV_XML_FEATURE_String_
+{
+  char strName[MV_MAX_XML_NODE_STRLEN_C];
+  char strDisplayName[MV_MAX_XML_NODE_STRLEN_C];
+  char strDescription
+    [MV_MAX_XML_DISC_STRLEN_C];  ///< \~chinese 目前暂不支持     \~english NOT SUPPORT NOW
+  char strToolTip[MV_MAX_XML_DISC_STRLEN_C];
+
+  enum MV_XML_Visibility enVisivility;  ///< \~chinese 是否可见         \~english Visible
+  enum MV_XML_AccessMode enAccessMode;  ///< \~chinese 访问模式         \~english Access Mode
+  int
+    bIsLocked;  ///< \~chinese 是否锁定。0-否；1-是,目前暂不支持    \~english Locked. 0-NO; 1-YES, NOT SUPPORT NOW
+  char
+    strValue[MV_MAX_XML_STRVALUE_STRLEN_C];  ///< \~chinese 当前值           \~english Current Value
+
+  unsigned int nReserved[4];
+} MV_XML_FEATURE_String;
+
+typedef struct _MV_XML_FEATURE_Register_
+{
+  char strName[MV_MAX_XML_NODE_STRLEN_C];
+  char strDisplayName[MV_MAX_XML_NODE_STRLEN_C];
+  char strDescription
+    [MV_MAX_XML_DISC_STRLEN_C];  ///< \~chinese 目前暂不支持     \~english NOT SUPPORT NOW
+  char strToolTip[MV_MAX_XML_DISC_STRLEN_C];
+
+  enum MV_XML_Visibility enVisivility;  ///< \~chinese 是否可见         \~english Visible
+  enum MV_XML_AccessMode enAccessMode;  ///< \~chinese 访问模式         \~english Access Mode
+  int
+    bIsLocked;  ///< \~chinese 是否锁定。0-否；1-是,目前暂不支持    \~english Locked. 0-NO; 1-YES, NOT SUPPORT NOW
+  int64_t nAddrValue;  ///< \~chinese 当前值           \~english Current Value
+
+  unsigned int nReserved[4];
+} MV_XML_FEATURE_Register;
+
+typedef struct _MV_XML_FEATURE_Category_
+{
+  char strDescription
+    [MV_MAX_XML_DISC_STRLEN_C];  ///< \~chinese 节点描述 目前暂不支持    \~english Node Description, NOT SUPPORT NOW
+  char strDisplayName
+    [MV_MAX_XML_NODE_STRLEN_C];               ///< \~chinese 显示名称         \~english Display Name
+  char strName[MV_MAX_XML_NODE_STRLEN_C];     ///< \~chinese 节点名           \~english Node Name
+  char strToolTip[MV_MAX_XML_DISC_STRLEN_C];  ///< \~chinese 提示             \~english Notice
+
+  enum MV_XML_Visibility enVisivility;  ///< \~chinese 是否可见         \~english Visible
+
+  unsigned int nReserved[4];
+} MV_XML_FEATURE_Category;
+
+typedef struct _MV_XML_FEATURE_EnumEntry_
+{
+  char strName[MV_MAX_XML_NODE_STRLEN_C];
+  char strDisplayName[MV_MAX_XML_NODE_STRLEN_C];
+  char strDescription
+    [MV_MAX_XML_DISC_STRLEN_C];  ///< \~chinese 目前暂不支持     \~english NOT SUPPORT NOW
+  char strToolTip[MV_MAX_XML_DISC_STRLEN_C];
+  int bIsImplemented;
+  int nParentsNum;
+  MV_XML_NODE_FEATURE stParentsList[MV_MAX_XML_PARENTS_NUM];
+
+  enum MV_XML_Visibility enVisivility;  ///< \~chinese 是否可见         \~english Visible
+  int64_t nValue;                       ///< \~chinese 当前值           \~english Current Value
+  enum MV_XML_AccessMode enAccessMode;  ///< \~chinese 访问模式         \~english Access Mode
+  int
+    bIsLocked;  ///< \~chinese 是否锁定。0-否；1-是,目前暂不支持    \~english Locked. 0-NO; 1-YES, NOT SUPPORT NOW
+  int nReserved[8];
+
+} MV_XML_FEATURE_EnumEntry;
+
+typedef struct _MV_XML_FEATURE_Enumeration_
+{
+  enum MV_XML_Visibility enVisivility;  ///< \~chinese 是否可见         \~english Visible
+  char strDescription
+    [MV_MAX_XML_DISC_STRLEN_C];  ///< \~chinese 节点描述 目前暂不支持    \~english Node Description, NOT SUPPORT NOW
+  char strDisplayName
+    [MV_MAX_XML_NODE_STRLEN_C];               ///< \~chinese 显示名称         \~english Display Name
+  char strName[MV_MAX_XML_NODE_STRLEN_C];     ///< \~chinese 节点名           \~english Node Name
+  char strToolTip[MV_MAX_XML_DISC_STRLEN_C];  ///< \~chinese 提示             \~english Notice
+
+  int nSymbolicNum;  ///< \~chinese ymbolic数        \~english Symbolic Number
+  char strCurrentSymbolic
+    [MV_MAX_XML_SYMBOLIC_STRLEN_C];  ///< \~chinese 当前Symbolic索引    \~english Current Symbolic Index
+  char strSymbolic[MV_MAX_XML_SYMBOLIC_NUM][MV_MAX_XML_SYMBOLIC_STRLEN_C];
+  enum MV_XML_AccessMode enAccessMode;  ////< \~chinese 访问模式        \~english Access Mode
+  int
+    bIsLocked;  ///< \~chinese 是否锁定。0-否；1-是,目前暂不支持    \~english Locked. 0-NO; 1-YES, NOT SUPPORT NOW
+  int64_t nValue;  ///< \~chinese 当前值           \~english Current Value
+
+  unsigned int nReserved[4];
+} MV_XML_FEATURE_Enumeration;
+
+typedef struct _MV_XML_FEATURE_Port_
+{
+  enum MV_XML_Visibility enVisivility;  ///< \~chinese 是否可见         \~english Visible
+  char strDescription
+    [MV_MAX_XML_DISC_STRLEN_C];  ///< \~chinese 节点描述,目前暂不支持    \~english Node Description, NOT SUPPORT NOW
+  char strDisplayName
+    [MV_MAX_XML_NODE_STRLEN_C];               ///< \~chinese 显示名称         \~english Display Name
+  char strName[MV_MAX_XML_NODE_STRLEN_C];     ///< \~chinese 节点名           \~english Node Name
+  char strToolTip[MV_MAX_XML_DISC_STRLEN_C];  ///< \~chinese 提示             \~english Notice
+
+  enum MV_XML_AccessMode enAccessMode;  ///< \~chinese 访问模式         \~english Access Mode
+  int
+    bIsLocked;  ///< \~chinese 是否锁定。0-否；1-是,目前暂不支持    \~english Locked. 0-NO; 1-YES, NOT SUPPORT NOW
+
+  unsigned int nReserved[4];
+} MV_XML_FEATURE_Port;
+
+typedef struct _MV_XML_CAMERA_FEATURE_
+{
+  enum MV_XML_InterfaceType enType;
+  union {
+    MV_XML_FEATURE_Integer stIntegerFeature;
+    MV_XML_FEATURE_Float stFloatFeature;
+    MV_XML_FEATURE_Enumeration stEnumerationFeature;
+    MV_XML_FEATURE_String stStringFeature;
+  } SpecialFeature;
+
+} MV_XML_CAMERA_FEATURE;
+
+/// \~chinese 图片保存参数              \~english Save Image Parameters
+typedef struct _MV_SAVE_IMAGE_PARAM_T_EX_
+{
+  unsigned char * pData;  ///< [IN]  \~chinese 输入数据缓存           \~english Input Data Buffer
+  unsigned int nDataLen;  ///< [IN]  \~chinese 输入数据长度           \~english Input Data length
+  enum MvGvspPixelType
+    enPixelType;  ///< [IN]  \~chinese 输入数据的像素格式     \~english Input Data Pixel Format
+  unsigned short nWidth;   ///< [IN]  \~chinese 图像宽                 \~english Image Width
+  unsigned short nHeight;  ///< [IN]  \~chinese 图像高                 \~english Image Height
+
+  unsigned char *
+    pImageBuffer;          ///< [OUT] \~chinese 输出图片缓存           \~english Output Image Buffer
+  unsigned int nImageLen;  ///< [OUT] \~chinese 输出图片长度           \~english Output Image length
+  unsigned int
+    nBufferSize;  ///< [IN]  \~chinese 提供的输出缓冲区大小   \~english Output buffer size provided
+  enum MV_SAVE_IAMGE_TYPE
+    enImageType;  ///< [IN]  \~chinese 输出图片格式           \~english Output Image Format
+  unsigned int
+    nJpgQuality;  ///< [IN]  \~chinese JPG编码质量(50-99]，其它格式无效   \~english Encoding quality(50-99]，Other formats are invalid
+
+  unsigned int
+    iMethodValue;  ///< [IN]  \~chinese 插值方法 0-快速 1-均衡 2-最优（其它值默认为最优）  \~english Bayer interpolation method  0-Fast 1-Equilibrium 2-Optimal
+
+  unsigned int nReserved[3];  ///<       \~chinese 预留                   \~english Reserved
+
+} MV_SAVE_IMAGE_PARAM_EX;
+
+/// \~chinese 图片保存参数              \~english Save Image Parameters
+typedef struct _MV_SAVE_IMG_TO_FILE_PARAM_
+{
+  enum MvGvspPixelType
+    enPixelType;  ///< [IN]  \~chinese输入数据的像素格式      \~english The pixel format of the input data
+  unsigned char * pData;   ///< [IN]  \~chinese 输入数据缓存           \~english Input Data Buffer
+  unsigned int nDataLen;   ///< [IN]  \~chinese 输入数据长度           \~english Input Data length
+  unsigned short nWidth;   ///< [IN]  \~chinese 图像宽                 \~english Image Width
+  unsigned short nHeight;  ///< [IN]  \~chinese 图像高                 \~english Image Height
+  enum MV_SAVE_IAMGE_TYPE
+    enImageType;  ///< [IN]  \~chinese 输入图片格式           \~english Input Image Format
+  unsigned int
+    nQuality;  ///< [IN]  \~chinese JPG编码质量(50-99]，PNG编码质量[0-9]，其它格式无效 \~english JPG Encoding quality(50-99],PNG Encoding quality[0-9]，Other formats are invalid
+  char pImagePath[256];  ///< [IN]  \~chinese 输入文件路径           \~english Input file path
+
+  int
+    iMethodValue;  ///< [IN]  \~chinese 插值方法 0-快速 1-均衡 2-最优（其它值默认为最优）  \~english Bayer interpolation method  0-Fast 1-Equilibrium 2-Optimal
+
+  unsigned int nReserved[8];  ///<       \~chinese 预留                   \~english Reserved
+
+} MV_SAVE_IMG_TO_FILE_PARAM;
+
+// \~chinese 像素转换结构体            \~english Pixel convert structure
+typedef struct _MV_CC_PIXEL_CONVERT_PARAM_
+{
+  unsigned short nWidth;   ///< [IN]  \~chinese 图像宽                 \~english Width
+  unsigned short nHeight;  ///< [IN]  \~chinese 图像高                 \~english Height
+
+  enum MvGvspPixelType
+    enSrcPixelType;  ///< [IN]  \~chinese 源像素格式             \~english Source pixel format
+  unsigned char * pSrcData;  ///< [IN]  \~chinese 输入数据缓存           \~english Input data buffer
+  unsigned int nSrcDataLen;  ///< [IN]  \~chinese 输入数据长度           \~english Input data length
+
+  enum MvGvspPixelType
+    enDstPixelType;  ///< [IN]  \~chinese 目标像素格式           \~english Destination pixel format
+  unsigned char *
+    pDstBuffer;          ///< [OUT] \~chinese 输出数据缓存           \~english Output data buffer
+  unsigned int nDstLen;  ///< [OUT] \~chinese 输出数据长度           \~english Output data length
+  unsigned int
+    nDstBufferSize;  ///< [IN]  \~chinese 提供的输出缓冲区大小   \~english Provided output buffer size
+
+  unsigned int nRes[4];  ///<       \~chinese 预留                   \~english Reserved
+
+} MV_CC_PIXEL_CONVERT_PARAM;
+
+#endif /* _MV_OBSOLETE_CAM_PARAMS_H_ */

src/device/hikrobot/include/PixelType.h

@@ -0,0 +1,248 @@
+
+#ifndef _MV_PIXEL_TYPE_H_
+#define _MV_PIXEL_TYPE_H_
+
+/************************************************************************/
+/*     GigE Vision (2.0.03) PIXEL FORMATS                               */
+/************************************************************************/
+
+// Indicate if pixel is monochrome or RGB
+#define MV_GVSP_PIX_MONO 0x01000000
+#define MV_GVSP_PIX_RGB 0x02000000  // deprecated in version 1.1
+#define MV_GVSP_PIX_COLOR 0x02000000
+#define MV_GVSP_PIX_CUSTOM 0x80000000
+#define MV_GVSP_PIX_COLOR_MASK 0xFF000000
+
+// Indicate effective number of bits occupied by the pixel (including padding).
+// This can be used to compute amount of memory required to store an image.
+#define MV_PIXEL_BIT_COUNT(n) ((n) << 16)
+
+#define MV_GVSP_PIX_EFFECTIVE_PIXEL_SIZE_MASK 0x00FF0000
+#define MV_GVSP_PIX_EFFECTIVE_PIXEL_SIZE_SHIFT 16
+
+// Pixel ID: lower 16-bit of the pixel formats
+#define MV_GVSP_PIX_ID_MASK 0x0000FFFF
+#define MV_GVSP_PIX_COUNT 0x46  // next Pixel ID available
+
+enum MvGvspPixelType
+{
+// Undefined pixel type
+#ifdef WIN32
+  PixelType_Gvsp_Undefined = 0xFFFFFFFF,
+
+#else
+  PixelType_Gvsp_Undefined = -1,
+
+#endif
+  // Mono buffer format defines
+  PixelType_Gvsp_Mono1p = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(1) | 0x0037),
+  PixelType_Gvsp_Mono2p = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(2) | 0x0038),
+  PixelType_Gvsp_Mono4p = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(4) | 0x0039),
+  PixelType_Gvsp_Mono8 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x0001),
+  PixelType_Gvsp_Mono8_Signed = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x0002),
+  PixelType_Gvsp_Mono10 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0003),
+  PixelType_Gvsp_Mono10_Packed = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0004),
+  PixelType_Gvsp_Mono12 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0005),
+  PixelType_Gvsp_Mono12_Packed = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0006),
+  PixelType_Gvsp_Mono14 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0025),
+  PixelType_Gvsp_Mono16 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0007),
+
+  // Bayer buffer format defines
+  PixelType_Gvsp_BayerGR8 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x0008),
+  PixelType_Gvsp_BayerRG8 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x0009),
+  PixelType_Gvsp_BayerGB8 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x000A),
+  PixelType_Gvsp_BayerBG8 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x000B),
+  PixelType_Gvsp_BayerRBGG8 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x0046),
+  PixelType_Gvsp_BayerGR10 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x000C),
+  PixelType_Gvsp_BayerRG10 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x000D),
+  PixelType_Gvsp_BayerGB10 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x000E),
+  PixelType_Gvsp_BayerBG10 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x000F),
+  PixelType_Gvsp_BayerGR12 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0010),
+  PixelType_Gvsp_BayerRG12 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0011),
+  PixelType_Gvsp_BayerGB12 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0012),
+  PixelType_Gvsp_BayerBG12 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0013),
+  PixelType_Gvsp_BayerGR10_Packed = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0026),
+  PixelType_Gvsp_BayerRG10_Packed = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0027),
+  PixelType_Gvsp_BayerGB10_Packed = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0028),
+  PixelType_Gvsp_BayerBG10_Packed = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0029),
+  PixelType_Gvsp_BayerGR12_Packed = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x002A),
+  PixelType_Gvsp_BayerRG12_Packed = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x002B),
+  PixelType_Gvsp_BayerGB12_Packed = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x002C),
+  PixelType_Gvsp_BayerBG12_Packed = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x002D),
+  PixelType_Gvsp_BayerGR16 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x002E),
+  PixelType_Gvsp_BayerRG16 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x002F),
+  PixelType_Gvsp_BayerGB16 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0030),
+  PixelType_Gvsp_BayerBG16 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0031),
+
+  // RGB Packed buffer format defines
+  PixelType_Gvsp_RGB8_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(24) | 0x0014),
+  PixelType_Gvsp_BGR8_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(24) | 0x0015),
+  PixelType_Gvsp_RGBA8_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(32) | 0x0016),
+  PixelType_Gvsp_BGRA8_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(32) | 0x0017),
+  PixelType_Gvsp_RGB10_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x0018),
+  PixelType_Gvsp_BGR10_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x0019),
+  PixelType_Gvsp_RGB12_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x001A),
+  PixelType_Gvsp_BGR12_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x001B),
+  PixelType_Gvsp_RGB16_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x0033),
+  PixelType_Gvsp_BGR16_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x004B),
+  PixelType_Gvsp_RGBA16_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(64) | 0x0064),
+  PixelType_Gvsp_BGRA16_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(64) | 0x0051),
+  PixelType_Gvsp_RGB10V1_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(32) | 0x001C),
+  PixelType_Gvsp_RGB10V2_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(32) | 0x001D),
+  PixelType_Gvsp_RGB12V1_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(36) | 0X0034),
+  PixelType_Gvsp_RGB565_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x0035),
+  PixelType_Gvsp_BGR565_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0X0036),
+
+  // YUV Packed buffer format defines
+  PixelType_Gvsp_YUV411_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(12) | 0x001E),
+  PixelType_Gvsp_YUV422_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x001F),
+  PixelType_Gvsp_YUV422_YUYV_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x0032),
+  PixelType_Gvsp_YUV444_Packed = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(24) | 0x0020),
+  PixelType_Gvsp_YCBCR8_CBYCR = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(24) | 0x003A),
+  PixelType_Gvsp_YCBCR422_8 = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x003B),
+  PixelType_Gvsp_YCBCR422_8_CBYCRY = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x0043),
+  PixelType_Gvsp_YCBCR411_8_CBYYCRYY = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(12) | 0x003C),
+  PixelType_Gvsp_YCBCR601_8_CBYCR = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(24) | 0x003D),
+  PixelType_Gvsp_YCBCR601_422_8 = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x003E),
+  PixelType_Gvsp_YCBCR601_422_8_CBYCRY = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x0044),
+  PixelType_Gvsp_YCBCR601_411_8_CBYYCRYY = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(12) | 0x003F),
+  PixelType_Gvsp_YCBCR709_8_CBYCR = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(24) | 0x0040),
+  PixelType_Gvsp_YCBCR709_422_8 = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x0041),
+  PixelType_Gvsp_YCBCR709_422_8_CBYCRY = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x0045),
+  PixelType_Gvsp_YCBCR709_411_8_CBYYCRYY = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(12) | 0x0042),
+
+  // YUV420
+  PixelType_Gvsp_YUV420SP_NV12 = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(12) | 0x8001),
+  PixelType_Gvsp_YUV420SP_NV21 = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(12) | 0x8002),
+
+  // RGB Planar buffer format defines
+  PixelType_Gvsp_RGB8_Planar = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(24) | 0x0021),
+  PixelType_Gvsp_RGB10_Planar = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x0022),
+  PixelType_Gvsp_RGB12_Planar = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x0023),
+  PixelType_Gvsp_RGB16_Planar = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x0024),
+
+  // 自定义的图片格式
+  PixelType_Gvsp_Jpeg = (MV_GVSP_PIX_CUSTOM | MV_PIXEL_BIT_COUNT(24) | 0x0001),
+
+  PixelType_Gvsp_Coord3D_ABC32f =
+    (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(96) | 0x00C0),  //0x026000C0
+  PixelType_Gvsp_Coord3D_ABC32f_Planar =
+    (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(96) | 0x00C1),  //0x026000C1
+
+  // 该值被废弃，请参考PixelType_Gvsp_Coord3D_AC32f_64; the value is discarded
+  PixelType_Gvsp_Coord3D_AC32f = (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(40) | 0x00C2),
+  // 该值被废弃; the value is discarded    (已放入Chunkdata)
+  PixelType_Gvsp_COORD3D_DEPTH_PLUS_MASK =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(28) | 0x0001),
+
+  PixelType_Gvsp_Coord3D_ABC32 =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(96) | 0x3001),  //0x82603001
+  PixelType_Gvsp_Coord3D_AB32f =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(64) | 0x3002),  //0x82403002
+  PixelType_Gvsp_Coord3D_AB32 =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(64) | 0x3003),  //0x82403003
+  PixelType_Gvsp_Coord3D_AC32f_64 =
+    (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(64) | 0x00C2),  //0x024000C2
+  PixelType_Gvsp_Coord3D_AC32f_Planar =
+    (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(64) | 0x00C3),  //0x024000C3
+  PixelType_Gvsp_Coord3D_AC32 =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(64) | 0x3004),        //0x82403004
+  PixelType_Gvsp_Coord3D_A32f = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(32) | 0x00BD),  //0x012000BD
+  PixelType_Gvsp_Coord3D_A32 =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(32) | 0x3005),         //0x81203005
+  PixelType_Gvsp_Coord3D_C32f = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(32) | 0x00BF),  //0x012000BF
+  PixelType_Gvsp_Coord3D_C32 =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(32) | 0x3006),  //0x81203006
+  PixelType_Gvsp_Coord3D_ABC16 =
+    (MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x00B9),                            //0x023000B9
+  PixelType_Gvsp_Coord3D_C16 = (MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x00B8),  //0x011000B8
+
+  PixelType_Gvsp_Float32 =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(32) | 0x0001),  //0x81200001
+
+  //无损压缩像素格式定义
+  PixelType_Gvsp_HB_Mono8 =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x0001),
+  PixelType_Gvsp_HB_Mono10 =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0003),
+  PixelType_Gvsp_HB_Mono10_Packed =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0004),
+  PixelType_Gvsp_HB_Mono12 =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0005),
+  PixelType_Gvsp_HB_Mono12_Packed =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0006),
+  PixelType_Gvsp_HB_Mono16 =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0007),
+  PixelType_Gvsp_HB_BayerGR8 =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x0008),
+  PixelType_Gvsp_HB_BayerRG8 =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x0009),
+  PixelType_Gvsp_HB_BayerGB8 =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x000A),
+  PixelType_Gvsp_HB_BayerBG8 =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x000B),
+  PixelType_Gvsp_HB_BayerRBGG8 =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(8) | 0x0046),
+  PixelType_Gvsp_HB_BayerGR10 =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x000C),
+  PixelType_Gvsp_HB_BayerRG10 =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x000D),
+  PixelType_Gvsp_HB_BayerGB10 =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x000E),
+  PixelType_Gvsp_HB_BayerBG10 =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x000F),
+  PixelType_Gvsp_HB_BayerGR12 =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0010),
+  PixelType_Gvsp_HB_BayerRG12 =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0011),
+  PixelType_Gvsp_HB_BayerGB12 =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0012),
+  PixelType_Gvsp_HB_BayerBG12 =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(16) | 0x0013),
+  PixelType_Gvsp_HB_BayerGR10_Packed =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0026),
+  PixelType_Gvsp_HB_BayerRG10_Packed =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0027),
+  PixelType_Gvsp_HB_BayerGB10_Packed =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0028),
+  PixelType_Gvsp_HB_BayerBG10_Packed =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x0029),
+  PixelType_Gvsp_HB_BayerGR12_Packed =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x002A),
+  PixelType_Gvsp_HB_BayerRG12_Packed =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x002B),
+  PixelType_Gvsp_HB_BayerGB12_Packed =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x002C),
+  PixelType_Gvsp_HB_BayerBG12_Packed =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_MONO | MV_PIXEL_BIT_COUNT(12) | 0x002D),
+  PixelType_Gvsp_HB_YUV422_Packed =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x001F),
+  PixelType_Gvsp_HB_YUV422_YUYV_Packed =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(16) | 0x0032),
+  PixelType_Gvsp_HB_RGB8_Packed =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(24) | 0x0014),
+  PixelType_Gvsp_HB_BGR8_Packed =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(24) | 0x0015),
+  PixelType_Gvsp_HB_RGBA8_Packed =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(32) | 0x0016),
+  PixelType_Gvsp_HB_BGRA8_Packed =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(32) | 0x0017),
+  PixelType_Gvsp_HB_RGB16_Packed =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x0033),
+  PixelType_Gvsp_HB_BGR16_Packed =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(48) | 0x004B),
+  PixelType_Gvsp_HB_RGBA16_Packed =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(64) | 0x0064),
+  PixelType_Gvsp_HB_BGRA16_Packed =
+    (MV_GVSP_PIX_CUSTOM | MV_GVSP_PIX_COLOR | MV_PIXEL_BIT_COUNT(64) | 0x0051),
+
+};
+
+#ifdef WIN32
+typedef __int64 int64_t;
+typedef unsigned __int64 uint64_t;
+#else
+#include <stdint.h>
+#endif
+
+#endif /* _MV_PIXEL_TYPE_H_ */

src/device/mindvision/include/CameraDefine.h

@@ -0,0 +1,842 @@
+#pragma once
+#ifndef _CAMERA_DEFINE_H_
+#define _CAMERA_DEFINE_H_
+
+#include "CameraStatus.h"
+
+#define MAX_CROSS_LINE 9
+
+//相机的句柄类型定义
+typedef int CameraHandle;
+typedef int INT;
+typedef int LONG;
+typedef unsigned int UINT;
+typedef unsigned long long UINT64;
+typedef int BOOL;
+typedef unsigned char BYTE;
+typedef unsigned int DWORD;
+typedef void* PVOID;
+typedef void* HWND;
+typedef char* LPCTSTR;
+typedef unsigned short USHORT;
+typedef short SHORT;
+ typedef unsigned char* LPBYTE;
+typedef char CHAR;
+typedef char TCHAR;
+typedef  unsigned short WORD;
+typedef INT HANDLE;
+typedef void VOID;
+typedef unsigned int ULONG;
+typedef void* LPVOID;
+typedef unsigned char UCHAR;
+typedef void* HMODULE;
+
+#define TRUE 1
+#define FALSE 0
+//图像查表变换的方式
+typedef enum
+{
+    LUTMODE_PARAM_GEN=0,//通过调节参数动态生成LUT表
+    LUTMODE_PRESET,     //使用预设的LUT表
+    LUTMODE_USER_DEF    //使用用户自定义的LUT表
+}emSdkLutMode;
+
+//相机的视频流控制
+typedef enum
+{
+    RUNMODE_PLAY=0,    //正常预览，捕获到图像就显示。（如果相机处于触发模式，则会等待触发帧的到来）
+    RUNMODE_PAUSE,     //暂停，会暂停相机的图像输出，同时也不会去捕获图像
+    RUNMODE_STOP       //停止相机工作。反初始化后，相机就处于停止模式
+}emSdkRunMode;
+
+//SDK内部显示接口的显示方式
+typedef enum
+{
+    DISPLAYMODE_SCALE=0, //缩放显示模式，缩放到显示控件的尺寸
+    DISPLAYMODE_REAL     //1:1显示模式，当图像尺寸大于显示控件的尺寸时，只显示局部
+}emSdkDisplayMode;
+
+//录像状态
+typedef enum
+{
+  RECORD_STOP = 0,  //停止
+  RECORD_START,     //录像中
+  RECORD_PAUSE      //暂停
+}emSdkRecordMode;
+
+//图像的镜像操作
+typedef enum
+{
+    MIRROR_DIRECTION_HORIZONTAL = 0,//水平镜像
+    MIRROR_DIRECTION_VERTICAL       //垂直镜像
+}emSdkMirrorDirection;
+
+/// @ingroup MV_ENUM_TYPE
+/// \~chinese 图像的旋转操作
+/// \~english Rotation of the image
+typedef enum
+{
+	ROTATE_DIRECTION_0=0,		///< \~chinese 不旋转		\~english Do not rotate
+	ROTATE_DIRECTION_90=1,		///< \~chinese 逆时针90度	\~english Counterclockwise 90 degrees
+	ROTATE_DIRECTION_180=2,		///< \~chinese 逆时针180度	\~english Counterclockwise 180 degrees
+	ROTATE_DIRECTION_270=3,		///< \~chinese 逆时针270度	\~english Counterclockwise 270 degrees
+}emSdkRotateDirection;
+
+//相机视频的帧率
+typedef enum
+{
+    FRAME_SPEED_LOW = 0,  //低速模式
+    FRAME_SPEED_NORMAL,   //普通模式
+    FRAME_SPEED_HIGH,     //高速模式(需要较高的传输带宽,多设备共享传输带宽时会对帧率的稳定性有影响)
+    FRAME_SPEED_SUPER     //超高速模式(需要较高的传输带宽,多设备共享传输带宽时会对帧率的稳定性有影响)
+}emSdkFrameSpeed;
+
+//保存文件的格式类型
+typedef enum
+{
+    FILE_JPG = 1,//JPG
+    FILE_BMP = 2,//BMP
+    FILE_RAW = 4,//相机输出的bayer格式文件,对于不支持bayer格式输出相机，无法保存为该格式
+    FILE_PNG = 8, //PNG
+    FILE_BMP_8BIT = 16,	  ///< \~chinese BMP 8bit		\~english BMP 8bit
+    FILE_PNG_8BIT = 32,   ///< \~chinese PNG 8bit		\~english PNG 8bit
+	  FILE_RAW_16BIT = 64,	///< \~chinese RAW 16bit	\~english RAW 16bit
+}emSdkFileType;
+
+/// @ingroup MV_ENUM_TYPE
+/// \~chinese 相机中的图像传感器的工作模式
+/// \~english Image Sensor Operation Mode in Camera
+typedef enum 
+{
+    /// \~chinese 连续采集模式
+    /// \~english Continuous acquisition mode
+    CONTINUATION=0,
+
+    /// \~chinese 软件触发模式，由软件发送指令后，传感器开始采集指定帧数的图像，采集完成后，停止输出
+    /// \~english Software trigger mode. After the software sends the instruction, the sensor starts to capture the image of the specified frame number. After the acquisition is completed, the output is stopped.
+    SOFT_TRIGGER=1,
+
+    /// \~chinese 硬件触发模式，当接收到外部信号，传感器开始采集指定帧数的图像，采集完成后，停止输出
+    /// \~english In the hardware trigger mode, when receiving an external signal, the sensor starts to capture the image of the specified frame number. After the acquisition is completed, the output is stopped.
+    EXTERNAL_TRIGGER=2,
+
+    /// \~chinese 编码器触发模式（仅用于线阵相机）
+    /// \~english Encoder trigger mode (only for line scan cameras)
+    ROTARYENC_TRIGGER=3,
+
+    /// \~chinese 编码器条件触发模式（仅用于线阵相机）
+    /// \~english Encoder condition trigger mode (only for line scan cameras)
+    ROTARYENC_COND_TRIGGER=4,
+} emSdkSnapMode;
+
+//自动曝光时抗频闪的频闪
+typedef enum
+{
+    LIGHT_FREQUENCY_50HZ = 0,//50HZ,一般的灯光都是50HZ
+    LIGHT_FREQUENCY_60HZ     //60HZ,主要是指显示器的
+}emSdkLightFrequency;
+
+//相机的配置参数，分为A,B,C,D 4组进行保存。
+typedef enum
+{
+    PARAMETER_TEAM_DEFAULT = 0xff,
+    PARAMETER_TEAM_A = 0,
+    PARAMETER_TEAM_B = 1,
+    PARAMETER_TEAM_C = 2,
+    PARAMETER_TEAM_D = 3
+}emSdkParameterTeam;
+
+
+/*emSdkParameterMode 相机参数加载模式，参数加载分为从文件和从设备加载两种方式
+
+PARAM_MODE_BY_MODEL:所有同型号的相机共用ABCD四组参数文件。修改
+             一台相机的参数文件，会影响到整个同型号的
+             相机参数加载。
+
+PARAM_MODE_BY_NAME:所有设备名相同的相机，共用ABCD四组参数文件。
+         默认情况下，当电脑上只接了某型号一台相机时，
+         设备名都是一样的，而您希望某一台相机能够加载
+         不同的参数文件，则可以通过修改其设备名的方式
+         来让其加载指定的参数文件。
+
+PARAM_MODE_BY_SN:相机按照自己的唯一序列号来加载ABCD四组参数文件，
+         序列号在出厂时已经固化在相机内，每台相机的序列号
+         都不相同，通过这种方式，每台相机的参数文件都是独立的。
+
+您可以根据自己的使用环境，灵活使用以上几种方式加载参数。例如，以
+MV-U300为例，您希望多台该型号的相机在您的 电脑上都共用4组参数，那么就
+使用PARAM_MODE_BY_MODEL方式;如果您希望其中某一台或者某几台MV-U300能
+使用自己参数文件而其余的MV-U300又要使用相同的参数文件，那么使用
+PARAM_MODE_BY_NAME方式;如果您希望每台MV-U300都使用不同的参数文件，那么
+使用PARAM_MODE_BY_SN方式。
+参数文件存在安装目录的 \Camera\Configs 目录下，以config为后缀名的文件。
+*/
+typedef enum
+{
+  PARAM_MODE_BY_MODEL = 0,  //根据相机型号名从文件中加载参数，例如MV-U300
+  PARAM_MODE_BY_NAME,       //根据设备昵称(tSdkCameraDevInfo.acFriendlyName)从文件中加载参数，例如MV-U300,该昵称可自定义
+  PARAM_MODE_BY_SN,         //根据设备的唯一序列号从文件中加载参数，序列号在出厂时已经写入设备，每台相机拥有不同的序列号。
+  PARAM_MODE_IN_DEVICE      //从设备的固态存储器中加载参数。不是所有的型号都支持从相机中读写参数组，由tSdkCameraCapbility.bParamInDevice决定
+}emSdkParameterMode;
+
+
+//SDK生成的相机配置页面掩码值
+typedef enum
+{
+  PROP_SHEET_INDEX_EXPOSURE=0,			///< \~chinese 曝光设置 \~english Exposure Settings
+  PROP_SHEET_INDEX_ISP_COLOR=1,			///< \~chinese 颜色矩阵设置 \~english Color Matrix Settings
+  PROP_SHEET_INDEX_ISP_LUT=2,			///< \~chinese LUT设置 \~english LUT setting
+  PROP_SHEET_INDEX_ISP_SHAPE=3,			///< \~chinese 变换设置 \~english transform settings
+  PROP_SHEET_INDEX_VIDEO_FORMAT=4,		///< \~chinese 格式设置 \~english Formatting
+  PROP_SHEET_INDEX_RESOLUTION=5,		///< \~chinese 分辨率设置 \~english resolution setting
+  PROP_SHEET_INDEX_IO_CTRL=6,			///< \~chinese IO控制 \~english IO control
+  PROP_SHEET_INDEX_TRIGGER_SET=7,		///< \~chinese 触发模式 \~english trigger setting
+  PROP_SHEET_INDEX_OVERLAY=8,			///< \~chinese 十字线 \~english Crosshair
+  PROP_SHEET_INDEX_DEVICE_INFO=9,		///< \~chinese 设备信息 \~english Device Information
+  PROP_SHEET_INDEX_WDR=10,				///< \~chinese 宽动态 \~english Wide Dynamic
+  PROP_SHEET_INDEX_MULTI_EXPOSURE=11,	///< \~chinese 多重曝光 \~english Multi exposure
+  PROP_SHEET_INDEX_SPECIAL=12,			///< \~chinese 特殊设置 \~english Special settings
+  PROP_SHEET_INDEX_GIGE=13,				///< \~chinese GIGE设置 \~english GIGE settings
+  PROP_SHEET_INDEX_GF_SETTING_I = 14,   ///< \~chinese GF系列红外相机设置I  \~english GF Settings I
+  PROP_SHEET_INDEX_GF_SETTING_II = 15,  ///< \~chinese GF系列红外相机设置II \~english GF Settings II
+  PROP_SHEET_INDEX_NEW_ISP_COLOR = 16,  ///< \~chinese 白平衡设置 \~english WB Settings
+}emSdkPropSheetMask;
+
+//SDK生成的相机配置页面的回调消息类型
+typedef enum
+{
+  SHEET_MSG_LOAD_PARAM_DEFAULT = 0, //参数被恢复成默认后，触发该消息
+  SHEET_MSG_LOAD_PARAM_GROUP,       //加载指定参数组，触发该消息
+  SHEET_MSG_LOAD_PARAM_FROMFILE,    //从指定文件加载参数后，触发该消息
+  SHEET_MSG_SAVE_PARAM_GROUP        //当前参数组被保存时，触发该消息
+}emSdkPropSheetMsg;
+
+//可视化选择参考窗口的类型
+typedef enum
+{
+  REF_WIN_AUTO_EXPOSURE = 0,
+  REF_WIN_WHITE_BALANCE,
+}emSdkRefWinType;
+
+//可视化选择参考窗口的类型
+typedef enum
+{
+  RES_MODE_PREVIEW = 0,
+  RES_MODE_SNAPSHOT,
+}emSdkResolutionMode;
+
+//白平衡时色温模式
+typedef enum
+{
+  CT_MODE_AUTO = 0, //自动识别色温
+  CT_MODE_PRESET,   //使用指定的预设色温
+  CT_MODE_USER_DEF  //自定义色温(增益和矩阵)
+}emSdkClrTmpMode;
+
+//LUT的颜色通道
+typedef enum
+{
+  LUT_CHANNEL_ALL = 0,//R,B,G三通道同时调节
+  LUT_CHANNEL_RED,    //红色通道
+  LUT_CHANNEL_GREEN,  //绿色通道
+  LUT_CHANNEL_BLUE,   //蓝色通道
+}emSdkLutChannel;
+
+//ISP处理单元
+typedef enum
+{
+  ISP_PROCESSSOR_PC = 0,//使用PC的软件ISP模块
+  ISP_PROCESSSOR_DEVICE //使用相机自带的硬件ISP模块
+}emSdkIspProcessor;
+
+//闪光灯信号控制方式
+typedef enum
+{
+  STROBE_SYNC_WITH_TRIG_AUTO = 0,    //和触发信号同步，触发后，相机进行曝光时，自动生成STROBE信号。此时，有效极性可设置(CameraSetStrobePolarity)。
+  STROBE_SYNC_WITH_TRIG_MANUAL,      //和触发信号同步，触发后，STROBE延时指定的时间后(CameraSetStrobeDelayTime)，再持续指定时间的脉冲(CameraSetStrobePulseWidth)，有效极性可设置(CameraSetStrobePolarity)。
+  STROBE_ALWAYS_HIGH,                //始终为高，忽略STROBE信号的其他设置
+  STROBE_ALWAYS_LOW                  //始终为低，忽略STROBE信号的其他设置
+}emStrobeControl;
+
+//硬件外触发的信号种类
+typedef enum
+{
+  EXT_TRIG_LEADING_EDGE = 0,     //上升沿触发，默认为该方式
+  EXT_TRIG_TRAILING_EDGE,        //下降沿触发
+  EXT_TRIG_HIGH_LEVEL,           //高电平触发,电平宽度决定曝光时间，仅部分型号的相机支持电平触发方式。
+  EXT_TRIG_LOW_LEVEL             //低电平触发,
+}emExtTrigSignal;
+
+//硬件外触发时的快门方式
+typedef enum
+{
+  EXT_TRIG_EXP_STANDARD = 0,     //标准方式，默认为该方式。
+  EXT_TRIG_EXP_GRR,              //全局复位方式，部分滚动快门的CMOS型号的相机支持该方式，配合外部机械快门，可以达到全局快门的效果，适合拍高速运动的物体
+}emExtTrigShutterMode;
+
+/// @ingroup MV_ENUM_TYPE
+/// \~chinese 清晰度评估算法
+/// \~english Sharpness assessment algorithm
+typedef enum
+{
+	EVALUATE_DEFINITION_DEVIATION=0,	///< \~chinese 方差法 \~english	Variance method
+	EVALUATE_DEFINITION_SMD=1,			///< \~chinese 相邻像素灰度方差法 \~english Adjacent Pixel Gray Difference Method
+	EVALUATE_DEFINITION_GRADIENT=2,		///< \~chinese 梯度统计 \~english Gradient statistics
+	EVALUATE_DEFINITION_SOBEL=3,		///< \~chinese Sobel \~english Sobel
+	EVALUATE_DEFINITION_ROBERT=4,		///< \~chinese Robert \~english Robert
+	EVALUATE_DEFINITION_LAPLACE=5,		///< \~chinese Laplace \~english Laplace
+
+	EVALUATE_DEFINITION_ALG_MAX=6,		///< \~chinese 算法个数 \~english The number of algorithms
+}emEvaluateDefinitionAlgorith;
+
+// GPIO模式
+typedef enum
+{
+  IOMODE_TRIG_INPUT=0,		    ///< \~chinese 触发输入 \~english Trigger input
+  IOMODE_STROBE_OUTPUT=1,		  ///< \~chinese 闪光灯输出 \~english Strobe output
+  IOMODE_GP_INPUT=2,			    ///< \~chinese 通用型输入 \~english Universal input
+  IOMODE_GP_OUTPUT=3,			    ///< \~chinese 通用型输出 \~english Universal output
+  IOMODE_PWM_OUTPUT=4,		    ///< \~chinese PWM型输出 \~english PWM output
+  IOMODE_ROTARYENC_INPUT=5,   ///< \~chinese 编码器输入 \~english rotary input
+}emCameraGPIOMode;
+
+/// @ingroup MV_ENUM_TYPE
+/// \~chinese GPIO 格式
+/// \~english GPIO Format
+typedef enum 
+{
+	IOFORMAT_SINGLE=0,			///< \~chinese 单端  \~english single ended
+	IOFORMAT_RS422=1,			  ///< \~chinese 差分RS422 \~english Differential RS422
+	IOFORMAT_RS422_TERM=2,	///< \~chinese 差分RS422带终端电阻 \~english Differential RS422 and Termination Enable
+  IOFORMAT_OCEP=3,			  ///< \~chinese 光耦  \~english opticalcoupler equipment
+}emCameraGPIOFormat;
+
+/// @ingroup MV_ENUM_TYPE
+/// \~chinese 取图优先级
+/// \~english Get Image priority
+typedef enum
+{
+	CAMERA_GET_IMAGE_PRIORITY_OLDEST=0,		///< \~chinese 获取缓存中最旧的一帧 \~english	Get the oldest frame in the cache
+	CAMERA_GET_IMAGE_PRIORITY_NEWEST=1,		///< \~chinese 获取缓存中最新的一帧（比此帧旧的将全部丢弃） \~english Get the latest frame in the cache (older than this frame will be discarded)
+
+	/// \~chinese 丢弃缓存中的所有帧，并且如果此刻相机正在曝光或传输将会被立即打断，等待接收下一帧
+	/// \note 某些型号的相机不支持此功能，对于不支持此功能的相机这个标志相当于@link #CAMERA_GET_IMAGE_PRIORITY_OLDEST @endlink
+	/// \~english All frames in the cache are discarded, and if the camera is now being exposed or transmitted it will be immediately interrupted, waiting to receive the next frame
+	/// \note Some models do not support this feature. For cameras that do not support this feature this flag is equivalent to @link #CAMERA_GET_IMAGE_PRIORITY_OLDEST @endlink
+	CAMERA_GET_IMAGE_PRIORITY_NEXT=2,
+}emCameraGetImagePriority;
+
+/// @ingroup MV_ENUM_TYPE
+/// \~chinese 软触发功能标志
+/// \~english Soft trigger function flag
+typedef enum
+{
+	CAMERA_ST_CLEAR_BUFFER_BEFORE	= 0x1,	///< \~chinese 在软触发之前先清空相机已缓存的帧 \~english Empty camera-cached frames before soft triggering
+}emCameraSoftTriggerExFlags;
+
+//相机的设备信息
+typedef struct
+{
+    char acProductSeries[32];   // 产品系列
+    char acProductName[32];     // 产品名称
+    char acFriendlyName[32];    // 产品昵称，用户可自定义改昵称，保存在相机内，用于区分多个相机同时使用,可以用CameraSetFriendlyName接口改变该昵称，设备重启后生效。
+    char acLinkName[32];        // 内核符号连接名，内部使用
+    char acDriverVersion[32];   // 驱动版本
+    char acSensorType[32];      // sensor类型
+    char acPortType[32];        // 接口类型
+    char acSn[32];              // 产品唯一序列号
+    UINT uInstance;             // 该型号相机在该电脑上的实例索引号，用于区分同型号多相机
+} tSdkCameraDevInfo;
+
+#define EXT_TRIG_MASK_GRR_SHUTTER  1	///< \~chinese 快门支持GRR模式 \~english Shutter supports GRR mode
+#define EXT_TRIG_MASK_LEVEL_MODE   2	///< \~chinese 支持电平触发 \~english Support level trigger
+#define EXT_TRIG_MASK_DOUBLE_EDGE  4	///< \~chinese 支持双边沿触发 \~english Supports bilateral triggering
+#define EXT_TRIG_MASK_BUFFERED_DELAY 8	///< \~chinese 支持信号后延 \~english Supports signal delayed activation
+
+//tSdkResolutionRange结构体中SKIP、 BIN、RESAMPLE模式的掩码值
+#define MASK_2X2_HD     (1<<0)    //硬件SKIP、BIN、重采样 2X2
+#define MASK_3X3_HD     (1<<1)
+#define MASK_4X4_HD     (1<<2)
+#define MASK_5X5_HD     (1<<3)
+#define MASK_6X6_HD     (1<<4)
+#define MASK_7X7_HD     (1<<5)
+#define MASK_8X8_HD     (1<<6)
+#define MASK_9X9_HD     (1<<7)
+#define MASK_10X10_HD   (1<<8)
+#define MASK_11X11_HD   (1<<9)
+#define MASK_12X12_HD   (1<<10)
+#define MASK_13X13_HD   (1<<11)
+#define MASK_14X14_HD   (1<<12)
+#define MASK_15X15_HD   (1<<13)
+#define MASK_16X16_HD   (1<<14)
+#define MASK_17X17_HD   (1<<15)
+#define MASK_2X2_SW     (1<<16)   //硬件SKIP、BIN、重采样 2X2
+#define MASK_3X3_SW     (1<<17)
+#define MASK_4X4_SW     (1<<18)
+#define MASK_5X5_SW     (1<<19)
+#define MASK_6X6_SW     (1<<20)
+#define MASK_7X7_SW     (1<<21)
+#define MASK_8X8_SW     (1<<22)
+#define MASK_9X9_SW     (1<<23)
+#define MASK_10X10_SW   (1<<24)
+#define MASK_11X11_SW   (1<<25)
+#define MASK_12X12_SW   (1<<26)
+#define MASK_13X13_SW   (1<<27)
+#define MASK_14X14_SW   (1<<28)
+#define MASK_15X15_SW   (1<<29)
+#define MASK_16X16_SW   (1<<30)
+#define MASK_17X17_SW   (1<<31)
+
+//相机的分辨率设定范围，用于构件UI
+typedef struct
+{
+  INT iHeightMax;             //图像最大高度
+  INT iHeightMin;             //图像最小高度
+  INT iWidthMax;              //图像最大宽度
+  INT iWidthMin;              //图像最小宽度
+  UINT uSkipModeMask;         //SKIP模式掩码，为0，表示不支持SKIP 。bit0为1,表示支持SKIP 2x2 ;bit1为1，表示支持SKIP 3x3....
+  UINT uBinSumModeMask;       //BIN(求和)模式掩码，为0，表示不支持BIN 。bit0为1,表示支持BIN 2x2 ;bit1为1，表示支持BIN 3x3....
+  UINT uBinAverageModeMask;   //BIN(求均值)模式掩码，为0，表示不支持BIN 。bit0为1,表示支持BIN 2x2 ;bit1为1，表示支持BIN 3x3....
+  UINT uResampleMask;         //硬件重采样的掩码
+} tSdkResolutionRange;
+
+
+//相机的分辨率描述
+typedef struct
+{
+  INT     iIndex;             // 索引号，[0,N]表示预设的分辨率(N 为预设分辨率的最大个数，一般不超过20),OXFF 表示自定义分辨率(ROI)
+  char    acDescription[32];  // 该分辨率的描述信息。仅预设分辨率时该信息有效。自定义分辨率可忽略该信息
+  UINT    uBinSumMode;        // BIN(求和)的模式,范围不能超过tSdkResolutionRange中uBinSumModeMask
+  UINT    uBinAverageMode;    // BIN(求均值)的模式,范围不能超过tSdkResolutionRange中uBinAverageModeMask
+  UINT    uSkipMode;          // 是否SKIP的尺寸，为0表示禁止SKIP模式，范围不能超过tSdkResolutionRange中uSkipModeMask
+  UINT    uResampleMask;      // 硬件重采样的掩码
+  INT     iHOffsetFOV;        // 采集视场相对于Sensor最大视场左上角的垂直偏移
+  INT     iVOffsetFOV;        // 采集视场相对于Sensor最大视场左上角的水平偏移
+  INT     iWidthFOV;          // 采集视场的宽度
+  INT     iHeightFOV;         // 采集视场的高度
+  INT     iWidth;             // 相机最终输出的图像的宽度
+  INT     iHeight;            // 相机最终输出的图像的高度
+  INT     iWidthZoomHd;       // 硬件缩放的宽度,不需要进行此操作的分辨率，此变量设置为0.
+  INT     iHeightZoomHd;      // 硬件缩放的高度,不需要进行此操作的分辨率，此变量设置为0.
+  INT     iWidthZoomSw;       // 软件缩放的宽度,不需要进行此操作的分辨率，此变量设置为0.
+  INT     iHeightZoomSw;      // 软件缩放的高度,不需要进行此操作的分辨率，此变量设置为0.
+} tSdkImageResolution;
+
+//相机白平衡色温模式描述信息
+typedef struct
+{
+    INT  iIndex;            // 模式索引号
+    char acDescription[32]; // 描述信息
+} tSdkColorTemperatureDes;
+
+//相机帧率描述信息
+typedef struct
+{
+    INT  iIndex;             // 帧率索引号，一般0对应于低速模式，1对应于普通模式，2对应于高速模式
+    char acDescription[32];  // 描述信息
+} tSdkFrameSpeed;
+
+//相机曝光功能范围定义
+typedef struct
+{
+    UINT  uiTargetMin;      //自动曝光亮度目标最小值
+    UINT  uiTargetMax;      //自动曝光亮度目标最大值
+    UINT  uiAnalogGainMin;  //模拟增益的最小值，单位为fAnalogGainStep中定义
+    UINT  uiAnalogGainMax;  //模拟增益的最大值，单位为fAnalogGainStep中定义
+    float fAnalogGainStep;  //模拟增益每增加1，对应的增加的放大倍数。例如，uiAnalogGainMin一般为16，fAnalogGainStep一般为0.125，那么最小放大倍数就是16*0.125 = 2倍
+    UINT  uiExposeTimeMin;  //手动模式下，曝光时间的最小值，单位:行。根据CameraGetExposureLineTime可以获得一行对应的时间(微秒),从而得到整帧的曝光时间
+    UINT  uiExposeTimeMax;  //手动模式下，曝光时间的最大值，单位:行
+} tSdkExpose;
+
+//触发模式描述
+typedef struct
+{
+  INT   iIndex;            //模式索引号
+  char  acDescription[32]; //该模式的描述信息
+} tSdkTrigger;
+
+//传输分包大小描述(主要是针对网络相机有效)
+typedef struct
+{
+    INT  iIndex;              //分包大小索引号
+    char acDescription[32];   //对应的描述信息
+    UINT iPackSize;
+} tSdkPackLength;
+
+//预设的LUT表描述
+typedef struct
+{
+    INT  iIndex;                //编号
+    char acDescription[32];     //描述信息
+} tSdkPresetLut;
+
+//AE算法描述
+typedef struct
+{
+    INT  iIndex;                //编号
+    char acDescription[32];     //描述信息
+} tSdkAeAlgorithm;
+
+//RAW转RGB算法描述
+typedef struct
+{
+    INT  iIndex;                //编号
+    char acDescription[32];     //描述信息
+} tSdkBayerDecodeAlgorithm;
+
+
+//帧率统计信息
+typedef struct
+{
+  INT iTotal;           //当前采集的总帧数（包括错误帧）
+    INT iCapture;       //当前采集的有效帧的数量
+    INT iLost;          //当前丢帧的数量
+} tSdkFrameStatistic;
+
+//相机输出的图像数据格式
+typedef struct
+{
+  INT     iIndex;             //格式种类编号
+  char    acDescription[32];  //描述信息
+  UINT    iMediaType;         //对应的图像格式编码，如CAMERA_MEDIA_TYPE_BAYGR8，在本文件中有定义。
+} tSdkMediaType;
+
+//伽马的设定范围
+typedef struct
+{
+  INT iMin;       //最小值
+  INT iMax;       //最大值
+} tGammaRange;
+
+//对比度的设定范围
+typedef struct
+{
+    INT iMin;   //最小值
+    INT iMax;   //最大值
+} tContrastRange;
+
+//RGB三通道数字增益的设定范围
+typedef struct
+{
+    INT iRGainMin;    //红色增益的最小值
+    INT iRGainMax;    //红色增益的最大值
+    INT iGGainMin;    //绿色增益的最小值
+    INT iGGainMax;    //绿色增益的最大值
+    INT iBGainMin;    //蓝色增益的最小值
+    INT iBGainMax;    //蓝色增益的最大值
+} tRgbGainRange;
+
+//饱和度设定的范围
+typedef struct
+{
+    INT iMin;   //最小值
+    INT iMax;   //最大值
+} tSaturationRange;
+
+//锐化的设定范围
+typedef struct
+{
+  INT iMin;   //最小值
+  INT iMax;   //最大值
+} tSharpnessRange;
+
+//ISP模块的使能信息
+typedef struct
+{
+    BOOL bMonoSensor;       //表示该型号相机是否为黑白相机,如果是黑白相机，则颜色相关的功能都无法调节
+    BOOL bWbOnce;           //表示该型号相机是否支持手动白平衡功能
+    BOOL bAutoWb;           //表示该型号相机是否支持自动白平衡功能
+    BOOL bAutoExposure;     //表示该型号相机是否支持自动曝光功能
+    BOOL bManualExposure;   //表示该型号相机是否支持手动曝光功能
+    BOOL bAntiFlick;        //表示该型号相机是否支持抗频闪功能
+    BOOL bDeviceIsp;        //表示该型号相机是否支持硬件ISP功能
+    BOOL bForceUseDeviceIsp;//bDeviceIsp和bForceUseDeviceIsp同时为TRUE时，表示强制只用硬件ISP，不可取消。
+    BOOL bZoomHD;           //相机硬件是否支持图像缩放输出(只能是缩小)。
+} tSdkIspCapacity;
+
+/* 定义整合的设备描述信息，这些信息可以用于动态构建UI */
+typedef struct
+{
+
+  tSdkTrigger   *pTriggerDesc;          // 触发模式
+  INT           iTriggerDesc;           // 触发模式的个数，即pTriggerDesc数组的大小
+
+  tSdkImageResolution   *pImageSizeDesc;// 预设分辨率选择
+  INT                   iImageSizeDesc; // 预设分辨率的个数，即pImageSizeDesc数组的大小
+
+  tSdkColorTemperatureDes *pClrTempDesc;// 预设色温模式，用于白平衡
+  INT                     iClrTempDesc;
+
+  tSdkMediaType     *pMediaTypeDesc;    // 相机输出图像格式
+  INT               iMediaTypdeDesc;    // 相机输出图像格式的种类个数，即pMediaTypeDesc数组的大小。
+
+  tSdkFrameSpeed    *pFrameSpeedDesc;   // 可调节帧速类型，对应界面上普通 高速 和超级三种速度设置
+  INT               iFrameSpeedDesc;    // 可调节帧速类型的个数，即pFrameSpeedDesc数组的大小。
+
+  tSdkPackLength    *pPackLenDesc;      // 传输包长度，一般用于网络设备
+  INT               iPackLenDesc;       // 可供选择的传输分包长度的个数，即pPackLenDesc数组的大小。
+
+  INT           iOutputIoCounts;        // 可编程输出IO的个数
+  INT           iInputIoCounts;         // 可编程输入IO的个数
+
+  tSdkPresetLut  *pPresetLutDesc;       // 相机预设的LUT表
+  INT            iPresetLut;            // 相机预设的LUT表的个数，即pPresetLutDesc数组的大小
+
+  INT           iUserDataMaxLen;        // 指示该相机中用于保存用户数据区的最大长度。为0表示无。
+  BOOL          bParamInDevice;         // 指示该设备是否支持从设备中读写参数组。1为支持，0不支持。
+
+  tSdkAeAlgorithm   *pAeAlmSwDesc;      // 软件自动曝光算法描述
+  int                iAeAlmSwDesc;      // 软件自动曝光算法个数
+
+  tSdkAeAlgorithm    *pAeAlmHdDesc;     // 硬件自动曝光算法描述，为NULL表示不支持硬件自动曝光
+  int                iAeAlmHdDesc;      // 硬件自动曝光算法个数，为0表示不支持硬件自动曝光
+
+  tSdkBayerDecodeAlgorithm   *pBayerDecAlmSwDesc; // 软件Bayer转换为RGB数据的算法描述
+  int                        iBayerDecAlmSwDesc;  // 软件Bayer转换为RGB数据的算法个数
+
+  tSdkBayerDecodeAlgorithm   *pBayerDecAlmHdDesc; // 硬件Bayer转换为RGB数据的算法描述，为NULL表示不支持
+  int                        iBayerDecAlmHdDesc;  // 硬件Bayer转换为RGB数据的算法个数，为0表示不支持
+
+  /* 图像参数的调节范围定义,用于动态构建UI*/
+  tSdkExpose            sExposeDesc;      // 曝光的范围值
+  tSdkResolutionRange   sResolutionRange; // 分辨率范围描述
+  tRgbGainRange         sRgbGainRange;    // 图像数字增益范围描述
+  tSaturationRange      sSaturationRange; // 饱和度范围描述
+  tGammaRange           sGammaRange;      // 伽马范围描述
+  tContrastRange        sContrastRange;   // 对比度范围描述
+  tSharpnessRange       sSharpnessRange;  // 锐化范围描述
+  tSdkIspCapacity       sIspCapacity;     // ISP能力描述
+
+
+} tSdkCameraCapbility;
+
+
+//图像帧头信息
+typedef struct
+{
+  UINT    uiMediaType;    // 图像格式,Image Format
+  UINT    uBytes;         // 图像数据字节数,Total bytes
+  INT     iWidth;         // 图像的宽度，调用图像处理函数后，该变量可能被动态修改，来指示处理后的图像尺寸
+  INT     iHeight;        // 图像的高度，调用图像处理函数后，该变量可能被动态修改，来指示处理后的图像尺寸
+  INT     iWidthZoomSw;   // 软件缩放的宽度,不需要进行软件裁剪的图像，此变量设置为0.
+  INT     iHeightZoomSw;  // 软件缩放的高度,不需要进行软件裁剪的图像，此变量设置为0.
+  BOOL    bIsTrigger;     // 指示是否为触发帧 is trigger
+  UINT    uiTimeStamp;    // 该帧的采集时间，单位0.1毫秒
+  UINT    uiExpTime;      // 当前图像的曝光值，单位为微秒us
+  float   fAnalogGain;    // 当前图像的模拟增益倍数
+  INT     iGamma;         // 该帧图像的伽马设定值，仅当LUT模式为动态参数生成时有效，其余模式下为-1
+  INT     iContrast;      // 该帧图像的对比度设定值，仅当LUT模式为动态参数生成时有效，其余模式下为-1
+  INT     iSaturation;    // 该帧图像的饱和度设定值，对于黑白相机无意义，为0
+  float   fRgain;         // 该帧图像处理的红色数字增益倍数，对于黑白相机无意义，为1
+  float   fGgain;         // 该帧图像处理的绿色数字增益倍数，对于黑白相机无意义，为1
+  float   fBgain;         // 该帧图像处理的蓝色数字增益倍数，对于黑白相机无意义，为1
+}tSdkFrameHead;
+
+//图像帧描述
+typedef struct sCameraFrame
+{
+  tSdkFrameHead   head;     //帧头
+  BYTE *          pBuffer;  //数据区
+}tSdkFrame;
+
+/// \~chinese 帧事件
+/// \~english Frame Event
+typedef struct tSdkFrameEvent_
+{
+	UINT 	uType;			///< \~chinese 事件类型(1:帧开始   2:帧结束) \~english Event type (1:frame start   2:frame end)
+	UINT	uStatus;		///< \~chinese 状态(0:成功  非0:错误) \~english Status (0:success, non-zero:error)
+	UINT 	uFrameID;		///< \~chinese 帧ID \~english Frame ID
+	UINT	uWidth;			///< \~chinese 宽度 \~english Width
+	UINT	uHeight;		///< \~chinese 高度 \~english Height
+	UINT	uPixelFormat;	///< \~chinese 图像格式 \~english Image Format 
+	UINT	TimeStampL;		///< \~chinese 时间戳低32位 \~english Lower 32 bits of timestamp
+	UINT	TimeStampH;		///< \~chinese 时间戳高32位 \~english High 32 bits of timestamp
+}tSdkFrameEvent;
+
+//图像捕获的回调函数定义
+typedef void (*CAMERA_SNAP_PROC)(CameraHandle hCamera, BYTE *pFrameBuffer, tSdkFrameHead* pFrameHead,PVOID pContext);
+
+//SDK生成的相机配置页面的消息回调函数定义
+typedef void (*CAMERA_PAGE_MSG_PROC)(CameraHandle hCamera,UINT MSG,UINT uParam,PVOID pContext);
+
+/// @ingroup API_RECONNECT
+/// \~chinese 相机连接状态回调
+/// \param [in] hCamera 相机句柄
+/// \param [in] MSG 消息，0: 相机连接断开    1: 相机连接恢复
+/// \param [in] uParam 附加信息
+/// \param [in] pContext 用户数据
+/// \return 无
+/// \note USB相机uParam取值：
+/// \note 		未定义
+/// \note 网口相机uParam取值：
+/// \note		当MSG=0时：未定义
+/// \note		当MSG=1时：
+/// \note			0：上次掉线原因，网络通讯失败
+/// \note			1：上次掉线原因，相机掉电
+/// \~english Camera connection status callback
+/// \param [in] hCamera Camera handle
+/// \param [in] MSG message, 0: Camera disconnected 1: Camera connection restored
+/// \param [in] uParam Additional Information
+/// \param [in] pContext user data
+/// \return None
+/// \note USB camera uParam value:
+/// \note       Undefined
+/// \note network camera uParam value:
+/// \note       When MSG=0: Undefined
+/// \note       When MSG=1:
+/// \note           0: The last dropped reason, network communication failed
+/// \note           1: The last dropped reason, the camera lost power
+typedef void (*CAMERA_CONNECTION_STATUS_CALLBACK)(CameraHandle hCamera,UINT MSG,UINT uParam,PVOID pContext);
+
+/// @ingroup API_ADVANCE
+/// \~chinese 帧事件回调函数定义
+/// \~english Callback function definition for frame event
+typedef void (*CAMERA_FRAME_EVENT_CALLBACK)(CameraHandle hCamera, tSdkFrameEvent* pEvent, PVOID pContext);
+
+
+//----------------------------IMAGE FORMAT DEFINE------------------------------------
+//----------------------------图像格式定义-------------------------------------------
+#define CAMERA_MEDIA_TYPE_MONO                           0x01000000
+#define CAMERA_MEDIA_TYPE_RGB                            0x02000000
+#define CAMERA_MEDIA_TYPE_COLOR                          0x02000000
+#define CAMERA_MEDIA_TYPE_CUSTOM                         0x80000000
+#define CAMERA_MEDIA_TYPE_COLOR_MASK                     0xFF000000
+#define CAMERA_MEDIA_TYPE_OCCUPY1BIT                     0x00010000
+#define CAMERA_MEDIA_TYPE_OCCUPY2BIT                     0x00020000
+#define CAMERA_MEDIA_TYPE_OCCUPY4BIT                     0x00040000
+#define CAMERA_MEDIA_TYPE_OCCUPY8BIT                     0x00080000
+#define CAMERA_MEDIA_TYPE_OCCUPY10BIT                    0x000A0000
+#define CAMERA_MEDIA_TYPE_OCCUPY12BIT                    0x000C0000
+#define CAMERA_MEDIA_TYPE_OCCUPY16BIT                    0x00100000
+#define CAMERA_MEDIA_TYPE_OCCUPY24BIT                    0x00180000
+#define CAMERA_MEDIA_TYPE_OCCUPY32BIT                    0x00200000
+#define CAMERA_MEDIA_TYPE_OCCUPY36BIT                    0x00240000
+#define CAMERA_MEDIA_TYPE_OCCUPY48BIT                    0x00300000
+#define CAMERA_MEDIA_TYPE_OCCUPY64BIT					 0x00400000
+
+#define CAMERA_MEDIA_TYPE_EFFECTIVE_PIXEL_SIZE_MASK      0x00FF0000
+#define CAMERA_MEDIA_TYPE_EFFECTIVE_PIXEL_SIZE_SHIFT     16
+
+#define CAMERA_MEDIA_TYPE_PIXEL_SIZE(type)                 (((type) & CAMERA_MEDIA_TYPE_EFFECTIVE_PIXEL_SIZE_MASK)>>CAMERA_MEDIA_TYPE_EFFECTIVE_PIXEL_SIZE_SHIFT)
+
+#define CAMERA_MEDIA_TYPE_ID_MASK                        0x0000FFFF
+#define CAMERA_MEDIA_TYPE_COUNT                          0x46
+
+/*mono*/
+#define CAMERA_MEDIA_TYPE_MONO1P             (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY1BIT | 0x0037)
+#define CAMERA_MEDIA_TYPE_MONO2P             (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY2BIT | 0x0038)
+#define CAMERA_MEDIA_TYPE_MONO4P             (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY4BIT | 0x0039)
+#define CAMERA_MEDIA_TYPE_MONO8              (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY8BIT | 0x0001)
+#define CAMERA_MEDIA_TYPE_MONO8S             (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY8BIT | 0x0002)
+#define CAMERA_MEDIA_TYPE_MONO10             (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY16BIT | 0x0003)
+#define CAMERA_MEDIA_TYPE_MONO10_PACKED      (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY12BIT | 0x0004)
+#define CAMERA_MEDIA_TYPE_MONO12             (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY16BIT | 0x0005)
+#define CAMERA_MEDIA_TYPE_MONO12_PACKED      (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY12BIT | 0x0006)
+#define CAMERA_MEDIA_TYPE_MONO14             (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY16BIT | 0x0025)
+#define CAMERA_MEDIA_TYPE_MONO16             (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY16BIT | 0x0007)
+
+/*Bayer */
+#define CAMERA_MEDIA_TYPE_BAYGR8             (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY8BIT | 0x0008)
+#define CAMERA_MEDIA_TYPE_BAYRG8             (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY8BIT | 0x0009)
+#define CAMERA_MEDIA_TYPE_BAYGB8             (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY8BIT | 0x000A)
+#define CAMERA_MEDIA_TYPE_BAYBG8             (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY8BIT | 0x000B)
+
+#define CAMERA_MEDIA_TYPE_BAYGR10_MIPI       (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY10BIT | 0x0026)
+#define CAMERA_MEDIA_TYPE_BAYRG10_MIPI       (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY10BIT | 0x0027)
+#define CAMERA_MEDIA_TYPE_BAYGB10_MIPI       (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY10BIT | 0x0028)
+#define CAMERA_MEDIA_TYPE_BAYBG10_MIPI       (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY10BIT | 0x0029)
+
+
+#define CAMERA_MEDIA_TYPE_BAYGR10            (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY16BIT | 0x000C)
+#define CAMERA_MEDIA_TYPE_BAYRG10            (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY16BIT | 0x000D)
+#define CAMERA_MEDIA_TYPE_BAYGB10            (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY16BIT | 0x000E)
+#define CAMERA_MEDIA_TYPE_BAYBG10            (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY16BIT | 0x000F)
+
+#define CAMERA_MEDIA_TYPE_BAYGR12            (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY16BIT | 0x0010)
+#define CAMERA_MEDIA_TYPE_BAYRG12            (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY16BIT | 0x0011)
+#define CAMERA_MEDIA_TYPE_BAYGB12            (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY16BIT | 0x0012)
+#define CAMERA_MEDIA_TYPE_BAYBG12            (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY16BIT | 0x0013)
+
+
+#define CAMERA_MEDIA_TYPE_BAYGR10_PACKED     (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY12BIT | 0x0026)
+#define CAMERA_MEDIA_TYPE_BAYRG10_PACKED     (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY12BIT | 0x0027)
+#define CAMERA_MEDIA_TYPE_BAYGB10_PACKED     (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY12BIT | 0x0028)
+#define CAMERA_MEDIA_TYPE_BAYBG10_PACKED     (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY12BIT | 0x0029)
+
+#define CAMERA_MEDIA_TYPE_BAYGR12_PACKED     (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY12BIT | 0x002A)
+#define CAMERA_MEDIA_TYPE_BAYRG12_PACKED     (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY12BIT | 0x002B)
+#define CAMERA_MEDIA_TYPE_BAYGB12_PACKED     (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY12BIT | 0x002C)
+#define CAMERA_MEDIA_TYPE_BAYBG12_PACKED     (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY12BIT | 0x002D)
+
+#define CAMERA_MEDIA_TYPE_BAYGR16            (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY16BIT | 0x002E)
+#define CAMERA_MEDIA_TYPE_BAYRG16            (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY16BIT | 0x002F)
+#define CAMERA_MEDIA_TYPE_BAYGB16            (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY16BIT | 0x0030)
+#define CAMERA_MEDIA_TYPE_BAYBG16            (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY16BIT | 0x0031)
+
+/*RGB */
+#define CAMERA_MEDIA_TYPE_RGB8               (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY24BIT | 0x0014)
+#define CAMERA_MEDIA_TYPE_BGR8               (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY24BIT | 0x0015)
+#define CAMERA_MEDIA_TYPE_RGBA8              (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY32BIT | 0x0016)
+#define CAMERA_MEDIA_TYPE_BGRA8              (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY32BIT | 0x0017)
+#define CAMERA_MEDIA_TYPE_RGB10              (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY48BIT | 0x0018)
+#define CAMERA_MEDIA_TYPE_BGR10              (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY48BIT | 0x0019)
+#define CAMERA_MEDIA_TYPE_RGB12              (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY48BIT | 0x001A)
+#define CAMERA_MEDIA_TYPE_BGR12              (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY48BIT | 0x001B)
+#define CAMERA_MEDIA_TYPE_RGB16              (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY48BIT | 0x0033)
+#define CAMERA_MEDIA_TYPE_BGR16              (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY48BIT | 0x004B)
+#define CAMERA_MEDIA_TYPE_RGBA16             (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY64BIT | 0x0064)
+#define CAMERA_MEDIA_TYPE_BGRA16             (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY64BIT | 0x0051)
+#define CAMERA_MEDIA_TYPE_RGB10V1_PACKED     (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY32BIT | 0x001C)
+#define CAMERA_MEDIA_TYPE_RGB10P32           (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY32BIT | 0x001D)
+#define CAMERA_MEDIA_TYPE_RGB12V1_PACKED     (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY36BIT | 0X0034)
+#define CAMERA_MEDIA_TYPE_RGB565P            (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY16BIT | 0x0035)
+#define CAMERA_MEDIA_TYPE_BGR565P            (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY16BIT | 0X0036)
+
+/*YUV and YCbCr*/
+#define CAMERA_MEDIA_TYPE_YUV411_8_UYYVYY    (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY12BIT | 0x001E)
+#define CAMERA_MEDIA_TYPE_YUV422_8_UYVY      (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY16BIT | 0x001F)
+#define CAMERA_MEDIA_TYPE_YUV422_8           (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY16BIT | 0x0032)
+#define CAMERA_MEDIA_TYPE_YUV8_UYV           (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY24BIT | 0x0020)
+#define CAMERA_MEDIA_TYPE_YCBCR8_CBYCR       (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY24BIT | 0x003A)
+//CAMERA_MEDIA_TYPE_YCBCR422_8 : YYYYCbCrCbCr
+#define CAMERA_MEDIA_TYPE_YCBCR422_8             (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY16BIT | 0x003B)
+#define CAMERA_MEDIA_TYPE_YCBCR422_8_CBYCRY      (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY16BIT | 0x0043)
+#define CAMERA_MEDIA_TYPE_YCBCR411_8_CBYYCRYY    (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY12BIT | 0x003C)
+#define CAMERA_MEDIA_TYPE_YCBCR601_8_CBYCR       (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY24BIT | 0x003D)
+#define CAMERA_MEDIA_TYPE_YCBCR601_422_8         (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY16BIT | 0x003E)
+#define CAMERA_MEDIA_TYPE_YCBCR601_422_8_CBYCRY  (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY16BIT | 0x0044)
+#define CAMERA_MEDIA_TYPE_YCBCR601_411_8_CBYYCRYY    (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY12BIT | 0x003F)
+#define CAMERA_MEDIA_TYPE_YCBCR709_8_CBYCR           (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY24BIT | 0x0040)
+#define CAMERA_MEDIA_TYPE_YCBCR709_422_8             (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY16BIT | 0x0041)
+#define CAMERA_MEDIA_TYPE_YCBCR709_422_8_CBYCRY      (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY16BIT | 0x0045)
+#define CAMERA_MEDIA_TYPE_YCBCR709_411_8_CBYYCRYY    (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY12BIT | 0x0042)
+
+/*RGB Planar */
+#define CAMERA_MEDIA_TYPE_RGB8_PLANAR        (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY24BIT | 0x0021)
+#define CAMERA_MEDIA_TYPE_RGB10_PLANAR       (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY48BIT | 0x0022)
+#define CAMERA_MEDIA_TYPE_RGB12_PLANAR       (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY48BIT | 0x0023)
+#define CAMERA_MEDIA_TYPE_RGB16_PLANAR       (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY48BIT | 0x0024)
+
+
+
+/*MindVision 12bit packed bayer*/
+#define CAMERA_MEDIA_TYPE_BAYGR12_PACKED_MV     (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY12BIT | 0x0060)
+#define CAMERA_MEDIA_TYPE_BAYRG12_PACKED_MV     (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY12BIT | 0x0061)
+#define CAMERA_MEDIA_TYPE_BAYGB12_PACKED_MV     (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY12BIT | 0x0062)
+#define CAMERA_MEDIA_TYPE_BAYBG12_PACKED_MV     (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY12BIT | 0x0063)
+
+/*MindVision 12bit packed monochome*/
+#define CAMERA_MEDIA_TYPE_MONO12_PACKED_MV     (CAMERA_MEDIA_TYPE_MONO | CAMERA_MEDIA_TYPE_OCCUPY12BIT | 0x0064)
+#define CAMERA_MEDIA_TYPE_YUV420P_MV			     (CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY12BIT | 0x0065)
+
+/*planar YUV 4:2:0, 12bpp, 1 plane for Y and 1 plane for the UV components, which are interleaved (first byte V and the following byte U)*/
+#define CAMERA_MEDIA_TYPE_YUV_NV21_MV			(CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY12BIT | 0x0066)
+
+/* H264 H265 */
+#define CAMERA_MEDIA_TYPE_H264_MV				(CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY8BIT | 0x0067)
+#define CAMERA_MEDIA_TYPE_H265_MV				(CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY8BIT | 0x0068)
+
+/* JPEG */
+#define CAMERA_MEDIA_TYPE_JPEG_MV				(CAMERA_MEDIA_TYPE_COLOR | CAMERA_MEDIA_TYPE_OCCUPY8BIT | 0x0069)
+
+#endif

src/device/mindvision/include/CameraStatus.h

@@ -0,0 +1,118 @@
+
+#ifndef __CAMERA_STATUS_DEF__
+#define __CAMERA_STATUS_DEF__
+
+typedef int CameraSdkStatus;
+
+/*常用的宏*/
+#define SDK_SUCCESS(_FUC_) ((_FUC_) == CAMERA_STATUS_SUCCESS)
+
+#define SDK_UNSUCCESS(_FUC_) ((_FUC_) != CAMERA_STATUS_SUCCESS)
+
+#define SDK_UNSUCCESS_RETURN(_FUC_, RET)          \
+  if ((RET = (_FUC_)) != CAMERA_STATUS_SUCCESS) { \
+    return RET;                                   \
+  }
+
+#define SDK_UNSUCCESS_BREAK(_FUC_)        \
+  if ((_FUC_) != CAMERA_STATUS_SUCCESS) { \
+    break;                                \
+  }
+
+/* 常用错误  */
+
+#define CAMERA_STATUS_SUCCESS 0                  // 操作成功
+#define CAMERA_STATUS_FAILED -1                  // 操作失败
+#define CAMERA_STATUS_INTERNAL_ERROR -2          // 内部错误
+#define CAMERA_STATUS_UNKNOW -3                  // 未知错误
+#define CAMERA_STATUS_NOT_SUPPORTED -4           // 不支持该功能
+#define CAMERA_STATUS_NOT_INITIALIZED -5         // 初始化未完成
+#define CAMERA_STATUS_PARAMETER_INVALID -6       // 参数无效
+#define CAMERA_STATUS_PARAMETER_OUT_OF_BOUND -7  // 参数越界
+#define CAMERA_STATUS_UNENABLED -8               // 未使能
+#define CAMERA_STATUS_USER_CANCEL -9             // 用户手动取消了，比如roi面板点击取消，返回
+#define CAMERA_STATUS_PATH_NOT_FOUND -10         // 注册表中没有找到对应的路径
+#define CAMERA_STATUS_SIZE_DISMATCH -11          // 获得图像数据长度和定义的尺寸不匹配
+#define CAMERA_STATUS_TIME_OUT -12               // 超时错误
+#define CAMERA_STATUS_IO_ERROR -13               // 硬件IO错误
+#define CAMERA_STATUS_COMM_ERROR -14             // 通讯错误
+#define CAMERA_STATUS_BUS_ERROR -15              // 总线错误
+#define CAMERA_STATUS_NO_DEVICE_FOUND -16        // 没有发现设备
+#define CAMERA_STATUS_NO_LOGIC_DEVICE_FOUND -17  // 未找到逻辑设备
+#define CAMERA_STATUS_DEVICE_IS_OPENED -18       // 设备已经打开
+#define CAMERA_STATUS_DEVICE_IS_CLOSED -19       // 设备已经关闭
+#define CAMERA_STATUS_DEVICE_VEDIO_CLOSED \
+  -20  // 没有打开设备视频，调用录像相关的函数时，如果相机视频没有打开，则回返回该错误。
+#define CAMERA_STATUS_NO_MEMORY -21           // 没有足够系统内存
+#define CAMERA_STATUS_FILE_CREATE_FAILED -22  // 创建文件失败
+#define CAMERA_STATUS_FILE_INVALID -23        // 文件格式无效
+#define CAMERA_STATUS_WRITE_PROTECTED -24     // 写保护，不可写
+#define CAMERA_STATUS_GRAB_FAILED -25         // 数据采集失败
+#define CAMERA_STATUS_LOST_DATA -26           // 数据丢失，不完整
+#define CAMERA_STATUS_EOF_ERROR -27           // 未接收到帧结束符
+#define CAMERA_STATUS_BUSY -28                // 正忙(上一次操作还在进行中)，此次操作不能进行
+#define CAMERA_STATUS_WAIT -29                // 需要等待(进行操作的条件不成立)，可以再次尝试trf
+#define CAMERA_STATUS_IN_PROCESS -30          // 正在进行，已经被操作过
+#define CAMERA_STATUS_IIC_ERROR -31           // IIC传输错误
+#define CAMERA_STATUS_SPI_ERROR -32           // SPI传输错误
+#define CAMERA_STATUS_USB_CONTROL_ERROR -33   // USB控制传输错误
+#define CAMERA_STATUS_USB_BULK_ERROR -34      // USB BULK传输错误
+#define CAMERA_STATUS_SOCKET_INIT_ERROR -35   // 网络传输套件初始化失败
+#define CAMERA_STATUS_GIGE_FILTER_INIT_ERROR \
+  -36  // 网络相机内核过滤驱动初始化失败，请检查是否正确安装了驱动，或者重新安装。
+#define CAMERA_STATUS_NET_SEND_ERROR -37        // 网络数据发送错误
+#define CAMERA_STATUS_DEVICE_LOST -38           // 与网络相机失去连接，心跳检测超时
+#define CAMERA_STATUS_DATA_RECV_LESS -39        // 接收到的字节数比请求的少
+#define CAMERA_STATUS_FUNCTION_LOAD_FAILED -40  // 从文件中加载程序失败
+#define CAMERA_STATUS_CRITICAL_FILE_LOST -41    // 程序运行所必须的文件丢失。
+#define CAMERA_STATUS_SENSOR_ID_DISMATCH -42    // 固件和程序不匹配，原因是下载了错误的固件。
+#define CAMERA_STATUS_OUT_OF_RANGE -43          // 参数超出有效范围。
+#define CAMERA_STATUS_REGISTRY_ERROR \
+  -44  // 安装程序注册错误。请重新安装程序，或者运行安装目录Setup/Installer.exe
+#define CAMERA_STATUS_ACCESS_DENY \
+  -45  // 禁止访问。指定相机已经被其他程序占用时，再申请访问该相机，会返回该状态。(一个相机不能被多个程序同时访问)
+#define CAMERA_STATUS_CAMERA_NEED_RESET \
+  -46  // 表示相机需要复位后才能正常使用，此时请让相机断电重启，或者重启操作系统后，便可正常使用。
+#define CAMERA_STATUS_ISP_MOUDLE_NOT_INITIALIZED -47  // ISP模块未初始化
+#define CAMERA_STATUS_ISP_DATA_CRC_ERROR -48          // 数据校验错误
+#define CAMERA_STATUS_MV_TEST_FAILED -49              // 数据测试失败
+#define CAMERA_STATUS_INTERNAL_ERR1 -50               // 内部错误1
+#define CAMERA_STATUS_U3V_NO_CONTROL_EP -51           // U3V控制端点未找到
+#define CAMERA_STATUS_U3V_CONTROL_ERROR -52           // U3V控制通讯错误
+#define CAMERA_STATUS_INVALID_FRIENDLY_NAME \
+  -53  ///< \~chinese 无效的设备名，名字里不能包含以下字符(\/:*?"<>|") \~english Invalid device name, the name cannot contain the following characters (\/:*?"<>|")
+#define CAMERA_STATUS_FORMAT_ERROR -54  ///< \~chinese 格式错误 \~english Format error
+#define CAMERA_STATUS_PCIE_OPEN_ERROR \
+  -55  ///< \~chinese PCIE设备打开失败 \~english PCIE device open failed
+#define CAMERA_STATUS_PCIE_COMM_ERROR \
+  -56  ///< \~chinese PCIE设备通讯失败 \~english PCIE device communication failed
+#define CAMERA_STATUS_PCIE_DDR_ERROR -57  ///< \~chinese PCIE DDR错误 \~english PCIE DDR error
+
+//和AIA制定的标准相同
+/*#define CAMERA_AIA_SUCCESS                        0x0000 */
+#define CAMERA_AIA_PACKET_RESEND 0x0100       //该帧需要重传
+#define CAMERA_AIA_NOT_IMPLEMENTED 0x8001     //设备不支持的命令
+#define CAMERA_AIA_INVALID_PARAMETER 0x8002   //命令参数非法
+#define CAMERA_AIA_INVALID_ADDRESS 0x8003     //不可访问的地址
+#define CAMERA_AIA_WRITE_PROTECT 0x8004       //访问的对象不可写
+#define CAMERA_AIA_BAD_ALIGNMENT 0x8005       //访问的地址没有按照要求对齐
+#define CAMERA_AIA_ACCESS_DENIED 0x8006       //没有访问权限
+#define CAMERA_AIA_BUSY 0x8007                //命令正在处理中
+#define CAMERA_AIA_DEPRECATED 0x8008          //0x8008-0x0800B  0x800F  该指令已经废弃
+#define CAMERA_AIA_PACKET_UNAVAILABLE 0x800C  //包无效
+#define CAMERA_AIA_DATA_OVERRUN 0x800D        //数据溢出，通常是收到的数据比需要的多
+#define CAMERA_AIA_INVALID_HEADER 0x800E      //数据包头部中某些区域与协议不匹配
+#define CAMERA_AIA_PACKET_NOT_YET_AVAILABLE \
+  0x8010  //图像分包数据还未准备好，多用于触发模式，应用程序访问超时
+#define CAMERA_AIA_PACKET_AND_PREV_REMOVED_FROM_MEMORY \
+  0x8011  //需要访问的分包已经不存在。多用于重传时数据已经不在缓冲区中
+#define CAMERA_AIA_PACKET_REMOVED_FROM_MEMORY \
+  0x8012                               //CAMERA_AIA_PACKET_AND_PREV_REMOVED_FROM_MEMORY
+#define CAMERA_AIA_NO_REF_TIME 0x0813  //没有参考时钟源。多用于时间同步的命令执行时
+#define CAMERA_AIA_PACKET_TEMPORARILY_UNAVAILABLE \
+  0x0814                               //由于信道带宽问题，当前分包暂时不可用，需稍后进行访问
+#define CAMERA_AIA_OVERFLOW 0x0815     //设备端数据溢出，通常是队列已满
+#define CAMERA_AIA_ACTION_LATE 0x0816  //命令执行已经超过有效的指定时间
+#define CAMERA_AIA_ERROR 0x8FFF        //错误
+
+#endif

src/device/mindvision/mindvision.cpp

@@ -0,0 +1,171 @@
+#include "mindvision.hpp"
+
+#include <libusb-1.0/libusb.h>
+
+#include <stdexcept>
+
+#include "src/component/logger.hpp"
+
+using namespace std::chrono_literals;
+
+namespace device
+{
+MindVision::MindVision(double exposure_ms, double gamma, const std::string & vid_pid)
+: exposure_ms_(exposure_ms),
+  gamma_(gamma),
+  handle_(-1),
+  quit_(false),
+  ok_(false),
+  queue_(1),
+  vid_(-1),
+  pid_(-1)
+{
+  set_vid_pid(vid_pid);
+  if (libusb_init(NULL)) component::logger()->warn("Unable to init libusb!");
+
+  try_open();
+
+  // 守护线程
+  daemon_thread_ = std::thread{[this] {
+    while (!quit_) {
+      std::this_thread::sleep_for(100ms);
+
+      if (ok_) continue;
+
+      if (capture_thread_.joinable()) capture_thread_.join();
+
+      close();
+      reset_usb();
+      try_open();
+    }
+  }};
+}
+
+MindVision::~MindVision()
+{
+  quit_ = true;
+  if (daemon_thread_.joinable()) daemon_thread_.join();
+  if (capture_thread_.joinable()) capture_thread_.join();
+  close();
+  component::logger()->info("Mindvision destructed.");
+}
+
+void MindVision::read(cv::Mat & img, std::chrono::steady_clock::time_point & timestamp)
+{
+  CameraData data;
+  queue_.pop(data);
+
+  img = data.img;
+  timestamp = data.timestamp;
+}
+
+void MindVision::open()
+{
+  int camera_num = 1;
+  tSdkCameraDevInfo camera_info_list;
+  tSdkCameraCapbility camera_capbility;
+  CameraSdkInit(1);
+  CameraEnumerateDevice(&camera_info_list, &camera_num);
+
+  if (camera_num == 0) throw std::runtime_error("Not found camera!");
+
+  if (CameraInit(&camera_info_list, -1, -1, &handle_) != CAMERA_STATUS_SUCCESS)
+    throw std::runtime_error("Failed to init camera!");
+
+  CameraGetCapability(handle_, &camera_capbility);
+  width_ = camera_capbility.sResolutionRange.iWidthMax;
+  height_ = camera_capbility.sResolutionRange.iHeightMax;
+
+  CameraSetAeState(handle_, FALSE);                        // 关闭自动曝光
+  CameraSetExposureTime(handle_, exposure_ms_ * 1e3);      // 设置曝光
+  CameraSetGamma(handle_, gamma_ * 1e2);                   // 设置伽马
+  CameraSetIspOutFormat(handle_, CAMERA_MEDIA_TYPE_BGR8);  // 设置输出格式为BGR
+  CameraSetTriggerMode(handle_, 0);                        // 设置为连续采集模式
+  CameraSetFrameSpeed(handle_, 1);                         // 设置为低帧率模式
+
+  CameraPlay(handle_);
+
+  // 取图线程
+  capture_thread_ = std::thread{[this] {
+    tSdkFrameHead head;
+    BYTE * raw;
+
+    ok_ = true;
+    while (!quit_) {
+      std::this_thread::sleep_for(1ms);
+
+      auto img = cv::Mat(height_, width_, CV_8UC3);
+
+      auto status = CameraGetImageBuffer(handle_, &head, &raw, 100);
+      auto timestamp = std::chrono::steady_clock::now();
+
+      if (status != CAMERA_STATUS_SUCCESS) {
+        component::logger()->warn("Camera dropped!");
+        ok_ = false;
+        break;
+      }
+
+      CameraImageProcess(handle_, raw, img.data, &head);
+      CameraReleaseImageBuffer(handle_, raw);
+
+      queue_.push({img, timestamp});
+    }
+  }};
+
+  component::logger()->info("Mindvision opened.");
+}
+
+void MindVision::try_open()
+{
+  try {
+    open();
+  } catch (const std::exception & e) {
+    component::logger()->warn("{}", e.what());
+  }
+}
+
+void MindVision::close()
+{
+  if (handle_ == -1) return;
+  CameraUnInit(handle_);
+}
+
+void MindVision::set_vid_pid(const std::string & vid_pid)
+{
+  auto index = vid_pid.find(':');
+  if (index == std::string::npos) {
+    component::logger()->warn("Invalid vid_pid: \"{}\"", vid_pid);
+    return;
+  }
+
+  auto vid_str = vid_pid.substr(0, index);
+  auto pid_str = vid_pid.substr(index + 1);
+
+  try {
+    vid_ = std::stoi(vid_str, 0, 16);
+    pid_ = std::stoi(pid_str, 0, 16);
+  } catch (const std::exception &) {
+    component::logger()->warn("Invalid vid_pid: \"{}\"", vid_pid);
+  }
+}
+
+void MindVision::reset_usb() const
+{
+  if (vid_ == -1 || pid_ == -1) return;
+
+  // https://github.com/ralight/usb-reset/blob/master/usb-reset.c
+  auto handle = libusb_open_device_with_vid_pid(NULL, vid_, pid_);
+  if (!handle) {
+    component::logger()->warn("Unable to open usb!");
+    return;
+  }
+
+  if (libusb_reset_device(handle))
+    component::logger()->warn("Unable to reset usb!");
+  else
+    component::logger()->info("Reset usb successfully :)");
+
+  libusb_close(handle);
+}
+
+}  // namespace device

src/device/mindvision/mindvision.hpp

@@ -0,0 +1,46 @@
+#ifndef DEVICE__MINDVISION_HPP
+#define DEVICE__MINDVISION_HPP
+
+#include <chrono>
+#include <opencv2/opencv.hpp>
+#include <thread>
+
+#include "CameraApi.h"
+#include "src/device/camera.hpp"
+#include "src/component/thread_safe_queue.hpp"
+
+namespace device
+{
+class MindVision : public CameraBase
+{
+public:
+  MindVision(double exposure_ms, double gamma, const std::string & vid_pid);
+  ~MindVision() override;
+  void read(cv::Mat & img, std::chrono::steady_clock::time_point & timestamp) override;
+
+private:
+  struct CameraData
+  {
+    cv::Mat img;
+    std::chrono::steady_clock::time_point timestamp;
+  };
+
+  double exposure_ms_, gamma_;
+  CameraHandle handle_;
+  int height_, width_;
+  bool quit_, ok_;
+  std::thread capture_thread_;
+  std::thread daemon_thread_;
+  component::ThreadSafeQueue<CameraData> queue_;
+  int vid_, pid_;
+
+  void open();
+  void try_open();
+  void close();
+  void set_vid_pid(const std::string & vid_pid);
+  void reset_usb() const;
+};
+
+}  // namespace device
+
+#endif  // DEVICE__MINDVISION_HPP

src/device/ros2/publish2nav.cpp

@@ -0,0 +1,48 @@
+#include "publish2nav.hpp"
+
+#include <Eigen/Dense>
+#include <chrono>
+#include <memory>
+#include <thread>
+
+#include "src/component/logger.hpp"
+
+namespace device
+{
+
+Publish2Nav::Publish2Nav() : Node("auto_aim_target_pos_publisher")
+{
+  publisher_ = this->create_publisher<std_msgs::msg::String>("auto_aim_target_pos", 10);
+
+  RCLCPP_INFO(this->get_logger(), "auto_aim_target_pos_publisher node initialized.");
+}
+
+Publish2Nav::~Publish2Nav()
+{
+  RCLCPP_INFO(this->get_logger(), "auto_aim_target_pos_publisher node shutting down.");
+}
+
+void Publish2Nav::send_data(const Eigen::Vector4d & target_pos)
+{
+  // 创建消息
+  auto message = std::make_shared<std_msgs::msg::String>();
+
+  // 将 Eigen::Vector3d 数据转换为字符串并存储在消息中
+  message->data = std::to_string(target_pos[0]) + "," + std::to_string(target_pos[1]) + "," +
+                  std::to_string(target_pos[2]) + "," + std::to_string(target_pos[3]);
+
+  // 发布消息
+  publisher_->publish(*message);
+
+  // RCLCPP_INFO(
+  //   this->get_logger(), "auto_aim_target_pos_publisher node sent message: '%s'",
+  //   message->data.c_str());
+}
+
+void Publish2Nav::start()
+{
+  RCLCPP_INFO(this->get_logger(), "auto_aim_target_pos_publisher node starting to spin...");
+  rclcpp::spin(this->shared_from_this());
+}
+
+}  // namespace device

src/device/ros2/publish2nav.hpp

@@ -0,0 +1,35 @@
+#ifndef DEVICE__PBLISH2NAV_HPP
+#define DEVICE__PBLISH2NAV_HPP
+
+#include <Eigen/Dense>  // For Eigen::Vector3d
+#include <chrono>
+#include <deque>
+#include <memory>
+#include <mutex>
+#include <optional>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+
+namespace device
+{
+class Publish2Nav : public rclcpp::Node
+{
+public:
+  Publish2Nav();
+
+  ~Publish2Nav();
+
+  void start();
+
+  void send_data(const Eigen::Vector4d & data);
+
+private:
+  // ROS2 发布者
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
+};
+
+}  // namespace device
+
+#endif  // Publish2Nav_HPP_

src/device/ros2/ros2.cpp

@@ -0,0 +1,36 @@
+#include "ros2.hpp"
+namespace device
+{
+ROS2::ROS2()
+{
+  rclcpp::init(0, nullptr);
+
+  publish2nav_ = std::make_shared<Publish2Nav>();
+
+  subscribe2nav_ = std::make_shared<Subscribe2Nav>();
+
+  publish_spin_thread_ = std::make_unique<std::thread>([this]() { publish2nav_->start(); });
+
+  subscribe_spin_thread_ = std::make_unique<std::thread>([this]() { subscribe2nav_->start(); });
+}
+
+ROS2::~ROS2()
+{
+  rclcpp::shutdown();
+  publish_spin_thread_->join();
+  subscribe_spin_thread_->join();
+}
+
+void ROS2::publish(const Eigen::Vector4d & target_pos) { publish2nav_->send_data(target_pos); }
+
+std::vector<int8_t> ROS2::subscribe_enemy_status()
+{
+  return subscribe2nav_->subscribe_enemy_status();
+}
+
+std::vector<int8_t> ROS2::subscribe_autoaim_target()
+{
+  return subscribe2nav_->subscribe_autoaim_target();
+}
+
+}  // namespace device

src/device/ros2/ros2.hpp

@@ -0,0 +1,45 @@
+#ifndef DEVICE__ROS2_HPP
+#define DEVICE__ROS2_HPP
+
+#include "publish2nav.hpp"
+#include "subscribe2nav.hpp"
+
+namespace device
+{
+class ROS2
+{
+public:
+  ROS2();
+
+  ~ROS2();
+
+  void publish(const Eigen::Vector4d & target_pos);
+
+  std::vector<int8_t> subscribe_enemy_status();
+
+  std::vector<int8_t> subscribe_autoaim_target();
+
+  template <typename T>
+  std::shared_ptr<rclcpp::Publisher<T>> create_publisher(
+    const std::string & node_name, const std::string & topic_name, size_t queue_size)
+  {
+    auto node = std::make_shared<rclcpp::Node>(node_name);
+
+    auto publisher = node->create_publisher<T>(topic_name, queue_size);
+
+    // 运行一个单独的线程来 spin 这个节点，确保消息可以被正确发布
+    std::thread([node]() { rclcpp::spin(node); }).detach();
+
+    return publisher;
+  }
+
+private:
+  std::shared_ptr<Publish2Nav> publish2nav_;
+  std::shared_ptr<Subscribe2Nav> subscribe2nav_;
+
+  std::unique_ptr<std::thread> publish_spin_thread_;
+  std::unique_ptr<std::thread> subscribe_spin_thread_;
+};
+
+}  // namespace device
+#endif

src/device/ros2/subscribe2nav.cpp

@@ -0,0 +1,108 @@
+#include "subscribe2nav.hpp"
+
+#include <sstream>
+#include <vector>
+
+namespace device
+{
+
+Subscribe2Nav::Subscribe2Nav()
+: Node("nav_subscriber"),
+  enemy_statue_queue_(1),
+  autoaim_target_queue_(1),
+  enemy_status_counter_(0),
+  autoaim_target_counter_(0)
+{
+  enemy_status_subscription_ = this->create_subscription<sp_msgs::msg::EnemyStatusMsg>(
+    "enemy_status", 10,
+    std::bind(&Subscribe2Nav::enemy_status_callback, this, std::placeholders::_1));
+
+  autoaim_target_subscription_ = this->create_subscription<sp_msgs::msg::AutoaimTargetMsg>(
+    "autoaim_target", 10,
+    std::bind(&Subscribe2Nav::autoaim_target_callback, this, std::placeholders::_1));
+
+  RCLCPP_INFO(this->get_logger(), "nav_subscriber node initialized.");
+}
+
+Subscribe2Nav::~Subscribe2Nav()
+{
+  RCLCPP_INFO(this->get_logger(), "nav_subscriber node shutting down.");
+}
+
+void Subscribe2Nav::enemy_status_callback(const sp_msgs::msg::EnemyStatusMsg::SharedPtr msg)
+{
+  enemy_statue_queue_.clear();
+  enemy_statue_queue_.push(*msg);
+
+  enemy_status_counter_++;
+
+  if (enemy_status_counter_ >= 2) {
+    if (enemy_status_timer_) {
+      enemy_status_timer_->cancel();
+    }
+    enemy_status_timer_ = this->create_wall_timer(std::chrono::milliseconds(1500), [this]() {
+      enemy_statue_queue_.clear();
+      enemy_status_counter_ = 0;
+      RCLCPP_INFO(
+        this->get_logger(), "Enemy status queue cleared due to inactivity after two messages.");
+    });
+  }
+}
+
+void Subscribe2Nav::autoaim_target_callback(const sp_msgs::msg::AutoaimTargetMsg::SharedPtr msg)
+{
+  autoaim_target_queue_.clear();
+  autoaim_target_queue_.push(*msg);
+
+  autoaim_target_counter_++;
+
+  if (autoaim_target_counter_ >= 2) {
+    if (autoaim_target_timer_) {
+      autoaim_target_timer_->cancel();
+    }
+    autoaim_target_timer_ = this->create_wall_timer(std::chrono::milliseconds(1500), [this]() {
+      autoaim_target_queue_.clear();
+      autoaim_target_counter_ = 0;
+      RCLCPP_INFO(
+        this->get_logger(), "Autoaim target queue cleared due to inactivity after two messages.");
+    });
+  }
+}
+
+void Subscribe2Nav::start()
+{
+  RCLCPP_INFO(this->get_logger(), "nav_subscriber node Starting to spin...");
+  rclcpp::spin(this->shared_from_this());
+}
+
+std::vector<int8_t> Subscribe2Nav::subscribe_enemy_status()
+{
+  if (enemy_statue_queue_.empty()) {
+    return std::vector<int8_t>();
+  }
+  sp_msgs::msg::EnemyStatusMsg msg;
+
+  enemy_statue_queue_.back(msg);
+  RCLCPP_INFO(
+    this->get_logger(), "Subscribe enemy_status at: %d.%09u", msg.timestamp.sec,
+    msg.timestamp.nanosec);
+
+  return msg.invincible_enemy_ids;
+}
+
+std::vector<int8_t> Subscribe2Nav::subscribe_autoaim_target()
+{
+  if (autoaim_target_queue_.empty()) {
+    return std::vector<int8_t>();
+  }
+  sp_msgs::msg::AutoaimTargetMsg msg;
+
+  autoaim_target_queue_.back(msg);
+  RCLCPP_INFO(
+    this->get_logger(), "Subscribe autoaim_target at: %d.%09u", msg.timestamp.sec,
+    msg.timestamp.nanosec);
+
+  return msg.target_ids;
+}
+
+}  // namespace device