Пример #1
0
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
Пример #2
0
                                   '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()
Пример #3
0
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
Пример #4
0

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()

Пример #5
0
    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})
Пример #6
0
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
Пример #7
0
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