Merge branch 'is_shoot_after_delay' into Gimbal_cascade_pid

# Conflicts:
#	rm_gimbal_controllers/src/gimbal_base.cpp

rm_gimbal_controllers/cfg/GimbalBase.cfg

@@ -5,8 +5,6 @@ from dynamic_reconfigure.parameter_generator_catkin import *
 
 gen = ParameterGenerator()
 
-gen.add("max_pid_yaw_pos_output", double_t, 0, "Pid yaw position max output", 0.0, 0, 99.0)
-gen.add("max_pid_pitch_pos_output", double_t, 0, "Pid pitch position max output", 0.0, 0, 99.0)
 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)
 

rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h

@@ -46,7 +46,7 @@
 #include <rm_common/hardware_interface/robot_state_interface.h>
 #include <rm_common/eigen_types.h>
 #include <rm_common/ros_utilities.h>
-#include <rm_msgs/TrackData.h>
+#include <rm_msgs/GimbalDesError.h>
 
 namespace rm_gimbal_controllers
 {
@@ -77,25 +77,25 @@ 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 isShootAfterDelay(const ros::Time& time);
   void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time);
   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::GimbalDesError>> is_shoot_after_delay_pub_;
   realtime_tools::RealtimeBuffer<Config> config_rt_buffer_;
   dynamic_reconfigure::Server<rm_gimbal_controllers::BulletSolverConfig>* d_srv_{};
-  rm_msgs::TrackData switch_target_;
-  ros::NodeHandle controller_nh_;
-  ros::Publisher switch_target_pub = controller_nh_.advertise<rm_msgs::TrackData>("IsSwitchTarget", 100);
   Config config_{};
   double max_track_target_vel_;
   bool dynamic_reconfig_initialized_{};
   double output_yaw_{}, output_pitch_{};
   double bullet_speed_{}, resistance_coff_{};
   int selected_armor_;
   bool track_target_;
+  double is_shoot_after_delay_ = 1.;
 
   geometry_msgs::Point target_pos_{};
   double fly_time_;

rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h

@@ -41,57 +41,29 @@
 #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_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 <rm_common/filters/lp_filter.h>
-#include <rm_gimbal_controllers/GimbalBaseConfig.h>
 #include <dynamic_reconfigure/server.h>
+#include <realtime_tools/realtime_publisher.h>
 
 namespace rm_gimbal_controllers
 {
 struct GimbalConfig
 {
-  double max_pid_yaw_pos_output, max_pid_pitch_pos_output;
   // Input feedforward
   double yaw_k_v_, pitch_k_v_;
 };
 
-class GimbalDesVel
-{
-public:
-  GimbalDesVel(ros::NodeHandle& nh)
-  {
-    ros::NodeHandle nh_yaw = ros::NodeHandle(nh, "yaw");
-    ros::NodeHandle nh_pitch = ros::NodeHandle(nh, "pitch");
-    pitch_vel_des_lp_filter_ = std::make_shared<LowPassFilter>(nh_pitch);
-    yaw_vel_des_lp_filter_ = std::make_shared<LowPassFilter>(nh_yaw);
-  }
-  std::shared_ptr<LowPassFilter> pitch_vel_des_lp_filter_, yaw_vel_des_lp_filter_;
-  void update(double yaw_vel_des, double pitch_vel_des, double period, const ros::Time& time)
-  {
-    if (period < 0)
-      return;
-    if (period > 0.1)
-    {
-      pitch_vel_des_lp_filter_->reset();
-      yaw_vel_des_lp_filter_->reset();
-    }
-    pitch_vel_des_lp_filter_->input(pitch_vel_des, time);
-    yaw_vel_des_lp_filter_->input(yaw_vel_des, time);
-  }
-};
-
 class ChassisVel
 {
 public:
@@ -186,7 +158,6 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
   control_toolbox::Pid pid_yaw_pos_, pid_pitch_pos_;
 
   std::shared_ptr<BulletSolver> bullet_solver_;
-  std::shared_ptr<GimbalDesVel> gimbal_des_vel_;
 
   // ROS Interface
   ros::Time last_publish_time_{};

rm_gimbal_controllers/src/bullet_solver.cpp

@@ -82,6 +82,7 @@ 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));
+  is_shoot_after_delay_pub_.reset(new realtime_tools::RealtimePublisher<rm_msgs::GimbalDesError>(controller_nh, "allow_shoot", 10));
 }
 
 double BulletSolver::getResistanceCoefficient(double bullet_speed) const
@@ -124,18 +125,18 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d
                                                   (-acos(r / target_rho) + config_.angle1 + config_.angle2) *
                                                       std::abs(v_yaw) / max_track_target_vel_ :
                                               config_.angle2;
-  bool switch_target = 0;
+  is_shoot_after_delay_ = 1.;
+  if (((((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.)) &&
+      track_target_)
+    is_shoot_after_delay_ = 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.))
   {
     selected_armor_ = v_yaw > 0. ? -1 : 1;
     r = armors_num == 4 ? r2 : r1;
     z = armors_num == 4 ? pos.z + dz : pos.z;
-    switch_target = 1;
   }
-  switch_target_.yaw = switch_armor_angle;
-  switch_target_.id = switch_target;
-  switch_target_pub.publish(switch_target_);
   int count{};
   double error = 999;
   if (track_target_)
@@ -219,6 +220,16 @@ void BulletSolver::getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, ge
   }
 }
 
+void BulletSolver::isShootAfterDelay(const ros::Time& time)
+{
+  if (is_shoot_after_delay_pub_->trylock())
+  {
+    is_shoot_after_delay_pub_->msg_.stamp = time;
+    is_shoot_after_delay_pub_->msg_.error = is_shoot_after_delay_;
+    is_shoot_after_delay_pub_->unlockAndPublish();
+  }
+}
+
 void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time)
 {
   marker_desire_.points.clear();