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

  • 发表于:
  • 原文链接https://kuaibao.qq.com/s/20180910G1Z6US00?refer=cp_1026
  • 腾讯「腾讯云开发者社区」是腾讯内容开放平台帐号(企鹅号)传播渠道之一,根据《腾讯内容开放平台服务协议》转载发布内容。
  • 如有侵权,请联系 cloudcommunity@tencent.com 删除。

扫码关注腾讯云开发者

领取腾讯云代金券