def __make_planning_scene_diff_req(collision_object):
scene = PlanningScene()
scene.is_diff = True
scene.world.collision_objects = [collision_object]
planning_scene_diff_req = ApplyPlanningSceneRequest()
planning_scene_diff_req.scene = scene
return planning_scene_diff_req
def __make_planning_scene_diff_req(collision_object):
scene = PlanningScene()
scene.is_diff = True
scene.world.collision_objects = [collision_object]
planning_scene_diff_req = ApplyPlanningSceneRequest()
planning_scene_diff_req.scene = scene
return planning_scene_diff_req
def execute_take(self, goal):
feedback = HandOverFeedback()
self._result = HandOverResult()
self._result.success = False
# Approach
feedback.phase = HandOverFeedback.PHASE_APPROACH
self._as_take.publish_feedback(feedback)
approach_result = tiago_play_motion(
self._approach_motion) # TODO check return value
# if approach_result == False or approach_result.error_code != approach_result.SUCCEEDED:
# Open gripper
feedback.phase = HandOverFeedback.PHASE_EXECUTING
self._as_take.publish_feedback(feedback)
open_gripper()
feedback.phase = HandOverFeedback.PHASE_WAITING_FOR_CONTACT
self._as_take.publish_feedback(feedback)
if self.wait_for_force_handover(self._as_take, self._threshold):
self._result.success = True
# Close gripper
feedback.phase = HandOverFeedback.PHASE_EXECUTING
self._as_take.publish_feedback(feedback)
close_gripper()
if len(goal.object.object.primitives) != 0 or len(
goal.object.object.meshes) != 0 or len(
goal.object.object.planes) != 0:
rospy.loginfo("AttachedCollisionObject given, attaching now")
scene = PlanningScene()
scene.is_diff = True
scene.robot_state.is_diff = True
scene.robot_state.attached_collision_objects = [goal.object]
planning_scene_diff_req = ApplyPlanningSceneRequest()
planning_scene_diff_req.scene = scene
rospy.loginfo(
self._apply_planning_scene_diff.call(
planning_scene_diff_req))
else:
rospy.loginfo(
"No AttachedCollisionObject given, so not attaching")
rospy.sleep(self._retreat_delay)
# Retreat
feedback.phase = HandOverFeedback.PHASE_RETREAT
self._as_take.publish_feedback(feedback)
retreat_result = tiago_play_motion(self._retreat_motion,
wait_duration=15.0)
if retreat_result == False or retreat_result.error_code != retreat_result.SUCCEEDED:
self._result.success = False
# Callback Finished
if self._result.success:
rospy.loginfo('Succeeded')
self._as_take.set_succeeded(self._result)
else:
self._as_take.set_aborted(self._result)
def __make_planning_scene_diff_req(collision_object, attach=False):
scene = PlanningScene()
scene.is_diff = True
scene.robot_state.is_diff = True
if attach:
scene.robot_state.attached_collision_objects = [collision_object]
else:
scene.world.collision_objects = [collision_object]
planning_scene_diff_req = ApplyPlanningSceneRequest()
planning_scene_diff_req.scene = scene
return planning_scene_diff_req
def clear_octomap():
rospy.wait_for_service("/clear_octomap")
try:
clear_octomap_call = rospy.ServiceProxy('/clear_octomap', Empty)
apcr = ApplyPlanningSceneRequest()
resp = clear_octomap_call()
return True
except Exception as e:
rospy.logerr(e)
return False