Merge pull request #159 from rm-controls/revert-158-Gimbal_cascade_pid

Revert "Gimbal cascade pid"

rm_gimbal_controllers/CMakeLists.txt

@@ -24,11 +24,10 @@ 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,7 +14,5 @@ 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/include/rm_gimbal_controllers/bullet_solver.h

@@ -46,14 +46,13 @@
 #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, angle1, angle2;
+      resistance_coff_qd_30, g, delay, dt, timeout;
 };
 
 class BulletSolver
@@ -77,15 +76,13 @@ 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_{};
@@ -95,7 +92,6 @@ 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,32 +38,22 @@
 #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 <hardware_interface/imu_sensor_interface.h>
 #include <rm_common/hardware_interface/robot_state_interface.h>
-#include <rm_common/filters/filters.h>
+#include <hardware_interface/imu_sensor_interface.h>
+#include <realtime_tools/realtime_publisher.h>
 #include <rm_msgs/GimbalCmd.h>
 #include <rm_msgs/TrackData.h>
 #include <rm_msgs/GimbalDesError.h>
-#include <rm_gimbal_controllers/GimbalBaseConfig.h>
+#include <dynamic_reconfigure/server.h>
 #include <rm_gimbal_controllers/bullet_solver.h>
 #include <tf2_eigen/tf2_eigen.h>
 #include <Eigen/Eigen>
-#include <control_toolbox/pid.h>
-#include <urdf/model.h>
-#include <dynamic_reconfigure/server.h>
-#include <realtime_tools/realtime_publisher.h>
+#include <rm_common/filters/filters.h>
 
 namespace rm_gimbal_controllers
 {
-struct GimbalConfig
-{
-  // Input feedforward
-  double yaw_k_v_, pitch_k_v_;
-};
-
 class ChassisVel
 {
 public:
@@ -149,42 +139,40 @@ 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::JointVelocityController ctrl_yaw_, ctrl_pitch_;
-  control_toolbox::Pid pid_yaw_pos_, pid_pitch_pos_;
+  effort_controllers::JointPositionController ctrl_yaw_, ctrl_pitch_;
 
   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_, last_odom2gimbal_des_;
+  geometry_msgs::TransformStamped odom2gimbal_des_, odom2pitch_, odom2base_, last_odom2base_;
 
   // 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_;
@@ -193,11 +181,6 @@ 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,9 +52,7 @@ 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.),
-              .angle1 = getParam(controller_nh, "angle1", 15.),
-              .angle2 = getParam(controller_nh, "angle2", 15.) };
+              .timeout = getParam(controller_nh, "timeout", 0.) };
   max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0);
   config_rt_buffer_.initRT(config_);
 
@@ -82,7 +80,6 @@ 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
@@ -119,17 +116,10 @@ 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_;
-  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.;
+  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.))
   {
@@ -220,16 +210,6 @@ 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();
@@ -336,8 +316,6 @@ 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,
@@ -348,9 +326,7 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config,
                         .g = config.g,
                         .delay = config.delay,
                         .dt = config.dt,
-                        .timeout = config.timeout,
-                        .angle1 = config.angle1,
-                        .angle2 = config.angle2 };
+                        .timeout = config.timeout };
   config_rt_buffer_.writeFromNonRT(config_non_rt);
 }
 }  // namespace rm_gimbal_controllers