Merge branch 'master' into dbus-follow-feedforward

rm_gimbal_controllers/CMakeLists.txt

@@ -28,6 +28,7 @@ find_package(catkin REQUIRED
 
 generate_dynamic_reconfigure_options(
         cfg/BulletSolver.cfg
+        cfg/GimbalBase.cfg
 )
 
 ###################################

rm_gimbal_controllers/cfg/BulletSolver.cfg

@@ -14,5 +14,13 @@ 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("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("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)
 
 exit(gen.generate(PACKAGE, "bullet_solver", "BulletSolver"))

rm_gimbal_controllers/cfg/GimbalBase.cfg

@@ -0,0 +1,14 @@
+#!/usr/bin/env python
+PACKAGE = "rm_gimbal_controllers"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+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("accel_yaw_", double_t, 0, "Acceleration of rate yaw", 0.0, 0, 999.0)
+gen.add("accel_pitch_", double_t, 0, "Acceleration of rate pitch", 0.0, 0, 999.0)
+gen.add("k_chassis_vel_", double_t, 0, "Yaw chassis velocity feedforward scale", 0.0, 0, 1.0)
+
+exit(gen.generate(PACKAGE, "gimbal_base", "GimbalBase"))

rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h

@@ -46,13 +46,19 @@
 #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, max_switch_angle,
+      min_switch_angle, max_chassis_angular_vel, track_rotate_target_delay, track_move_target_delay;
+  int min_fit_switch_count;
 };
 
 class BulletSolver
@@ -61,7 +67,7 @@ class BulletSolver
   explicit BulletSolver(ros::NodeHandle& controller_nh);
 
   bool solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, double v_yaw,
-             double r1, double r2, double dz, int armors_num);
+             double r1, double r2, double dz, int armors_num, double chassis_angular_vel_z);
   double getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r1,
                         double r2, double dz, int armors_num, double yaw_real, double pitch_real, double bullet_speed);
   double getResistanceCoefficient(double bullet_speed) const;
@@ -76,25 +82,35 @@ 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_{};
+  double fly_time_;
+  int shoot_beforehand_cmd_{};
   int selected_armor_;
+  int count_;
   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_;
   visualization_msgs::Marker marker_desire_;
   visualization_msgs::Marker marker_real_;
 };

rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h

@@ -37,23 +37,33 @@
 
 #pragma once
 
-#include <effort_controllers/joint_position_controller.h>
+#include <effort_controllers/joint_velocity_controller.h>
 #include <controller_interface/multi_interface_controller.h>
 #include <hardware_interface/joint_command_interface.h>
-#include <rm_common/hardware_interface/robot_state_interface.h>
 #include <hardware_interface/imu_sensor_interface.h>
-#include <realtime_tools/realtime_publisher.h>
+#include <rm_common/hardware_interface/robot_state_interface.h>
+#include <rm_common/filters/filters.h>
 #include <rm_msgs/GimbalCmd.h>
 #include <rm_msgs/TrackData.h>
 #include <rm_msgs/GimbalDesError.h>
-#include <dynamic_reconfigure/server.h>
+#include <rm_msgs/GimbalPosState.h>
+#include <rm_gimbal_controllers/GimbalBaseConfig.h>
 #include <rm_gimbal_controllers/bullet_solver.h>
 #include <tf2_eigen/tf2_eigen.h>
 #include <Eigen/Eigen>
-#include <rm_common/filters/filters.h>
+#include <control_toolbox/pid.h>
+#include <urdf/model.h>
+#include <dynamic_reconfigure/server.h>
+#include <realtime_tools/realtime_publisher.h>
 
 namespace rm_gimbal_controllers
 {
+struct GimbalConfig
+{
+  double yaw_k_v_, pitch_k_v_, k_chassis_vel_;
+  double accel_pitch_{}, accel_yaw_{};
+};
+
 class ChassisVel
 {
 public:
@@ -139,27 +149,33 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
   void updateChassisVel();
   void commandCB(const rm_msgs::GimbalCmdConstPtr& msg);
   void trackCB(const rm_msgs::TrackDataConstPtr& msg);
+  void reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uint32_t);
 
   rm_control::RobotStateHandle robot_state_handle_;
   hardware_interface::ImuSensorHandle imu_sensor_handle_;
   bool has_imu_ = true;
-  effort_controllers::JointPositionController ctrl_yaw_, ctrl_pitch_;
+  effort_controllers::JointVelocityController ctrl_yaw_, ctrl_pitch_;
+  control_toolbox::Pid pid_yaw_pos_, pid_pitch_pos_;
 
   std::shared_ptr<BulletSolver> bullet_solver_;
 
   // ROS Interface
   ros::Time last_publish_time_{};
+  std::unique_ptr<realtime_tools::RealtimePublisher<rm_msgs::GimbalPosState>> yaw_pos_state_pub_, pitch_pos_state_pub_;
   std::shared_ptr<realtime_tools::RealtimePublisher<rm_msgs::GimbalDesError>> error_pub_;
   ros::Subscriber cmd_gimbal_sub_;
   ros::Subscriber data_track_sub_;
   realtime_tools::RealtimeBuffer<rm_msgs::GimbalCmd> cmd_rt_buffer_;
   realtime_tools::RealtimeBuffer<rm_msgs::TrackData> track_rt_buffer_;
+  urdf::JointConstSharedPtr pitch_joint_urdf_, yaw_joint_urdf_;
 
   rm_msgs::GimbalCmd cmd_gimbal_;
   rm_msgs::TrackData data_track_;
   std::string gimbal_des_frame_id_{}, imu_name_{};
   double publish_rate_{};
   bool state_changed_{};
+  bool pitch_des_in_limit_{}, yaw_des_in_limit_{};
+  int loop_count_{};
 
   // Transform
   geometry_msgs::TransformStamped odom2gimbal_des_, odom2pitch_, odom2base_, last_odom2base_;
@@ -169,18 +185,16 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
   double gravity_;
   bool enable_gravity_compensation_;
 
-  // Input feedforward
-  double yaw_k_v_;
-  double pitch_k_v_;
-
-  // 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_{};
+  GimbalConfig config_{};
+  realtime_tools::RealtimeBuffer<GimbalConfig> config_rt_buffer_;
+  dynamic_reconfigure::Server<rm_gimbal_controllers::GimbalBaseConfig>* d_srv_{};
+
+  RampFilter<double>*ramp_rate_pitch_{}, *ramp_rate_yaw_{};
+
   enum
   {
     RATE,

rm_gimbal_controllers/src/bullet_solver.cpp

@@ -52,7 +52,15 @@ 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),
+              .max_switch_angle = getParam(controller_nh, "max_switch_angle", 40.0),
+              .min_switch_angle = getParam(controller_nh, "min_switch_angle", 2.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.),
+              .min_fit_switch_count = getParam(controller_nh, "min_fit_switch_count", 3) };
   max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0);
   config_rt_buffer_.initRT(config_);
 
@@ -80,6 +88,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>("/change", 10, &BulletSolver::identifiedTargetChangeCB, this);
 }
 
 double BulletSolver::getResistanceCoefficient(double bullet_speed) const
@@ -100,7 +114,7 @@ double BulletSolver::getResistanceCoefficient(double bullet_speed) const
 }
 
 bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw,
-                         double v_yaw, double r1, double r2, double dz, int armors_num)
+                         double v_yaw, double r1, double r2, double dz, int armors_num, double chassis_angular_vel_z)
 {
   config_ = *config_rt_buffer_.readFromRT();
   bullet_speed_ = bullet_speed;
@@ -115,18 +129,48 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d
   selected_armor_ = 0;
   double r = r1;
   double z = pos.z;
+  double max_switch_angle = config_.max_switch_angle / 180 * M_PI;
+  double min_switch_angle = config_.min_switch_angle / 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.))
+  if (std::abs(chassis_angular_vel_z) >= config_.max_chassis_angular_vel)
+    track_target_ = 0;
+  double switch_armor_angle =
+      track_target_ ?
+          (acos(r / target_rho) - max_switch_angle +
+           (-acos(r / target_rho) + (max_switch_angle + min_switch_angle)) * std::abs(v_yaw) / max_track_target_vel_) *
+                  (1 - std::abs(chassis_angular_vel_z) / config_.max_chassis_angular_vel) +
+              min_switch_angle * std::abs(chassis_angular_vel_z) / config_.max_chassis_angular_vel :
+          min_switch_angle;
+  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.)) &&
+      std::abs(v_yaw) >= 1.0)
   {
-    selected_armor_ = v_yaw > 0. ? -1 : 1;
-    r = armors_num == 4 ? r2 : r1;
-    z = armors_num == 4 ? pos.z + dz : pos.z;
+    count_++;
+    if (identified_target_change_)
+    {
+      count_ = 0;
+      identified_target_change_ = false;
+    }
+    if (count_ >= config_.min_fit_switch_count)
+    {
+      if (count_ == config_.min_fit_switch_count)
+        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;
+    }
   }
+  is_in_delay_before_switch_ =
+      (((((yaw + selected_armor_ * 2 * M_PI / armors_num) + v_yaw * (rough_fly_time + config_.delay)) >
+         output_yaw_ + switch_armor_angle) &&
+        v_yaw > 0.) ||
+       ((((yaw + selected_armor_ * 2 * M_PI / armors_num) + v_yaw * (rough_fly_time + config_.delay)) <
+         output_yaw_ - switch_armor_angle) &&
+        v_yaw < 0.)) &&
+      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{};
   double error = 999;
   if (track_target_)
@@ -180,6 +224,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 +350,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 ((ros::Time::now() - switch_armor_time_).toSec() < ros::Duration(config_.gimbal_switch_duration).toSec())
+    shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT;
+  else if (is_in_delay_before_switch_)
+    shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_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 +391,14 @@ 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.max_switch_angle = init_config.max_switch_angle;
+    config.min_switch_angle = init_config.min_switch_angle;
+    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;
+    config.min_fit_switch_count = init_config.min_fit_switch_count;
     dynamic_reconfig_initialized_ = true;
   }
   Config config_non_rt{ .resistance_coff_qd_10 = config.resistance_coff_qd_10,
@@ -326,7 +409,15 @@ 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,
+                        .max_switch_angle = config.max_switch_angle,
+                        .min_switch_angle = config.min_switch_angle,
+                        .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,
+                        .min_fit_switch_count = config.min_fit_switch_count };
   config_rt_buffer_.writeFromNonRT(config_non_rt);
 }
 }  // namespace rm_gimbal_controllers