00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
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
00048 import pyassimp
00049
00050
00051
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