Add mode for shooting second target when tracking center.

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)

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;
 };
 

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,