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::MultiInterfaceControllerchild_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 {