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 import thread, copy
00029 import rospy
00030
00031 try:
00032 from pyassimp import pyassimp
00033 use_pyassimp = True
00034 except:
00035
00036
00037 use_pyassimp = False
00038
00039 from geometry_msgs.msg import Pose, PoseStamped, Point
00040 from moveit_msgs.msg import CollisionObject, AttachedCollisionObject
00041 from moveit_msgs.msg import PlanningScene, PlanningSceneComponents, ObjectColor
00042 from moveit_msgs.srv import GetPlanningScene, ApplyPlanningScene
00043 from shape_msgs.msg import MeshTriangle, Mesh, SolidPrimitive, Plane
00044
00045
00046
00047
00048
00049
00050
00051 class PlanningSceneInterface(object):
00052 def __init__(self, frame, ns='', init_from_service=True):
00053
00054 if not isinstance(ns, basestring):
00055 rospy.logerr('Namespace must be a string!')
00056 ns = ''
00057 elif not ns.endswith('/'):
00058 ns += '/'
00059
00060 self._fixed_frame = frame
00061
00062 self._scene_pub = rospy.Publisher(ns + 'planning_scene',
00063 PlanningScene,
00064 queue_size=10)
00065 self._apply_service = rospy.ServiceProxy(ns + 'apply_planning_scene', ApplyPlanningScene)
00066
00067 self._mutex = thread.allocate_lock()
00068
00069 self._attached = list()
00070 self._collision = list()
00071
00072 self._objects = dict()
00073 self._attached_objects = dict()
00074 self._removed = dict()
00075 self._attached_removed = dict()
00076 self._colors = dict()
00077
00078
00079 if init_from_service:
00080 rospy.loginfo('Waiting for get_planning_scene')
00081 rospy.wait_for_service(ns + 'get_planning_scene')
00082 self._service = rospy.ServiceProxy(ns + 'get_planning_scene',
00083 GetPlanningScene)
00084 try:
00085 req = PlanningSceneComponents()
00086 req.components = sum([
00087 PlanningSceneComponents.WORLD_OBJECT_NAMES,
00088 PlanningSceneComponents.WORLD_OBJECT_GEOMETRY,
00089 PlanningSceneComponents.ROBOT_STATE_ATTACHED_OBJECTS])
00090 scene = self._service(req)
00091 self.sceneCb(scene.scene, initial=True)
00092 except rospy.ServiceException as e:
00093 rospy.logerr('Failed to get initial planning scene, results may be wonky: %s', e)
00094
00095
00096 rospy.Subscriber(ns + 'move_group/monitored_planning_scene',
00097 PlanningScene,
00098 self.sceneCb)
00099
00100
00101
00102
00103
00104 def sendUpdate(self, collision_object, attached_collision_object, use_service=True):
00105 ps = PlanningScene()
00106 ps.is_diff = True
00107 if collision_object:
00108 ps.world.collision_objects.append(collision_object)
00109
00110 if attached_collision_object:
00111 ps.robot_state.attached_collision_objects.append(attached_collision_object)
00112
00113 if use_service:
00114 resp = self._apply_service.call(ps)
00115 if not resp.success:
00116 rospy.logerr("Could not apply planning scene diff.")
00117 else:
00118 self._scene_pub.publish(ps)
00119
00120
00121 def clear(self):
00122 for name in self.getKnownCollisionObjects():
00123 self.removeCollisionObject(name, True)
00124 for name in self.getKnownAttachedObjects():
00125 self.removeAttachedObject(name, True)
00126 self.waitForSync()
00127
00128
00129
00130
00131
00132 def makeMesh(self, name, pose, filename):
00133 if not use_pyassimp:
00134 rospy.logerr('pyassimp is broken on your platform, cannot load meshes')
00135 return
00136 scene = pyassimp.load(filename)
00137 if not scene.meshes:
00138 rospy.logerr('Unable to load mesh')
00139 return
00140
00141 mesh = Mesh()
00142 for face in scene.meshes[0].faces:
00143 triangle = MeshTriangle()
00144 if len(face.indices) == 3:
00145 triangle.vertex_indices = [face.indices[0],
00146 face.indices[1],
00147 face.indices[2]]
00148 mesh.triangles.append(triangle)
00149 for vertex in scene.meshes[0].vertices:
00150 point = Point()
00151 point.x = vertex[0]
00152 point.y = vertex[1]
00153 point.z = vertex[2]
00154 mesh.vertices.append(point)
00155 pyassimp.release(scene)
00156
00157 o = CollisionObject()
00158 o.header.stamp = rospy.Time.now()
00159 o.header.frame_id = self._fixed_frame
00160 o.id = name
00161 o.meshes.append(mesh)
00162 o.mesh_poses.append(pose)
00163 o.operation = o.ADD
00164 return o
00165
00166
00167
00168
00169
00170 def makeSolidPrimitive(self, name, solid, pose):
00171 o = CollisionObject()
00172 o.header.stamp = rospy.Time.now()
00173 o.header.frame_id = self._fixed_frame
00174 o.id = name
00175 o.primitives.append(solid)
00176 o.primitive_poses.append(pose)
00177 o.operation = o.ADD
00178 return o
00179
00180
00181 def makeAttached(self, link_name, obj, touch_links, detach_posture,
00182 weight):
00183 o = AttachedCollisionObject()
00184 o.link_name = link_name
00185 o.object = obj
00186 if touch_links:
00187 o.touch_links = touch_links
00188 if detach_posture:
00189 o.detach_posture = detach_posture
00190 o.weight = weight
00191 return o
00192
00193
00194
00195
00196
00197
00198 def addMesh(self, name, pose, filename, use_service=True):
00199 o = self.makeMesh(name, pose, filename)
00200 self._objects[name] = o
00201 self.sendUpdate(o, None, use_service)
00202
00203
00204
00205
00206
00207
00208 def attachMesh(self, name, pose, filename, link_name, touch_links=None,
00209 detach_posture=None, weight=0.0, use_service=True):
00210 o = self.makeMesh(name, pose, filename)
00211 o.header.frame_id = link_name
00212 a = self.makeAttached(link_name, o, touch_links, detach_posture,
00213 weight)
00214 self._attached_objects[name] = a
00215 self.sendUpdate(None, a, use_service)
00216
00217
00218
00219 def addSolidPrimitive(self, name, solid, pose, use_service=True):
00220 o = self.makeSolidPrimitive(name, solid, pose)
00221 self._objects[name] = o
00222 self.sendUpdate(o, None, use_service)
00223
00224
00225
00226 def addCylinder(self, name, height, radius, x, y, z, use_service=True):
00227 s = SolidPrimitive()
00228 s.dimensions = [height, radius]
00229 s.type = s.CYLINDER
00230
00231 ps = PoseStamped()
00232 ps.header.frame_id = self._fixed_frame
00233 ps.pose.position.x = x
00234 ps.pose.position.y = y
00235 ps.pose.position.z = z
00236 ps.pose.orientation.w = 1.0
00237
00238 self.addSolidPrimitive(name, s, ps.pose, use_service)
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249 def addBox(self, name, size_x, size_y, size_z, x, y, z, use_service=True):
00250 s = SolidPrimitive()
00251 s.dimensions = [size_x, size_y, size_z]
00252 s.type = s.BOX
00253
00254 ps = PoseStamped()
00255 ps.header.frame_id = self._fixed_frame
00256 ps.pose.position.x = x
00257 ps.pose.position.y = y
00258 ps.pose.position.z = z
00259 ps.pose.orientation.w = 1.0
00260
00261 self.addSolidPrimitive(name, s, ps.pose, use_service)
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274 def attachBox(self, name, size_x, size_y, size_z, x, y, z, link_name,
00275 touch_links=None, detach_posture=None, weight=0.0,
00276 use_service=True):
00277 s = SolidPrimitive()
00278 s.dimensions = [size_x, size_y, size_z]
00279 s.type = s.BOX
00280
00281 p = Pose()
00282 p.position.x = x
00283 p.position.y = y
00284 p.position.z = z
00285 p.orientation.w = 1.0
00286 o = self.makeSolidPrimitive(name, s, p)
00287 o.header.frame_id = link_name
00288 a = self.makeAttached(link_name, o, touch_links, detach_posture, weight)
00289 self._attached_objects[name] = a
00290 self.sendUpdate(None, a, use_service)
00291
00292
00293
00294 def addCube(self, name, size, x, y, z, use_service=True):
00295 self.addBox(name, size, size, size, x, y, z, use_service)
00296
00297
00298
00299 def removeCollisionObject(self, name, use_service=True):
00300 """ Remove an object. """
00301 o = CollisionObject()
00302 o.header.stamp = rospy.Time.now()
00303 o.header.frame_id = self._fixed_frame
00304 o.id = name
00305 o.operation = o.REMOVE
00306
00307 try:
00308 del self._objects[name]
00309 self._removed[name] = o
00310 except KeyError:
00311 pass
00312
00313 self.sendUpdate(o, None, use_service)
00314
00315
00316
00317 def removeAttachedObject(self, name, use_service=True):
00318 """ Remove an attached object. """
00319 o = AttachedCollisionObject()
00320 o.object.operation = CollisionObject.REMOVE
00321 o.object.id = name
00322
00323 try:
00324 del self._attached_objects[name]
00325 self._attached_removed[name] = o
00326 except KeyError:
00327 pass
00328
00329 self.sendUpdate(None, o, use_service)
00330
00331
00332 def sceneCb(self, msg, initial=False):
00333 """ Recieve updates from move_group. """
00334 self._mutex.acquire()
00335 for obj in msg.world.collision_objects:
00336 try:
00337 if obj.operation == obj.ADD:
00338 self._collision.append(obj.id)
00339 rospy.logdebug('ObjectManager: Added Collision Obj "%s"',
00340 obj.id)
00341 if initial:
00342
00343 self._objects[obj.id] = obj
00344 elif obj.operation == obj.REMOVE:
00345 self._collision.remove(obj.id)
00346 self._removed.pop(obj.id, None)
00347 rospy.logdebug('ObjectManager: Removed Collision Obj "%s"',
00348 obj.id)
00349 except ValueError:
00350 pass
00351 self._attached = list()
00352 for obj in msg.robot_state.attached_collision_objects:
00353 rospy.logdebug('ObjectManager: attached collision Obj "%s"',
00354 obj.object.id)
00355 self._attached.append(obj.object.id)
00356 if initial:
00357
00358 self._attached_objects[obj.object.id] = obj
00359 self._mutex.release()
00360
00361
00362 def getKnownCollisionObjects(self):
00363 self._mutex.acquire()
00364 l = copy.deepcopy(self._collision)
00365 self._mutex.release()
00366 return l
00367
00368
00369 def getKnownAttachedObjects(self):
00370 self._mutex.acquire()
00371 l = copy.deepcopy(self._attached)
00372 self._mutex.release()
00373 return l
00374
00375
00376 def waitForSync(self, max_time=2.0):
00377 sync = False
00378 t = rospy.Time.now()
00379 while not sync:
00380 sync = True
00381
00382 for name in self._collision:
00383 if name in self._removed.keys():
00384
00385 rospy.logwarn('ObjectManager: %s not removed yet', name)
00386 self.removeCollisionObject(name, False)
00387 sync = False
00388 for name in self._attached:
00389 if name in self._attached_removed.keys():
00390
00391 rospy.logwarn('ObjectManager: Attached object name: %s not removed yet', name)
00392 self.removeAttachedObject(name, False)
00393 sync = False
00394
00395 for name in self._objects.keys():
00396 if name not in self._collision + self._attached:
00397 rospy.logwarn('ObjectManager: %s not added yet', name)
00398 self.sendUpdate(self._objects[name], None, False)
00399 sync = False
00400 for name in self._attached_objects.keys():
00401 if name not in self._attached:
00402 rospy.logwarn('ObjectManager: %s not attached yet', name)
00403 self.sendUpdate(None, self._attached_objects[name], False)
00404 sync = False
00405
00406 if rospy.Time.now() - t > rospy.Duration(max_time):
00407 rospy.logerr('ObjectManager: sync timed out.')
00408 break
00409 rospy.logdebug('ObjectManager: waiting for sync.')
00410 rospy.sleep(0.1)
00411
00412
00413 def setColor(self, name, r, g, b, a=0.9):
00414
00415 color = ObjectColor()
00416 color.id = name
00417 color.color.r = r
00418 color.color.g = g
00419 color.color.b = b
00420 color.color.a = a
00421 self._colors[name] = color
00422
00423
00424 def sendColors(self):
00425
00426 p = PlanningScene()
00427 p.is_diff = True
00428 for color in self._colors.values():
00429 p.object_colors.append(color)
00430 resp = self._apply_service.call(p)
00431 if not resp.success:
00432 rospy.logerr("Could not update colors through service, using topic instead.")
00433 self._scene_pub.publish(p)