diff --git a/CMakeLists.txt b/CMakeLists.txt index fde7aaf..0488663 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/configs/calibration.yaml b/configs/calibration.yaml index 20d0f1d..852d375 100644 --- a/configs/calibration.yaml +++ b/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 -gain: 10.0 +exposure_ms: 30 +gain: 16.0 vid_pid: "2bdf:0001" #####-----cboard参数-----##### @@ -20,6 +20,6 @@ can_interface: "can0" 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] +# 重投影误差: 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] diff --git a/configs/hero.yaml b/configs/hero.yaml new file mode 100644 index 0000000..984fff3 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/tools/plot_receiver.py b/tools/plot_receiver.py new file mode 100644 index 0000000..fea6825 --- /dev/null +++ b/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()