Add gimbal cascade pid and modify bullet_solver

rm_gimbal_controllers/cfg/BulletSolver.cfg

@@ -14,5 +14,9 @@ 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("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("angle1", double_t, 0, "Switch angle1", 40.0, 0.0, 90.0)
+gen.add("angle2", double_t, 0, "Switch angle2", 2.0, 0.0, 90.0)
 
 exit(gen.generate(PACKAGE, "bullet_solver", "BulletSolver"))

rm_gimbal_controllers/cfg/GimbalBase.cfg

@@ -5,7 +5,10 @@ from dynamic_reconfigure.parameter_generator_catkin import *
 
 gen = ParameterGenerator()
 
-gen.add("yaw_k_v_", double_t, 0, "Pid pitch position max output", 0.0, 0, 1.0)
-gen.add("pitch_k_v_", double_t, 0, "Pid pitch position max output", 0.0, 0, 1.0)
+gen.add("yaw_k_v_", double_t, 0, "Yaw input feedforward scale", 0.0, 0, 1.0)
+gen.add("pitch_k_v_", double_t, 0, "Pitch input feedforward scale", 0.0, 0, 1.0)
+gen.add("k_chassis_vel_", double_t, 0, "Yaw chassis velocity feedforward scale", 0.0, 0, 1.0)
+gen.add("delay", double_t, 0, "Delay used to estimate current armor's yaw", 0.0, -1.0, 1.0)
+gen.add("delay2", double_t, 0, "Delay2 used to estimate current armor's position when moving", 0.0, -1.0, 1.0)
 
 exit(gen.generate(PACKAGE, "gimbal_base", "GimbalBase"))

rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h

@@ -46,13 +46,17 @@
 #include <rm_common/hardware_interface/robot_state_interface.h>
 #include <rm_common/eigen_types.h>
 #include <rm_common/ros_utilities.h>
+#include <std_msgs/Bool.h>
+#include <std_msgs/Float64.h>
+#include <rm_msgs/TrackData.h>
+#include <rm_msgs/ShootBeforehandCmd.h>
 
 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;
+      resistance_coff_qd_30, g, delay, dt, timeout, ban_shoot_duration, gimbal_switch_duration, angle1, angle2;
 };
 
 class BulletSolver
@@ -76,22 +80,31 @@ 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 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);
   ~BulletSolver() = default;
 
 private:
   std::shared_ptr<realtime_tools::RealtimePublisher<visualization_msgs::Marker>> path_desire_pub_;
   std::shared_ptr<realtime_tools::RealtimePublisher<visualization_msgs::Marker>> path_real_pub_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<rm_msgs::ShootBeforehandCmd>> shoot_beforehand_cmd_pub_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::Float64>> fly_time_pub_;
+  ros::Subscriber identified_target_change_sub_;
+  ros::Time switch_armor_time_{};
   realtime_tools::RealtimeBuffer<Config> config_rt_buffer_;
   dynamic_reconfigure::Server<rm_gimbal_controllers::BulletSolverConfig>* d_srv_{};
   Config config_{};
   double max_track_target_vel_;
-  bool dynamic_reconfig_initialized_{};
   double output_yaw_{}, output_pitch_{};
   double bullet_speed_{}, resistance_coff_{};
+  int shoot_beforehand_cmd_{};
   int selected_armor_;
   bool track_target_;
+  bool identified_target_change_ = true;
+  bool is_in_delay_before_switch_{};
+  bool dynamic_reconfig_initialized_{};
 
   geometry_msgs::Point target_pos_{};
   double fly_time_;

rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h

@@ -55,13 +55,15 @@
 #include <urdf/model.h>
 #include <dynamic_reconfigure/server.h>
 #include <realtime_tools/realtime_publisher.h>
+#include <rm_common/filters/lp_filter.h>
 
 namespace rm_gimbal_controllers
 {
 struct GimbalConfig
 {
-  // Input feedforward
-  double yaw_k_v_, pitch_k_v_;
+  // feedforward
+  double yaw_k_v_, pitch_k_v_, k_chassis_vel_;
+  double delay, delay2;
 };
 
 class ChassisVel
@@ -74,6 +76,8 @@ class ChassisVel
     nh.param("debug", is_debug_, true);
     linear_ = std::make_shared<Vector3WithFilter<double>>(num_data);
     angular_ = std::make_shared<Vector3WithFilter<double>>(num_data);
+    ros::NodeHandle nh_lp = ros::NodeHandle(nh, "lp");
+    angular_lp_filter_ = std::make_unique<LowPassFilter>(nh_lp);
     if (is_debug_)
     {
       real_pub_.reset(new realtime_tools::RealtimePublisher<geometry_msgs::Twist>(nh, "real", 1));
@@ -82,6 +86,7 @@ class ChassisVel
   }
   std::shared_ptr<Vector3WithFilter<double>> linear_;
   std::shared_ptr<Vector3WithFilter<double>> angular_;
+  std::shared_ptr<LowPassFilter> angular_lp_filter_;
   void update(double linear_vel[3], double angular_vel[3], double period)
   {
     if (period < 0)
@@ -90,9 +95,11 @@ class ChassisVel
     {
       linear_->clear();
       angular_->clear();
+      angular_lp_filter_->reset();
     }
     linear_->input(linear_vel);
     angular_->input(angular_vel);
+    angular_lp_filter_->input(angular_vel[2]);
     if (is_debug_ && loop_count_ % 10 == 0)
     {
       if (real_pub_->trylock())
@@ -185,12 +192,7 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
   double gravity_;
   bool enable_gravity_compensation_;
 
-  // Resistance compensation
-  double yaw_resistance_;
-  double velocity_saturation_point_, effort_saturation_point_;
-
   // Chassis
-  double k_chassis_vel_;
   std::shared_ptr<ChassisVel> chassis_vel_;
 
   bool dynamic_reconfig_initialized_{};

rm_gimbal_controllers/src/bullet_solver.cpp

@@ -52,7 +52,11 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh)
               .g = getParam(controller_nh, "g", 0.),
               .delay = getParam(controller_nh, "delay", 0.),
               .dt = getParam(controller_nh, "dt", 0.),
-              .timeout = getParam(controller_nh, "timeout", 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),
+              .angle1 = getParam(controller_nh, "angle1", 40.0),
+              .angle2 = getParam(controller_nh, "angle2", 2.0) };
   max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0);
   config_rt_buffer_.initRT(config_);
 
@@ -80,6 +84,12 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh)
       new realtime_tools::RealtimePublisher<visualization_msgs::Marker>(controller_nh, "model_desire", 10));
   path_real_pub_.reset(
       new realtime_tools::RealtimePublisher<visualization_msgs::Marker>(controller_nh, "model_real", 10));
+  shoot_beforehand_cmd_pub_.reset(
+      new realtime_tools::RealtimePublisher<rm_msgs::ShootBeforehandCmd>(controller_nh, "shoot_beforehand_cmd", 10));
+  fly_time_pub_.reset(new realtime_tools::RealtimePublisher<std_msgs::Float64>(controller_nh, "fly_time", 10));
+
+  identified_target_change_sub_ = controller_nh.subscribe<std_msgs::Bool>(
+      "/armor_processor/change", 10, &BulletSolver::identifiedTargetChangeCB, this);
 }
 
 double BulletSolver::getResistanceCoefficient(double bullet_speed) const
@@ -115,14 +125,26 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d
   selected_armor_ = 0;
   double r = r1;
   double z = pos.z;
+  double angle1 = config_.angle1 / 180 * M_PI;
+  double angle2 = config_.angle2 / 180 * M_PI;
   track_target_ = std::abs(v_yaw) < max_track_target_vel_;
-  double switch_armor_angle = track_target_ ?
-                                  acos(r / target_rho) - M_PI / 12 +
-                                      (-acos(r / target_rho) + M_PI / 6) * std::abs(v_yaw) / max_track_target_vel_ :
-                                  M_PI / 12;
-  if ((((yaw + v_yaw * rough_fly_time) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) ||
-      (((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.))
+  double switch_armor_angle =
+      track_target_ ? acos(r / target_rho) - angle1 +
+                          (-acos(r / target_rho) + (angle1 + angle2)) * std::abs(v_yaw) / max_track_target_vel_ :
+                      angle2;
+  is_in_delay_before_switch_ =
+      (((yaw + v_yaw * (rough_fly_time + config_.delay)) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) ||
+      (((yaw + v_yaw * (rough_fly_time + config_.delay)) < output_yaw_ - switch_armor_angle) && v_yaw < 0.);
+  bool is_low_speed = std::abs(v_yaw) < 1.0;
+  if (((((yaw + v_yaw * rough_fly_time) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) ||
+       (((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) &&
+      !is_low_speed)
   {
+    if (identified_target_change_)
+    {
+      identified_target_change_ = false;
+      switch_armor_time_ = ros::Time::now();
+    }
     selected_armor_ = v_yaw > 0. ? -1 : 1;
     r = armors_num == 4 ? r2 : r1;
     z = armors_num == 4 ? pos.z + dz : pos.z;
@@ -180,6 +202,11 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d
     if (count >= 20 || std::isnan(error))
       return false;
   }
+  if (fly_time_pub_->trylock())
+  {
+    fly_time_pub_->msg_.data = fly_time_;
+    fly_time_pub_->unlockAndPublish();
+  }
   return true;
 }
 
@@ -301,6 +328,32 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec
   return error;
 }
 
+void BulletSolver::identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg)
+{
+  if (msg->data)
+    identified_target_change_ = true;
+}
+
+void BulletSolver::judgeShootBeforehand(const ros::Time& time)
+{
+  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 (is_in_delay_before_switch_ && selected_armor_ == 0)
+    shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT;
+  else if ((ros::Time::now() - switch_armor_time_).toSec() < ros::Duration(config_.gimbal_switch_duration).toSec())
+    shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT;
+  else
+    shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR;
+  if (shoot_beforehand_cmd_pub_->trylock())
+  {
+    shoot_beforehand_cmd_pub_->msg_.stamp = time;
+    shoot_beforehand_cmd_pub_->msg_.cmd = shoot_beforehand_cmd_;
+    shoot_beforehand_cmd_pub_->unlockAndPublish();
+  }
+}
+
 void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t /*unused*/)
 {
   ROS_INFO("[Bullet Solver] Dynamic params change");
@@ -316,6 +369,10 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config,
     config.delay = init_config.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.angle1 = init_config.angle1;
+    config.angle2 = init_config.angle2;
     dynamic_reconfig_initialized_ = true;
   }
   Config config_non_rt{ .resistance_coff_qd_10 = config.resistance_coff_qd_10,
@@ -326,7 +383,11 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config,
                         .g = config.g,
                         .delay = config.delay,
                         .dt = config.dt,
-                        .timeout = config.timeout };
+                        .timeout = config.timeout,
+                        .ban_shoot_duration = config.ban_shoot_duration,
+                        .gimbal_switch_duration = config.gimbal_switch_duration,
+                        .angle1 = config.angle1,
+                        .angle2 = config.angle2 };
   config_rt_buffer_.writeFromNonRT(config_non_rt);
 }
 }  // namespace rm_gimbal_controllers

rm_gimbal_controllers/src/gimbal_base.cpp

@@ -62,23 +62,21 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro
   gravity_ = enable_feedforward ? (double)xml_rpc_value["gravity"] : 0.;
   enable_gravity_compensation_ = enable_feedforward && (bool)xml_rpc_value["enable_gravity_compensation"];
 
-  ros::NodeHandle resistance_compensation_nh(controller_nh, "yaw/resistance_compensation");
-  yaw_resistance_ = getParam(resistance_compensation_nh, "resistance", 0.);
-  velocity_saturation_point_ = getParam(resistance_compensation_nh, "velocity_saturation_point", 0.);
-  effort_saturation_point_ = getParam(resistance_compensation_nh, "effort_saturation_point", 0.);
-
-  k_chassis_vel_ = getParam(controller_nh, "yaw/k_chassis_vel", 0.);
   ros::NodeHandle chassis_vel_nh(controller_nh, "chassis_vel");
   chassis_vel_ = std::make_shared<ChassisVel>(chassis_vel_nh);
   ros::NodeHandle nh_bullet_solver = ros::NodeHandle(controller_nh, "bullet_solver");
   bullet_solver_ = std::make_shared<BulletSolver>(nh_bullet_solver);
 
   ros::NodeHandle nh_yaw = ros::NodeHandle(controller_nh, "yaw");
   ros::NodeHandle nh_pitch = ros::NodeHandle(controller_nh, "pitch");
-  ros::NodeHandle nh_pid_yaw_pos = ros::NodeHandle(controller_nh, "pid_yaw_pos");
-  ros::NodeHandle nh_pid_pitch_pos = ros::NodeHandle(controller_nh, "pid_pitch_pos");
-
-  config_ = { .yaw_k_v_ = getParam(nh_pitch, "k_v", 0.), .pitch_k_v_ = getParam(nh_yaw, "k_v", 0.) };
+  ros::NodeHandle nh_pid_yaw_pos = ros::NodeHandle(controller_nh, "yaw/pid_pos");
+  ros::NodeHandle nh_pid_pitch_pos = ros::NodeHandle(controller_nh, "pitch/pid_pos");
+
+  config_ = { .yaw_k_v_ = getParam(nh_yaw, "k_v", 0.),
+              .pitch_k_v_ = getParam(nh_pitch, "k_v", 0.),
+              .k_chassis_vel_ = getParam(controller_nh, "yaw/k_chassis_vel", 0.),
+              .delay = getParam(controller_nh, "delay", 0.),
+              .delay2 = getParam(controller_nh, "delay2", 0.) };
   config_rt_buffer_.initRT(config_);
   d_srv_ = new dynamic_reconfigure::Server<rm_gimbal_controllers::GimbalBaseConfig>(controller_nh);
   dynamic_reconfigure::Server<rm_gimbal_controllers::GimbalBaseConfig>::CallbackType cb =
@@ -141,10 +139,10 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro
   data_track_sub_ = controller_nh.subscribe<rm_msgs::TrackData>("/track", 1, &Controller::trackCB, this);
   publish_rate_ = getParam(controller_nh, "publish_rate", 100.);
   error_pub_.reset(new realtime_tools::RealtimePublisher<rm_msgs::GimbalDesError>(controller_nh, "error", 100));
-  pid_yaw_pos_state_pub_.reset(new realtime_tools::RealtimePublisher<control_msgs::JointControllerState>(
-      nh_pid_yaw_pos, "pid_yaw_pos_state", 1));
-  pid_pitch_pos_state_pub_.reset(new realtime_tools::RealtimePublisher<control_msgs::JointControllerState>(
-      nh_pid_pitch_pos, "pid_pitch_pos_state", 1));
+  pid_yaw_pos_state_pub_.reset(
+      new realtime_tools::RealtimePublisher<control_msgs::JointControllerState>(nh_yaw, "pos_state", 1));
+  pid_pitch_pos_state_pub_.reset(
+      new realtime_tools::RealtimePublisher<control_msgs::JointControllerState>(nh_pitch, "pos_state", 1));
   return true;
 }
 
@@ -267,7 +265,9 @@ void Controller::track(const ros::Time& time)
   double yaw_compute = yaw_real;
   double pitch_compute = -pitch_real;
   geometry_msgs::Point target_pos = data_track_.position;
-  geometry_msgs::Vector3 target_vel = data_track_.velocity;
+  geometry_msgs::Vector3 target_vel{};
+  if (data_track_.id != 12)
+    target_vel = data_track_.velocity;
   try
   {
     if (!data_track_.header.frame_id.empty())
@@ -282,15 +282,19 @@ void Controller::track(const ros::Time& time)
   {
     ROS_WARN("%s", ex.what());
   }
-  target_pos.x -= odom2pitch_.transform.translation.x;
-  target_pos.y -= odom2pitch_.transform.translation.y;
-  target_pos.z -= odom2pitch_.transform.translation.z;
+  double yaw = data_track_.yaw + data_track_.v_yaw * ((time - data_track_.header.stamp).toSec() + config_.delay);
+  target_pos.x +=
+      target_vel.x * ((time - data_track_.header.stamp).toSec() + config_.delay2) - odom2pitch_.transform.translation.x;
+  target_pos.y +=
+      target_vel.y * ((time - data_track_.header.stamp).toSec() + config_.delay2) - odom2pitch_.transform.translation.y;
+  target_pos.z += target_vel.z * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.z;
   target_vel.x -= chassis_vel_->linear_->x();
   target_vel.y -= chassis_vel_->linear_->y();
   target_vel.z -= chassis_vel_->linear_->z();
   bool solve_success =
-      bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed, data_track_.yaw, data_track_.v_yaw,
+      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);
+  bullet_solver_->judgeShootBeforehand(time);
 
   if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time)
   {
@@ -472,13 +476,15 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period)
   }
   loop_count_++;
 
-  ctrl_yaw_.setCommand(pid_yaw_pos_.getCurrentCmd() + config_.yaw_k_v_ * yaw_vel_des + ctrl_yaw_.joint_.getVelocity() -
-                       angular_vel_yaw.z);
+  ctrl_yaw_.setCommand(pid_yaw_pos_.getCurrentCmd() -
+                       config_.k_chassis_vel_ * chassis_vel_->angular_lp_filter_->output() +
+                       config_.yaw_k_v_ * yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z);
   ctrl_pitch_.setCommand(pid_pitch_pos_.getCurrentCmd() + config_.pitch_k_v_ * pitch_vel_des +
                          ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y);
-  ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time));
   ctrl_yaw_.update(time, period);
   ctrl_pitch_.update(time, period);
+  if (data_track_.id == 12)
+    ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time));
 }
 
 double Controller::feedForward(const ros::Time& time)
@@ -539,12 +545,16 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin
     GimbalConfig init_config = *config_rt_buffer_.readFromNonRT();  // config init use yaml
     config.yaw_k_v_ = init_config.yaw_k_v_;
     config.pitch_k_v_ = init_config.pitch_k_v_;
+    config.k_chassis_vel_ = init_config.k_chassis_vel_;
+    config.delay = init_config.delay;
+    config.delay2 = init_config.delay2;
     dynamic_reconfig_initialized_ = true;
   }
-  GimbalConfig config_non_rt{
-    .yaw_k_v_ = config.yaw_k_v_,
-    .pitch_k_v_ = config.pitch_k_v_,
-  };
+  GimbalConfig config_non_rt{ .yaw_k_v_ = config.yaw_k_v_,
+                              .pitch_k_v_ = config.pitch_k_v_,
+                              .k_chassis_vel_ = config.k_chassis_vel_,
+                              .delay = config.delay,
+                              .delay2 = config.delay2 };
   config_rt_buffer_.writeFromNonRT(config_non_rt);
 }