diff --git a/CMakeLists.txt b/CMakeLists.txt index b9cb30c..0488663 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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() diff --git a/README.md b/README.md index ed0285b..17404cd 100644 --- a/README.md +++ b/README.md @@ -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） diff --git a/configs/sentry.yaml b/configs/sentry.yaml index cb75fdc..6c8b88f 100644 --- a/configs/sentry.yaml +++ b/configs/sentry.yaml @@ -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 \ No newline at end of file diff --git a/src/task/auto_aim_debug_mpc_ros.cpp b/src/task/auto_aim_debug_mpc_ros.cpp new file mode 100644 index 0000000..6de62f5 --- /dev/null +++ b/src/task/auto_aim_debug_mpc_ros.cpp @@ -0,0 +1,185 @@ +#ifdef USE_ROS2 + +#include + +#include +#include +#include +#include +#include +#include + +#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(0); + if (cli.has("help") || config_path.empty()) { + cli.printMessage(); + return 0; + } + + // 初始化ROS2 + rclcpp::init(argc, argv); + auto node = std::make_shared("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, true> target_queue(1); + target_queue.push(std::nullopt); + + std::atomic 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 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 + +int main() +{ + std::cerr << "Error: This program requires ROS2 support. Please build with ROS2 enabled." << std::endl; + return 1; +} + +#endif // USE_ROS2