From 9367f6e92a2ad1a9266ca84cbd68de7fea267d12 Mon Sep 17 00:00:00 2001 From: cyy_mac Date: Sat, 28 Mar 2026 04:32:34 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E8=87=AA=E5=8A=A8=E6=9B=9D?= =?UTF-8?q?=E5=85=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .DS_Store | Bin 6148 -> 6148 bytes .../armor_yolo_detect/armor_yolo_detector.hpp | 3 + .../armor_yolo_detector_node.hpp | 18 ++ .../src/armor_yolo_detector_node.cpp | 197 ++++++++++++++++++ .../node_params/armor_yolo_detect_params.yaml | 13 ++ 5 files changed, 231 insertions(+) diff --git a/.DS_Store b/.DS_Store index 5008ddfcf53c02e82d7eee2e57c38e5672ef89f6..fc018fc89321f054100d9ce24a2da12965e944ea 100644 GIT binary patch delta 149 zcmZoMXfc=|&e%RNQEZ}~q6iZM0|O%ig8)NvPEvk;4v@(pFfmcX0VK}MP|Q%okepJC zB*#$BPzn?-VTflaW5@)uG8yt2@)&d(av2hV>^zv_jfpnwlMO^zHgj|Ea4@!SO#IF~ XnO{Uv5M-_b5GMdJ7;JVFIm`?Iof#kj delta 68 zcmZoMXfc=|&Zs)EP;8=}A}a$BFiuR+-uOVBed2?}&FmZk9E_?PFMem9%rBxS$Ox29 Q0I2~25W8WsqsU=q0A?}|^#A|> 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 ba12f7b..6da54b8 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 @@ -105,6 +105,9 @@ public: // Get current mode DetectorMode getMode() const { return mode_; } + // Get latest YOLO objects (for auto exposure control) + const std::vector& getYoloObjects() const { return yolo_objects_; } + // Set ROI update interval (frames between YOLO updates) void setRoiUpdateInterval(int interval); 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 c56c9e1..c0eb9d8 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 @@ -157,6 +157,24 @@ private: void performBinaryThresCalibration(const sensor_msgs::msg::Image::ConstSharedPtr& img_msg); void saveBinaryThresToYaml(int binary_thres); + // Auto exposure control + bool auto_exposure_enable_ = false; + rclcpp::TimerBase::SharedPtr auto_exposure_timer_; + double current_exposure_time_ = 1000.0; + double target_brightness_ = 100.0; + double brightness_tolerance_ = 20.0; + double exposure_step_gain_ = 100.0; + double exposure_decay_step_ = 20.0; + double exposure_min_ = 600.0; + double exposure_max_ = 2000.0; + double auto_exposure_control_interval_ms_ = 100.0; + rclcpp::CallbackQueue::SharedPtr exposure_callback_queue_; + rclcpp::executors::SingleThreadedExecutor exposure_executor_; + std::thread exposure_executor_thread_; + void autoExposureTimerCallback(); + double computeBrightnessInRois(const cv::Mat& gray_img, const std::vector& yolo_objects); + void callSetExposureService(double exposure_time); + // BA bool use_ba_ = true; 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 d3ce374..9f6aae3 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 @@ -188,6 +188,10 @@ ArmorYoloDetectorNode::~ArmorYoloDetectorNode() { if (image_processing_thread_.joinable()) { image_processing_thread_.join(); } + if (exposure_executor_thread_.joinable()) { + exposure_executor_.cancel(); + exposure_executor_thread_.join(); + } } void ArmorYoloDetectorNode::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr img_msg) { @@ -415,6 +419,31 @@ std::unique_ptr ArmorYoloDetectorNode::initDetector() { on_set_parameters_callback_handle_ = this->add_on_set_parameters_callback( std::bind(&ArmorYoloDetectorNode::onSetParameters, this, std::placeholders::_1)); + // Auto exposure parameters + auto_exposure_enable_ = this->declare_parameter("auto_exposure.enable", false); + target_brightness_ = this->declare_parameter("auto_exposure.target_brightness", 100.0); + brightness_tolerance_ = this->declare_parameter("auto_exposure.brightness_tolerance", 10.0); + exposure_step_gain_ = this->declare_parameter("auto_exposure.step_gain", 100.0); + exposure_decay_step_ = this->declare_parameter("auto_exposure.decay_step", 5.0); + exposure_min_ = this->declare_parameter("auto_exposure.exposure_min", 100.0); + exposure_max_ = this->declare_parameter("auto_exposure.exposure_max", 20000.0); + auto_exposure_control_interval_ms_ = this->declare_parameter("auto_exposure.control_interval_ms", 100.0); + current_exposure_time_ = this->declare_parameter("auto_exposure.init_exposure_time", 5000.0); + + if (auto_exposure_enable_) { + exposure_callback_queue_ = std::make_shared(); + exposure_executor_.add_callback_queue(exposure_callback_queue_); + exposure_executor_thread_ = std::thread([this]() { + exposure_executor_.spin(); + }); + auto_exposure_timer_ = this->create_wall_timer( + std::chrono::milliseconds(static_cast(auto_exposure_control_interval_ms_)), + [this]() { this->autoExposureTimerCallback(); }, + exposure_callback_queue_); + FYT_INFO("armor_yolo_detect", "Auto exposure enabled (target_brightness={}, interval={}ms)", + target_brightness_, auto_exposure_control_interval_ms_); + } + return detector; } @@ -543,6 +572,54 @@ rcl_interfaces::msg::SetParametersResult ArmorYoloDetectorNode::onSetParameters( debug_markers_ = param.as_bool(); } else if (param.get_name() == "debug.enable_result_img") { debug_result_img_ = param.as_bool(); + } else if (param.get_name() == "auto_exposure.enable") { + bool new_enable = param.as_bool(); + if (new_enable != auto_exposure_enable_) { + auto_exposure_enable_ = new_enable; + if (auto_exposure_enable_) { + exposure_callback_queue_ = std::make_shared(); + exposure_executor_.add_callback_queue(exposure_callback_queue_); + exposure_executor_thread_ = std::thread([this]() { + exposure_executor_.spin(); + }); + auto_exposure_timer_ = this->create_wall_timer( + std::chrono::milliseconds(static_cast(auto_exposure_control_interval_ms_)), + [this]() { this->autoExposureTimerCallback(); }, + exposure_callback_queue_); + FYT_INFO("armor_yolo_detect", "Auto exposure enabled"); + } else { + auto_exposure_timer_.reset(); + exposure_callback_queue_.reset(); + if (exposure_executor_thread_.joinable()) { + exposure_executor_.cancel(); + exposure_executor_thread_.join(); + } + FYT_INFO("armor_yolo_detect", "Auto exposure disabled"); + } + } + } else if (param.get_name() == "auto_exposure.target_brightness") { + target_brightness_ = param.as_double(); + } else if (param.get_name() == "auto_exposure.brightness_tolerance") { + brightness_tolerance_ = param.as_double(); + } else if (param.get_name() == "auto_exposure.step_gain") { + exposure_step_gain_ = param.as_double(); + } else if (param.get_name() == "auto_exposure.decay_step") { + exposure_decay_step_ = param.as_double(); + } else if (param.get_name() == "auto_exposure.exposure_min") { + exposure_min_ = param.as_double(); + } else if (param.get_name() == "auto_exposure.exposure_max") { + exposure_max_ = param.as_double(); + } else if (param.get_name() == "auto_exposure.control_interval_ms") { + auto_exposure_control_interval_ms_ = param.as_double(); + if (auto_exposure_timer_ && auto_exposure_enable_) { + auto_exposure_timer_->cancel(); + auto_exposure_timer_ = this->create_wall_timer( + std::chrono::milliseconds(static_cast(auto_exposure_control_interval_ms_)), + [this]() { this->autoExposureTimerCallback(); }, + exposure_callback_queue_); + } + } else if (param.get_name() == "auto_exposure.init_exposure_time") { + current_exposure_time_ = param.as_double(); } } @@ -835,6 +912,126 @@ void ArmorYoloDetectorNode::saveBinaryThresToYaml(int binary_thres) { FYT_INFO("armor_yolo_detect", "Binary threshold saved to yaml: {}", calib_save_yaml_path_); } +void ArmorYoloDetectorNode::autoExposureTimerCallback() { + if (!auto_exposure_enable_) { + return; + } + + std::lock_guard lock(processing_mutex_); + + if (detector_ == nullptr) { + return; + } + + // Get the latest yolo objects from detector + const auto& yolo_objects = detector_->getYoloObjects(); + if (yolo_objects.empty()) { + // No detection, apply decay step to move toward target brightness + double exposure_time = current_exposure_time_; + exposure_time -= exposure_decay_step_; + if (exposure_time < exposure_min_) { + exposure_time = exposure_min_; + } + if (exposure_time > exposure_max_) { + exposure_time = exposure_max_; + } + if (std::abs(exposure_time - current_exposure_time_) > 1.0) { + current_exposure_time_ = exposure_time; + callSetExposureService(current_exposure_time_); + } + return; + } + + // Get latest image from frame queue + cv::Mat gray_img; + { + std::lock_guard lock_queue(queue_mutex_); + if (frame_queue_.empty()) { + return; + } + const auto& img_msg = frame_queue_.back().img_msg; + auto img = cv_bridge::toCvShare(img_msg, "bgr8")->image; + cv::cvtColor(img, gray_img, cv::COLOR_BGR2GRAY); + } + + // Compute brightness in actual YOLO detected bbox regions (NOT expanded ROI) + double brightness = computeBrightnessInRois(gray_img, yolo_objects); + + if (brightness < 0) { + return; + } + + const double diff = brightness - target_brightness_; + const double last_exposure_time = current_exposure_time_; + + if (std::fabs(diff) > brightness_tolerance_) { + current_exposure_time_ -= diff * exposure_step_gain_; + } else { + current_exposure_time_ -= exposure_decay_step_; + } + + if (current_exposure_time_ < exposure_min_) { + current_exposure_time_ = exposure_min_; + } + if (current_exposure_time_ > exposure_max_) { + current_exposure_time_ = exposure_max_; + } + + // Only call service if exposure change is significant + if (std::abs(current_exposure_time_ - last_exposure_time) > 10.0) { + callSetExposureService(current_exposure_time_); + } +} + +double ArmorYoloDetectorNode::computeBrightnessInRois( + const cv::Mat& gray_img, + const std::vector& yolo_objects) { + if (yolo_objects.empty() || gray_img.empty()) { + return -1.0; + } + + double total_brightness = 0.0; + int pixel_count = 0; + + for (const auto& obj : yolo_objects) { + // Use actual YOLO detected bbox, NOT expanded ROI + cv::Rect bbox( + static_cast(obj.rect.x), + static_cast(obj.rect.y), + static_cast(obj.rect.width), + static_cast(obj.rect.height)); + + // Clamp to image bounds + bbox &= cv::Rect(0, 0, gray_img.cols, gray_img.rows); + + if (bbox.area() < 10) { + continue; + } + + cv::Mat roi = gray_img(bbox); + cv::Scalar mean_val = cv::mean(roi); + total_brightness += mean_val[0] * roi.total(); + pixel_count += static_cast(roi.total()); + } + + if (pixel_count == 0) { + return -1.0; + } + + return total_brightness / pixel_count; +} + +void ArmorYoloDetectorNode::callSetExposureService(double exposure_time) { + // Set exposure time via parameter - camera driver should subscribe to this parameter + // or this node should be in same container with camera driver to share parameter + auto result = this->set_parameter(rclcpp::Parameter("exposure_time", static_cast(exposure_time))); + if (result.successful) { + FYT_DEBUG("armor_yolo_detect", "Set exposure time to {:.1f}", exposure_time); + } else { + FYT_WARN("armor_yolo_detect", "Failed to set exposure time: {}", result.reason); + } +} + } // namespace armor_yolo_detect #include "rclcpp_components/register_node_macro.hpp" diff --git a/src/rm_bringup/config/node_params/armor_yolo_detect_params.yaml b/src/rm_bringup/config/node_params/armor_yolo_detect_params.yaml index d46153e..19ea9d9 100644 --- a/src/rm_bringup/config/node_params/armor_yolo_detect_params.yaml +++ b/src/rm_bringup/config/node_params/armor_yolo_detect_params.yaml @@ -49,3 +49,16 @@ debug.enable_terminal_log: true debug.enable_markers: true debug.enable_result_img: true + + # Auto exposure control + # When enabled, automatically adjusts camera exposure based on YOLO detected regions (actual bbox, not expanded ROI) + # Algorithm: if brightness diff > tolerance, adjust by diff * step_gain; else decay by decay_step toward target + auto_exposure.enable: false + auto_exposure.target_brightness: 100.0 # Target brightness for detected regions (0-255) + auto_exposure.brightness_tolerance: 20.0 # Brightness tolerance, no adjustment if within this range + auto_exposure.step_gain: 10.0 # Exposure adjustment gain when brightness diff exceeds tolerance + auto_exposure.decay_step: 20.0 # Exposure decay step when brightness is within tolerance + auto_exposure.exposure_min: 600.0 # Minimum exposure time (us) + auto_exposure.exposure_max: 2000.0 # Maximum exposure time (us) + auto_exposure.control_interval_ms: 100.0 # Auto exposure control loop interval (ms) + auto_exposure.init_exposure_time: 1000.0 # Initial exposure time for auto exposure mode (us)