def _get_all_collision_objects(self):
collision_object_names = []
planning_scene_request = PlanningSceneComponents()
planning_scene_request.components = PlanningSceneComponents.WORLD_OBJECT_NAMES
planning_scene = self.plannig_scene_service(planning_scene_request)
for collision_object in planning_scene.scene.world.collision_objects:
collision_object_names.append(collision_object.id)
return collision_object_names
def __init__(self, frame, ns='', init_from_service=True):
# ns must be a string
if not isinstance(ns, basestring):
rospy.logerr('Namespace must be a string!')
else:
ns += '/'
self._fixed_frame = frame
# publisher to send objects to MoveIt
self._pub = rospy.Publisher(ns + 'collision_object',
CollisionObject,
queue_size=10)
self._attached_pub = rospy.Publisher(ns + 'attached_collision_object',
AttachedCollisionObject,
queue_size=10)
self._scene_pub = rospy.Publisher(ns + 'planning_scene',
PlanningScene,
queue_size=10)
# track the attached and collision objects
self._mutex = thread.allocate_lock()
# these are updated based what the planning scene actually contains
self._attached = list()
self._collision = list()
# these are updated based on internal state
self._objects = dict()
self._attached_objects = dict()
self._removed = dict()
self._attached_removed = dict()
self._colors = dict()
# get the initial planning scene
if init_from_service:
rospy.loginfo('Waiting for get_planning_scene')
rospy.wait_for_service(ns + 'get_planning_scene')
self._service = rospy.ServiceProxy(ns + 'get_planning_scene',
GetPlanningScene)
try:
req = PlanningSceneComponents()
req.components = sum([
PlanningSceneComponents.WORLD_OBJECT_NAMES,
PlanningSceneComponents.WORLD_OBJECT_GEOMETRY,
PlanningSceneComponents.ROBOT_STATE_ATTACHED_OBJECTS
])
scene = self._service(req)
self.sceneCb(scene.scene, initial=True)
except rospy.ServiceException as e:
rospy.logerr(
'Failed to get initial planning scene, results may be wonky: %s',
e)
# subscribe to planning scene
rospy.Subscriber(ns + 'move_group/monitored_planning_scene',
PlanningScene, self.sceneCb)
def _get_all_collision_objects(self):
rospy.wait_for_service('/get_planning_scene', 10.0)
plannig_scene_service = rospy.ServiceProxy('get_planning_scene',
GetPlanningScene)
collision_object_names = []
planning_scene_request = PlanningSceneComponents()
planning_scene_request.components = PlanningSceneComponents.WORLD_OBJECT_NAMES
planning_scene = plannig_scene_service(planning_scene_request)
for collision_object in planning_scene.scene.world.collision_objects:
collision_object_names.append(collision_object.id)
return collision_object_names
def _get_scene(self):
req = PlanningSceneComponents()
req.components = sum([
PlanningSceneComponents.WORLD_OBJECT_NAMES,
PlanningSceneComponents.WORLD_OBJECT_GEOMETRY,
PlanningSceneComponents.ROBOT_STATE_ATTACHED_OBJECTS
])
try:
resp = self._get_planning_scene(req)
return resp.scene
except rospy.ServiceException as e:
raise RuntimeError("Failed to get initial planning scene.")
def __init__(self, frame, init_from_service=True):
self._fixed_frame = frame
# publisher to send objects to MoveIt
self._pub = rospy.Publisher('collision_object',
CollisionObject,
queue_size=10)
self._attached_pub = rospy.Publisher('attached_collision_object',
AttachedCollisionObject,
queue_size=10)
self._scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
# track the attached and collision objects
self._mutex = thread.allocate_lock()
# these are updated based what the planning scene actually contains
self._attached = list()
self._collision = list()
# these are updated based on internal state
self._objects = dict()
self._attached_objects = dict()
self._removed = dict()
self._attached_removed = dict()
self._colors = dict()
# get the initial planning scene
if init_from_service:
rospy.loginfo('Waiting for get_planning_scene')
rospy.wait_for_service('get_planning_scene')
self._service = rospy.ServiceProxy('get_planning_scene',
GetPlanningScene)
try:
req = PlanningSceneComponents()
req.components = sum([
PlanningSceneComponents.WORLD_OBJECT_NAMES,
PlanningSceneComponents.WORLD_OBJECT_GEOMETRY,
PlanningSceneComponents.ROBOT_STATE_ATTACHED_OBJECTS])
scene = self._service(req)
self.sceneCb(scene.scene, initial=True)
except rospy.ServiceException as e:
rospy.logerr('Failed to get initial planning scene, results may be wonky: %s', e)
# subscribe to planning scene
rospy.Subscriber('move_group/monitored_planning_scene',
PlanningScene,
self.sceneCb)
def __init__(self, frame, ns='', init_from_service=True):
# ns must be a string
if not isinstance(ns, basestring):
rospy.logerr('Namespace must be a string!')
ns = ''
elif not ns.endswith('/'):
ns += '/'
self._fixed_frame = frame
self._scene_pub = rospy.Publisher(ns + 'planning_scene',
PlanningScene,
queue_size=10)
self._apply_service = rospy.ServiceProxy(ns + 'apply_planning_scene', ApplyPlanningScene)
# track the attached and collision objects
self._mutex = thread.allocate_lock()
# these are updated based what the planning scene actually contains
self._attached = list()
self._collision = list()
# these are updated based on internal state
self._objects = dict()
self._attached_objects = dict()
self._removed = dict()
self._attached_removed = dict()
self._colors = dict()
# get the initial planning scene
if init_from_service:
rospy.loginfo('Waiting for get_planning_scene')
rospy.wait_for_service(ns + 'get_planning_scene')
self._service = rospy.ServiceProxy(ns + 'get_planning_scene',
GetPlanningScene)
try:
req = PlanningSceneComponents()
req.components = sum([
PlanningSceneComponents.WORLD_OBJECT_NAMES,
PlanningSceneComponents.WORLD_OBJECT_GEOMETRY,
PlanningSceneComponents.ROBOT_STATE_ATTACHED_OBJECTS])
scene = self._service(req)
self.sceneCb(scene.scene, initial=True)
except rospy.ServiceException as e:
rospy.logerr('Failed to get initial planning scene, results may be wonky: %s', e)
# subscribe to planning scene
rospy.Subscriber(ns + 'move_group/monitored_planning_scene',
PlanningScene,
self.sceneCb)
def _get_planning_scene(self):
"""Get planning scene.
Returns:
PlanningScene: current scene state
"""
get_planning_scene = rospy.ServiceProxy('/get_planning_scene',
GetPlanningScene)
get_planning_scene.wait_for_service(timeout=5.0)
req = PlanningSceneComponents()
req.components = PlanningSceneComponents.WORLD_OBJECT_NAMES
try:
resp = get_planning_scene(req)
return resp.scene
except rospy.ServiceException as ex:
rospy.logerr('Failed to get initial planning scene: %s', ex)
return PlanningScene()
def __init__(self, frame, init_from_service=True):
self._fixed_frame = frame
self._scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
self._apply_service = rospy.ServiceProxy('apply_planning_scene',
ApplyPlanningScene)
# track the attached and collision objects
self._mutex = thread.allocate_lock()
# these are updated based what the planning scene actually contains
self._attached = list()
self._collision = list()
# these are updated based on internal state
self._objects = dict()
self._attached_objects = dict()
self._removed = dict()
self._attached_removed = dict()
self._colors = dict()
# get the initial planning scene
if init_from_service:
rospy.loginfo('Waiting for get_planning_scene')
rospy.wait_for_service('get_planning_scene')
self._service = rospy.ServiceProxy('get_planning_scene',
GetPlanningScene)
try:
req = PlanningSceneComponents()
req.components = sum([
PlanningSceneComponents.WORLD_OBJECT_NAMES,
PlanningSceneComponents.WORLD_OBJECT_GEOMETRY,
PlanningSceneComponents.ROBOT_STATE_ATTACHED_OBJECTS
])
scene = self._service(req)
self.sceneCb(scene.scene, initial=True)
except rospy.ServiceException as e:
rospy.logerr(
'Failed to get initial planning scene, results may be wonky: %s',
e)
# subscribe to planning scene
rospy.Subscriber('move_group/monitored_planning_scene', PlanningScene,
self.sceneCb)
def __init__(self, frame, ns='', init_from_service=True):
'''
Constructor of PlanningSceneInterface class
:param frame: The fixed frame in which planning is being
done (needs to be part of robot?)
:param ns: A namespace to push all topics down into.
:param init_from_service: Whether to initialize our list of objects
by calling the service
NOTE: this requires that said service be in the move_group
launch file, which is not the default from the setup assistant.
'''
# ns must be a string
# if not isinstance(ns, basestring):
# rospy.logerr('Namespace must be a string!')
# ns = ''
# elif not ns.endswith('/'):
# ns += '/'
self._fixed_frame = frame
self._scene_pub = rospy.Publisher(ns + 'planning_scene',
PlanningScene,
queue_size=10)
self._apply_service = rospy.ServiceProxy(ns + \
'apply_planning_scene', ApplyPlanningScene)
# track the attached and collision objects
self._mutex = thread.allocate_lock()
# these are updated based what the planning scene actually contains
self._attached = list()
self._collision = list()
# these are updated based on internal state
self._objects = dict()
self._attached_objects = dict()
self._removed = dict()
self._attached_removed = dict()
self._colors = dict()
# get the initial planning scene
if init_from_service:
rospy.loginfo('Waiting for get_planning_scene')
rospy.wait_for_service(ns + 'get_planning_scene')
self._service = rospy.ServiceProxy(ns + 'get_planning_scene',
GetPlanningScene)
try:
req = PlanningSceneComponents()
req.components = sum([
PlanningSceneComponents.WORLD_OBJECT_NAMES,
PlanningSceneComponents.WORLD_OBJECT_GEOMETRY,
PlanningSceneComponents.ROBOT_STATE_ATTACHED_OBJECTS
])
scene = self._service(req)
self.sceneCb(scene.scene, initial=True)
except rospy.ServiceException as e:
rospy.logerr(
'Failed to get initial planning scene, results may be wonky: %s',
e)
# subscribe to planning scene
rospy.Subscriber(ns + 'move_group/monitored_planning_scene',
PlanningScene, self.sceneCb)