diff --git a/src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/armor_yolo_detector_node.hpp b/src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/armor_yolo_detector_node.hpp index d46fe3a..c56c9e1 100644 --- a/src/rm_auto_aim/armor_yolo_detect/include/armor_yolo_detect/armor_yolo_detector_node.hpp +++ b/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::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); diff --git a/src/rm_auto_aim/armor_yolo_detect/src/armor_yolo_detector_node.cpp b/src/rm_auto_aim/armor_yolo_detect/src/armor_yolo_detector_node.cpp index 6264b1b..d3ce374 100644 --- a/src/rm_auto_aim/armor_yolo_detect/src/armor_yolo_detector_node.cpp +++ b/src/rm_auto_aim/armor_yolo_detect/src/armor_yolo_detector_node.cpp @@ -359,6 +359,8 @@ std::unique_ptr ArmorYoloDetectorNode::initDetector() { calib_best_error_ = std::numeric_limits::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::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) {