From 9670317e99768f89047b428a3c83fe6d6e27aafe Mon Sep 17 00:00:00 2001
From: 1moule <1961466188@qq.com>
Date: Sat, 29 Jun 2024 14:44:18 +0800
Subject: [PATCH] Add TRAJ mode for gimbal.

---
 .../include/rm_gimbal_controllers/gimbal_base.h     |  4 +++-
 rm_gimbal_controllers/src/gimbal_base.cpp           | 13 +++++++++++++
 2 files changed, 16 insertions(+), 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
index 0a166875..9d7a1a95 100644
--- 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
index 770a0dff..ba80e545 100644
--- 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)
 {