Add ballistic solution code

rm_gimbal_controllers/CMakeLists.txt

@@ -14,9 +14,7 @@ add_definitions(-Wall -Werror)
 find_package(catkin REQUIRED
         COMPONENTS
         roscpp
-
         rm_common
-
         controller_interface
         effort_controllers
         tf2_eigen
@@ -27,6 +25,7 @@ find_package(catkin REQUIRED
         )
 
 generate_dynamic_reconfigure_options(
+        cfg/BallisticSolver.cfg
         cfg/BulletSolver.cfg
         cfg/GimbalBase.cfg
 )
@@ -46,9 +45,7 @@ catkin_package(
         LIBRARIES
         CATKIN_DEPENDS
         roscpp
-
         rm_common
-
         effort_controllers
         tf2_eigen
         tf2_geometry_msgs
@@ -68,8 +65,7 @@ include_directories(
 )
 
 ## Declare a cpp library
-add_library(${PROJECT_NAME} src/gimbal_base.cpp src/bullet_solver.cpp
-                                )
+add_library(${PROJECT_NAME} src/gimbal_base.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/BallisticSolver.cfg

@@ -0,0 +1,22 @@
+#!/usr/bin/env python
+PACKAGE = "rm_gimbal_controllers"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("mass", double_t, 0, "Bullet mass (kg)", 0.0445, 0.01, 0.1)
+gen.add("radius", double_t, 0, "Bullet radius (m)", 0.02125, 0.01, 0.05)
+gen.add("Cd_value", double_t, 0, "Fitting air drag coefficient", 0.4, 0.0, 1.0)
+gen.add("Cd_distance", double_t, 0, "Distance of fitting air drag coefficient (m)", 12.0, 0.0, 30.0)
+gen.add("Cd_slope", double_t, 0, "Fitting air drag coefficient change rate", 0.0, -10.0, 10.0)
+gen.add("air_density", double_t, 0, "Air density (kg/m³)", 1.2, 0.8, 1.5)
+gen.add("g", double_t, 0, "Gravity (m/s²)", 9.81, 9.0, 10.0)
+gen.add("initial_vel", double_t, 0, "Initial velocity (m/s)", 14.0, 10.0, 20.0)
+gen.add("rk4_simulate_step", double_t, 0, "Simulate step (s)", 0.01, 0.001, 0.05)
+gen.add("newton_convergence_tol", double_t, 0, "Newton convergence tolerance", 2e-5, 1e-6, 1e-4)
+gen.add("newton_pitch_epsilon", double_t, 0, "Finite difference epsilon", 1e-4, 1e-6, 1e-3)
+gen.add("max_newton_step", double_t, 0, "Max Newton step", 0.04, 0.01, 0.1)
+gen.add("max_newton_iterations", int_t, 0, "Max Newton iterations", 0, 1, 5)
+
+exit(gen.generate(PACKAGE, "ballistic_solver", "BallisticSolver"))

rm_gimbal_controllers/cfg/BulletSolver.cfg

@@ -19,14 +19,13 @@ gen.add("switch_angle_offset", double_t, 0, "Switch angle offset", 0.0, -20.0, 2
 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, 1, 99)
+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("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("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("move_switch_time_coff_",double_t, 0, "The offset of the traject_switch_time", 0.0 , -1.0, 1.0)
 
 exit(gen.generate(PACKAGE, "bullet_solver", "BulletSolver"))

rm_gimbal_controllers/include/rm_gimbal_controllers/ballistic_solver.h

@@ -0,0 +1,52 @@
+//
+// Created by ljyi on 25-9-20.
+//
+
+#pragma once
+
+#include <std_msgs/Bool.h>
+#include <std_msgs/Float32.h>
+#include <std_msgs/Float64.h>
+#include <realtime_tools/realtime_buffer.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <dynamic_reconfigure/server.h>
+#include <rm_gimbal_controllers/BallisticSolverConfig.h>
+#include <rm_common/linear_interpolation.h>
+#include <rm_common/ros_utilities.h>
+#include <rm_common/ori_tool.h>
+#include <rm_msgs/ShootData.h>
+#include <rm_msgs/TrackData.h>
+
+namespace rm_gimbal_controllers
+{
+struct BallisticConfig
+{
+  double mass, radius, gun_offset_x, gun_offset_z, Cd_value, Cd_distance, Cd_slope, air_density, g;
+  double initial_vel, rk4_simulate_step;
+  double newton_convergence_tol, newton_pitch_epsilon, max_newton_step;
+  int max_newton_iterations;
+};
+
+class BallisticSolver
+{
+public:
+  explicit BallisticSolver(ros::NodeHandle& nh);
+  ~BallisticSolver() = default;
+  double simulate(double pitch_angle_, double initial_vel_, double target_dis, double target_hgt);
+  void solver(const geometry_msgs::TransformStamped& base2gimbal, double& yaw, double& pitch);
+  void reconfigCB(rm_gimbal_controllers::BallisticSolverConfig& config, uint32_t);
+
+private:
+  void base2targetDataCB(const geometry_msgs::PointConstPtr& msg);
+  ros::Subscriber base2target_data_sub_;
+  dynamic_reconfigure::Server<rm_gimbal_controllers::BallisticSolverConfig>* d_srv_{};
+  BallisticConfig config_{};
+  realtime_tools::RealtimeBuffer<BallisticConfig> config_rt_buffer_;
+  rm_common::LinearInterp output_pitch_match_lut_;
+  bool dynamic_reconfig_initialized_{};
+  geometry_msgs::Point base2target_;
+  std::mutex data_mutex_;
+  std::atomic<bool> base2target_data_valid_{ false };
+};
+}  // namespace rm_gimbal_controllers

rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h

@@ -65,7 +65,7 @@ struct Config
   int max_selected_armor_;
   double traject_ahead_;
   int clean_shoot_num_;
-  double end_pos_offset, move_switch_time_coff_;
+  double end_pos_offset;
 };
 struct TrajectoryFunctionCoefficients
 {
@@ -146,30 +146,27 @@ class BulletSolver
   double last_yaw_{}, filtered_yaw_{};
   double gimbal_switch_duration_{};
   double yaw_subtract_;
-  double switch_armor_angle{};
+  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_{};
+  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 next_count_;
   int traject_count_;
   int ban_shoot_count_ = 0;
   int selected_armor_ = 0;
-  int last_selected_armor_ = 0;
 
   bool track_target_ = true;
   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 is_aheading_two_{};
+  bool using_traject_;
+  bool last_shoot_state_;
 
   geometry_msgs::Point after_traject_output_yaw_{};
   geometry_msgs::Point target_pos_{};

rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h

@@ -49,28 +49,30 @@
 #include <rm_msgs/GimbalPosState.h>
 #include <rm_gimbal_controllers/GimbalBaseConfig.h>
 #include <rm_gimbal_controllers/bullet_solver.h>
+#include <rm_gimbal_controllers/ballistic_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 <unordered_map>
+#include <nav_msgs/Odometry.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <std_msgs/Float32MultiArray.h>
 
 namespace rm_gimbal_controllers
 {
 struct GimbalConfig
 {
-  double yaw_k_v_, pitch_k_v_;
-  double chassis_comp_a_, chassis_comp_b_, chassis_comp_c_,
-      chassis_comp_d_;  // sine wave compensation, a * sin(b * chassis_angular_z + c) + d
-  double accel_pitch_{}, accel_yaw_{};
+  double yaw_k_v_{}, pitch_k_v_{}, accel_pitch_{}, accel_yaw_{};
+  double chassis_comp_a_{}, chassis_comp_b_{}, chassis_comp_c_{}, chassis_comp_d_{};
 };
 
 class ChassisVel
 {
 public:
-  ChassisVel(const ros::NodeHandle& nh)
+  explicit ChassisVel(const ros::NodeHandle& nh)
   {
     double num_data;
     nh.param("num_data", num_data, 20.0);
@@ -125,8 +127,8 @@ class ChassisVel
   }
 
 private:
-  bool is_debug_;
-  int loop_count_;
+  bool is_debug_{};
+  int loop_count_{};
   std::shared_ptr<realtime_tools::RealtimePublisher<geometry_msgs::Twist>> real_pub_{}, filtered_pub_{};
 };
 
@@ -150,10 +152,12 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
   bool setDesIntoLimit(double& angle, const urdf::JointConstSharedPtr& joint_urdf, bool update, double current_angle);
   void moveJoint(const ros::Time& time, const ros::Duration& period);
   void updateChassisVel();
-  double feedForward(const ros::Time& time);
+  void updateBallisticSolution(const ros::Time& time);
+  double gravityFeedForward(const ros::Time& time);
   double updateCompensation(double chassis_vel_angular_z);
   void commandCB(const rm_msgs::GimbalCmdConstPtr& msg);
   void trackCB(const rm_msgs::TrackDataConstPtr& msg);
+  void ballisticSolverRequestCB(const std_msgs::BoolConstPtr& msg);
   void reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uint32_t);
   std::string getGimbalFrameID(std::unordered_map<int, urdf::JointConstSharedPtr> joint_urdfs);
   std::string getBaseFrameID(std::unordered_map<int, urdf::JointConstSharedPtr> joint_urdfs);
@@ -165,17 +169,23 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
   std::unordered_map<int, urdf::JointConstSharedPtr> joint_urdfs_;
   std::unordered_map<int, bool> pos_des_in_limit_;
   bool has_imu_ = true;
+  double ballistic_yaw_{}, ballistic_pitch_{};
 
   std::shared_ptr<BulletSolver> bullet_solver_;
+  std::shared_ptr<BallisticSolver> ballistic_solver_;
 
   // ROS Interface
   ros::Time last_publish_time_{};
+  ros::Time last_ballistic_publish_time_{};
   std::unordered_map<int, std::unique_ptr<realtime_tools::RealtimePublisher<rm_msgs::GimbalPosState>>> pos_state_pub_;
   std::shared_ptr<realtime_tools::RealtimePublisher<rm_msgs::GimbalDesError>> error_pub_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::Float32MultiArray>> ballistic_solution_pub_;
   ros::Subscriber cmd_gimbal_sub_;
   ros::Subscriber data_track_sub_;
+  ros::Subscriber ballistic_solver_request_sub_;
   realtime_tools::RealtimeBuffer<rm_msgs::GimbalCmd> cmd_rt_buffer_;
   realtime_tools::RealtimeBuffer<rm_msgs::TrackData> track_rt_buffer_;
+  realtime_tools::RealtimeBuffer<std_msgs::Bool> ballistic_track_rt_buffer_;
 
   rm_msgs::GimbalCmd cmd_gimbal_;
   rm_msgs::TrackData data_track_;
@@ -185,16 +195,17 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
   int loop_count_{};
 
   // Transform
-  geometry_msgs::TransformStamped odom2gimbal_des_, odom2gimbal_, odom2base_, last_odom2base_, odom2gimbal_traject_des_;
+  geometry_msgs::TransformStamped odom2gimbal_des_, odom2gimbal_, odom2base_, last_odom2base_, base2gimbal_,
+      odom2gimbal_traject_des_;
 
   // Gravity Compensation
   geometry_msgs::Vector3 mass_origin_;
-  double gravity_;
-  bool enable_gravity_compensation_;
+  double gravity_{};
+  bool enable_gravity_compensation_{};
 
   // Chassis
   std::shared_ptr<ChassisVel> chassis_vel_;
-  double chassis_compensation_;
+  double chassis_compensation_{};
 
   bool dynamic_reconfig_initialized_{};
   GimbalConfig config_{};
@@ -210,13 +221,9 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
   };
   int state_ = RATE;
   bool start_ = false;
-
-  ros::Duration period_;
-  ros::Time time_;
   double pos_real[3]{ 0. };
   double pos_des[3]{ 0. }, vel_des[3]{ 0. }, angle_error[3]{ 0. }, traject_pos_des[3]{ 0. },
       traject_angle_error[3]{ 0. };
-  double last_acc_yaw = 0;
 };
 
 }  // namespace rm_gimbal_controllers