From 0a6fd76f0dfc16bebc1a96a16c3e4789765165a9 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sat, 7 Mar 2026 15:56:15 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E6=B7=BB=E5=8A=A0ROS2=E9=80=9A?= =?UTF-8?q?=E4=BF=A1=E6=94=AF=E6=8C=81=E5=92=8C=E5=93=A8=E5=85=B5MPC?= =?UTF-8?q?=E7=A8=8B=E5=BA=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 添加ROS2条件编译支持，自动检测ROS2环境 - 创建GimbalROS类，使用rm_msgs进行ROS2通信 - 发布data_aim话题（DataAim消息） - 订阅data_mcu话题（DataMCU消息） - 新增sentry_mpc程序，使用ROS2通信的哨兵自瞄 - 新增capture_ros程序，使用ROS2通信的标定采集 - 更新README.md，添加完整的ROS2使用说明 - 移除旧的sentry相关程序，统一使用sentry_mpc Co-Authored-By: Claude Opus 4.6 (1M context) --- CMakeLists.txt | 30 +++++ README.md | 133 ++++++++++++++++++--- calibration/capture_ros.cpp | 164 ++++++++++++++++++++++++++ src/device/CMakeLists.txt | 23 +++- src/device/gimbal/gimbal_ros.cpp | 144 +++++++++++++++++++++++ src/device/gimbal/gimbal_ros.hpp | 58 +++++++++ src/task/sentry.cpp | 106 ----------------- src/task/sentry_bp.cpp | 104 ---------------- src/task/sentry_debug.cpp | 196 ------------------------------- src/task/sentry_mpc.cpp | 179 ++++++++++++++++++++++++++++ src/task/sentry_multithread.cpp | 125 -------------------- 11 files changed, 711 insertions(+), 551 deletions(-) create mode 100644 calibration/capture_ros.cpp create mode 100644 src/device/gimbal/gimbal_ros.cpp create mode 100644 src/device/gimbal/gimbal_ros.hpp delete mode 100644 src/task/sentry.cpp delete mode 100644 src/task/sentry_bp.cpp delete mode 100644 src/task/sentry_debug.cpp create mode 100644 src/task/sentry_mpc.cpp delete mode 100644 src/task/sentry_multithread.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 23eaead..b9cb30c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,6 +15,22 @@ find_package(nlohmann_json REQUIRED) set(OpenVINO_DIR "/opt/intel/openvino_2024.6.0/runtime/cmake/") find_package(OpenVINO REQUIRED) +# 尝试查找ROS2 +find_package(ament_cmake QUIET) +find_package(rclcpp QUIET) + +if(ament_cmake_FOUND AND rclcpp_FOUND) + message(STATUS "ROS2 found, enabling ROS2 support") + set(USE_ROS2 ON) + add_definitions(-DUSE_ROS2) + + # 查找rm_msgs包 + find_package(rm_msgs REQUIRED) +else() + message(STATUS "ROS2 not found, building without ROS2 support") + set(USE_ROS2 OFF) +endif() + include_directories(${EIGEN3_INCLUDE_DIR}) include_directories(${OpenCV_INCLUDE_DIRS}) include_directories(${PROJECT_SOURCE_DIR}) @@ -41,6 +57,13 @@ target_link_libraries(calibrate_handeye ${OpenCV_LIBS} fmt::fmt yaml-cpp compone target_link_libraries(calibrate_robotworld_handeye ${OpenCV_LIBS} fmt::fmt yaml-cpp component) target_link_libraries(split_video ${OpenCV_LIBS} fmt::fmt component) +# ROS2版本的capture +if(USE_ROS2) + add_executable(capture_ros calibration/capture_ros.cpp) + target_link_libraries(capture_ros ${OpenCV_LIBS} fmt::fmt yaml-cpp component device rclcpp::rclcpp ${rm_msgs_TARGETS}) + ament_target_dependencies(capture_ros rclcpp rm_msgs) +endif() + ##################tests################## add_executable(auto_aim_test src/task/test/auto_aim_test.cpp) add_executable(auto_buff_test src/task/test/auto_buff_test.cpp) @@ -117,3 +140,10 @@ target_link_libraries(balance_infantry ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann add_executable(balance_infantry_mpc src/task/balance_infantry_mpc.cpp) target_link_libraries(balance_infantry_mpc ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann_json::nlohmann_json auto_aim auto_buff component device) +################## ROS2 tasks ################## +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) +endif() + diff --git a/README.md b/README.md index 6be3daf..ed0285b 100644 --- a/README.md +++ b/README.md @@ -25,6 +25,7 @@ - 运算平台：Intel NUC（i7-1260P / i7-1165G7） - 相机：海康 MV-CS016-10UC + 6mm 镜头 - 下位机：RoboMaster C 型开发板（STM32F407）/ 达妙 MC02（STM32H7） +- ROS2 Humble（可选，用于哨兵ROS2通信） ## 依赖安装 @@ -43,14 +44,45 @@ nlohmann-json3-dev openssh-server screen ``` +3. ROS2 依赖（可选，用于哨兵）： + ```bash + # 安装 ROS2 Humble + sudo apt install ros-humble-desktop + + # 编译 rm_msgs 包 + cd ~/rm_msgs + source /opt/ros/humble/setup.bash + colcon build + ``` + ## 编译与运行 +### 标准编译（不含ROS2） + ```bash cmake -B build make -C build/ -j$(nproc) ./build/auto_aim_test # 运行测试 ``` +### ROS2编译（哨兵专用） + +```bash +# 设置ROS2环境 +source /opt/ros/humble/setup.bash +source ~/rm_msgs/install/setup.bash + +# 编译 +cmake -B build +make -C build/ -j$(nproc) + +# 运行ROS2版本程序 +./build/sentry_mpc configs/sentry.yaml +./build/capture_ros configs/calibration.yaml -o assets/img_with_q +``` + +**注意**：CMake会自动检测ROS2环境，如果检测到ROS2和rm_msgs包，会自动启用ROS2支持并编译相关程序。 + ## 可执行目标 ### 主程序（task） @@ -59,27 +91,31 @@ make -C build/ -j$(nproc) |------|------|----------| | `standard` | 步兵自瞄 | `configs/standard3.yaml` | | `standard_mpc` | 步兵自瞄（MPC 规划） | 需指定 | +| `sentry_mpc` | 哨兵自瞄（ROS2通信） | 需指定 | | `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` | +| `balance_infantry` | 平衡步兵 | 需指定 | +| `balance_infantry_mpc` | 平衡步兵（MPC规划） | 需指定 | | `auto_aim_debug_mpc` | 自瞄 MPC 调试 | 需指定 | | `auto_buff_debug` | 打符调试 | 需指定 | | `auto_buff_debug_mpc` | 打符 MPC 调试 | 需指定 | | `mt_auto_aim_debug` | 多线程自瞄调试 | 需指定 | +**注意**：`sentry_mpc` 需要ROS2环境，只在检测到ROS2时才会编译。 + ### 标定工具（calibration） -| 目标 | 说明 | -|------|------| -| `capture` | 采集标定图像 | -| `calibrate_camera` | 相机内参标定 | -| `calibrate_handeye` | 手眼标定 | -| `calibrate_robotworld_handeye` | 机器人-世界手眼标定 | -| `split_video` | 视频拆帧 | +| 目标 | 说明 | 通信方式 | +|------|------|----------| +| `capture` | 采集标定图像 | 串口 | +| `capture_ros` | 采集标定图像（ROS2） | ROS2话题 | +| `calibrate_camera` | 相机内参标定 | - | +| `calibrate_handeye` | 手眼标定 | - | +| `calibrate_robotworld_handeye` | 机器人-世界手眼标定 | - | +| `split_video` | 视频拆帧 | - | + +**注意**：`capture_ros` 需要ROS2环境，只在检测到ROS2时才会编译。 ### 测试用例（test） @@ -225,11 +261,78 @@ make -C build/ -j$(nproc) ### 4. ROS2 通信（哨兵专有） +**rm_msgs 自定义消息** + | 方向 | 话题 | 消息类型 | 内容 | |------|------|----------|------| -| 发布 | `auto_aim_target_pos` | `std_msgs/String` | CSV: "x,y,z,w" | -| 订阅 | `enemy_status` | `sp_msgs/EnemyStatusMsg` | 无敌敌人 ID 列表 | -| 订阅 | `autoaim_target` | `sp_msgs/AutoaimTargetMsg` | 目标敌人 ID 列表 | +| 发布 | `data_aim` | `rm_msgs/DataAim` | 视觉控制指令 | +| 订阅 | `data_mcu` | `rm_msgs/DataMCU` | MCU状态数据 | + +**DataAim 消息定义（视觉 → MCU）** + +``` +uint8 mode # 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火 +float32 yaw # 目标偏航角 (rad) +float32 yaw_vel # 偏航角速度 (rad/s) +float32 yaw_acc # 偏航角加速度 (rad/s²) +float32 pitch # 目标俯仰角 (rad) +float32 pitch_vel # 俯仰角速度 (rad/s) +float32 pitch_acc # 俯仰角加速度 (rad/s²) +``` + +**DataMCU 消息定义（MCU → 视觉）** + +``` +uint8 mode # 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符 +float32 q0 # 四元数 w +float32 q1 # 四元数 x +float32 q2 # 四元数 y +float32 q3 # 四元数 z +float32 yaw # 偏航角 (rad) +float32 yaw_vel # 偏航角速度 (rad/s) +float32 pitch # 俯仰角 (rad) +float32 pitch_vel # 俯仰角速度 (rad/s) +float32 bullet_speed # 弹速 (m/s) +uint16 bullet_count # 子弹累计发送次数 +``` + +**ROS2 程序使用说明** + +1. 编译 rm_msgs 包： + ```bash + cd ~/rm_msgs + source /opt/ros/humble/setup.bash + colcon build + ``` + +2. 编译视觉程序： + ```bash + cd /home/robofish/MOVE_AI + source /opt/ros/humble/setup.bash + source ~/rm_msgs/install/setup.bash + cmake -B build + make -C build/ sentry_mpc capture_ros -j$(nproc) + ``` + +3. 运行程序： + ```bash + # 运行哨兵自瞄 + source /opt/ros/humble/setup.bash + source ~/rm_msgs/install/setup.bash + ./build/sentry_mpc configs/sentry.yaml + + # 运行标定采集 + ./build/capture_ros configs/calibration.yaml -o assets/img_with_q + ``` + +**ROS2 兼容性说明** + +- 项目支持条件编译，自动检测ROS2环境 +- 如果未安装ROS2，只编译串口/CAN版本的程序 +- 如果安装了ROS2和rm_msgs，会额外编译ROS2版本： + - `sentry_mpc`: 使用ROS2通信的哨兵自瞄程序 + - `capture_ros`: 使用ROS2通信的标定采集程序 +- ROS2版本与串口版本功能完全相同，只是通信方式不同 ### 5. 协议总览 @@ -238,4 +341,4 @@ make -C build/ -j$(nproc) | CBoard | CAN | 1Mbps | 8B | — | C 板 (STM32F407) | | Gimbal | 串口 | 可配置 | 29/43B | CRC16 | 达妙 MC02 (STM32H7) | | DM IMU | 串口 | 921600 | 57B | CRC16 | 达妙 IMU | -| ROS2 | TCP/IP | — | 可变 | — | 导航系统 | +| ROS2 | DDS | — | 可变 | DDS | 哨兵导航系统 | diff --git a/calibration/capture_ros.cpp b/calibration/capture_ros.cpp new file mode 100644 index 0000000..3e88927 --- /dev/null +++ b/calibration/capture_ros.cpp @@ -0,0 +1,164 @@ +#ifdef USE_ROS2 + +#include +#include + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "calibration/board_pattern.hpp" +#include "src/component/img_tools.hpp" +#include "src/component/logger.hpp" +#include "src/component/math_tools.hpp" +#include "src/device/camera.hpp" +#include "src/device/gimbal/gimbal_ros.hpp" + +const std::string keys = + "{help h usage ? | | 输出命令行参数说明}" + "{@config-path c | configs/calibration.yaml | yaml配置文件路径 }" + "{output-folder o | assets/img_with_q | 输出文件夹路径 }"; + +void write_q(const std::string q_path, const Eigen::Quaterniond & q) +{ + std::ofstream q_file(q_path); + Eigen::Vector4d xyzw = q.coeffs(); + // 输出顺序为wxyz + q_file << fmt::format("{} {} {} {}", xyzw[3], xyzw[0], xyzw[1], xyzw[2]); + q_file.close(); +} + +void capture_loop( + const std::string & config_path, const std::string & output_folder, + std::shared_ptr node) +{ + // 从配置文件加载标定板参数（支持 circles_grid 和 chessboard） + auto yaml = YAML::LoadFile(config_path); + auto board_pattern = calibration::load_board_pattern(yaml); + + device::GimbalROS gimbal(config_path, node); + device::Camera camera(config_path); + cv::Mat img; + std::chrono::steady_clock::time_point timestamp; + + // ROS2 spin线程 + std::atomic quit = false; + auto ros_thread = std::thread([&]() { + while (!quit && rclcpp::ok()) { + rclcpp::spin_some(node); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + }); + + int count = 0; + component::logger()->info("[CaptureROS] Started with ROS2 communication"); + component::logger()->info("[CaptureROS] Press 's' to save, 'q' to quit"); + + while (rclcpp::ok()) { + camera.read(img, timestamp); + Eigen::Quaterniond q = gimbal.q(timestamp); + + // 在图像上显示欧拉角，用来判断imuabs系的xyz正方向，同时判断imu是否存在零漂 + auto img_with_ypr = img.clone(); + Eigen::Vector3d zyx = component::eulers(q, 2, 1, 0) * 57.3; // degree + component::draw_text(img_with_ypr, fmt::format("Z {:.2f}", zyx[0]), {40, 40}, {0, 0, 255}); + component::draw_text(img_with_ypr, fmt::format("Y {:.2f}", zyx[1]), {40, 80}, {0, 0, 255}); + component::draw_text(img_with_ypr, fmt::format("X {:.2f}", zyx[2]), {40, 120}, {0, 0, 255}); + + std::vector centers_2d; + bool success; + if (board_pattern.pattern_type == calibration::PatternType::chessboard) { + // 棋盘格检测很慢，先在缩小图上快速检测，再映射回原图做亚像素精化 + cv::Mat small; + double scale = 0.5; + cv::resize(img, small, {}, scale, scale); + std::vector small_pts; + success = calibration::find_pattern_points(small, board_pattern, small_pts); + if (success) { + for (auto & p : small_pts) { p.x /= scale; p.y /= scale; } + cv::Mat gray; + cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY); + cv::cornerSubPix( + gray, small_pts, cv::Size(11, 11), cv::Size(-1, -1), + cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, 1e-3)); + centers_2d = std::move(small_pts); + } + } else { + success = calibration::find_pattern_points(img, board_pattern, centers_2d); + } + cv::drawChessboardCorners(img_with_ypr, board_pattern.pattern_size, centers_2d, success); + cv::resize(img_with_ypr, img_with_ypr, {}, 0.5, 0.5); // 显示时缩小图片尺寸 + + // 按"s"保存图片和对应四元数，按"q"退出程序 + cv::imshow("Press s to save, q to quit", img_with_ypr); + auto key = cv::waitKey(1); + if (key == 'q') + break; + else if (key != 's') + continue; + + // 保存图片和四元数 + count++; + auto img_path = fmt::format("{}/{}.jpg", output_folder, count); + auto q_path = fmt::format("{}/{}.txt", output_folder, count); + cv::imwrite(img_path, img); + write_q(q_path, q); + component::logger()->info("[{}] Saved in {}", count, output_folder); + } + + quit = true; + if (ros_thread.joinable()) ros_thread.join(); + + // 离开该作用域时，camera和gimbal会自动关闭 +} + +int main(int argc, char * argv[]) +{ + // 读取命令行参数 + cv::CommandLineParser cli(argc, argv, keys); + if (cli.has("help")) { + cli.printMessage(); + return 0; + } + auto config_path = cli.get(0); + auto output_folder = cli.get("output-folder"); + + // 初始化ROS2 + rclcpp::init(argc, argv); + auto node = std::make_shared("capture_ros"); + + // 新建输出文件夹 + std::filesystem::create_directory(output_folder); + + // 从配置文件读取标定板类型和尺寸 + auto yaml = YAML::LoadFile(config_path); + auto board_pattern = calibration::load_board_pattern(yaml); + component::logger()->info( + "标定板类型: {}, 尺寸: {}列{}行", + calibration::pattern_name(board_pattern.pattern_type), + board_pattern.pattern_size.width, board_pattern.pattern_size.height); + + // 主循环，保存图片和对应四元数 + capture_loop(config_path, output_folder, node); + + component::logger()->warn("注意四元数输出顺序为wxyz"); + + 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 diff --git a/src/device/CMakeLists.txt b/src/device/CMakeLists.txt index d1f612c..0dcdb0a 100644 --- a/src/device/CMakeLists.txt +++ b/src/device/CMakeLists.txt @@ -5,16 +5,23 @@ find_package(yaml-cpp REQUIRED) add_subdirectory(serial) # 创建目标 device -add_library(device STATIC - hikrobot/hikrobot.cpp - mindvision/mindvision.cpp - usbcamera/usbcamera.cpp +set(DEVICE_SOURCES + hikrobot/hikrobot.cpp + mindvision/mindvision.cpp + usbcamera/usbcamera.cpp camera.cpp cboard.cpp dm_imu/dm_imu.cpp gimbal/gimbal.cpp ) +# 如果启用ROS2，添加ROS2版本的gimbal +if(USE_ROS2) + list(APPEND DEVICE_SOURCES gimbal/gimbal_ros.cpp) +endif() + +add_library(device STATIC ${DEVICE_SOURCES}) + # hikrobot target_include_directories(device PUBLIC hikrobot/include) if(CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64") @@ -35,4 +42,10 @@ else() message(FATAL_ERROR "Unsupported architecture: ${CMAKE_HOST_SYSTEM_PROCESSOR}!") endif() -target_link_libraries(device MvCameraControl MVSDK usb-1.0 yaml-cpp serial) \ No newline at end of file +target_link_libraries(device MvCameraControl MVSDK usb-1.0 yaml-cpp serial) + +# 如果启用ROS2，添加ROS2依赖 +if(USE_ROS2) + target_include_directories(device PUBLIC ${rclcpp_INCLUDE_DIRS} ${rm_msgs_INCLUDE_DIRS}) + target_link_libraries(device ${rclcpp_LIBRARIES}) +endif() \ No newline at end of file diff --git a/src/device/gimbal/gimbal_ros.cpp b/src/device/gimbal/gimbal_ros.cpp new file mode 100644 index 0000000..0561804 --- /dev/null +++ b/src/device/gimbal/gimbal_ros.cpp @@ -0,0 +1,144 @@ +#ifdef USE_ROS2 + +#include "gimbal_ros.hpp" + +#include "src/component/logger.hpp" +#include "src/component/math_tools.hpp" +#include "src/component/yaml.hpp" + +namespace device +{ + +GimbalROS::GimbalROS(const std::string & config_path, std::shared_ptr node) +: node_(node) +{ + // 创建发布者和订阅者 + aim_pub_ = node_->create_publisher("data_aim", 10); + mcu_sub_ = node_->create_subscription( + "data_mcu", 10, std::bind(&GimbalROS::mcu_callback, this, std::placeholders::_1)); + + component::logger()->info("[GimbalROS] ROS2 communication initialized"); + component::logger()->info("[GimbalROS] Publishing to: data_aim"); + component::logger()->info("[GimbalROS] Subscribing to: data_mcu"); +} + +GimbalROS::~GimbalROS() +{ + component::logger()->info("[GimbalROS] Shutting down"); +} + +GimbalMode GimbalROS::mode() const +{ + std::lock_guard lock(mutex_); + return mode_; +} + +GimbalState GimbalROS::state() const +{ + std::lock_guard lock(mutex_); + return state_; +} + +std::string GimbalROS::str(GimbalMode mode) const +{ + switch (mode) { + case GimbalMode::IDLE: + return "IDLE"; + case GimbalMode::AUTO_AIM: + return "AUTO_AIM"; + case GimbalMode::SMALL_BUFF: + return "SMALL_BUFF"; + case GimbalMode::BIG_BUFF: + return "BIG_BUFF"; + default: + return "INVALID"; + } +} + +Eigen::Quaterniond GimbalROS::q(std::chrono::steady_clock::time_point t) +{ + while (true) { + auto [q_a, t_a] = queue_.pop(); + auto [q_b, t_b] = queue_.front(); + auto t_ab = component::delta_time(t_a, t_b); + auto t_ac = component::delta_time(t_a, t); + auto k = t_ac / t_ab; + Eigen::Quaterniond q_c = q_a.slerp(k, q_b).normalized(); + if (t < t_a) return q_c; + if (!(t_a < t && t <= t_b)) continue; + + return q_c; + } +} + +void GimbalROS::send( + bool control, bool fire, float yaw, float yaw_vel, float yaw_acc, float pitch, float pitch_vel, + float pitch_acc) +{ + auto msg = rm_msgs::msg::DataAim(); + msg.mode = control ? (fire ? 2 : 1) : 0; + msg.yaw = yaw; + msg.yaw_vel = yaw_vel; + msg.yaw_acc = yaw_acc; + msg.pitch = pitch; + msg.pitch_vel = pitch_vel; + msg.pitch_acc = pitch_acc; + + aim_pub_->publish(msg); +} + +void GimbalROS::mcu_callback(const rm_msgs::msg::DataMCU::SharedPtr msg) +{ + auto t = std::chrono::steady_clock::now(); + + // 验证四元数范数 + float q_norm = msg->q0 * msg->q0 + msg->q1 * msg->q1 + msg->q2 * msg->q2 + msg->q3 * msg->q3; + if (q_norm < 0.9f || q_norm > 1.1f) { + component::logger()->warn("[GimbalROS] Invalid quaternion norm {:.3f}, skipping", q_norm); + return; + } + + // 验证模式 + if (msg->mode > 3) { + component::logger()->warn("[GimbalROS] Invalid mode {}, skipping", msg->mode); + return; + } + + // 存储四元数到队列 + Eigen::Quaterniond q(msg->q0, msg->q1, msg->q2, msg->q3); + queue_.push({q, t}); + + // 更新状态 + std::lock_guard lock(mutex_); + + state_.yaw = msg->yaw; + state_.yaw_vel = msg->yaw_vel; + state_.pitch = msg->pitch; + state_.pitch_vel = msg->pitch_vel; + state_.bullet_speed = msg->bullet_speed; + state_.bullet_count = msg->bullet_count; + + // 更新模式 + switch (msg->mode) { + case 0: + mode_ = GimbalMode::IDLE; + break; + case 1: + mode_ = GimbalMode::AUTO_AIM; + break; + case 2: + mode_ = GimbalMode::SMALL_BUFF; + break; + case 3: + mode_ = GimbalMode::BIG_BUFF; + break; + default: + mode_ = GimbalMode::IDLE; + component::logger()->warn("[GimbalROS] Invalid mode: {}", msg->mode); + break; + } +} + +} // namespace device + +#endif // USE_ROS2 diff --git a/src/device/gimbal/gimbal_ros.hpp b/src/device/gimbal/gimbal_ros.hpp new file mode 100644 index 0000000..5d075f8 --- /dev/null +++ b/src/device/gimbal/gimbal_ros.hpp @@ -0,0 +1,58 @@ +#ifndef DEVICE__GIMBAL_ROS_HPP +#define DEVICE__GIMBAL_ROS_HPP + +#ifdef USE_ROS2 + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rm_msgs/msg/data_aim.hpp" +#include "rm_msgs/msg/data_mcu.hpp" +#include "src/component/thread_safe_queue.hpp" +#include "src/device/gimbal/gimbal.hpp" + +namespace device +{ + +class GimbalROS +{ +public: + GimbalROS(const std::string & config_path, std::shared_ptr node); + + ~GimbalROS(); + + GimbalMode mode() const; + GimbalState state() const; + std::string str(GimbalMode mode) const; + Eigen::Quaterniond q(std::chrono::steady_clock::time_point t); + + void send( + bool control, bool fire, float yaw, float yaw_vel, float yaw_acc, float pitch, float pitch_vel, + float pitch_acc); + +private: + std::shared_ptr node_; + + rclcpp::Publisher::SharedPtr aim_pub_; + rclcpp::Subscription::SharedPtr mcu_sub_; + + mutable std::mutex mutex_; + + GimbalMode mode_ = GimbalMode::IDLE; + GimbalState state_; + component::ThreadSafeQueue> + queue_{1000}; + + void mcu_callback(const rm_msgs::msg::DataMCU::SharedPtr msg); +}; + +} // namespace device + +#endif // USE_ROS2 + +#endif // DEVICE__GIMBAL_ROS_HPP diff --git a/src/task/sentry.cpp b/src/task/sentry.cpp deleted file mode 100644 index 99f8285..0000000 --- a/src/task/sentry.cpp +++ /dev/null @@ -1,106 +0,0 @@ -#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 deleted file mode 100644 index 821cea1..0000000 --- a/src/task/sentry_bp.cpp +++ /dev/null @@ -1,104 +0,0 @@ -#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 deleted file mode 100644 index 008062b..0000000 --- a/src/task/sentry_debug.cpp +++ /dev/null @@ -1,196 +0,0 @@ -#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_mpc.cpp b/src/task/sentry_mpc.cpp new file mode 100644 index 0000000..792b888 --- /dev/null +++ b/src/task/sentry_mpc.cpp @@ -0,0 +1,179 @@ +#ifdef USE_ROS2 + +#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/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; + } + + // 初始化ROS2 + rclcpp::init(argc, argv); + auto node = std::make_shared("sentry_mpc"); + + component::Exiter exiter; + component::Plotter plotter; + component::Recorder recorder; + + 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); + + 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); + } + }); + + // ROS2 spin线程 + auto ros_thread = std::thread([&]() { + while (!quit && rclcpp::ok()) { + rclcpp::spin_some(node); + std::this_thread::sleep_for(1ms); + } + }); + + component::logger()->info("[SentryMPC] Started with ROS2 communication"); + + while (!exiter.exit() && rclcpp::ok()) { + 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(); + 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 diff --git a/src/task/sentry_multithread.cpp b/src/task/sentry_multithread.cpp deleted file mode 100644 index 9446640..0000000 --- a/src/task/sentry_multithread.cpp +++ /dev/null @@ -1,125 +0,0 @@ -#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