From dc4e9f46615e043b07336b5c18820fa5958e1a79 Mon Sep 17 00:00:00 2001
From: liyixin135 <421159734@qq.com>
Date: Thu, 1 Aug 2024 22:49:54 +0800
Subject: [PATCH] Initialize odom2pitch_des in rate through starting.

---
 .../rm_gimbal_controllers/gimbal_base.h       |  1 +
 rm_gimbal_controllers/src/gimbal_base.cpp     | 22 ++++++++-----------
 2 files changed, 10 insertions(+), 13 deletions(-)

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 9d7a1a95..c5a07105 100644
--- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h
+++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h
@@ -204,6 +204,7 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
     TRAJ
   };
   int state_ = RATE;
+  bool start_ = false;
 };
 
 }  // namespace rm_gimbal_controllers
diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp
index 283ce9b3..be7b8c27 100644
--- a/rm_gimbal_controllers/src/gimbal_base.cpp
+++ b/rm_gimbal_controllers/src/gimbal_base.cpp
@@ -148,22 +148,11 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro
   return true;
 }
 
-void Controller::starting(const ros::Time& time)
+void Controller::starting(const ros::Time& /*unused*/)
 {
   state_ = RATE;
   state_changed_ = true;
-  try
-  {
-    odom2pitch_ = robot_state_handle_.lookupTransform("odom", pitch_joint_urdf_->child_link_name, time);
-  }
-  catch (tf2::TransformException& ex)
-  {
-    ROS_WARN("%s", ex.what());
-    return;
-  }
-  odom2gimbal_des_.transform.rotation = odom2pitch_.transform.rotation;
-  odom2gimbal_des_.header.stamp = time;
-  robot_state_handle_.setTransform(odom2gimbal_des_, "rm_gimbal_controllers");
+  start_ = true;
 }
 
 void Controller::update(const ros::Time& time, const ros::Duration& period)
@@ -266,6 +255,13 @@ void Controller::rate(const ros::Time& time, const ros::Duration& period)
   {  // on enter
     state_changed_ = false;
     ROS_INFO("[Gimbal] Enter RATE");
+    if (start_)
+    {
+      odom2gimbal_des_.transform.rotation = odom2pitch_.transform.rotation;
+      odom2gimbal_des_.header.stamp = time;
+      robot_state_handle_.setTransform(odom2gimbal_des_, "rm_gimbal_controllers");
+      start_ = false;
+    }
   }
   else
   {