planning_scene_interface.py
Go to the documentation of this file.
1 # Software License Agreement (BSD License)
2 #
3 # Copyright (c) 2008, Willow Garage, Inc.
4 # All rights reserved.
5 #
6 # Redistribution and use in source and binary forms, with or without
7 # modification, are permitted provided that the following conditions
8 # are met:
9 #
10 # * Redistributions of source code must retain the above copyright
11 # notice, this list of conditions and the following disclaimer.
12 # * Redistributions in binary form must reproduce the above
13 # copyright notice, this list of conditions and the following
14 # disclaimer in the documentation and/or other materials provided
15 # with the distribution.
16 # * Neither the name of Willow Garage, Inc. nor the names of its
17 # contributors may be used to endorse or promote products derived
18 # from this software without specific prior written permission.
19 #
20 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 # POSSIBILITY OF SUCH DAMAGE.
32 #
33 # Author: Ioan Sucan, Felix Messmer
34 
35 import rospy
36 from . import conversions
37 
38 from moveit_msgs.msg import PlanningScene, CollisionObject, AttachedCollisionObject
39 from moveit_ros_planning_interface import _moveit_planning_scene_interface
40 from geometry_msgs.msg import Pose, Point
41 from shape_msgs.msg import SolidPrimitive, Plane, Mesh, MeshTriangle
42 from .exception import MoveItCommanderException
43 from moveit_msgs.srv import ApplyPlanningScene, ApplyPlanningSceneRequest
44 
45 try:
46  from pyassimp import pyassimp
47 except:
48  # support pyassimp > 3.0
49  try:
50  import pyassimp
51  except:
52  pyassimp = False
53  print("Failed to import pyassimp, see https://github.com/ros-planning/moveit/issues/86 for more info")
54 
55 
56 class PlanningSceneInterface(object):
57  """ Simple interface to making updates to a planning scene """
58 
59  def __init__(self, ns='', synchronous=False, service_timeout=5.0):
60  """ Create a planning scene interface; it uses both C++ wrapped methods and scene manipulation topics. """
61  self._psi = _moveit_planning_scene_interface.PlanningSceneInterface(ns)
62 
63  self._pub_co = rospy.Publisher(ns + '/collision_object', CollisionObject, queue_size=100)
64  self._pub_aco = rospy.Publisher(ns + '/attached_collision_object', AttachedCollisionObject, queue_size=100)
65  self.__synchronous = synchronous
66  if self.__synchronous:
67  self._apply_planning_scene_diff = rospy.ServiceProxy(ns + '/apply_planning_scene', ApplyPlanningScene)
68  self._apply_planning_scene_diff.wait_for_service(service_timeout)
69 
70  def __submit(self, collision_object, attach=False):
71  if self.__synchronous:
72  diff_req = self.__make_planning_scene_diff_req(collision_object, attach)
73  self._apply_planning_scene_diff.call(diff_req)
74  else:
75  if attach:
76  self._pub_aco.publish(collision_object)
77  else:
78  self._pub_co.publish(collision_object)
79 
80  def add_sphere(self, name, pose, radius=1):
81  """
82  Add a sphere to the planning scene
83  """
84  co = self.__make_sphere(name, pose, radius)
85  self.__submit(co, attach=False)
86 
87  def add_cylinder(self, name, pose, height, radius):
88  """
89  Add a cylinder to the planning scene
90  """
91  co = self.__make_cylinder(name, pose, height, radius)
92  self.__submit(co, attach=False)
93 
94  def add_mesh(self, name, pose, filename, size=(1, 1, 1)):
95  """
96  Add a mesh to the planning scene
97  """
98  co = self.__make_mesh(name, pose, filename, size)
99  self.__submit(co, attach=False)
100 
101  def add_box(self, name, pose, size=(1, 1, 1)):
102  """
103  Add a box to the planning scene
104  """
105  co = self.__make_box(name, pose, size)
106  self.__submit(co, attach=False)
107 
108  def add_plane(self, name, pose, normal=(0, 0, 1), offset=0):
109  """ Add a plane to the planning scene """
110  co = CollisionObject()
111  co.operation = CollisionObject.ADD
112  co.id = name
113  co.header = pose.header
114  p = Plane()
115  p.coef = list(normal)
116  p.coef.append(offset)
117  co.planes = [p]
118  co.plane_poses = [pose.pose]
119  self.__submit(co, attach=False)
120 
121  def attach_mesh(self, link, name, pose=None, filename='', size=(1, 1, 1), touch_links=[]):
122  aco = AttachedCollisionObject()
123  if (pose is not None) and filename:
124  aco.object = self.__make_mesh(name, pose, filename, size)
125  else:
126  aco.object = self.__make_existing(name)
127  aco.link_name = link
128  aco.touch_links = [link]
129  if len(touch_links) > 0:
130  aco.touch_links = touch_links
131  self.__submit(aco, attach=True)
132 
133  def attach_box(self, link, name, pose=None, size=(1, 1, 1), touch_links=[]):
134  aco = AttachedCollisionObject()
135  if pose is not None:
136  aco.object = self.__make_box(name, pose, size)
137  else:
138  aco.object = self.__make_existing(name)
139  aco.link_name = link
140  if len(touch_links) > 0:
141  aco.touch_links = touch_links
142  else:
143  aco.touch_links = [link]
144  self.__submit(aco, attach=True)
145 
146  def remove_world_object(self, name=None):
147  """
148  Remove an object from planning scene, or all if no name is provided
149  """
150  co = CollisionObject()
151  co.operation = CollisionObject.REMOVE
152  if name is not None:
153  co.id = name
154  self.__submit(co, attach=False)
155 
156  def remove_attached_object(self, link, name=None):
157  """
158  Remove an attached object from planning scene, or all objects attached to this link if no name is provided
159  """
160  aco = AttachedCollisionObject()
161  aco.object.operation = CollisionObject.REMOVE
162  aco.link_name = link
163  if name is not None:
164  aco.object.id = name
165  self.__submit(aco, attach=True)
166 
167  def get_known_object_names(self, with_type=False):
168  """
169  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.
170  """
171  return self._psi.get_known_object_names(with_type)
172 
173  def get_known_object_names_in_roi(self, minx, miny, minz, maxx, maxy, maxz, with_type=False):
174  """
175  Get the names of known objects in the world that are located within a bounding region (specified in the frame reported by
176  get_planning_frame()). If with_type is set to true, only return objects that have a known type.
177  """
178  return self._psi.get_known_object_names_in_roi(minx, miny, minz, maxx, maxy, maxz, with_type)
179 
180  def get_object_poses(self, object_ids):
181  """
182  Get the poses from the objects identified by the given object ids list.
183  """
184  ser_ops = self._psi.get_object_poses(object_ids)
185  ops = dict()
186  for key in ser_ops:
187  msg = Pose()
188  conversions.msg_from_string(msg, ser_ops[key])
189  ops[key] = msg
190  return ops
191 
192  def get_objects(self, object_ids=[]):
193  """
194  Get the objects identified by the given object ids list. If no ids are provided, return all the known objects.
195  """
196  ser_objs = self._psi.get_objects(object_ids)
197  objs = dict()
198  for key in ser_objs:
199  msg = CollisionObject()
200  conversions.msg_from_string(msg, ser_objs[key])
201  objs[key] = msg
202  return objs
203 
204  def get_attached_objects(self, object_ids=[]):
205  """
206  Get the attached objects identified by the given object ids list. If no ids are provided, return all the attached objects.
207  """
208  ser_aobjs = self._psi.get_attached_objects(object_ids)
209  aobjs = dict()
210  for key in ser_aobjs:
211  msg = AttachedCollisionObject()
212  conversions.msg_from_string(msg, ser_aobjs[key])
213  aobjs[key] = msg
214  return aobjs
215 
216  @staticmethod
217  def __make_existing(name):
218  """
219  Create an empty Collision Object, used when the object already exists
220  """
221  co = CollisionObject()
222  co.id = name
223  return co
224 
225  @staticmethod
226  def __make_box(name, pose, size):
227  co = CollisionObject()
228  co.operation = CollisionObject.ADD
229  co.id = name
230  co.header = pose.header
231  box = SolidPrimitive()
232  box.type = SolidPrimitive.BOX
233  box.dimensions = list(size)
234  co.primitives = [box]
235  co.primitive_poses = [pose.pose]
236  return co
237 
238  @staticmethod
239  def __make_mesh(name, pose, filename, scale=(1, 1, 1)):
240  co = CollisionObject()
241  if pyassimp is False:
243  "Pyassimp needs patch https://launchpadlibrarian.net/319496602/patchPyassim.txt")
244  scene = pyassimp.load(filename)
245  if not scene.meshes or len(scene.meshes) == 0:
246  raise MoveItCommanderException("There are no meshes in the file")
247  if len(scene.meshes[0].faces) == 0:
248  raise MoveItCommanderException("There are no faces in the mesh")
249  co.operation = CollisionObject.ADD
250  co.id = name
251  co.header = pose.header
252 
253  mesh = Mesh()
254  first_face = scene.meshes[0].faces[0]
255  if hasattr(first_face, '__len__'):
256  for face in scene.meshes[0].faces:
257  if len(face) == 3:
258  triangle = MeshTriangle()
259  triangle.vertex_indices = [face[0], face[1], face[2]]
260  mesh.triangles.append(triangle)
261  elif hasattr(first_face, 'indices'):
262  for face in scene.meshes[0].faces:
263  if len(face.indices) == 3:
264  triangle = MeshTriangle()
265  triangle.vertex_indices = [face.indices[0],
266  face.indices[1],
267  face.indices[2]]
268  mesh.triangles.append(triangle)
269  else:
270  raise MoveItCommanderException("Unable to build triangles from mesh due to mesh object structure")
271  for vertex in scene.meshes[0].vertices:
272  point = Point()
273  point.x = vertex[0] * scale[0]
274  point.y = vertex[1] * scale[1]
275  point.z = vertex[2] * scale[2]
276  mesh.vertices.append(point)
277  co.meshes = [mesh]
278  co.mesh_poses = [pose.pose]
279  pyassimp.release(scene)
280  return co
281 
282  @staticmethod
283  def __make_sphere(name, pose, radius):
284  co = CollisionObject()
285  co.operation = CollisionObject.ADD
286  co.id = name
287  co.header = pose.header
288  sphere = SolidPrimitive()
289  sphere.type = SolidPrimitive.SPHERE
290  sphere.dimensions = [radius]
291  co.primitives = [sphere]
292  co.primitive_poses = [pose.pose]
293  return co
294 
295  @staticmethod
296  def __make_cylinder(name, pose, height, radius):
297  co = CollisionObject()
298  co.operation = CollisionObject.ADD
299  co.id = name
300  co.header = pose.header
301  cylinder = SolidPrimitive()
302  cylinder.type = SolidPrimitive.CYLINDER
303  cylinder.dimensions = [height, radius]
304  co.primitives = [cylinder]
305  co.primitive_poses = [pose.pose]
306  return co
307 
308  @staticmethod
309  def __make_planning_scene_diff_req(collision_object, attach=False):
310  scene = PlanningScene()
311  scene.is_diff = True
312  scene.robot_state.is_diff = True
313  if attach:
314  scene.robot_state.attached_collision_objects = [collision_object]
315  else:
316  scene.world.collision_objects = [collision_object]
317  planning_scene_diff_req = ApplyPlanningSceneRequest()
318  planning_scene_diff_req.scene = scene
319  return planning_scene_diff_req
def attach_mesh(self, link, name, pose=None, filename='', size=(1, 1, 1), touch_links=[])
def get_known_object_names_in_roi(self, minx, miny, minz, maxx, maxy, maxz, with_type=False)
def add_plane(self, name, pose, normal=(0, 0, 1), offset=0)
def __init__(self, ns='', synchronous=False, service_timeout=5.0)
def attach_box(self, link, name, pose=None, size=(1, 1, 1), touch_links=[])


moveit_commander
Author(s): Ioan Sucan
autogenerated on Sat Jul 11 2020 03:52:32