From 6d43c6742be44f41bddba98c86043a46d5d095a3 Mon Sep 17 00:00:00 2001
From: 1moule <1961466188@qq.com>
Date: Sat, 27 Jul 2024 10:36:50 +0800
Subject: [PATCH] Initialize odom2gimbal_des.

---
 rm_chassis_controllers/CMakeLists.txt     |  9 ++++++---
 rm_gimbal_controllers/src/gimbal_base.cpp | 17 +++++++++++++----
 2 files changed, 19 insertions(+), 7 deletions(-)

diff --git a/rm_chassis_controllers/CMakeLists.txt b/rm_chassis_controllers/CMakeLists.txt
index 08976ce7..ec9391d0 100644
--- a/rm_chassis_controllers/CMakeLists.txt
+++ b/rm_chassis_controllers/CMakeLists.txt
@@ -19,8 +19,9 @@ find_package(catkin REQUIRED
         controller_interface
         effort_controllers
         tf2_geometry_msgs
+        nav_msgs
         angles
-        )
+)
 
 find_package(Eigen3 REQUIRED)
 
@@ -45,6 +46,8 @@ catkin_package(
 
         effort_controllers
         tf2_geometry_msgs
+        nav_msgs
+        angles
         LIBRARIES ${PROJECT_NAME}
 )
 
@@ -66,12 +69,12 @@ add_library(${PROJECT_NAME}
         src/sentry.cpp
         src/swerve.cpp
         src/balance.cpp
-        )
+)
 
 ## Specify libraries to link executable targets against
 target_link_libraries(${PROJECT_NAME}
         ${catkin_LIBRARIES}
-        )
+)
 
 #############
 ## Install ##
diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp
index 770a0dff..37a64c50 100644
--- a/rm_gimbal_controllers/src/gimbal_base.cpp
+++ b/rm_gimbal_controllers/src/gimbal_base.cpp
@@ -148,10 +148,22 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro
   return true;
 }
 
-void Controller::starting(const ros::Time& /*unused*/)
+void Controller::starting(const ros::Time& time)
 {
   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");
 }
 
 void Controller::update(const ros::Time& time, const ros::Duration& period)
@@ -251,9 +263,6 @@ void Controller::rate(const ros::Time& time, const ros::Duration& period)
   {  // on enter
     state_changed_ = false;
     ROS_INFO("[Gimbal] Enter RATE");
-    odom2gimbal_des_.transform.rotation = odom2pitch_.transform.rotation;
-    odom2gimbal_des_.header.stamp = time;
-    robot_state_handle_.setTransform(odom2gimbal_des_, "rm_gimbal_controllers");
   }
   else
   {