From 8de3d02299042614e6d3d75eec4a632c9f4f7387 Mon Sep 17 00:00:00 2001 From: Yoan Mollard Date: Wed, 21 Jan 2015 15:34:08 +0100 Subject: [PATCH 1/2] Allow to attash an existing object without recreating the whole CollisionObject --- .../planning_scene_interface.py | 22 +++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/src/moveit_commander/planning_scene_interface.py b/src/moveit_commander/planning_scene_interface.py index 3455690..300929b 100644 --- a/src/moveit_commander/planning_scene_interface.py +++ b/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,18 +138,24 @@ 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 From d1ad7faa8839f3cf6c8a5c94036d5f5c5151aebc Mon Sep 17 00:00:00 2001 From: Yoan Mollard Date: Thu, 22 Jan 2015 11:58:07 +0100 Subject: [PATCH 2/2] Allow to clean all objects in a row --- src/moveit_commander/planning_scene_interface.py | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/moveit_commander/planning_scene_interface.py b/src/moveit_commander/planning_scene_interface.py index 300929b..bf7cf31 100644 --- a/src/moveit_commander/planning_scene_interface.py +++ b/src/moveit_commander/planning_scene_interface.py @@ -163,22 +163,24 @@ def attach_box(self, link, name, pose = None, size = (1, 1, 1), touch_links = [] 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)