更新自动曝光

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<YoloObject>& getYoloObjects() const { return yolo_objects_; }
+
   // Set ROI update interval (frames between YOLO updates)
   void setRoiUpdateInterval(int interval);
 

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<YoloObject>& yolo_objects);
+  void callSetExposureService(double exposure_time);
+
   // BA
   bool use_ba_ = true;
 

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<Detector> 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<rclcpp::CallbackQueue>();
+    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<int>(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<rclcpp::CallbackQueue>();
+          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<int>(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<int>(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<std::mutex> 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<std::mutex> 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<YoloObject>& 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<int>(obj.rect.x),
+        static_cast<int>(obj.rect.y),
+        static_cast<int>(obj.rect.width),
+        static_cast<int>(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<int>(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<int>(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"

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)