add auto binarythres fix logic strong 11

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

@@ -149,6 +149,8 @@ private:
   int calib_current_thres_ = 250;
   int calib_best_thres_ = 160;
   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
   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

@@ -31,6 +31,7 @@
 #include <numeric>
 #include <sstream>
 #include <string>
+#include <thread>
 #include <vector>
 
 #include <cv_bridge/cv_bridge.h>
@@ -356,6 +357,8 @@ std::unique_ptr<Detector> ArmorYoloDetectorNode::initDetector() {
   calib_current_thres_ = 250;
   calib_best_thres_ = 160;
   calib_best_error_ = std::numeric_limits<double>::max();
+  calib_frame_skip_ = 0;
+  calib_frame_skip_interval_ = 10;
   calib_save_yaml_path_ = this->declare_parameter("calib_save_yaml_path", std::string(""));
 
   // Light detection params
@@ -627,8 +630,14 @@ void ArmorYoloDetectorNode::performBinaryThresCalibration(
     detector_->binary_thres = calib_best_thres_;
 
     // Update parameter so it persists
-    this->set_parameter(rclcpp::Parameter("binary_thres", calib_best_thres_));
+    auto result1 = this->set_parameter(rclcpp::Parameter("binary_thres", calib_best_thres_));
-    this->set_parameter(rclcpp::Parameter("calib_binarythres", false));
+    FYT_INFO("armor_yolo_detect", "Set binary_thres={}, success={}", calib_best_thres_, result1.successful);
+
+    // Small delay before disabling calibration
+    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);
 
     // Save to yaml file if path is configured
     if (!calib_save_yaml_path_.empty()) {
@@ -679,12 +688,9 @@ void ArmorYoloDetectorNode::performBinaryThresCalibration(
   detector_->binary_thres = original_thres;
 
   if (armors.empty()) {
-    // No detection at this threshold, decrement and try again next frame
+    // No detection at this threshold
     FYT_INFO("armor_yolo_detect", "Calibration: thres={}, no detection", calib_current_thres_);
-    calib_current_thres_--;
+  } else {
-    return;
-  }
-
     // Detection succeeded! Calculate error
     double trad_avg_area = 0;
     for (const auto& armor : armors) {
@@ -706,9 +712,6 @@ void ArmorYoloDetectorNode::performBinaryThresCalibration(
       FYT_INFO("armor_yolo_detect", "Calibration: new best thres={}, error={:.1f}", calib_best_thres_, calib_best_error_);
     }
 
-  // Decrement threshold for next frame
-  calib_current_thres_--;
-
     // Check if we should finish (error is small enough)
     constexpr double error_tolerance = 100.0;  // pixels^2
     if (error <= error_tolerance) {
@@ -716,8 +719,14 @@ void ArmorYoloDetectorNode::performBinaryThresCalibration(
       detector_->binary_thres = calib_best_thres_;
 
       // Update parameter so it persists
-    this->set_parameter(rclcpp::Parameter("binary_thres", calib_best_thres_));
+      auto result1 = this->set_parameter(rclcpp::Parameter("binary_thres", calib_best_thres_));
-    this->set_parameter(rclcpp::Parameter("calib_binarythres", false));
+      FYT_INFO("armor_yolo_detect", "Set binary_thres={}, success={}", calib_best_thres_, result1.successful);
+
+      // Small delay before disabling calibration
+      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);
 
       // Save to yaml file if path is configured
       if (!calib_save_yaml_path_.empty()) {
@@ -726,6 +735,18 @@ void ArmorYoloDetectorNode::performBinaryThresCalibration(
 
       FYT_INFO("armor_yolo_detect", "=== Binary threshold calibration complete: {} ===", calib_best_thres_);
       FYT_INFO("armor_yolo_detect", "Calibration result saved. Set calib_binarythres=false to disable calibration mode.");
+      return;
+    }
+  }
+
+  // Only decrement every few frames to slow down
+  calib_frame_skip_++;
+  if (calib_frame_skip_ >= calib_frame_skip_interval_) {
+    calib_frame_skip_ = 0;
+    calib_current_thres_--;
+    FYT_INFO("armor_yolo_detect", "Calibration: decrementing to thres={}", calib_current_thres_);
+  } else {
+    FYT_INFO("armor_yolo_detect", "Calibration: skipping (frame {}/{})", calib_frame_skip_, calib_frame_skip_interval_);
   }
 }