def add(label, state, transitions = None, autonomy = None, remapping = None):
"""
Add a state to the opened state machine.
@type label: string
@param label: The label of the state being added.
@param state: An instance of a class implementing the L{State} interface.
@param transitions: A dictionary mapping state outcomes to other state
labels or container outcomes.
@param autonomy: A dictionary mapping state outcomes to their required
autonomy level
@param remapping: A dictionary mapping local userdata keys to userdata
keys in the container.
"""
self = StateMachine._currently_opened_container()
# add loopback transition to loopback states
if isinstance(state, LoopbackState):
transitions[LoopbackState._loopback_name] = label
autonomy[LoopbackState._loopback_name] = -1
self._ordered_states.append(state)
state.name = label
state.transitions = transitions
state.autonomy = autonomy
state._parent = self
StateMachine.add(label, state, transitions, remapping)
self._autonomy[label] = autonomy
def add(label, state, transitions=None, autonomy=None, remapping=None):
"""
Add a state to the opened state machine.
@type label: string
@param label: The label of the state being added.
@param state: An instance of a class implementing the L{State} interface.
@param transitions: A dictionary mapping state outcomes to other state
labels or container outcomes.
@param autonomy: A dictionary mapping state outcomes to their required
autonomy level. Not relevant for this class.
@param remapping: A dictrionary mapping local userdata keys to userdata
keys in the container.
"""
self = StateMachine._currently_opened_container()
# add loopback transition to loopback states
if isinstance(state, LoopbackState):
transitions[LoopbackState._loopback_name] = label
StateMachine.add(label, state, transitions, remapping)
state.name = label
state.transitions = transitions
state.autonomy = None
state._parent = self
state._mute = True
def main():
rospy.init_node("smach_example_state_machine")
# Create a SMACH state machine
sm = StateMachine(outcomes=["outcome4", "outcome5"])
# Open the container
with sm:
# Add states to the container
StateMachine.add("FOO",
Foo(),
transitions={
"outcome1": "BAR",
"outcome2": "outcome4"
})
StateMachine.add("BAR", Bar(), transitions={"outcome2": "FOO"})
# Create and start the introspection server
sis = smach_ros.IntrospectionServer("my_smach_introspection_server", sm,
"/SM_ROOT")
sis.start()
# Execute SMACH plan
outcome = sm.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()
def sm_with_userdata(self):
def execute(name, outcome):
self._trace.append(name)
return outcome
sm = ResumableStateMachine(outcomes=['ok'], input_keys=['key'])
with sm:
StateMachine.add(
'A',
CBState(lambda ud: execute('A', ud['key']),
outcomes=['B'],
io_keys=['key']), {'B': 'B'})
StateMachine.add(
'B',
CBState(lambda ud: execute('B', ud['key']),
outcomes=['ok'],
input_keys=['key']))
return sm
def __init__(self):
rospy.init_node("speech")
rospy.on_shutdown(self.shutdown)
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait up to 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'base_link'
nav_goal.target_pose.header.stamp = rospy.Time.now()
q_angle = quaternion_from_euler(0, 0,0, axes='sxyz')
q = Quaternion(*q_angle)
nav_goal.target_pose.pose =Pose( Point(0.3,0,0),q)
move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(60.0),
server_wait_timeout=rospy.Duration(60.0))
smach_top=StateMachine(outcomes=['succeeded','aborted','preempted',"valid","invalid"])
with smach_top:
StateMachine.add("Wait_4_Statr", MonitorState("task_comming", xm_Task, self.start_cb),
transitions={'invalid':'NAV',
'valid':'Wait_4_Statr',
'preempted':'NAV'})
StateMachine.add("NAV", move_base_state, transitions={"succeeded":"Wait_4_Stop","aborted":"NAV","preempted":"Wait_4_Stop"})
StateMachine.add("Wait_4_Stop",MonitorState("task_comming", xm_Task, self.stop_cb),
transitions={'invalid':'',
'valid':'Wait_4_Stop',
'preempted':''})
smach_top.execute()
def simple_sm(self):
def execute(name):
self._trace.append(name)
return 'ok'
sm = ResumableStateMachine(outcomes=['ok'])
with sm:
StateMachine.add('A',
CBState(lambda ud: execute('A'), outcomes=['ok']),
{'ok': 'B'})
StateMachine.add('B',
CBState(lambda ud: execute('B'), outcomes=['ok']),
{'ok': 'C'})
StateMachine.add('C',
CBState(lambda ud: execute('C'), outcomes=['ok']))
return sm
def nested_sm(self):
def execute(name, outcome):
self._trace.append(name)
return outcome
resumable_inner_sm = ResumableStateMachine(outcomes=['ok'])
inner_sm = StateMachine(outcomes=['ok'])
outer_sm = ResumableStateMachine(outcomes=['ok'])
with outer_sm:
StateMachine.add('A', resumable_inner_sm, {'ok': 'B'})
StateMachine.add('B', inner_sm, {'ok': 'C'})
StateMachine.add(
'C', CBState(lambda ud: execute('C', 'ok'), outcomes=['ok']))
with resumable_inner_sm:
StateMachine.add(
'A', CBState(lambda ud: execute('A.A', 'ok'), outcomes=['ok']),
{'ok': 'B'})
StateMachine.add(
'B', CBState(lambda ud: execute('A.B', 'ok'), outcomes=['ok']))
with inner_sm:
StateMachine.add(
'A', CBState(lambda ud: execute('B.A', 'ok'), outcomes=['ok']),
{'ok': 'B'})
StateMachine.add(
'B', CBState(lambda ud: execute('B.B', 'ok'), outcomes=['ok']))
return outer_sm
def make_sm(self):
inner_sm_1 = ResumableStateMachine(outcomes=['ok'],
input_keys=['key'],
output_keys=['key'])
inner_sm_2 = ResumableStateMachine(outcomes=['ok'],
input_keys=['key'],
output_keys=['key'])
outer_sm = ResumableStateMachine(outcomes=['ok'],
input_keys=['key'],
output_keys=['key'])
def execute(ud):
ud['key'] += 1
return 'ok'
def make_state():
return CBState(execute, outcomes=['ok'], io_keys=['key'])
with outer_sm:
StateMachine.add('A', inner_sm_1, {'ok': 'B'})
StateMachine.add('B', inner_sm_2, {'ok': 'C'})
StateMachine.add('C', make_state())
with inner_sm_1:
StateMachine.add('A', make_state(), {'ok': 'B'})
StateMachine.add('B', make_state())
with inner_sm_2:
StateMachine.add('A', make_state(), {'ok': 'B'})
StateMachine.add('B', make_state())
return outer_sm
