.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,6 +170,8 @@ qtcreator-*
 COLCON_IGNORE
 AMENT_IGNORE
 
-
+# Keep camera SDK .so files
+!src/device/hikrobot/lib
+!src/device/mindvision/lib
 
 .claude/

CMakeLists.txt

@@ -35,7 +35,7 @@ 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(capture ${OpenCV_LIBS} fmt::fmt 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)
@@ -117,3 +117,4 @@ target_link_libraries(balance_infantry ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann
 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)
 
+

build.sh

@@ -1,2 +1,3 @@
+conda deactivate
 cmake -B build
 make -C build/ -j $(nproc)

calibration/board_pattern.hpp

@@ -78,7 +78,7 @@ inline bool find_pattern_points(
   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 flags = cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_NORMALIZE_IMAGE;
   auto success = cv::findChessboardCorners(gray, board_pattern.pattern_size, points, flags);
   if (!success) return false;
 

calibration/capture.cpp

@@ -1,16 +1,16 @@
 #include <fmt/core.h>
-#include <yaml-cpp/yaml.h>
 
 #include <filesystem>
 #include <fstream>
 #include <opencv2/opencv.hpp>
 
 #include "calibration/board_pattern.hpp"
+#include "src/device/camera.hpp"
+#include "src/device/cboard.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/component/yaml.hpp"
-#include "src/device/gimbal/gimbal.hpp"
 
 const std::string keys =
   "{help h usage ?  |                          | 输出命令行参数说明}"
@@ -27,13 +27,10 @@ void write_q(const std::string q_path, const Eigen::Quaterniond & q)
 }
 
 void capture_loop(
-  const std::string & config_path, const std::string & output_folder)
+  const std::string & config_path, const std::string & output_folder,
+  const calibration::BoardPattern & board_pattern)
 {
-  // 从配置文件加载标定板参数（支持 circles_grid 和 chessboard）
+  device::CBoard cboard(config_path);
-  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;
@@ -41,7 +38,7 @@ void capture_loop(
   int count = 0;
   while (true) {
     camera.read(img, timestamp);
-    Eigen::Quaterniond q = gimbal.q(timestamp);
+    Eigen::Quaterniond q = cboard.imu_at(timestamp);
 
     // 在图像上显示欧拉角，用来判断imuabs系的xyz正方向，同时判断imu是否存在零漂
     auto img_with_ypr = img.clone();
@@ -51,26 +48,7 @@ void capture_loop(
     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;
+    auto success = calibration::find_pattern_points(img, board_pattern, centers_2d);
-    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);  // 显示时缩小图片尺寸
 
@@ -91,7 +69,7 @@ void capture_loop(
     component::logger()->info("[{}] Saved in {}", count, output_folder);
   }
 
-  // 离开该作用域时，camera和gimbal会自动关闭
+  // 离开该作用域时，camera和cboard会自动关闭
 }
 
 int main(int argc, char * argv[])
@@ -104,20 +82,17 @@ int main(int argc, char * argv[])
   }
   auto config_path = cli.get<std::string>(0);
   auto output_folder = cli.get<std::string>("output-folder");
+  auto yaml = component::load(config_path);
+  auto board_pattern = calibration::load_board_pattern(yaml);
 
   // 新建输出文件夹
   std::filesystem::create_directory(output_folder);
 
-  // 从配置文件读取标定板类型和尺寸
-  auto yaml = YAML::LoadFile(config_path);
-  auto board_pattern = calibration::load_board_pattern(yaml);
   component::logger()->info(
-    "标定板类型: {}, 尺寸: {}列{}行",
+    "标定板类型: {}, 尺寸: {}x{}", calibration::pattern_name(board_pattern.pattern_type),
-    calibration::pattern_name(board_pattern.pattern_type),
     board_pattern.pattern_size.width, board_pattern.pattern_size.height);
-
   // 主循环，保存图片和对应四元数
-  capture_loop(config_path, output_folder);
+  capture_loop(config_path, output_folder, board_pattern);
 
   component::logger()->warn("注意四元数输出顺序为wxyz");
 

calibration/test/camera_calibration.py

@@ -1,184 +0,0 @@
-#!/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

@@ -1,221 +0,0 @@
-#!/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

@@ -1,127 +0,0 @@
-#!/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

@@ -76,7 +76,7 @@ can_interface: "can0"
 
 #####-----gimbal参数-----#####
 com_port: "/dev/gimbal"
-baudrate: 115200
+
 #####-----buff_detector参数-----#####
 model: "assets/yolo11_buff_int8.xml"
 

configs/calibration.yaml

@@ -1,7 +1,7 @@
-pattern_cols: 11
+pattern_cols: 10
-pattern_rows: 8
+pattern_rows: 7
-center_distance_mm: 30
+center_distance_mm: 40
-pattern_type: chessboard   # circles_grid | chessboard
+pattern_type: circles_grid   # circles_grid | chessboard
 
 R_gimbal2imubody: [1, 0, 0, 0, 1, 0,  0, 0, 1]
 
@@ -15,10 +15,3 @@ quaternion_canid: 0x01
 bullet_speed_canid: 0x110
 send_canid: 0xff
 can_interface: "can0"
-#####-----gimbal参数-----#####
-com_port: "/dev/ttyUSB0"
-baudrate: 115200
-
-# 重投影误差: 0.1791px
-camera_matrix: [1827.8294221039337, 0, 716.86057740384501, 0, 1828.9736207357851, 613.69509305531699, 0, 0, 1]
-distort_coeffs: [-0.083642708058668358, 0.18891600176175308, -0.00030362184648520616, -0.00066798903909152669, 0]

configs/camera.yaml

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

configs/demo.yaml

@@ -78,7 +78,7 @@ can_interface: "can0"
 
 #####-----gimbal参数-----#####
 com_port: "/dev/gimbal"
-baudrate: 115200
+
 #####-----planner-----#####
 fire_thresh: 0.003
 

configs/standard3.yaml

@@ -1,5 +1,5 @@
-# enemy_color: "red"
+enemy_color: "red"
-enemy_color: "blue"
+# enemy_color: "blue"
 
 #####-----神经网络参数-----#####
 yolo_name: yolov5
@@ -7,7 +7,7 @@ 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
+device: GPU
 min_confidence: 0.8
 use_traditional: true
 
@@ -78,8 +78,7 @@ send_canid: 0xff
 can_interface: "can0"
 
 #####-----gimbal参数-----#####
-com_port: "/dev/ttyUSB0"
+com_port: "/dev/gimbal"
-baudrate: 115200
 yaw_kp: 0
 yaw_kd: 0
 pitch_kp: 0

configs/standard4.yaml

@@ -79,7 +79,7 @@ can_interface: "can0"
 
 #####-----gimbal参数-----#####
 com_port: "/dev/gimbal"
-baudrate: 115200
+
 #####-----planner-----#####
 fire_thresh: 0.003
 

src/device/gimbal/gimbal.cpp

@@ -11,14 +11,10 @@ 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);
@@ -135,99 +131,36 @@ 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...",
+      component::logger()->warn("[Gimbal] Too many errors, attempting to reconnect...");
-                                total_bytes_read, valid_packets);
       reconnect();
       continue;
     }
 
-    // 逐字节查找包头第一个字节 'M'
+    if (!read(reinterpret_cast<uint8_t *>(&rx_data_), sizeof(rx_data_.head))) {
-    if (!read(&byte, 1)) {
       error_count++;
       continue;
     }
 
-    // 读取成功，重置错误计数
+    if (rx_data_.head[0] != 'S' || rx_data_.head[1] != 'P') 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 (!component::check_crc16(reinterpret_cast<uint8_t *>(&rx_data_), sizeof(rx_data_))) {
-    if (rx_data_.mode > 3) {
+      component::logger()->debug("[Gimbal] CRC16 check failed.");
-      // 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});

src/device/gimbal/gimbal.hpp

@@ -16,7 +16,7 @@ namespace device
 {
 struct __attribute__((packed)) GimbalToVision
 {
-  uint8_t head[2] = {'M', 'R'};
+  uint8_t head[2] = {'S', 'P'};
   uint8_t mode;  // 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符
   float q[4];    // wxyz顺序
   float yaw;
@@ -32,7 +32,7 @@ static_assert(sizeof(GimbalToVision) <= 64);
 
 struct __attribute__((packed)) VisionToGimbal
 {
-  uint8_t head[2] = {'M', 'R'};
+  uint8_t head[2] = {'S', 'P'};
   uint8_t mode;  // 0: 不控制, 1: 控制云台但不开火，2: 控制云台且开火
   float yaw;
   float yaw_vel;

src/device/hikrobot/hikrobot.cpp

@@ -138,35 +138,8 @@ void HikRobot::capture_start()
         {PixelType_Gvsp_BayerRG8, cv::COLOR_BayerRG2RGB},
         {PixelType_Gvsp_BayerGB8, cv::COLOR_BayerGB2RGB},
         {PixelType_Gvsp_BayerBG8, cv::COLOR_BayerBG2RGB}};
-
+      cv::cvtColor(img, dst_image, type_map.at(pixel_type));
-      auto it = type_map.find(pixel_type);
+      img = dst_image;
-      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});
 

src/task/auto_aim_debug_mpc.cpp

@@ -23,7 +23,7 @@ using namespace std::chrono_literals;
 
 const std::string keys =
   "{help h usage ? |                        | 输出命令行参数说明}"
-  "{@config-path   | configs/standard3.yaml | 位置参数，yaml配置文件路径 }";
+  "{@config-path   | configs/sentry.yaml | 位置参数，yaml配置文件路径 }";
 
 int main(int argc, char * argv[])
 {

uninstall-berity.sh

@@ -1,273 +0,0 @@
-#!/usr/bin/env bash
-
-set -u
-
-TARGET="berity"
-ASSUME_YES=0
-DRY_RUN=0
-REMOVED_ANY=0
-
-usage() {
-  cat <<'EOF'
-Usage:
-  uninstall-berity.sh [--yes] [--dry-run] [target_name]
-
-Examples:
-  ./uninstall-berity.sh
-  ./uninstall-berity.sh --yes
-  ./uninstall-berity.sh --dry-run berity
-EOF
-}
-
-log() {
-  printf '%s\n' "$*"
-}
-
-run() {
-  if [ "$DRY_RUN" -eq 1 ]; then
-    printf '[DRY-RUN] %q ' "$@"
-    printf '\n'
-    return 0
-  fi
-  "$@"
-}
-
-run_root() {
-  if [ "$(id -u)" -eq 0 ]; then
-    run "$@"
-  else
-    run sudo "$@"
-  fi
-}
-
-is_cmd() {
-  command -v "$1" >/dev/null 2>&1
-}
-
-parse_args() {
-  while [ "$#" -gt 0 ]; do
-    case "$1" in
-      --yes|-y)
-        ASSUME_YES=1
-        ;;
-      --dry-run|-n)
-        DRY_RUN=1
-        ;;
-      --help|-h)
-        usage
-        exit 0
-        ;;
-      *)
-        TARGET="$1"
-        ;;
-    esac
-    shift
-  done
-}
-
-confirm() {
-  if [ "$ASSUME_YES" -eq 1 ]; then
-    return 0
-  fi
-  printf 'This will uninstall "%s" and clean common leftovers. Continue? [y/N] ' "$TARGET"
-  read -r answer
-  case "$answer" in
-    y|Y|yes|YES) return 0 ;;
-    *) return 1 ;;
-  esac
-}
-
-remove_file_if_exists() {
-  local path="$1"
-  if [ -e "$path" ] || [ -L "$path" ]; then
-    log "[remove] $path"
-    run_root rm -rf "$path" && REMOVED_ANY=1
-  fi
-}
-
-uninstall_apt() {
-  is_cmd apt-get || return 0
-  local pkg
-  for pkg in "$@"; do
-    if dpkg -s "$pkg" >/dev/null 2>&1; then
-      log "[apt] removing $pkg"
-      run_root apt-get remove --purge -y "$pkg" && REMOVED_ANY=1
-    fi
-  done
-}
-
-uninstall_dnf_or_yum() {
-  local pkg mgr
-  if is_cmd dnf; then
-    mgr="dnf"
-  elif is_cmd yum; then
-    mgr="yum"
-  else
-    return 0
-  fi
-  for pkg in "$@"; do
-    if is_cmd rpm && rpm -q "$pkg" >/dev/null 2>&1; then
-      log "[$mgr] removing $pkg"
-      run_root "$mgr" remove -y "$pkg" && REMOVED_ANY=1
-    fi
-  done
-}
-
-uninstall_pacman() {
-  is_cmd pacman || return 0
-  local pkg
-  for pkg in "$@"; do
-    if pacman -Q "$pkg" >/dev/null 2>&1; then
-      log "[pacman] removing $pkg"
-      run_root pacman -Rns --noconfirm "$pkg" && REMOVED_ANY=1
-    fi
-  done
-}
-
-uninstall_zypper() {
-  is_cmd zypper || return 0
-  local pkg
-  for pkg in "$@"; do
-    if is_cmd rpm && rpm -q "$pkg" >/dev/null 2>&1; then
-      log "[zypper] removing $pkg"
-      run_root zypper --non-interactive rm "$pkg" && REMOVED_ANY=1
-    fi
-  done
-}
-
-uninstall_snap() {
-  is_cmd snap || return 0
-  local pkg
-  for pkg in "$@"; do
-    if snap list 2>/dev/null | awk '{print $1}' | grep -Fxq "$pkg"; then
-      log "[snap] removing $pkg"
-      run_root snap remove "$pkg" && REMOVED_ANY=1
-    fi
-  done
-}
-
-uninstall_flatpak() {
-  is_cmd flatpak || return 0
-  local pkg
-  for pkg in "$@"; do
-    if flatpak list --app --columns=application 2>/dev/null | grep -Fxq "$pkg"; then
-      log "[flatpak user] removing $pkg"
-      run flatpak uninstall -y "$pkg" && REMOVED_ANY=1
-    fi
-    if flatpak list --system --app --columns=application 2>/dev/null | grep -Fxq "$pkg"; then
-      log "[flatpak system] removing $pkg"
-      run_root flatpak uninstall --system -y "$pkg" && REMOVED_ANY=1
-    fi
-  done
-}
-
-uninstall_pip() {
-  local pkg
-  if is_cmd pip3; then
-    for pkg in "$@"; do
-      if pip3 show "$pkg" >/dev/null 2>&1; then
-        log "[pip3] removing $pkg"
-        run pip3 uninstall -y "$pkg" && REMOVED_ANY=1
-      fi
-    done
-  fi
-  if is_cmd python3; then
-    for pkg in "$@"; do
-      if python3 -m pip show "$pkg" >/dev/null 2>&1; then
-        log "[python3 -m pip] removing $pkg"
-        run python3 -m pip uninstall -y "$pkg" && REMOVED_ANY=1
-      fi
-    done
-  fi
-}
-
-uninstall_npm() {
-  is_cmd npm || return 0
-  local pkg
-  for pkg in "$@"; do
-    if npm -g ls --depth=0 "$pkg" >/dev/null 2>&1; then
-      log "[npm global] removing $pkg"
-      run npm -g uninstall "$pkg" && REMOVED_ANY=1
-    fi
-  done
-}
-
-remove_systemd_units() {
-  is_cmd systemctl || return 0
-  local unit
-  for unit in "$@"; do
-    if systemctl list-unit-files 2>/dev/null | awk '{print $1}' | grep -Fxq "$unit"; then
-      log "[systemd] disabling/stopping $unit"
-      run_root systemctl disable --now "$unit" || true
-      REMOVED_ANY=1
-    fi
-  done
-}
-
-main() {
-  parse_args "$@"
-
-  local packages=(
-    "$TARGET"
-    "${TARGET}-cli"
-    "${TARGET}-agent"
-    "${TARGET}-desktop"
-    "com.${TARGET}.app"
-  )
-
-  local units=(
-    "${TARGET}.service"
-    "${TARGET}-agent.service"
-    "${TARGET}-server.service"
-  )
-
-  if ! confirm; then
-    log "Cancelled."
-    exit 1
-  fi
-
-  uninstall_apt "${packages[@]}"
-  uninstall_dnf_or_yum "${packages[@]}"
-  uninstall_pacman "${packages[@]}"
-  uninstall_zypper "${packages[@]}"
-  uninstall_snap "${packages[@]}"
-  uninstall_flatpak "${packages[@]}"
-  uninstall_pip "${packages[@]}"
-  uninstall_npm "${packages[@]}"
-
-  remove_systemd_units "${units[@]}"
-
-  remove_file_if_exists "/usr/local/bin/$TARGET"
-  remove_file_if_exists "/usr/bin/$TARGET"
-  remove_file_if_exists "$HOME/.local/bin/$TARGET"
-  remove_file_if_exists "/etc/systemd/system/${TARGET}.service"
-  remove_file_if_exists "/etc/systemd/system/${TARGET}-agent.service"
-  remove_file_if_exists "/opt/$TARGET"
-  remove_file_if_exists "/etc/$TARGET"
-  remove_file_if_exists "/var/lib/$TARGET"
-  remove_file_if_exists "/var/log/$TARGET"
-  remove_file_if_exists "$HOME/.${TARGET}"
-  remove_file_if_exists "$HOME/.config/$TARGET"
-  remove_file_if_exists "$HOME/.cache/$TARGET"
-  remove_file_if_exists "$HOME/.local/share/$TARGET"
-
-  if is_cmd systemctl; then
-    run_root systemctl daemon-reload || true
-  fi
-
-  if command -v "$TARGET" >/dev/null 2>&1; then
-    log
-    log "Uninstall attempted, but '$TARGET' is still on PATH: $(command -v "$TARGET")"
-    exit 2
-  fi
-
-  if [ "$REMOVED_ANY" -eq 1 ]; then
-    log
-    log "Uninstall completed for target '$TARGET'."
-  else
-    log
-    log "Nothing found to remove for target '$TARGET'."
-  fi
-}
-
-main "$@"