Merge pull request #157 from ye-luo-xi-tui/dev

Adopt windmill kinematics model

rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h → rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver/bullet_solver.h

@@ -46,6 +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_gimbal_controllers/bullet_solver/target_kinematics_model.h"
 
 namespace rm_gimbal_controllers
 {
@@ -55,16 +56,6 @@ struct Config
       resistance_coff_qd_30, g, delay, dt, timeout;
 };
 
-struct TargetState
-{
-  geometry_msgs::Point current_target_center_pos;
-  geometry_msgs::Vector3 current_target_center_vel;
-  double yaw;
-  double v_yaw;
-  double r;
-  int armors_num;
-};
-
 enum class SelectedArmor
 {
   FRONT = 0,
@@ -77,9 +68,12 @@ class BulletSolver
 {
 public:
   explicit BulletSolver(ros::NodeHandle& controller_nh);
-
-  bool solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, double v_yaw,
+  // normal target(including robots and buildings)
+  void input(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);
+  // windmill
+  void input(double theta, double theta_dot, double bullet_speed, geometry_msgs::TransformStamped windmill2odom);
+  bool solve();
   double getGimbalError(double yaw_real, double pitch_real);
   double getResistanceCoefficient(double bullet_speed) const;
   double getYaw() const
@@ -108,13 +102,12 @@ class BulletSolver
   double last_pitch_vel_des_{};
   ros::Time last_pitch_vel_des_solve_time_{ 0 };
   double bullet_speed_{}, resistance_coff_{};
+  double windmill_radius_;
   SelectedArmor selected_armor_;
   bool track_target_;
 
+  std::shared_ptr<TargetKinematicsBase> target_kinematics_;
   geometry_msgs::Point target_pos_{};
-  geometry_msgs::Vector3 target_vel_{};
-  geometry_msgs::Vector3 target_accel_{};
-  TargetState target_state_{};
   double fly_time_;
   visualization_msgs::Marker marker_desire_;
   visualization_msgs::Marker marker_real_;

rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver/target_kinematics_model.h

@@ -0,0 +1,143 @@
+//
+// Created by yezi on 24-1-26.
+//
+
+#pragma once
+
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf/transform_datatypes.h>
+
+class TargetKinematicsBase
+{
+public:
+  virtual ~TargetKinematicsBase() = default;
+  virtual geometry_msgs::Point position(double time) = 0;
+  virtual geometry_msgs::Vector3 velocity(double time) = 0;
+  virtual geometry_msgs::Vector3 acceleration(double time) = 0;
+};
+
+class NormalTargetKinematics : public TargetKinematicsBase
+{
+public:
+  NormalTargetKinematics(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r)
+  {
+    pos_ = pos;
+    vel_ = vel;
+    yaw_ = yaw;
+    v_yaw_ = v_yaw;
+    r_ = r;
+  }
+
+protected:
+  geometry_msgs::Point pos_;
+  geometry_msgs::Vector3 vel_;
+  double yaw_;
+  double v_yaw_;
+  double r_;
+};
+
+class TrackedTargetKinematics : public NormalTargetKinematics
+{
+public:
+  TrackedTargetKinematics(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r)
+    : NormalTargetKinematics(pos, vel, yaw, v_yaw, r)
+  {
+  }
+  geometry_msgs::Point position(double time) override
+  {
+    geometry_msgs::Point target_pos;
+    target_pos.x = pos_.x + vel_.x * time - r_ * cos(yaw_ + v_yaw_ * time);
+    target_pos.y = pos_.y + vel_.y * time - r_ * sin(yaw_ + v_yaw_ * time);
+    target_pos.z = pos_.z + vel_.z * time;
+    return target_pos;
+  }
+  geometry_msgs::Vector3 velocity(double time) override
+  {
+    geometry_msgs::Vector3 target_vel;
+    target_vel.x = vel_.x + v_yaw_ * r_ * sin(yaw_ + v_yaw_ * time);
+    target_vel.y = vel_.y - v_yaw_ * r_ * cos(yaw_ + v_yaw_ * time);
+    return target_vel;
+  }
+  geometry_msgs::Vector3 acceleration(double time) override
+  {
+    geometry_msgs::Vector3 target_accel;
+    target_accel.x = pow(v_yaw_, 2) * r_ * cos(yaw_ + v_yaw_ * time);
+    target_accel.y = pow(v_yaw_, 2) * r_ * sin(yaw_ + v_yaw_ * time);
+    return target_accel;
+  }
+};
+
+class UntrackedTargetKinematic : public NormalTargetKinematics
+{
+public:
+  UntrackedTargetKinematic(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r)
+    : NormalTargetKinematics(pos, vel, yaw, v_yaw, r)
+  {
+  }
+  geometry_msgs::Point position(double time) override
+  {
+    geometry_msgs::Point target_pos;
+    double target_center_pos[2];
+    target_center_pos[0] = pos_.x + vel_.x * time;
+    target_center_pos[1] = pos_.y + vel_.y * time;
+    target_pos.x = target_center_pos[0] - r_ * cos(atan2(target_center_pos[1], target_center_pos[0]));
+    target_pos.y = target_center_pos[1] - r_ * sin(atan2(target_center_pos[1], target_center_pos[0]));
+    target_pos.z = pos_.z + vel_.z * time;
+    return target_pos;
+  }
+  geometry_msgs::Vector3 velocity(double time) override
+  {
+    geometry_msgs::Vector3 target_vel;
+    return target_vel;
+  }
+  geometry_msgs::Vector3 acceleration(double time) override
+  {
+    geometry_msgs::Vector3 target_accel;
+    return target_accel;
+  }
+};
+
+class WindmillKinematics : public TargetKinematicsBase
+{
+public:
+  WindmillKinematics(double theta, double theta_dot, double radius, geometry_msgs::TransformStamped windmill2odom)
+  {
+    theta_ = theta;
+    theta_dot_ = theta_dot;
+    radius_ = radius;
+    windmill2odom_ = windmill2odom;
+  }
+  geometry_msgs::Point position(double time) override
+  {
+    geometry_msgs::Point target_pos;
+    target_pos.x = 0.;
+    target_pos.y = -radius_ * sin(theta_ + theta_dot_ * time);
+    target_pos.z = radius_ * cos(theta_ + theta_dot_ * time);
+    tf2::doTransform(target_pos, target_pos, windmill2odom_);
+    return target_pos;
+  }
+  geometry_msgs::Vector3 velocity(double time) override
+  {
+    geometry_msgs::Vector3 target_vel;
+    target_vel.x = 0.;
+    target_vel.y = -theta_dot_ * radius_ * cos(theta_ + theta_dot_ * time);
+    target_vel.z = -theta_dot_ * radius_ * sin(theta_ + theta_dot_ * time);
+    tf2::doTransform(target_vel, target_vel, windmill2odom_);
+    return target_vel;
+  }
+  geometry_msgs::Vector3 acceleration(double time) override
+  {
+    geometry_msgs::Vector3 target_accel;
+    target_accel.x = 0.;
+    target_accel.y = pow(theta_dot_, 2) * radius_ * sin(theta_ + theta_dot_ * time);
+    target_accel.z = -pow(theta_dot_, 2) * radius_ * cos(theta_ + theta_dot_ * time);
+    tf2::doTransform(target_accel, target_accel, windmill2odom_);
+    return target_accel;
+  }
+
+private:
+  double theta_;
+  double theta_dot_;
+  double radius_;
+  geometry_msgs::TransformStamped windmill2odom_;
+};

rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h

@@ -47,7 +47,7 @@
 #include <rm_msgs/TrackData.h>
 #include <rm_msgs/GimbalDesError.h>
 #include <dynamic_reconfigure/server.h>
-#include <rm_gimbal_controllers/bullet_solver.h>
+#include "rm_gimbal_controllers/bullet_solver/bullet_solver.h"
 #include <tf2_eigen/tf2_eigen.h>
 #include <Eigen/Eigen>
 #include <rm_common/filters/filters.h>