From 833b15840db145e3efda96a47dd3413ed3acdbca Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Tue, 5 Mar 2024 10:50:45 +0100 Subject: [PATCH] [ROS] fixed bullet world reference in VizMarkerPublisher --- src/pycram/ros/viz_marker_publisher.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/pycram/ros/viz_marker_publisher.py b/src/pycram/ros/viz_marker_publisher.py index 52bfe879c..b36773f1a 100644 --- a/src/pycram/ros/viz_marker_publisher.py +++ b/src/pycram/ros/viz_marker_publisher.py @@ -18,6 +18,7 @@ class VizMarkerPublisher: """ Publishes an Array of visualization marker which represent the situation in the Bullet World """ + def __init__(self, topic_name="/pycram/viz_marker", interval=0.1): """ The Publisher creates an Array of Visualization marker with a Marker for each link of each Object in the Bullet @@ -33,8 +34,9 @@ def __init__(self, topic_name="/pycram/viz_marker", interval=0.1): self.thread = threading.Thread(target=self._publish) self.kill_event = threading.Event() - self.thread.start() + self.main_world = BulletWorld.current_bullet_world if not BulletWorld.current_bullet_world.is_shadow_world else BulletWorld.current_bullet_world.world_sync.world + self.thread.start() atexit.register(self._stop_publishing) def _publish(self) -> None: @@ -56,7 +58,7 @@ def _make_marker_array(self) -> MarkerArray: :return: An Array of Visualization Marker """ marker_array = MarkerArray() - for obj in BulletWorld.current_bullet_world.objects: + for obj in self.main_world.objects: if obj.name == "floor": continue for link in obj.links.keys(): @@ -72,7 +74,8 @@ def _make_marker_array(self) -> MarkerArray: link_pose = obj.get_link_pose(link).to_transform(link) if obj.urdf_object.link_map[link].collision.origin: link_origin = Transform(obj.urdf_object.link_map[link].collision.origin.xyz, - list(quaternion_from_euler(*obj.urdf_object.link_map[link].collision.origin.rpy))) + list(quaternion_from_euler( + *obj.urdf_object.link_map[link].collision.origin.rpy))) else: link_origin = Transform() link_pose_with_origin = link_pose * link_origin @@ -107,4 +110,3 @@ def _stop_publishing(self) -> None: """ self.kill_event.set() self.thread.join() -