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 3337100..d46fe3a 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 @@ -149,6 +149,8 @@ private: int calib_current_thres_ = 250; int calib_best_thres_ = 160; 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 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 977049b..85e07a5 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 @@ -31,6 +31,7 @@ #include #include #include +#include #include #include @@ -356,6 +357,8 @@ std::unique_ptr ArmorYoloDetectorNode::initDetector() { calib_current_thres_ = 250; calib_best_thres_ = 160; calib_best_error_ = std::numeric_limits::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_)); - this->set_parameter(rclcpp::Parameter("calib_binarythres", false)); + 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); + + // 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,53 +688,65 @@ 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_--; - return; - } + } else { + // Detection succeeded! Calculate error + double trad_avg_area = 0; + for (const auto& armor : armors) { + double w = cv::norm(armor.left_light.top - armor.right_light.top); + double h = cv::norm(armor.left_light.top - armor.left_light.bottom); + trad_avg_area += w * h; + } + trad_avg_area /= armors.size(); - // Detection succeeded! Calculate error - double trad_avg_area = 0; - for (const auto& armor : armors) { - double w = cv::norm(armor.left_light.top - armor.right_light.top); - double h = cv::norm(armor.left_light.top - armor.left_light.bottom); - trad_avg_area += w * h; - } - trad_avg_area /= armors.size(); + double error = std::abs(trad_avg_area - yolo_avg_area); - double error = std::abs(trad_avg_area - yolo_avg_area); + FYT_INFO("armor_yolo_detect", "Calibration: thres={}, yolo_area={:.1f}, trad_area={:.1f}, error={:.1f}", + calib_current_thres_, yolo_avg_area, trad_avg_area, error); - FYT_INFO("armor_yolo_detect", "Calibration: thres={}, yolo_area={:.1f}, trad_area={:.1f}, error={:.1f}", - calib_current_thres_, yolo_avg_area, trad_avg_area, error); - - // Track best threshold (minimum error) - if (error < calib_best_error_) { - calib_best_error_ = error; - calib_best_thres_ = calib_current_thres_; - 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) { - calib_done_ = true; - detector_->binary_thres = calib_best_thres_; - - // Update parameter so it persists - this->set_parameter(rclcpp::Parameter("binary_thres", calib_best_thres_)); - this->set_parameter(rclcpp::Parameter("calib_binarythres", false)); - - // Save to yaml file if path is configured - if (!calib_save_yaml_path_.empty()) { - saveBinaryThresToYaml(calib_best_thres_); + // Track best threshold (minimum error) + if (error < calib_best_error_) { + calib_best_error_ = error; + calib_best_thres_ = calib_current_thres_; + FYT_INFO("armor_yolo_detect", "Calibration: new best thres={}, error={:.1f}", calib_best_thres_, calib_best_error_); } - 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."); + // Check if we should finish (error is small enough) + constexpr double error_tolerance = 100.0; // pixels^2 + if (error <= error_tolerance) { + calib_done_ = true; + detector_->binary_thres = calib_best_thres_; + + // Update parameter so it persists + 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); + + // 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()) { + saveBinaryThresToYaml(calib_best_thres_); + } + + 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_); } }