Merge pull request #158 from liyixin135/Gimbal_cascade_pid

Gimbal cascade pid

rm_gimbal_controllers/CMakeLists.txt

@@ -24,10 +24,11 @@ find_package(catkin REQUIRED
         visualization_msgs
         dynamic_reconfigure
         angles
-        )
+)
 
 generate_dynamic_reconfigure_options(
         cfg/BulletSolver.cfg
+        cfg/GimbalBase.cfg
 )
 
 ###################################

rm_gimbal_controllers/cfg/BulletSolver.cfg

@@ -14,5 +14,7 @@ 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("angle1", double_t, 0, "Min switch angle", 15.0, 0.0, 90.0)
+gen.add("angle2", double_t, 0, "Min switch angle", 15.0, 0.0, 90.0)
 
 exit(gen.generate(PACKAGE, "bullet_solver", "BulletSolver"))

rm_gimbal_controllers/cfg/GimbalBase.cfg

@@ -0,0 +1,11 @@
+#!/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, "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)
+
+exit(gen.generate(PACKAGE, "gimbal_base", "GimbalBase"))

rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h

@@ -46,13 +46,14 @@
 #include <rm_common/hardware_interface/robot_state_interface.h>
 #include <rm_common/eigen_types.h>
 #include <rm_common/ros_utilities.h>
+#include <rm_msgs/GimbalDesError.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, angle1, angle2;
 };
 
 class BulletSolver
@@ -76,13 +77,15 @@ 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_{};
   Config config_{};
@@ -92,6 +95,7 @@ class BulletSolver
   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

@@ -38,22 +38,32 @@
 #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_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
+{
+  // Input feedforward
+  double yaw_k_v_, pitch_k_v_;
+};
+
 class ChassisVel
 {
 public:
@@ -139,40 +149,42 @@ 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<control_msgs::JointControllerState>> pid_yaw_pos_state_pub_,
+      pid_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_{};
+  int loop_count_{};
 
   // Transform
-  geometry_msgs::TransformStamped odom2gimbal_des_, odom2pitch_, odom2base_, last_odom2base_;
+  geometry_msgs::TransformStamped odom2gimbal_des_, odom2pitch_, odom2base_, last_odom2base_, last_odom2gimbal_des_;
 
   // Gravity Compensation
   geometry_msgs::Vector3 mass_origin_;
   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_;
@@ -181,6 +193,11 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
   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_{};
+
   enum
   {
     RATE,

rm_gimbal_controllers/src/bullet_solver.cpp

@@ -52,7 +52,9 @@ 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.),
+              .angle1 = getParam(controller_nh, "angle1", 15.),
+              .angle2 = getParam(controller_nh, "angle2", 15.) };
   max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0);
   config_rt_buffer_.initRT(config_);
 
@@ -80,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
@@ -116,10 +119,17 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d
   double r = r1;
   double z = pos.z;
   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;
+  config_.angle1 = config_.angle1 * M_PI / 180;
+  config_.angle2 = config_.angle2 * M_PI / 180;
+  double switch_armor_angle = track_target_ ? acos(r / target_rho) - config_.angle1 +
+                                                  (-acos(r / target_rho) + config_.angle1 + config_.angle2) *
+                                                      std::abs(v_yaw) / max_track_target_vel_ :
+                                              config_.angle2;
+  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.))
   {
@@ -210,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();
@@ -316,6 +336,8 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config,
     config.delay = init_config.delay;
     config.dt = init_config.dt;
     config.timeout = init_config.timeout;
+    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 +348,9 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config,
                         .g = config.g,
                         .delay = config.delay,
                         .dt = config.dt,
-                        .timeout = config.timeout };
+                        .timeout = config.timeout,
+                        .angle1 = config.angle1,
+                        .angle2 = config.angle2 };
   config_rt_buffer_.writeFromNonRT(config_non_rt);
 }
 }  // namespace rm_gimbal_controllers