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 import conversions
37 
38 from moveit_msgs.msg import CollisionObject, AttachedCollisionObject
39 from moveit_ros_planning_interface import _moveit_planning_scene_interface
40 from geometry_msgs.msg import PoseStamped, Pose, Point
41 from shape_msgs.msg import SolidPrimitive, Plane, Mesh, MeshTriangle
42 from exception import MoveItCommanderException
43 
44 try:
45  from pyassimp import pyassimp
46 except:
47  # support pyassimp > 3.0
48  try:
49  import pyassimp
50  except:
51  pyassimp = False
52  print("Failed to import pyassimp, see https://github.com/ros-planning/moveit/issues/86 for more info")
53 
54 # This is going to have more functionality; (feel free to add some!)
55 # This class will include simple Python code for publishing messages for a planning scene
56 
57 class PlanningSceneInterface(object):
58  """ Simple interface to making updates to a planning scene """
59 
60  def __init__(self):
61  """ Create a planning scene interface; it uses both C++ wrapped methods and scene manipulation topics. """
62  self._psi = _moveit_planning_scene_interface.PlanningSceneInterface()
63 
64  self._pub_co = rospy.Publisher('/collision_object', CollisionObject, queue_size=100)
65  self._pub_aco = rospy.Publisher('/attached_collision_object', AttachedCollisionObject, queue_size=100)
66 
67  def __make_sphere(self, name, pose, radius):
68  co = CollisionObject()
69  co.operation = CollisionObject.ADD
70  co.id = name
71  co.header = pose.header
72  sphere = SolidPrimitive()
73  sphere.type = SolidPrimitive.SPHERE
74  sphere.dimensions = [radius]
75  co.primitives = [sphere]
76  co.primitive_poses = [pose.pose]
77  return co
78 
79  def add_sphere(self, name, pose, radius = 1):
80  """
81  Add a sphere to the planning scene
82  """
83  self._pub_co.publish(self.__make_sphere(name, pose, radius))
84 
85  def __make_box(self, name, pose, size):
86  co = CollisionObject()
87  co.operation = CollisionObject.ADD
88  co.id = name
89  co.header = pose.header
90  box = SolidPrimitive()
91  box.type = SolidPrimitive.BOX
92  box.dimensions = list(size)
93  co.primitives = [box]
94  co.primitive_poses = [pose.pose]
95  return co
96 
97  def __make_mesh(self, name, pose, filename, scale = (1, 1, 1)):
98  co = CollisionObject()
99  if pyassimp is False:
100  raise MoveItCommanderException("Pyassimp needs patch https://launchpadlibrarian.net/319496602/patchPyassim.txt")
101  scene = pyassimp.load(filename)
102  if not scene.meshes or len(scene.meshes) == 0:
103  raise MoveItCommanderException("There are no meshes in the file")
104  if len(scene.meshes[0].faces) == 0:
105  raise MoveItCommanderException("There are no faces in the mesh")
106  co.operation = CollisionObject.ADD
107  co.id = name
108  co.header = pose.header
109 
110  mesh = Mesh()
111  first_face = scene.meshes[0].faces[0]
112  if hasattr(first_face, '__len__'):
113  for face in scene.meshes[0].faces:
114  if len(face) == 3:
115  triangle = MeshTriangle()
116  triangle.vertex_indices = [face[0], face[1], face[2]]
117  mesh.triangles.append(triangle)
118  elif hasattr(first_face, 'indices'):
119  for face in scene.meshes[0].faces:
120  if len(face.indices) == 3:
121  triangle = MeshTriangle()
122  triangle.vertex_indices = [face.indices[0],
123  face.indices[1],
124  face.indices[2]]
125  mesh.triangles.append(triangle)
126  else:
127  raise MoveItCommanderException("Unable to build triangles from mesh due to mesh object structure")
128  for vertex in scene.meshes[0].vertices:
129  point = Point()
130  point.x = vertex[0]*scale[0]
131  point.y = vertex[1]*scale[1]
132  point.z = vertex[2]*scale[2]
133  mesh.vertices.append(point)
134  co.meshes = [mesh]
135  co.mesh_poses = [pose.pose]
136  pyassimp.release(scene)
137  return co
138 
139  def __make_existing(self, name):
140  """
141  Create an empty Collision Object, used when the object already exists
142  """
143  co = CollisionObject()
144  co.id = name
145  return co
146 
147  def add_mesh(self, name, pose, filename, size = (1, 1, 1)):
148  """
149  Add a mesh to the planning scene
150  """
151  self._pub_co.publish(self.__make_mesh(name, pose, filename, size))
152 
153  def add_box(self, name, pose, size = (1, 1, 1)):
154  """
155  Add a box to the planning scene
156  """
157  self._pub_co.publish(self.__make_box(name, pose, size))
158 
159  def add_plane(self, name, pose, normal = (0, 0, 1), offset = 0):
160  """ Add a plane to the planning scene """
161  co = CollisionObject()
162  co.operation = CollisionObject.ADD
163  co.id = name
164  co.header = pose.header
165  p = Plane()
166  p.coef = list(normal)
167  p.coef.append(offset)
168  co.planes = [p]
169  co.plane_poses = [pose.pose]
170  self._pub_co.publish(co)
171 
172  def attach_mesh(self, link, name, pose = None, filename = '', size = (1, 1, 1), touch_links = []):
173  aco = AttachedCollisionObject()
174  if pose!=None and filename:
175  aco.object = self.__make_mesh(name, pose, filename, size)
176  else:
177  aco.object = self.__make_existing(name)
178  aco.link_name = link
179  aco.touch_links = [link]
180  if len(touch_links) > 0:
181  aco.touch_links = touch_links
182  self._pub_aco.publish(aco)
183 
184  def attach_box(self, link, name, pose = None, size = (1, 1, 1), touch_links = []):
185  aco = AttachedCollisionObject()
186  if pose!=None:
187  aco.object = self.__make_box(name, pose, size)
188  else:
189  aco.object = self.__make_existing(name)
190  aco.link_name = link
191  if len(touch_links) > 0:
192  aco.touch_links = touch_links
193  else:
194  aco.touch_links = [link]
195  self._pub_aco.publish(aco)
196 
197  def remove_world_object(self, name = None):
198  """
199  Remove an object from planning scene, or all if no name is provided
200  """
201  co = CollisionObject()
202  co.operation = CollisionObject.REMOVE
203  if name != None:
204  co.id = name
205  self._pub_co.publish(co)
206 
207  def remove_attached_object(self, link, name = None):
208  """
209  Remove an attached object from planning scene, or all objects attached to this link if no name is provided
210  """
211  aco = AttachedCollisionObject()
212  aco.object.operation = CollisionObject.REMOVE
213  aco.link_name = link
214  if name != None:
215  aco.object.id = name
216  self._pub_aco.publish(aco)
217 
218  def get_known_object_names(self, with_type = False):
219  """
220  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.
221  """
222  return self._psi.get_known_object_names(with_type)
223 
224  def get_known_object_names_in_roi(self, minx, miny, minz, maxx, maxy, maxz, with_type = False):
225  """
226  Get the names of known objects in the world that are located within a bounding region (specified in the frame reported by
227  get_planning_frame()). If with_type is set to true, only return objects that have a known type.
228  """
229  return self._psi.get_known_object_names_in_roi(minx, miny, minz, maxx, maxy, maxz, with_type)
230 
231  def get_object_poses(self, object_ids):
232  """
233  Get the poses from the objects identified by the given object ids list.
234  """
235  ser_ops = self._psi.get_object_poses(object_ids)
236  ops = dict()
237  for key in ser_ops:
238  msg = Pose()
239  conversions.msg_from_string(msg, ser_ops[key])
240  ops[key] = msg
241  return ops
242 
243  def get_objects(self, object_ids = []):
244  """
245  Get the objects identified by the given object ids list. If no ids are provided, return all the known objects.
246  """
247  ser_objs = self._psi.get_objects(object_ids)
248  objs = dict()
249  for key in ser_objs:
250  msg = CollisionObject()
251  conversions.msg_from_string(msg, ser_objs[key])
252  objs[key] = msg
253  return objs
254 
255  def get_attached_objects(self, object_ids = []):
256  """
257  Get the attached objects identified by the given object ids list. If no ids are provided, return all the attached objects.
258  """
259  ser_aobjs = self._psi.get_attached_objects(object_ids)
260  aobjs = dict()
261  for key in ser_aobjs:
262  msg = AttachedCollisionObject()
263  conversions.msg_from_string(msg, ser_aobjs[key])
264  aobjs[key] = msg
265  return aobjs
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 __make_mesh(self, name, pose, filename, scale=(1, 1, 1))
def attach_box(self, link, name, pose=None, size=(1, 1, 1), touch_links=[])


moveit_commander
Author(s): Ioan Sucan
autogenerated on Sat Apr 21 2018 02:56:06