1.源码获取

官方并没有提供 ROS2 版本的 smach 元功能包，我们可以使用 DeepX 移植的包：

https://github.com/DeepX-inc/executive_smach
https://github.com/DeepX-inc/executive_smach_visualization

将上述两个包 clone 到工作空间下，然后 rosdep + colcon build + source 即可

2.例程获取

https://www.guyuehome.com/1069
http://wiki.ros.org/smach/Tutorials/Getting
https://blog.csdn.net/weixin_43455581/article/details/97136945

网上的示例教程都是 ROS1 的，我们自己来改成 ROS2 版本：

首先 cd 到 src 目录下创建一个新的功能包

ros2 pkg create --build-type ament_python --dependencies rclpy smach smach_ros

import rclpy
from rclpy.node import Node
import smach
import smach_rosclass SmachTestNode(Node):def __init__(self, name):super().__init__(name)self.get_logger().info("启动 demo 节点")# 定义状态 Foo
class Foo(smach.State):def __init__(self):smach.State.__init__(self, outcomes=['outcome1','outcome2'])self.counter = 0def execute(self, userdata):print('Executing state FOO')if self.counter < 3:self.counter += 1return 'outcome1'else:return 'outcome2'# 定义状态 Bar
class Bar(smach.State):def __init__(self):smach.State.__init__(self, outcomes=['outcome2'])def execute(self, userdata):print('Executing state BAR')return 'outcome2'# main
def main(args=None):rclpy.init(args=args) # 初始化 rosnode = SmachTestNode("execute_smach_test")# Create a SMACH state machinesm = smach.StateMachine(outcomes=['outcome4', 'outcome5'])# Open the containerwith sm:# Add states to the containersmach.StateMachine.add('FOO', Foo(), transitions={'outcome1':'BAR', 'outcome2':'outcome4'})smach.StateMachine.add('BAR', Bar(), transitions={'outcome2':'FOO'})# Create and start the introspection serversis = smach_ros.IntrospectionServer('my_smach_introspection_server', sm, '/SM_ROOT')sis.start()# Execute SMACH planoutcome = sm.execute()# Wait for ctrl-c to stop the applicationrclpy.spin(node)sis.stop()if __name__ == '__main__':main()

启动该节点后显示

在另一个终端中输入ros2 run smach_viewer smach_viewer_gui.py即可可视化显示状态机

3 例程分析

状态机有四大概念。

State ，状态。一个状态机至少要包含两个状态。例如自动门，有 open 和 closed 两个状态。
Event ，事件。事件就是执行某个操作的触发条件或者口令。对于自动门，“按下开门按钮”就是一个事件。
Action ，动作。事件发生以后要执行动作。例如事件是“按开门按钮”，动作是“开门”。编程的时候，一个 Action 一般就对应一个函数。
Transition ，变换。也就是从一个状态变化为另一个状态。例如“开门过程”就是一个变换。

作为状态机，首先需要有状态，这个例程中有两个状态：FOO、BAR。这两个状态都是通过Python的函数进行定义的，而且结构相似，都包含初始化（init）和执行（execute）这两个函数。

3.1 状态初始化函数

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

def __init__(self):smach.State.__init__(self, outcomes=['outcome1','outcome2'])self.counter = 0

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

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

3.2 动作执行函数

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

def execute(self, userdata):print('Executing state FOO')if self.counter < 3:self.counter += 1return 'outcome1'else:return 'outcome2'

3.3 main函数

在main函数中，首先初始化ROS节点

然后使用StateMachine创建一个状态机，并且指定状态机执行结束后的最终输出值有两个：outcome4和outcome5.

# 创建一个 SMACH 状态机sm = smach.StateMachine(outcomes=['outcome4', 'outcome5'])

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

# 打开容器with sm:# 将状态添加到容器中smach.StateMachine.add('FOO', Foo(), transitions={'outcome1':'BAR', 'outcome2':'outcome4'})smach.StateMachine.add('BAR', Bar(), transitions={'outcome2':'FOO'})

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

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

# Create and start the introspection serversis = smach_ros.IntrospectionServer('my_smach_introspection_server', sm, '/SM_ROOT')sis.start()

IntrospectionServer（）方法用来创建内部可视化服务器，有三个参数：

第一个参数是服务器的名字，可以根据需要自由给定；
第二个参数是要监测的状态机；
第三个参数代表状态机的层级，因为SMACH状态机支持嵌套，状态内部还可以有自己的状态机。

然后就可以使用execute()方法开始执行状态机了：

# Execute SMACH planoutcome = sm.execute()

执行结束后需要将内部可视化服务器停止：

sis.stop()

4.总结

现在再来回顾整个状态机：

从图中我们可以看到，状态机开始工作后首先跳入我们添加的第一个状态“FOO”，然后在该状态中累加counter变量，counter小于3时，会输出outcome1，状态结束后就跳转到“BAR”状态。在“BAR”状态中什么都没做，输出outcome2回到“FOO”状态。就这样来回几次之后，counter等于3，“FOO”状态的输出值变成outcome2，继而跳转到outcome4，也就代表着有限状态机运行结束。outcome5全程并没有涉及到，所以在图上成为了一个孤立的节点。

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

后记：一个复杂的状态机：

【ROS2】状态机 Smach 包的获取与使用 - Part1相关推荐

使用最新的python代码发送nbns协议包，获取同一局域网下其它电脑的名称
可以使用python的socket库发送NBNS协议包来获取局域网内其他计算机的名称.下面是示例代码: importsocketdef get_host_by_name(name):addr = so ...
使用最新的python代码发送可用nbns协议包，获取同一局域网下其它电脑的名称
使用Python发送NBNS协议包并获取其他电脑的名称,可以使用第三方库scapy: from scapy.all import *def get_hostname(ip):ans,unans = s ...
npm包-js-pinyin获取中文拼音,实现按26个首字母展示城市
npm包-js-pinyin获取中文拼音,实现按26个首字母展示城市 npm安装js-pinyin vue实现代码城市展示效果图你越是认真生活,你的生活就会越美好--弗兰克·劳埃德·莱特 < ...
java下载json需要的包_jenkins 获取插件，下载插件提速，配置国内镜像
jenkins 是目前比较流行的自动化运维工具,由于 jenkins 的镜像源是国外的所以在下载插件的时候会非常慢,甚至会超时,所以本文就重点介绍如何解决这个问题. 正确的做法自然是修改为国内的镜像源 ...
用Python实现手机抓包，获取当当图书差评数据！
在这个万物互联的时代,手机端(App).电脑端(Web),连接着你我他. 本次学习了手机抓包的相关知识,了解了Charles-mitmproxy-Appium的基本使用,通过对当当图书评论的爬取,得以 ...
root 红米note5_红米Note5 root教程_红米Note5卡刷root包来获取root权限
下面也来把这个红米Note5手机的root教程整理一下了,因为这个root也是相当实用的,可以解决很多的问题,不过有的机友却不知道如何进行具体的root,所以下面特意整理了一个详细的获取root权限的 ...
4x root 红米_红米4X root教程_红米4X卡刷root包来获取root权限的方法
下面也是咱们的这个红米4X手机的相关root操作了,同样这个root也是在电脑上进行的,因为有的机友敢是在找这个相关的root包,可是一直没有找到,因此下面特意整理了一个详细的获取root权限的方法了 ...
如何通过抓包来获取数据API
前言写了一个猫眼电影小程序,数据是通过抓包获取的,开始以为抓包很简单就没说明,结果有很多人问如何获取猫眼电影数据,这里就写了一篇博客说明. 通过谷歌浏览器的调试工具可以抓取任意网站的包,这里以猫眼电 ...
(七)Linux搭载4G模块——AT指令实现短信包的获取和删除
文章目录一.前言二.回显短信的两种方式 2.1AT+CMGF 配置短消息模式 2.2AT+CMGL 按照状态读取短消息 2.3 PDU编码显示 2.4 Unicode编码显示三.删除短信四.索 ...

【ROS2】状态机 Smach 包的获取与使用 - Part1