From a25d31841938c09986e3cf5075eb9654fc07d691 Mon Sep 17 00:00:00 2001
From: liyixin135 <421159734@qq.com>
Date: Wed, 24 Jul 2024 11:22:05 +0800
Subject: [PATCH 1/7] Don't shoot beforehand when target vel too slow.

---
 rm_gimbal_controllers/cfg/BulletSolver.cfg                | 1 +
 .../include/rm_gimbal_controllers/bullet_solver.h         | 5 +++--
 rm_gimbal_controllers/src/bullet_solver.cpp               | 8 ++++++--
 rm_gimbal_controllers/src/gimbal_base.cpp                 | 2 +-
 4 files changed, 11 insertions(+), 5 deletions(-)

diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg
index 44b1b1e1..c68c0ce5 100755
--- a/rm_gimbal_controllers/cfg/BulletSolver.cfg
+++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg
@@ -19,6 +19,7 @@ gen.add("gimbal_switch_duration", double_t, 0, "Gimbal switch duration", 0.0, 0.
 gen.add("max_switch_angle", double_t, 0, "Max switch angle", 40.0, 0.0, 90.0)
 gen.add("min_switch_angle", double_t, 0, "Min switch angle", 2.0, 0.0, 90.0)
 gen.add("min_fit_switch_count", int_t, 0, "Min count that current angle fit switch angle", 0, 3, 99)
+gen.add("min_shoot_beforehand_vel", double_t, 0, "Min velocity to shoot beforehand", 0, 5.0, 20)
 gen.add("max_chassis_angular_vel", double_t, 0, "Max chassis angular vel to switch target armor to center", 0.0, 0.0, 99.0)
 gen.add("track_rotate_target_delay", double_t, 0, "Used to estimate current armor's yaw", 0.0, -1.0, 1.0)
 gen.add("track_move_target_delay", double_t, 0, "Used to estimate current armor's position when moving", 0.0, -1.0, 1.0)
diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h
index 33ab54f9..aff317b2 100644
--- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h
+++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h
@@ -57,7 +57,8 @@ struct Config
 {
   double resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, resistance_coff_qd_18,
       resistance_coff_qd_30, g, delay, dt, timeout, ban_shoot_duration, gimbal_switch_duration, max_switch_angle,
-      min_switch_angle, max_chassis_angular_vel, track_rotate_target_delay, track_move_target_delay;
+      min_switch_angle, min_shoot_beforehand_vel, max_chassis_angular_vel, track_rotate_target_delay,
+      track_move_target_delay;
   int min_fit_switch_count;
 };
 
@@ -82,7 +83,7 @@ class BulletSolver
   void getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, geometry_msgs::Vector3& armor_vel,
                                  geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw,
                                  double r1, double r2, double dz, int armors_num);
-  void judgeShootBeforehand(const ros::Time& time);
+  void judgeShootBeforehand(const ros::Time& time, double v_yaw);
   void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time);
   void identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg);
   void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t);
diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp
index 15cda288..360dde9d 100644
--- a/rm_gimbal_controllers/src/bullet_solver.cpp
+++ b/rm_gimbal_controllers/src/bullet_solver.cpp
@@ -57,6 +57,7 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh)
               .gimbal_switch_duration = getParam(controller_nh, "gimbal_switch_duration", 0.0),
               .max_switch_angle = getParam(controller_nh, "max_switch_angle", 40.0),
               .min_switch_angle = getParam(controller_nh, "min_switch_angle", 2.0),
+              .min_shoot_beforehand_vel = getParam(controller_nh, "min_shoot_beforehand_vel", 5.0),
               .max_chassis_angular_vel = getParam(controller_nh, "max_chassis_angular_vel", 8.5),
               .track_rotate_target_delay = getParam(controller_nh, "track_rotate_target_delay", 0.),
               .track_move_target_delay = getParam(controller_nh, "track_move_target_delay", 0.),
@@ -356,13 +357,14 @@ void BulletSolver::identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg)
     identified_target_change_ = true;
 }
 
-void BulletSolver::judgeShootBeforehand(const ros::Time& time)
+void BulletSolver::judgeShootBeforehand(const ros::Time& time, double v_yaw)
 {
   if (!track_target_)
     shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR;
   else if ((ros::Time::now() - switch_armor_time_).toSec() < ros::Duration(config_.ban_shoot_duration).toSec())
     shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT;
-  else if ((ros::Time::now() - switch_armor_time_).toSec() < ros::Duration(config_.gimbal_switch_duration).toSec())
+  else if (((ros::Time::now() - switch_armor_time_).toSec() < ros::Duration(config_.gimbal_switch_duration).toSec()) &&
+           std::abs(v_yaw) > 5.0)
     shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT;
   else if (is_in_delay_before_switch_)
     shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT;
@@ -395,6 +397,7 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config,
     config.gimbal_switch_duration = init_config.gimbal_switch_duration;
     config.max_switch_angle = init_config.max_switch_angle;
     config.min_switch_angle = init_config.min_switch_angle;
+    config.min_shoot_beforehand_vel = init_config.min_shoot_beforehand_vel;
     config.max_chassis_angular_vel = init_config.max_chassis_angular_vel;
     config.track_rotate_target_delay = init_config.track_rotate_target_delay;
     config.track_move_target_delay = init_config.track_move_target_delay;
@@ -414,6 +417,7 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config,
                         .gimbal_switch_duration = config.gimbal_switch_duration,
                         .max_switch_angle = config.max_switch_angle,
                         .min_switch_angle = config.min_switch_angle,
+                        .min_shoot_beforehand_vel = config.min_shoot_beforehand_vel,
                         .max_chassis_angular_vel = config.max_chassis_angular_vel,
                         .track_rotate_target_delay = config.track_rotate_target_delay,
                         .track_move_target_delay = config.track_move_target_delay,
diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp
index 770a0dff..d270e7e3 100644
--- a/rm_gimbal_controllers/src/gimbal_base.cpp
+++ b/rm_gimbal_controllers/src/gimbal_base.cpp
@@ -302,7 +302,7 @@ void Controller::track(const ros::Time& time)
   bool solve_success = bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed, yaw, data_track_.v_yaw,
                                              data_track_.radius_1, data_track_.radius_2, data_track_.dz,
                                              data_track_.armors_num, chassis_vel_->angular_->z());
-  bullet_solver_->judgeShootBeforehand(time);
+  bullet_solver_->judgeShootBeforehand(time, data_track_.v_yaw);
 
   if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time)
   {

From 8657c96fcc59365a7da43885adbc6051c46f2f78 Mon Sep 17 00:00:00 2001
From: liyixin135 <421159734@qq.com>
Date: Wed, 24 Jul 2024 11:24:21 +0800
Subject: [PATCH 2/7] Modify cfg's value.

---
 rm_gimbal_controllers/cfg/BulletSolver.cfg | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg
index c68c0ce5..1acdec8e 100755
--- a/rm_gimbal_controllers/cfg/BulletSolver.cfg
+++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg
@@ -19,7 +19,7 @@ gen.add("gimbal_switch_duration", double_t, 0, "Gimbal switch duration", 0.0, 0.
 gen.add("max_switch_angle", double_t, 0, "Max switch angle", 40.0, 0.0, 90.0)
 gen.add("min_switch_angle", double_t, 0, "Min switch angle", 2.0, 0.0, 90.0)
 gen.add("min_fit_switch_count", int_t, 0, "Min count that current angle fit switch angle", 0, 3, 99)
-gen.add("min_shoot_beforehand_vel", double_t, 0, "Min velocity to shoot beforehand", 0, 5.0, 20)
+gen.add("min_shoot_beforehand_vel", double_t, 0, "Min velocity to shoot beforehand", 5.0, 0.0, 20)
 gen.add("max_chassis_angular_vel", double_t, 0, "Max chassis angular vel to switch target armor to center", 0.0, 0.0, 99.0)
 gen.add("track_rotate_target_delay", double_t, 0, "Used to estimate current armor's yaw", 0.0, -1.0, 1.0)
 gen.add("track_move_target_delay", double_t, 0, "Used to estimate current armor's position when moving", 0.0, -1.0, 1.0)

From cce0ea0490e083732db2d3bfdddbee0e2988d8a0 Mon Sep 17 00:00:00 2001
From: liyixin135 <421159734@qq.com>
Date: Wed, 24 Jul 2024 11:36:14 +0800
Subject: [PATCH 3/7] Modify wrong.

---
 rm_gimbal_controllers/src/bullet_solver.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp
index 360dde9d..e8439b01 100644
--- a/rm_gimbal_controllers/src/bullet_solver.cpp
+++ b/rm_gimbal_controllers/src/bullet_solver.cpp
@@ -364,7 +364,7 @@ void BulletSolver::judgeShootBeforehand(const ros::Time& time, double v_yaw)
   else if ((ros::Time::now() - switch_armor_time_).toSec() < ros::Duration(config_.ban_shoot_duration).toSec())
     shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT;
   else if (((ros::Time::now() - switch_armor_time_).toSec() < ros::Duration(config_.gimbal_switch_duration).toSec()) &&
-           std::abs(v_yaw) > 5.0)
+           std::abs(v_yaw) > config_.min_shoot_beforehand_vel)
     shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT;
   else if (is_in_delay_before_switch_)
     shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT;

From 3aac0f4bd079f41ce679ce1fd375fc51780acbe0 Mon Sep 17 00:00:00 2001
From: liyixin135 <421159734@qq.com>
Date: Sun, 28 Jul 2024 14:21:40 +0800
Subject: [PATCH 4/7] Add mode for shooting second target when tracking center.

---
 rm_gimbal_controllers/cfg/BulletSolver.cfg    |  2 +
 .../rm_gimbal_controllers/bullet_solver.h     |  6 +--
 rm_gimbal_controllers/src/bullet_solver.cpp   | 38 ++++++++++++++++---
 3 files changed, 37 insertions(+), 9 deletions(-)

diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg
index 1acdec8e..01bcc6dd 100755
--- a/rm_gimbal_controllers/cfg/BulletSolver.cfg
+++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg
@@ -12,6 +12,8 @@ gen.add("resistance_coff_qd_18", double_t, 0, "Air resistance divided by mass of
 gen.add("resistance_coff_qd_30", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0)
 gen.add("g", double_t, 0, "Air resistance divided by mass", 9.8, 9.6, 10.0)
 gen.add("delay", double_t, 0, "Delay of bullet firing", 0.0, 0, 0.5)
+gen.add("track_center_next_delay", double_t, 0, "Delay of shoot next target when in center mode", 0.0, 0, 0.5)
+gen.add("track_center_second_delay", double_t, 0, "Delay of shoot second target when in center mode", 0.0, 0, 0.5)
 gen.add("dt", double_t, 0, "Iteration interval", 0.01, 0.0001, 0.1)
 gen.add("timeout", double_t, 0, "Flight time exceeded", 2.0, 0.5, 3.0)
 gen.add("ban_shoot_duration", double_t, 0, "Ban shoot duration while beforehand shooting", 0.0, 0.0, 2.0)
diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h
index aff317b2..7fb369cd 100644
--- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h
+++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h
@@ -56,9 +56,9 @@ namespace rm_gimbal_controllers
 struct Config
 {
   double resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, resistance_coff_qd_18,
-      resistance_coff_qd_30, g, delay, dt, timeout, ban_shoot_duration, gimbal_switch_duration, max_switch_angle,
-      min_switch_angle, min_shoot_beforehand_vel, max_chassis_angular_vel, track_rotate_target_delay,
-      track_move_target_delay;
+      resistance_coff_qd_30, g, delay, track_center_next_delay, track_center_second_delay, dt, timeout,
+      ban_shoot_duration, gimbal_switch_duration, max_switch_angle, min_switch_angle, min_shoot_beforehand_vel,
+      max_chassis_angular_vel, track_rotate_target_delay, track_move_target_delay;
   int min_fit_switch_count;
 };
 
diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp
index e8439b01..0e4fcf05 100644
--- a/rm_gimbal_controllers/src/bullet_solver.cpp
+++ b/rm_gimbal_controllers/src/bullet_solver.cpp
@@ -51,13 +51,15 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh)
               .resistance_coff_qd_30 = getParam(controller_nh, "resistance_coff_qd_30", 0.),
               .g = getParam(controller_nh, "g", 0.),
               .delay = getParam(controller_nh, "delay", 0.),
+              .track_center_next_delay = getParam(controller_nh, "track_center_next_delay", 0.105),
+              .track_center_second_delay = getParam(controller_nh, "track_center_second_delay", 0.105),
               .dt = getParam(controller_nh, "dt", 0.),
               .timeout = getParam(controller_nh, "timeout", 0.),
               .ban_shoot_duration = getParam(controller_nh, "ban_shoot_duration", 0.0),
               .gimbal_switch_duration = getParam(controller_nh, "gimbal_switch_duration", 0.0),
               .max_switch_angle = getParam(controller_nh, "max_switch_angle", 40.0),
               .min_switch_angle = getParam(controller_nh, "min_switch_angle", 2.0),
-              .min_shoot_beforehand_vel = getParam(controller_nh, "min_shoot_beforehand_vel", 5.0),
+              .min_shoot_beforehand_vel = getParam(controller_nh, "min_shoot_beforehand_vel", 4.5),
               .max_chassis_angular_vel = getParam(controller_nh, "max_chassis_angular_vel", 8.5),
               .track_rotate_target_delay = getParam(controller_nh, "track_rotate_target_delay", 0.),
               .track_move_target_delay = getParam(controller_nh, "track_move_target_delay", 0.),
@@ -169,7 +171,8 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d
          output_yaw_ - switch_armor_angle) &&
         v_yaw < 0.)) &&
       track_target_;
-  yaw += v_yaw * config_.track_rotate_target_delay;
+  if (track_target_)
+    yaw += v_yaw * config_.track_rotate_target_delay;
   pos.x += vel.x * config_.track_move_target_delay;
   pos.y += vel.y * config_.track_move_target_delay;
   int count{};
@@ -183,6 +186,17 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d
   {
     target_pos_.x = pos.x - r * cos(atan2(pos.y, pos.x));
     target_pos_.y = pos.y - r * sin(atan2(pos.y, pos.x));
+    if (((v_yaw > 1.0 && (yaw + v_yaw * (fly_time_ + config_.track_center_next_delay) +
+                          selected_armor_ * 2 * M_PI / armors_num) > output_yaw_) ||
+         (v_yaw < -1.0 && (yaw + v_yaw * (fly_time_ + config_.track_center_next_delay) +
+                           selected_armor_ * 2 * M_PI / armors_num) < output_yaw_)) &&
+        std::abs(v_yaw) > 12.0)
+      selected_armor_ = v_yaw > 0. ? -2 : 2;
+    if (selected_armor_ % 2 == 0)
+    {
+      r = armors_num == 4 ? r1 : r2;
+      z = armors_num == 4 ? pos.z : pos.z + dz;
+    }
   }
   target_pos_.z = z;
   while (error >= 0.001)
@@ -313,10 +327,18 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec
                                     double r1, double r2, double dz, int armors_num, double yaw_real, double pitch_real,
                                     double bullet_speed)
 {
-  double delay = track_target_ ? 0 : config_.delay;
-  double r = r1;
-  double z = pos.z;
-  if (selected_armor_ != 0)
+  double delay;
+  if (track_target_)
+    delay = 0.;
+  else
+    delay = selected_armor_ % 2 == 0 ? config_.track_center_second_delay : config_.track_center_next_delay;
+  double r, z;
+  if (selected_armor_ % 2 == 0)
+  {
+    r = armors_num == 4 ? r1 : r2;
+    z = armors_num == 4 ? pos.z : pos.z + dz;
+  }
+  else
   {
     r = armors_num == 4 ? r2 : r1;
     z = armors_num == 4 ? pos.z + dz : pos.z;
@@ -391,6 +413,8 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config,
     config.resistance_coff_qd_30 = init_config.resistance_coff_qd_30;
     config.g = init_config.g;
     config.delay = init_config.delay;
+    config.track_center_next_delay = init_config.track_center_next_delay;
+    config.track_center_second_delay = init_config.track_center_second_delay;
     config.dt = init_config.dt;
     config.timeout = init_config.timeout;
     config.ban_shoot_duration = init_config.ban_shoot_duration;
@@ -411,6 +435,8 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config,
                         .resistance_coff_qd_30 = config.resistance_coff_qd_30,
                         .g = config.g,
                         .delay = config.delay,
+                        .track_center_next_delay = config.track_center_next_delay,
+                        .track_center_second_delay = config.track_center_second_delay,
                         .dt = config.dt,
                         .timeout = config.timeout,
                         .ban_shoot_duration = config.ban_shoot_duration,

From b34f30028a971177497aaaef5469746833f1b55f Mon Sep 17 00:00:00 2001
From: liyixin135 <421159734@qq.com>
Date: Mon, 29 Jul 2024 14:00:21 +0800
Subject: [PATCH 5/7] Modify something unreasonable.

---
 rm_gimbal_controllers/src/bullet_solver.cpp | 17 ++++++++---------
 1 file changed, 8 insertions(+), 9 deletions(-)

diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp
index 0e4fcf05..3e35a517 100644
--- a/rm_gimbal_controllers/src/bullet_solver.cpp
+++ b/rm_gimbal_controllers/src/bullet_solver.cpp
@@ -186,16 +186,15 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d
   {
     target_pos_.x = pos.x - r * cos(atan2(pos.y, pos.x));
     target_pos_.y = pos.y - r * sin(atan2(pos.y, pos.x));
-    if (((v_yaw > 1.0 && (yaw + v_yaw * (fly_time_ + config_.track_center_next_delay) +
-                          selected_armor_ * 2 * M_PI / armors_num) > output_yaw_) ||
-         (v_yaw < -1.0 && (yaw + v_yaw * (fly_time_ + config_.track_center_next_delay) +
-                           selected_armor_ * 2 * M_PI / armors_num) < output_yaw_)) &&
-        std::abs(v_yaw) > 12.0)
+    if ((v_yaw > 1.0 && (yaw + v_yaw * (fly_time_ + config_.track_center_next_delay) +
+                         selected_armor_ * 2 * M_PI / armors_num) > output_yaw_) ||
+        (v_yaw < -1.0 && (yaw + v_yaw * (fly_time_ + config_.track_center_next_delay) +
+                          selected_armor_ * 2 * M_PI / armors_num) < output_yaw_))
       selected_armor_ = v_yaw > 0. ? -2 : 2;
     if (selected_armor_ % 2 == 0)
     {
-      r = armors_num == 4 ? r1 : r2;
-      z = armors_num == 4 ? pos.z : pos.z + dz;
+      r = r1;
+      z = pos.z;
     }
   }
   target_pos_.z = z;
@@ -335,8 +334,8 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec
   double r, z;
   if (selected_armor_ % 2 == 0)
   {
-    r = armors_num == 4 ? r1 : r2;
-    z = armors_num == 4 ? pos.z : pos.z + dz;
+    r = r1;
+    z = pos.z;
   }
   else
   {

From 422f470592f9d45fd89f23f409d77f394057b0ff Mon Sep 17 00:00:00 2001
From: liyixin135 <421159734@qq.com>
Date: Mon, 29 Jul 2024 14:09:27 +0800
Subject: [PATCH 6/7] Modify param's value.

---
 rm_gimbal_controllers/cfg/BulletSolver.cfg | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg
index 01bcc6dd..d3c0848b 100755
--- a/rm_gimbal_controllers/cfg/BulletSolver.cfg
+++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg
@@ -12,8 +12,8 @@ gen.add("resistance_coff_qd_18", double_t, 0, "Air resistance divided by mass of
 gen.add("resistance_coff_qd_30", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0)
 gen.add("g", double_t, 0, "Air resistance divided by mass", 9.8, 9.6, 10.0)
 gen.add("delay", double_t, 0, "Delay of bullet firing", 0.0, 0, 0.5)
-gen.add("track_center_next_delay", double_t, 0, "Delay of shoot next target when in center mode", 0.0, 0, 0.5)
-gen.add("track_center_second_delay", double_t, 0, "Delay of shoot second target when in center mode", 0.0, 0, 0.5)
+gen.add("track_center_next_delay", double_t, 0, "Delay of shoot next target when in center mode", 0.0, 0.105, 0.5)
+gen.add("track_center_second_delay", double_t, 0, "Delay of shoot second target when in center mode", 0.0, 0.105, 0.5)
 gen.add("dt", double_t, 0, "Iteration interval", 0.01, 0.0001, 0.1)
 gen.add("timeout", double_t, 0, "Flight time exceeded", 2.0, 0.5, 3.0)
 gen.add("ban_shoot_duration", double_t, 0, "Ban shoot duration while beforehand shooting", 0.0, 0.0, 2.0)
@@ -21,7 +21,7 @@ gen.add("gimbal_switch_duration", double_t, 0, "Gimbal switch duration", 0.0, 0.
 gen.add("max_switch_angle", double_t, 0, "Max switch angle", 40.0, 0.0, 90.0)
 gen.add("min_switch_angle", double_t, 0, "Min switch angle", 2.0, 0.0, 90.0)
 gen.add("min_fit_switch_count", int_t, 0, "Min count that current angle fit switch angle", 0, 3, 99)
-gen.add("min_shoot_beforehand_vel", double_t, 0, "Min velocity to shoot beforehand", 5.0, 0.0, 20)
+gen.add("min_shoot_beforehand_vel", double_t, 0, "Min velocity to shoot beforehand", 0.0, 4.5, 20)
 gen.add("max_chassis_angular_vel", double_t, 0, "Max chassis angular vel to switch target armor to center", 0.0, 0.0, 99.0)
 gen.add("track_rotate_target_delay", double_t, 0, "Used to estimate current armor's yaw", 0.0, -1.0, 1.0)
 gen.add("track_move_target_delay", double_t, 0, "Used to estimate current armor's position when moving", 0.0, -1.0, 1.0)

From c74536ce592064bc061d1f1a64358b78db12434d Mon Sep 17 00:00:00 2001
From: liyixin135 <421159734@qq.com>
Date: Mon, 29 Jul 2024 14:36:27 +0800
Subject: [PATCH 7/7] Modify param's name.

---
 rm_gimbal_controllers/cfg/BulletSolver.cfg     |  4 ++--
 .../rm_gimbal_controllers/bullet_solver.h      |  2 +-
 rm_gimbal_controllers/src/bullet_solver.cpp    | 18 +++++++++---------
 3 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg
index d3c0848b..1e5a9535 100755
--- a/rm_gimbal_controllers/cfg/BulletSolver.cfg
+++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg
@@ -12,8 +12,8 @@ gen.add("resistance_coff_qd_18", double_t, 0, "Air resistance divided by mass of
 gen.add("resistance_coff_qd_30", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0)
 gen.add("g", double_t, 0, "Air resistance divided by mass", 9.8, 9.6, 10.0)
 gen.add("delay", double_t, 0, "Delay of bullet firing", 0.0, 0, 0.5)
-gen.add("track_center_next_delay", double_t, 0, "Delay of shoot next target when in center mode", 0.0, 0.105, 0.5)
-gen.add("track_center_second_delay", double_t, 0, "Delay of shoot second target when in center mode", 0.0, 0.105, 0.5)
+gen.add("wait_next_armor_delay", double_t, 0, "Delay of shooting next target when in center mode", 0.0, 0.105, 0.5)
+gen.add("wait_diagonal_armor_delay", double_t, 0, "Delay of shooting diagonal target when in center mode", 0.0, 0.105, 0.5)
 gen.add("dt", double_t, 0, "Iteration interval", 0.01, 0.0001, 0.1)
 gen.add("timeout", double_t, 0, "Flight time exceeded", 2.0, 0.5, 3.0)
 gen.add("ban_shoot_duration", double_t, 0, "Ban shoot duration while beforehand shooting", 0.0, 0.0, 2.0)
diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h
index 7fb369cd..eb171b8b 100644
--- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h
+++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h
@@ -56,7 +56,7 @@ namespace rm_gimbal_controllers
 struct Config
 {
   double resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, resistance_coff_qd_18,
-      resistance_coff_qd_30, g, delay, track_center_next_delay, track_center_second_delay, dt, timeout,
+      resistance_coff_qd_30, g, delay, wait_next_armor_delay, wait_diagonal_armor_delay, dt, timeout,
       ban_shoot_duration, gimbal_switch_duration, max_switch_angle, min_switch_angle, min_shoot_beforehand_vel,
       max_chassis_angular_vel, track_rotate_target_delay, track_move_target_delay;
   int min_fit_switch_count;
diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp
index 3e35a517..7deb568c 100644
--- a/rm_gimbal_controllers/src/bullet_solver.cpp
+++ b/rm_gimbal_controllers/src/bullet_solver.cpp
@@ -51,8 +51,8 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh)
               .resistance_coff_qd_30 = getParam(controller_nh, "resistance_coff_qd_30", 0.),
               .g = getParam(controller_nh, "g", 0.),
               .delay = getParam(controller_nh, "delay", 0.),
-              .track_center_next_delay = getParam(controller_nh, "track_center_next_delay", 0.105),
-              .track_center_second_delay = getParam(controller_nh, "track_center_second_delay", 0.105),
+              .wait_next_armor_delay = getParam(controller_nh, "wait_next_armor_delay", 0.105),
+              .wait_diagonal_armor_delay = getParam(controller_nh, "wait_diagonal_armor_delay", 0.105),
               .dt = getParam(controller_nh, "dt", 0.),
               .timeout = getParam(controller_nh, "timeout", 0.),
               .ban_shoot_duration = getParam(controller_nh, "ban_shoot_duration", 0.0),
@@ -186,9 +186,9 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d
   {
     target_pos_.x = pos.x - r * cos(atan2(pos.y, pos.x));
     target_pos_.y = pos.y - r * sin(atan2(pos.y, pos.x));
-    if ((v_yaw > 1.0 && (yaw + v_yaw * (fly_time_ + config_.track_center_next_delay) +
+    if ((v_yaw > 1.0 && (yaw + v_yaw * (fly_time_ + config_.wait_next_armor_delay) +
                          selected_armor_ * 2 * M_PI / armors_num) > output_yaw_) ||
-        (v_yaw < -1.0 && (yaw + v_yaw * (fly_time_ + config_.track_center_next_delay) +
+        (v_yaw < -1.0 && (yaw + v_yaw * (fly_time_ + config_.wait_next_armor_delay) +
                           selected_armor_ * 2 * M_PI / armors_num) < output_yaw_))
       selected_armor_ = v_yaw > 0. ? -2 : 2;
     if (selected_armor_ % 2 == 0)
@@ -330,7 +330,7 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec
   if (track_target_)
     delay = 0.;
   else
-    delay = selected_armor_ % 2 == 0 ? config_.track_center_second_delay : config_.track_center_next_delay;
+    delay = selected_armor_ % 2 == 0 ? config_.wait_diagonal_armor_delay : config_.wait_next_armor_delay;
   double r, z;
   if (selected_armor_ % 2 == 0)
   {
@@ -412,8 +412,8 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config,
     config.resistance_coff_qd_30 = init_config.resistance_coff_qd_30;
     config.g = init_config.g;
     config.delay = init_config.delay;
-    config.track_center_next_delay = init_config.track_center_next_delay;
-    config.track_center_second_delay = init_config.track_center_second_delay;
+    config.wait_next_armor_delay = init_config.wait_next_armor_delay;
+    config.wait_diagonal_armor_delay = init_config.wait_diagonal_armor_delay;
     config.dt = init_config.dt;
     config.timeout = init_config.timeout;
     config.ban_shoot_duration = init_config.ban_shoot_duration;
@@ -434,8 +434,8 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config,
                         .resistance_coff_qd_30 = config.resistance_coff_qd_30,
                         .g = config.g,
                         .delay = config.delay,
-                        .track_center_next_delay = config.track_center_next_delay,
-                        .track_center_second_delay = config.track_center_second_delay,
+                        .wait_next_armor_delay = config.wait_next_armor_delay,
+                        .wait_diagonal_armor_delay = config.wait_diagonal_armor_delay,
                         .dt = config.dt,
                         .timeout = config.timeout,
                         .ban_shoot_duration = config.ban_shoot_duration,