From 02ceddee958936070cd0fd4d6daed3e0ea95312a Mon Sep 17 00:00:00 2001 From: MobKBK <15059009+mobkbk@user.noreply.gitee.com> Date: Mon, 23 Mar 2026 18:22:32 +0800 Subject: [PATCH] feat(armor_yolo_detect): add detector mode support with ROI cache - Add detector_mode parameter for mode selection: - HYBRID_SYNC (0): YOLO runs every frame (original behavior) - HYBRID_ROI_CACHE (1): YOLO runs every N frames, detection every frame - Add roi_update_interval parameter (default 5) for ROI cache mode - Add detector_mode.hpp for mode definitions - Add AsyncDetector class skeleton for future async mode - Add debug ROI visualization (blue rectangles) Co-Authored-By: Claude Opus 4.6 --- .../armor_yolo_detect/CMakeLists.txt | 1 + .../armor_yolo_detect/armor_yolo_detector.hpp | 35 +++++- .../armor_yolo_detector_node.hpp | 4 + .../armor_yolo_detect/async_detector.hpp | 100 ++++++++++++++++ .../armor_yolo_detect/detector_mode.hpp | 49 ++++++++ .../src/armor_yolo_detector.cpp | 81 ++++++++++--- .../src/armor_yolo_detector_node.cpp | 20 ++++ .../armor_yolo_detect/src/async_detector.cpp | 113 ++++++++++++++++++ 8 files changed, 384 insertions(+), 19 deletions(-) create mode 100644 src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/async_detector.hpp create mode 100644 src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/detector_mode.hpp create mode 100644 src/rm_auto_aim/armor_yolo_detect/src/async_detector.cpp diff --git a/src/rm_auto_aim/armor_yolo_detect/CMakeLists.txt b/src/rm_auto_aim/armor_yolo_detect/CMakeLists.txt index fb30589..132af46 100644 --- a/src/rm_auto_aim/armor_yolo_detect/CMakeLists.txt +++ b/src/rm_auto_aim/armor_yolo_detect/CMakeLists.txt @@ -32,6 +32,7 @@ find_package(CUDA REQUIRED) ament_auto_add_library(${PROJECT_NAME} SHARED src/armor_yolo_detector.cpp src/armor_yolo_detector_node.cpp + src/async_detector.cpp src/trt_logger.cpp src/yolo_tensorrt.cpp ) diff --git a/src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/armor_yolo_detector.hpp b/src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/armor_yolo_detector.hpp index 0f3be87..31d95ca 100644 --- a/src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/armor_yolo_detector.hpp +++ b/src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/armor_yolo_detector.hpp @@ -4,6 +4,11 @@ // Uses YOLO to provide ROI regions, then applies traditional binary + light detection // for precise corner localization. Input/output compatible with armor_detector. // +// Supports multiple detection modes: +// - HYBRID_SYNC: YOLO runs every frame (original) +// - HYBRID_ROI_CACHE: YOLO runs every N frames, detection runs every frame +// - HYBRID_ASYNC: Async queue with ROI cache +// // Copyright (C) FYT Vision Group. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -24,11 +29,16 @@ #include #include #include +#include +#include +#include +#include #include // project #include "armor_yolo_detect/types.hpp" #include "armor_yolo_detect/yolo_tensorrt.hpp" +#include "armor_yolo_detect/detector_mode.hpp" #include "armor_detector/types.hpp" #include "armor_detector/light_corner_corrector.hpp" #include "rm_utils/common.hpp" @@ -71,8 +81,6 @@ public: * @brief Initialize detector with TensorRT engine and traditional vision params * @param engine_path Path to TensorRT engine file * @param params Detection parameters - * @param light_params Light bar detection parameters - * @param armor_params Armor matching parameters */ bool init(const std::string& engine_path, const Params& params); @@ -90,6 +98,15 @@ public: */ std::vector detectRaw(const cv::Mat& input); + // Set detection mode + void setMode(DetectorMode mode); + + // Get current mode + DetectorMode getMode() const { return mode_; } + + // Set ROI update interval (frames between YOLO updates) + void setRoiUpdateInterval(int interval); + // Parameters Params params; LightParams light_params; @@ -120,10 +137,24 @@ private: // Expand YOLO bbox to ROI cv::Rect expandBBox(const cv::Rect& bbox, const cv::Size& img_size, int pixel_expand); + // Process ROIs to detect armors (common logic for all modes) + std::vector processROIs( + const cv::Mat& input, + const cv::Mat& gray_img, + const cv::Mat& binary_img); + std::unique_ptr tensorrt_; std::vector yolo_objects_; std::vector armors_; std::unique_ptr corner_corrector_; + + // Detection mode + DetectorMode mode_ = DetectorMode::HYBRID_SYNC; + + // ROI cache for HYBRID_ROI_CACHE and HYBRID_ASYNC modes + std::vector cached_yolo_objects_; + int roi_update_interval_ = 5; + std::atomic frame_counter_{0}; }; } // namespace armor_yolo_detect diff --git a/src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/armor_yolo_detector_node.hpp b/src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/armor_yolo_detector_node.hpp index f7e991c..ffc1f4d 100644 --- a/src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/armor_yolo_detector_node.hpp +++ b/src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/armor_yolo_detector_node.hpp @@ -42,6 +42,7 @@ #include "armor_yolo_detect/armor_yolo_detector.hpp" #include "armor_yolo_detect/types.hpp" +#include "armor_yolo_detect/detector_mode.hpp" #include "armor_detector/armor_pose_estimator.hpp" #include "rm_utils/logger/log.hpp" #include "rm_utils/heartbeat.hpp" @@ -108,6 +109,9 @@ private: std::unique_ptr detector_; cv::Point2f cam_center_; + // Detection mode + DetectorMode detector_mode_ = DetectorMode::HYBRID_SYNC; + // Camera info for pose estimation sensor_msgs::msg::CameraInfo::SharedPtr cam_info_; std::unique_ptr armor_pose_estimator_; diff --git a/src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/async_detector.hpp b/src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/async_detector.hpp new file mode 100644 index 0000000..3e7dc80 --- /dev/null +++ b/src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/async_detector.hpp @@ -0,0 +1,100 @@ +// Copyright chenyy 2026. Licensed under the MIT License. +// Async detector for armor_yolo_detect +// +// Uses a separate processing thread with an async queue to reduce data blocking. +// Combined with ROI cache to reduce YOLO inference frequency. +// +// Copyright (C) FYT Vision Group. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ARMOR_YOLO_DETECT_ASYNC_DETECTOR_HPP_ +#define ARMOR_YOLO_DETECT_ASYNC_DETECTOR_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +// project +#include "armor_yolo_detect/armor_yolo_detector.hpp" +#include "armor_detector/types.hpp" + +namespace armor_yolo_detect { + +struct AsyncFrame { + cv::Mat img; + uint64_t frame_id; + std::chrono::steady_clock::time_point timestamp; +}; + +class AsyncDetector { +public: + AsyncDetector(); + ~AsyncDetector(); + + // Initialize with a existing detector + void init(std::shared_ptr detector, int queue_size = 3); + + // Start async processing thread + void start(); + + // Stop async processing thread + void stop(); + + // Enqueue a frame for async processing + void enqueue(const cv::Mat& img); + + // Get latest detection result (non-blocking) + std::vector getLatestResult(); + + // Check if processing is running + bool isRunning() const { return running_; } + + // Get queue size + size_t queueSize() const { + std::lock_guard lock(queue_mutex_); + return frame_queue_.size(); + } + +private: + void processingLoop(); + + // Shared detector pointer + std::shared_ptr detector_; + + // Frame queue + std::deque frame_queue_; + size_t max_queue_size_ = 3; + + // Processing thread + std::thread processing_thread_; + std::atomic running_{false}; + + // Synchronization + std::mutex queue_mutex_; + std::condition_variable queue_cv_; + + // Latest result + std::vector latest_result_; + std::mutex result_mutex_; +}; + +} // namespace armor_yolo_detect + +#endif // ARMOR_YOLO_DETECT_ASYNC_DETECTOR_HPP_ diff --git a/src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/detector_mode.hpp b/src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/detector_mode.hpp new file mode 100644 index 0000000..5b74077 --- /dev/null +++ b/src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/detector_mode.hpp @@ -0,0 +1,49 @@ +// Copyright chenyy 2026. Licensed under the MIT License. +// +// Detection modes for armor_yolo_detect +// +// Copyright (C) FYT Vision Group. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ARMOR_YOLO_DETECT_DETECTOR_MODE_HPP_ +#define ARMOR_YOLO_DETECT_DETECTOR_MODE_HPP_ + +namespace armor_yolo_detect { + +// Detection modes +enum class DetectorMode { + // Original hybrid detection: YOLO runs every frame + HYBRID_SYNC = 0, + // ROI cache mode: YOLO runs every N frames, detection runs every frame + HYBRID_ROI_CACHE = 1, + // Async mode: async queue with ROI cache + HYBRID_ASYNC = 2 +}; + +inline std::string detectorModeToString(DetectorMode mode) { + switch (mode) { + case DetectorMode::HYBRID_SYNC: + return "hybrid_sync"; + case DetectorMode::HYBRID_ROI_CACHE: + return "hybrid_roi_cache"; + case DetectorMode::HYBRID_ASYNC: + return "hybrid_async"; + default: + return "unknown"; + } +} + +} // namespace armor_yolo_detect + +#endif // ARMOR_YOLO_DETECT_DETECTOR_MODE_HPP_ diff --git a/src/rm_auto_aim/armor_yolo_detect/src/armor_yolo_detector.cpp b/src/rm_auto_aim/armor_yolo_detect/src/armor_yolo_detector.cpp index d857bc1..7cd8623 100644 --- a/src/rm_auto_aim/armor_yolo_detect/src/armor_yolo_detector.cpp +++ b/src/rm_auto_aim/armor_yolo_detect/src/armor_yolo_detector.cpp @@ -4,6 +4,11 @@ // Uses YOLO to provide ROI regions, then applies traditional binary + light detection // for precise corner localization. Input/output compatible with armor_detector. // +// Supports multiple detection modes: +// - HYBRID_SYNC: YOLO runs every frame (original) +// - HYBRID_ROI_CACHE: YOLO runs every N frames, detection runs every frame +// - HYBRID_ASYNC: Async queue with ROI cache +// // Copyright (C) FYT Vision Group. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -42,25 +47,68 @@ bool Detector::init(const std::string& engine_path, const Params& params) { return true; } +void Detector::setMode(DetectorMode mode) { + mode_ = mode; + if (mode_ != DetectorMode::HYBRID_SYNC) { + // Clear cache when switching to cached modes + cached_yolo_objects_.clear(); + frame_counter_.store(0, std::memory_order_relaxed); + } +} + +void Detector::setRoiUpdateInterval(int interval) { + roi_update_interval_ = std::max(1, interval); +} + std::vector Detector::detect(const cv::Mat& input) { armors_.clear(); debug_rois_.clear(); - // 1. Run YOLO inference to get ROI regions - yolo_objects_ = tensorrt_->infer(input, params.detect_color); - - if (yolo_objects_.empty()) { - return armors_; - } - - // 2. Preprocess image once + // Preprocess image once cv::Mat gray_img; cv::cvtColor(input, gray_img, cv::COLOR_BGR2GRAY); cv::Mat full_binary_img; cv::threshold(gray_img, full_binary_img, binary_thres, 255, cv::THRESH_BINARY); - // 3. For each YOLO detection, create ROI and detect lights + uint64_t current_frame = frame_counter_.fetch_add(1, std::memory_order_relaxed); + + // Update YOLO objects based on mode + switch (mode_) { + case DetectorMode::HYBRID_SYNC: + // YOLO runs every frame + yolo_objects_ = tensorrt_->infer(input, params.detect_color); + break; + + case DetectorMode::HYBRID_ROI_CACHE: + case DetectorMode::HYBRID_ASYNC: + // YOLO runs every roi_update_interval_ frames + if (current_frame % roi_update_interval_ == 0) { + yolo_objects_ = tensorrt_->infer(input, params.detect_color); + cached_yolo_objects_ = yolo_objects_; + } else { + // Use cached ROIs + yolo_objects_ = cached_yolo_objects_; + } + break; + } + + if (yolo_objects_.empty()) { + return armors_; + } + + // Process ROIs to detect armors + armors_ = processROIs(input, gray_img, full_binary_img); + + return armors_; +} + +std::vector Detector::processROIs( + const cv::Mat& input, + const cv::Mat& gray_img, + const cv::Mat& binary_img) { + std::vector result_armors; + for (const auto& yolo_obj : yolo_objects_) { // Get bounding box from YOLO cv::Rect_ bbox = yolo_obj.rect; @@ -80,19 +128,19 @@ std::vector Detector::detect(const cv::Mat& input) { // Extract ROI images cv::Mat roi_gray = gray_img(roi); - cv::Mat roi_binary = full_binary_img(roi); + cv::Mat roi_binary = binary_img(roi); - // 4. Find lights within this ROI + // Find lights within this ROI auto lights = findLightsInROI(input, roi_gray, roi_binary, roi); if (lights.empty()) { continue; } - // 5. Match lights to form armors within this ROI + // Match lights to form armors within this ROI auto roi_armors = matchLightsInROI(lights); - // 6. Process each armor: assign YOLO classification info + // Process each armor: assign YOLO classification info for (auto& armor : roi_armors) { // Convert ROI coordinates to global coordinates armor.left_light.top += cv::Point2f(static_cast(roi.x), static_cast(roi.y)); @@ -108,16 +156,15 @@ std::vector Detector::detect(const cv::Mat& input) { armor.confidence = yolo_obj.prob; // Use armor type from traditional detection (more accurate) - // YOLO info available if needed: yolo_obj.color - // 7. Correct corners using traditional method + // Correct corners using traditional method correctCorners(armor, gray_img); - armors_.push_back(armor); + result_armors.push_back(armor); } } - return armors; + return result_armors; } std::vector Detector::detectRaw(const cv::Mat& input) { diff --git a/src/rm_auto_aim/armor_yolo_detect/src/armor_yolo_detector_node.cpp b/src/rm_auto_aim/armor_yolo_detect/src/armor_yolo_detector_node.cpp index 9ad0ebb..1bfb0e4 100644 --- a/src/rm_auto_aim/armor_yolo_detect/src/armor_yolo_detector_node.cpp +++ b/src/rm_auto_aim/armor_yolo_detect/src/armor_yolo_detector_node.cpp @@ -49,6 +49,8 @@ #include #include "armor_yolo_detect/types.hpp" +#include "armor_yolo_detect/detector_mode.hpp" +#include "armor_yolo_detect/async_detector.hpp" #include "armor_detector/ba_solver.hpp" #include "rm_utils/assert.hpp" #include "rm_utils/common.hpp" @@ -368,7 +370,18 @@ std::unique_ptr ArmorYoloDetectorNode::initDetector() { this->declare_parameter("armor.max_large_center_distance", 5.0); detector->armor_params.max_angle = this->declare_parameter("armor.max_angle", 35.0); + // Detection mode + int mode_int = this->declare_parameter("detector_mode", 0); + detector_mode_ = static_cast(mode_int); + detector->setMode(detector_mode_); + + // ROI update interval (for roi_cache and async modes) + int roi_update_interval = this->declare_parameter("roi_update_interval", 5); + detector->setRoiUpdateInterval(roi_update_interval); + FYT_INFO("armor_yolo_detect", "Inference backend: TensorRT ({})", engine_path.string()); + FYT_INFO("armor_yolo_detect", "Detector mode: {} (roi_update_interval={})", + detectorModeToString(detector_mode_), roi_update_interval); // Set dynamic parameter callback on_set_parameters_callback_handle_ = this->add_on_set_parameters_callback( @@ -463,6 +476,13 @@ rcl_interfaces::msg::SetParametersResult ArmorYoloDetectorNode::onSetParameters( detector_->armor_params.max_large_center_distance = param.as_double(); } else if (param.get_name() == "armor.max_angle") { detector_->armor_params.max_angle = param.as_double(); + } else if (param.get_name() == "detector_mode") { + detector_mode_ = static_cast(param.as_int()); + detector_->setMode(detector_mode_); + FYT_INFO("armor_yolo_detect", "Detector mode changed to: {}", + detectorModeToString(detector_mode_)); + } else if (param.get_name() == "roi_update_interval") { + detector_->setRoiUpdateInterval(param.as_int()); } else if (param.get_name() == "debug.enable_terminal_log") { debug_terminal_log_ = param.as_bool(); } else if (param.get_name() == "debug.enable_markers") { diff --git a/src/rm_auto_aim/armor_yolo_detect/src/async_detector.cpp b/src/rm_auto_aim/armor_yolo_detect/src/async_detector.cpp new file mode 100644 index 0000000..ec79a12 --- /dev/null +++ b/src/rm_auto_aim/armor_yolo_detect/src/async_detector.cpp @@ -0,0 +1,113 @@ +// Copyright chenyy 2026. Licensed under the MIT License. +// Async detector implementation +// +// Copyright (C) FYT Vision Group. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "armor_yolo_detect/async_detector.hpp" + +namespace armor_yolo_detect { + +AsyncDetector::AsyncDetector() = default; + +AsyncDetector::~AsyncDetector() { + stop(); +} + +void AsyncDetector::init(std::shared_ptr detector, int queue_size) { + detector_ = detector; + max_queue_size_ = static_cast(std::max(1, queue_size)); +} + +void AsyncDetector::start() { + if (running_) { + return; + } + + running_ = true; + processing_thread_ = std::thread(&AsyncDetector::processingLoop, this); +} + +void AsyncDetector::stop() { + if (!running_) { + return; + } + + running_ = false; + queue_cv_.notify_one(); + + if (processing_thread_.joinable()) { + processing_thread_.join(); + } +} + +void AsyncDetector::enqueue(const cv::Mat& img) { + std::lock_guard lock(queue_mutex_); + + // Remove oldest frame if queue is full + if (frame_queue_.size() >= max_queue_size_) { + frame_queue_.pop_front(); + } + + static uint64_t frame_id_counter = 0; + AsyncFrame frame; + frame.img = img.clone(); + frame.frame_id = frame_id_counter++; + frame.timestamp = std::chrono::steady_clock::now(); + + frame_queue_.push_back(std::move(frame)); + queue_cv_.notify_one(); +} + +std::vector AsyncDetector::getLatestResult() { + std::lock_guard lock(result_mutex_); + return latest_result_; +} + +void AsyncDetector::processingLoop() { + while (running_) { + AsyncFrame frame; + + { + std::unique_lock lock(queue_mutex_); + queue_cv_.wait_for( + lock, + std::chrono::milliseconds(100), + [this]() { return !frame_queue_.empty() || !running_; }); + + if (!running_ && frame_queue_.empty()) { + break; + } + + if (frame_queue_.empty()) { + continue; + } + + frame = std::move(frame_queue_.front()); + frame_queue_.pop_front(); + } + + // Process frame + if (detector_) { + auto result = detector_->detect(frame.img); + + { + std::lock_guard lock(result_mutex_); + latest_result_ = result; + } + } + } +} + +} // namespace armor_yolo_detect