add hero
This commit is contained in:
parent
8ff5ff5be9
commit
dfbc04346f
@ -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)
|
||||
|
||||
@ -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]
|
||||
|
||||
105
configs/hero.yaml
Normal file
105
configs/hero.yaml
Normal file
@ -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
|
||||
153
tools/plot_receiver.py
Normal file
153
tools/plot_receiver.py
Normal file
@ -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()
|
||||
Loading…
Reference in New Issue
Block a user