ROS技术点滴——有限状态机smach（1）

SMACH是状态机的意思，是基于Python实现的一个功能强大且易于扩展的库。 smach本质上并不依赖于ROS，可以用于任意python项目，不过在ROS中元功能包executive_smach将smach和ROS很好的集成在了一起，可以为机器人复杂应用开发提供任务级的状态机框架，此外元功能包还集成了actionlib和smach_viewer。

为避免误导，本文以下提到的SMACH均指ROS中的SMACH功能包。

— 一、关于SMACH—

1. 什么时候用

在很多应用场景中，我们需要设计一些复杂的机器人任务，任务中包含多个状态模块，而这些状态模块之间在某些情况下会发生跳转，这就是SMACH可以发挥作用的地方。

快速原型设计：基于Python语法的SMACH可以实现状态机原型的快速开发测试；

复杂状态机模型：SMACH支持设计、维护、调试大型复杂的状态机；

可视化：SMACH提供可视化观测工具smach_viewer ，可以看到完整状态机的状态跳转、数据流等信息

2. 什么时候不用

在某些场景下，SMACH也并不适用：

非结构化任务：非结构化任务调度中可能存在未知的状态跳转

低层次系统：SMACH适用于任务机调度，不适合相对简单、不包含任务级调度的系统。

拆分模块：SMACH的使用并不是为了让我们将模块拆分

— 二、安装SMACH—

无论是ROS indigo还是kinetic，都有smach的二进制安装包，可以直接使用如下命令安装：

smach提供了不少官方例程源码，可以直接下载运行，不过其中很多例程没有加入内部观测器，所以古月君对代码进行了一些修改，大家可以下载修改之后的源码：

https://github.com/huchunxu/ros_blog_sources/tree/master/smach_tutorials

— 三、状态机跑起来—

先看一个简单的示例，state_machine_simple_introspection.py：

使用如下命令运行，看下启动之后的效果：

在终端中可以看到状态的跳转，但是这样的信息并不是很清晰，我们可以启动一个观测神器来可视化显示状态机：

— 四、代码分析—

通过上边运行的效果你可能还没看明白，接下来我们就对照代码进行分析。

作为状态机，首先需要有状态，这个例程中有两个状态：FOO、BAR，我们来看一下这两个状态在代码中的定义：

这两个状态都是通过Python的函数进行定义的，而且结构相似，都包含初始化（__init__）和执行（execute）这两个函数。

1. 初始化函数

初始化函数用来初始化该状态类，调用smach中状态的初始化函数，同时需要定义输出状态：outcome1、outcome2。

这里的outcome代表状态结束时的输出值，使用字符串表示，由用户定义取值的范围，例如我们可以定义状态执行是否成功：['succeeded', 'failed', 'awesome']。 每个状态的输出值可以有多个，根据不同额输出值有可能跳转到不同的下一个状态。

初始化函数中不能阻塞，如果需要实现同步等阻塞功能，可以使用多线程实现。

2. 执行函数

执行函数就是每个状态中的具体工作内容了，可以进行阻塞工作，当工作后需要返回定义的输出值，该状态结束。

再来看一下main函数：

在main函数中，首先初始化ROS节点，然后使用StateMachine创建一个状态机，并且指定状态机执行结束后的最终输出值有两个：outcome4和outcome5。

SMACH状态机是一个容器，我们可以使用add方法添加需要的状态到状态机容器当中，同时需要设置状态之间的跳转关系。

例如这里我们在状态机中添加一个名为“FOO”的状态，该状态的类就是我们之前定义的Foo，transitions代表状态的跳转，如果FOO状态执行输出outcome1时，则跳转到“BAR”状态，如果执行输出outcome2时，则结束这个状态机，并且输出outcome4。

还记得我们上边看到的可视化界面么，为了将状态机可视化显示，我们需要在代码中加入观测器：

IntrospectionServer()方法用来创建内部观测器，有三个参数：第一个参数是观测服务器的名字，可以根据需要自由给定；第二个参数是所要观测的状态机；第三个参数代表状态机的层级，因为SMACH状态机支持嵌套，状态内部还可以有自己的状态机。

然后就可以使用execute()方法开始执行状态机了，执行结束后需要讲内部观测器停止。

现在再来回顾整个状态机，从图中我们可以看到：

1. 状态机开始工作后首先跳入我们添加的第一个状态“FOO”，然后在该状态中累加counter变量。

2. 当counter小于3时，会输出outcome1，状态结束后就跳转到“BAR”状态。

3. 在“BAR”状态中什么都没做，输出outcome2回到“FOO”状态。

4. 就这样来回几次之后，counter等于3，“FOO”状态的输出值变成outcome2，继而跳转到outcome4，也就代表着有限状态机运行结束。

5. outcome5全程并没有涉及到，所以在图上成为了一个孤立的节点。

ROS中的SMACH状态机是不是也并不复杂，将上边的状态机想象成一个简单的机器人应用：机器人去抓取桌子上的杯子，如果抓取到就结束任务，如果抓取不到就继续尝试，尝试3次还没抓到，就放弃抓取，结束任务。

SMACH的功能远远不止如此，比如这是一个复杂的状态机。。。。

参考资料

http://wiki.ros.org/smach/Tutorials

http://wiki.ros.org/smach/Tutorials/Smach%20Viewer

http://www.cnblogs.com/cv-pr/p/5155828.html