diff --git a/.DS_Store b/.DS_Store index 5008ddf..fc018fc 100644 Binary files a/.DS_Store and b/.DS_Store differ 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)