Add TRAJ mode for gimbal.

rm-controls · Jun 29, 2024 · 9670317 · 9670317
1 parent cb20569
commit 9670317
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 1 deletion.
diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/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;
 };

diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/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)
 {