diff --git a/src/rm_auto_aim/armor_detector/src/armor_detector_node.cpp b/src/rm_auto_aim/armor_detector/src/armor_detector_node.cpp index a521b92..4c30c1d 100644 --- a/src/rm_auto_aim/armor_detector/src/armor_detector_node.cpp +++ b/src/rm_auto_aim/armor_detector/src/armor_detector_node.cpp @@ -419,7 +419,10 @@ std::unique_ptr ArmorDetectorNode::initDetector() { .max_large_center_distance = declare_parameter("armor.max_large_center_distance", 5.0), .max_angle = declare_parameter("armor.max_angle", 35.0)}; - auto detector = std::make_unique(binary_thres, EnemyColor::RED, l_params, a_params); + // detect_color: 0=RED, 1=BLUE (从 yaml 读取,setMode 会动态更新) + int detect_color_param = declare_parameter("detect_color", 0); + EnemyColor init_color = (detect_color_param == 1) ? EnemyColor::BLUE : EnemyColor::RED; + auto detector = std::make_unique(binary_thres, init_color, l_params, a_params); // Init classifier namespace fs = std::filesystem; @@ -534,6 +537,9 @@ rcl_interfaces::msg::SetParametersResult ArmorDetectorNode::onSetParameters( for (const auto ¶m : parameters) { if (param.get_name() == "binary_thres") { detector_->binary_thres = param.as_int(); + } else if (param.get_name() == "detect_color") { + int color_val = param.as_int(); + detector_->detect_color = (color_val == 1) ? EnemyColor::BLUE : EnemyColor::RED; } else if (param.get_name() == "debug.enable_terminal_log") { debug_terminal_log_ = param.as_bool(); } else if (param.get_name() == "debug.enable_markers") { diff --git a/src/rm_auto_aim/armor_solver/include/armor_solver/armor_solver_node.hpp b/src/rm_auto_aim/armor_solver/include/armor_solver/armor_solver_node.hpp index 315a1a2..c4df676 100644 --- a/src/rm_auto_aim/armor_solver/include/armor_solver/armor_solver_node.hpp +++ b/src/rm_auto_aim/armor_solver/include/armor_solver/armor_solver_node.hpp @@ -112,7 +112,10 @@ private: std::atomic bullet_speed_{0.0}; std::atomic bullet_speed_param_{20.0}; std::atomic bullet_speed_debug_{false}; - std::atomic shoot_rate_{3}; + std::atomic shoot_rate_min_{3}; + std::atomic shoot_rate_max_{13}; + std::atomic ekf_detect_count_{0}; + std::atomic ekf_count_threshold_{30}; // Enable/Disable Armor Solver bool enable_; diff --git a/src/rm_auto_aim/armor_solver/src/armor_solver_node.cpp b/src/rm_auto_aim/armor_solver/src/armor_solver_node.cpp index 59ce21e..de2a95e 100644 --- a/src/rm_auto_aim/armor_solver/src/armor_solver_node.cpp +++ b/src/rm_auto_aim/armor_solver/src/armor_solver_node.cpp @@ -36,7 +36,9 @@ ArmorSolverNode::ArmorSolverNode(const rclcpp::NodeOptions &options) debug_mode_ = this->declare_parameter("debug", true); bullet_speed_debug_.store(this->declare_parameter("bullet_speed_debug", false), std::memory_order_relaxed); - shoot_rate_.store(this->declare_parameter("solver.shoot_rate", 3), std::memory_order_relaxed); + shoot_rate_min_.store(this->declare_parameter("solver.shoot_rate_min", 3), std::memory_order_relaxed); + shoot_rate_max_.store(this->declare_parameter("solver.shoot_rate_max", 13), std::memory_order_relaxed); + ekf_count_threshold_.store(this->declare_parameter("solver.ekf_count_threshold", 30), std::memory_order_relaxed); if (this->has_parameter("solver.bullet_speed")) { bullet_speed_param_.store(this->get_parameter("solver.bullet_speed").as_double(), std::memory_order_relaxed); @@ -205,7 +207,7 @@ ArmorSolverNode::ArmorSolverNode(const rclcpp::NodeOptions &options) control_msg_last.yaw = 0; control_msg_last.fire_advice = false; control_msg_last.shoot_rate = - static_cast(shoot_rate_.load(std::memory_order_relaxed)); + static_cast(shoot_rate_min_.load(std::memory_order_relaxed)); } @@ -231,14 +233,25 @@ void ArmorSolverNode::timerCallback() { // Init message rm_interfaces::msg::GimbalCmd control_msg; - int shoot_rate = shoot_rate_.load(std::memory_order_relaxed); - if (shoot_rate < 0) - { + int shoot_rate_min = shoot_rate_min_.load(std::memory_order_relaxed); + int shoot_rate_max = shoot_rate_max_.load(std::memory_order_relaxed); + int ekf_count = ekf_detect_count_.load(std::memory_order_relaxed); + int ekf_threshold = ekf_count_threshold_.load(std::memory_order_relaxed); + int shoot_rate; + + // Calculate shoot_rate based on EKF detect count + // Range: shoot_rate_min ~ shoot_rate_max + if (ekf_count >= ekf_threshold) { + // Gradually increase shoot_rate from min to max as count increases + int range = shoot_rate_max - shoot_rate_min; + int additional_rate = std::min(range, (ekf_count - ekf_threshold) / 3); + shoot_rate = shoot_rate_min + additional_rate; + } else { + shoot_rate = shoot_rate_min; + } + + if (shoot_rate < 0) { shoot_rate = 0; - } - else if (shoot_rate > 8) - { - shoot_rate = 8; } // If target never detected @@ -367,7 +380,11 @@ void ArmorSolverNode::armorsCallback(const rm_interfaces::msg::Armors::SharedPtr if (tracker_->tracker_state == Tracker::LOST) { tracker_->init(armors_msg); target_msg.tracking = false; + // Reset EKF detect count when tracker is lost + ekf_detect_count_.store(0, std::memory_order_relaxed); } else { + // Increment EKF detect count when successfully tracking (not lost) + ekf_detect_count_.fetch_add(1, std::memory_order_relaxed); dt_ = (time - last_time_).seconds(); tracker_->lost_thres = std::abs(static_cast(lost_time_thres_ / dt_)); if (tracker_->tracked_id == "outpost") { @@ -406,6 +423,7 @@ void ArmorSolverNode::armorsCallback(const rm_interfaces::msg::Armors::SharedPtr target_msg.radius_2 = tracker_->another_r; target_msg.d_zc = state(9); target_msg.d_za = tracker_->d_za; + target_msg.ekf_detect_count = ekf_detect_count_.load(std::memory_order_relaxed); } } @@ -563,8 +581,12 @@ rcl_interfaces::msg::SetParametersResult ArmorSolverNode::onSetParameters( bullet_speed_debug_.store(param.as_bool(), std::memory_order_relaxed); } else if (param.get_name() == "solver.bullet_speed") { bullet_speed_param_.store(param.as_double(), std::memory_order_relaxed); - } else if (param.get_name() == "solver.shoot_rate") { - shoot_rate_.store(param.as_int(), std::memory_order_relaxed); + } else if (param.get_name() == "solver.shoot_rate_min") { + shoot_rate_min_.store(param.as_int(), std::memory_order_relaxed); + } else if (param.get_name() == "solver.shoot_rate_max") { + shoot_rate_max_.store(param.as_int(), std::memory_order_relaxed); + } else if (param.get_name() == "solver.ekf_count_threshold") { + ekf_count_threshold_.store(param.as_int(), std::memory_order_relaxed); } else if (param.get_name() == "tracker.max_match_distance") { const double max_match_distance = param.as_double(); tracker_->setMatchThreshold(max_match_distance, this->get_parameter("tracker.max_match_yaw_diff").as_double()); diff --git a/src/rm_bringup/config/node_params/armor_solver_params.yaml b/src/rm_bringup/config/node_params/armor_solver_params.yaml index dc578f1..589982b 100644 --- a/src/rm_bringup/config/node_params/armor_solver_params.yaml +++ b/src/rm_bringup/config/node_params/armor_solver_params.yaml @@ -30,7 +30,9 @@ lost_time_thres: 1.0 solver: - shoot_rate: 3 + shoot_rate_min: 3 + shoot_rate_max: 13 + ekf_count_threshold: 30 bullet_speed: 20.5 shooting_range_width: 0.10 #射击范围 shooting_range_height: 0.10 #射击范围 diff --git a/src/rm_interfaces/msg/Target.msg b/src/rm_interfaces/msg/Target.msg index c3c8b42..53d9930 100644 --- a/src/rm_interfaces/msg/Target.msg +++ b/src/rm_interfaces/msg/Target.msg @@ -12,3 +12,4 @@ float64 d_za float64 d_zc float64 yaw_diff float64 position_diff +int32 ekf_detect_count