add auto binarythres fix logic 1

src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/armor_yolo_detector_node.hpp

@@ -146,7 +146,9 @@ private:
   // Binary threshold calibration
   bool calib_binarythres_ = false;
   bool calib_done_ = false;
+  bool calib_searching_ = true;  // true=searching for valid thres, false=doing binary search
   int calib_frame_count_ = 0;
+  int calib_current_thres_ = 250;
   std::vector<double> calib_size_errors_;
   std::string calib_save_yaml_path_;
   void performBinaryThresCalibration(const sensor_msgs::msg::Image::ConstSharedPtr& img_msg);

src/rm_auto_aim/armor_yolo_detect/src/armor_yolo_detector_node.cpp

@@ -353,7 +353,9 @@ std::unique_ptr<Detector> ArmorYoloDetectorNode::initDetector() {
   // Binary threshold calibration mode
   calib_binarythres_ = this->declare_parameter("calib_binarythres", false);
   calib_done_ = false;
+  calib_searching_ = true;
   calib_frame_count_ = 0;
+  calib_current_thres_ = 250;
   calib_size_errors_.reserve(100);
   calib_save_yaml_path_ = this->declare_parameter("calib_save_yaml_path", std::string(""));
 
@@ -525,8 +527,10 @@ rcl_interfaces::msg::SetParametersResult ArmorYoloDetectorNode::onSetParameters(
       calib_binarythres_ = param.as_bool();
       if (calib_binarythres_) {
         calib_done_ = false;
+        calib_searching_ = true;
         calib_frame_count_ = 0;
-        FYT_INFO("armor_yolo_detect", "Binary threshold calibration started");
+        calib_current_thres_ = 250;
+        FYT_INFO("armor_yolo_detect", "Binary threshold calibration started (searching from 250)");
       }
     } else if (param.get_name() == "debug.enable_terminal_log") {
       debug_terminal_log_ = param.as_bool();
@@ -630,6 +634,8 @@ void ArmorYoloDetectorNode::performBinaryThresCalibration(
   if (yolo_objects.empty()) {
     FYT_INFO("armor_yolo_detect", "Calibration: waiting for YOLO detection...");
     calib_frame_count_ = 0;  // Reset counter when YOLO loses detection
+    calib_searching_ = true;  // Reset to searching mode
+    calib_current_thres_ = 250;  // Reset threshold
     return;
   }
 
@@ -652,8 +658,32 @@ void ArmorYoloDetectorNode::performBinaryThresCalibration(
   }
   yolo_avg_area /= yolo_objects.size();
 
-  // Binary search for optimal threshold
-  int low = 50, high = 250, best_thres = detector_->binary_thres;
+  if (calib_searching_) {
+    // Phase 1: Decrement threshold from 250 until detection succeeds
+    // Temporarily set binary threshold and run detection
+    int original_thres = detector_->binary_thres;
+    detector_->binary_thres = calib_current_thres_;
+    auto armors = detector_->processROIs(img, gray_img, rois);
+    detector_->binary_thres = original_thres;
+
+    if (armors.empty()) {
+      // No detection at this threshold, decrement and try again next frame
+      calib_current_thres_--;
+      if (calib_current_thres_ < 30) {
+        calib_current_thres_ = 30;  // Don't go too low
+      }
+      FYT_INFO("armor_yolo_detect", "Calibration searching: thres={}, no detection yet", calib_current_thres_);
+      return;
+    } else {
+      // Detection succeeded! Switch to binary search mode
+      calib_searching_ = false;
+      calib_frame_count_ = 0;
+      FYT_INFO("armor_yolo_detect", "Calibration: detection found at thres={}, starting binary search", calib_current_thres_);
+    }
+  }
+
+  // Phase 2: Binary search to find optimal threshold
+  int low = 30, high = calib_current_thres_, best_thres = calib_current_thres_;
   double best_error = std::numeric_limits<double>::max();
   constexpr int max_iterations = 5;
   constexpr double error_tolerance = 100.0;  // pixels^2
@@ -668,13 +698,8 @@ void ArmorYoloDetectorNode::performBinaryThresCalibration(
     detector_->binary_thres = original_thres;
 
     if (armors.empty()) {
-      // No armors detected at this threshold, adjust search range
-      if (iter == 0) {
-        // First iteration, try higher threshold
-        low = mid + 1;
-      } else {
-        high = mid - 1;
-      }
+      // No armors detected at this threshold, need lower threshold
+      high = mid - 1;
       continue;
     }
 
@@ -712,9 +737,12 @@ void ArmorYoloDetectorNode::performBinaryThresCalibration(
     }
   }
 
-  // Only count frames where we got valid YOLO detection
+  // Update threshold to best found
+  calib_current_thres_ = best_thres;
+
+  // Only count frames where we got valid detection
   calib_frame_count_++;
-  FYT_INFO("armor_yolo_detect", "Calibration progress: {}/10", calib_frame_count_);
+  FYT_INFO("armor_yolo_detect", "Calibration progress: {}/10 (thres={})", calib_frame_count_, best_thres);
 
   if (calib_frame_count_ >= 10) {
     calib_done_ = true;