Add dual yaw controller and update gimbal_base

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/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

rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h

@@ -44,35 +44,37 @@
 #include <rm_common/hardware_interface/robot_state_interface.h>
 #include <rm_common/filters/filters.h>
 #include <rm_msgs/GimbalCmd.h>
+#include <rm_msgs/MPCGimbalAimCmd.h>
 #include <rm_msgs/TrackData.h>
 #include <rm_msgs/GimbalDesError.h>
 #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>
+#include <rm_msgs/StatusChange.h>
+#include <std_msgs/Bool.h>
+#include <std_msgs/Float64.h>
 
 namespace rm_gimbal_controllers
 {
 struct GimbalConfig
 {
   double yaw_k_v_{}, pitch_k_v_{}, accel_pitch_{}, accel_yaw_{};
-  double chassis_comp_a_{}, chassis_comp_b_{}, chassis_comp_c_{}, chassis_comp_d_{};
+  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 moment_of_inertia_{};
 };
 
 class ChassisVel
 {
 public:
-  explicit ChassisVel(const ros::NodeHandle& nh)
+  ChassisVel(const ros::NodeHandle& nh)
   {
     double num_data;
     nh.param("num_data", num_data, 20.0);
@@ -141,26 +143,46 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
   bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override;
   void starting(const ros::Time& time) override;
   void update(const ros::Time& time, const ros::Duration& period) override;
-  void setDes(const ros::Time& time, double yaw_des, double pitch_des, double traject_yaw_des, bool update_yaw = true,
-              bool update_pitch = true);
+  void setDes(const ros::Time& time, double yaw_des, double pitch_des, double traject_yaw_des = 0.,
+              bool update_yaw = true, bool update_pitch = true);
 
-private:
+protected:
+  virtual bool shouldInitializeController(const std::string& name, const urdf::JointConstSharedPtr& joint_urdf,
+                                          int axis) const;
+  virtual bool shouldApplyJointLimit(int axis) const;
+  virtual std::string getGimbalFrameID(const std::unordered_map<int, urdf::JointConstSharedPtr>& joint_urdfs);
+  virtual std::string getBaseFrameID(const std::unordered_map<int, urdf::JointConstSharedPtr>& joint_urdfs);
+  virtual void updatePitchJoint(const ros::Time& time, const ros::Duration& period,
+                                const geometry_msgs::Vector3& angular_vel, const double vel_des[3],
+                                const double angle_error[3]);
+  virtual 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]);
+  void updateGimbalYawController(const ros::Time& time, const ros::Duration& period,
+                                 const geometry_msgs::Vector3& angular_vel,
+                                 effort_controllers::JointVelocityController& ctrl, control_toolbox::Pid& pid_pos,
+                                 double k_v, const double vel_des[3], const double angle_error[3],
+                                 const double traject_angle_error[3]);
   void rate(const ros::Time& time, const ros::Duration& period);
-  void track(const ros::Time& time);
+  void track(const ros::Time& time, const ros::Duration& period);
+  void externalAimTrack(const ros::Time& time);
   void direct(const ros::Time& time);
   void traj(const ros::Time& time);
+  bool externalAimIsFresh(const ros::Time& time) const;
   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();
-  void updateBallisticSolution(const ros::Time& time);
   double gravityFeedForward(const ros::Time& time);
+  void updateChassisVel();
   double updateCompensation(double chassis_vel_angular_z);
+  void publishShootBeforehand(const ros::Time& time, uint8_t cmd);
   void commandCB(const rm_msgs::GimbalCmdConstPtr& msg);
   void trackCB(const rm_msgs::TrackDataConstPtr& msg);
-  void ballisticSolverRequestCB(const std_msgs::BoolConstPtr& msg);
+  void mpcGimbalAimCB(const rm_msgs::MPCGimbalAimCmdConstPtr& 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);
+  bool TrackSolver_typeCB(rm_msgs::StatusChangeRequest& req, rm_msgs::StatusChangeResponse& res);
+  void publishTargetIsArmor();
 
   rm_control::RobotStateHandle robot_state_handle_;
   hardware_interface::ImuSensorHandle imu_sensor_handle_;
@@ -169,34 +191,43 @@ 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_;
+  double gimbal_real_z_vel_{};
+  enum class TrackSolverType
+  {
+    BULLET_SOLVER,
+    EXTERNAL_MPC_AIM
+  };
+  TrackSolverType track_solver_type_{ TrackSolverType::BULLET_SOLVER };
+  double external_aim_timeout_{ 0.1 };
+  bool external_aim_active_{ false };
 
   // ROS Interface
   ros::Time last_publish_time_{};
-  ros::Time last_ballistic_publish_time_{};
+  ros::Time last_track_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_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<rm_msgs::ShootBeforehandCmd>> shoot_beforehand_cmd_pub_;
   ros::Subscriber cmd_gimbal_sub_;
   ros::Subscriber data_track_sub_;
-  ros::Subscriber ballistic_solver_request_sub_;
+  ros::Subscriber mpc_gimbal_aim_sub_;
+  ros::ServiceServer TrackSolver_type_srv_;
+  ros::Publisher target_is_armor_pub_;
   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_;
+  realtime_tools::RealtimeBuffer<rm_msgs::MPCGimbalAimCmd> gimbal_aim_rt_buffer_;
 
   rm_msgs::GimbalCmd cmd_gimbal_;
   rm_msgs::TrackData data_track_;
+  rm_msgs::MPCGimbalAimCmd gimbal_aim_;
   std::string gimbal_des_frame_id_{}, imu_name_{}, gimbal_traject_des_frame_id_;
   double publish_rate_{};
   bool state_changed_{};
   int loop_count_{};
 
   // Transform
-  geometry_msgs::TransformStamped odom2gimbal_des_, odom2gimbal_, odom2base_, last_odom2base_, base2gimbal_,
-      odom2gimbal_traject_des_;
+  geometry_msgs::TransformStamped odom2gimbal_des_, odom2gimbal_, odom2base_, last_odom2base_, odom2gimbal_traject_des_;
 
   // Gravity Compensation
   geometry_msgs::Vector3 mass_origin_;
@@ -221,9 +252,12 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
   };
   int state_ = RATE;
   bool start_ = false;
+
   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. };
+      traject_angle_error[3]{ 0. }, pos_des_temp[3]{ 0. };
+  double last_acc_yaw = 0;
+  bool target_is_armor_{ true };
 };
 
 }  // namespace rm_gimbal_controllers

rm_gimbal_controllers/src/dual_yaw_controller.cpp

@@ -0,0 +1,154 @@
+#include "rm_gimbal_controllers/dual_yaw_controller.h"
+
+#include <angles/angles.h>
+#include <cmath>
+#include <pluginlib/class_list_macros.hpp>
+#include <rm_common/ros_utilities.h>
+#include <rm_common/ori_tool.h>
+#include <urdf/model.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+namespace rm_gimbal_controllers
+{
+bool DualYawController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh,
+                             ros::NodeHandle& controller_nh)
+{
+  if (!initBaseYaw(robot_hw, controller_nh))
+    return false;
+  return Controller::init(robot_hw, root_nh, controller_nh);
+}
+
+bool DualYawController::initBaseYaw(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& controller_nh)
+{
+  ros::NodeHandle base_yaw_nh(controller_nh, "controllers/base_yaw");
+  ros::NodeHandle base_yaw_pid_nh(controller_nh, "controllers/base_yaw/pid_pos");
+
+  urdf::Model urdf;
+  if (!urdf.initParamWithNodeHandle("robot_description", controller_nh))
+  {
+    ROS_ERROR("Failed to parse urdf file");
+    return false;
+  }
+  base_yaw_joint_ = urdf.getJoint(getParam(base_yaw_nh, "joint", std::string("base_yaw_joint")));
+  if (!base_yaw_joint_)
+  {
+    ROS_ERROR("Could not find base_yaw joint in urdf");
+    return false;
+  }
+
+  hardware_interface::EffortJointInterface* effort_joint_interface =
+      robot_hw->get<hardware_interface::EffortJointInterface>();
+  base_yaw_ctrl_ = std::make_unique<effort_controllers::JointVelocityController>();
+  base_yaw_pid_pos_ = std::make_unique<control_toolbox::Pid>();
+  base_yaw_pos_state_pub_ =
+      std::make_unique<realtime_tools::RealtimePublisher<rm_msgs::GimbalPosState>>(base_yaw_nh, "pos_state", 1);
+  return base_yaw_ctrl_->init(effort_joint_interface, base_yaw_nh) && base_yaw_pid_pos_->init(base_yaw_pid_nh);
+}
+
+bool DualYawController::shouldInitializeController(const std::string& /*name*/,
+                                                   const urdf::JointConstSharedPtr& joint_urdf, int axis) const
+{
+  return !(axis == 2 && base_yaw_joint_ && joint_urdf->name == base_yaw_joint_->name);
+}
+
+bool DualYawController::shouldApplyJointLimit(int axis) const
+{
+  return axis != 2;
+}
+
+std::string DualYawController::getBaseFrameID(const std::unordered_map<int, urdf::JointConstSharedPtr>& joint_urdfs)
+{
+  if (base_yaw_joint_)
+    return base_yaw_joint_->parent_link_name.c_str();
+  return Controller::getBaseFrameID(joint_urdfs);
+}
+
+void DualYawController::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])
+{
+  if (pid_pos_.find(2) != pid_pos_.end() && ctrls_.find(2) != ctrls_.end())
+  {
+    updateGimbalYawController(time, period, angular_vel, *ctrls_.at(2), *pid_pos_.at(2), config_.yaw_k_v_, vel_des,
+                              angle_error, traject_angle_error);
+  }
+
+  if (!base_yaw_ctrl_ || !base_yaw_pid_pos_)
+    return;
+
+  double base_yaw_pos_real = 0.;
+  try
+  {
+    geometry_msgs::TransformStamped odom2base_yaw =
+        robot_state_handle_.lookupTransform("odom", base_yaw_joint_->child_link_name, time);
+    double roll{}, pitch{};
+    quatToRPY(odom2base_yaw.transform.rotation, roll, pitch, base_yaw_pos_real);
+  }
+  catch (tf2::TransformException& ex)
+  {
+    ROS_WARN("%s", ex.what());
+    return;
+  }
+
+  double armor_set_point{};
+  double base_yaw_set_point = getTrackArmorSetPoint(time, armor_set_point) ? armor_set_point : pos_des[2];
+  double base_yaw_error = angles::shortest_angular_distance(base_yaw_pos_real, base_yaw_set_point);
+  base_yaw_pid_pos_->computeCommand(base_yaw_error, period);
+  base_yaw_ctrl_->setCommand(base_yaw_pid_pos_->getCurrentCmd() - chassis_vel_->angular_->z());
+  base_yaw_ctrl_->update(time, period);
+  publishBaseYawState(time, base_yaw_pos_real, base_yaw_set_point, vel_des, base_yaw_error);
+}
+
+bool DualYawController::getTrackArmorSetPoint(const ros::Time& time, double& armor_set_point)
+{
+  if (state_ != TRACK || !base_yaw_joint_ || data_track_.header.stamp.isZero())
+    return false;
+
+  geometry_msgs::Point target_pos = data_track_.position;
+  if (!std::isfinite(target_pos.x) || !std::isfinite(target_pos.y) || !std::isfinite(target_pos.z))
+    return false;
+
+  try
+  {
+    geometry_msgs::TransformStamped odom2base_yaw =
+        robot_state_handle_.lookupTransform("odom", base_yaw_joint_->child_link_name, time);
+    geometry_msgs::Point base_yaw_pos_odom;
+    base_yaw_pos_odom.x = odom2base_yaw.transform.translation.x;
+    base_yaw_pos_odom.y = odom2base_yaw.transform.translation.y;
+    base_yaw_pos_odom.z = odom2base_yaw.transform.translation.z;
+
+    armor_set_point = std::atan2(target_pos.y - base_yaw_pos_odom.y, target_pos.x - base_yaw_pos_odom.x);
+    if (!std::isfinite(armor_set_point))
+      return false;
+
+    return true;
+  }
+  catch (tf2::TransformException& ex)
+  {
+    ROS_WARN("%s", ex.what());
+    return false;
+  }
+}
+
+void DualYawController::publishBaseYawState(const ros::Time& time, double base_yaw_pos_real, double base_yaw_set_point,
+                                            const double vel_des[3], double base_yaw_error)
+{
+  if (base_yaw_pos_state_pub_ && loop_count_ % 10 == 0 && base_yaw_pos_state_pub_->trylock())
+  {
+    base_yaw_pos_state_pub_->msg_.header.stamp = time;
+    base_yaw_pos_state_pub_->msg_.set_point = base_yaw_set_point;
+    base_yaw_pos_state_pub_->msg_.traject_set_point = base_yaw_set_point;
+    base_yaw_pos_state_pub_->msg_.set_point_dot = vel_des[2];
+    base_yaw_pos_state_pub_->msg_.process_value = base_yaw_pos_real;
+    base_yaw_pos_state_pub_->msg_.error = base_yaw_error;
+    base_yaw_pos_state_pub_->msg_.command = base_yaw_pid_pos_->getCurrentCmd();
+    base_yaw_pos_state_pub_->msg_.shoot_number = bullet_solver_->getShootnum();
+    base_yaw_pos_state_pub_->unlockAndPublish();
+  }
+}
+
+}  // namespace rm_gimbal_controllers
+
+PLUGINLIB_EXPORT_CLASS(rm_gimbal_controllers::DualYawController, controller_interface::ControllerBase)