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
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"])
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'
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
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
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()
