Allow to attach an existing object without recreating the whole CollisionObject

src/moveit_commander/planning_scene_interface.py

@@ -105,6 +105,14 @@ def __make_mesh(self, name, pose, filename, scale = (1, 1, 1)):
         pyassimp.release(scene)
         return co
 
+    def __make_existing(self, name):
+        """
+        Create an empty Collision Object, used when the object already exists 
+        """
+        co = CollisionObject()
+        co.id = name
+        return co
+
     def add_mesh(self, name, pose, filename, size = (1, 1, 1)):
         """
         Add a mesh to the planning scene
@@ -130,41 +138,49 @@ def add_plane(self, name, pose, normal = (0, 0, 1), offset = 0):
         co.plane_poses = [pose.pose]
         self._pub_co.publish(co)
 
-    def attach_mesh(self, link, name, pose, filename, size = (1, 1, 1), touch_links = []):
+    def attach_mesh(self, link, name, pose = None, filename = '', size = (1, 1, 1), touch_links = []):
         aco = AttachedCollisionObject()
-        aco.object = self.__make_mesh(name, pose, filename, size)
+        if pose!=None and not filename.empty():
+            aco.object = self.__make_mesh(name, pose, filename, size)
+        else:
+            aco.object = self.__make_existing(name)
         aco.link_name = link
         aco.touch_links = [link]
         if len(touch_links) > 0:
             aco.touch_links = touch_links
         self._pub_aco.publish(aco)
 
-    def attach_box(self, link, name, pose, size = (1, 1, 1), touch_links = []):
+    def attach_box(self, link, name, pose = None, size = (1, 1, 1), touch_links = []):
         aco = AttachedCollisionObject()
-        aco.object = self.__make_box(name, pose, size)
+        if pose!=None:
+            aco.object = self.__make_box(name, pose, size)
+        else:
+            aco.object = self.__make_existing(name)
         aco.link_name = link
         if len(touch_links) > 0:
             aco.touch_links = touch_links
         else:
             aco.touch_links = [link]
         self._pub_aco.publish(aco)
 
-    def remove_world_object(self, name):
+    def remove_world_object(self, name = None):
         """
-        Remove object from planning scene         
+        Remove an object from planning scene, or all if no name is provided         
         """
         co = CollisionObject()
         co.operation = CollisionObject.REMOVE
-        co.id = name
+        if name != None:
+            co.id = name
         self._pub_co.publish(co)
 
-    def remove_attached_object(self, link, name = ''):
+    def remove_attached_object(self, link, name = None):
         """
-        Remove object from planning scene         
+        Remove an attached object from planning scene, or all objects attached to this link if no name is provided             
         """
         aco = AttachedCollisionObject()
         aco.object.operation = CollisionObject.REMOVE
         aco.link_name = link
-        aco.object.id = name
+        if name != None:
+            aco.object.id = name
         self._pub_aco.publish(aco)