add auto binarythres fix logic strong 1112

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

@@ -151,6 +151,8 @@ private:
   double calib_best_error_ = std::numeric_limits<double>::max();
   int calib_frame_skip_ = 0;  // Frame counter to slow down decrement
   int calib_frame_skip_interval_ = 10;  // Decrement every 10 frames
+  int calib_no_detection_count_ = 0;  // Consecutive frames with no detection
+  int calib_max_no_detection_ = 5;  // Exit after this many consecutive no-detection frames
   std::string calib_save_yaml_path_;
   void performBinaryThresCalibration(const sensor_msgs::msg::Image::ConstSharedPtr& img_msg);
   void saveBinaryThresToYaml(int binary_thres);

src/rm_auto_aim/armor_yolo_detect/src/armor_yolo_detector_node.cpp

@@ -359,6 +359,8 @@ std::unique_ptr<Detector> ArmorYoloDetectorNode::initDetector() {
   calib_best_error_ = std::numeric_limits<double>::max();
   calib_frame_skip_ = 0;
   calib_frame_skip_interval_ = 10;
+  calib_no_detection_count_ = 0;
+  calib_max_no_detection_ = 5;
   calib_save_yaml_path_ = this->declare_parameter("calib_save_yaml_path", std::string(""));
 
   // Light detection params
@@ -532,6 +534,7 @@ rcl_interfaces::msg::SetParametersResult ArmorYoloDetectorNode::onSetParameters(
         calib_current_thres_ = 250;
         calib_best_thres_ = 160;
         calib_best_error_ = std::numeric_limits<double>::max();
+        calib_no_detection_count_ = 0;
         FYT_INFO("armor_yolo_detect", "Binary threshold calibration started (searching from 250)");
       }
     } else if (param.get_name() == "debug.enable_terminal_log") {
@@ -693,8 +696,40 @@ void ArmorYoloDetectorNode::performBinaryThresCalibration(
 
   if (armors.empty()) {
     // No detection at this threshold
-    FYT_INFO("armor_yolo_detect", "Calibration: thres={}, no detection", calib_current_thres_);
+    calib_no_detection_count_++;
+    FYT_INFO("armor_yolo_detect", "Calibration: thres={}, no detection ({}/{})",
+             calib_current_thres_, calib_no_detection_count_, calib_max_no_detection_);
+
+    // If too many consecutive no-detection, finish with best threshold
+    if (calib_no_detection_count_ >= calib_max_no_detection_) {
+      calib_done_ = true;
+      detector_->binary_thres = calib_best_thres_;
+
+      FYT_INFO("armor_yolo_detect", "Calibration: too many no-detections, using best thres={}", calib_best_thres_);
+
+      auto result1 = this->set_parameter(rclcpp::Parameter("binary_thres", calib_best_thres_));
+      FYT_INFO("armor_yolo_detect", "Set binary_thres={}, success={}", calib_best_thres_, result1.successful);
+
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
+      auto result2 = this->set_parameter(rclcpp::Parameter("calib_binarythres", false));
+      FYT_INFO("armor_yolo_detect", "Set calib_binarythres=false, success={}", result2.successful);
+
+      if (!calib_save_yaml_path_.empty()) {
+        saveBinaryThresToYaml(calib_best_thres_);
+      }
+
+      FYT_INFO("armor_yolo_detect", "========================================");
+      FYT_INFO("armor_yolo_detect", "Binary threshold calibration FINISHED (no detection timeout)!");
+      FYT_INFO("armor_yolo_detect", "Selected binary_thres = {} (best_error = {:.1f})", calib_best_thres_, calib_best_error_);
+      FYT_INFO("armor_yolo_detect", "binary_thres has been updated to {}", calib_best_thres_);
+      FYT_INFO("armor_yolo_detect", "Result saved to yaml file.");
+      FYT_INFO("armor_yolo_detect", "========================================");
+      return;
+    }
   } else {
+    // Detection succeeded, reset no-detection counter
+    calib_no_detection_count_ = 0;
     // Detection succeeded! Calculate error
     double trad_avg_area = 0;
     for (const auto& armor : armors) {