World Visualization Bug Fix If Objects Removed/Attached

src/giskardpy/plugin_pybullet.py

@@ -341,6 +341,7 @@ def attach_object(self, req):
             p = transform_pose(req.pose.header.frame_id, p)
             world_object = self.unsafe_get_world().get_object(req.body.name)
             self.unsafe_get_world().attach_existing_obj_to_robot(req.body.name, req.pose.header.frame_id, p.pose)
+            self.delete_object_marker(req.body.name)
             m = world_object.as_marker_msg()
             m.header.frame_id = p.header.frame_id
             m.pose = p.pose
@@ -359,14 +360,17 @@ def attach_object(self, req):
         except:
             pass
 
-    def remove_object(self, name):
-        # assumes that parent has god map lock
+    def delete_object_marker(self, name):
         try:
             m = self.unsafe_get_world().get_object(name).as_marker_msg()
             m.action = m.DELETE
             self.publish_object_as_marker(m)
         except:
             pass
+
+    def remove_object(self, name):
+        # assumes that parent has god map lock
+        self.delete_object_marker(name)
         self.unsafe_get_world().remove_object(name)
         if name in self.object_js_subs:
             self.object_js_subs[name].unregister()

src/giskardpy/plugin_world_visualization.py

@@ -46,13 +46,11 @@ def get_id_str(self, object_name, link_name):
 
     def has_environment_changed(self):
         """
-        Checks if new objects in the world were added and if so it returns True. Otherwise, False.
+        Checks if objects in the world were added or removed and if so it returns True. Otherwise, False.
         """
-        objects_dict = self.get_world().get_objects()
-        for object_name, object in objects_dict.items():
-            if object_name not in self.currently_publishing_objects:
-                return True
-        return False
+        object_names = self.get_world().get_object_names()
+        curr_publishing_object_names = [object_name for object_name, _ in self.currently_publishing_objects.items()]
+        return object_names != curr_publishing_object_names
 
     def update(self):
         markers = []
@@ -67,19 +65,22 @@ def update(self):
         for object_name, object in objects_dict.items():
             for link_name in object.get_link_names():
                 if object.has_link_visuals(link_name):
-                    marker = object.link_as_marker(link_name)
-                    if marker is None:
+                    # Simple objects (containing only one link) are skipped, since they are already managed
+                    # in plugin_pybullet.py and as marker encoded with the function as_marker_msg from urdf_object.py
+                    if link_name == object_name and len(object.get_link_names()) == 1:
                         continue
-                    marker.header.frame_id = self.map_frame
-                    id_str = self.get_id_str(object_name, link_name)
-                    marker.id = int(hashlib.md5(id_str).hexdigest()[:6],
-                                    16)  # FIXME find a better way to give the same link the same id
-                    self.ids.add(marker.id)
-                    marker.ns = self.marker_namespace
-                    marker.header.stamp = time_stamp
-                    if link_name == object_name:
-                        marker.pose = object.base_pose
+                    # More complex objects will be published here:
                     else:
+                        marker = object.link_as_marker(link_name)
+                        if marker is None:
+                            continue
+                        marker.header.frame_id = self.map_frame
+                        id_str = self.get_id_str(object_name, link_name)
+                        marker.id = int(hashlib.md5(id_str).hexdigest()[:6],
+                                        16)  # FIXME find a better way to give the same link the same id
+                        self.ids.add(marker.id)
+                        marker.ns = self.marker_namespace
+                        marker.header.stamp = time_stamp
                         try:
                             full_link_name = self.links_full_frame_name[link_name]
                         except KeyError: