planning_scene_interface.py
Go to the documentation of this file.
1 # Copyright 2011-2021, Michael Ferguson
2 # All rights reserved.
3 #
4 # Redistribution and use in source and binary forms, with or without
5 # modification, are permitted provided that the following conditions
6 # are met:
7 #
8 # * Redistributions of source code must retain the above copyright
9 # notice, this list of conditions and the following disclaimer.
10 # * Redistributions in binary form must reproduce the above
11 # copyright notice, this list of conditions and the following
12 # disclaimer in the documentation and/or other materials provided
13 # with the distribution.
14 #
15 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
16 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
17 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
18 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
19 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
20 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
21 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
23 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
24 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
25 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
26 # POSSIBILITY OF SUCH DAMAGE.
27 
28 import threading, copy
29 import rospy
30 
31 try:
32  from pyassimp import pyassimp
33  use_pyassimp = True
34 except:
35  # support pyassimp > 3.0
36  try:
37  import pyassimp
38  use_pyassimp = True
39  except:
40  # In 16.04, pyassimp is busted
41  # https://bugs.launchpad.net/ubuntu/+source/assimp/+bug/1589949
42  use_pyassimp = False
43 
44 try:
45  basestring
46 except NameError:
47  basestring = str
48 
49 from geometry_msgs.msg import Pose, PoseStamped, Point
50 from moveit_msgs.msg import CollisionObject, AttachedCollisionObject
51 from moveit_msgs.msg import PlanningScene, PlanningSceneComponents, ObjectColor
52 from moveit_msgs.srv import GetPlanningScene, ApplyPlanningScene
53 from shape_msgs.msg import MeshTriangle, Mesh, SolidPrimitive, Plane
54 
55 
61 class PlanningSceneInterface(object):
62  def __init__(self, frame, ns='', init_from_service=True):
63  # ns must be a string
64  if not isinstance(ns, basestring):
65  rospy.logerr('Namespace must be a string!')
66  ns = ''
67  elif not ns.endswith('/'):
68  ns += '/'
69 
70  self._fixed_frame = frame
71 
72  self._scene_pub = rospy.Publisher(ns + 'planning_scene',
73  PlanningScene,
74  queue_size=10)
75  self._apply_service = rospy.ServiceProxy(ns + 'apply_planning_scene', ApplyPlanningScene)
76  # track the attached and collision objects
77  self._mutex = threading.Lock()
78  # these are updated based what the planning scene actually contains
79  self._attached = list()
80  self._collision = list()
81  # these are updated based on internal state
82  self._objects = dict()
83  self._attached_objects = dict()
84  self._removed = dict()
85  self._attached_removed = dict()
86  self._colors = dict()
87 
88  # get the initial planning scene
89  if init_from_service:
90  rospy.loginfo('Waiting for get_planning_scene')
91  rospy.wait_for_service(ns + 'get_planning_scene')
92  self._service = rospy.ServiceProxy(ns + 'get_planning_scene',
93  GetPlanningScene)
94  try:
95  req = PlanningSceneComponents()
96  req.components = sum([
97  PlanningSceneComponents.WORLD_OBJECT_NAMES,
98  PlanningSceneComponents.WORLD_OBJECT_GEOMETRY,
99  PlanningSceneComponents.ROBOT_STATE_ATTACHED_OBJECTS])
100  scene = self._service(req)
101  self.sceneCb(scene.scene, initial=True)
102  except rospy.ServiceException as e:
103  rospy.logerr('Failed to get initial planning scene, results may be wonky: %s', e)
104 
105  # subscribe to planning scene
106  rospy.Subscriber(ns + 'move_group/monitored_planning_scene',
107  PlanningScene,
108  self.sceneCb)
109 
110 
114  def sendUpdate(self, collision_object, attached_collision_object, use_service=True):
115  ps = PlanningScene()
116  ps.is_diff = True
117  ps.robot_state.is_diff = True
118  if collision_object:
119  ps.world.collision_objects.append(collision_object)
120 
121  if attached_collision_object:
122  ps.robot_state.attached_collision_objects.append(attached_collision_object)
123 
124  if use_service:
125  resp = self._apply_service.call(ps)
126  if not resp.success:
127  rospy.logerr("Could not apply planning scene diff.")
128  else:
129  self._scene_pub.publish(ps)
130 
131 
132  def clear(self):
133  for name in self.getKnownCollisionObjects():
134  self.removeCollisionObject(name, True)
135  for name in self.getKnownAttachedObjects():
136  self.removeAttachedObject(name, True)
137  self.waitForSync()
138 
139 
143  def makeMesh(self, name, pose, filename):
144  if not use_pyassimp:
145  rospy.logerr('pyassimp is broken on your platform, cannot load meshes')
146  return
147  scene = pyassimp.load(filename)
148  if not scene.meshes:
149  rospy.logerr('Unable to load mesh')
150  return
151 
152  mesh = Mesh()
153  for face in scene.meshes[0].faces:
154  triangle = MeshTriangle()
155  if hasattr(face, 'indices'):
156  if len(face.indices) == 3:
157  triangle.vertex_indices = [face.indices[0],
158  face.indices[1],
159  face.indices[2]]
160  else:
161  if len(face) == 3:
162  triangle.vertex_indices = [face[0],
163  face[1],
164  face[2]]
165  mesh.triangles.append(triangle)
166  for vertex in scene.meshes[0].vertices:
167  point = Point()
168  point.x = vertex[0]
169  point.y = vertex[1]
170  point.z = vertex[2]
171  mesh.vertices.append(point)
172  pyassimp.release(scene)
173 
174  o = CollisionObject()
175  o.header.stamp = rospy.Time.now()
176  o.header.frame_id = self._fixed_frame
177  o.id = name
178  o.meshes.append(mesh)
179  o.mesh_poses.append(pose)
180  o.operation = o.ADD
181  return o
182 
183 
188  def makeSolidPrimitive(self, name, solid, pose, frame_id=None):
189  o = CollisionObject()
190  o.header.stamp = rospy.Time.now()
191  if frame_id:
192  o.header.frame_id = frame_id
193  else:
194  o.header.frame_id = self._fixed_frame
195  o.id = name
196  o.primitives.append(solid)
197  o.primitive_poses.append(pose)
198  o.operation = o.ADD
199  return o
200 
201 
202  def makeAttached(self, link_name, obj, touch_links, detach_posture,
203  weight):
204  o = AttachedCollisionObject()
205  o.link_name = link_name
206  o.object = obj
207  if touch_links:
208  o.touch_links = touch_links
209  if detach_posture:
210  o.detach_posture = detach_posture
211  o.weight = weight
212  return o
213 
214 
219  def addMesh(self, name, pose, filename, use_service=True):
220  o = self.makeMesh(name, pose, filename)
221  self._objects[name] = o
222  self.sendUpdate(o, None, use_service)
223 
224 
229  def attachMesh(self, name, pose, filename, link_name, touch_links=None,
230  detach_posture=None, weight=0.0, use_service=True):
231  o = self.makeMesh(name, pose, filename)
232  o.header.frame_id = link_name
233  a = self.makeAttached(link_name, o, touch_links, detach_posture,
234  weight)
235  self._attached_objects[name] = a
236  self.sendUpdate(None, a, use_service)
237 
238 
244  def addSolidPrimitive(self, name, solid, pose, use_service=True, frame_id=None):
245  o = self.makeSolidPrimitive(name, solid, pose, frame_id)
246  self._objects[name] = o
247  self.sendUpdate(o, None, use_service)
248 
249 
257  def addCylinder(self, name, height, radius, x, y, z, use_service=True, frame_id=None):
258  s = SolidPrimitive()
259  s.dimensions = [height, radius]
260  s.type = s.CYLINDER
261 
262  ps = PoseStamped()
263  ps.header.frame_id = self._fixed_frame
264  ps.pose.position.x = x
265  ps.pose.position.y = y
266  ps.pose.position.z = z
267  ps.pose.orientation.w = 1.0
268 
269  self.addSolidPrimitive(name, s, ps.pose, use_service, frame_id)
270 
271 
278  def addSphere(self, name, radius, x, y, z, use_service=True, frame_id=None):
279  s = SolidPrimitive()
280  s.dimensions = [radius]
281  s.type = s.SPHERE
282 
283  ps = PoseStamped()
284  ps.header.frame_id = self._fixed_frame
285  ps.pose.position.x = x
286  ps.pose.position.y = y
287  ps.pose.position.z = z
288  ps.pose.orientation.w = 1.0
289 
290  self.addSolidPrimitive(name, s, ps.pose, use_service, frame_id)
291 
292 
300  def addCone(self, name, height, radius, x, y, z, use_service=True, frame_id=None):
301  s = SolidPrimitive()
302  s.dimensions = [height, radius]
303  s.type = s.CONE
304 
305  ps = PoseStamped()
306  ps.header.frame_id = self._fixed_frame
307  ps.pose.position.x = x
308  ps.pose.position.y = y
309  ps.pose.position.z = z
310  ps.pose.orientation.w = 1.0
311 
312  self.addSolidPrimitive(name, s, ps.pose, use_service, frame_id)
313 
314 
324  def addBox(self, name, size_x, size_y, size_z, x, y, z, use_service=True, frame_id=None):
325  s = SolidPrimitive()
326  s.dimensions = [size_x, size_y, size_z]
327  s.type = s.BOX
328 
329  ps = PoseStamped()
330  ps.header.frame_id = self._fixed_frame
331  ps.pose.position.x = x
332  ps.pose.position.y = y
333  ps.pose.position.z = z
334  ps.pose.orientation.w = 1.0
335 
336  self.addSolidPrimitive(name, s, ps.pose, use_service, frame_id)
337 
338 
349  def attachBox(self, name, size_x, size_y, size_z, x, y, z, link_name,
350  touch_links=None, detach_posture=None, weight=0.0,
351  use_service=True):
352  s = SolidPrimitive()
353  s.dimensions = [size_x, size_y, size_z]
354  s.type = s.BOX
355 
356  p = Pose()
357  p.position.x = x
358  p.position.y = y
359  p.position.z = z
360  p.orientation.w = 1.0
361  o = self.makeSolidPrimitive(name, s, p)
362  o.header.frame_id = link_name
363  a = self.makeAttached(link_name, o, touch_links, detach_posture, weight)
364  self._attached_objects[name] = a
365  self.sendUpdate(None, a, use_service)
366 
367 
370  def addCube(self, name, size, x, y, z, use_service=True, frame_id=None):
371  self.addBox(name, size, size, size, x, y, z, use_service, frame_id)
372 
373 
375  def removeCollisionObject(self, name, use_service=True):
376  """ Remove an object. """
377  o = CollisionObject()
378  o.header.stamp = rospy.Time.now()
379  o.header.frame_id = self._fixed_frame
380  o.id = name
381  o.operation = o.REMOVE
382 
383  try:
384  del self._objects[name]
385  self._removed[name] = o
386  except KeyError:
387  pass
388 
389  self.sendUpdate(o, None, use_service)
390 
391 
393  def removeAttachedObject(self, name, use_service=True):
394  """ Remove an attached object. """
395  o = AttachedCollisionObject()
396  o.object.operation = CollisionObject.REMOVE
397  o.object.id = name
398 
399  try:
400  del self._attached_objects[name]
401  self._attached_removed[name] = o
402  except KeyError:
403  pass
404 
405  self.sendUpdate(None, o, use_service)
406 
407 
408  def sceneCb(self, msg, initial=False):
409  """ Recieve updates from move_group. """
410  self._mutex.acquire()
411  for obj in msg.world.collision_objects:
412  try:
413  if obj.operation == obj.ADD:
414  self._collision.append(obj.id)
415  rospy.logdebug('ObjectManager: Added Collision Obj "%s"',
416  obj.id)
417  if initial:
418  # this is our initial planning scene, hold onto each object
419  self._objects[obj.id] = obj
420  elif obj.operation == obj.REMOVE:
421  self._collision.remove(obj.id)
422  self._removed.pop(obj.id, None)
423  rospy.logdebug('ObjectManager: Removed Collision Obj "%s"',
424  obj.id)
425  except ValueError:
426  pass
427  self._attached = list()
428  for obj in msg.robot_state.attached_collision_objects:
429  rospy.logdebug('ObjectManager: attached collision Obj "%s"',
430  obj.object.id)
431  self._attached.append(obj.object.id)
432  if initial:
433  # this is our initial planning scene, hold onto each object
434  self._attached_objects[obj.object.id] = obj
435  self._mutex.release()
436 
437 
439  self._mutex.acquire()
440  l = copy.deepcopy(self._collision)
441  self._mutex.release()
442  return l
443 
444 
446  self._mutex.acquire()
447  l = copy.deepcopy(self._attached)
448  self._mutex.release()
449  return l
450 
451 
452  def waitForSync(self, max_time=2.0):
453  sync = False
454  t = rospy.Time.now()
455  while not sync:
456  sync = True
457  # delete objects that should be gone
458  for name in self._collision:
459  if name in self._removed.keys():
460  # should be removed, is not
461  rospy.logwarn('ObjectManager: %s not removed yet', name)
462  self.removeCollisionObject(name, False)
463  sync = False
464  for name in self._attached:
465  if name in self._attached_removed.keys():
466  # should be removed, is not
467  rospy.logwarn('ObjectManager: Attached object name: %s not removed yet', name)
468  self.removeAttachedObject(name, False)
469  sync = False
470  # add missing objects
471  for name in self._objects.keys():
472  if name not in self._collision + self._attached:
473  rospy.logwarn('ObjectManager: %s not added yet', name)
474  self.sendUpdate(self._objects[name], None, False)
475  sync = False
476  for name in self._attached_objects.keys():
477  if name not in self._attached:
478  rospy.logwarn('ObjectManager: %s not attached yet', name)
479  self.sendUpdate(None, self._attached_objects[name], False)
480  sync = False
481  # timeout
482  if rospy.Time.now() - t > rospy.Duration(max_time):
483  rospy.logerr('ObjectManager: sync timed out.')
484  break
485  rospy.logdebug('ObjectManager: waiting for sync.')
486  rospy.sleep(0.1)
487 
488 
489  def setColor(self, name, r, g, b, a=0.9):
490  # Create our color
491  color = ObjectColor()
492  color.id = name
493  color.color.r = r
494  color.color.g = g
495  color.color.b = b
496  color.color.a = a
497  self._colors[name] = color
498 
499 
500  def sendColors(self):
501  # Need to send a planning scene diff
502  p = PlanningScene()
503  p.is_diff = True
504  for color in self._colors.values():
505  p.object_colors.append(color)
506  resp = self._apply_service.call(p)
507  if not resp.success:
508  rospy.logerr("Could not update colors through service, using topic instead.")
509  self._scene_pub.publish(p)
moveit_python.planning_scene_interface.PlanningSceneInterface._scene_pub
_scene_pub
Definition: planning_scene_interface.py:72
moveit_python.planning_scene_interface.PlanningSceneInterface.sendColors
def sendColors(self)
Actually send the colors to MoveIt!
Definition: planning_scene_interface.py:500
moveit_python.planning_scene_interface.PlanningSceneInterface.sceneCb
def sceneCb(self, msg, initial=False)
Update the object lists from a PlanningScene message.
Definition: planning_scene_interface.py:408
moveit_python.planning_scene_interface.PlanningSceneInterface.clear
def clear(self)
Clear the planning scene of all objects.
Definition: planning_scene_interface.py:132
moveit_python.planning_scene_interface.PlanningSceneInterface._removed
_removed
Definition: planning_scene_interface.py:84
moveit_python.planning_scene_interface.PlanningSceneInterface.__init__
def __init__(self, frame, ns='', init_from_service=True)
Definition: planning_scene_interface.py:62
msg
moveit_python.planning_scene_interface.PlanningSceneInterface.setColor
def setColor(self, name, r, g, b, a=0.9)
Set the color of an object.
Definition: planning_scene_interface.py:489
moveit_python.planning_scene_interface.PlanningSceneInterface._attached
_attached
Definition: planning_scene_interface.py:79
moveit_python.planning_scene_interface.PlanningSceneInterface
A class for managing the state of the planning scene.
Definition: planning_scene_interface.py:61
moveit_python.planning_scene_interface.PlanningSceneInterface._apply_service
_apply_service
Definition: planning_scene_interface.py:75
moveit_python.planning_scene_interface.PlanningSceneInterface._colors
_colors
Definition: planning_scene_interface.py:86
moveit_python.planning_scene_interface.PlanningSceneInterface.addCone
def addCone(self, name, height, radius, x, y, z, use_service=True, frame_id=None)
Insert new cone into planning scene.
Definition: planning_scene_interface.py:300
moveit_python.planning_scene_interface.PlanningSceneInterface.makeAttached
def makeAttached(self, link_name, obj, touch_links, detach_posture, weight)
Make an attachedCollisionObject.
Definition: planning_scene_interface.py:202
moveit_python.planning_scene_interface.PlanningSceneInterface._fixed_frame
_fixed_frame
Definition: planning_scene_interface.py:70
moveit_python.planning_scene_interface.PlanningSceneInterface.removeCollisionObject
def removeCollisionObject(self, name, use_service=True)
Send message to remove object.
Definition: planning_scene_interface.py:375
moveit_python.planning_scene_interface.PlanningSceneInterface.attachBox
def attachBox(self, name, size_x, size_y, size_z, x, y, z, link_name, touch_links=None, detach_posture=None, weight=0.0, use_service=True)
Attach a box into the planning scene.
Definition: planning_scene_interface.py:349
moveit_python.planning_scene_interface.PlanningSceneInterface.makeSolidPrimitive
def makeSolidPrimitive(self, name, solid, pose, frame_id=None)
Make a solid primitive collision object.
Definition: planning_scene_interface.py:188
moveit_python.planning_scene_interface.PlanningSceneInterface.addCube
def addCube(self, name, size, x, y, z, use_service=True, frame_id=None)
Insert new cube to planning scene.
Definition: planning_scene_interface.py:370
moveit_python.planning_scene_interface.PlanningSceneInterface._collision
_collision
Definition: planning_scene_interface.py:80
moveit_python.planning_scene_interface.PlanningSceneInterface._attached_objects
_attached_objects
Definition: planning_scene_interface.py:83
moveit_python.planning_scene_interface.PlanningSceneInterface.addCylinder
def addCylinder(self, name, height, radius, x, y, z, use_service=True, frame_id=None)
Insert new cylinder into planning scene.
Definition: planning_scene_interface.py:257
moveit_python.planning_scene_interface.PlanningSceneInterface._service
_service
Definition: planning_scene_interface.py:92
moveit_python.planning_scene_interface.PlanningSceneInterface.sendUpdate
def sendUpdate(self, collision_object, attached_collision_object, use_service=True)
Send new update to planning scene.
Definition: planning_scene_interface.py:114
moveit_python.planning_scene_interface.PlanningSceneInterface.waitForSync
def waitForSync(self, max_time=2.0)
Wait for sync.
Definition: planning_scene_interface.py:452
moveit_python.planning_scene_interface.PlanningSceneInterface.addBox
def addBox(self, name, size_x, size_y, size_z, x, y, z, use_service=True, frame_id=None)
Insert new box into planning scene.
Definition: planning_scene_interface.py:324
moveit_python.planning_scene_interface.PlanningSceneInterface.removeAttachedObject
def removeAttachedObject(self, name, use_service=True)
Send message to remove object.
Definition: planning_scene_interface.py:393
moveit_python.planning_scene_interface.PlanningSceneInterface._objects
_objects
Definition: planning_scene_interface.py:82
moveit_python.planning_scene_interface.PlanningSceneInterface.addSolidPrimitive
def addSolidPrimitive(self, name, solid, pose, use_service=True, frame_id=None)
Insert a solid primitive into planning scene.
Definition: planning_scene_interface.py:244
moveit_python.planning_scene_interface.PlanningSceneInterface._attached_removed
_attached_removed
Definition: planning_scene_interface.py:85
moveit_python.planning_scene_interface.PlanningSceneInterface._mutex
_mutex
Definition: planning_scene_interface.py:77
moveit_python.planning_scene_interface.PlanningSceneInterface.attachMesh
def attachMesh(self, name, pose, filename, link_name, touch_links=None, detach_posture=None, weight=0.0, use_service=True)
Attach a mesh into the planning scene.
Definition: planning_scene_interface.py:229
moveit_python.planning_scene_interface.PlanningSceneInterface.getKnownAttachedObjects
def getKnownAttachedObjects(self)
Get a list of names of attached objects.
Definition: planning_scene_interface.py:445
moveit_python.planning_scene_interface.PlanningSceneInterface.addMesh
def addMesh(self, name, pose, filename, use_service=True)
Insert a mesh into the planning scene.
Definition: planning_scene_interface.py:219
moveit_python.planning_scene_interface.PlanningSceneInterface.getKnownCollisionObjects
def getKnownCollisionObjects(self)
Get a list of names of collision objects.
Definition: planning_scene_interface.py:438
moveit_python.planning_scene_interface.PlanningSceneInterface.addSphere
def addSphere(self, name, radius, x, y, z, use_service=True, frame_id=None)
Insert new sphere into planning scene.
Definition: planning_scene_interface.py:278
moveit_python.planning_scene_interface.PlanningSceneInterface.makeMesh
def makeMesh(self, name, pose, filename)
Make a mesh collision object.
Definition: planning_scene_interface.py:143


moveit_python
Author(s): Michael Ferguson
autogenerated on Sun Jan 8 2023 03:16:41