add hero

CMakeLists.txt

@@ -74,7 +74,6 @@ 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(gimbal src/)
 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)

configs/calibration.yaml

@@ -7,8 +7,8 @@ R_gimbal2imubody: [1, 0, 0, 0, 1, 0,  0, 0, 1]
 
 rotate_180: false
 camera_name: "hikrobot"
-exposure_ms: 3
+exposure_ms: 30
-gain: 10.0
+gain: 16.0
 vid_pid: "2bdf:0001"
 
 #####-----cboard参数-----#####
@@ -20,6 +20,6 @@ can_interface: "can0"
 com_port: "/dev/ttyUSB0"
 baudrate: 115200
 
-# 重投影误差: 0.1791px
+# 重投影误差: 0.0654px
-camera_matrix: [1827.8294221039337, 0, 716.86057740384501, 0, 1828.9736207357851, 613.69509305531699, 0, 0, 1]
+camera_matrix: [1794.7763123680743, 0, 762.48990119722805, 0, 1794.6747019516008, 546.1981535799863, 0, 0, 1]
-distort_coeffs: [-0.083642708058668358, 0.18891600176175308, -0.00030362184648520616, -0.00066798903909152669, 0]
+distort_coeffs: [-0.07577568384219846, 0.14361612515698613, 0.00057329040489542368, 0.0001190829309863327, 0]

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: 0 # degree -2.5
+pitch_offset: 0 # 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.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

tools/plot_receiver.py

@@ -0,0 +1,153 @@
+#!/usr/bin/env python3
+"""
+UDP JSON 波形接收器 - 用于 auto_aim_debug_mpc 实时调参
+监听 127.0.0.1:9870，实时绘制 MPC 规划数据波形
+
+用法: python3 tools/plot_receiver.py
+"""
+
+import socket
+import json
+import threading
+import time
+from collections import deque
+
+import matplotlib
+matplotlib.use('TkAgg')
+import matplotlib.pyplot as plt
+from matplotlib.animation import FuncAnimation
+
+# ========== 配置 ==========
+HOST = "127.0.0.1"
+PORT = 9870
+MAX_POINTS = 500  # 最多显示的数据点数
+
+# 定义要绘制的子图和曲线
+# 每个子图: (标题, [(数据key, 显示名, 颜色), ...])
+PLOT_CONFIG = [
+    ("Yaw (rad)", [
+        ("gimbal_yaw",  "gimbal_yaw",  "tab:blue"),
+        ("target_yaw",  "target_yaw",  "tab:orange"),
+        ("plan_yaw",    "plan_yaw",    "tab:red"),
+    ]),
+    ("Yaw Velocity (rad/s)", [
+        ("gimbal_yaw_vel", "gimbal_yaw_vel", "tab:blue"),
+        ("plan_yaw_vel",   "plan_yaw_vel",   "tab:red"),
+    ]),
+    ("Yaw Acceleration (rad/s²)", [
+        ("plan_yaw_acc", "plan_yaw_acc", "tab:red"),
+    ]),
+    ("Pitch (rad)", [
+        ("gimbal_pitch",  "gimbal_pitch",  "tab:blue"),
+        ("target_pitch",  "target_pitch",  "tab:orange"),
+        ("plan_pitch",    "plan_pitch",    "tab:red"),
+    ]),
+    ("Pitch Velocity (rad/s)", [
+        ("gimbal_pitch_vel", "gimbal_pitch_vel", "tab:blue"),
+        ("plan_pitch_vel",   "plan_pitch_vel",   "tab:red"),
+    ]),
+    ("Pitch Acceleration (rad/s²)", [
+        ("plan_pitch_acc", "plan_pitch_acc", "tab:red"),
+    ]),
+    ("Fire & Target", [
+        ("fire",  "fire_cmd",  "tab:green"),
+        ("fired", "fired",     "tab:red"),
+    ]),
+    ("Target State", [
+        ("w",         "omega (rad/s)", "tab:purple"),
+        ("target_z",  "z (m)",         "tab:cyan"),
+        ("target_vz", "vz (m/s)",      "tab:orange"),
+    ]),
+]
+
+# ========== 数据存储 ==========
+data_lock = threading.Lock()
+time_buf = deque(maxlen=MAX_POINTS)
+buffers = {}  # key -> deque
+
+for _, curves in PLOT_CONFIG:
+    for key, _, _ in curves:
+        if key not in buffers:
+            buffers[key] = deque(maxlen=MAX_POINTS)
+
+
+def udp_listener():
+    """后台线程：接收 UDP JSON 数据"""
+    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+    sock.bind((HOST, PORT))
+    sock.settimeout(1.0)
+    print(f"[plot_receiver] 监听 {HOST}:{PORT} ...")
+
+    while True:
+        try:
+            raw, _ = sock.recvfrom(65536)
+            msg = json.loads(raw.decode("utf-8"))
+        except socket.timeout:
+            continue
+        except Exception as e:
+            print(f"[plot_receiver] 解析错误: {e}")
+            continue
+
+        with data_lock:
+            t = msg.get("t", 0.0)
+            time_buf.append(t)
+            for key, buf in buffers.items():
+                buf.append(msg.get(key, float('nan')))
+
+
+# ========== 绘图 ==========
+def main():
+    # 启动 UDP 接收线程
+    listener = threading.Thread(target=udp_listener, daemon=True)
+    listener.start()
+
+    n_plots = len(PLOT_CONFIG)
+    n_cols = 2
+    n_rows = (n_plots + 1) // 2
+
+    fig, axes = plt.subplots(n_rows, n_cols, figsize=(16, 3 * n_rows), squeeze=False)
+    fig.suptitle("auto_aim_debug_mpc  Real-time Plot", fontsize=14)
+    plt.subplots_adjust(hspace=0.45, wspace=0.25)
+
+    lines = {}  # (subplot_idx, key) -> line object
+
+    for idx, (title, curves) in enumerate(PLOT_CONFIG):
+        ax = axes[idx // n_cols][idx % n_cols]
+        ax.set_title(title, fontsize=10)
+        ax.set_xlabel("t (s)", fontsize=8)
+        ax.grid(True, alpha=0.3)
+        for key, label, color in curves:
+            ln, = ax.plot([], [], label=label, color=color, linewidth=1.2)
+            lines[(idx, key)] = ln
+        ax.legend(fontsize=7, loc="upper left")
+
+    # 隐藏多余子图
+    for idx in range(n_plots, n_rows * n_cols):
+        axes[idx // n_cols][idx % n_cols].set_visible(False)
+
+    def update(_frame):
+        with data_lock:
+            t_list = list(time_buf)
+            snap = {k: list(v) for k, v in buffers.items()}
+
+        if len(t_list) < 2:
+            return lines.values()
+
+        for idx, (_, curves) in enumerate(PLOT_CONFIG):
+            ax = axes[idx // n_cols][idx % n_cols]
+            for key, _, _ in curves:
+                ln = lines[(idx, key)]
+                y = snap.get(key, [])
+                ln.set_data(t_list[:len(y)], y)
+            ax.relim()
+            ax.autoscale_view()
+
+        return lines.values()
+
+    _ani = FuncAnimation(fig, update, interval=50, blit=False, cache_frame_data=False)
+    plt.show()
+
+
+if __name__ == "__main__":
+    main()