ROS 状态机 SMACH 基础教程
Getting Started with smach
1.Creating a State Machine
To create a Smach state machine, you first create a number of states, and then add those states to a State Machine container. Both state machine and state are classes.
2.Creating a state
To create a state, you simply inherit from the State base class, and implement the State.execute(userdata) method:
class Foo(smach.State):
def __init__(self, outcomes=['outcome1', 'outcome2']):
# Your state initialization goes here
def execute(self, userdata):
# Your state execution goes here
if xxxx:
return 'outcome1'
else:
return 'outcome2'- init: initialize your state class and sure to never block in the init method!
- execute: method of a state the actual work is done. Here you can execute any code you want.
3.Adding states to a state machine
A state machine is a container that holds a number of states and state machines. When adding a state to a state machine container, you specify the transitions between the states.
sm = smach.StateMachine(outcomes=['outcome4','outcome5'])
with sm:
smach.StateMachine.add('FOO', Foo(),
transitions={'outcome1':'BAR',
'outcome2':'outcome4'})
smach.StateMachine.add('BAR', Bar(),
transitions={'outcome2':'FOO'})Passing User Data between States
1.Specifying User Data
The input and output data of a state is called userdata of the state.
class Foo(smach.State):
def __init__(self, outcomes=['outcome1', 'outcome2'],
input_keys=['foo_input'],
output_keys=['foo_output'])
def execute(self, userdata):
# Do something with userdata
if userdata.foo_input == 1:
return 'outcome1'
else:
userdata.foo_output = 3
return 'outcome2'- input_keys list enumerates all the inputs that a state needs to run(only read)
output_keys list enumerates all the outputs that a state provides(write)
The interface to a state is defined by its outcomes, its input keys and its output keys:
2.Connecting User Data
Connect the user data fields, pass data to each other.
sm_top = smach.StateMachine(outcomes=['outcome4','outcome5'],
input_keys=['sm_input'],
output_keys=['sm_output'])
with sm_top:
smach.StateMachine.add('FOO', Foo(),
transitions={'outcome1':'BAR',
'outcome2':'outcome4'},
remapping={'foo_input':'sm_input',
'foo_output':'sm_data'})
smach.StateMachine.add('BAR', Bar(),
transitions={'outcome2':'FOO'},
remapping={'bar_input':'sm_data',
'bar_output1':'sm_output'})The remapping mechanism enables us to pass data between states.
FOO: remapping={'foo_output':'sm_user_data'}
BAR: remapping={'bar_input':'sm_user_data'}
Create a Hierarchical State Machine
We create a top level state machine, and start adding states to it. One of the states we add is another state machine:
# Create the top level SMACH state machine
sm_top = smach.StateMachine(outcomes=['outcome5'])
# Open the container
with sm_top:
smach.StateMachine.add('BAS', Bas(),
transitions={'outcome3':'SUB'})
# Create the sub SMACH state machine
sm_sub = smach.StateMachine(outcomes=['outcome4'])
# Open the container
with sm_sub:
# Add states to the container
smach.StateMachine.add('FOO', Foo(),
transitions={'outcome1':'BAR',
'outcome2':'outcome4'})
smach.StateMachine.add('BAR', Bar(),
transitions={'outcome1':'FOO'})
smach.StateMachine.add('SUB', sm_sub,
transitions={'outcome4':'outcome5'})
Calling Actions from a State Machine
1.Goal Message
The goal message can be specified in 3 different way.
- Empty goal message
sm = StateMachine(['succeeded','aborted','preempted'])
with sm:
smach.StateMachine.add('TRIGGER_GRIPPER',
SimpleActionState('action_server_namespace',
GripperAction),
transitions={'succeeded':'APPROACH_PLUG'})- Fixed goal message
sm = StateMachine(['succeeded','aborted','preempted'])
with sm:
gripper_goal = Pr2GripperCommandGoal()
gripper_goal.command.position = 0.07
gripper_goal.command.max_effort = 99999
StateMachine.add('TRIGGER_GRIPPER',
SimpleActionState('action_server_namespace',
GripperAction,
goal=gripper_goal),
transitions={'succeeded':'APPROACH_PLUG'})- Goal from user data
sm = StateMachine(['succeeded','aborted','preempted'])
with sm:
StateMachine.add('TRIGGER_GRIPPER',
SimpleActionState('action_server_namespace',
GripperAction,
goal_slots=['max_effort',
'position']),
transitions={'succeeded':'APPROACH_PLUG'},
remapping={'max_effort':'user_data_max',
'position':'user_data_position'})Goal callback
This is the ultimate power version: you can get a callback when the action needs a goal, and you can create your own goal message on demand.
sm = StateMachine(['succeeded','aborted','preempted'])
with sm:
def gripper_goal_cb(userdata, goal):
gripper_goal = GripperGoal()
gripper_goal.position.x = 2.0
gripper_goal.max_effort = userdata.gripper_input
return gripper_goal
StateMachine.add('TRIGGER_GRIPPER',
SimpleActionState('action_server_namespace',
GripperAction,
goal_cb=gripper_goal_cb,
input_keys=['gripper_input'])
transitions={'succeeded':'APPROACH_PLUG'},
remapping={'gripper_input':'userdata_input'})2.Result Message
It’s similar to the goal message.
sm = StateMachine(['succeeded','aborted','preempted'])
with sm:
StateMachine.add('TRIGGER_GRIPPER',
SimpleActionState('action_server_namespace',
GripperAction,
result_slots=['max_effort',
'position']),
transitions={'succeeded':'APPROACH_PLUG'},
remapping={'max_effort':'user_data_max',
'position':'user_data_position'})Result callback
sm = StateMachine(['succeeded','aborted','preempted'])
with sm:
def gripper_result_cb(userdata, status, result):
if status == GoalStatus.SUCCEEDED:
userdata.gripper_output = result.num_iterations
return 'my_outcome'
StateMachine.add('TRIGGER_GRIPPER',
SimpleActionState('action_server_namespace',
GripperAction,
result_cb=gripper_result_cb,
output_keys=['gripper_output'])
transitions={'succeeded':'APPROACH_PLUG'},
remapping={'gripper_output':'userdata_output'})Viewing State Machines
1.Creating an Introspection Server
The SMACH viewer can use this debugging interface to visualize and interact with your state machine.
# First you create a state machine sm
# .....
# Creating of state machine sm finished
# Create and start the introspection server
sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
sis.start()
# Execute the state machine
outcome = sm.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()- server_name: this name is used to create a namespace for the ROS introspection topics. You can name this anything you like, as long as this name is unique in your system. This name is not shown in the smach viewer.
- SM_ROOT:The “SM_ROOT” argument is simply used for visualization, and forced nesting of different servers. If you have sub-state machines that are in different executables, you can make them show up as hierarchical state machines by choosing this name in a clever way: if the top level state machine is called ‘SM_TOP’, you can call the sub state machine ‘SM_TOP/SM_SUB’, and the viewer will recognize the sub state machine as being part of the top state machine.