planning_scene_interface.py
Go to the documentation of this file.
00001 # Software License Agreement (BSD License)
00002 #
00003 # Copyright (c) 2008, Willow Garage, Inc.
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions
00008 # are met:
00009 #
00010 #  * Redistributions of source code must retain the above copyright
00011 #    notice, this list of conditions and the following disclaimer.
00012 #  * Redistributions in binary form must reproduce the above
00013 #    copyright notice, this list of conditions and the following
00014 #    disclaimer in the documentation and/or other materials provided
00015 #    with the distribution.
00016 #  * Neither the name of Willow Garage, Inc. nor the names of its
00017 #    contributors may be used to endorse or promote products derived
00018 #    from this software without specific prior written permission.
00019 #
00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 # POSSIBILITY OF SUCH DAMAGE.
00032 #
00033 # Author: Ioan Sucan, Felix Messmer
00034 
00035 import rospy
00036 import conversions
00037 
00038 from moveit_msgs.msg import CollisionObject, AttachedCollisionObject
00039 from moveit_ros_planning_interface import _moveit_planning_scene_interface
00040 from geometry_msgs.msg import PoseStamped, Pose, Point
00041 from shape_msgs.msg import SolidPrimitive, Plane, Mesh, MeshTriangle
00042 from exception import MoveItCommanderException
00043 
00044 try:
00045     from pyassimp import pyassimp
00046 except:
00047     # support pyassimp > 3.0
00048     import pyassimp
00049 
00050 # This is going to have more functionality; (feel free to add some!)
00051 # This class will include simple Python code for publishing messages for a planning scene
00052 
00053 class PlanningSceneInterface(object):
00054     """ Simple interface to making updates to a planning scene """
00055 
00056     def __init__(self):
00057         """ Create a planning scene interface; it uses both C++ wrapped methods and scene manipulation topics. """
00058         self._psi = _moveit_planning_scene_interface.PlanningSceneInterface()
00059 
00060         self._pub_co = rospy.Publisher('/collision_object', CollisionObject, queue_size=100)
00061         self._pub_aco = rospy.Publisher('/attached_collision_object', AttachedCollisionObject, queue_size=100)
00062 
00063     def __make_sphere(self, name, pose, radius):
00064         co = CollisionObject()
00065         co.operation = CollisionObject.ADD
00066         co.id = name
00067         co.header = pose.header
00068         sphere = SolidPrimitive()
00069         sphere.type = SolidPrimitive.SPHERE
00070         sphere.dimensions = [radius]
00071         co.primitives = [sphere]
00072         co.primitive_poses = [pose.pose]
00073         return co
00074 
00075     def add_sphere(self, name, pose, radius = 1):
00076         """
00077         Add a sphere to the planning scene 
00078         """
00079         self._pub_co.publish(self.__make_sphere(name, pose, radius))
00080 
00081     def __make_box(self, name, pose, size):
00082         co = CollisionObject()
00083         co.operation = CollisionObject.ADD
00084         co.id = name
00085         co.header = pose.header
00086         box = SolidPrimitive()
00087         box.type = SolidPrimitive.BOX
00088         box.dimensions = list(size)
00089         co.primitives = [box]
00090         co.primitive_poses = [pose.pose]
00091         return co
00092     
00093     def __make_mesh(self, name, pose, filename, scale = (1, 1, 1)):
00094         co = CollisionObject()
00095         scene = pyassimp.load(filename)
00096         if not scene.meshes or len(scene.meshes) == 0:
00097             raise MoveItCommanderException("There are no meshes in the file")
00098         if len(scene.meshes[0].faces) == 0:
00099             raise MoveItCommanderException("There are no faces in the mesh")
00100         co.operation = CollisionObject.ADD
00101         co.id = name
00102         co.header = pose.header
00103         
00104         mesh = Mesh()
00105         first_face = scene.meshes[0].faces[0]
00106         if hasattr(first_face, '__len__'):
00107             for face in scene.meshes[0].faces:
00108                 if len(face) == 3:
00109                     triangle = MeshTriangle()
00110                     triangle.vertex_indices = [face[0], face[1], face[2]]
00111                     mesh.triangles.append(triangle)
00112         elif hasattr(first_face, 'indices'):
00113             for face in scene.meshes[0].faces:
00114                 if len(face.indices) == 3:
00115                     triangle = MeshTriangle()
00116                     triangle.vertex_indices = [face.indices[0],
00117                                                face.indices[1],
00118                                                face.indices[2]]
00119                     mesh.triangles.append(triangle)
00120         else:
00121             raise MoveItCommanderException("Unable to build triangles from mesh due to mesh object structure")
00122         for vertex in scene.meshes[0].vertices:
00123             point = Point()
00124             point.x = vertex[0]*scale[0]
00125             point.y = vertex[1]*scale[1]
00126             point.z = vertex[2]*scale[2]
00127             mesh.vertices.append(point)
00128         co.meshes = [mesh]
00129         co.mesh_poses = [pose.pose]
00130         pyassimp.release(scene)
00131         return co
00132     
00133     def __make_existing(self, name):
00134         """
00135         Create an empty Collision Object, used when the object already exists 
00136         """
00137         co = CollisionObject()
00138         co.id = name
00139         return co
00140 
00141     def add_mesh(self, name, pose, filename, size = (1, 1, 1)):
00142         """
00143         Add a mesh to the planning scene
00144         """
00145         self._pub_co.publish(self.__make_mesh(name, pose, filename, size))
00146 
00147     def add_box(self, name, pose, size = (1, 1, 1)):
00148         """
00149         Add a box to the planning scene 
00150         """
00151         self._pub_co.publish(self.__make_box(name, pose, size))
00152 
00153     def add_plane(self, name, pose, normal = (0, 0, 1), offset = 0):
00154         """ Add a plane to the planning scene """
00155         co = CollisionObject()
00156         co.operation = CollisionObject.ADD
00157         co.id = name
00158         co.header = pose.header
00159         p = Plane()
00160         p.coef = list(normal)
00161         p.coef.append(offset)
00162         co.planes = [p]
00163         co.plane_poses = [pose.pose]
00164         self._pub_co.publish(co)
00165         
00166     def attach_mesh(self, link, name, pose = None, filename = '', size = (1, 1, 1), touch_links = []):
00167         aco = AttachedCollisionObject()
00168         if pose!=None and filename:
00169             aco.object = self.__make_mesh(name, pose, filename, size)
00170         else:
00171             aco.object = self.__make_existing(name)
00172         aco.link_name = link
00173         aco.touch_links = [link]
00174         if len(touch_links) > 0:
00175             aco.touch_links = touch_links
00176         self._pub_aco.publish(aco)
00177 
00178     def attach_box(self, link, name, pose = None, size = (1, 1, 1), touch_links = []):
00179         aco = AttachedCollisionObject()
00180         if pose!=None:
00181             aco.object = self.__make_box(name, pose, size)
00182         else:
00183             aco.object = self.__make_existing(name)
00184         aco.link_name = link
00185         if len(touch_links) > 0:
00186             aco.touch_links = touch_links
00187         else:
00188             aco.touch_links = [link]
00189         self._pub_aco.publish(aco)
00190 
00191     def remove_world_object(self, name = None):
00192         """
00193         Remove an object from planning scene, or all if no name is provided         
00194         """
00195         co = CollisionObject()
00196         co.operation = CollisionObject.REMOVE
00197         if name != None:
00198             co.id = name
00199         self._pub_co.publish(co)
00200 
00201     def remove_attached_object(self, link, name = None):
00202         """
00203         Remove an attached object from planning scene, or all objects attached to this link if no name is provided             
00204         """
00205         aco = AttachedCollisionObject()
00206         aco.object.operation = CollisionObject.REMOVE
00207         aco.link_name = link
00208         if name != None:
00209             aco.object.id = name
00210         self._pub_aco.publish(aco)
00211 
00212     def get_known_object_names(self, with_type = False):
00213         """
00214         Get the names of all known objects in the world. If with_type is set to true, only return objects that have a known type.
00215         """
00216         return self._psi.get_known_object_names(with_type)
00217 
00218     def get_known_object_names_in_roi(self, minx, miny, minz, maxx, maxy, maxz, with_type = False):
00219         """
00220         Get the names of known objects in the world that are located within a bounding region (specified in the frame reported by
00221         get_planning_frame()). If with_type is set to true, only return objects that have a known type.
00222         """
00223         return self._psi.get_known_object_names_in_roi(minx, miny, minz, maxx, maxy, maxz, with_type)
00224 
00225     def get_object_poses(self, object_ids):
00226         """
00227         Get the poses from the objects identified by the given object ids list.
00228         """
00229         ser_ops = self._psi.get_object_poses(object_ids)
00230         ops = dict()
00231         for key in ser_ops:
00232             msg = Pose()
00233             conversions.msg_from_string(msg, ser_ops[key])
00234             ops[key] = msg
00235         return ops 
00236 
00237     def get_objects(self, object_ids = []):
00238         """
00239         Get the objects identified by the given object ids list. If no ids are provided, return all the known objects.
00240         """
00241         ser_objs = self._psi.get_objects(object_ids)
00242         objs = dict()
00243         for key in ser_objs:
00244             msg = CollisionObject()
00245             conversions.msg_from_string(msg, ser_objs[key])
00246             objs[key] = msg
00247         return objs 
00248 
00249     def get_attached_objects(self, object_ids = []):
00250         """
00251         Get the attached objects identified by the given object ids list. If no ids are provided, return all the attached objects.
00252         """
00253         ser_aobjs = self._psi.get_attached_objects(object_ids)
00254         aobjs = dict()
00255         for key in ser_aobjs:
00256             msg = AttachedCollisionObject()
00257             conversions.msg_from_string(msg, ser_aobjs[key])
00258             aobjs[key] = msg
00259         return aobjs 


moveit_commander
Author(s): Ioan Sucan
autogenerated on Wed Jan 17 2018 03:32:53