robofish/MOVE_AI
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: 添加ROS2版本的自瞄调试程序和配置优化

Browse Source 
- 新增auto_aim_debug_mpc_ros程序，使用ROS2通信
  - 支持实时可视化装甲板重投影
  - 支持数据绘图和调试
  - 完全兼容原版auto_aim_debug_mpc功能
- 更新CMakeLists.txt，添加auto_aim_debug_mpc_ros编译配置
- 更新README.md，补充ROS2调试程序说明
- 修复configs/sentry.yaml配置文件
  - 修正打符模型路径
  - 添加fire_thresh参数

Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
This commit is contained in:
Robofish 2026-03-07 17:32:47 +08:00
parent 0a6fd76f0d
commit 92ca4a5871
4 changed files with 249 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

@ -145,5 +145,9 @@ 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()

4
README.md
View File

@ -78,6 +78,7 @@ 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
```
@ -98,11 +99,12 @@ make -C build/ -j$(nproc)
| `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` 需要ROS2环境，只在检测到ROS2时才会编译。
**注意**：`sentry_mpc` 和 `auto_aim_debug_mpc_ros` 需要ROS2环境，只在检测到ROS2时才会编译。
### 标定工具（calibration）

106
configs/sentry.yaml
View File

@ -1,14 +1,13 @@
# 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
device: CPU
min_confidence: 0.8
use_traditional: true
@ -21,24 +20,6 @@ roi:
use_roi: false
#####-----USB相机参数-----#####
image_width: 1280
image_height: 720
fov_h: 57.7 #87.7
fov_v: 56.7
new_fov_h: 27 #67
new_fov_v: 40.9
usb_frame_rate: 120
usb_exposure: 500 #1-80000______250
usb_gamma: 160
usb_gain: 10 #0-96
#####-----工业相机参数-----#####
camera_name: "hikrobot"
exposure_ms: 0.8
gain: 16.9
vid_pid: "2bdf:0001"
#####-----传统方法参数-----#####
threshold: 150
max_angle_error: 45 # degree
@ -49,48 +30,75 @@ 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: 25
max_temp_lost_count: 15
outpost_max_temp_lost_count: 75
#####-----aimer参数-----#####
yaw_offset: -0.8 # degree -2.5
pitch_offset: -1 # degree 2
comming_angle: 60 # degree
yaw_offset: 2 # degree -2.5
pitch_offset: 6.5 # degree 2
comming_angle: 55 # degree
leaving_angle: 20 # degree
left_yaw_offset: -1
right_yaw_offset: -0.6
decision_speed: 10 # rad/s
high_speed_delay_time: 0.026 # s
low_speed_delay_time: 0.010 # s
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: 5 # 近距离射击容差，degree
first_tolerance: 3 # 近距离射击容差，degree
second_tolerance: 2 # 远距离射击容差，degree
judge_distance: 3 #距离判断阈值
judge_distance: 2 #距离判断阈值
auto_fire: true # 是否由自瞄控制射击
#####-----decider参数-----#####
mode: 1
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.1833px
camera_matrix: [2414.9359264386621, 0, 717.26243105567414, 0, 2418.0489262208148, 582.68540529942845, 0, 0, 1]
distort_coeffs: [-0.0209453389287673, 0.15028138841073832, -0.0006517722113234505, -0.0016861906197686788, 0]
# 重投影误差: 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]
# 相机同理想情况的偏角: yaw-1.11 pitch0.01 roll-0.06 degree
# 标定板到世界坐标系原点的水平距离: 1.20 m
# 标定板同竖直摆放时的偏角: yaw123.89 pitch14.05 roll-0.86 degree
R_camera2gimbal: [0.01928451708725664, 0.00012696140743255463, 0.99981402834802846, -0.99981344688553653, -0.0010834913551969569, 0.019284643459122196, 0.0010857382619952124, -0.99999940496346484, 0.00010604311483339372]
t_camera2gimbal: [0.13089617453251859, 0.0038468007459533785, 0.094139945222010288]
# 相机同理想情况的偏角: 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

185
src/task/auto_aim_debug_mpc_ros.cpp Normal file
View File

@ -0,0 +1,185 @@
#ifdef USE_ROS2
#include <fmt/core.h>
#include <atomic>
#include <chrono>
#include <memory>
#include <nlohmann/json.hpp>
#include <opencv2/opencv.hpp>
#include <thread>
#include "rclcpp/rclcpp.hpp"
#include "src/device/camera.hpp"
#include "src/device/gimbal/gimbal_ros.hpp"
#include "src/module/auto_aim/planner/planner.hpp"
#include "src/module/auto_aim/solver.hpp"
#include "src/module/auto_aim/tracker.hpp"
#include "src/module/auto_aim/yolo.hpp"
#include "src/component/exiter.hpp"
#include "src/component/img_tools.hpp"
#include "src/component/logger.hpp"
#include "src/component/math_tools.hpp"
#include "src/component/plotter.hpp"
#include "src/component/thread_safe_queue.hpp"
using namespace std::chrono_literals;
const std::string keys =
"{help h usage ? | | 输出命令行参数说明}"
"{@config-path | configs/standard3.yaml | 位置参数，yaml配置文件路径 }";
int main(int argc, char * argv[])
{
component::Exiter exiter;
component::Plotter plotter;
cv::CommandLineParser cli(argc, argv, keys);
auto config_path = cli.get<std::string>(0);
if (cli.has("help") || config_path.empty()) {
cli.printMessage();
return 0;
}
// 初始化ROS2
rclcpp::init(argc, argv);
auto node = std::make_shared<rclcpp::Node>("auto_aim_debug_mpc_ros");
device::GimbalROS gimbal(config_path, node);
device::Camera camera(config_path);
auto_aim::YOLO yolo(config_path, true);
auto_aim::Solver solver(config_path);
auto_aim::Tracker tracker(config_path, solver);
auto_aim::Planner planner(config_path);
component::ThreadSafeQueue<std::optional<auto_aim::Target>, true> target_queue(1);
target_queue.push(std::nullopt);
std::atomic<bool> quit = false;
// 规划线程
auto plan_thread = std::thread([&]() {
auto t0 = std::chrono::steady_clock::now();
uint16_t last_bullet_count = 0;
while (!quit) {
auto target = target_queue.front();
auto gs = gimbal.state();
auto plan = planner.plan(target, gs.bullet_speed);
gimbal.send(
plan.control, plan.fire, plan.yaw, plan.yaw_vel, plan.yaw_acc, plan.pitch, plan.pitch_vel,
plan.pitch_acc);
auto fired = gs.bullet_count > last_bullet_count;
last_bullet_count = gs.bullet_count;
nlohmann::json data;
data["t"] = component::delta_time(std::chrono::steady_clock::now(), t0);
data["gimbal_yaw"] = gs.yaw;
data["gimbal_yaw_vel"] = gs.yaw_vel;
data["gimbal_pitch"] = gs.pitch;
data["gimbal_pitch_vel"] = gs.pitch_vel;
data["target_yaw"] = plan.target_yaw;
data["target_pitch"] = plan.target_pitch;
data["plan_yaw"] = plan.yaw;
data["plan_yaw_vel"] = plan.yaw_vel;
data["plan_yaw_acc"] = plan.yaw_acc;
data["plan_pitch"] = plan.pitch;
data["plan_pitch_vel"] = plan.pitch_vel;
data["plan_pitch_acc"] = plan.pitch_acc;
data["fire"] = plan.fire ? 1 : 0;
data["fired"] = fired ? 1 : 0;
if (target.has_value()) {
data["target_z"] = target->ekf_x()[4]; //z
data["target_vz"] = target->ekf_x()[5]; //vz
}
if (target.has_value()) {
data["w"] = target->ekf_x()[7];
} else {
data["w"] = 0.0;
}
plotter.plot(data);
std::this_thread::sleep_for(10ms);
}
});
// ROS2 spin线程
auto ros_thread = std::thread([&]() {
while (!quit && rclcpp::ok()) {
rclcpp::spin_some(node);
std::this_thread::sleep_for(1ms);
}
});
component::logger()->info("[AutoAimDebugMPC_ROS] Started with ROS2 communication");
cv::Mat img;
std::chrono::steady_clock::time_point t;
while (!exiter.exit() && rclcpp::ok()) {
camera.read(img, t);
auto q = gimbal.q(t);
solver.set_R_gimbal2world(q);
auto armors = yolo.detect(img);
auto targets = tracker.track(armors, t);
if (!targets.empty())
target_queue.push(targets.front());
else
target_queue.push(std::nullopt);
if (!targets.empty()) {
auto target = targets.front();
// 当前帧target更新后
std::vector<Eigen::Vector4d> armor_xyza_list = target.armor_xyza_list();
for (const Eigen::Vector4d & xyza : armor_xyza_list) {
auto image_points =
solver.reproject_armor(xyza.head(3), xyza[3], target.armor_type, target.name);
component::draw_points(img, image_points, {0, 255, 0});
}
Eigen::Vector4d aim_xyza = planner.debug_xyza;
auto image_points =
solver.reproject_armor(aim_xyza.head(3), aim_xyza[3], target.armor_type, target.name);
component::draw_points(img, image_points, {0, 0, 255});
}
cv::resize(img, img, {}, 0.5, 0.5); // 显示时缩小图片尺寸
cv::imshow("reprojection", img);
auto key = cv::waitKey(1);
if (key == 'q') break;
}
quit = true;
if (plan_thread.joinable()) plan_thread.join();
if (ros_thread.joinable()) ros_thread.join();
gimbal.send(false, false, 0, 0, 0, 0, 0, 0);
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