def generate_goal_based_task_sm(goal_formulator, input_keys):
    assert isinstance(goal_formulator, FormulateGoal)

    sm = StateMachine(**gen_task_sig(input_keys))
    with sm:
        StateMachine.add_auto("FORMULATE_GOAL", goal_formulator, ['succeeded'])
        StateMachine.add("EXECUTE_GOAL", get_execute_sm())
    return sm
Exemple #2
0
def main():
    smach_states = rospy.get_param("smach_states")
    # The autonomy_sm handles the full autonomy sequence for the competition run
    autonomy_sm = StateMachine(outcomes=['succeeded','aborted','preempted'])
    with autonomy_sm:
        # TODO: create localization state. might just be a MonitorState

        # Sequentially add all the states from the config file to the state machine,
        # where state i transitions to state i+1
        names = []
        for i in range(len(smach_states)):
            state = smach_states[i]
            name, action_state = create_action_state(state)

            # If the state name is already in the state machine, add the new
            # one with increasing numbers
            if name in names:
                name = name+"2"
                while name in names:
                    name = name[:-1]+str(int(name[-1])+1) # increase num
            names.append(name)

            StateMachine.add_auto(name, action_state, connector_outcomes=['succeeded'])

        # StateMachine.add()

    # Create the concurrence contained for the fully autonomy sequence. This
    # runs the state machine for the competition run.  It also concurrently runs
    # a state with a timer counting down from 10 minutes and a state that listens
    # to the /click/start_button topic. If either of these are triggered, it will
    # end autonomy and place us into the teleop state.
    # TODO: add 10 minute competition timer state
    autonomy_concurrence = Concurrence(outcomes=['enter_teleop', 'stay', 'aborted'],
                            default_outcome='enter_teleop',
                            child_termination_cb=autonomy_child_term_cb,
                            outcome_cb=autonomy_out_cb)
    with autonomy_concurrence:
        # state that runs full autonomy state machine
        Concurrence.add('AUTONOMY', autonomy_sm)
        # state that listens for toggle message
        Concurrence.add('TOGGLE_LISTEN', MonitorState('/click/start_button', Empty, monitor_cb))

    # Top level state machine, containing the autonomy and teleop machines.
    top_sm = StateMachine(outcomes=['DONE'])
    with top_sm:
        StateMachine.add('TELEOP_MODE', MonitorState('/click/start_button', Empty, monitor_cb), transitions={'invalid':'AUTONOMY_MODE', 'valid':'TELEOP_MODE', 'preempted':'AUTONOMY_MODE'})
        StateMachine.add('AUTONOMY_MODE', autonomy_concurrence,
          transitions={'enter_teleop':'TELEOP_MODE', 'stay':'AUTONOMY_MODE', 'aborted':'DONE'})

        #StateMachine.add('TELEOP_MODE', MonitorState('/click/start_button', Empty, monitor_cb),
        #  transitions={'invalid':'DONE', 'valid':'TELEOP_MODE', 'preempted':'DONE'})

    sis = IntrospectionServer('smach_introspection_server', top_sm, '/COMPETITION_SMACH')
    sis.start()
    top_sm.execute()
    rospy.spin()
    sis.stop()
def select_ith_auto(name, from_key, to_key):
    class SelectIth(State):
        def __init__(self):
            State.__init__(self, outcomes=["succeeded"], input_keys=[from_key, "index"], output_keys=[to_key])

        def execute(self, ud):
            ud[to_key] = ud[from_key][ud["index"]]
            return "succeeded"

    StateMachine.add_auto(name, SelectIth(), ["succeeded"])
def inject_userdata_auto(name, output_key, value):
    class InjectUserdata(State):
        def __init__(self, ):
            State.__init__(self, outcomes=['succeeded'], output_keys=[output_key])
            self.output_key = output_key

        def execute(self, userdata):
            userdata[output_key] = value
            return 'succeeded'

    StateMachine.add_auto(name, InjectUserdata(), ["succeeded"])
def break_dict_out_auto(name, input_key, output_keys):
    class BreakDataOut(State):
        def __init__(self, ):
            State.__init__(self, outcomes=['succeeded'], input_keys=[input_key], output_keys=output_keys)
            self.output_keys = output_keys
            self.input_key = input_key

        def execute(self, userdata):
            for key in self.output_keys:
                userdata[key] = userdata[self.input_key][key]
            return 'succeeded'

    StateMachine.add_auto(name, BreakDataOut(), ["succeeded"])
Exemple #6
0
    def test_sequence(self):
        """Test adding a sequence of states."""
        sm = StateMachine(['succeeded', 'aborted', 'preempted', 'done'])
        with sm:
            StateMachine.add_auto(
                'FIRST',
                SimpleActionState('reference_action', TestAction, goal=g1),
                ['succeeded'])
            StateMachine.add_auto(
                'SECOND',
                SimpleActionState('reference_action', TestAction, goal=g1),
                ['succeeded'])
            StateMachine.add(
                'THIRD',
                SimpleActionState('reference_action', TestAction, goal=g1),
                {'succeeded': 'done'})
        outcome = sm.execute()

        assert outcome == 'done'
Exemple #7
0
def obtain_confirmation():
    obtain_confirmation_wrist_sm = StateMachine(outcomes=['succeeded', 'aborted'])
    with obtain_confirmation_wrist_sm:
        if util.in_sim():
            control_flow.transition_to("BYPASS", "succeeded")
        else:
            StateMachine.add_auto('ASK_FOR_TAP', Say("Can you shake my wrist to confirm?"), ["succeeded"])
            StateMachine.add("AWAIT_TAP", states.WaitForStart(),{"signalled": "succeeded", "not_signalled": "aborted"})
    obtain_confirmation_sm = StateMachine(outcomes=['confirmed', 'denied', 'aborted'])
    with obtain_confirmation_sm:
        if util.in_sim():
            control_flow.transition_to("BYPASS", "confirmed")
        else:
            StateMachine.add('TRANSCRIBE_SPEECH', TranscribeSpeech(5),
                             transitions={"succeeded": "CHECK_IF_AFFIRMATIVE", "aborted": "OBTAIN_CONFIRMATION_WRIST" })
	    StateMachine.add("OBTAIN_CONFIRMATION_WRIST", obtain_confirmation_wrist_sm, transitions={"succeeded": "confirmed"})
            StateMachine.add('CHECK_IF_AFFIRMATIVE', CheckIfAffirmative(),
                             transitions={"affirmative": "confirmed", "negative": "denied",
                                          "indeterminate": 'denied'})
    return obtain_confirmation_sm
def get_execute_sm():
    # This state machine is large, so we will only instantiate once and point references to one copy
    global shared_execute_sm
    if shared_execute_sm:
        return shared_execute_sm
    shared_execute_sm = StateMachine(outcomes=["succeeded", "preempted", "aborted"], input_keys=["goal"],
                                     output_keys=["msg_for_operator"])
    with shared_execute_sm:
        control_flow.inject_userdata_auto("_SET_DEFAULT_MSG_FOR_OPERATOR", "msg_for_operator", "")
        repeat_state = control_flow.RepeatN(0)
        StateMachine.add_auto("RESET_REPEAT", control_flow.ResetRepeat(repeat_state), ["succeeded"])
        StateMachine.add("EXECUTE_GOAL", states.ExecuteGoal(),
                         transitions={"preempted": "RECOVERY_REPEAT_GATE",
                                      "aborted": "RECOVERY_REPEAT_GATE"})
        StateMachine.add("EXECUTE_RECOVERY_GOAL", states.ExecuteGoal(),
                         transitions={"preempted": "RECOVERY_REPEAT_GATE",
                                      "aborted": "RECOVERY_REPEAT_GATE"})
        StateMachine.add("RECOVERY_REPEAT_GATE", repeat_state,
                         transitions={"repeat": "RECOVER", "done": "aborted"})
        StateMachine.add("RECOVER", get_shared_recover_from_failure_sm(),
                         transitions={'succeeded': "EXECUTE_RECOVERY_GOAL"})
    return shared_execute_sm
Exemple #9
0
def rgoap_path_to_smach_container(start_node):
    sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])

    node = start_node
    with sm:
        while not node.is_goal():  # skipping the goal node at the end
            next_node = node.parent_node()

            if isinstance(node.action, SMACHStateWrapperAction):
                # TODO: when smach executes SMACHStateWrapperActions, their action.check_freeform_context() is never called!
                StateMachine.add_auto(
                    '%s_%X' % (node.action.__class__.__name__, id(node)),
                    node.action.state, ['succeeded'],
                    remapping=node.action.get_remapping())
                node.action.translate_worldstate_to_userdata(
                    next_node.worldstate, sm.userdata)
            else:
                StateMachine.add_auto(
                    '%s_%X' % (node.action.__class__.__name__, id(node)),
                    RGOAPNodeWrapperState(node), ['succeeded'])

            node = next_node

    return sm
Exemple #10
0
def main():
    # The autonomy_sm handles the full autonomy sequence for the competition run
    autonomy_sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
    with autonomy_sm:
        # Add states for setup (these run once each)
        for i in range(len(setup_sequence)):
            name, action_type = get_name_and_type(setup_sequence[i])

            func_to_run = None
            if action_type == 'localize':
                func_to_run = localization_cb
            elif action_type == 'move':
                # action instead of function
                func_to_run = 'action'
            else:
                raise NotImplementedError('Only localize is supported rn')

            # if this is the last setup state
            if i == len(loop_sequence) - 1:
                # tie the last setup state to the first loop state
                first_loop_name = get_name_and_type(loop_sequence[0])
                if func_to_run == 'action':
                    action_state = create_action_state(name, action_type)
                    StateMachine.add(
                        name,
                        action_state,
                        transitions={'succeeded': first_loop_name})
                else:
                    StateMachine.add(name, CBState(func_to_run),
                                     {'succeeded': first_loop_name})
            else:
                # add_auto adds this to the state machine and automatically
                # ties the next and previous states together
                if func_to_run == 'action':
                    action_state = create_action_state(name, action_type)
                    StateMachine.add_auto(name,
                                          action_state,
                                          connector_outcomes=['succeeded'])
                else:
                    StateMachine.add_auto(name,
                                          CBState(func_to_run),
                                          connector_outcomes=['succeeded'])

        names = []  # store names to check and handle dupes
        for i in range(len(loop_sequence)):
            name, action_type = get_name_and_type(loop_sequence[i])
            action_state = create_action_state(name, action_type)

            # "move" states all have the same name and smach requires unique
            # state names, so check and add a number to the name if needed
            if name in names:
                name = name + "2"
                while name in names:
                    name = name[:-1] + str(
                        int(name[-1]) + 1)  # increase num by 1
            names.append(name)

            if i == len(loop_sequence) - 1:
                # tie the last state to the first one, so they just keep looping
                StateMachine.add(name,
                                 action_state,
                                 transitions={'succeeded': names[0]})
            else:
                # add_auto adds this to the state machine and automatically
                # ties the next and previous states together
                StateMachine.add_auto(name,
                                      action_state,
                                      connector_outcomes=['succeeded'])

    # Create the concurrence container for the fully autonomy sequence. This
    # runs the state machine for the competition run.  It also concurrently runs
    # a state that listens to the /click_start_button topic. If this is
    # triggered place us into the teleop state.
    autonomy_concurrence = Concurrence(
        outcomes=['enter_teleop', 'stay', 'aborted'],
        default_outcome='enter_teleop',
        child_termination_cb=autonomy_child_term_cb,
        outcome_cb=autonomy_out_cb)

    with autonomy_concurrence:
        # state that runs full autonomy state machine
        Concurrence.add('AUTONOMY', autonomy_sm)
        # state that listens for toggle message
        Concurrence.add(
            'TOGGLE_LISTEN',
            MonitorState('/click_start_button', Empty, start_btn_cb))

    teleop_concurrence = Concurrence(
        outcomes=['enter_autonomy', 'stay', 'done'],
        default_outcome='enter_autonomy',
        child_termination_cb=teleop_child_term_cb,
        outcome_cb=teleop_out_cb)

    with teleop_concurrence:
        Concurrence.add(
            'TOGGLE_LISTEN',
            MonitorState('/click_start_button', Empty, start_btn_cb))
        Concurrence.add(
            'EXIT_LISTEN',
            MonitorState('/click_select_button', Empty, select_btn_cb))

    # Top level state machine, containing the autonomy and teleop machines.
    top_sm = StateMachine(outcomes=['DONE'])
    with top_sm:
        StateMachine.add('TELEOP_MODE',
                         teleop_concurrence,
                         transitions={
                             'enter_autonomy': 'AUTONOMY_MODE',
                             'stay': 'TELEOP_MODE',
                             'done': 'DONE'
                         })
        StateMachine.add('AUTONOMY_MODE',
                         autonomy_concurrence,
                         transitions={
                             'enter_teleop': 'TELEOP_MODE',
                             'stay': 'AUTONOMY_MODE',
                             'aborted': 'DONE'
                         })

        #StateMachine.add('TELEOP_MODE', MonitorState('/click_start_button', Empty, monitor_cb),
        #  transitions={'invalid':'DONE', 'valid':'TELEOP_MODE', 'preempted':'DONE'})

    sis = IntrospectionServer('smach_introspection_server', top_sm,
                              '/COMPETITION_SMACH')
    sis.start()
    top_sm.execute()
    rospy.spin()
    sis.stop()