Merge pull request rm-controls#175 from liyixin135/master

Initialize odom2pitch_des in rate through starting
liyixin135 · Aug 2, 2024 · eb8bb7f · eb8bb7f
2 parents 5a71357 + dc4e9f4
commit eb8bb7f
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 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
@@ -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
@@ -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
   {