From 59a70720029656551caf9929c70af8cbfd0961a2 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 1 Mar 2026 02:43:03 +0800 Subject: [PATCH] add task --- CMakeLists.txt | 68 ++++-- README.md | 139 ++++++++++++- src/task/.gitkeep | 0 src/task/auto_aim_debug_mpc.cpp | 150 ++++++++++++++ src/task/auto_buff_debug.cpp | 158 ++++++++++++++ src/task/auto_buff_debug_mpc.cpp | 165 +++++++++++++++ src/task/mt_auto_aim_debug.cpp | 173 ++++++++++++++++ src/task/mt_standard.cpp | 138 ++++++++++++ src/task/sentry.cpp | 106 ++++++++++ src/task/sentry_bp.cpp | 104 ++++++++++ src/task/sentry_debug.cpp | 196 ++++++++++++++++++ src/task/sentry_multithread.cpp | 125 +++++++++++ src/task/standard.cpp | 83 ++++++++ src/task/standard_mpc.cpp | 145 +++++++++++++ src/task/{ => test}/auto_aim_test.cpp | 0 src/task/{ => test}/auto_buff_test.cpp | 0 src/task/{ => test}/camera_detect_test.cpp | 0 src/task/{ => test}/camera_test.cpp | 0 src/task/{ => test}/camera_thread_test.cpp | 0 src/task/{ => test}/cboard_test.cpp | 0 src/task/{ => test}/detector_video_test.cpp | 0 src/task/{ => test}/dm_test.cpp | 0 src/task/{ => test}/fire_test.cpp | 0 src/task/{ => test}/gimbal_response_test.cpp | 0 src/task/{ => test}/gimbal_test.cpp | 0 src/task/{ => test}/handeye_test.cpp | 0 src/task/{ => test}/minimum_vision_system.cpp | 0 src/task/{ => test}/multi_usbcamera_test.cpp | 0 src/task/{ => test}/planner_test.cpp | 0 src/task/{ => test}/planner_test_offline.cpp | 0 src/task/{ => test}/publish_test.cpp | 0 src/task/{ => test}/subscribe_test.cpp | 0 src/task/{ => test}/topic_loop_test.cpp | 0 src/task/{ => test}/usbcamera_detect_test.cpp | 0 src/task/{ => test}/usbcamera_test.cpp | 0 src/task/uav.cpp | 120 +++++++++++ src/task/uav_debug.cpp | 173 ++++++++++++++++ 37 files changed, 2022 insertions(+), 21 deletions(-) delete mode 100644 src/task/.gitkeep create mode 100644 src/task/auto_aim_debug_mpc.cpp create mode 100755 src/task/auto_buff_debug.cpp create mode 100755 src/task/auto_buff_debug_mpc.cpp create mode 100644 src/task/mt_auto_aim_debug.cpp create mode 100644 src/task/mt_standard.cpp create mode 100644 src/task/sentry.cpp create mode 100644 src/task/sentry_bp.cpp create mode 100644 src/task/sentry_debug.cpp create mode 100644 src/task/sentry_multithread.cpp create mode 100644 src/task/standard.cpp create mode 100644 src/task/standard_mpc.cpp rename src/task/{ => test}/auto_aim_test.cpp (100%) rename src/task/{ => test}/auto_buff_test.cpp (100%) rename src/task/{ => test}/camera_detect_test.cpp (100%) rename src/task/{ => test}/camera_test.cpp (100%) rename src/task/{ => test}/camera_thread_test.cpp (100%) rename src/task/{ => test}/cboard_test.cpp (100%) rename src/task/{ => test}/detector_video_test.cpp (100%) rename src/task/{ => test}/dm_test.cpp (100%) rename src/task/{ => test}/fire_test.cpp (100%) rename src/task/{ => test}/gimbal_response_test.cpp (100%) rename src/task/{ => test}/gimbal_test.cpp (100%) rename src/task/{ => test}/handeye_test.cpp (100%) rename src/task/{ => test}/minimum_vision_system.cpp (100%) rename src/task/{ => test}/multi_usbcamera_test.cpp (100%) rename src/task/{ => test}/planner_test.cpp (100%) rename src/task/{ => test}/planner_test_offline.cpp (100%) rename src/task/{ => test}/publish_test.cpp (100%) rename src/task/{ => test}/subscribe_test.cpp (100%) rename src/task/{ => test}/topic_loop_test.cpp (100%) rename src/task/{ => test}/usbcamera_detect_test.cpp (100%) rename src/task/{ => test}/usbcamera_test.cpp (100%) create mode 100644 src/task/uav.cpp create mode 100644 src/task/uav_debug.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 43df713..a6436df 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -42,21 +42,21 @@ target_link_libraries(calibrate_robotworld_handeye ${OpenCV_LIBS} fmt::fmt yaml- target_link_libraries(split_video ${OpenCV_LIBS} fmt::fmt component) ##################tests################## -add_executable(auto_aim_test src/task/auto_aim_test.cpp) -add_executable(auto_buff_test src/task/auto_buff_test.cpp) -add_executable(camera_detect_test src/task/camera_detect_test.cpp) -add_executable(camera_test src/task/camera_test.cpp) -add_executable(camera_thread_test src/task/camera_thread_test.cpp) -add_executable(cboard_test src/task/cboard_test.cpp) -add_executable(fire_test src/task/fire_test.cpp) -add_executable(detector_video_test src/task/detector_video_test.cpp) -add_executable(gimbal_response_test src/task/gimbal_response_test.cpp) -add_executable(multi_usbcamera_test src/task/multi_usbcamera_test.cpp) -add_executable(usbcamera_detect_test src/task/usbcamera_detect_test.cpp) -add_executable(usbcamera_test src/task/usbcamera_test.cpp) -add_executable(handeye_test src/task/handeye_test.cpp) -add_executable(dm_test src/task/dm_test.cpp) -add_executable(minimum_vision_system src/task/minimum_vision_system.cpp) +add_executable(auto_aim_test src/task/test/auto_aim_test.cpp) +add_executable(auto_buff_test src/task/test/auto_buff_test.cpp) +add_executable(camera_detect_test src/task/test/camera_detect_test.cpp) +add_executable(camera_test src/task/test/camera_test.cpp) +add_executable(camera_thread_test src/task/test/camera_thread_test.cpp) +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(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) +add_executable(handeye_test src/task/test/handeye_test.cpp) +add_executable(dm_test src/task/test/dm_test.cpp) +add_executable(minimum_vision_system src/task/test/minimum_vision_system.cpp) target_link_libraries(auto_aim_test ${OpenCV_LIBS} fmt::fmt yaml-cpp component device auto_aim) target_link_libraries(auto_buff_test ${OpenCV_LIBS} fmt::fmt yaml-cpp auto_buff component device) @@ -74,11 +74,41 @@ target_link_libraries(handeye_test ${OpenCV_LIBS} fmt::fmt yaml-cpp component de target_link_libraries(dm_test ${OpenCV_LIBS} fmt::fmt yaml-cpp component device) target_link_libraries(minimum_vision_system ${OpenCV_LIBS} fmt::fmt yaml-cpp component device auto_aim) -add_executable(gimbal_test src/task/gimbal_test.cpp) +add_executable(gimbal_test src/task/test/gimbal_test.cpp) target_link_libraries(gimbal_test ${OpenCV_LIBS} fmt::fmt yaml-cpp auto_aim component device) -add_executable(planner_test src/task/planner_test.cpp) +add_executable(planner_test src/task/test/planner_test.cpp) target_link_libraries(planner_test ${OpenCV_LIBS} fmt::fmt yaml-cpp auto_aim component device) -add_executable(planner_test_offline src/task/planner_test_offline.cpp) -target_link_libraries(planner_test_offline ${OpenCV_LIBS} fmt::fmt yaml-cpp auto_aim component device) \ No newline at end of file +add_executable(planner_test_offline src/task/test/planner_test_offline.cpp) +target_link_libraries(planner_test_offline ${OpenCV_LIBS} fmt::fmt yaml-cpp auto_aim component device) + +################## new tasks (no ROS2) ################## +add_executable(auto_aim_debug_mpc src/task/auto_aim_debug_mpc.cpp) +target_link_libraries(auto_aim_debug_mpc ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann_json::nlohmann_json auto_aim component device) + +add_executable(auto_buff_debug src/task/auto_buff_debug.cpp) +target_link_libraries(auto_buff_debug ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann_json::nlohmann_json auto_aim auto_buff component device) + +add_executable(auto_buff_debug_mpc src/task/auto_buff_debug_mpc.cpp) +target_link_libraries(auto_buff_debug_mpc ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann_json::nlohmann_json auto_aim auto_buff component device) + +add_executable(mt_auto_aim_debug src/task/mt_auto_aim_debug.cpp) +target_link_libraries(mt_auto_aim_debug ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann_json::nlohmann_json auto_aim component device) + +add_executable(mt_standard src/task/mt_standard.cpp) +target_link_libraries(mt_standard ${OpenCV_LIBS} fmt::fmt yaml-cpp auto_aim auto_buff component device) + +add_executable(standard src/task/standard.cpp) +target_link_libraries(standard ${OpenCV_LIBS} fmt::fmt yaml-cpp auto_aim component device) + +add_executable(standard_mpc src/task/standard_mpc.cpp) +target_link_libraries(standard_mpc ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann_json::nlohmann_json auto_aim auto_buff component device) + +add_executable(uav src/task/uav.cpp) +target_link_libraries(uav ${OpenCV_LIBS} fmt::fmt yaml-cpp auto_aim auto_buff component device) + +add_executable(uav_debug src/task/uav_debug.cpp) +target_link_libraries(uav_debug ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann_json::nlohmann_json auto_aim auto_buff component device) + + diff --git a/README.md b/README.md index 721b88f..9f8037a 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,137 @@ -# 通用自瞄 -适用于单一云台的自瞄 \ No newline at end of file +# MOVE_AI + +适用于 RoboMaster 机器人的视觉自瞄系统，参考同济大学 Superpower 战队 25 年开源设计，适配 MOVE。 + +## 项目结构 + +``` +├── calibration/ # 标定工具 +├── configs/ # 配置文件（yaml） +├── src/ +│ ├── component/ # 通用组件（EKF、弹道、日志、绘图等） +│ ├── device/ # 设备驱动（相机、串口、CAN、IMU） +│ ├── module/ +│ │ ├── auto_aim/ # 自瞄模块（检测、解算、跟踪、瞄准、规划） +│ │ ├── auto_buff/ # 打符模块 +│ │ └── omniperception/ # 全向感知模块（哨兵用） +│ └── task/ +│ ├── *.cpp # 各兵种主程序 +│ └── test/ # 测试用例 +``` + +## 环境要求 + +- Ubuntu 22.04 +- 运算平台：Intel NUC（i7-1260P / i7-1165G7） +- 相机：海康 MV-CS016-10UC + 6mm 镜头 +- 下位机：RoboMaster C 型开发板（STM32F407）/ 达妙 MC02（STM32H7） + +## 依赖安装 + +1. SDK： + - [HikRobot MVS SDK](https://www.hikrobotics.com/cn2/source/support/software/MVS_STD_GML_V2.1.2_231116.zip) + - [MindVision SDK](https://mindvision.com.cn/category/software/sdk-installation-package/)（可选） + - [OpenVINO 2024](https://docs.openvino.ai/2024/get-started/install-openvino/install-openvino-archive-linux.html) + - [Ceres Solver](http://ceres-solver.org/installation.html) + +2. 系统依赖： + ```bash + sudo apt install -y \ + git g++ cmake can-utils \ + libopencv-dev libfmt-dev libeigen3-dev \ + libspdlog-dev libyaml-cpp-dev libusb-1.0-0-dev \ + nlohmann-json3-dev openssh-server screen + ``` + +## 编译与运行 + +```bash +cmake -B build +make -C build/ -j$(nproc) +./build/auto_aim_test # 运行测试 +``` + +## 可执行目标 + +### 主程序（task） + +| 目标 | 说明 | 配置文件 | +|------|------|----------| +| `standard` | 步兵自瞄 | `configs/standard3.yaml` | +| `standard_mpc` | 步兵自瞄（MPC 规划） | 需指定 | +| `uav` | 无人机自瞄 + 打符 | `configs/uav.yaml` | +| `uav_debug` | 无人机调试（含可视化） | `configs/uav.yaml` | +| `mt_standard` | 多线程步兵 | 需指定 | +| `sentry` | 哨兵 | `configs/sentry.yaml` | +| `sentry_multithread` | 哨兵多线程 | `configs/sentry.yaml` | +| `sentry_debug` | 哨兵调试 | `configs/sentry.yaml` | +| `sentry_bp` | 哨兵（带弹道预测） | `configs/sentry.yaml` | +| `auto_aim_debug_mpc` | 自瞄 MPC 调试 | 需指定 | +| `auto_buff_debug` | 打符调试 | 需指定 | +| `auto_buff_debug_mpc` | 打符 MPC 调试 | 需指定 | +| `mt_auto_aim_debug` | 多线程自瞄调试 | 需指定 | + +### 标定工具（calibration） + +| 目标 | 说明 | +|------|------| +| `capture` | 采集标定图像 | +| `calibrate_camera` | 相机内参标定 | +| `calibrate_handeye` | 手眼标定 | +| `calibrate_robotworld_handeye` | 机器人-世界手眼标定 | +| `split_video` | 视频拆帧 | + +### 测试用例（test） + +| 目标 | 说明 | +|------|------| +| `auto_aim_test` | 自瞄全流程测试 | +| `auto_buff_test` | 打符全流程测试 | +| `camera_test` | 相机基础测试 | +| `camera_detect_test` | 相机 + 检测测试 | +| `camera_thread_test` | 相机多线程测试 | +| `cboard_test` | C 板通信测试 | +| `gimbal_test` | 云台通信测试 | +| `gimbal_response_test` | 云台响应测试 | +| `fire_test` | 发射测试 | +| `dm_test` | 达妙 IMU 测试 | +| `handeye_test` | 手眼标定测试 | +| `detector_video_test` | 离线视频检测测试 | +| `planner_test` | MPC 规划器测试 | +| `planner_test_offline` | MPC 规划器离线测试 | +| `usbcamera_test` | USB 相机测试 | +| `usbcamera_detect_test` | USB 相机 + 检测测试 | +| `multi_usbcamera_test` | 多 USB 相机测试 | +| `minimum_vision_system` | 最小视觉系统 | + +## 串口设置 + +1. 授予权限： + ```bash + sudo usermod -a -G dialout $USER + ``` + +2. 获取端口 ID（serial, idVendor, idProduct）： + ```bash + udevadm info -a -n /dev/ttyACM0 | grep -E '({serial}|{idVendor}|{idProduct})' + ``` + +3. 创建 udev 规则： + ```bash + sudo touch /etc/udev/rules.d/99-usb-serial.rules + ``` + 写入（用实际 ID 替换）： + ``` + SUBSYSTEM=="tty", ATTRS{idVendor}=="1234", ATTRS{idProduct}=="1234", ATTRS{serial}=="A1234567", SYMLINK+="gimbal" + ``` + +4. 重新加载规则： + ```bash + sudo udevadm control --reload-rules + sudo udevadm trigger + ``` + +5. 验证： + ```bash + ls -l /dev/gimbal + ``` diff --git a/src/task/.gitkeep b/src/task/.gitkeep deleted file mode 100644 index e69de29..0000000 diff --git a/src/task/auto_aim_debug_mpc.cpp b/src/task/auto_aim_debug_mpc.cpp new file mode 100644 index 0000000..39532a8 --- /dev/null +++ b/src/task/auto_aim_debug_mpc.cpp @@ -0,0 +1,150 @@ +#include + +#include +#include +#include +#include +#include + +#include "src/device/camera.hpp" +#include "src/device/gimbal/gimbal.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/sentry.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; + } + + device::Gimbal gimbal(config_path); + 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); + } + }); + + cv::Mat img; + std::chrono::steady_clock::time_point t; + + while (!exiter.exit()) { + 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(); + gimbal.send(false, false, 0, 0, 0, 0, 0, 0); + + return 0; +} \ No newline at end of file diff --git a/src/task/auto_buff_debug.cpp b/src/task/auto_buff_debug.cpp new file mode 100755 index 0000000..4184881 --- /dev/null +++ b/src/task/auto_buff_debug.cpp @@ -0,0 +1,158 @@ +#include + +#include + +#include "src/device/camera.hpp" +#include "src/device/cboard.hpp" +#include "src/module/auto_buff/buff_aimer.hpp" +#include "src/module/auto_buff/buff_detector.hpp" +#include "src/module/auto_buff/buff_solver.hpp" +#include "src/module/auto_buff/buff_target.hpp" +#include "src/module/auto_buff/buff_type.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/recorder.hpp" +#include "src/component/trajectory.hpp" + +// 定义命令行参数 +const std::string keys = + "{help h usage ? | | 输出命令行参数说明}" + "{@config-path | | yaml配置文件路径 }"; + +int main(int argc, char * argv[]) +{ + // 读取命令行参数 + cv::CommandLineParser cli(argc, argv, keys); + auto config_path = cli.get(0); + if (cli.has("help") || config_path.empty()) { + cli.printMessage(); + return 0; + } + + // 初始化绘图器、录制器、退出器 + component::Plotter plotter; + component::Recorder recorder; + component::Exiter exiter; + + // 初始化C板、相机 + device::CBoard cboard(config_path); + device::Camera camera(config_path); + + // 初始化识别器、解算器、追踪器、瞄准器 + auto_buff::Buff_Detector detector(config_path); + auto_buff::Solver solver(config_path); + auto_buff::SmallTarget target; + // auto_buff::BigTarget target; + auto_buff::Aimer aimer(config_path); + + cv::Mat img; + Eigen::Quaterniond q; + std::chrono::steady_clock::time_point t; + + while (!exiter.exit()) { + camera.read(img, t); + q = cboard.imu_at(t); + // recorder.record(img, q, t); + + // -------------- 打符核心逻辑 -------------- + + solver.set_R_gimbal2world(q); + + auto power_runes = detector.detect(img); + + solver.solve(power_runes); + + target.get_target(power_runes, t); + + auto target_copy = target; + + auto command = aimer.aim(target_copy, t, cboard.bullet_speed, true); + + cboard.send(command); + + // -------------- 调试输出 -------------- + + nlohmann::json data; + + // buff原始观测数据 + if (power_runes.has_value()) { + const auto & p = power_runes.value(); + data["buff_R_yaw"] = p.ypd_in_world[0]; + data["buff_R_pitch"] = p.ypd_in_world[1]; + data["buff_R_dis"] = p.ypd_in_world[2]; + data["buff_yaw"] = p.ypr_in_world[0] * 57.3; + data["buff_pitch"] = p.ypr_in_world[1] * 57.3; + data["buff_roll"] = p.ypr_in_world[2] * 57.3; + } + + if (!target.is_unsolve()) { + auto & p = power_runes.value(); + + // 显示 + for (int i = 0; i < 4; i++) component::draw_point(img, p.target().points[i]); + component::draw_point(img, p.target().center, {0, 0, 255}, 3); + component::draw_point(img, p.r_center, {0, 0, 255}, 3); + + // 当前帧target更新后buff + auto Rxyz_in_world_now = target.point_buff2world(Eigen::Vector3d(0.0, 0.0, 0.0)); + auto image_points = + solver.reproject_buff(Rxyz_in_world_now, target.ekf_x()[4], target.ekf_x()[5]); + component::draw_points( + img, std::vector(image_points.begin(), image_points.begin() + 4), {0, 255, 0}); + component::draw_points( + img, std::vector(image_points.begin() + 4, image_points.end()), {0, 255, 0}); + + // buff瞄准位置(预测) + double dangle = target.ekf_x()[5] - target_copy.ekf_x()[5]; + auto Rxyz_in_world_pre = target.point_buff2world(Eigen::Vector3d(0.0, 0.0, 0.0)); + image_points = + solver.reproject_buff(Rxyz_in_world_pre, target_copy.ekf_x()[4], target_copy.ekf_x()[5]); + component::draw_points( + img, std::vector(image_points.begin(), image_points.begin() + 4), {255, 0, 0}); + component::draw_points( + img, std::vector(image_points.begin() + 4, image_points.end()), {255, 0, 0}); + + // 观测器内部数据 + Eigen::VectorXd x = target.ekf_x(); + data["R_yaw"] = x[0]; + data["R_V_yaw"] = x[1]; + data["R_pitch"] = x[2]; + data["R_dis"] = x[3]; + data["yaw"] = x[4] * 57.3; + + data["angle"] = x[5] * 57.3; + data["spd"] = x[6] * 57.3; + if (x.size() >= 10) { + data["spd"] = x[6]; + data["a"] = x[7]; + data["w"] = x[8]; + data["fi"] = x[9]; + data["spd0"] = target.spd; + } + } + + // 云台响应情况 + Eigen::Vector3d ypr = component::eulers(solver.R_gimbal2world(), 2, 1, 0); + data["gimbal_yaw"] = ypr[0] * 57.3; + data["gimbal_pitch"] = ypr[1] * 57.3; + + if (command.control) { + data["cmd_yaw"] = command.yaw * 57.3; + data["cmd_pitch"] = command.pitch * 57.3; + data["shoot"] = command.shoot ? 1 : 0; + } + + plotter.plot(data); + + cv::resize(img, img, {}, 0.5, 0.5); + cv::imshow("result", img); + + auto key = cv::waitKey(1); + if (key == 'q') break; + } + + return 0; +} \ No newline at end of file diff --git a/src/task/auto_buff_debug_mpc.cpp b/src/task/auto_buff_debug_mpc.cpp new file mode 100755 index 0000000..6129271 --- /dev/null +++ b/src/task/auto_buff_debug_mpc.cpp @@ -0,0 +1,165 @@ +#include + +#include + +#include "src/device/camera.hpp" +#include "src/device/gimbal/gimbal.hpp" +#include "src/module/auto_buff/buff_aimer.hpp" +#include "src/module/auto_buff/buff_detector.hpp" +#include "src/module/auto_buff/buff_solver.hpp" +#include "src/module/auto_buff/buff_target.hpp" +#include "src/module/auto_buff/buff_type.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/recorder.hpp" +#include "src/component/trajectory.hpp" + +// 定义命令行参数 +const std::string keys = + "{help h usage ? | | 输出命令行参数说明}" + "{@config-path | | yaml配置文件路径 }"; + +int main(int argc, char * argv[]) +{ + // 读取命令行参数 + cv::CommandLineParser cli(argc, argv, keys); + auto config_path = cli.get(0); + if (cli.has("help") || config_path.empty()) { + cli.printMessage(); + return 0; + } + + // 初始化绘图器、录制器、退出器 + component::Plotter plotter; + component::Recorder recorder; + component::Exiter exiter; + + // 初始化云台、相机 + device::Gimbal gimbal(config_path); + device::Camera camera(config_path); + + // 初始化识别器、解算器、追踪器、瞄准器 + auto_buff::Buff_Detector detector(config_path); + auto_buff::Solver solver(config_path); + auto_buff::SmallTarget target; + // auto_buff::BigTarget target; + auto_buff::Aimer aimer(config_path); + + cv::Mat img; + Eigen::Quaterniond q; + std::chrono::steady_clock::time_point t; + + while (!exiter.exit()) { + camera.read(img, t); + q = gimbal.q(t); + auto gs = gimbal.state(); + // recorder.record(img, q, t); + + // -------------- 打符核心逻辑 -------------- + + solver.set_R_gimbal2world(q); + + auto power_runes = detector.detect(img); + + solver.solve(power_runes); + + target.get_target(power_runes, t); + + auto target_copy = target; + + auto plan = aimer.mpc_aim(target_copy, t, gs, true); + + gimbal.send( + plan.control, plan.fire, plan.yaw, plan.yaw_vel, plan.yaw_acc, plan.pitch, plan.pitch_vel, + plan.pitch_acc); + // -------------- 调试输出 -------------- + + nlohmann::json data; + + // buff原始观测数据 + if (power_runes.has_value()) { + const auto & p = power_runes.value(); + data["buff_R_yaw"] = p.ypd_in_world[0]; + data["buff_R_pitch"] = p.ypd_in_world[1]; + data["buff_R_dis"] = p.ypd_in_world[2]; + data["buff_yaw"] = p.ypr_in_world[0] * 57.3; + data["buff_pitch"] = p.ypr_in_world[1] * 57.3; + data["buff_roll"] = p.ypr_in_world[2] * 57.3; + } + + if (!target.is_unsolve()) { + auto & p = power_runes.value(); + + // 显示 + for (int i = 0; i < 4; i++) component::draw_point(img, p.target().points[i]); + component::draw_point(img, p.target().center, {0, 0, 255}, 3); + component::draw_point(img, p.r_center, {0, 0, 255}, 3); + + // 当前帧target更新后buff + auto Rxyz_in_world_now = target.point_buff2world(Eigen::Vector3d(0.0, 0.0, 0.0)); + auto image_points = + solver.reproject_buff(Rxyz_in_world_now, target.ekf_x()[4], target.ekf_x()[5]); + component::draw_points( + img, std::vector(image_points.begin(), image_points.begin() + 4), {0, 255, 0}); + component::draw_points( + img, std::vector(image_points.begin() + 4, image_points.end()), {0, 255, 0}); + + // buff瞄准位置(预测) + double dangle = target.ekf_x()[5] - target_copy.ekf_x()[5]; + auto Rxyz_in_world_pre = target.point_buff2world(Eigen::Vector3d(0.0, 0.0, 0.0)); + image_points = + solver.reproject_buff(Rxyz_in_world_pre, target_copy.ekf_x()[4], target_copy.ekf_x()[5]); + component::draw_points( + img, std::vector(image_points.begin(), image_points.begin() + 4), {255, 0, 0}); + component::draw_points( + img, std::vector(image_points.begin() + 4, image_points.end()), {255, 0, 0}); + + // 观测器内部数据 + Eigen::VectorXd x = target.ekf_x(); + data["R_yaw"] = x[0]; + data["R_V_yaw"] = x[1]; + data["R_pitch"] = x[2]; + data["R_dis"] = x[3]; + data["yaw"] = x[4] * 57.3; + + data["angle"] = x[5] * 57.3; + data["spd"] = x[6] * 57.3; + if (x.size() >= 10) { + data["spd"] = x[6]; + data["a"] = x[7]; + data["w"] = x[8]; + data["fi"] = x[9]; + data["spd0"] = target.spd; + } + } + + // 云台响应情况 + data["gimbal_yaw"] = gs.yaw * 57.3; + data["gimbal_pitch"] = gs.pitch * 57.3; + data["gimbal_yaw_vel"] = gs.yaw_vel * 57.3; + data["gimbal_pitch_vel"] = gs.pitch_vel * 57.3; + + if (plan.control) { + data["plan_yaw"] = plan.yaw * 57.3; + data["plan_pitch"] = plan.pitch * 57.3; + data["plan_yaw_vel"] = plan.yaw_vel * 57.3; + data["plan_pitch_vel"] = plan.pitch_vel * 57.3; + data["plan_yaw_acc"] = plan.yaw_acc * 57.3; + data["plan_pitch_acc"] = plan.pitch_acc * 57.3; + data["shoot"] = plan.fire ? 1 : 0; + } + + plotter.plot(data); + + cv::resize(img, img, {}, 0.5, 0.5); + cv::imshow("result", img); + + auto key = cv::waitKey(1); + if (key == 'q') break; + } + + return 0; +} \ No newline at end of file diff --git a/src/task/mt_auto_aim_debug.cpp b/src/task/mt_auto_aim_debug.cpp new file mode 100644 index 0000000..eb07b3c --- /dev/null +++ b/src/task/mt_auto_aim_debug.cpp @@ -0,0 +1,173 @@ +#include + +#include +#include +#include + +#include "src/device/camera.hpp" +#include "src/device/cboard.hpp" +#include "src/module/auto_aim/aimer.hpp" +#include "src/module/auto_aim/multithread/commandgener.hpp" +#include "src/module/auto_aim/multithread/mt_detector.hpp" +#include "src/module/auto_aim/shooter.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/recorder.hpp" + +const std::string keys = + "{help h usage ? | | 输出命令行参数说明}" + "{@config-path | configs/sentry.yaml | 位置参数，yaml配置文件路径 }"; + +using namespace std::chrono; + +int main(int argc, char * argv[]) +{ + cv::CommandLineParser cli(argc, argv, keys); + auto config_path = cli.get(0); + if (cli.has("help") || config_path.empty()) { + cli.printMessage(); + return 0; + } + + component::Exiter exiter; + component::Plotter plotter; + component::Recorder recorder(100); //根据实际帧率调整 + + device::CBoard cboard(config_path); + device::Camera camera(config_path); + + auto_aim::multithread::MultiThreadDetector detector(config_path, true); + auto_aim::Solver solver(config_path); + auto_aim::Tracker tracker(config_path, solver); + auto_aim::Aimer aimer(config_path); + auto_aim::Shooter shooter(config_path); + auto_aim::multithread::CommandGener commandgener(shooter, aimer, cboard, plotter, true); + + auto detect_thread = std::thread([&]() { + cv::Mat img; + std::chrono::steady_clock::time_point t; + + while (!exiter.exit()) { + camera.read(img, t); + detector.push(img, t); + } + }); + + auto mode = device::Mode::idle; + auto last_mode = device::Mode::idle; + + while (!exiter.exit()) { + auto t0 = std::chrono::steady_clock::now(); + /// 自瞄核心逻辑 + auto [img, armors, t] = detector.debug_pop(); + Eigen::Quaterniond q = cboard.imu_at(t - 1ms); + mode = cboard.mode; + + if (last_mode != mode) { + component::logger()->info("Switch to {}", device::MODES[mode]); + last_mode = mode; + } + + solver.set_R_gimbal2world(q); + + Eigen::Vector3d ypr = component::eulers(solver.R_gimbal2world(), 2, 1, 0); + + auto targets = tracker.track(armors, t); + + commandgener.push(targets, t, cboard.bullet_speed, ypr); // 发送给决策线程 + + /// debug + component::draw_text(img, fmt::format("[{}]", tracker.state()), {10, 30}, {255, 255, 255}); + + nlohmann::json data; + data["t"] = component::delta_time(std::chrono::steady_clock::now(), t0); + + // 装甲板原始观测数据 + data["armor_num"] = armors.size(); + if (!armors.empty()) { + auto min_x = 1e10; + auto & armor = armors.front(); + for (auto & a : armors) { + if (a.center.x < min_x) { + min_x = a.center.x; + armor = a; + } + } //always left + solver.solve(armor); + data["armor_x"] = armor.xyz_in_world[0]; + data["armor_y"] = armor.xyz_in_world[1]; + data["armor_yaw"] = armor.ypr_in_world[0] * 57.3; + data["armor_yaw_raw"] = armor.yaw_raw * 57.3; + } + + 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}); + } + + // aimer瞄准位置 + auto aim_point = aimer.debug_aim_point; + Eigen::Vector4d aim_xyza = aim_point.xyza; + auto image_points = + solver.reproject_armor(aim_xyza.head(3), aim_xyza[3], target.armor_type, target.name); + if (aim_point.valid) + component::draw_points(img, image_points, {0, 0, 255}); + else + component::draw_points(img, image_points, {255, 0, 0}); + + // 观测器内部数据 + Eigen::VectorXd x = target.ekf_x(); + data["x"] = x[0]; + data["vx"] = x[1]; + data["y"] = x[2]; + data["vy"] = x[3]; + data["z"] = x[4]; + data["vz"] = x[5]; + data["a"] = x[6] * 57.3; + data["w"] = x[7]; + data["r"] = x[8]; + data["l"] = x[9]; + data["h"] = x[10]; + data["last_id"] = target.last_id; + + // 卡方检验数据 + data["residual_yaw"] = target.ekf().data.at("residual_yaw"); + data["residual_pitch"] = target.ekf().data.at("residual_pitch"); + data["residual_distance"] = target.ekf().data.at("residual_distance"); + data["residual_angle"] = target.ekf().data.at("residual_angle"); + data["nis"] = target.ekf().data.at("nis"); + data["nees"] = target.ekf().data.at("nees"); + data["nis_fail"] = target.ekf().data.at("nis_fail"); + data["nees_fail"] = target.ekf().data.at("nees_fail"); + data["recent_nis_failures"] = target.ekf().data.at("recent_nis_failures"); + } + + // 云台响应情况 + data["gimbal_yaw"] = ypr[0] * 57.3; + data["gimbal_pitch"] = ypr[1] * 57.3; + data["bullet_speed"] = cboard.bullet_speed; + + plotter.plot(data); + + cv::resize(img, img, {}, 0.5, 0.5); // 显示时缩小图片尺寸 + cv::imshow("reprojection", img); + auto key = cv::waitKey(1); + if (key == 'q') break; + } + + detect_thread.join(); + + return 0; +} \ No newline at end of file diff --git a/src/task/mt_standard.cpp b/src/task/mt_standard.cpp new file mode 100644 index 0000000..c6a9953 --- /dev/null +++ b/src/task/mt_standard.cpp @@ -0,0 +1,138 @@ +#include +#include +#include + +#include "src/device/camera.hpp" +#include "src/device/dm_imu/dm_imu.hpp" +#include "src/module/auto_aim/aimer.hpp" +#include "src/module/auto_aim/multithread/commandgener.hpp" +#include "src/module/auto_aim/multithread/mt_detector.hpp" +#include "src/module/auto_aim/shooter.hpp" +#include "src/module/auto_aim/solver.hpp" +#include "src/module/auto_aim/tracker.hpp" +#include "src/module/auto_buff/buff_aimer.hpp" +#include "src/module/auto_buff/buff_detector.hpp" +#include "src/module/auto_buff/buff_solver.hpp" +#include "src/module/auto_buff/buff_target.hpp" +#include "src/module/auto_buff/buff_type.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/recorder.hpp" + +const std::string keys = + "{help h usage ? | | 输出命令行参数说明}" + "{@config-path | | yaml配置文件路径 }"; + +using namespace std::chrono_literals; + +int main(int argc, char * argv[]) +{ + cv::CommandLineParser cli(argc, argv, keys); + auto config_path = cli.get("@config-path"); + if (cli.has("help") || !cli.has("@config-path")) { + cli.printMessage(); + return 0; + } + + component::Exiter exiter; + component::Plotter plotter; + component::Recorder recorder; + + device::Camera camera(config_path); + device::CBoard cboard(config_path); + + auto_aim::multithread::MultiThreadDetector detector(config_path); + auto_aim::Solver solver(config_path); + auto_aim::Tracker tracker(config_path, solver); + auto_aim::Aimer aimer(config_path); + auto_aim::Shooter shooter(config_path); + + auto_buff::Buff_Detector buff_detector(config_path); + auto_buff::Solver buff_solver(config_path); + auto_buff::SmallTarget buff_small_target; + auto_buff::BigTarget buff_big_target; + auto_buff::Aimer buff_aimer(config_path); + + auto_aim::multithread::CommandGener commandgener(shooter, aimer, cboard, plotter); + + std::atomic mode{device::Mode::idle}; + auto last_mode{device::Mode::idle}; + + auto detect_thread = std::thread([&]() { + cv::Mat img; + std::chrono::steady_clock::time_point t; + + while (!exiter.exit()) { + if (mode.load() == device::Mode::auto_aim) { + camera.read(img, t); + detector.push(img, t); + } else + continue; + } + }); + + while (!exiter.exit()) { + mode = cboard.mode; + + if (last_mode != mode) { + component::logger()->info("Switch to {}", device::MODES[mode]); + last_mode = mode.load(); + } + + /// 自瞄 + if (mode.load() == device::Mode::auto_aim) { + auto [img, armors, t] = detector.debug_pop(); + Eigen::Quaterniond q = cboard.imu_at(t - 1ms); + + // recorder.record(img, q, t); + + solver.set_R_gimbal2world(q); + + Eigen::Vector3d ypr = component::eulers(solver.R_gimbal2world(), 2, 1, 0); + + auto targets = tracker.track(armors, t); + + commandgener.push(targets, t, cboard.bullet_speed, ypr); // 发送给决策线程 + + } + + /// 打符 + else if (mode.load() == device::Mode::small_buff || mode.load() == device::Mode::big_buff) { + cv::Mat img; + Eigen::Quaterniond q; + std::chrono::steady_clock::time_point t; + + camera.read(img, t); + q = cboard.imu_at(t - 1ms); + + // recorder.record(img, q, t); + + buff_solver.set_R_gimbal2world(q); + + auto power_runes = buff_detector.detect(img); + + buff_solver.solve(power_runes); + + device::Command buff_command; + if (mode.load() == device::Mode::small_buff) { + buff_small_target.get_target(power_runes, t); + auto target_copy = buff_small_target; + buff_command = buff_aimer.aim(target_copy, t, cboard.bullet_speed, true); + } else if (mode.load() == device::Mode::big_buff) { + buff_big_target.get_target(power_runes, t); + auto target_copy = buff_big_target; + buff_command = buff_aimer.aim(target_copy, t, cboard.bullet_speed, true); + } + cboard.send(buff_command); + + } else + continue; + } + + detect_thread.join(); + + return 0; +} \ No newline at end of file diff --git a/src/task/sentry.cpp b/src/task/sentry.cpp new file mode 100644 index 0000000..99f8285 --- /dev/null +++ b/src/task/sentry.cpp @@ -0,0 +1,106 @@ +#include + +#include +#include +#include +#include + +#include "src/device/camera.hpp" +#include "src/device/cboard.hpp" +#include "src/device/ros2/publish2nav.hpp" +#include "src/device/ros2/ros2.hpp" +#include "src/device/usbcamera/usbcamera.hpp" +#include "src/module/auto_aim/aimer.hpp" +#include "src/module/auto_aim/shooter.hpp" +#include "src/module/auto_aim/solver.hpp" +#include "src/module/auto_aim/tracker.hpp" +#include "src/module/auto_aim/yolo.hpp" +#include "src/module/omniperception/decider.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/recorder.hpp" + +using namespace std::chrono; + +const std::string keys = + "{help h usage ? | | 输出命令行参数说明}" + "{@config-path | configs/sentry.yaml | 位置参数，yaml配置文件路径 }"; + +int main(int argc, char * argv[]) +{ + component::Exiter exiter; + component::Plotter plotter; + component::Recorder recorder; + + cv::CommandLineParser cli(argc, argv, keys); + if (cli.has("help")) { + cli.printMessage(); + return 0; + } + auto config_path = cli.get(0); + + device::ROS2 ros2; + device::CBoard cboard(config_path); + device::Camera camera(config_path); + device::Camera back_camera("configs/camera.yaml"); + device::USBCamera usbcam1("video0", config_path); + device::USBCamera usbcam2("video2", config_path); + + auto_aim::YOLO yolo(config_path, false); + auto_aim::Solver solver(config_path); + auto_aim::Tracker tracker(config_path, solver); + auto_aim::Aimer aimer(config_path); + auto_aim::Shooter shooter(config_path); + + omniperception::Decider decider(config_path); + + cv::Mat img; + + std::chrono::steady_clock::time_point timestamp; + device::Command last_command; + + while (!exiter.exit()) { + camera.read(img, timestamp); + Eigen::Quaterniond q = cboard.imu_at(timestamp - 1ms); + // recorder.record(img, q, timestamp); + + /// 自瞄核心逻辑 + solver.set_R_gimbal2world(q); + + Eigen::Vector3d gimbal_pos = component::eulers(solver.R_gimbal2world(), 2, 1, 0); + + auto armors = yolo.detect(img); + + decider.get_invincible_armor(ros2.subscribe_enemy_status()); + + decider.armor_filter(armors); + + // decider.get_auto_aim_target(armors, ros2.subscribe_autoaim_target()); + + decider.set_priority(armors); + + auto targets = tracker.track(armors, timestamp); + + device::Command command{false, false, 0, 0}; + + /// 全向感知逻辑 + if (tracker.state() == "lost") + command = decider.decide(yolo, gimbal_pos, usbcam1, usbcam2, back_camera); + else + command = aimer.aim(targets, timestamp, cboard.bullet_speed, cboard.shoot_mode); + + /// 发射逻辑 + command.shoot = shooter.shoot(command, aimer, targets, gimbal_pos); + + cboard.send(command); + + /// ROS2通信 + Eigen::Vector4d target_info = decider.get_target_info(armors, targets); + + ros2.publish(target_info); + } + return 0; +} \ No newline at end of file diff --git a/src/task/sentry_bp.cpp b/src/task/sentry_bp.cpp new file mode 100644 index 0000000..821cea1 --- /dev/null +++ b/src/task/sentry_bp.cpp @@ -0,0 +1,104 @@ +#include + +#include +#include +#include +#include + +#include "src/device/camera.hpp" +#include "src/device/cboard.hpp" +#include "src/device/ros2/publish2nav.hpp" +#include "src/device/ros2/ros2.hpp" +#include "src/device/usbcamera/usbcamera.hpp" +#include "src/module/auto_aim/aimer.hpp" +#include "src/module/auto_aim/shooter.hpp" +#include "src/module/auto_aim/solver.hpp" +#include "src/module/auto_aim/tracker.hpp" +#include "src/module/auto_aim/yolo.hpp" +#include "src/module/omniperception/decider.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/recorder.hpp" + +using namespace std::chrono; + +const std::string keys = + "{help h usage ? | | 输出命令行参数说明}" + "{@config-path | configs/sentry.yaml | 位置参数，yaml配置文件路径 }"; + +int main(int argc, char * argv[]) +{ + component::Exiter exiter; + component::Plotter plotter; + component::Recorder recorder; + + cv::CommandLineParser cli(argc, argv, keys); + if (cli.has("help")) { + cli.printMessage(); + return 0; + } + auto config_path = cli.get(0); + + device::ROS2 ros2; + device::CBoard cboard(config_path); + device::Camera camera(config_path); + device::Camera back_camera("configs/camera.yaml"); + + auto_aim::YOLO yolo(config_path, false); + auto_aim::Solver solver(config_path); + auto_aim::Tracker tracker(config_path, solver); + auto_aim::Aimer aimer(config_path); + auto_aim::Shooter shooter(config_path); + + omniperception::Decider decider(config_path); + + cv::Mat img; + + std::chrono::steady_clock::time_point timestamp; + device::Command last_command; + + while (!exiter.exit()) { + camera.read(img, timestamp); + Eigen::Quaterniond q = cboard.imu_at(timestamp - 1ms); + // recorder.record(img, q, timestamp); + + /// 自瞄核心逻辑 + solver.set_R_gimbal2world(q); + + Eigen::Vector3d gimbal_pos = component::eulers(solver.R_gimbal2world(), 2, 1, 0); + + auto armors = yolo.detect(img); + + decider.get_invincible_armor(ros2.subscribe_enemy_status()); + + decider.armor_filter(armors); + + // decider.get_auto_aim_target(armors, ros2.subscribe_autoaim_target()); + + decider.set_priority(armors); + + auto targets = tracker.track(armors, timestamp); + + device::Command command{false, false, 0, 0}; + + /// 全向感知逻辑 + if (tracker.state() == "lost") + command = decider.decide(yolo, gimbal_pos, back_camera); + else + command = aimer.aim(targets, timestamp, cboard.bullet_speed, cboard.shoot_mode); + + /// 发射逻辑 + command.shoot = shooter.shoot(command, aimer, targets, gimbal_pos); + + cboard.send(command); + + /// ROS2通信 + Eigen::Vector4d target_info = decider.get_target_info(armors, targets); + + ros2.publish(target_info); + } + return 0; +} \ No newline at end of file diff --git a/src/task/sentry_debug.cpp b/src/task/sentry_debug.cpp new file mode 100644 index 0000000..008062b --- /dev/null +++ b/src/task/sentry_debug.cpp @@ -0,0 +1,196 @@ +#include + +#include +#include +#include +#include + +#include "src/device/camera.hpp" +#include "src/device/cboard.hpp" +#include "src/device/ros2/publish2nav.hpp" +#include "src/device/ros2/ros2.hpp" +#include "src/device/usbcamera/usbcamera.hpp" +#include "src/module/auto_aim/aimer.hpp" +#include "src/module/auto_aim/shooter.hpp" +#include "src/module/auto_aim/solver.hpp" +#include "src/module/auto_aim/tracker.hpp" +#include "src/module/auto_aim/yolo.hpp" +#include "src/module/omniperception/decider.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/recorder.hpp" + +using namespace std::chrono; + +const std::string keys = + "{help h usage ? | | 输出命令行参数说明}" + "{@config-path | configs/sentry.yaml | 位置参数，yaml配置文件路径 }"; + +int main(int argc, char * argv[]) +{ + component::Exiter exiter; + component::Plotter plotter; + component::Recorder recorder; + + cv::CommandLineParser cli(argc, argv, keys); + if (cli.has("help")) { + cli.printMessage(); + return 0; + } + auto config_path = cli.get(0); + + device::ROS2 ros2; + device::CBoard cboard(config_path); + device::Camera camera(config_path); + device::Camera back_camera("configs/camera.yaml"); + device::USBCamera usbcam1("video0", config_path); + device::USBCamera usbcam2("video2", config_path); + + auto_aim::YOLO yolo(config_path, false); + auto_aim::Solver solver(config_path); + auto_aim::Tracker tracker(config_path, solver); + auto_aim::Aimer aimer(config_path); + auto_aim::Shooter shooter(config_path); + + omniperception::Decider decider(config_path); + + cv::Mat img; + + std::chrono::steady_clock::time_point timestamp; + device::Command last_command; + + while (!exiter.exit()) { + camera.read(img, timestamp); + Eigen::Quaterniond q = cboard.imu_at(timestamp - 1ms); + // recorder.record(img, q, timestamp); + + /// 自瞄核心逻辑 + solver.set_R_gimbal2world(q); + + Eigen::Vector3d gimbal_pos = component::eulers(solver.R_gimbal2world(), 2, 1, 0); + + auto armors = yolo.detect(img); + + decider.get_invincible_armor(ros2.subscribe_enemy_status()); + + decider.armor_filter(armors); + + decider.get_auto_aim_target(armors, ros2.subscribe_autoaim_target()); + + decider.set_priority(armors); + + auto targets = tracker.track(armors, timestamp); + + device::Command command{false, false, 0, 0}; + + /// 全向感知逻辑 + if (tracker.state() == "lost") + command = decider.decide(yolo, gimbal_pos, usbcam1, usbcam2, back_camera); + else + command = aimer.aim(targets, timestamp, cboard.bullet_speed, cboard.shoot_mode); + + /// 发射逻辑 + command.shoot = shooter.shoot(command, aimer, targets, gimbal_pos); + + cboard.send(command); + + /// ROS2通信 + Eigen::Vector4d target_info = decider.get_target_info(armors, targets); + + ros2.publish(target_info); + + /// debug + component::draw_text(img, fmt::format("[{}]", tracker.state()), {10, 30}, {255, 255, 255}); + + nlohmann::json data; + + // 装甲板原始观测数据 + data["armor_num"] = armors.size(); + if (!armors.empty()) { + auto min_x = 1e10; + auto & armor = armors.front(); + for (auto & a : armors) { + if (a.center.x < min_x) { + min_x = a.center.x; + armor = a; + } + } //always left + solver.solve(armor); + data["armor_x"] = armor.xyz_in_world[0]; + data["armor_y"] = armor.xyz_in_world[1]; + data["armor_yaw"] = armor.ypr_in_world[0] * 57.3; + data["armor_yaw_raw"] = armor.yaw_raw * 57.3; + } + + 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}); + } + + // aimer瞄准位置 + auto aim_point = aimer.debug_aim_point; + Eigen::Vector4d aim_xyza = aim_point.xyza; + auto image_points = + solver.reproject_armor(aim_xyza.head(3), aim_xyza[3], target.armor_type, target.name); + if (aim_point.valid) + component::draw_points(img, image_points, {0, 0, 255}); + else + component::draw_points(img, image_points, {255, 0, 0}); + + // 观测器内部数据 + Eigen::VectorXd x = target.ekf_x(); + data["x"] = x[0]; + data["vx"] = x[1]; + data["y"] = x[2]; + data["vy"] = x[3]; + data["z"] = x[4]; + data["vz"] = x[5]; + data["a"] = x[6] * 57.3; + data["w"] = x[7]; + data["r"] = x[8]; + data["l"] = x[9]; + data["h"] = x[10]; + data["last_id"] = target.last_id; + + // 卡方检验数据 + data["residual_yaw"] = target.ekf().data.at("residual_yaw"); + data["residual_pitch"] = target.ekf().data.at("residual_pitch"); + data["residual_distance"] = target.ekf().data.at("residual_distance"); + data["residual_angle"] = target.ekf().data.at("residual_angle"); + data["nis"] = target.ekf().data.at("nis"); + data["nees"] = target.ekf().data.at("nees"); + data["nis_fail"] = target.ekf().data.at("nis_fail"); + data["nees_fail"] = target.ekf().data.at("nees_fail"); + data["recent_nis_failures"] = target.ekf().data.at("recent_nis_failures"); + } + + // 云台响应情况 + data["gimbal_yaw"] = gimbal_pos[0] * 57.3; + data["gimbal_pitch"] = -gimbal_pos[1] * 57.3; + data["shootmode"] = cboard.shoot_mode; + if (command.control) { + data["cmd_yaw"] = command.yaw * 57.3; + data["cmd_pitch"] = command.pitch * 57.3; + data["cmd_shoot"] = command.shoot; + } + + data["bullet_speed"] = cboard.bullet_speed; + + plotter.plot(data); + + cv::resize(img, img, {}, 0.5, 0.5); // 显示时缩小图片尺寸 + cv::imshow("reprojection", img); + auto key = cv::waitKey(1); + if (key == 'q') break; + } + return 0; +} \ No newline at end of file diff --git a/src/task/sentry_multithread.cpp b/src/task/sentry_multithread.cpp new file mode 100644 index 0000000..9446640 --- /dev/null +++ b/src/task/sentry_multithread.cpp @@ -0,0 +1,125 @@ +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "src/device/camera.hpp" +#include "src/device/cboard.hpp" +#include "src/device/ros2/ros2.hpp" +#include "src/device/usbcamera/usbcamera.hpp" +#include "src/module/auto_aim/aimer.hpp" +#include "src/module/auto_aim/shooter.hpp" +#include "src/module/auto_aim/solver.hpp" +#include "src/module/auto_aim/tracker.hpp" +#include "src/module/auto_aim/yolo.hpp" +#include "src/module/omniperception/decider.hpp" +#include "src/module/omniperception/perceptron.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/recorder.hpp" + +using namespace std::chrono; + +const std::string keys = + "{help h usage ? | | 输出命令行参数说明}" + "{@config-path | configs/sentry.yaml | 位置参数，yaml配置文件路径 }"; + +int main(int argc, char * argv[]) +{ + component::Exiter exiter; + component::Plotter plotter; + component::Recorder recorder; + + cv::CommandLineParser cli(argc, argv, keys); + if (cli.has("help")) { + cli.printMessage(); + return 0; + } + auto config_path = cli.get(0); + + device::ROS2 ros2; + device::CBoard cboard(config_path); + device::Camera camera(config_path); + device::USBCamera usbcam1("video0", config_path); + device::USBCamera usbcam2("video2", config_path); + device::USBCamera usbcam3("video4", config_path); + device::USBCamera usbcam4("video6", config_path); + + auto_aim::YOLO yolo(config_path, false); + auto_aim::Solver solver(config_path); + auto_aim::Tracker tracker(config_path, solver); + auto_aim::Aimer aimer(config_path); + auto_aim::Shooter shooter(config_path); + + omniperception::Decider decider(config_path); + omniperception::Perceptron perceptron(&usbcam1, &usbcam2, &usbcam3, &usbcam4, config_path); + + omniperception::DetectionResult switch_target; + cv::Mat img; + std::chrono::steady_clock::time_point timestamp; + device::Command last_command; + + while (!exiter.exit()) { + camera.read(img, timestamp); + Eigen::Quaterniond q = cboard.imu_at(timestamp - 1ms); + recorder.record(img, q, timestamp); + /// 自瞄核心逻辑 + solver.set_R_gimbal2world(q); + + Eigen::Vector3d gimbal_pos = component::eulers(solver.R_gimbal2world(), 2, 1, 0); + + auto armors = yolo.detect(img); + + decider.get_invincible_armor(ros2.subscribe_enemy_status()); + + decider.armor_filter(armors); + + decider.set_priority(armors); + + auto detection_queue = perceptron.get_detection_queue(); + + decider.sort(detection_queue); + + auto [switch_target, targets] = tracker.track(detection_queue, armors, timestamp); + + device::Command command{false, false, 0, 0}; + + /// 全向感知逻辑 + if (tracker.state() == "switching") { + command.control = switch_target.armors.empty() ? false : true; + command.shoot = false; + command.pitch = component::limit_rad(switch_target.delta_pitch); + command.yaw = component::limit_rad(switch_target.delta_yaw + gimbal_pos[0]); + } + + else if (tracker.state() == "lost") { + command = decider.decide(detection_queue); + command.yaw = component::limit_rad(command.yaw + gimbal_pos[0]); + } + + else { + command = aimer.aim(targets, timestamp, cboard.bullet_speed); + } + + /// 发射逻辑 + command.shoot = shooter.shoot(command, aimer, targets, gimbal_pos); + // command.shoot = false; + + cboard.send(command); + + /// ROS2通信 + Eigen::Vector4d target_info = decider.get_target_info(armors, targets); + + ros2.publish(target_info); + } + + return 0; +} \ No newline at end of file diff --git a/src/task/standard.cpp b/src/task/standard.cpp new file mode 100644 index 0000000..493f958 --- /dev/null +++ b/src/task/standard.cpp @@ -0,0 +1,83 @@ +#include + +#include +#include +#include + +#include "src/device/camera.hpp" +#include "src/device/cboard.hpp" +#include "src/module/auto_aim/aimer.hpp" +#include "src/module/auto_aim/multithread/commandgener.hpp" +#include "src/module/auto_aim/shooter.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/recorder.hpp" + +using namespace std::chrono; + +const std::string keys = + "{help h usage ? | | 输出命令行参数说明}" + "{@config-path | configs/standard3.yaml | 位置参数，yaml配置文件路径 }"; + +int main(int argc, char * argv[]) +{ + cv::CommandLineParser cli(argc, argv, keys); + auto config_path = cli.get(0); + if (cli.has("help") || config_path.empty()) { + cli.printMessage(); + return 0; + } + + component::Exiter exiter; + component::Plotter plotter; + component::Recorder recorder; + + device::CBoard cboard(config_path); + device::Camera camera(config_path); + + auto_aim::YOLO detector(config_path, false); + auto_aim::Solver solver(config_path); + auto_aim::Tracker tracker(config_path, solver); + auto_aim::Aimer aimer(config_path); + auto_aim::Shooter shooter(config_path); + + cv::Mat img; + Eigen::Quaterniond q; + std::chrono::steady_clock::time_point t; + + auto mode = device::Mode::idle; + auto last_mode = device::Mode::idle; + + while (!exiter.exit()) { + camera.read(img, t); + q = cboard.imu_at(t - 1ms); + mode = cboard.mode; + + if (last_mode != mode) { + component::logger()->info("Switch to {}", device::MODES[mode]); + last_mode = mode; + } + + // recorder.record(img, q, t); + + solver.set_R_gimbal2world(q); + + Eigen::Vector3d ypr = component::eulers(solver.R_gimbal2world(), 2, 1, 0); + + auto armors = detector.detect(img); + + auto targets = tracker.track(armors, t); + + auto command = aimer.aim(targets, t, cboard.bullet_speed); + + cboard.send(command); + } + + return 0; +} \ No newline at end of file diff --git a/src/task/standard_mpc.cpp b/src/task/standard_mpc.cpp new file mode 100644 index 0000000..ff39dca --- /dev/null +++ b/src/task/standard_mpc.cpp @@ -0,0 +1,145 @@ +#include +#include +#include + +#include "src/device/camera.hpp" +#include "src/device/dm_imu/dm_imu.hpp" +#include "src/module/auto_aim/aimer.hpp" +#include "src/module/auto_aim/multithread/commandgener.hpp" +#include "src/module/auto_aim/multithread/mt_detector.hpp" +#include "src/module/auto_aim/shooter.hpp" +#include "src/module/auto_aim/solver.hpp" +#include "src/module/auto_aim/tracker.hpp" +#include "src/module/auto_buff/buff_aimer.hpp" +#include "src/module/auto_buff/buff_detector.hpp" +#include "src/module/auto_buff/buff_solver.hpp" +#include "src/module/auto_buff/buff_target.hpp" +#include "src/module/auto_buff/buff_type.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/recorder.hpp" + +const std::string keys = + "{help h usage ? | | 输出命令行参数说明}" + "{@config-path | | yaml配置文件路径 }"; + +using namespace std::chrono_literals; + +int main(int argc, char * argv[]) +{ + cv::CommandLineParser cli(argc, argv, keys); + auto config_path = cli.get("@config-path"); + if (cli.has("help") || !cli.has("@config-path")) { + cli.printMessage(); + return 0; + } + + component::Exiter exiter; + component::Plotter plotter; + component::Recorder recorder; + + device::Gimbal gimbal(config_path); + 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); + + auto_buff::Buff_Detector buff_detector(config_path); + auto_buff::Solver buff_solver(config_path); + auto_buff::SmallTarget buff_small_target; + auto_buff::BigTarget buff_big_target; + auto_buff::Aimer buff_aimer(config_path); + + cv::Mat img; + Eigen::Quaterniond q; + std::chrono::steady_clock::time_point t; + + std::atomic quit = false; + + std::atomic mode{device::GimbalMode::IDLE}; + auto last_mode{device::GimbalMode::IDLE}; + + auto plan_thread = std::thread([&]() { + auto t0 = std::chrono::steady_clock::now(); + uint16_t last_bullet_count = 0; + + while (!quit) { + if (!target_queue.empty() && mode == device::GimbalMode::AUTO_AIM) { + 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); + + std::this_thread::sleep_for(10ms); + } else + std::this_thread::sleep_for(200ms); + } + }); + + while (!exiter.exit()) { + mode = gimbal.mode(); + + if (last_mode != mode) { + component::logger()->info("Switch to {}", gimbal.str(mode)); + last_mode = mode.load(); + } + + camera.read(img, t); + auto q = gimbal.q(t); + auto gs = gimbal.state(); + recorder.record(img, q, t); + solver.set_R_gimbal2world(q); + + /// 自瞄 + if (mode.load() == device::GimbalMode::AUTO_AIM) { + 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); + } + + /// 打符 + else if (mode.load() == device::GimbalMode::SMALL_BUFF || mode.load() == device::GimbalMode::BIG_BUFF) { + buff_solver.set_R_gimbal2world(q); + + auto power_runes = buff_detector.detect(img); + + buff_solver.solve(power_runes); + + auto_aim::Plan buff_plan; + if (mode.load() == device::GimbalMode::SMALL_BUFF) { + buff_small_target.get_target(power_runes, t); + auto target_copy = buff_small_target; + buff_plan = buff_aimer.mpc_aim(target_copy, t, gs, true); + } else if (mode.load() == device::GimbalMode::BIG_BUFF) { + buff_big_target.get_target(power_runes, t); + auto target_copy = buff_big_target; + buff_plan = buff_aimer.mpc_aim(target_copy, t, gs, true); + } + gimbal.send( + buff_plan.control, buff_plan.fire, buff_plan.yaw, buff_plan.yaw_vel, buff_plan.yaw_acc, + buff_plan.pitch, buff_plan.pitch_vel, buff_plan.pitch_acc); + + } else + gimbal.send(false, false, 0, 0, 0, 0, 0, 0); + } + + quit = true; + if (plan_thread.joinable()) plan_thread.join(); + gimbal.send(false, false, 0, 0, 0, 0, 0, 0); + + return 0; +} \ No newline at end of file diff --git a/src/task/auto_aim_test.cpp b/src/task/test/auto_aim_test.cpp similarity index 100% rename from src/task/auto_aim_test.cpp rename to src/task/test/auto_aim_test.cpp diff --git a/src/task/auto_buff_test.cpp b/src/task/test/auto_buff_test.cpp similarity index 100% rename from src/task/auto_buff_test.cpp rename to src/task/test/auto_buff_test.cpp diff --git a/src/task/camera_detect_test.cpp b/src/task/test/camera_detect_test.cpp similarity index 100% rename from src/task/camera_detect_test.cpp rename to src/task/test/camera_detect_test.cpp diff --git a/src/task/camera_test.cpp b/src/task/test/camera_test.cpp similarity index 100% rename from src/task/camera_test.cpp rename to src/task/test/camera_test.cpp diff --git a/src/task/camera_thread_test.cpp b/src/task/test/camera_thread_test.cpp similarity index 100% rename from src/task/camera_thread_test.cpp rename to src/task/test/camera_thread_test.cpp diff --git a/src/task/cboard_test.cpp b/src/task/test/cboard_test.cpp similarity index 100% rename from src/task/cboard_test.cpp rename to src/task/test/cboard_test.cpp diff --git a/src/task/detector_video_test.cpp b/src/task/test/detector_video_test.cpp similarity index 100% rename from src/task/detector_video_test.cpp rename to src/task/test/detector_video_test.cpp diff --git a/src/task/dm_test.cpp b/src/task/test/dm_test.cpp similarity index 100% rename from src/task/dm_test.cpp rename to src/task/test/dm_test.cpp diff --git a/src/task/fire_test.cpp b/src/task/test/fire_test.cpp similarity index 100% rename from src/task/fire_test.cpp rename to src/task/test/fire_test.cpp diff --git a/src/task/gimbal_response_test.cpp b/src/task/test/gimbal_response_test.cpp similarity index 100% rename from src/task/gimbal_response_test.cpp rename to src/task/test/gimbal_response_test.cpp diff --git a/src/task/gimbal_test.cpp b/src/task/test/gimbal_test.cpp similarity index 100% rename from src/task/gimbal_test.cpp rename to src/task/test/gimbal_test.cpp diff --git a/src/task/handeye_test.cpp b/src/task/test/handeye_test.cpp similarity index 100% rename from src/task/handeye_test.cpp rename to src/task/test/handeye_test.cpp diff --git a/src/task/minimum_vision_system.cpp b/src/task/test/minimum_vision_system.cpp similarity index 100% rename from src/task/minimum_vision_system.cpp rename to src/task/test/minimum_vision_system.cpp diff --git a/src/task/multi_usbcamera_test.cpp b/src/task/test/multi_usbcamera_test.cpp similarity index 100% rename from src/task/multi_usbcamera_test.cpp rename to src/task/test/multi_usbcamera_test.cpp diff --git a/src/task/planner_test.cpp b/src/task/test/planner_test.cpp similarity index 100% rename from src/task/planner_test.cpp rename to src/task/test/planner_test.cpp diff --git a/src/task/planner_test_offline.cpp b/src/task/test/planner_test_offline.cpp similarity index 100% rename from src/task/planner_test_offline.cpp rename to src/task/test/planner_test_offline.cpp diff --git a/src/task/publish_test.cpp b/src/task/test/publish_test.cpp similarity index 100% rename from src/task/publish_test.cpp rename to src/task/test/publish_test.cpp diff --git a/src/task/subscribe_test.cpp b/src/task/test/subscribe_test.cpp similarity index 100% rename from src/task/subscribe_test.cpp rename to src/task/test/subscribe_test.cpp diff --git a/src/task/topic_loop_test.cpp b/src/task/test/topic_loop_test.cpp similarity index 100% rename from src/task/topic_loop_test.cpp rename to src/task/test/topic_loop_test.cpp diff --git a/src/task/usbcamera_detect_test.cpp b/src/task/test/usbcamera_detect_test.cpp similarity index 100% rename from src/task/usbcamera_detect_test.cpp rename to src/task/test/usbcamera_detect_test.cpp diff --git a/src/task/usbcamera_test.cpp b/src/task/test/usbcamera_test.cpp similarity index 100% rename from src/task/usbcamera_test.cpp rename to src/task/test/usbcamera_test.cpp diff --git a/src/task/uav.cpp b/src/task/uav.cpp new file mode 100644 index 0000000..268b420 --- /dev/null +++ b/src/task/uav.cpp @@ -0,0 +1,120 @@ +#include +#include +#include + +#include "src/device/camera.hpp" +#include "src/device/dm_imu/dm_imu.hpp" +#include "src/module/auto_aim/aimer.hpp" +#include "src/module/auto_aim/detector.hpp" +#include "src/module/auto_aim/shooter.hpp" +#include "src/module/auto_aim/solver.hpp" +#include "src/module/auto_aim/tracker.hpp" +#include "src/module/auto_aim/yolo.hpp" +#include "src/module/auto_buff/buff_aimer.hpp" +#include "src/module/auto_buff/buff_detector.hpp" +#include "src/module/auto_buff/buff_solver.hpp" +#include "src/module/auto_buff/buff_target.hpp" +#include "src/module/auto_buff/buff_type.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/recorder.hpp" + +const std::string keys = + "{help h usage ? | | 输出命令行参数说明}" + "{@config-path | configs/uav.yaml | yaml配置文件路径 }"; + +using namespace std::chrono_literals; + +int main(int argc, char * argv[]) +{ + cv::CommandLineParser cli(argc, argv, keys); + auto config_path = cli.get("@config-path"); + if (cli.has("help") || !cli.has("@config-path")) { + cli.printMessage(); + return 0; + } + + component::Exiter exiter; + component::Plotter plotter; + component::Recorder recorder; + + device::Camera camera(config_path); + device::CBoard cboard(config_path); + + auto_aim::Detector detector(config_path); + auto_aim::Solver solver(config_path); + // auto_aim::YOLO yolo(config_path); + auto_aim::Tracker tracker(config_path, solver); + auto_aim::Aimer aimer(config_path); + auto_aim::Shooter shooter(config_path); + + auto_buff::Buff_Detector buff_detector(config_path); + auto_buff::Solver buff_solver(config_path); + auto_buff::SmallTarget buff_small_target; + auto_buff::BigTarget buff_big_target; + auto_buff::Aimer buff_aimer(config_path); + + cv::Mat img; + Eigen::Quaterniond q; + std::chrono::steady_clock::time_point t; + + auto mode = device::Mode::idle; + auto last_mode = device::Mode::idle; + + while (!exiter.exit()) { + camera.read(img, t); + q = cboard.imu_at(t - 1ms); + mode = cboard.mode; + // recorder.record(img, q, t); + if (last_mode != mode) { + component::logger()->info("Switch to {}", device::MODES[mode]); + last_mode = mode; + } + + /// 自瞄 + if (mode == device::Mode::auto_aim || mode == device::Mode::outpost) { + solver.set_R_gimbal2world(q); + + Eigen::Vector3d ypr = component::eulers(solver.R_gimbal2world(), 2, 1, 0); + + auto armors = detector.detect(img); + + auto targets = tracker.track(armors, t); + + auto command = aimer.aim(targets, t, cboard.bullet_speed); + + command.shoot = shooter.shoot(command, aimer, targets, ypr); + + cboard.send(command); + } + + /// 打符 + else if (mode == device::Mode::small_buff || mode == device::Mode::big_buff) { + buff_solver.set_R_gimbal2world(q); + + auto power_runes = buff_detector.detect(img); + + buff_solver.solve(power_runes); + + device::Command buff_command; + if (mode == device::Mode::small_buff) { + buff_small_target.get_target(power_runes, t); + auto target_copy = buff_small_target; + buff_command = buff_aimer.aim(target_copy, t, cboard.bullet_speed, true); + } else if (mode == device::Mode::big_buff) { + buff_big_target.get_target(power_runes, t); + auto target_copy = buff_big_target; + buff_command = buff_aimer.aim(target_copy, t, cboard.bullet_speed, true); + } + cboard.send(buff_command); + } + + else + continue; + } + + return 0; +} \ No newline at end of file diff --git a/src/task/uav_debug.cpp b/src/task/uav_debug.cpp new file mode 100644 index 0000000..962603f --- /dev/null +++ b/src/task/uav_debug.cpp @@ -0,0 +1,173 @@ +#include +#include +#include + +#include "src/device/camera.hpp" +#include "src/device/dm_imu/dm_imu.hpp" +#include "src/module/auto_aim/aimer.hpp" +#include "src/module/auto_aim/detector.hpp" +#include "src/module/auto_aim/shooter.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/recorder.hpp" + +const std::string keys = + "{help h usage ? | | 输出命令行参数说明}" + "{@config-path | configs/uav.yaml | yaml配置文件路径 }"; + +using namespace std::chrono_literals; + +int main(int argc, char * argv[]) +{ + cv::CommandLineParser cli(argc, argv, keys); + auto config_path = cli.get("@config-path"); + if (cli.has("help") || !cli.has("@config-path")) { + cli.printMessage(); + return 0; + } + + component::Exiter exiter; + component::Plotter plotter; + component::Recorder recorder; + + device::Camera camera(config_path); + device::CBoard cboard(config_path); + + auto_aim::Detector detector(config_path); + auto_aim::Solver solver(config_path); + auto_aim::YOLO yolo(config_path); + auto_aim::Tracker tracker(config_path, solver); + auto_aim::Aimer aimer(config_path); + auto_aim::Shooter shooter(config_path); + + cv::Mat img; + Eigen::Quaterniond q; + std::chrono::steady_clock::time_point t; + + auto mode = device::Mode::idle; + auto last_mode = device::Mode::idle; + + auto t0 = std::chrono::steady_clock::now(); + + while (!exiter.exit()) { + camera.read(img, t); + q = cboard.imu_at(t - 1ms); + mode = cboard.mode; + // recorder.record(img, q, t); + if (last_mode != mode) { + component::logger()->info("Switch to {}", device::MODES[mode]); + last_mode = mode; + } + + /// 自瞄 + solver.set_R_gimbal2world(q); + + Eigen::Vector3d ypr = component::eulers(solver.R_gimbal2world(), 2, 1, 0); + + auto armors = detector.detect(img); + + auto targets = tracker.track(armors, t); + + auto command = aimer.aim(targets, t, cboard.bullet_speed); + + command.shoot = shooter.shoot(command, aimer, targets, ypr); + + cboard.send(command); + + /// debug + component::draw_text(img, fmt::format("[{}]", tracker.state()), {10, 30}, {255, 255, 255}); + + nlohmann::json data; + data["t"] = component::delta_time(std::chrono::steady_clock::now(), t0); + + // 装甲板原始观测数据 + data["armor_num"] = armors.size(); + if (!armors.empty()) { + auto min_x = 1e10; + auto & armor = armors.front(); + for (auto & a : armors) { + if (a.center.x < min_x) { + min_x = a.center.x; + armor = a; + } + } //always left + solver.solve(armor); + data["armor_x"] = armor.xyz_in_world[0]; + data["armor_y"] = armor.xyz_in_world[1]; + data["armor_yaw"] = armor.ypr_in_world[0] * 57.3; + data["armor_yaw_raw"] = armor.yaw_raw * 57.3; + } + + 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}); + } + + // aimer瞄准位置 + auto aim_point = aimer.debug_aim_point; + Eigen::Vector4d aim_xyza = aim_point.xyza; + auto image_points = + solver.reproject_armor(aim_xyza.head(3), aim_xyza[3], target.armor_type, target.name); + if (aim_point.valid) + component::draw_points(img, image_points, {0, 0, 255}); + else + component::draw_points(img, image_points, {255, 0, 0}); + + // 观测器内部数据 + Eigen::VectorXd x = target.ekf_x(); + data["x"] = x[0]; + data["vx"] = x[1]; + data["y"] = x[2]; + data["vy"] = x[3]; + data["z"] = x[4]; + data["vz"] = x[5]; + data["a"] = x[6] * 57.3; + data["w"] = x[7]; + data["r"] = x[8]; + data["l"] = x[9]; + data["h"] = x[10]; + data["last_id"] = target.last_id; + + // 卡方检验数据 + data["residual_yaw"] = target.ekf().data.at("residual_yaw"); + data["residual_pitch"] = target.ekf().data.at("residual_pitch"); + data["residual_distance"] = target.ekf().data.at("residual_distance"); + data["residual_angle"] = target.ekf().data.at("residual_angle"); + data["nis"] = target.ekf().data.at("nis"); + data["nees"] = target.ekf().data.at("nees"); + data["nis_fail"] = target.ekf().data.at("nis_fail"); + data["nees_fail"] = target.ekf().data.at("nees_fail"); + data["recent_nis_failures"] = target.ekf().data.at("recent_nis_failures"); + } + + // 云台响应情况 + data["gimbal_yaw"] = ypr[0] * 57.3; + data["gimbal_pitch"] = ypr[1] * 57.3; + data["bullet_speed"] = cboard.bullet_speed; + if (command.control) { + data["cmd_yaw"] = command.yaw * 57.3; + data["cmd_pitch"] = command.pitch * 57.3; + data["cmd_shoot"] = command.shoot; + } + plotter.plot(data); + + cv::resize(img, img, {}, 0.5, 0.5); // 显示时缩小图片尺寸 + cv::imshow("reprojection", img); + auto key = cv::waitKey(1); + if (key == 'q') break; + } + + return 0; +} \ No newline at end of file