try fix gray

.rm_bringup.pid

@@ -0,0 +1 @@
+34927

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

@@ -34,6 +34,7 @@
 #include <image_transport/image_transport.hpp>
 #include <rm_interfaces/msg/armors.hpp>
 #include <rm_interfaces/srv/set_mode.hpp>
+#include <std_msgs/msg/float32.hpp>
 
 #include <cv_bridge/cv_bridge.h>
 #include <tf2_ros/buffer.h>
@@ -95,6 +96,7 @@ private:
   rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr cam_info_sub_;
   rclcpp::Publisher<rm_interfaces::msg::Armors>::SharedPtr armors_pub_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_pub_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr auto_exposure_pub_;
   rclcpp::Service<rm_interfaces::srv::SetMode>::SharedPtr set_mode_srv_;
 
   // Image transport for debug
@@ -157,6 +159,21 @@ 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;
+  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

@@ -118,6 +118,10 @@ ArmorYoloDetectorNode::ArmorYoloDetectorNode(const rclcpp::NodeOptions& options)
   marker_pub_ =
       this->create_publisher<visualization_msgs::msg::MarkerArray>("armor_detector/marker", 10);
 
+  // Auto exposure publisher
+  auto_exposure_pub_ =
+      this->create_publisher<std_msgs::msg::Float32>("auto_exposure/set_exposure", 10);
+
   // Debug
   debug_ = this->declare_parameter("debug", false);
   debug_log_interval_frames_ =
@@ -415,6 +419,25 @@ 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_) {
+    auto_exposure_timer_ = this->create_wall_timer(
+        std::chrono::milliseconds(static_cast<int>(auto_exposure_control_interval_ms_)),
+        [this]() { this->autoExposureTimerCallback(); });
+    FYT_INFO("armor_yolo_detect", "Auto exposure enabled (target_brightness={}, interval={}ms)",
+             target_brightness_, auto_exposure_control_interval_ms_);
+  }
+
   return detector;
 }
 
@@ -543,6 +566,42 @@ 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_) {
+          auto_exposure_timer_ = this->create_wall_timer(
+              std::chrono::milliseconds(static_cast<int>(auto_exposure_control_interval_ms_)),
+              [this]() { this->autoExposureTimerCallback(); });
+          FYT_INFO("armor_yolo_detect", "Auto exposure enabled");
+        } else {
+          auto_exposure_timer_.reset();
+          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(); });
+      }
+    } else if (param.get_name() == "auto_exposure.init_exposure_time") {
+      current_exposure_time_ = param.as_double();
     }
   }
 
@@ -835,6 +894,123 @@ 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) {
+  // Publish exposure time to camera driver via topic
+  std_msgs::msg::Float32 msg;
+  msg.data = exposure_time;
+  auto_exposure_pub_->publish(msg);
+  FYT_DEBUG("armor_yolo_detect", "Auto exposure set exposure_time: {:.1f}", exposure_time);
+}
+
 }  // namespace armor_yolo_detect
 
 #include "rclcpp_components/register_node_macro.hpp"

src/rm_bringup/config/node_params/armor_solver_params.yaml

@@ -36,8 +36,8 @@
       lost_time_thres: 1.0
     
     solver:
-      shoot_rate_min: 3
-      shoot_rate_max: 13
+      shoot_rate_min: 6
+      shoot_rate_max: 12
       ekf_count_threshold: 30
       bullet_speed: 20.5
       shooting_range_width: 0.10   #射击范围

src/rm_bringup/config/node_params/armor_yolo_detect_params.yaml

@@ -13,7 +13,7 @@
     roi_expand_pixel: 160
 
     # Binary threshold for light detection
-    binary_thres: 80
+    binary_thres: 120
 
     # Binary threshold calibration mode
     # When enabled, automatically calibrates binary_thres to match traditional detection size with YOLO
@@ -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: true
+    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)

src/rm_bringup/config/node_params/camera_driver_params.yaml

@@ -1,7 +1,7 @@
 /**:
   ros__parameters:
     camera_info_url: package://rm_bringup/config/camera_info.yaml
-    exposure_time: 1000
+    exposure_time: 800
     gain: 18.3
     resolution_width: 1440
     resolution_height: 1080
@@ -9,3 +9,4 @@
     offsetY: 0
     camera_name: "hik"
     recording: false    #是否录制
+    open_delay_ms: 2000  # Camera open delay after previous instance closed (ms)

src/rm_hardware_driver/ros2_hik_camera/src/hik_camera_node.cpp

@@ -11,6 +11,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/camera_info.hpp>
 #include <sensor_msgs/msg/image.hpp>
+#include <std_msgs/msg/float32.hpp>
 #include <string>
 #include <thread>
 
@@ -65,6 +66,20 @@ public:
     // Heartbeat
     heartbeat_ = fyt::HeartBeatPublisher::create(this);
 
+    // Auto exposure subscriber - receives target exposure time from armor detector
+    auto_exposure_sub_ = this->create_subscription<std_msgs::msg::Float32>(
+        "auto_exposure/set_exposure", rclcpp::SensorDataQoS(),
+        [this](const std_msgs::msg::Float32::SharedPtr msg) {
+          if (msg->data > 0) {
+            std::lock_guard<std::mutex> lock(camera_mutex_);
+            exposure_time_ = static_cast<int>(msg->data);
+            if (camera_handle_ != nullptr) {
+              (void)MV_CC_SetFloatValue(camera_handle_, "ExposureTime", static_cast<float>(exposure_time_));
+              FYT_DEBUG("camera_driver", "Auto exposure set exposure_time: {}", exposure_time_);
+            }
+          }
+        });
+
     // Param set callback
     params_callback_handle_ = this->add_on_set_parameters_callback(
       std::bind(&HikCameraNode::parametersCallback, this, std::placeholders::_1));
@@ -83,8 +98,10 @@ public:
     }
 
     // Watch dog timer (also handles initial open)
+    // Add initial delay to allow previous camera instance to fully release resources
+    int open_delay_ms = this->declare_parameter("open_delay_ms", 1000);
     timer_ = this->create_wall_timer(
-      std::chrono::milliseconds(1000), std::bind(&HikCameraNode::timerCallback, this));
+      std::chrono::milliseconds(open_delay_ms), std::bind(&HikCameraNode::timerCallback, this));
 
     // Start capture thread
     capture_thread_ = std::thread([this]() { this->captureLoop(); });
@@ -193,20 +210,29 @@ private:
 
   void timerCallback()
   {
-    // Open camera
-    while (!is_open_ && rclcpp::ok()) {
+    // Open camera with retry delay
+    if (!is_open_) {
+      static int retry_count = 0;
       if (!open()) {
-        FYT_ERROR("camera_driver", "open() failed");
+        retry_count++;
+        if (retry_count >= 3) {
+          FYT_ERROR("camera_driver", "open() failed {} times, will keep retrying", retry_count);
+        }
         close();
+        std::this_thread::sleep_for(std::chrono::milliseconds(500));
         return;
       }
+      retry_count = 0;
+      is_open_ = true;
     }
 
     // Watchdog: no frames for too long => reconnect
     double dt = (this->now() - latest_frame_stamp_).seconds();
     if (dt > 5.0) {
-      FYT_WARN("camera_driver", "Camera is not alive! lost frame for {:.2f} seconds", dt);
+      FYT_WARN("camera_driver", "Camera is not alive! lost frame for {:.2f} seconds, reconnecting...", dt);
       close();
+      is_open_ = false;
+      std::this_thread::sleep_for(std::chrono::milliseconds(500));
     }
   }
 
@@ -214,10 +240,15 @@ private:
   {
     if (is_open_) {
       close();
+      // Wait for camera to fully release resources after close
+      std::this_thread::sleep_for(std::chrono::milliseconds(500));
     }
 
     FYT_INFO("camera_driver", "Opening Hik Camera Device!");
 
+    // Additional wait before enumerating devices
+    std::this_thread::sleep_for(std::chrono::milliseconds(200));
+
     MV_CC_DEVICE_INFO_LIST device_list;
     int ret = MV_CC_EnumDevices(MV_GIGE_DEVICE | MV_USB_DEVICE, &device_list);
     if (ret != MV_OK || device_list.nDeviceNum == 0) {
@@ -249,6 +280,24 @@ private:
       // Apply ROI/offset before querying image info
       applyRoiLocked();
 
+      // Set pixel format to RGB8 (try to force color output)
+      // PixelType_Gvsp_RGB8_Packed = 0x02100014
+      int set_ret = MV_CC_SetEnumValue(camera_handle_, "PixelFormat", PixelType_Gvsp_RGB8_Packed);
+      if (set_ret != MV_OK) {
+        FYT_WARN("camera_driver", "Failed to set PixelFormat to RGB8: [0x{:x}]", set_ret);
+      } else {
+        FYT_INFO("camera_driver", "PixelFormat set to RGB8");
+      }
+
+      // Wait for pixel format to take effect
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
+      // Verify pixel format
+      MVCC_ENUMVALUE enum_value = {0};
+      if (MV_CC_GetEnumValue(camera_handle_, "PixelFormat", &enum_value) == MV_OK) {
+        FYT_INFO("camera_driver", "Current PixelFormat: 0x{:x}", enum_value.nCurValue);
+      }
+
       // Set exposure / gain
       (void)MV_CC_SetFloatValue(camera_handle_, "ExposureTime", static_cast<float>(exposure_time_));
       (void)MV_CC_SetFloatValue(camera_handle_, "Gain", static_cast<float>(gain_));
@@ -295,6 +344,9 @@ private:
       return false;
     }
 
+    // Wait for camera to be ready after starting grabbing
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
     is_open_ = true;
     FYT_INFO("camera_driver", "Hik Camera Device Open Success!");
     return true;
@@ -343,36 +395,69 @@ private:
         continue;
       }
 
-      // Ensure output buffer
-      const size_t need_size =
-        static_cast<size_t>(out_frame.stFrameInfo.nWidth) * out_frame.stFrameInfo.nHeight * 3;
-      if (image_msg_.data.size() != need_size) {
-        image_msg_.data.resize(need_size);
-      }
+      // Handle different pixel formats
+      // Mono8 (0x1080009) needs to be converted to RGB8 by duplicating the channel
+      // RGB8 (PixelType_Gvsp_RGB8_Packed = 0x02100014) can be used directly
+      const bool is_mono8 = (out_frame.stFrameInfo.enPixelType == 0x1080009);
+      const bool is_rgb8 = (out_frame.stFrameInfo.enPixelType == 0x02100014);
 
-      convert_param_.pDstBuffer = image_msg_.data.data();
-      convert_param_.nDstBufferSize = image_msg_.data.size();
-      convert_param_.pSrcData = out_frame.pBufAddr;
-      convert_param_.nSrcDataLen = out_frame.stFrameInfo.nFrameLen;
-      convert_param_.enSrcPixelType = out_frame.stFrameInfo.enPixelType;
-
-      {
-        std::lock_guard<std::mutex> lock(camera_mutex_);
-        if (!is_open_ || camera_handle_ == nullptr) {
-          continue;
+      if (is_mono8) {
+        // Convert Mono8 to RGB8 by duplicating the single channel
+        const size_t mono_size = static_cast<size_t>(out_frame.stFrameInfo.nWidth) * out_frame.stFrameInfo.nHeight;
+        const size_t rgb_size = mono_size * 3;
+        if (image_msg_.data.size() != rgb_size) {
+          image_msg_.data.resize(rgb_size);
         }
-        ret = MV_CC_ConvertPixelType(camera_handle_, &convert_param_);
-      }
-      if (ret != MV_OK) {
-        FYT_WARN("camera_driver", "ConvertPixelType failed: [0x{:x}]", ret);
+        uint8_t* src = static_cast<uint8_t*>(out_frame.pBufAddr);
+        uint8_t* dst = image_msg_.data.data();
+        for (size_t i = 0; i < mono_size; i++) {
+          dst[i * 3 + 0] = src[i];  // B
+          dst[i * 3 + 1] = src[i];  // G
+          dst[i * 3 + 2] = src[i];  // R
+        }
+        image_msg_.encoding = "rgb8";
+      } else if (is_rgb8) {
+        // Already RGB8, just copy directly
+        const size_t rgb_size = static_cast<size_t>(out_frame.stFrameInfo.nWidth) * out_frame.stFrameInfo.nHeight * 3;
+        if (image_msg_.data.size() != rgb_size) {
+          image_msg_.data.resize(rgb_size);
+        }
+        memcpy(image_msg_.data.data(), out_frame.pBufAddr, rgb_size);
+        image_msg_.encoding = "rgb8";
+      } else {
+        // Try SDK conversion for other formats
+        const size_t need_size =
+          static_cast<size_t>(out_frame.stFrameInfo.nWidth) * out_frame.stFrameInfo.nHeight * 3;
+        if (image_msg_.data.size() != need_size) {
+          image_msg_.data.resize(need_size);
+        }
+
+        convert_param_.pDstBuffer = image_msg_.data.data();
+        convert_param_.nDstBufferSize = image_msg_.data.size();
+        convert_param_.pSrcData = out_frame.pBufAddr;
+        convert_param_.nSrcDataLen = out_frame.stFrameInfo.nFrameLen;
+        convert_param_.enSrcPixelType = out_frame.stFrameInfo.enPixelType;
+
         {
           std::lock_guard<std::mutex> lock(camera_mutex_);
-          if (camera_handle_ != nullptr) {
-            (void)MV_CC_FreeImageBuffer(camera_handle_, &out_frame);
+          if (!is_open_ || camera_handle_ == nullptr) {
+            continue;
           }
+          ret = MV_CC_ConvertPixelType(camera_handle_, &convert_param_);
         }
-        fail_count_++;
-        continue;
+        if (ret != MV_OK) {
+          FYT_WARN("camera_driver", "ConvertPixelType failed: [0x{:x}], src_pixel_type=0x{:x}, width={}, height={}",
+                    ret, out_frame.stFrameInfo.enPixelType, out_frame.stFrameInfo.nWidth, out_frame.stFrameInfo.nHeight);
+          {
+            std::lock_guard<std::mutex> lock(camera_mutex_);
+            if (camera_handle_ != nullptr) {
+              (void)MV_CC_FreeImageBuffer(camera_handle_, &out_frame);
+            }
+          }
+          fail_count_++;
+          continue;
+        }
+        image_msg_.encoding = "rgb8";
       }
 
       auto stamp = this->now();
@@ -482,6 +567,9 @@ private:
   // Heartbeat
   fyt::HeartBeatPublisher::SharedPtr heartbeat_;
 
+  // Auto exposure subscriber
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr auto_exposure_sub_;
+
   // Params
   int frame_rate_{210};
   int exposure_time_{5000};