diff --git a/CMakeLists.txt b/CMakeLists.txt index 8e2d049c..fecb845e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,9 +22,8 @@ endif() ## 依赖查找 find_package(yaml-cpp REQUIRED) -find_package(OpenCV 4.5 REQUIRED) +find_package(OpenCV REQUIRED) find_package(OpenVINO REQUIRED) -# find_package(Ceres REQUIRED) ## ROS2 相关依赖 find_package(type_description_interfaces REQUIRED) @@ -33,32 +32,21 @@ find_package(rclcpp REQUIRED) find_package(visualization_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) -find_package(rmcs_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rmcs_msgs REQUIRED) find_package(rmcs_executor REQUIRED) find_package(rmcs_description REQUIRED) find_package(fast_tf REQUIRED) find_package(hikcamera REQUIRED) -## 目录包含 -include_directories( - ${PROJECT_SOURCE_DIR}/src/ - - ${type_description_interfaces_INCLUDE_DIRS} - ${rclcpp_INCLUDE_DIRS} - ${ament_index_cpp_INCLUDE_DIRS} - ${visualization_msgs_INCLUDE_DIRS} - ${geometry_msgs_INCLUDE_DIRS} - ${tf2_ros_INCLUDE_DIRS} - ${rmcs_msgs_INCLUDE_DIRS} - - ${rmcs_executor_INCLUDE_DIRS} - ${rmcs_description_INCLUDE_DIRS} - ${fast_tf_INCLUDE_DIRS} - ${hikcamera_INCLUDE_DIRS} -) ## 代码资源搜索 +file(GLOB_RECURSE AUTO_AIM_UTILITY + CONFIGURE_DEPENDS + ${PROJECT_SOURCE_DIR}/src/utility/*.cpp + ${PROJECT_SOURCE_DIR}/src/utility/*.cc +) file(GLOB_RECURSE AUTO_AIM_KERNEL CONFIGURE_DEPENDS ${PROJECT_SOURCE_DIR}/src/kernel/*.cpp @@ -68,8 +56,35 @@ file(GLOB_RECURSE AUTO_AIM_MODULE CONFIGURE_DEPENDS ${PROJECT_SOURCE_DIR}/src/module/*.cpp ${PROJECT_SOURCE_DIR}/src/module/*.cc - ${PROJECT_SOURCE_DIR}/src/utility/*.cpp - ${PROJECT_SOURCE_DIR}/src/utility/*.cc +) + +## 基础设施: utility +add_library( + ${PROJECT_NAME}_utility SHARED + ${AUTO_AIM_UTILITY} +) +target_include_directories( + ${PROJECT_NAME}_utility + PUBLIC + ${PROJECT_SOURCE_DIR}/src/ + ${rmcs_msgs_INCLUDE_DIRS} + PRIVATE + ${visualization_msgs_INCLUDE_DIRS} + ${geometry_msgs_INCLUDE_DIRS} + ${tf2_ros_INCLUDE_DIRS} +) +target_link_libraries( + ${PROJECT_NAME}_utility + PUBLIC + yaml-cpp::yaml-cpp + ament_index_cpp::ament_index_cpp + rclcpp::rclcpp + ${OpenCV_LIBS} + ${rmcs_msgs_LIBRARIES} + PRIVATE + ${visualization_msgs_LIBRARIES} + ${geometry_msgs_LIBRARIES} + ${tf2_ros_LIBRARIES} ) ## 核心构建: kernel and module @@ -77,29 +92,38 @@ add_library( ${PROJECT_NAME}_kernel SHARED ${AUTO_AIM_KERNEL} ) +target_include_directories( + ${PROJECT_NAME}_kernel + PUBLIC + ${PROJECT_SOURCE_DIR}/src/ + PRIVATE + ${hikcamera_INCLUDE_DIRS} +) target_link_libraries( ${PROJECT_NAME}_kernel - ${OpenCV_LIBS} - yaml-cpp::yaml-cpp + PUBLIC + ${PROJECT_NAME}_utility ) + add_library( ${PROJECT_NAME}_module SHARED ${AUTO_AIM_MODULE} ) +target_include_directories( + ${PROJECT_NAME}_module + PUBLIC + ${PROJECT_SOURCE_DIR}/src/ + PRIVATE + ${hikcamera_INCLUDE_DIRS} +) target_link_libraries( ${PROJECT_NAME}_module PUBLIC - yaml-cpp::yaml-cpp - ament_index_cpp::ament_index_cpp + ${PROJECT_NAME}_utility openvino::runtime - rclcpp::rclcpp - ${OpenCV_LIBS} - ${hikcamera_LIBRARIES} - ${visualization_msgs_LIBRARIES} - ${geometry_msgs_LIBRARIES} - ${tf2_ros_LIBRARIES} PRIVATE tinympcstatic + ${hikcamera_LIBRARIES} ) ## 组件构建: component @@ -107,22 +131,17 @@ add_library( ${PROJECT_NAME}_component SHARED ${PROJECT_SOURCE_DIR}/src/component.cpp ) -target_link_libraries( +target_include_directories( ${PROJECT_NAME}_component - ${PROJECT_NAME}_kernel - ${PROJECT_NAME}_module - ${rmcs_executor_LIBRARIES} -) - -## 运行时构建: runtime -add_executable( - ${PROJECT_NAME}_runtime - ${PROJECT_SOURCE_DIR}/src/runtime.cpp + PRIVATE + ${rmcs_description_INCLUDE_DIRS} + ${rmcs_executor_INCLUDE_DIRS} ) target_link_libraries( - ${PROJECT_NAME}_runtime + ${PROJECT_NAME}_component ${PROJECT_NAME}_kernel ${PROJECT_NAME}_module + ${rmcs_executor_LIBRARIES} ) ## 安装配置 @@ -131,26 +150,19 @@ install( ${PROJECT_NAME}_component ${PROJECT_NAME}_kernel ${PROJECT_NAME}_module + ${PROJECT_NAME}_utility DESTINATION lib/ ) install( DIRECTORY config/ - launch/ models/ DESTINATION share/${PROJECT_NAME} ) -install( - TARGETS - ${PROJECT_NAME}_runtime - DESTINATION - lib/${PROJECT_NAME} -) ## plugins for rmcs_executor -find_package(pluginlib REQUIRED) pluginlib_export_plugin_description_file( rmcs_executor plugins.xml ) diff --git a/README.md b/README.md index bfaf7db9..0f6e1910 100644 --- a/README.md +++ b/README.md @@ -21,16 +21,6 @@ git clone https://github.com/Alliance-Algorithm/rmcs_auto_aim_v2.git # 构建依赖 build-rmcs - -# 启动运行时 -ros2 run rmcs_auto_aim_v2 rmcs_auto_aim_v2_runtime - -# 或使用 launch 文件启动（带自动重启） -ros2 launch rmcs_auto_aim_v2 launch.py - -# 运行测试 -colcon test --packages-select rmcs_auto_aim_v2 -colcon test-result --all --verbose ``` ### 在 RMCS 控制系统中启用自瞄 @@ -43,6 +33,44 @@ colcon test-result --all --verbose 与其他 RMCS 组件不同，自瞄组件的参数不由机器人 YAML 管理，而是由本项目自己的配置文件统一配置，因此实例名可以随意取，不影响参数读取。 +**输入接口** + +| 接口名称 | 类型 | 必填 | 说明 | +|----------|------|------|------| +| `/auto_aim/camera_transform` | `Eigen::Isometry3d` | 否 | 相机在 OdomImu 坐标系下的位姿 | +| `/auto_aim/barrel_direction` | `Eigen::Vector3d` | 否 | 枪口在 OdomImu 坐标系下的方向 | +| `/referee/id` | `rmcs_msgs::RobotId` | 否 | 机器人 ID，用于判断敌方颜色 | + +如果使用 `fast_tf` 发布 TF 树，可以通过以下方式获取这两个输入： + +```cpp +#include +#include + +// 从 TF 树中查询相机位姿 +auto camera_transform = fast_tf::lookup_transform< + rmcs_description::OdomImu, + rmcs_description::CameraLink>(*tf); + +// 从 TF 树中查询枪口方向 +auto barrel_direction = *fast_tf::cast( + rmcs_description::PitchLink::DirectionVector { Eigen::Vector3d::UnitX() }, *tf); +``` + +**输出接口** + +| 接口名称 | 类型 | 说明 | +|----------|------|------| +| `/auto_aim/should_control` | `bool` | 是否需要云台跟踪 | +| `/auto_aim/should_shoot` | `bool` | 是否可以发弹 | +| `/auto_aim/control_direction` | `Eigen::Vector3d` | 目标方向向量 | +| `/auto_aim/robot_center` | `Eigen::Vector3d` | 目标机器人中心位置 | +| `/auto_aim/yaw_rate` | `double` | yaw 角速度 | +| `/auto_aim/pitch_rate` | `double` | pitch 角速度 | +| `/auto_aim/yaw_acc` | `double` | yaw 角加速度 | +| `/auto_aim/pitch_acc` | `double` | pitch 角加速度 | +| `/auto_aim/feedforward_valid` | `bool` | 前馈数据是否有效 | + ### 配置文件 配置文件按以下优先级加载（位于运行时 share 目录下）： @@ -52,25 +80,11 @@ colcon test-result --all --verbose 3. `config.override.yaml` / `config.override.yml` 4. `config.yaml` / `config.yml` -启用后，RMCS 控制系统启动时会自动加载 Component，但 Runtime 需要单独启动： - -```sh -# 方式一：直接运行 -ros2 run rmcs_auto_aim_v2 rmcs_auto_aim_v2_runtime - -# 方式二：通过 launch 文件启动（带自动重启） -ros2 launch rmcs_auto_aim_v2 launch.py -``` - -两个进程通过共享内存（`feishu`）通信：Runtime 负责图像采集、识别、跟踪、火控解算等算法逻辑，Component 负责与 RMCS 控制系统对接并下发控制指令 - ## 项目架构 ### 文件排布 -- `adapter`: 车辆适配层，将不同车型的底盘、云台等接口统一为自瞄可用的抽象接口 - -- `kernel`: 运行时业务内核，与业务逻辑强相关，包含识别、跟踪、位姿估计、火控、可视化等核心流程，以及进程间通信（`feishu`） +- `kernel`: 运行时业务内核，与业务逻辑强相关，包含识别、跟踪、位姿估计、火控、可视化等核心流程，以及 `AutoAim` 类（主循环） - `module`: 特定任务的通用实现模块，不包含运行时逻辑，可在不同上下文中复用 @@ -78,13 +92,13 @@ ros2 launch rmcs_auto_aim_v2 launch.py ### 架构设计 -本项目采用**进程分离**架构，自瞄系统分为两个独立进程： +本项目采用**单进程多线程**架构，自瞄系统作为 Component 集成到 RMCS 控制系统中： -- **Component**（`component.cpp`）：运行在 RMCS 控制系统中，负责与 RMCS 通信，通过进程间共享内存（`feishu`）接收 Runtime 下发的控制指令。与 RMCS 共享进程空间，不执行任何图像处理或算法逻辑 +- **AutoAim**（`auto_aim.hpp`）：在独立的 `worker` 线程中运行自瞄主循环，负责图像采集、目标识别、位姿估计、跟踪、火控解算等算法逻辑 -- **Runtime**（`runtime.cpp`）：独立运行的自瞄主进程，负责图像采集、目标识别、位姿估计、跟踪、火控解算等内存密集型操作。通过 `feishu` 将控制指令发送给 Component +- **AutoAimComponent**（`component.cpp`）：运行在 RMCS 主线程中，负责与 RMCS 控制系统对接。通过 `with_context()` 传入相机位姿等信息，通过 `with_command()` 获取自瞄控制指令 -这种分离确保了自瞄系统在进行频繁内存操作时，即使出现异常也不会影响 RMCS 控制系统的稳定性。两个进程通过 `feishu`（基于共享内存的进程间通信）进行数据交换 +两个线程通过 `std::mutex` 保护的共享状态进行数据交换，使用 `std::atomic` 避免不必要的锁竞争 ## 调试指南 diff --git a/config/config.yaml b/config/config.yaml index 7894c97b..9c01e30d 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -1,4 +1,3 @@ -localhost_develop: false use_visualization: true camera_matrix: @@ -20,7 +19,7 @@ capturer: show_loss_framerate_interval: 500 reconnect_wait_interval: 100 - record_enable: false + record_enable: true saving_pathes: ["/home/root/autoaim/", "/home/ubuntu/autoaim/", "/tmp/autoaim/"] max_duration_seconds: 60 @@ -89,7 +88,7 @@ pose_estimator: fixed_outpost_pitch: true fixed_normal_pitch: false - outpost_armor_thickness: 0.02 + outpost_armor_thickness: 0.022 fire_control: initial_bullet_speed: 21.4 # m/s diff --git a/doc/streaming.md b/doc/streaming.md index 46370c8a..5c942bdd 100644 --- a/doc/streaming.md +++ b/doc/streaming.md @@ -115,8 +115,7 @@ ffmpeg -i "rtp://:5000" -c:v copy video.mp4 using namespace rmcs::debug; -// 检查依赖支持，对于 rmcs-runtime 可以忽略这个步骤 -// 镜像内已经内置好了相关依赖 +// 检查依赖支持 auto check = StreamContext::check_support(); if (!check) std::println("{}", check.error()); diff --git a/launch/launch.py b/launch/launch.py deleted file mode 100644 index 93eea5df..00000000 --- a/launch/launch.py +++ /dev/null @@ -1,17 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description(): - launch = LaunchDescription() - - node = Node( - package="rmcs_auto_aim_v2", - executable="rmcs_auto_aim_v2_runtime", - output="screen", - respawn=True, - respawn_delay=1.0, - ) - launch.add_action(node) - - return launch diff --git a/src/adapter/adapter.hpp b/src/adapter/adapter.hpp deleted file mode 100644 index 72dbadbe..00000000 --- a/src/adapter/adapter.hpp +++ /dev/null @@ -1,29 +0,0 @@ -#pragma once - -#include -#include -#include - -namespace rmcs { - -class Adapter { -public: - explicit Adapter(rmcs_executor::Component& component) { component.register_input("/tf", tf_); } - - [[nodiscard]] auto ready() const -> bool { return tf_.ready(); } - - [[nodiscard]] auto camera_transform() const -> Eigen::Isometry3d { - return fast_tf::lookup_transform( - *tf_); - } - - [[nodiscard]] auto barrel_direction() const -> Eigen::Vector3d { - return *fast_tf::cast( - rmcs_description::PitchLink::DirectionVector { Eigen::Vector3d::UnitX() }, *tf_); - } - -private: - rmcs_executor::Component::InputInterface tf_; -}; - -} // namespace rmcs diff --git a/src/adapter/deformable-infantry.hpp b/src/adapter/deformable-infantry.hpp deleted file mode 100644 index 72dbadbe..00000000 --- a/src/adapter/deformable-infantry.hpp +++ /dev/null @@ -1,29 +0,0 @@ -#pragma once - -#include -#include -#include - -namespace rmcs { - -class Adapter { -public: - explicit Adapter(rmcs_executor::Component& component) { component.register_input("/tf", tf_); } - - [[nodiscard]] auto ready() const -> bool { return tf_.ready(); } - - [[nodiscard]] auto camera_transform() const -> Eigen::Isometry3d { - return fast_tf::lookup_transform( - *tf_); - } - - [[nodiscard]] auto barrel_direction() const -> Eigen::Vector3d { - return *fast_tf::cast( - rmcs_description::PitchLink::DirectionVector { Eigen::Vector3d::UnitX() }, *tf_); - } - -private: - rmcs_executor::Component::InputInterface tf_; -}; - -} // namespace rmcs diff --git a/src/adapter/sentry.hpp b/src/adapter/sentry.hpp deleted file mode 100644 index f7263996..00000000 --- a/src/adapter/sentry.hpp +++ /dev/null @@ -1,29 +0,0 @@ -#pragma once - -#include -#include -#include - -namespace rmcs { - -class Adapter { -public: - explicit Adapter(rmcs_executor::Component& component) { component.register_input("/tf", tf_); } - - [[nodiscard]] auto ready() const -> bool { return tf_.ready(); } - - [[nodiscard]] auto camera_transform() const -> Eigen::Isometry3d { - return fast_tf::lookup_transform(*tf_); - } - - [[nodiscard]] auto barrel_direction() const -> Eigen::Vector3d { - return *fast_tf::cast( - rmcs_description::PitchLink::DirectionVector { Eigen::Vector3d::UnitX() }, *tf_); - } - -private: - rmcs_executor::Component::InputInterface tf_; -}; - -} diff --git a/src/component.cpp b/src/component.cpp index 10963809..838fa19c 100644 --- a/src/component.cpp +++ b/src/component.cpp @@ -1,42 +1,26 @@ -#if defined(__clang__) -#pragma clang diagnostic ignored "-Wdeprecated-declarations" -#elif defined(__GNUC__) -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#endif - -#include "adapter/sentry.hpp" -#include "kernel/feishu.hpp" -#include "utility/rclcpp/node.hpp" +#include "kernel/auto_aim.hpp" #include "utility/shared/context.hpp" +#include #include -#include #include -#include namespace rmcs { -using namespace util; -using namespace kernel; - class AutoAimComponent final : public rmcs_executor::Component { + static inline const auto kNaN = std::numeric_limits::quiet_NaN(); + static inline const auto kTNaN = Eigen::Vector3d { kNaN, kNaN, kNaN }; + static inline const auto kQNaN = Eigen::Quaterniond { kNaN, kNaN, kNaN, kNaN }; + private: - static constexpr auto kNaN = std::numeric_limits::quiet_NaN(); - static inline const auto kVectorNaN = Eigen::Vector3d { kNaN, kNaN, kNaN }; + AutoAim auto_aim { }; - Adapter adapter; - RclcppNode rclcpp; - Feishu feishu; + InputInterface camera_transform; + InputInterface barrel_direction; + InputInterface robot_id; - // 当自瞄跟踪上某目标时，会开启此标志，代表需要云台跟踪 OutputInterface should_control; - - // 当火控认为可以发弹时，会开启此标志 OutputInterface should_shoot; - - // 一个手动开火的标志，开启此项，则下游需要将开火意图也纳入判断 - // OutputInterface should_manual; - OutputInterface target_direction; OutputInterface robot_center; OutputInterface yaw_rate; @@ -45,102 +29,93 @@ class AutoAimComponent final : public rmcs_executor::Component { OutputInterface pitch_acc; OutputInterface feedforward_valid; - InputInterface robot_id; - - auto make_context() const { - auto context = SystemContext { }; - - context.timestamp = Clock::now(); - - const auto dir = adapter.barrel_direction(); - context.yaw = std::atan2(dir.y(), dir.x()); - context.pitch = std::atan2(-dir.z(), std::hypot(dir.x(), dir.y())); - - const auto iso = adapter.camera_transform(); - context.camera_transform.translation = iso.translation(); - context.camera_transform.orientation = Eigen::Quaterniond(iso.rotation()); - - // TODO:无敌状态下的装甲板需要从裁判系统获取并在此更新 - context.invincible_devices = DeviceIds::None(); - - context.id = *robot_id; - - return context; - } + std::chrono::steady_clock::time_point last_command_timestamp; public: - explicit AutoAimComponent() noexcept - : adapter { *this } - , rclcpp { get_component_name() } { + explicit AutoAimComponent() noexcept { + register_input("/auto_aim/camera_transform", camera_transform, false); + register_input("/auto_aim/barrel_direction", barrel_direction, false); + register_input("/referee/id", robot_id, false); register_output("/auto_aim/should_control", should_control, false); - register_output("/auto_aim/control_direction", target_direction, Eigen::Vector3d::Zero()); - register_output("/auto_aim/robot_center", robot_center, kVectorNaN); + register_output("/auto_aim/control_direction", target_direction, kTNaN); + register_output("/auto_aim/robot_center", robot_center, kTNaN); register_output("/auto_aim/should_shoot", should_shoot, false); - register_output("/auto_aim/yaw_rate", yaw_rate, 0.0); - register_output("/auto_aim/pitch_rate", pitch_rate, 0.0); - register_output("/auto_aim/yaw_acc", yaw_acc, 0.0); - register_output("/auto_aim/pitch_acc", pitch_acc, 0.0); + register_output("/auto_aim/yaw_rate", yaw_rate, kNaN); + register_output("/auto_aim/pitch_rate", pitch_rate, kNaN); + register_output("/auto_aim/yaw_acc", yaw_acc, kNaN); + register_output("/auto_aim/pitch_acc", pitch_acc, kNaN); register_output("/auto_aim/feedforward_valid", feedforward_valid, false); - - register_input("/referee/id", robot_id, true); } - auto update() -> void override { - if (!adapter.ready()) [[unlikely]] { - feishu.send(SystemContext::kInvalid()); - return; + auto before_updating() -> void override { + if (!camera_transform.ready()) { + camera_transform.make_and_bind_directly(Eigen::Isometry3d::Identity()); } - feishu.send(make_context()); - - // Reset all command - { - *should_control = false; - *should_shoot = false; - *target_direction = kVectorNaN; - *robot_center = kVectorNaN; - *yaw_rate = kNaN; - *pitch_rate = kNaN; - *yaw_acc = kNaN; - *pitch_acc = kNaN; - *feedforward_valid = false; + if (!barrel_direction.ready()) { + barrel_direction.make_and_bind_directly(Eigen::Vector3d::UnitX()); } + if (!robot_id.ready()) { + robot_id.make_and_bind_directly(rmcs_msgs::RobotId::UNKNOWN); + } + last_command_timestamp = std::chrono::steady_clock::now(); + } - feishu.heartbeat(); - if (!feishu.latest()) return; - - // If heartbeat successfully, latest exists - const auto command = *feishu.latest(); + auto update() -> void override { + auto_aim.with_context([this](SystemContext& ctx) { + ctx.timestamp = Clock::now(); + + const auto& dir = *barrel_direction; + + ctx.yaw = std::atan2(dir.y(), dir.x()); + ctx.pitch = std::atan2(-dir.z(), std::hypot(dir.x(), dir.y())); + + const auto& iso = *camera_transform; + + ctx.camera_transform.translation = Translation { iso.translation() }; + ctx.camera_transform.orientation = Orientation { Eigen::Quaterniond(iso.rotation()) }; + + ctx.id = *robot_id; + }); + + const auto now = std::chrono::steady_clock::now(); + if (auto_aim.command_updated()) { + auto_aim.with_command([this](const AutoAimState& cmd) { + using namespace std::chrono_literals; + if (Clock::now() - cmd.timestamp > 100ms) return; + + *should_control = cmd.should_control; + *should_shoot = cmd.should_shoot; + *yaw_rate = cmd.yaw_rate; + *pitch_rate = cmd.pitch_rate; + *yaw_acc = cmd.yaw_acc; + *pitch_acc = cmd.pitch_acc; + *feedforward_valid = cmd.feedforward_valid; + *robot_center = { + cmd.robot_center.x, + cmd.robot_center.y, + cmd.robot_center.z, + }; + + if (!cmd.should_control) return; + + const auto pitch = cmd.pitch; + const auto yaw = cmd.yaw; + *target_direction = Eigen::Vector3d { + std::cos(pitch) * std::cos(yaw), + std::cos(pitch) * std::sin(yaw), + std::sin(pitch), + }; + }); + last_command_timestamp = now; + } - // Timeout using namespace std::chrono_literals; - if (Clock::now() - command.timestamp > 100ms) { - return; + if (now - last_command_timestamp > 100ms) { + *should_control = false; + *should_shoot = false; + *target_direction = kTNaN; } - - // 业务开关 - *should_control = command.should_control; - *should_shoot = command.should_shoot; - *yaw_rate = command.yaw_rate; - *pitch_rate = command.pitch_rate; - *yaw_acc = command.yaw_acc; - *pitch_acc = command.pitch_acc; - *feedforward_valid = command.feedforward_valid; - *robot_center = { - command.robot_center.x, - command.robot_center.y, - command.robot_center.z, - }; - - if (!command.should_control) return; - - const auto pitch = command.pitch; - const auto yaw = command.yaw; - *target_direction = Eigen::Vector3d { - std::cos(pitch) * std::cos(yaw), - std::cos(pitch) * std::sin(yaw), - std::sin(pitch), - }; } }; diff --git a/src/kernel/auto_aim.cpp b/src/kernel/auto_aim.cpp new file mode 100644 index 00000000..701a0f43 --- /dev/null +++ b/src/kernel/auto_aim.cpp @@ -0,0 +1,221 @@ +#include "auto_aim.hpp" + +#include "kernel/capturer.hpp" +#include "kernel/fire_control.hpp" +#include "kernel/identifier.hpp" +#include "kernel/pose_estimator.hpp" +#include "kernel/tracker.hpp" +#include "kernel/visualization.hpp" + +#include "utility/framerate.hpp" +#include "utility/math/linear.hpp" +#include "utility/panic.hpp" +#include "utility/rclcpp/configuration.hpp" +#include "utility/rclcpp/node.hpp" +#include "utility/rclcpp/parameters.hpp" +#include "utility/singleton/running.hpp" + +#include +#include +#include +#include + +using namespace rmcs; +using namespace rmcs::util; +using namespace rmcs::kernel; + +struct AutoAim::Impl { + RclcppNode node { "auto_aim" }; + + Capturer cap { }; + Identifier identifier { }; + Tracker tracker { }; + PoseEstimator estimator { }; + FireControl fire { }; + Visualization visual { }; + + std::jthread worker; + + auto run(AutoAim& self, const std::stop_token& stop) -> void { + using namespace std::chrono_literals; + + node.set_pub_topic_prefix("/rmcs/auto_aim/"); + + auto framerate = FramerateCounter { }; + framerate.set_interval(std::chrono::seconds { 5 }); + + while (util::get_running() && !stop.stop_requested()) { + node.spin_once(); + + auto image = cap.fetch_image(); + if (!image) continue; + + auto context = SystemContext::kIdentity(); + { + std::lock_guard lock { self.context_mutex }; + context = self.current_context; + } + visual.publish(context.camera_transform, "camera_link"); + + if (framerate.tick()) { + node.info("Autoaim Framerate: {}", framerate.fps()); + } + + [[maybe_unused]] auto streamer = std::experimental::scope_exit { [&] { + visual.draw_later( // 录制开关 + Text { cap.recording() ? "RECORD ON" : "RECORD OFF", { 10, 700 } }); + visual.draw_later( // 自瞄帧率 + Text { std::format("FPS: {}", framerate.fps()), { 10, 680 } }); + visual.update_image(*image); + } }; + + /// 1. Identify Armor + auto armors_2d = Armor2ds { }; + { + auto result = identifier.sync_identify(*image); + if (!result.has_value()) continue; + + for (const auto& roi : result->areas) { + visual.draw_later(roi); + } + visual.draw_later(result->armors); + visual.draw_later(result->green_light); + + using namespace rmcs_msgs; + if (context.id != RobotId::UNKNOWN) { + if (context.id.color() == RobotColor::RED) { + tracker.set_enemy_color(CampColor::BLUE); + } else { + tracker.set_enemy_color(CampColor::RED); + } + } + + tracker.set_invincible_armors(context.invincible_devices); + armors_2d = tracker.filter_armors(result->armors); + + if (armors_2d.empty()) continue; + } + + /// 2. Transform 2d to 3d + auto armors_3d = Armor3ds { }; + { + estimator.update_camera_transform(context.camera_transform); + + auto result = estimator.estimate_armor(armors_2d, *image); + + const auto& addition = estimator.addition(); + visual.draw_later(addition.detected_2d); + visual.draw_later(addition.areas); + visual.draw_later(addition.predicted_near); + visual.draw_later(addition.predicted_away); + + visual.publish(addition.origin, "origin_armors"); + visual.publish(addition.detected_3d, "outpost_lightbars"); + visual.publish({ addition.center_3d, Orientation::kIdentity() }, "outpost_center"); + + armors_3d = result; + if (armors_3d.empty()) continue; + + visual.publish(armors_3d, "visible_armors"); + } + + /// 3. Apply Tracker + auto target = tracker.decide(armors_3d, image->get_timestamp()); + auto cmd = AutoAimState::kInvalid(); + if (target.snapshot) { + auto& snapshot = target.snapshot; + auto armors = snapshot->predicted_armors(Clock::now()); + visual.publish(armors, "visible_robot"); + + const auto yaw = context.yaw; + if (auto result = fire.solve(*snapshot, yaw)) { + cmd.should_control = true; + cmd.target = target.target_id; + cmd.should_shoot = result->shoot_permitted; + cmd.yaw = result->yaw; + cmd.pitch = result->pitch; + cmd.yaw_rate = result->yaw_rate; + cmd.pitch_rate = result->pitch_rate; + cmd.yaw_acc = result->yaw_acc; + cmd.pitch_acc = result->pitch_acc; + cmd.feedforward_valid = result->feedforward_valid; + cmd.robot_center = result->center_position; + + /// TODO: + /// 统一控制侧和自瞄侧的 Pitch 符号方向定义 + visual.update_aiming_direction(cmd.yaw, -cmd.pitch); + visual.update_mpc_plan(cmd.yaw, cmd.pitch, cmd.yaw_rate, cmd.pitch_rate, + cmd.yaw_acc, cmd.pitch_acc); + } + } + visual.draw_later(Text { "ATTACK", { 10, 660 }, cmd.should_shoot ? kRed : kWhite }); + visual.draw_later(Text { "CONTROL", { 10, 640 }, cmd.should_control ? kRed : kWhite }); + + { + std::lock_guard lock { self.command_mutex }; + self.current_command = cmd; + self.unread_command.store(true, std::memory_order::release); + } + } + RclcppNode::shutdown(); + } + + explicit Impl(AutoAim& self) { + std::signal(SIGINT, +[](int) { util::set_running(false); }); + + const auto configs = util::configs(); + const auto camera_matrix = configs["camera_matrix"].as>(); + const auto distort_coeff = configs["distort_coeff"].as>(); + const auto use_visualization = configs["use_visualization"].as(); + + const auto handle_result = [&](auto runtime_name, const auto& result) { + if (!result.has_value()) { + node.error("Failed to init '{}'", runtime_name); + node.error(" {}", result.error()); + util::panic(std::format("Failed to initialize {}", runtime_name)); + } + }; + + { + auto config = configs["capturer"]; + handle_result("capturer", cap.initialize(config)); + } + { + auto config = configs["identifier"]; + const auto model_location = std::filesystem::path { util::Parameters::share_location() } + / std::filesystem::path { config["model_location"].as() }; + config["model_location"] = model_location.string(); + handle_result("identifier", identifier.initialize(config)); + } + { + auto config = configs["tracker"]; + handle_result("tracker", tracker.initialize(config)); + } + { + auto config = configs["pose_estimator"]; + handle_result("estimator", estimator.initialize(config)); + estimator.configure_camera(camera_matrix, distort_coeff); + } + { + auto config = configs["fire_control"]; + handle_result("fire_control", fire.initialize(config)); + } + if (use_visualization) { + auto config = configs["visualization"]; + handle_result("visualization", visual.initialize(config)); + } + + worker = std::jthread([this, &self](const std::stop_token& stop) { run(self, stop); }); + } + ~Impl() { + worker.request_stop(); + if (worker.joinable()) { + worker.join(); + } + } +}; + +AutoAim::AutoAim() noexcept + : pimpl { std::make_unique(*this) } { } + +AutoAim::~AutoAim() noexcept = default; diff --git a/src/kernel/auto_aim.hpp b/src/kernel/auto_aim.hpp new file mode 100644 index 00000000..49d1d9be --- /dev/null +++ b/src/kernel/auto_aim.hpp @@ -0,0 +1,42 @@ +#pragma once + +#include "utility/pimpl.hpp" +#include "utility/shared/context.hpp" + +#include +#include + +namespace rmcs { + +class AutoAim { + RMCS_PIMPL_DEFINITION(AutoAim) + +public: + template + requires std::invocable + auto with_context(WithFunc&& func) { + std::lock_guard lock(context_mutex); + func(current_context); + } + + template + requires std::invocable + auto with_command(WithFunc&& func) { + std::lock_guard lock(command_mutex); + func(current_command); + } + + auto command_updated() -> bool { + return unread_command.exchange(false, std::memory_order::acquire); + } + +private: + std::mutex context_mutex; + SystemContext current_context { SystemContext::kIdentity() }; + + std::mutex command_mutex; + std::atomic unread_command = false; + AutoAimState current_command { AutoAimState::kInvalid() }; +}; + +} diff --git a/src/kernel/fire_control.cpp b/src/kernel/fire_control.cpp index 411a2cbe..69e57685 100644 --- a/src/kernel/fire_control.cpp +++ b/src/kernel/fire_control.cpp @@ -73,7 +73,7 @@ struct FireControl::Impl { }; } - return {}; + return { }; } auto solve(predictor::Snapshot const& snapshot, double current_yaw) -> std::optional { @@ -141,15 +141,15 @@ struct FireControl::Impl { .yaw_acc = yaw_acc, .feedforward_valid = feedforward_valid, .shoot_permitted = shoot_permitted, - .center_position = center_position, + .center_position = Point3d { center_position }, }; } - MpcTrajectoryPlanner mpc_trajectory_planner {}; - ReferenceTrajectoryBuilder reference_trajectory_builder {}; - TargetSolutionSolver target_solution_solver {}; - ShootEvaluator shoot_evaluator {}; - AimPointChooser aim_point_chooser {}; + MpcTrajectoryPlanner mpc_trajectory_planner { }; + ReferenceTrajectoryBuilder reference_trajectory_builder { }; + TargetSolutionSolver target_solution_solver { }; + ShootEvaluator shoot_evaluator { }; + AimPointChooser aim_point_chooser { }; }; FireControl::FireControl() noexcept diff --git a/src/kernel/fire_control.hpp b/src/kernel/fire_control.hpp index 8130f889..b2f6d22b 100644 --- a/src/kernel/fire_control.hpp +++ b/src/kernel/fire_control.hpp @@ -1,7 +1,5 @@ #pragma once -#include - #include #include @@ -23,7 +21,7 @@ class FireControl { double yaw_acc; bool feedforward_valid; bool shoot_permitted; - Eigen::Vector3d center_position; + Point3d center_position; }; auto initialize(const YAML::Node&) noexcept -> std::expected; diff --git a/src/module/fire_control/aim_point_chooser.hpp b/src/module/fire_control/aim_point_chooser.hpp index c287d234..46914240 100644 --- a/src/module/fire_control/aim_point_chooser.hpp +++ b/src/module/fire_control/aim_point_chooser.hpp @@ -1,6 +1,6 @@ #pragma once -#include +#include #include #include diff --git a/src/module/fire_control/shoot_evaluator.hpp b/src/module/fire_control/shoot_evaluator.hpp index 5490b3c4..627e21f0 100644 --- a/src/module/fire_control/shoot_evaluator.hpp +++ b/src/module/fire_control/shoot_evaluator.hpp @@ -1,6 +1,6 @@ #pragma once -#include +#include #include #include diff --git a/src/module/fire_control/solver/aim_point_sampling.cpp b/src/module/fire_control/solver/aim_point_sampling.cpp index 7bca483f..a952b8fb 100644 --- a/src/module/fire_control/solver/aim_point_sampling.cpp +++ b/src/module/fire_control/solver/aim_point_sampling.cpp @@ -30,11 +30,12 @@ auto rmcs::fire_control::AimPointSampler::sample_aim_point_at(predictor::Snapsho AimPointChooser& chooser, TimePoint t) -> std::expected { auto predicted_armors = snapshot.predicted_armors(t); auto predicted_kinematics = snapshot.kinematics_at(t); - auto chosen_armor = chooser.choose_armor(predicted_armors, predicted_kinematics.center_position, + auto chosen_armor = chooser.choose_armor(predicted_armors, + predicted_kinematics.center_position.make(), predicted_kinematics.angular_velocity); if (!chosen_armor.has_value()) return std::unexpected { "choose_armor returned nullopt" }; - auto aim_point = Eigen::Vector3d {}; + auto aim_point = Eigen::Vector3d { }; chosen_armor->translation.copy_to(aim_point); return aim_point; } @@ -46,7 +47,7 @@ auto rmcs::fire_control::AimPointSampler::solve_aim_attitude(Eigen::Vector3d con return std::unexpected { "invalid target distance" }; } - auto solution = TrajectorySolution {}; + auto solution = TrajectorySolution { }; solution.input.v0 = bullet_speed; solution.input.target_position = aim_point; diff --git a/src/module/fire_control/solver/aim_point_sampling.hpp b/src/module/fire_control/solver/aim_point_sampling.hpp index c2cec115..2a8a36d8 100644 --- a/src/module/fire_control/solver/aim_point_sampling.hpp +++ b/src/module/fire_control/solver/aim_point_sampling.hpp @@ -1,10 +1,9 @@ #pragma once #include -#include #include -#include +#include #include "module/fire_control/aim_point_chooser.hpp" #include "module/predictor/snapshot.hpp" diff --git a/src/module/fire_control/solver/target_solution_solver.cpp b/src/module/fire_control/solver/target_solution_solver.cpp index 689b5aea..d64b445f 100644 --- a/src/module/fire_control/solver/target_solution_solver.cpp +++ b/src/module/fire_control/solver/target_solution_solver.cpp @@ -22,7 +22,7 @@ struct TargetSolutionSolver::Impl { }; auto target_kinematics = snapshot.kinematics(); - auto target_position = target_kinematics.center_position; + auto target_position = target_kinematics.center_position.make(); auto current_fly_time = target_position.norm() / bullet_speed; auto best_candidate = std::optional {}; @@ -42,7 +42,7 @@ struct TargetSolutionSolver::Impl { best_candidate = BestCandidate { .attitude = sample->attitude, .impact_time = t_target, - .center_position = predicted_kinematics.center_position, + .center_position = predicted_kinematics.center_position.make(), .aim_point = sample->aim_point, }; if (time_error < kMaxFlyTimeThreshold) break; diff --git a/src/module/fire_control/solver/target_solution_solver.hpp b/src/module/fire_control/solver/target_solution_solver.hpp index 0a981bc1..14c9bac0 100644 --- a/src/module/fire_control/solver/target_solution_solver.hpp +++ b/src/module/fire_control/solver/target_solution_solver.hpp @@ -1,6 +1,6 @@ #pragma once -#include +#include #include #include diff --git a/src/module/fire_control/trajectory_solution.hpp b/src/module/fire_control/trajectory_solution.hpp index 1c3335ee..6e03a960 100644 --- a/src/module/fire_control/trajectory_solution.hpp +++ b/src/module/fire_control/trajectory_solution.hpp @@ -1,6 +1,6 @@ #pragma once -#include +#include #include #include diff --git a/src/module/predictor/outpost/snapshot.cpp b/src/module/predictor/outpost/snapshot.cpp index 249cd100..806dca80 100644 --- a/src/module/predictor/outpost/snapshot.cpp +++ b/src/module/predictor/outpost/snapshot.cpp @@ -27,7 +27,7 @@ namespace { } struct OutpostSnapshotBackend final : ISnapshotBackend { - explicit OutpostSnapshotBackend(Snapshot::OutpostEKF::XVec x, CampColor color, + explicit OutpostSnapshotBackend(detail::OutpostEKF::XVec x, CampColor color, int armor_num, TimePoint stamp, int spin_sign, OutpostArmorLayout layout) noexcept : ISnapshotBackend { DeviceId::OUTPOST, color, armor_num, stamp } , x { std::move(x) } @@ -63,7 +63,7 @@ namespace { } private: - auto kinematics_of(Snapshot::OutpostEKF::XVec const& x) const -> Snapshot::Kinematics { + auto kinematics_of(detail::OutpostEKF::XVec const& x) const -> Snapshot::Kinematics { auto const max_armors = std::clamp(armor_num, 0, OutpostEKFParameters::kOutpostArmorCount); double height_sum = 0.0; @@ -77,22 +77,22 @@ namespace { auto const center_z = x[4] + (assigned_count == 0 ? 0.0 : height_sum / static_cast(assigned_count)); auto const angular_velocity = static_cast(spin_sign) * kOutpostAngularSpeed; - return { Eigen::Vector3d { x[0], x[2], center_z }, angular_velocity }; + return { Point3d { x[0], x[2], center_z }, angular_velocity }; } - auto predict_state_at(TimePoint t) const -> Snapshot::OutpostEKF::XVec { + auto predict_state_at(TimePoint t) const -> detail::OutpostEKF::XVec { auto const dt = util::delta_time(t, stamp).count(); return OutpostEKFParameters::f(dt, spin_sign)(x); } - Snapshot::OutpostEKF::XVec x; + detail::OutpostEKF::XVec x; int spin_sign; OutpostArmorLayout layout; }; } // namespace -auto detail::make_outpost_snapshot(Snapshot::OutpostEKF::XVec ekf_x, CampColor color, int armor_num, +auto detail::make_outpost_snapshot(detail::OutpostEKF::XVec ekf_x, CampColor color, int armor_num, TimePoint stamp, int outpost_spin_sign, OutpostArmorLayout outpost_layout) noexcept -> Snapshot { return detail::make_snapshot(std::make_unique( diff --git a/src/module/predictor/outpost/snapshot.hpp b/src/module/predictor/outpost/snapshot.hpp index 7d4f84e9..d74acdef 100644 --- a/src/module/predictor/outpost/snapshot.hpp +++ b/src/module/predictor/outpost/snapshot.hpp @@ -2,11 +2,14 @@ #include "module/predictor/outpost/armor_layout.hpp" #include "module/predictor/snapshot.hpp" +#include "utility/math/kalman_filter/ekf.hpp" #include "utility/robot/color.hpp" namespace rmcs::predictor::detail { -auto make_outpost_snapshot(Snapshot::OutpostEKF::XVec ekf_x, CampColor color, int armor_num, - TimePoint stamp, int outpost_spin_sign, OutpostArmorLayout outpost_layout) noexcept -> Snapshot; +using OutpostEKF = util::EKF<6, 4>; + +auto make_outpost_snapshot(OutpostEKF::XVec ekf_x, CampColor color, int armor_num, TimePoint stamp, + int outpost_spin_sign, OutpostArmorLayout outpost_layout) noexcept -> Snapshot; } // namespace rmcs::predictor::detail diff --git a/src/module/predictor/regular/snapshot.cpp b/src/module/predictor/regular/snapshot.cpp index d0b3e3e7..12d3290e 100644 --- a/src/module/predictor/regular/snapshot.cpp +++ b/src/module/predictor/regular/snapshot.cpp @@ -20,8 +20,8 @@ namespace { } struct RegularSnapshotBackend final : ISnapshotBackend { - explicit RegularSnapshotBackend(Snapshot::NormalEKF::XVec x, DeviceId device, - CampColor color, int armor_num, TimePoint stamp) noexcept + explicit RegularSnapshotBackend(detail::NormalEKF::XVec x, DeviceId device, CampColor color, + int armor_num, TimePoint stamp) noexcept : ISnapshotBackend { device, color, armor_num, stamp } , x { std::move(x) } { } @@ -50,22 +50,22 @@ namespace { } private: - static auto kinematics_of(Snapshot::NormalEKF::XVec const& x) -> Snapshot::Kinematics { - return { Eigen::Vector3d { x[0], x[2], x[4] }, x[7] }; + static auto kinematics_of(detail::NormalEKF::XVec const& x) -> Snapshot::Kinematics { + return { Point3d { x[0], x[2], x[4] }, x[7] }; } - auto predict_state_at(TimePoint t) const -> Snapshot::NormalEKF::XVec { + auto predict_state_at(TimePoint t) const -> detail::NormalEKF::XVec { auto const dt = util::delta_time(t, stamp).count(); return EKFParameters::f(dt)(x); } - Snapshot::NormalEKF::XVec x; + detail::NormalEKF::XVec x; }; } // namespace -auto detail::make_regular_snapshot(Snapshot::NormalEKF::XVec ekf_x, DeviceId device, - CampColor color, int armor_num, TimePoint stamp) noexcept -> Snapshot { +auto detail::make_regular_snapshot(NormalEKF::XVec ekf_x, DeviceId device, CampColor color, + int armor_num, TimePoint stamp) noexcept -> Snapshot { return detail::make_snapshot(std::make_unique( std::move(ekf_x), device, color, armor_num, stamp)); } diff --git a/src/module/predictor/regular/snapshot.hpp b/src/module/predictor/regular/snapshot.hpp index da77793d..a275c9f2 100644 --- a/src/module/predictor/regular/snapshot.hpp +++ b/src/module/predictor/regular/snapshot.hpp @@ -1,12 +1,16 @@ #pragma once #include "module/predictor/snapshot.hpp" +#include "utility/math/kalman_filter/ekf.hpp" #include "utility/robot/color.hpp" #include "utility/robot/id.hpp" namespace rmcs::predictor::detail { -auto make_regular_snapshot(Snapshot::NormalEKF::XVec ekf_x, DeviceId device, CampColor color, - int armor_num, TimePoint stamp) noexcept -> Snapshot; +using NormalEKF = util::EKF<11, 4>; +using OutpostEKF = util::EKF<6, 4>; + +auto make_regular_snapshot(NormalEKF::XVec ekf_x, DeviceId device, CampColor color, int armor_num, + TimePoint stamp) noexcept -> Snapshot; } // namespace rmcs::predictor::detail diff --git a/src/module/predictor/snapshot.cpp b/src/module/predictor/snapshot.cpp index 9713eecf..a6ff5c69 100644 --- a/src/module/predictor/snapshot.cpp +++ b/src/module/predictor/snapshot.cpp @@ -14,7 +14,7 @@ namespace { : ISnapshotBackend { DeviceId::UNKNOWN, CampColor::UNKNOWN, 0, stamp } { } [[nodiscard]] auto kinematics_at(TimePoint) const -> Snapshot::Kinematics override { - return { Eigen::Vector3d::Zero(), 0.0 }; + return { Point3d::kZero(), 0.0 }; } [[nodiscard]] auto predicted_armors(TimePoint) const -> std::vector override { diff --git a/src/module/predictor/snapshot.hpp b/src/module/predictor/snapshot.hpp index 1649125f..6df12cff 100644 --- a/src/module/predictor/snapshot.hpp +++ b/src/module/predictor/snapshot.hpp @@ -1,11 +1,9 @@ #pragma once -#include #include #include #include "utility/clock.hpp" -#include "utility/math/kalman_filter/ekf.hpp" #include "utility/robot/armor.hpp" #include "utility/robot/id.hpp" @@ -20,11 +18,8 @@ namespace detail { class Snapshot { public: - using NormalEKF = util::EKF<11, 4>; - using OutpostEKF = util::EKF<6, 4>; - struct Kinematics { - Eigen::Vector3d center_position; + Point3d center_position; double angular_velocity; }; diff --git a/src/runtime.cpp b/src/runtime.cpp deleted file mode 100644 index cc87ce49..00000000 --- a/src/runtime.cpp +++ /dev/null @@ -1,234 +0,0 @@ -#include "kernel/capturer.hpp" -#include "kernel/feishu.hpp" -#include "kernel/fire_control.hpp" -#include "kernel/identifier.hpp" -#include "kernel/pose_estimator.hpp" -#include "kernel/tracker.hpp" -#include "kernel/visualization.hpp" - -#include "utility/framerate.hpp" -#include "utility/image/text.hpp" -#include "utility/math/linear.hpp" -#include "utility/panic.hpp" -#include "utility/rclcpp/configuration.hpp" -#include "utility/rclcpp/node.hpp" -#include "utility/rclcpp/parameters.hpp" -#include "utility/shared/context.hpp" -#include "utility/singleton/running.hpp" - -#include -#include -#include -#include -#include -#include - -using namespace rmcs; -using namespace rmcs::util; -using namespace rmcs::kernel; - -auto main() -> int { - using namespace std::chrono_literals; - - std::signal(SIGINT, [](int) { util::set_running(false); }); - - auto node = RclcppNode { "AutoAim" }; - node.set_pub_topic_prefix("/rmcs/auto_aim/"); - - /// Runtime - auto feishu = kernel::Feishu { }; - auto capturer = kernel::Capturer { }; - auto identifier = kernel::Identifier { }; - auto tracker = kernel::Tracker { }; - auto pose_estimator = kernel::PoseEstimator { }; - auto fire_control = kernel::FireControl { }; - auto visualization = kernel::Visualization { }; - - /// Configure - const auto configs = util::configs(); - const auto localhost_develop = configs["localhost_develop"].as(); - const auto use_visualization = configs["use_visualization"].as(); - - const auto camera_matrix = configs["camera_matrix"].as>(); - const auto distort_coeff = configs["distort_coeff"].as>(); - - const auto handle_result = [&](auto runtime_name, const auto& result) { - if (!result.has_value()) { - node.error("Failed to init '{}'", runtime_name); - node.error(" {}", result.error()); - util::panic(std::format("Failed to initialize {}", runtime_name)); - } - }; - - // CAPTURER - { - auto config = configs["capturer"]; - auto result = capturer.initialize(config); - handle_result("capturer", result); - } - // IDENTIFIER - { - auto config = configs["identifier"]; - - const auto model_location = std::filesystem::path { util::Parameters::share_location() } - / std::filesystem::path { config["model_location"].as() }; - config["model_location"] = model_location.string(); - - auto result = identifier.initialize(config); - handle_result("identifier", result); - } - // TRACKER - { - auto config = configs["tracker"]; - auto result = tracker.initialize(config); - handle_result("tracker", result); - } - // POSE ESTIMATOR - { - auto config = configs["pose_estimator"]; - auto result = pose_estimator.initialize(config); - handle_result("pose_estimator", result); - pose_estimator.configure_camera(camera_matrix, distort_coeff); - } - // FIRE CONTROL - { - auto config = configs["fire_control"]; - auto result = fire_control.initialize(config); - handle_result("fire_control", result); - } - // VISUALIZATION - if (use_visualization) { - auto config = configs["visualization"]; - auto result = visualization.initialize(config); - handle_result("visualization", result); - } - - auto framerate = FramerateCounter { }; - framerate.set_interval(std::chrono::seconds { 5 }); - - while (util::get_running()) { - node.spin_once(); - - if (!localhost_develop && !feishu.heartbeat()) continue; - - auto image = capturer.fetch_image(); - if (!image) continue; - - auto context = SystemContext::kIdentity(); - if (!localhost_develop) { - using namespace std::chrono_literals; - const auto timestamp = image->get_timestamp() - 8'200'000ns; - const auto closest = feishu.search(timestamp, 50ms); - if (!closest) continue; - - context = *closest; - } - visualization.publish(context.camera_transform, "camera_link"); - - if (framerate.tick()) { - node.info("Autoaim Framerate: {}", framerate.fps()); - } - - // 结束流程后发送串流帧 - [[maybe_unused]] auto streamer = std::experimental::scope_exit { [&] { - visualization.draw_later( - Text { capturer.recording() ? "RECORD ON" : "RECORD OFF", { 640, 20 } }); - visualization.update_image(*image); - } }; - - /// 1. Identify Armor - /// - auto armors_2d = Armor2ds { }; - { - auto result = identifier.sync_identify(*image); - if (!result.has_value()) continue; // 一般不会推理出错喵~ - - for (const auto& roi : result->areas) { - visualization.draw_later(roi); - } - visualization.draw_later(result->armors); - visualization.draw_later(result->green_light); - - using namespace rmcs_msgs; - if (context.id != RobotId::UNKNOWN) { - if (context.id.color() == RobotColor::RED) { - tracker.set_enemy_color(CampColor::BLUE); - } else { - tracker.set_enemy_color(CampColor::RED); - } - } - - tracker.set_invincible_armors(context.invincible_devices); - armors_2d = tracker.filter_armors(result->armors); - - if (armors_2d.empty()) continue; - } - - /// 2. Transform 2d to 3d - /// - auto armors_3d = Armor3ds { }; - { - pose_estimator.update_camera_transform(context.camera_transform); - - auto result = pose_estimator.estimate_armor(armors_2d, *image); - - const auto& addition = pose_estimator.addition(); - visualization.draw_later(addition.detected_2d); - visualization.draw_later(addition.areas); - visualization.draw_later(addition.predicted_near); - visualization.draw_later(addition.predicted_away); - - visualization.publish(addition.origin, "origin_armors"); - visualization.publish(addition.detected_3d, "outpost_lightbars"); - visualization.publish( - { addition.center_3d, Orientation::kIdentity() }, "outpost_center"); - - armors_3d = result; - if (armors_3d.empty()) continue; - - visualization.publish(armors_3d, "visible_armors"); - } - - /// 3. Apply Tracker - /// - auto target = tracker.decide(armors_3d, image->get_timestamp()); - auto command = AutoAimState::kInvalid(); - if (target.snapshot) { - auto& snapshot = target.snapshot; - auto armors = snapshot->predicted_armors(Clock::now()); - visualization.publish(armors, "visible_robot"); - - const auto yaw = context.yaw; - if (auto result = fire_control.solve(*snapshot, yaw)) { - command.should_control = true; - command.target = target.target_id; - command.should_shoot = result->shoot_permitted; - command.yaw = result->yaw; - command.pitch = result->pitch; - command.yaw_rate = result->yaw_rate; - command.pitch_rate = result->pitch_rate; - command.yaw_acc = result->yaw_acc; - command.pitch_acc = result->pitch_acc; - command.feedforward_valid = result->feedforward_valid; - command.robot_center = result->center_position; - - visualization.update_aiming_direction(command.yaw, -command.pitch); - - // TODO: 将 MPC 的 target 曲线和 Yaw 置于一个统一的参考系 - visualization.update_mpc_plan(command.yaw, command.pitch, command.yaw_rate, - command.pitch_rate, command.yaw_acc, command.pitch_acc); - } - } - if (use_visualization) { - util::draw_text(*image, command.should_shoot ? "ATTACK" : "IDLE"); - } - - /// 4. Transmit State - /// - feishu.send(command); - - } // runtime loop scope - - node.shutdown(); - return 0; -} diff --git a/src/utility/image/drawable.cpp b/src/utility/image/drawable.cpp index 447d4642..ef16ae06 100644 --- a/src/utility/image/drawable.cpp +++ b/src/utility/image/drawable.cpp @@ -82,20 +82,21 @@ auto Canvas::draw(const Text& text) -> void { auto& mat = canvas.details().mat; const auto font = cv::FONT_HERSHEY_SIMPLEX; - const auto scale = 0.6; - const auto white = cv::Scalar { 255, 255, 255, static_cast(transparency) }; - const auto black = cv::Scalar { 0, 0, 0, static_cast(transparency) }; + const auto scale = 0.5; + + const auto font_color = text.color; + const auto back_color = cv::Scalar { 0, 0, 0 }; auto baseline = 0; const auto size = cv::getTextSize(text.content, font, scale, line_thickness, &baseline); const auto org = cv::Point2i { - text.center.x - size.width / 2, - text.center.y + size.height / 2, + text.top_left.x, + text.top_left.y + size.height, }; - cv::putText(mat, text.content, org + cv::Point2i { 1, 1 }, font, scale, black, + cv::putText(mat, text.content, org + cv::Point2i { 1, 1 }, font, scale, back_color, line_thickness + 2, cv::LINE_AA); - cv::putText(mat, text.content, org, font, scale, white, line_thickness, cv::LINE_AA); + cv::putText(mat, text.content, org, font, scale, font_color, line_thickness, cv::LINE_AA); } } diff --git a/src/utility/image/drawable.hpp b/src/utility/image/drawable.hpp index 3e6a1961..2d16e2e8 100644 --- a/src/utility/image/drawable.hpp +++ b/src/utility/image/drawable.hpp @@ -4,13 +4,29 @@ namespace rmcs { +inline const auto kRed = cv::Scalar { 000, 000, 255, 255 }; +inline const auto kGreen = cv::Scalar { 000, 255, 000, 255 }; +inline const auto kBlue = cv::Scalar { 255, 000, 000, 255 }; +inline const auto kYellow = cv::Scalar { 000, 255, 255, 255 }; +inline const auto kMagenta = cv::Scalar { 255, 000, 255, 255 }; +inline const auto kCyan = cv::Scalar { 255, 255, 000, 255 }; +inline const auto kBlack = cv::Scalar { 000, 000, 000, 255 }; +inline const auto kWhite = cv::Scalar { 255, 255, 255, 255 }; +inline const auto kGray = cv::Scalar { 192, 192, 192, 255 }; +inline const auto kDarkGray = cv::Scalar { 128, 128, 128, 255 }; +inline const auto kBrown = cv::Scalar { 000, 000, 128, 255 }; +inline const auto kOrange = cv::Scalar { 000, 165, 255, 255 }; +inline const auto kPurple = cv::Scalar { 128, 000, 128, 255 }; +inline const auto kPink = cv::Scalar { 203, 192, 255, 255 }; + struct Text { std::string content; - cv::Point2i center; + cv::Point2i top_left; + cv::Scalar color = kWhite; }; struct Area { cv::Rect2i rect; - cv::Scalar color { 0, 0, 0, 255 }; + cv::Scalar color = kWhite; }; struct Canvas { diff --git a/src/utility/math/solve_armors.cpp b/src/utility/math/solve_armors.cpp index 54c8e437..cfe977bb 100644 --- a/src/utility/math/solve_armors.cpp +++ b/src/utility/math/solve_armors.cpp @@ -1,5 +1,5 @@ #include "solve_armors.hpp" -#include +#include #include namespace rmcs::util {