Merge pull request cram2#237 from sunava/robotstate-updater

[robotStateUpdater] making this more robust
sunava · Dec 9, 2024 · f5bffc2 · f5bffc2
2 parents 4045d9e + de328e9
commit f5bffc2
Showing 1 changed file with 4 additions and 1 deletion.
diff --git a/src/pycram/ros_utils/object_state_updater.py b/src/pycram/ros_utils/object_state_updater.py
@@ -32,6 +32,8 @@ def __init__(self, tf_topic: str, joint_state_topic: str):
         :param joint_state_topic: Name of the joint state topic, needs to publish sensor_msgs/JointState
         """
         self.tf_listener = tf.TransformListener()
+        self.tf_listener.clear()
+
         time.sleep(1)
         self.tf_topic = tf_topic
         self.joint_state_topic = joint_state_topic
@@ -46,7 +48,8 @@ def _subscribe_tf(self, msg: TransformStamped) -> None:
 
         :param msg: TransformStamped message published to the topic
         """
-        trans, rot = self.tf_listener.lookupTransform("/map", RobotDescription.current_robot_description.base_link, Time(0))
+        self.tf_listener.waitForTransform("map", RobotDescription.current_robot_description.base_link, Time(0.0), Duration(5))
+        trans, rot = self.tf_listener.lookupTransform("map", RobotDescription.current_robot_description.base_link, Time(0.0))
         World.robot.set_pose(Pose(trans, rot))
 
     def _subscribe_joint_state(self, msg: JointState) -> None: