class PlanningScene: def __init__(self): self._g = PlanningSceneInterface() def add_sphere(self, id_name, frame_id, position, orientation, radius): """ Add sphere to the planning scene in specified frame position: [x,y,z] orientation: [x,y,z,w] """ self._g.add_sphere(id_name, frame_id, position, orientation, radius) def add_cylinder(self, id_name, frame_id, position, orientation, height, radius): self._g.add_cylinder(id_name, frame_id, position, orientation, height, radius) def add_box(self, id_name, frame_id, position, orientation, length, width, height): self._g.add_box(id_name, frame_id, position, orientation, length, width, height) def add_cone(self, id_name, frame_id, position, orientation, height, radius): self._g.add_cone(id_name, frame_id, position, orientation, height, radius) def remove_simple_object(self, id_name, frame_id): """ Remove object from planning scene with id_name and frame_id """ self._g.remove_simple_object(id_name, frame_id) def attach_sphere(self, id_name, frame_id, link_name, touch_links, position, orientation, radius): """ Add sphere to the planning scene in attached to specified link touch_links: [link1, link2, link3, etc] <- links that object can touch position: [x,y,z] orientation: [x,y,z,w] """ self._g.attach_sphere(id_name, frame_id, link_name, touch_links, position, orientation, radius) def attach_cylinder(self, id_name, frame_id, link_name, touch_links, position, orientation, height, radius): self._g.attach_cylinder(id_name, frame_id, link_name, touch_links, position, orientation, height, radius) def attach_box(self, id_name, frame_id, link_name, touch_links, position, orientation, length, width, height): self._g.attach_box(id_name, frame_id, link_name, touch_links, position, orientation, length, width, height) def attach_cone(self, id_name, frame_id, link_name, touch_links, position, orientation, height, radius): self._g.attach_cone(id_name, frame_id, link_name, touch_links, position, orientation, height, radius) def remove_simple_attached_object(self, id_name, frame_id, link_name): """ Detach object from link, add it back into planning scene """ self._g.remove_simple_attached_object(id_name, frame_id, link_name)