Add TRAJ mode for gimbal.

Author: 1moule

rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h

@@ -142,6 +142,7 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
   void rate(const ros::Time& time, const ros::Duration& period);
   void track(const ros::Time& time);
   void direct(const ros::Time& time);
+  void traj(const ros::Time& time);
   bool setDesIntoLimit(double& real_des, double current_des, double base2gimbal_current_des,
                        const urdf::JointConstSharedPtr& joint_urdf);
   void moveJoint(const ros::Time& time, const ros::Duration& period);
@@ -199,7 +200,8 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
   {
     RATE,
     TRACK,
-    DIRECT
+    DIRECT,
+    TRAJ
   };
   int state_ = RATE;
 };

rm_gimbal_controllers/src/gimbal_base.cpp

@@ -192,6 +192,9 @@ void Controller::update(const ros::Time& time, const ros::Duration& period)
     case DIRECT:
       direct(time);
       break;
+    case TRAJ:
+      traj(time);
+      break;
   }
   moveJoint(time, period);
 }
@@ -356,6 +359,16 @@ void Controller::direct(const ros::Time& time)
   setDes(time, yaw, pitch);
 }
 
+void Controller::traj(const ros::Time& time)
+{
+  if (state_changed_)
+  {  // on enter
+    state_changed_ = false;
+    ROS_INFO("[Gimbal] Enter TRAJ");
+  }
+  setDes(time, cmd_gimbal_.yaw_des, cmd_gimbal_.pitch_des);
+}
+
 bool Controller::setDesIntoLimit(double& real_des, double current_des, double base2gimbal_current_des,
                                  const urdf::JointConstSharedPtr& joint_urdf)
 {