From de328e9b54f40bb4894745692ecdc280e85980c5 Mon Sep 17 00:00:00 2001 From: Vanessa Hassouna Date: Sun, 8 Dec 2024 12:51:31 +0100 Subject: [PATCH] [robotStateUpdater] making this more robust --- src/pycram/ros_utils/object_state_updater.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/pycram/ros_utils/object_state_updater.py b/src/pycram/ros_utils/object_state_updater.py index cccb72ac8..def713584 100644 --- 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: