def setup_statemachine(robot): sm = smach.StateMachine(outcomes=['done', 'aborted']) robot.ed.reset() with sm: smach.StateMachine.add('STORE_WAYPOINT', StoreWaypoint(robot), transitions={'done': 'RESET', 'continue': 'RESET'}) smach.StateMachine.add('RESET', states.ResetED(robot), transitions={'done': 'WAIT_SAY'}) return sm
'continue': 'RESET' }) smach.StateMachine.add('RESET', states.ResetED(robot), transitions={'done': 'WAIT_SAY'}) return sm if __name__ == '__main__': rospy.init_node('automatic_side_detection') from robot_skills.amigo import Amigo robot = Amigo() robot.ed.reset() sm = smach.StateMachine(outcomes=['done', 'aborted']) with sm: smach.StateMachine.add('STORE_WAYPOINT', StoreWaypoint(robot, "kitchen"), transitions={ 'done': 'RESET', 'continue': 'RESET' }) smach.StateMachine.add('RESET', states.ResetED(robot), transitions={'done': 'done'}) # states.startup(setup_statemachine, challenge_name="automatic_side_detection") sm.execute()
def setup_statemachine(robot): sm = smach.StateMachine(outcomes=['Done', 'Aborted']) with sm: # Start challenge via StartChallengeRobust smach.StateMachine.add("START_CHALLENGE_ROBUST", states.StartChallengeRobust( robot, STARTING_POINT), transitions={ "Done": "GO_TO_INTERMEDIATE_WAYPOINT", "Aborted": "GO_TO_INTERMEDIATE_WAYPOINT", "Failed": "GO_TO_INTERMEDIATE_WAYPOINT" }) # There is no transition to Failed in StartChallengeRobust (28 May) smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT', states.NavigateToWaypoint( robot, EntityByIdDesignator(robot, id=INTERMEDIATE_1), radius=0.5), transitions={ 'arrived': 'ASK_CONTINUE', 'unreachable': 'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1', 'goal_not_defined': 'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1' }) smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1', states.NavigateToWaypoint( robot, EntityByIdDesignator(robot, id=INTERMEDIATE_2), radius=0.5), transitions={ 'arrived': 'ASK_CONTINUE', 'unreachable': 'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2', 'goal_not_defined': 'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2' }) smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2', states.NavigateToWaypoint( robot, EntityByIdDesignator(robot, id=INTERMEDIATE_3), radius=0.5), transitions={ 'arrived': 'ASK_CONTINUE', 'unreachable': 'ASK_CONTINUE', 'goal_not_defined': 'ASK_CONTINUE' }) smach.StateMachine.add("ASK_CONTINUE", states.AskContinue(robot, 30), transitions={ 'continue': 'SAY_CONTINUEING', 'no_response': 'SAY_CONTINUEING' }) smach.StateMachine.add( 'SAY_CONTINUEING', states.Say(robot, [ "I heard continue, so I will move to the exit now. See you guys later!" ], block=False), transitions={'spoken': 'GO_TO_EXIT'}) # Amigo goes to the exit (waypoint stated in knowledge base) smach.StateMachine.add('GO_TO_EXIT', states.NavigateToWaypoint(robot, EntityByIdDesignator( robot, id=EXIT_1), radius=0.7), transitions={ 'arrived': 'AT_END', 'unreachable': 'GO_TO_EXIT_2', 'goal_not_defined': 'GO_TO_EXIT_2' }) smach.StateMachine.add('GO_TO_EXIT_2', states.NavigateToWaypoint(robot, EntityByIdDesignator( robot, id=EXIT_2), radius=0.5), transitions={ 'arrived': 'AT_END', 'unreachable': 'GO_TO_EXIT_3', 'goal_not_defined': 'GO_TO_EXIT_3' }) smach.StateMachine.add('GO_TO_EXIT_3', states.NavigateToWaypoint(robot, EntityByIdDesignator( robot, id=EXIT_3), radius=0.5), transitions={ 'arrived': 'AT_END', 'unreachable': 'RESET_ED_TARGET', 'goal_not_defined': 'AT_END' }) smach.StateMachine.add('RESET_ED_TARGET', states.ResetED(robot), transitions={'done': 'GO_TO_EXIT'}) # Finally amigo will stop and says 'goodbye' to show that he's done. smach.StateMachine.add('AT_END', states.Say(robot, "Goodbye"), transitions={'spoken': 'Done'}) analyse_designators(sm, "rips") return sm
def setup_statemachine(robot): sm = smach.StateMachine(outcomes=['done', 'aborted']) robot.ed.reset() with sm: smach.StateMachine.add('STORE_WAYPOINT', StoreWaypoint(robot), transitions={'done': 'RESET', 'continue': 'RESET'}) smach.StateMachine.add('RESET', states.ResetED(robot), transitions={'done': 'WAIT_SAY'}) return sm if __name__ == '__main__': rospy.init_node('automatic_side_detection') from robot_skills.amigo import Amigo robot = Amigo() robot.ed.reset() sm = smach.StateMachine(outcomes=['done', 'aborted']) with sm: smach.StateMachine.add('STORE_WAYPOINT', StoreWaypoint(robot, "kitchen"), transitions={'done': 'RESET', 'continue': 'RESET'}) smach.StateMachine.add('RESET', states.ResetED(robot), transitions={'done': 'done'}) # states.startup(setup_statemachine, challenge_name="automatic_side_detection") sm.execute()
def __init__(self, robot, location_id, room_id, navigate_area, segment_areas, known_types): smach.StateMachine.__init__(self, outcomes=['done']) # Set up the designators for this machine e_classifications_des = VariableDesignator( [], resolve_type=[ClassificationResult], name='e_classifications_des') e_des = EdEntityDesignator(robot, id=location_id, name='e_des') room_des = EntityByIdDesignator(robot, id=room_id, name='room_des') with self: # Loop over the areas that we have to segment and handle segmented objects for the specified navigation area for i, segment_area in enumerate(segment_areas): smach.StateMachine.add("RESET_ED_%d" % i, robot_smach_states.ResetED(robot), transitions={'done': 'NAVIGATE_%d' % i}) smach.StateMachine.add("NAVIGATE_%d" % i, robot_smach_states.NavigateToSymbolic( robot, { e_des: navigate_area, room_des: "in" }, e_des), transitions={ 'arrived': 'SEGMENT_SAY_%d' % i, 'unreachable': "SAY_UNREACHABLE_%d" % i, 'goal_not_defined': "SAY_UNREACHABLE_%d" % i }) smach.StateMachine.add( "SEGMENT_SAY_%d" % i, robot_smach_states.Say( robot, ["Looking %s the %s" % (segment_area, location_id)], block=False), transitions={"spoken": "SEGMENT_%d" % i}) smach.StateMachine.add( 'SEGMENT_%d' % i, robot_smach_states.SegmentObjects( robot, e_classifications_des.writeable, e_des, segment_area), transitions={'done': "HANDLE_DETECTED_ENTITIES_%d" % i}) # Determine the next state, either it is the next iter or done next_state = "RESET_ED_%d" % ( i + 1) if i + 1 < len(segment_areas) else "done" smach.StateMachine.add( "SAY_UNREACHABLE_%d" % i, robot_smach_states.Say( robot, ["I failed to inspect the %s" % location_id], block=True), transitions={"spoken": next_state}) smach.StateMachine.add("HANDLE_DETECTED_ENTITIES_%d" % i, HandleDetectedEntities( robot, e_classifications_des, known_types, location_id, segment_area), transitions={"done": next_state})
def setup_statemachine(robot): sm = smach.StateMachine(outcomes=['Done', 'Aborted']) start_waypoint = ds.EntityByIdDesignator(robot, id="manipulation_init_pose", name="start_waypoint") placed_items = [] with sm: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized': 'INIT_WM', 'abort': 'Aborted' }) smach.StateMachine.add("INIT_WM", InitializeWorldModel(robot), transitions={'done': 'AWAIT_START'}) # smach.StateMachine.add("INSTRUCT_WAIT_FOR_DOOR", # states.Say(robot, ["Hi there, I will now wait until you remove the cup", # "I'm waiting for you to remove the cup"], block=False), # transitions={"spoken": "WAIT_FOR_DOOR"}) # # smach.StateMachine.add("WAIT_FOR_DOOR", # states.WaitForDoorOpen(robot, timeout=10), # transitions={"closed": "DOOR_CLOSED", # "open": "AWAIT_START"}) # # smach.StateMachine.add("DOOR_CLOSED", # states.Say(robot, ["I am waiting for you to remove the cup", # "I'd start, if you remove the cup from my laser"]), # transitions={"spoken": "WAIT_FOR_DOOR"}) if USE_SLAM: drive_state = "RESET_ED_SLAM" else: drive_state = "NAV_TO_START" smach.StateMachine.add("AWAIT_START", states.AskContinue(robot), transitions={ 'continue': drive_state, 'no_response': 'AWAIT_START' }) cabinet = ds.EntityByIdDesignator(robot, id=CABINET) room = ds.EntityByIdDesignator(robot, id=ROOM) if USE_SLAM: # vth = 1.0 # smach.StateMachine.add("NAV_TO_FIT_POSE", # ForceDrive(robot, 0, 0, vth, 3.14/vth), # transitions={'done': 'FIT_ENTITY'}) smach.StateMachine.add("RESET_ED_SLAM", states.ResetED(robot), transitions={'done': 'FIT_ENTITY'}) smach.StateMachine.add("FIT_ENTITY", FitEntity(robot, CABINET), transitions={ 'succeeded': 'NAV_TO_START', 'failed': 'SAY_FITTING_FAILED' }) smach.StateMachine.add( "SAY_FITTING_FAILED", states.Say(robot, [ "Fitting the {0} failed, I will stop now.".format(CABINET) ], mood="sad"), transitions={'spoken': 'Aborted'}) smach.StateMachine.add("NAV_TO_START", states.NavigateToSymbolic( robot, {cabinet: "in_front_of"}, cabinet), transitions={ 'arrived': 'INSPECT_SHELVES', 'unreachable': 'FORCE_ROTATE', 'goal_not_defined': 'INSPECT_SHELVES' }) smach.StateMachine.add("FORCE_ROTATE", ForceRotate(robot, 0.5, 2.0, 30.0), transitions={ 'done': "NAV_TO_START", 'timedout': "INSPECT_SHELVES" }) # smach.StateMachine.add("RESET_ED", # states.ResetED(robot), # transitions={'done' :'INSPECT_SHELVES'}) smach.StateMachine.add("INSPECT_SHELVES", InspectShelves(robot, OBJECT_SHELVES), transitions={ 'succeeded': 'EXPORT_PDF', 'nothing_found': 'EXPORT_PDF', 'failed': 'EXPORT_PDF' }) @smach.cb_interface(outcomes=["exported"]) def export_to_pdf(userdata=None): global DETECTED_OBJECTS_WITH_PROBS entities = [e[0] for e in DETECTED_OBJECTS_WITH_PROBS] # Export images (Only best MAX_NUM_ENTITIES_IN_PDF) # pdf.entities_to_pdf(robot.ed, entities[:MAX_NUM_ENTITIES_IN_PDF], "tech_united_manipulation_challenge") return "exported" smach.StateMachine.add('EXPORT_PDF', smach.CBState(export_to_pdf), transitions={'exported': 'RANGE_ITERATOR'}) # Begin setup iterator range_iterator = smach.Iterator( outcomes=['succeeded', 'failed'], #Outcomes of the iterator state input_keys=[], output_keys=[], it=lambda: range(5), it_label='index', exhausted_outcome='succeeded' ) #The exhausted argument should be set to the preffered state machine outcome with range_iterator: single_item = ManipRecogSingleItem( robot, ds.VariableDesignator(placed_items, [Entity], name="placed_items")) smach.Iterator.set_contained_state( 'SINGLE_ITEM', single_item, loop_outcomes=['succeeded', 'failed']) smach.StateMachine.add('RANGE_ITERATOR', range_iterator, { 'succeeded': 'AT_END', 'failed': 'Aborted' }) # End setup iterator smach.StateMachine.add('AT_END', states.Say(robot, "Goodbye"), transitions={'spoken': 'Done'}) ds.analyse_designators(sm, "manipulation") return sm
def setup_statemachine(robot): sm = smach.StateMachine(outcomes=['Done', 'Aborted']) with sm: # Start challenge via StartChallengeRobust smach.StateMachine.add("START_CHALLENGE_ROBUST", states.StartChallengeRobust( robot, challenge_knowledge.starting_point, use_entry_points=True), transitions={ "Done": "SAY_GOTO_TARGET2", "Aborted": "SAY_GOTO_TARGET2", "Failed": "SAY_GOTO_TARGET2" }) smach.StateMachine.add( 'SAY_GOTO_TARGET1', states.Say(robot, [ "I will go to target 1 now", "I will now go to target 1", "Lets go to target 1", "Going to target 1" ], block=False), transitions={'spoken': 'GOTO_TARGET1'}) ###################################################################################################################################################### # # TARGET 1 # ###################################################################################################################################################### smach.StateMachine.add('GOTO_TARGET1', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.target1), challenge_knowledge.target1_radius1), transitions={ 'arrived': 'SAY_TARGET1_REACHED', 'unreachable': 'RESET_ED_TARGET1', 'goal_not_defined': 'RESET_ED_TARGET1' }) smach.StateMachine.add( 'SAY_TARGET1_REACHED', states.Say(robot, [ "Reached target 1", "I have arrived at target 1", "I am now at target 1" ], block=True), transitions={'spoken': 'SAY_GOTO_TARGET3'}) smach.StateMachine.add('RESET_ED_TARGET1', states.ResetED(robot), transitions={'done': 'GOTO_TARGET1_BACKUP'}) smach.StateMachine.add('GOTO_TARGET1_BACKUP', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.target1), challenge_knowledge.target1_radius2), transitions={ 'arrived': 'SAY_TARGET1_REACHED', 'unreachable': 'TIMEOUT1', 'goal_not_defined': 'TIMEOUT1' }) smach.StateMachine.add('TIMEOUT1', checkTimeOut( robot, challenge_knowledge.time_out_seconds), transitions={ 'not_yet': 'GOTO_TARGET1', 'time_out': 'SAY_TARGET1_FAILED' }) # Should we mention that we failed??? smach.StateMachine.add( 'SAY_TARGET1_FAILED', states.Say(robot, [ "I am not able to reach target 1", "I cannot reach target 1", "Target 1 is unreachable" ], block=True), transitions={'spoken': 'SAY_GOTO_TARGET3'}) ###################################################################################################################################################### # # TARGET 2 # ###################################################################################################################################################### smach.StateMachine.add( 'SAY_GOTO_TARGET2', states.Say(robot, [ "I will go to target 2 now", "I will now go to target 2", "Lets go to target 2", "Going to target 2" ], block=False), transitions={'spoken': 'GOTO_TARGET2_PRE'}) smach.StateMachine.add( 'GOTO_TARGET2_PRE', states.NavigateToWaypoint( robot, EntityByIdDesignator(robot, id=challenge_knowledge.target2_pre), challenge_knowledge.target2_pre_radius1, EntityByIdDesignator(robot, id=challenge_knowledge.target2)), transitions={ 'arrived': 'GOTO_TARGET2', 'unreachable': 'TIMEOUT2', 'goal_not_defined': 'TIMEOUT2' }) smach.StateMachine.add('GOTO_TARGET2', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.target2), challenge_knowledge.target2_radius1), transitions={ 'arrived': 'SAY_TARGET2_REACHED', 'unreachable': 'DETERMINE_OBJECT', 'goal_not_defined': 'DETERMINE_OBJECT' }) smach.StateMachine.add( 'DETERMINE_OBJECT', DetermineObject(robot, challenge_knowledge.target2, challenge_knowledge.target2_obstacle_radius), transitions={ 'done': 'GOTO_TARGET2_AGAIN', 'timeout': 'GOTO_TARGET2_AGAIN' }) smach.StateMachine.add('GOTO_TARGET2_AGAIN', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.target2), challenge_knowledge.target2_radius1), transitions={ 'arrived': 'SAY_TARGET2_REACHED', 'unreachable': 'RESET_ED_TARGET2', 'goal_not_defined': 'RESET_ED_TARGET2' }) smach.StateMachine.add( 'SAY_TARGET2_REACHED', states.Say(robot, [ "Reached target 2", "I have arrived at target 2", "I am now at target 2" ], block=True), transitions={'spoken': 'SAY_GOTO_TARGET1'}) smach.StateMachine.add('RESET_ED_TARGET2', states.ResetED(robot), transitions={'done': 'GOTO_TARGET2_BACKUP'}) smach.StateMachine.add('GOTO_TARGET2_BACKUP', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.target2), challenge_knowledge.target2_radius2), transitions={ 'arrived': 'SAY_TARGET2_REACHED', 'unreachable': 'TIMEOUT2', 'goal_not_defined': 'TIMEOUT2' }) smach.StateMachine.add('TIMEOUT2', checkTimeOut( robot, challenge_knowledge.time_out_seconds), transitions={ 'not_yet': 'GOTO_TARGET2_PRE', 'time_out': 'SAY_TARGET2_FAILED' }) smach.StateMachine.add( 'SAY_TARGET2_FAILED', states.Say(robot, [ "I am unable to reach target 2", "I cannot reach target 2", "Target 2 is unreachable" ], block=True), transitions={'spoken': 'SAY_GOTO_TARGET1'}) ###################################################################################################################################################### # # TARGET 3 # ###################################################################################################################################################### smach.StateMachine.add( 'SAY_GOTO_TARGET3', states.Say(robot, [ "I will go to target 3 now", "I will now go to target 3", "Lets go to target 3", "Going to target 3" ], block=False), transitions={'spoken': 'GOTO_TARGET3'}) smach.StateMachine.add('GOTO_TARGET3', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.target3), challenge_knowledge.target3_radius1), transitions={ 'arrived': 'SAY_TARGET3_REACHED', 'unreachable': 'RESET_ED_TARGET3', 'goal_not_defined': 'RESET_ED_TARGET3' }) smach.StateMachine.add( 'SAY_TARGET3_REACHED', states.Say(robot, [ "Reached target 3", "I have arrived at target 3", "I am now at target 3" ], block=True), transitions={'spoken': 'TURN'}) smach.StateMachine.add('RESET_ED_TARGET3', states.ResetED(robot), transitions={'done': 'GOTO_TARGET3_BACKUP'}) smach.StateMachine.add('GOTO_TARGET3_BACKUP', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.target3), challenge_knowledge.target3_radius2), transitions={ 'arrived': 'SAY_TARGET3_REACHED', 'unreachable': 'TIMEOUT3', 'goal_not_defined': 'TIMEOUT3' }) smach.StateMachine.add('TIMEOUT3', checkTimeOut( robot, challenge_knowledge.time_out_seconds), transitions={ 'not_yet': 'GOTO_TARGET3', 'time_out': 'SAY_TARGET3_FAILED' }) # Should we mention that we failed??? smach.StateMachine.add( 'SAY_TARGET3_FAILED', states.Say(robot, [ "I am unable to reach target 3", "I cannot reach target 3", "Target 3 is unreachable" ], block=True), transitions={'spoken': 'TURN'}) ###################################################################################################################################################### # # Follow waiter # ###################################################################################################################################################### smach.StateMachine.add('TURN', Turn(robot, challenge_knowledge.rotation), transitions={'turned': 'SAY_STAND_IN_FRONT'}) smach.StateMachine.add( 'SAY_STAND_IN_FRONT', states.Say(robot, "Please stand in front of me!", block=True, look_at_standing_person=True), transitions={'spoken': 'FOLLOW_WITH_DOOR_CHECK'}) # TODO: Fix concurrence door_id_designator = VariableDesignator( challenge_knowledge.target_door_1) open_door_wp1_des = VariableDesignator(resolve_type=str) open_door_wp2_des = VariableDesignator(resolve_type=str) cc = smach.Concurrence( ['stopped', 'no_operator', 'lost_operator'], default_outcome='no_operator', child_termination_cb=lambda so: True, outcome_map={ 'stopped': { 'FOLLOW_OPERATOR': 'stopped' }, # 'stopped': {'FOLLOW_OPERATOR': 'stopped', 'DETERMINE_DOOR': 'door_found'}, 'no_operator': { 'FOLLOW_OPERATOR': 'no_operator' }, # 'no_operator': {'FOLLOW_OPERATOR': 'no_operator', 'DETERMINE_DOOR': 'door_found'}, # 'lost_operator': {'FOLLOW_OPERATOR': 'lost_operator', 'DETERMINE_DOOR': 'preempted'}, 'lost_operator': { 'FOLLOW_OPERATOR': 'lost_operator' } }) with cc: smach.Concurrence.add('FOLLOW_OPERATOR', states.FollowOperator(robot, replan=True)) smach.Concurrence.add('DETERMINE_DOOR', DetermineDoor(robot, door_id_designator)) smach.StateMachine.add('FOLLOW_WITH_DOOR_CHECK', cc, transitions={ 'no_operator': 'FOLLOW_WITH_DOOR_CHECK', 'stopped': 'SAY_SHOULD_I_RETURN', 'lost_operator': 'SAY_SHOULD_I_RETURN' }) # smach.StateMachine.add( 'FOLLOW_OPERATOR', states.FollowOperator(robot, replan=True), transitions={ 'no_operator':'SAY_SHOULD_I_RETURN', 'stopped' : 'SAY_SHOULD_I_RETURN', 'lost_operator' : 'SAY_SHOULD_I_RETURN'}) smach.StateMachine.add('SAY_SHOULD_I_RETURN', states.Say(robot, "Should I return to target 3?", look_at_standing_person=True), transitions={'spoken': 'HEAR_SHOULD_I_RETURN'}) smach.StateMachine.add('HEAR_SHOULD_I_RETURN', states.HearOptions(robot, ["yes", "no"]), transitions={ 'no_result': 'SAY_STAND_IN_FRONT', "yes": "SELECT_WAYPOINTS", "no": "SAY_STAND_IN_FRONT" }) smach.StateMachine.add('SELECT_WAYPOINTS', SelectWaypoints(door_id_designator, open_door_wp1_des, open_door_wp2_des), transitions={'done': 'SAY_GOBACK_ARENA'}) ###################################################################################################################################################### # # RETURN TO ARENA DOOR # ###################################################################################################################################################### smach.StateMachine.add( 'SAY_GOBACK_ARENA', states.Say(robot, [ "I will go back to the arena", "I will return to the arena", "Lets return to the arena", "Going back to the arena", "Returning to the arena" ], block=False), transitions={'spoken': 'GOTO_ARENA_DOOR'}) smach.StateMachine.add( 'GOTO_ARENA_DOOR', states.NavigateToWaypoint( robot, EdEntityDesignator(robot, id_designator=door_id_designator), challenge_knowledge.target_door_radius), transitions={ 'arrived': 'ARENA_DOOR_REACHED', 'unreachable': 'RESET_ED_ARENA_DOOR', 'goal_not_defined': 'RESET_ED_ARENA_DOOR' }) smach.StateMachine.add('ARENA_DOOR_REACHED', states.Say(robot, [ "I am at the door of the arena", "I have arrived at the door of the arena", "I am now at the door of the arena" ], block=True), transitions={'spoken': 'SAY_OPEN_DOOR'}) smach.StateMachine.add('RESET_ED_ARENA_DOOR', states.ResetED(robot), transitions={'done': 'GOTO_ARENA_DOOR_BACKUP'}) smach.StateMachine.add( 'GOTO_ARENA_DOOR_BACKUP', states.NavigateToWaypoint( robot, EdEntityDesignator(robot, id_designator=door_id_designator), challenge_knowledge.target_door_radius), transitions={ 'arrived': 'ARENA_DOOR_REACHED', 'unreachable': 'TIMEOUT_ARENA_DOOR', 'goal_not_defined': 'TIMEOUT_ARENA_DOOR' }) smach.StateMachine.add('TIMEOUT_ARENA_DOOR', checkTimeOut( robot, challenge_knowledge.time_out_seconds_door), transitions={ 'not_yet': 'GOTO_ARENA_DOOR', 'time_out': 'SAY_GOTO_ARENA_DOOR_FAILED' }) smach.StateMachine.add('SAY_GOTO_ARENA_DOOR_FAILED', states.Say(robot, [ "I am unable to reach the arena door", "I cannot reach the arena door", "The arena door is unreachable" ], block=True), transitions={'spoken': 'Done'}) ###################################################################################################################################################### # # Opening Door # ###################################################################################################################################################### smach.StateMachine.add( 'OPEN_DOOR', states.OpenDoorByPushing( robot, EdEntityDesignator(robot, id_designator=open_door_wp1_des), EdEntityDesignator(robot, id_designator=open_door_wp2_des)), transitions={ 'succeeded': 'SAY_RETURN_TARGET3', 'failed': 'TIMEOUT_ARENA_DOOR_OPENING' }) smach.StateMachine.add( 'SAY_OPEN_DOOR', states.Say(robot, [ "I am going to open the door", "Going to open the door of the arena", "Door, open sesame" ], block=True), transitions={'spoken': 'OPEN_DOOR'}) smach.StateMachine.add( 'SAY_OPEN_DOOR_AGAIN', states.Say(robot, [ "I failed to open the door. I will try it again", "Let me try again to open the door" ], block=True), transitions={'spoken': 'OPEN_DOOR'}) smach.StateMachine.add('TIMEOUT_ARENA_DOOR_OPENING', checkTimeOut( robot, challenge_knowledge.time_out_seconds), transitions={ 'not_yet': 'SAY_OPEN_DOOR_AGAIN', 'time_out': 'SAY_OPEN_DOOR_FAILED' }) smach.StateMachine.add( 'SAY_OPEN_DOOR_FAILED', states.Say(robot, [ "I was not able to open the door. I am done with this challange", "I was not able to open the door. I am done with this challange" ], block=True), transitions={'spoken': 'Done'}) ###################################################################################################################################################### # # RETURN TO TARGET 3 # ###################################################################################################################################################### smach.StateMachine.add( 'SAY_RETURN_TARGET3', states.Say(robot, [ "I will go back to target 3 now", "I will return to target 3", "Lets go to target 3 again", "Going to target 3, again" ], block=False), transitions={'spoken': 'RETURN_TARGET3'}) smach.StateMachine.add( 'RETURN_TARGET3', states.NavigateToWaypoint( robot, EntityByIdDesignator(robot, id=challenge_knowledge.target4), challenge_knowledge.target4_radius1), transitions={ 'arrived': 'SAY_TARGET3_RETURN_REACHED', 'unreachable': 'RESET_ED_RETURN_TARGET3', 'goal_not_defined': 'RESET_ED_RETURN_TARGET3' }) smach.StateMachine.add( 'SAY_TARGET3_RETURN_REACHED', states.Say(robot, [ "Reached target 3 again", "I have arrived at target 3 again", "I am now at target 3 again" ], block=True), transitions={'spoken': 'SAY_GOTO_EXIT'}) smach.StateMachine.add( 'RESET_ED_RETURN_TARGET3', states.ResetED(robot), transitions={'done': 'GOTO_RETURN_TARGET3_BACKUP'}) smach.StateMachine.add('GOTO_RETURN_TARGET3_BACKUP', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.target4), challenge_knowledge.target4_radius2), transitions={ 'arrived': 'SAY_TARGET3_RETURN_REACHED', 'unreachable': 'TIMEOUT3_RETURN', 'goal_not_defined': 'TIMEOUT3_RETURN' }) smach.StateMachine.add('TIMEOUT3_RETURN', checkTimeOut( robot, challenge_knowledge.time_out_seconds), transitions={ 'not_yet': 'RETURN_TARGET3', 'time_out': 'SAY_RETURN_TARGET3_FAILED' }) # Should we mention that we failed??? smach.StateMachine.add( 'SAY_RETURN_TARGET3_FAILED', states.Say(robot, [ "I am unable to reach target 3 again", "I cannot reach target 3 again", "Target 3 is unreachable" ], block=True), transitions={'spoken': 'SAY_GOTO_EXIT'}) ###################################################################################################################################################### # # TARGET EXIT # ###################################################################################################################################################### smach.StateMachine.add( 'SAY_GOTO_EXIT', states.Say(robot, [ "I will move to the exit now. See you guys later!", "I am done with this challenge. Going to the exit" ], block=False), transitions={'spoken': 'GO_TO_EXIT'}) # Amigo goes to the exit (waypoint stated in knowledge base) smach.StateMachine.add('GO_TO_EXIT', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.exit1), radius=0.6), transitions={ 'arrived': 'AT_END', 'unreachable': 'RESET_ED_EXIT', 'goal_not_defined': 'RESET_ED_EXIT' }) smach.StateMachine.add('RESET_ED_EXIT', states.ResetED(robot), transitions={'done': 'GO_TO_EXIT_BACKUP'}) smach.StateMachine.add('GO_TO_EXIT_BACKUP', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.exit2), radius=0.6), transitions={ 'arrived': 'AT_END', 'unreachable': 'RESET_ED_EXIT2', 'goal_not_defined': 'RESET_ED_EXIT2' }) smach.StateMachine.add('RESET_ED_EXIT2', states.ResetED(robot), transitions={'done': 'GO_TO_EXIT_BACKUP2'}) smach.StateMachine.add('GO_TO_EXIT_BACKUP2', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.exit3), radius=0.6), transitions={ 'arrived': 'GO_TO_EXIT_BACKUP3', 'unreachable': 'RESET_ED_EXIT3', 'goal_not_defined': 'RESET_ED_EXIT3' }) smach.StateMachine.add('RESET_ED_EXIT3', states.ResetED(robot), transitions={'done': 'GO_TO_EXIT_BACKUP3'}) smach.StateMachine.add('GO_TO_EXIT_BACKUP3', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.exit4), radius=0.6), transitions={ 'arrived': 'AT_END', 'unreachable': 'AT_END', 'goal_not_defined': 'AT_END' }) smach.StateMachine.add('AT_END', states.Say(robot, "Goodbye"), transitions={'spoken': 'Done'}) analyse_designators(sm, "navigation") return sm