shoot_rate add add adddd

src/rm_auto_aim/armor_detector/src/armor_detector_node.cpp

@@ -419,7 +419,10 @@ std::unique_ptr<Detector> 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<Detector>(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<Detector>(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 &param : 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") {

src/rm_auto_aim/armor_solver/include/armor_solver/armor_solver_node.hpp

@@ -112,7 +112,10 @@ private:
   std::atomic<double> bullet_speed_{0.0};
   std::atomic<double> bullet_speed_param_{20.0};
   std::atomic<bool> bullet_speed_debug_{false};
-  std::atomic<int> shoot_rate_{3};
+  std::atomic<int> shoot_rate_min_{3};
+  std::atomic<int> shoot_rate_max_{13};
+  std::atomic<int> ekf_detect_count_{0};
+  std::atomic<int> ekf_count_threshold_{30};
   
   // Enable/Disable Armor Solver
   bool enable_;

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<uint8_t>(shoot_rate_.load(std::memory_order_relaxed));
+    static_cast<uint8_t>(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<int>(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());

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  #射击范围

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