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):
self.ignored_submodels = []
self.collision_objects = {}
self.collision_objects_updated = {}
self.ignored_robot_model = []
self.planning_scene_pub = rospy.Publisher('/planning_scene',
PlanningScene,
queue_size=10)
while self.planning_scene_pub.get_num_connections() < 1:
rospy.sleep(0.1)
timeout = rospy.get_param('~timeout', 0)
if timeout <= 0:
timeout = None
rospy.loginfo(
'Waiting {} for /get_planning_scene service to be advertised.'.
format(
'{} seconds'.format(timeout) if timeout else 'indefinitely'))
rospy.wait_for_service('/get_planning_scene', timeout)
rospy.loginfo(
'/get_planning_scene service has been advertised, proceeding.')
self.get_planning_scene = rospy.ServiceProxy('/get_planning_scene',
GetPlanningScene)
self.request = PlanningSceneComponents(
components=PlanningSceneComponents.WORLD_OBJECT_NAMES)
self.ignored_robot_model = rospy.get_param('~ignore_robot_model',
'').split(';')
rospy.loginfo('Ignoring robot models of: %s' %
self.ignored_robot_model)
def _monitor_func(self, evt):
try:
planning_scene = self._get_octomap(
components=PlanningSceneComponents(
PlanningSceneComponents.OCTOMAP))
except rospy.ServiceException as e:
rospy.logerr("Error fetching planning scene: {}".format(e))
return
# Update the octomap and then check the threshold
new_octomap = np.array(planning_scene.scene.world.octomap.octomap.data)
free_space_num = np.sum(
(1 -
(1 /
(1 + np.exp(new_octomap)))) # Convert Log-Odds to Probabilities
<= OctomapMonitor.OCTOMAP_OCCUPIED_PROBABILITY_THRESHOLD)
free_space_perc = free_space_num / len(new_octomap)
# Update the trace
self.update_trace(
OctomapMonitor.OCTOMAP_MONITOR_EVENT_NAME,
free_space_perc < OctomapMonitor.FREE_PERC_FAULT_THRESHOLD,
{'free_space_perc': free_space_perc})
# Update the pointer to the octomap
with self._octomap_lock:
self._octomap = new_octomap
def remove_object(name="Object"):
co = CollisionObject()
co.operation = CollisionObject.REMOVE
co.id = name
co.header.frame_id = "/base_link"
co.header.stamp = rospy.Time.now()
while scene._pub_co.get_num_connections() == 0:
rospy.sleep(0.01)
scene._pub_co.publish(co)
while True:
rospy.sleep(0.1)
result, success = get_planning_scene(
PlanningSceneComponents(
PlanningSceneComponents.WORLD_OBJECT_NAMES
+ PlanningSceneComponents.WORLD_OBJECT_GEOMETRY
)
)
if not success:
continue
found = False
for object in result.scene.world.collision_objects:
if object.id == name:
found = True
if not found:
return
def add_shelf(quality=Shelf.SIMPLE):
pose = get_shelf_pose()
while scene._pub_co.get_num_connections() == 0:
rospy.sleep(0.01)
if quality == Shelf.SIMPLE:
pose.pose.position.z += HEIGHT / 2
scene.add_box(name="shelf", pose=pose, size=(DEPTH, HEIGHT, WIDTH))
elif quality == Shelf.FULL:
scene.add_mesh(name="shelf", pose=pose, filename=kiva_pod)
elif quality == Shelf.PADDED:
for dx, dy in [(1, 1), (1, -1), (-1, -1), (-1, 1), (0, 0)]:
mypose = deepcopy(pose)
mypose.pose.position.x += dx * PADDING
mypose.pose.position.y += dy * PADDING
if (dx, dy) == (0, 0):
name = "shelf"
else:
name = "shelf" + str(dx) + str(dy)
scene.add_mesh(name=name, pose=mypose, filename=kiva_pod)
elif quality in APC_BINS:
add_bin(quality)
else:
rospy.logwarn("Unsupported quality %s" % quality)
while True:
rospy.sleep(0.1)
result, success = get_planning_scene(
PlanningSceneComponents(
PlanningSceneComponents.WORLD_OBJECT_NAMES +
PlanningSceneComponents.WORLD_OBJECT_GEOMETRY))
if not success:
continue
for object in result.scene.world.collision_objects:
if object.id == "shelf":
return
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_acm(self):
req = PlanningSceneComponents(
components=(PlanningSceneComponents.ALLOWED_COLLISION_MATRIX
| PlanningSceneComponents.SCENE_SETTINGS))
res = self.get_planning_scene(req)
acm = res.scene.allowed_collision_matrix
return acm
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 check_fingers_collisions(self, enable=True):
"""
Disables or enables the collisions check between the fingers and the objects / table
@param enable: set to True to enable / False to disable
@return True on success
"""
objects = ["cricket_ball__link", "drill__link", "cafe_table__link"]
while self.__pub_planning_scene.get_num_connections() < 1:
rospy.loginfo(
"waiting for someone to subscribe to the /planning_scene")
rospy.sleep(0.1)
request = PlanningSceneComponents(
components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX)
response = self.__get_planning_scene(request)
acm = response.scene.allowed_collision_matrix
for object_name in objects:
if object_name not in acm.entry_names:
# add object to allowed collision matrix
acm.entry_names += [object_name]
for row in range(len(acm.entry_values)):
acm.entry_values[row].enabled += [False]
new_row = deepcopy(acm.entry_values[0])
acm.entry_values += {new_row}
for index_entry_values, entry_values in enumerate(acm.entry_values):
if "H1_F" in acm.entry_names[index_entry_values]:
for index_value, _ in enumerate(entry_values.enabled):
if acm.entry_names[index_value] in objects:
if enable:
acm.entry_values[index_entry_values].enabled[
index_value] = False
else:
acm.entry_values[index_entry_values].enabled[
index_value] = True
elif acm.entry_names[index_entry_values] in objects:
for index_value, _ in enumerate(entry_values.enabled):
if "H1_F" in acm.entry_names[index_value]:
if enable:
acm.entry_values[index_entry_values].enabled[
index_value] = False
else:
acm.entry_values[index_entry_values].enabled[
index_value] = True
planning_scene_diff = PlanningScene(is_diff=True,
allowed_collision_matrix=acm)
self.__pub_planning_scene.publish(planning_scene_diff)
rospy.sleep(1.0)
return True
def remove_object_from_acm(self, object_name):
req = PlanningSceneComponents(
components=(PlanningSceneComponents.ALLOWED_COLLISION_MATRIX
| PlanningSceneComponents.SCENE_SETTINGS))
res = self.get_planning_scene(req)
acm = res.scene.allowed_collision_matrix
planning_scene_diff = PlanningScene()
planning_scene_diff.is_diff = True
acm = remove_from_acm(acm, object_name)
planning_scene_diff.allowed_collision_matrix = acm
self.planning_scene_pub.publish(planning_scene_diff)
def add_object_to_acm(self, object_name):
req = PlanningSceneComponents(
components=(PlanningSceneComponents.ALLOWED_COLLISION_MATRIX
| PlanningSceneComponents.SCENE_SETTINGS))
res = self.get_planning_scene(req)
acm = res.scene.allowed_collision_matrix
planning_scene_diff = PlanningScene(is_diff=True)
acm = add_to_acm(acm, object_name)
planning_scene_diff.allowed_collision_matrix = acm
self.planning_scene_pub.publish(planning_scene_diff)
rospy.sleep(2)
def on_enter(self, userdata):
self.return_code = None
self.scene = None
# Retrieve the relevant data
try :
if (self.given_action_topic is None):
self.current_action_topic = userdata.action_topic
except Exception as e:
Logger.logwarn('Failed to set up the action client for %s - invalid user data parameters\n%s' % (self.current_action_topic, str(e)))
self.return_code = 'failed'
return
try:
if (self.client is None):
self.client = ProxyActionClient({self.current_action_topic: GetPlanningSceneAction},
self.wait_duration)
except Exception as e:
Logger.logwarn('Failed to set up the action client for %s\n%s' % (self.current_action_topic, str(e)))
self.return_code = 'failed'
return
try:
if not self.client.is_available(self.current_action_topic):
self.client.setup_action_client(self.current_action_topic, GetPlanningSceneAction, self.wait_duration)
if not self.setup_action_client.is_available(self.current_action_topic):
Logger.logerr( 'Action client is not available for %s' % (self.current_action_topic))
self.return_code = 'failed'
return
except Exception as e:
Logger.logwarn('Failed to setup the action client for %s\n%s' % (self.name, str(e)))
self.return_code = 'failed'
try:
# Action Initialization
action_goal = GetPlanningSceneGoal()
action_goal.components = PlanningSceneComponents()
action_goal.components.components = self.components
if (self.timeout_duration > rospy.Duration(0.0)):
self.timeout_target = rospy.Time.now() + self.timeout_duration
else:
self.timeout_target = None
self.client.send_goal(self.current_action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to send action goal for group - %s\n%s' % (self.name, str(e)))
self.return_code = 'failed'
def get_body_names_from_planner(self):
rospy.wait_for_service(self.planning_scene_topic, 5)
components = PlanningSceneComponents(
PlanningSceneComponents.WORLD_OBJECT_NAMES +
PlanningSceneComponents.TRANSFORMS)
ps_request = moveit_msgs.srv.GetPlanningSceneRequest(
components=components)
ps_response = self.planning_scene_service_proxy.call(ps_request)
body_names = [
co.id for co in ps_response.scene.world.collision_objects
]
return body_names
def __init__(self):
self.ignored_submodels = []
self.collision_objects = {}
self.collision_objects_updated = {}
self.planning_scene_pub = rospy.Publisher('/planning_scene',
PlanningScene,
queue_size=10)
while self.planning_scene_pub.get_num_connections() < 1:
rospy.sleep(0.1)
rospy.wait_for_service('/get_planning_scene', 10.0)
self.get_planning_scene = rospy.ServiceProxy('/get_planning_scene',
GetPlanningScene)
self.request = PlanningSceneComponents(
components=PlanningSceneComponents.WORLD_OBJECT_NAMES)
def enable_fingers_collisions(self, gripper_prefix, object_names, enable=True):
# type: (str, list, float) -> None
"""
Disables or enables the collisions check between the fingers and a list of objects
When closing the gripper, we should disable fingers collisions. If not, MoveIt! will
fail its planning.
:param gripper_prefix: either lgripper or rgripper
:param object_names: a list of objects
:param enable: set to True to enable / False to disable
:return: None
"""
while self.planning_scene_publisher.get_num_connections() < 1:
rospy.loginfo("Waiting to subscribe to the /planning_scene")
rospy.sleep(0.1)
request = PlanningSceneComponents(
components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX)
response = self.get_planning_scene(request)
acm = response.scene.allowed_collision_matrix
for name in object_names:
if name not in acm.entry_names:
# Add object to allowed collision matrix
acm.entry_names += [name]
for row in range(len(acm.entry_values)):
acm.entry_values[row].enabled += [False]
new_row = deepcopy(acm.entry_values[0])
acm.entry_values += {new_row}
for index_entry_values, entry_values in enumerate(acm.entry_values):
if gripper_prefix in acm.entry_names[index_entry_values]:
for index_value, _ in enumerate(entry_values.enabled):
if acm.entry_names[index_value] in object_names:
if enable:
acm.entry_values[index_entry_values].enabled[index_value] = False
else:
acm.entry_values[index_entry_values].enabled[index_value] = True
elif acm.entry_names[index_entry_values] in object_names:
for index_value, _ in enumerate(entry_values.enabled):
if gripper_prefix in acm.entry_names[index_value]:
if enable:
acm.entry_values[index_entry_values].enabled[index_value] = False
else:
acm.entry_values[index_entry_values].enabled[index_value] = True
planning_scene_diff = PlanningScene(is_diff=True, allowed_collision_matrix=acm)
self.planning_scene_publisher.publish(planning_scene_diff)
rospy.sleep(1.0)
def allow_all(self):
request = PlanningSceneComponents(
components=(PlanningSceneComponents.ALLOWED_COLLISION_MATRIX
| PlanningSceneComponents.SCENE_SETTINGS))
response = self.get_planning_scene(request)
acm = response.scene.allowed_collision_matrix
planning_scene_diff = PlanningScene()
planning_scene_diff.is_diff = True
for object_name in self.object_names:
rospy.logwarn('Adding object {} to allowed collisions...'.format(
object_name))
# acm = acm_allow_grasping(acm, object_name, allow=True)
acm = add_to_acm(acm, object_name)
planning_scene_diff.allowed_collision_matrix = acm
self.planning_scene_publisher.publish(planning_scene_diff)
def disallow_scoop_collision():
request = PlanningSceneComponents(
components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX)
response = get_planning_scene(request)
acm = response.scene.allowed_collision_matrix
print acm
if not SCOOP in acm.default_entry_names:
# add button to allowed collision matrix
acm.default_entry_names += [SCOOP]
acm.default_entry_values += [False]
else:
acm.default_entry_values[acm.default_entry_names.index(SCOOP)] = False
print acm.default_entry_names, acm.default_entry_values[
acm.default_entry_names.index(SCOOP)], SCOOP in acm.entry_names
planning_scene_diff = PlanningScene(is_diff=True,
allowed_collision_matrix=acm)
pubPlanningScene.publish(planning_scene_diff)
rospy.sleep(1.0)
def remove_object_handler(self, req):
request = PlanningSceneComponents(
components=(PlanningSceneComponents.ALLOWED_COLLISION_MATRIX
| PlanningSceneComponents.SCENE_SETTINGS))
response = self.get_planning_scene(request)
acm = response.scene.allowed_collision_matrix
planning_scene_diff = PlanningScene()
planning_scene_diff.is_diff = True
if not '__link_0' in req.object_name:
req.object_name += '__link_0'
acm = remove_from_acm(acm, req.object_name)
planning_scene_diff.allowed_collision_matrix = acm
self.planning_scene_publisher.publish(planning_scene_diff)
return ChangeCollisionObjectResponse(True)
def test_planning_scene(self):
'''
Check if publish_simple_scene.py is working
'''
rospy.wait_for_service('/get_planning_scene', timeout=20)
get_planning_scene = rospy.ServiceProxy("/get_planning_scene",
GetPlanningScene)
collision_objects = []
# wait for 10 sec
time_start = rospy.Time.now()
while not collision_objects and (rospy.Time.now() -
time_start).to_sec() < 10.0:
world = get_planning_scene(
PlanningSceneComponents(components=PlanningSceneComponents.
WORLD_OBJECT_NAMES)).scene.world
collision_objects = world.collision_objects
rospy.loginfo("collision_objects = " +
str(world.collision_objects))
rospy.sleep(1)
self.assertTrue(world.collision_objects != [], world)
def allow_collision(self):
# self.pubPlanningScene = rospy.Publisher("planning_scene", PlanningScene)
ps = PlanningScene()
psc = PlanningSceneComponents()
acm = AllowedCollisionMatrix()
ace = AllowedCollisionEntry()
is_allow = Bool()
is_allow.data = False
ace.enabled = [False]
# get scene components
# psc.ALLOWED_COLLISION_MATRIX
getScene = self.gps(psc) # make it empty for now
# ps = getScene
ps.allowed_collision_matrix.entry_names = ["cube"]
ps.allowed_collision_matrix.entry_values = [ace]
ps.allowed_collision_matrix.default_entry_names = ["cube"]
ps.allowed_collision_matrix.default_entry_values = [1]
ps.is_diff = 1
# print "gripper name", self.gripper
applyScene = self.aps(ps)
print ps
def toggleOctomap(boolean, pubPlanningScene, get_planning_scene):
'''
Set whether to ignore the point cloud or not.
'''
request = PlanningSceneComponents(
components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX)
response = get_planning_scene(request)
acm = response.scene.allowed_collision_matrix
if not '<octomap>' in acm.default_entry_names:
acm.default_entry_names += ['<octomap>']
acm.default_entry_values += [boolean]
else:
index = acm.default_entry_names.index('<octomap>')
acm.default_entry_values[index] = boolean
planning_scene_diff = PlanningScene(is_diff=True,
allowed_collision_matrix=acm)
pubPlanningScene.publish(planning_scene_diff)
rospy.sleep(1.0)
def update_planning_scene_from_moveit(self):
""" Update the current cached scene directly from moveit.
Returns true if it the service call succeeds. False implies moveit hasn't started correctly.
"""
try:
rospy.wait_for_service("/get_planning_scene", 5)
components = PlanningSceneComponents(PlanningSceneComponents.WORLD_OBJECT_NAMES + PlanningSceneComponents.TRANSFORMS)
"""
:type ps_response :moveit_msgs.srv.GetPlanningSceneResponse
"""
ps_request = moveit_msgs.srv.GetPlanningSceneRequest(components=components)
ps_response = self.planning_scene_service.call(ps_request)
self.body_name_cache = [co.id for co in ps_response.scene.world.collision_objects]
rospy.loginfo("body name cache:%s"%(', '.join(self.body_name_cache)))
return True
except Exception as e:
raise e
rospy.logerror("Failed to find service /get_planning_scene %s and force use moveit enabled"%(e))
rospy.shutdown()
rospy.logwarn("Failed to find service /get_planning_scene %s"%(e))
return False
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)
def execute(self, goal):
try:
mAndm_logger.info("===== Planning request received from client ==========")
mAndm_logger.info(goal)
# re-initialize feedback and result
self.feedback = PlanAndExecuteTrajFeedback()
self.result = PlanAndExecuteTrajResult()
mAndm_logger.info("===== Parameters check: Identifying action type =========")
if goal.action_type not in self.possilbe_action_types:
mAndm_logger.error("Action parameter: {action_type} does not match with expected values."\
.format(action_type=goal.action_type))
self.result.error_type = 6
self.server.set_aborted(result=self.result)
return
else:
action_type = goal.action_type
mAndm_logger.info("- Action is: {action_type}".format(action_type=goal.action_type))
mAndm_logger.info("===== Parameters check: target tag or target pose =========")
if action_type == 'move_to' and not goal.tag_name and not (goal.target_pose.position.x or goal.target_pose.position.y or\
goal.target_pose.position.z or goal.target_pose.orientation.x or\
goal.target_pose.orientation.y or goal.target_pose.orientation.z or\
goal.target_pose.orientation.w):
# return that we fail
mAndm_logger.error("No Tag Name or Pose is provided for action move_to. Quitting path planning.")
self.result.error_type = 1
self.server.set_aborted(result=self.result)
return
elif action_type == 'get_tag_pose' and not goal.tag_name:
# return that we fail
mAndm_logger.error("No Tag Name is provided for action get_tag_pose. Quitting..")
self.result.error_type = 10
self.server.set_aborted(result=self.result)
return
mAndm_logger.info("- Tag Name is: {tag_name}".format(tag_name=goal.tag_name))
mAndm_logger.info("- Tag Name is: {target_pose}".format(target_pose=goal.target_pose))
# first figure out the target arm
mAndm_logger.info("===== Parameters check: Finding target arm =========")
if not goal.arm or goal.arm == self.arm:
target_arm = self.arm
elif goal.arm in self.possible_arm_groups:
mAndm_logger.warning("This is a {arm} traj server. We may have a problem!".format(arm=self.arm))
target_arm = goal.arm
else:
mAndm_logger.warning("Arm parameter:{arm} does not match with expected values:left_arm, right_arm or both_arms. Using both_arms.".format(arm=goal.arm))
#target_arm = "both_arms"
self.result.error_type = 7
self.server.set_aborted(result=self.result)
return
mAndm_logger.info("-- Target arm is {target_arm}".format(target_arm=target_arm))
self.feedback.arm = target_arm
mAndm_logger.info("===== Parameters check: Identifying planning mode =========")
if goal.planning_mode not in self.possible_planning_modes:
mAndm_logger.warning("Planning mode: {planning_mode} does not match with expected values: rough, fine or rough_and_fine. Using rough_and_fine.".format(planning_mode=goal.planning_mode))
planning_mode = 'rough_and_fine'
else:
planning_mode = goal.planning_mode
mAndm_logger.info("Planning mode is: {planning_mode}".format(planning_mode=planning_mode))
#############################
########## MODES ###########
############################
# ------------ close_gripper
# position: 72.65 for module, start with 100, closed 3.2
if action_type == 'close_gripper':
self.feedback.executing_grasp = True
mAndm_logger.info("===== Closing gripper ======================")
# operations on gripper if pickup or drop
# open gripper before fine plan
if target_arm in ['left_arm','right_arm']:
mAndm_logger.debug(target_arm.replace('_arm',''))
self.gripper[target_arm.replace('_arm','')].close()
time.sleep(2) # so that internal data is updated
# feeback
if self.gripper[target_arm.replace('_arm','')].gripping():
gripping_successful = True
else:
gripping_successful = False
else: # both arms
self.gripper['left'].close()
self.gripper['right'].close()
time.sleep(2) # so that internal data is updated
# feeback
if self.gripper['left'].gripping() and self.gripper['right'].gripping():
gripping_successful = True
else:
gripping_successful = False
#mAndm_logger.debug("Position when gripper is closed: {pos}".format(pos=self.gripper['right'].position()))
#mAndm_logger.debug("Force when gripper is closed: {pos}".format(pos=self.gripper['right'].force()))
#mAndm_logger.debug("Gripping: {pos}".format(pos=self.gripper['right'].gripping()))
#mAndm_logger.debug("Missed: {pos}".format(pos=self.gripper['right'].missed()))
# send result
self.result.grasp_result = gripping_successful
mAndm_logger.debug("--Grasping module : {status}".format(status=gripping_successful))
if gripping_successful:
self.server.set_succeeded(result=self.result)
mAndm_logger.info("===== Closing gripper: successful ======================")
else:
self.server.set_aborted(result=self.result)
mAndm_logger.error("===== Closing gripper: failed - no object in hand =======")
# ------------ open_gripper
elif action_type == 'open_gripper':
self.feedback.executing_grasp = True
mAndm_logger.info("===== Opening gripper ======================")
# operations on gripper if pickup or drop
# open gripper before fine plan
if target_arm in ['left_arm','right_arm']:
mAndm_logger.debug(target_arm.replace('_arm',''))
self.gripper[target_arm.replace('_arm','')].open()
else: # both arms
self.gripper['left'].open()
self.gripper['right'].open()
#self.limb['left'].set_joint_velocities({'left_w2':0.3})
#time.sleep(7)
#self.limb['left'].set_joint_velocities({'left_w2':0.0})
# feedback on whether grasp is good?
self.result.grasp_result = True
self.server.set_succeeded(result=self.result)
mAndm_logger.info("===== Opening gripper: successful ======================")
# ------------ move_to
elif action_type == 'move_to':
mAndm_logger.info("===== Mode move_to ======================")
########################
###### rough plan ######
########################
mAndm_logger.debug("planning_mode:" + str(planning_mode))
if planning_mode in ['rough','rough_and_fine']:
#rough plan
if not self.planning_traj(goal.tag_name, goal.target_pose, target_arm, 'rough'):
return
########################
###### add module ######
########################
if self.using_octamap:
mAndm_logger.info("===== Adding module ======================")
# add module:
if goal.tag_name:
self.add_module(goal.tag_name)
mAndm_logger.info("===== Excluding tag object ======================")
request = PlanningSceneComponents(components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX)
response = self.get_planning_scene(request)
acm = response.scene.allowed_collision_matrix
# add gripper fingers to allowed collision matrix
acm.default_entry_names += [goal.tag_name]
acm.default_entry_values += [True]
planning_scene_diff = PlanningScene(is_diff=True, allowed_collision_matrix=acm)
#mAndm_logger.log(4, "acm: {acm}".format(acm=acm))
self._pubPlanningScene.publish(planning_scene_diff)
########################
###### fine plan ######
########################
if planning_mode in ['fine','rough_and_fine']:
#fine plan
if not self.planning_traj(goal.tag_name, goal.target_pose, target_arm, 'fine'):
return
###########################
###### remove module ######
###########################
if self.using_octamap:
mAndm_logger.info("===== Removing tag object ======================")
# remove gripper fingers to allowed collision matrix
acm.default_entry_names = []
acm.default_entry_values = []
planning_scene_diff = PlanningScene(is_diff=True, allowed_collision_matrix=acm)
#mAndm_logger.log(4, "acm: {acm}".format(acm=acm))
self._pubPlanningScene.publish(planning_scene_diff)
self.scene.remove_world_object(goal.tag_name)
self.server.set_succeeded(result=self.result)
# ------------ move_to_camera_pos
elif action_type == 'move_to_camera_pos':
mAndm_logger.info("===== Mode move_to_camera_pos ==============")
#fine plan
if target_arm not in ['left_arm','right_arm']:
mAndm_logger.error('move to camera pose only works for left_arm or right_arm.Given {target_arm}'.format(target_arm=target_arm))
self.result.error_type = 8
self.server.set_aborted(result=self.result)
return
target_pose = self.move_to_camera_poses[target_arm.replace('_arm','')]
#mAndm_logger.debug(target_pose)
if not self.planning_traj("", target_pose, target_arm, 'rough'):
return
self.server.set_succeeded(result=self.result)
# ------------ get_tag_pose
elif action_type == 'get_tag_pose':
mAndm_logger.info("===== Mode get_tag_pose ======================")
if self.find_target_pose(goal.tag_name, 'rough', target_arm):
self.server.set_succeeded(result=self.result)
else:
self.server.set_aborted(result=self.result)
mAndm_logger.info("++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++")
mAndm_logger.info("++++++++++++ Motion Planning Execution Done! +++++++++++++++++")
mAndm_logger.info("++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n")
except:
mAndm_logger.error("Unexpected error:")
exc_type, exc_value, exc_traceback = sys.exc_info()
traceback.print_tb(exc_traceback, limit=1, file=sys.stdout)
traceback.print_exception(exc_type, exc_value, exc_traceback,
limit=2, file=sys.stdout)
self.result.error_type = 99
self.server.set_aborted(result=self.result)
while not rospy.is_shutdown():
pose = PoseStamped()
pose.header.frame_id = 'BASE'
pose.pose.position.x = 0.3
pose.pose.position.y = -0.35
pose.pose.position.z = 0.5
pose.pose.orientation.w = 1.0
scene_interface.add_box('simple_object_1', pose, (0.2, 0.5, 0.04))
pose.pose.position.x = 0.3
pose.pose.position.y = -0.12
pose.pose.position.z = 0.72
scene_interface.add_box('simple_object_2', pose, (0.2, 0.04, 0.4))
pose.pose.position.x = -0.2
pose.pose.position.y = 0.0
pose.pose.position.z = 0.7
scene_interface.add_box('simple_object_3', pose, (0.04, 1.0, 0.2))
rate.sleep()
print len(
get_planning_scene(
PlanningSceneComponents(
components=PlanningSceneComponents.WORLD_OBJECT_NAMES)).scene.
world.collision_objects)
if get_planning_scene and len(
get_planning_scene(
PlanningSceneComponents(
components=PlanningSceneComponents.WORLD_OBJECT_NAMES)).
scene.world.collision_objects) >= 3:
break
