Shoot second target when tracking center

rm_gimbal_controllers/cfg/BulletSolver.cfg

@@ -12,13 +12,16 @@ 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("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)
 gen.add("gimbal_switch_duration", double_t, 0, "Gimbal switch duration", 0.0, 0.0, 2.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.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)

rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h

@@ -56,8 +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, max_chassis_angular_vel, track_rotate_target_delay, track_move_target_delay;
+      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;
 };
 
@@ -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);

rm_gimbal_controllers/src/bullet_solver.cpp

@@ -51,12 +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.),
+              .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),
               .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", 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.),
@@ -168,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{};
@@ -182,6 +186,16 @@ 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_.wait_next_armor_delay) +
+                         selected_armor_ * 2 * M_PI / armors_num) > output_yaw_) ||
+        (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)
+    {
+      r = r1;
+      z = pos.z;
+    }
   }
   target_pos_.z = z;
   while (error >= 0.001)
@@ -312,10 +326,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_.wait_diagonal_armor_delay : config_.wait_next_armor_delay;
+  double r, z;
+  if (selected_armor_ % 2 == 0)
+  {
+    r = r1;
+    z = pos.z;
+  }
+  else
   {
     r = armors_num == 4 ? r2 : r1;
     z = armors_num == 4 ? pos.z + dz : pos.z;
@@ -356,13 +378,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) > 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;
@@ -389,12 +412,15 @@ 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.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;
     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;
@@ -408,12 +434,15 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config,
                         .resistance_coff_qd_30 = config.resistance_coff_qd_30,
                         .g = config.g,
                         .delay = config.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,
                         .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,

rm_gimbal_controllers/src/gimbal_base.cpp

@@ -314,7 +314,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)
   {