Update gimbal and orientation code.

rm_chassis_controllers/src/chassis_base.cpp

@@ -527,4 +527,4 @@ void ChassisBase<T...>::capacityCallback(const rm_msgs::PowerManagementSampleAnd
   }
 }
 
-}  // namespace rm_chassis_controllers
+}  // namespace rm_chassis_controllers

rm_gimbal_controllers/CMakeLists.txt

@@ -14,6 +14,7 @@ add_definitions(-Wall -Werror)
 find_package(catkin REQUIRED
         COMPONENTS
         roscpp
+        std_msgs
         rm_common
         controller_interface
         effort_controllers
@@ -45,6 +46,7 @@ catkin_package(
         LIBRARIES
         CATKIN_DEPENDS
         roscpp
+        std_msgs
         rm_common
         effort_controllers
         tf2_eigen
@@ -65,7 +67,7 @@ include_directories(
 )
 
 ## Declare a cpp library
-add_library(${PROJECT_NAME} src/gimbal_base.cpp src/bullet_solver.cpp src/ballistic_solver.cpp)
+add_library(${PROJECT_NAME} src/gimbal_base.cpp src/dual_yaw_controller.cpp src/bullet_solver.cpp src/ballistic_solver.cpp)
 
 ## Specify libraries to link executable targets against
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})

rm_gimbal_controllers/cfg/BulletSolver.cfg

@@ -10,22 +10,26 @@ gen.add("resistance_coff_qd_15", double_t, 0, "Air resistance divided by mass of
 gen.add("resistance_coff_qd_16", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0)
 gen.add("resistance_coff_qd_18", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0)
 gen.add("resistance_coff_qd_30", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0)
+gen.add("resistance_coff_qd_800", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0)
 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("outpost_delay", double_t, 0, "Delay of bullet firing of outpost", 0.0, 0, 0.5)
 gen.add("center_delay", double_t, 0, "Delay of shooting this target when in center mode", 0.105, -0.5, 0.5)
 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("switch_angle_offset", double_t, 0, "Switch angle offset", 0.0, -20.0, 20)
-gen.add("switch_duration_scale",double_t, 0,"A param of gimbal switch duration", 0.0, -99.0, 99.0)
-gen.add("switch_duration_rate",double_t, 0,"A param of gimbal switch duration", 0.0, -99.0, 99.0)
-gen.add("switch_duration_offset",double_t, 0,"A param of gimbal switch duration", 0.0, -99.0, 99.0)
-gen.add("min_fit_switch_count", int_t, 0, "Min count that current angle fit switch angle", 0, 3, 99)
-gen.add("max_selected_armor_", int_t, 0, "Max selected armor number", 0, -4, 4)
+gen.add("outpost_switch_angle_offset", double_t, 0,"Switch angle offset", 0.0, -20.0, 20)
+gen.add("min_fit_switch_count", int_t, 0, "Min count that current angle fit switch angle", 0, 1, 99)
+gen.add("traject_start_fit_", int_t, 0, "The time of traject start ahead of switch", 0, 1, 99)
 gen.add("min_shoot_beforehand_vel", double_t, 0, "Min velocity to shoot beforehand", 3.0, 0.0, 20)
 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)
+gen.add("yaw_max_acc", double_t, 0, "the max acc of yaw joint" , 0.0, -400.0, 400.0)
+gen.add("track_rotate_outpost_delay", double_t, 0, "Used to estimate current armor's yaw", 0.0, -1.0, 1.0)
 gen.add("traject_ahead_",double_t, 0, "Used to switch in advance ", 0.0, -5.0, 5.0)
 gen.add("clean_shoot_num_", int_t, 0, "Used to clean shoot_num", 0, 0, 1)
 gen.add("end_pos_offset",double_t, 0, "The offset of the end pos", 0.0 , -1.0, 1.0)
+gen.add("traject_k_effort",double_t, 0, "The offset of the traject_switch_time", 0.0 , -10.0, 10.0)
+gen.add("traject_k_vel",double_t, 0, "The offset of the traject_switch_time", 0.0 , -10.0, 10.0)
 
 exit(gen.generate(PACKAGE, "bullet_solver", "BulletSolver"))

rm_gimbal_controllers/cfg/GimbalBase.cfg

@@ -13,5 +13,6 @@ gen.add("chassis_comp_a_",double_t, 0,"A param of chassis_compensation", 0.0, 0,
 gen.add("chassis_comp_b_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0)
 gen.add("chassis_comp_c_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0)
 gen.add("chassis_comp_d_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0)
+gen.add("moment_of_inertia_", double_t, 0, "Moment of inertia of gimbal", 0.0, 0, 999.0)
 
 exit(gen.generate(PACKAGE, "gimbal_base", "GimbalBase"))

rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h

@@ -57,15 +57,15 @@ 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, center_delay, max_switch_angle, switch_angle_offset, switch_duration_scale,
-      switch_duration_rate, switch_duration_offset, min_shoot_beforehand_vel, track_rotate_target_delay,
-      track_move_target_delay;
-  int min_fit_switch_count;
-  int max_selected_armor_;
+  double resistance_coff_qd_1, resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16,
+      resistance_coff_qd_18, resistance_coff_qd_30, resistance_coff_qd_800, g, delay, outpost_delay, center_delay,
+      max_switch_angle, switch_angle_offset, outpost_switch_angle_offset, min_shoot_beforehand_vel,
+      track_rotate_target_delay, track_move_target_delay, yaw_max_acc, track_rotate_outpost_delay;
+  int min_fit_switch_count, traject_start_fit_;
   double traject_ahead_;
   int clean_shoot_num_;
   double end_pos_offset;
+  double traject_k_effort, traject_k_vel_;
 };
 struct TrajectoryFunctionCoefficients
 {
@@ -82,22 +82,27 @@ 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 start_vel);
+             double r1, double r2, double dz, double armors_num, double start_vel, double track_id);
   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;
+                        double r2, double dz, double armors_num, double yaw_real, double pitch_real,
+                        double bullet_speed);
+  double getResistanceCoefficient(double target_distance) const;
   double getYaw() const
   {
-    return output_yaw_;
+    return output_yaw_[0];
   }
   double getPitch() const
   {
-    return -output_pitch_;
+    return -output_pitch_[0];
   }
   double getTrajectYaw() const
   {
     return traject_output_yaw_;
   }
+  double getTrajectVel() const
+  {
+    return traject_vel_;
+  }
   bool getUsingtraject() const
   {
     return using_traject_;
@@ -110,26 +115,32 @@ class BulletSolver
   {
     return traject_effort_ff_;
   }
-  bool getTrackTarget()
+  bool getTrackTarget() const
   {
     return track_target_;
   }
-  double getGimbalSwitchDuration(double v_yaw);
+  void CleanTrackCount()
+  {
+    track_count_ = 0;
+  }
+  double getFlyTime()
+  {
+    return fly_time_[0];
+  }
   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, double v_yaw);
+                                 double r1, double r2, double dz, double armors_num);
+  uint8_t judgeShootBeforehand(double v_yaw, int id);
   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);
-  double planningPoint(ros::Time& time, ros::Time& start_trajectory_time_);
+  double planningPoint(ros::Time& time, ros::Time& start_trajectory_time_, double v_yaw);
   void heatCB(const rm_msgs::LocalHeatStateConstPtr& msg);
   ~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_;
   std::shared_ptr<realtime_tools::RealtimePublisher<rm_msgs::BulletSolverData>> bullet_solver_pub;
   ros::Subscriber identified_target_change_sub_;
@@ -138,46 +149,56 @@ class BulletSolver
   realtime_tools::RealtimeBuffer<Config> config_rt_buffer_;
   dynamic_reconfigure::Server<rm_gimbal_controllers::BulletSolverConfig>* d_srv_{};
   Config config_{};
-  double max_track_target_vel_;
-  double output_yaw_{}, output_pitch_{}, traject_output_yaw_{};
+  double yaw_[150], pos_x[150], pos_y[150];
+  double max_track_target_vel_{};
+  double output_yaw_[150], output_pitch_[150], traject_output_yaw_{};
   double bullet_speed_{}, resistance_coff_{};
-  double fly_time_;
-  double switch_hysteresis_;
+  double fly_time_[150];
+  double switch_hysteresis_{};
   double last_yaw_{}, filtered_yaw_{};
   double gimbal_switch_duration_{};
-  double yaw_subtract_;
-  double switch_armor_angle;
+  double yaw_subtract_{};
+  double switch_armor_angle{};
   double filtered_v_yaw_{};
-  double switchtime;
-  double traject_effort_ff_;
-  double traject_switch_time_;
-  double switche_time_yaw_;
-  double traject_max_acc_;
-  int shoot_num_ = 0;
-  int shoot_beforehand_cmd_{};
-  int count_;
-  int traject_count_;
-  int ban_shoot_count_ = 0;
-  int selected_armor_ = 0;
+  double switchtime{};
+  double traject_effort_ff_{};
+  double traject_switch_time_{};
+  double switche_time_yaw_{};
+  double traject_max_acc_{};
+  double last_output_yaw_{};
+  double traject_vel_{};
+  double r_traject_{};
+  double intital_yaw_{};
 
-  bool track_target_ = true;
+  int shoot_num_{};
+  int shoot_beforehand_cmd_{};
+  int count_[150]{};
+  int next_count_[150]{};
+  int ban_shoot_count_{};
+  int selected_armor_[150] = {};
+  int last_selected_armor_ = {};
+  bool track_target_ = false;
+  int track_count_{};
   bool identified_target_change_ = true;
-  bool is_in_delay_before_switch_{};
   bool dynamic_reconfig_initialized_{};
-  bool change_armor = false;
-  bool using_traject_;
-  bool last_shoot_state_;
+  bool using_traject_{};
+  bool last_shoot_state_{};
+  bool is_aheading_two_[150]{};
+  bool start_traject_{};
+  double filtered_vel_des{};
 
   geometry_msgs::Point after_traject_output_yaw_{};
-  geometry_msgs::Point target_pos_{};
+  geometry_msgs::Point target_pos_[150]{};
   visualization_msgs::Marker marker_desire_;
   visualization_msgs::Marker marker_real_;
-  ros::Time start_using_traject_time;
-  ros::Time ban_shoot_time_;
+  ros::Time start_using_traject_time{};
+  ros::Time ban_shoot_time_{};
+  ros::Time last_output_time_{};
+  ros::Time ban_shoot_start_time_{};
 
-  TrajectoryFunctionCoefficients trajectory_function_coefficients;
-  TrajectoryLimitParams stauts_limit_;
+  TrajectoryFunctionCoefficients trajectory_function_coefficients{};
+  TrajectoryLimitParams stauts_limit_{};
 
-  mutable std::mutex heat_mutex_;
+  mutable std::mutex heat_mutex_{};
 };
 }  // namespace rm_gimbal_controllers

rm_gimbal_controllers/include/rm_gimbal_controllers/dual_yaw_controller.h

@@ -0,0 +1,38 @@
+/*******************************************************************************
+ * BSD 3-Clause License
+ ******************************************************************************/
+
+#pragma once
+
+#include <rm_gimbal_controllers/gimbal_base.h>
+
+namespace rm_gimbal_controllers
+{
+class DualYawController : public Controller
+{
+public:
+  DualYawController() = default;
+  bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override;
+
+protected:
+  bool shouldInitializeController(const std::string& name, const urdf::JointConstSharedPtr& joint_urdf,
+                                  int axis) const override;
+  bool shouldApplyJointLimit(int axis) const override;
+  std::string getBaseFrameID(const std::unordered_map<int, urdf::JointConstSharedPtr>& joint_urdfs) override;
+  void updateYawJoint(const ros::Time& time, const ros::Duration& period, const geometry_msgs::Vector3& angular_vel,
+                      const double pos_real[3], const double pos_des[3], const double pos_des_temp[3],
+                      const double vel_des[3], const double angle_error[3], const double traject_pos_des[3],
+                      const double traject_angle_error[3]) override;
+
+private:
+  bool initBaseYaw(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& controller_nh);
+  bool getTrackArmorSetPoint(const ros::Time& time, double& armor_set_point);
+  void publishBaseYawState(const ros::Time& time, double base_yaw_pos_real, double base_yaw_set_point,
+                           const double vel_des[3], double base_yaw_error);
+
+  urdf::JointConstSharedPtr base_yaw_joint_;
+  std::unique_ptr<effort_controllers::JointVelocityController> base_yaw_ctrl_;
+  std::unique_ptr<control_toolbox::Pid> base_yaw_pid_pos_;
+  std::unique_ptr<realtime_tools::RealtimePublisher<rm_msgs::GimbalPosState>> base_yaw_pos_state_pub_;
+};
+}  // namespace rm_gimbal_controllers