Smach是状态机的意思,是基于python实现的一个功能强大且易于扩展的库。 smach本质上并不依赖于ROS,可以用于任意python项目,不过在ROS中功能包集executive_smach将smach和ROS很好的集成在了一起,该功能包集还集成了actionlib和smach_viewer(用于可视监控状态机变化,不过目前对于Kinetic版本还没有支持)。
本文主要叙述smach的 1)状态创建和2)如何把状态加入到状态机
1 状态创建
1 class Foo(smach.State):
2 def __init__(self, outcomes=['outcome1', 'outcome2']):
3 # Your state initialization goes here
4
5 def execute(self, userdata):
6 # Your state execution goes here
7 if xxxx:
8 return 'outcome1'
9 else:
10 return 'outcome2'
-
init用于初始化状态class,必须确保该函数不会block执行。
-
execute 用于定义该状态下要完成的任务,它可以允许block执行,一旦你从该方法返回也意味着当前状态结束。
-
当状态结束时候会返回outcome。每个状态都可能会有几种可能的outcomes。用户可以自定义 outcome来描述该状态完成的如何(如['succeeded', 'failed', 'awesome'),outcome也是一个状态切换到下一个状态 的依据。
2 添加状态到状态机
状态机可以看作是一个容器,用于放置不同的状态。当把一个状态加入到状态机时候,我们需要指定状态转换规则。
1 sm = smach.StateMachine(outcomes=['outcome4','outcome5'])
2 with sm:
3 smach.StateMachine.add('FOO', Foo(),
4 transitions={'outcome1':'BAR',
5 'outcome2':'outcome4'})
6 smach.StateMachine.add('BAR', Bar(),
7 transitions={'outcome2':'FOO'})
注意:状态机也可以看做状态,这意味着我们可以把一个状态机看做状态加入到另一个状态机。
3 实例代码
说明:以下代码来源于ROS WiKi。
#!/usr/bin/env python
import roslib; roslib.load_manifest('smach_tutorials')
import rospy
import smach
import smach_ros
<strong>
# define state Foo</strong>
class Foo(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['outcome1','outcome2'])
self.counter = 0
def execute(self, userdata):
rospy.loginfo('Executing state FOO')
if self.counter < 3:
self.counter += 1
return 'outcome1'
else:
return 'outcome2'
<strong># define state Bar</strong>
class Bar(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['outcome2'])
def execute(self, userdata):
rospy.loginfo('Executing state BAR')
return 'outcome2'
<strong># main</strong>
def main():
rospy.init_node('smach_example_state_machine')
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['outcome4', 'outcome5'])
# Open the container
with sm:
# Add states to the container
smach.StateMachine.add('FOO', Foo(),
transitions={'outcome1':'BAR',
'outcome2':'outcome4'})
smach.StateMachine.add('BAR', Bar(),
transitions={'outcome2':'FOO'})
<strong>#<span class="line"><span class="Comment">Create and start the introspection server</span></span><span style="font-family:Microsoft YaHei;">(For debugging interface by smach_viewer )</span></strong>
<span style="font-family:Microsoft YaHei;"> </span> sis = smach_ros.IntrospectionServer('sm_server', sm, '/SM_ROOT')
sis.start()
<strong># Execute SMACH plan</strong>
outcome = sm.execute()
<pre dir="ltr" id="CA-06a92aec5f65b340e778bf2c2ce007b12b82e73a" lang="en"><span class="line"><span class="Comment"> <strong>#Wait for ctrl-c to stop the application</strong></span></span><pre name="code" class="python"><pre dir="ltr" id="CA-06a92aec5f65b340e778bf2c2ce007b12b82e73a" lang="en"><span class="line"><span class="Comment"></span></span><pre name="code" class="python"><strong><span style="font-family:Microsoft YaHei;"> #(For debugging interface by smach_viewer )</span></strong>
rospy.spin() sis.stop()if __name__ == '__main__': main()
输出信息:
[INFO] [WallTime: 1478497355.866746] State machine starting in initial state 'FOO' with userdata:
[]
[INFO] [WallTime: 1478497355.867673] Executing state FOO
[INFO] [WallTime: 1478497355.868092] State machine transitioning 'FOO':'outcome1'-->'BAR'
[INFO] [WallTime: 1478497355.868703] Executing state BAR
[INFO] [WallTime: 1478497355.869094] State machine transitioning 'BAR':'outcome2'-->'FOO'
[INFO] [WallTime: 1478497355.869660] Executing state FOO
[INFO] [WallTime: 1478497355.870035] State machine transitioning 'FOO':'outcome1'-->'BAR'
[INFO] [WallTime: 1478497355.870590] Executing state BAR
[INFO] [WallTime: 1478497355.871010] State machine transitioning 'BAR':'outcome2'-->'FOO'
[INFO] [WallTime: 1478497355.871513] Executing state FOO
[INFO] [WallTime: 1478497355.871843] State machine transitioning 'FOO':'outcome1'-->'BAR'
[INFO] [WallTime: 1478497355.872400] Executing state BAR
[INFO] [WallTime: 1478497355.872766] State machine transitioning 'BAR':'outcome2'-->'FOO'
[INFO] [WallTime: 1478497355.873100] Executing state FOO
[INFO] [WallTime: 1478497355.873282] State machine terminating 'FOO':'outcome2':'outcome4'
4 预定义的状态和状态机容器
上面代码是用户自定义状态和状态机容器,其实smach自身已经包含预定义的状态库和容器库,能够支持一些常见的状态和状态机。
4.1 状态库
-
SimpleActionState: automatically call actionlib actions.
-
ServiceState: automatically call ROS services
-
MonitorState
4.2容器库
-
StateMachine: the generic state machine container
-
Concurrence: a state machine that can run multiple states in parallel.
-
Sequence: a state machine that makes it easy to execute a set of states in sequence. The 'Smach Containers' section on the tutorials page gives an overview of all available containers.
如果ROS支持smach_viewer,你能输出图: