ROS Smach-----状态创建和添加

    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,你能输出图：