因为近期的工作涉及到一个多智能体(机器人)的室内导览项目,在这个项目中我们设计了机器人的语音控制、人脸识别、物体识别等一系列子项目。在一开始,这些子项目我们基本都是通过订阅话题的回调函数进行处理和实现项目之间的切换。但是出现了很多问题(需要用信号量进行控制切换,当多数据融合的时候,逻辑是极其的混乱的,也容易导致冗余代码),也无法很有效的将各个子项目进行整合。所以,我进行了对状态机的学习,并有一些心得,总结一下。
一、安装
首先,学习smach之前需要进行安装
1.使用apt-get直接安装(推荐),只需要执行如下代码:
sudo apt-get install ros-kinetic-executive-smach //我的Ubuntu版本是kinetic
sudo apt-get install ros-kinetic-executive-smach-visualization
二、简述
1.状态机简介
a.有限状态机由一组状态、一个初始状态、输入和根据输入及现有状态转换为下一个状态的转换函数组成。
b.Smach代表"状态机",它是一种基于python的强大的、可伸缩的分级状态机库。Smach库不依赖于ROS,并且可以在任何Python项目中使用。executive_smach堆栈提供了与ROS非常好的集成。
2.状态机的应用
适用:
当你希望机器人执行一些复杂的计划时,SMACH是有用的,其中所有可能的状态和状态转换都可以被明确地描述。这基本上是将不同模块组合成为一个系统。
a.基于快速原型的状态机:基于Python的SMACH语法很明确,简便快捷的制造一个原型状态机并运行它。
b.复杂的状态机:SMACH允许你设计、维护和调试大型、复杂的层次状态机。
不适用:
a.非结构化的任务: 对于你的非结构化任务,SMACH将调度不足。
b.低级系统: 对于要求高效率的低级系统,SMACH并不意味着可以作为一个状态机使用,SMACH是一个任务级体系。
c.Smash: 当你想要使用smash的时候,不要使用SMACH
可能看这些定义还是一头雾水,下面用状态图和一些例子来进行解释。
三、状态机详解
1.状态图
状态存储关于过去的信息,就是说:它反映从系统开始到现在时刻的输入变化。
进入动作(entry action):在进入状态时进行
退出动作:在退出状态时进行
输入动作:依赖于当前状态和输入条件进行
转移动作:在进行特定转移时进行
状态(State):表示对象的某种形态,在当前形态下可能会拥有不同的行为和属性。
转移(Transition):表示状态变更,并且必须满足确使转移发生的条件来执行。
动作(Action):表示在给定时刻要进行的活动。
事件(Event):事件通常会引起状态的变迁,促使状态机从一种状态切换到另一种状态。
2.状态转移表
当前状态(B)和条件(Y)的组合指示出下一个状态(C)。完整的动作信息可以只使用脚注来增加。包括完整动作信息的FSM定义可以使用状态表。
3.特征
状态总数(state)是有限的,任一时刻只处在一种状态之中,在某个条件下会状态转变(transition)。
四、例子
1.基础状态机(state_machine_simple.py)
a.代码部分
仅有两个状态类,FOO和Bar
#!/usr/bin/env python
import rospy
import smach
import smach_ros
# define state Foo
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'
# define state Bar
class Bar(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['outcome2'])
def execute(self, userdata):
rospy.loginfo('Executing state BAR')
return 'outcome2'
# main
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'})
# Create and start the introspection server
sis = smach_ros.IntrospectionServer('my_smach_introspection_server', sm, '/SM_ROOT')
sis.start()
# Execute SMACH plan
outcome = sm.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()
if __name__ == '__main__':
main()
b.运行.py文件,查看效果
在终端中输入以下命令,来运行文件
roscore
rosrun smach_tutorials state_machine_simple.py
d.可视化状态机
rosrun smach_viewer smach_viewer.py
e.代码解析
作为状态机,首先需要有状态,这个例程中有两个状态:FOO、BAR,来看一下这两个状态的定义。
(1)初始化函数(def init)
用来初始化该状态类,调用smach中状态的初始化函数,同时需要定义输出状态:outcome1、outcome2。这里的outcome代表状态结束时的输出值,使用字符串表示,由用户定义取值的范围,例如我们可以定义状态执行是否成功:[‘succeeded’,‘failed’, ‘awesome’]。每个状态的输出值可以有多个,根据不同额输出值有可能跳转到不同的下一个状态。
(2)执行函数(def execute)
就是每个状态中的具体工作内容了,可以进行阻塞工作,当工作后需要返回定义的输出值,该状态结束。
(3)main主函数
初始化节点,并使用StateMachine创建一个状态机sm,并且指定状态机执行结束后的最终输出值有两个:outcome4和outcome5.我们使用add方法添加需要的状态到状态机容器当中,同时需要设置状态之间的跳转关系,例如add的FOO,收到输出状态outcome1将跳转到BAR
smach.StateMachine.add('FOO', Foo(), transitions={'outcome1':'BAR', 'outcome2':'outcome4'})
(4)为了让状态机能够可视化,记得在代码中添加可视化服务器
# Create and start the introspection server
sis = smach_ros.IntrospectionServer('my_smach_introspection_server', sm, '/SM_ROOT')
sis.start()
IntrospectionServer()方法用来创建内部可视化服务器,有三个参数:第一个参数时服务器的名字,可以根据需要自由给定;第二个参数时索要监测的状态机;第三个参数代表状态机的层级,因为SMACH状态机支持嵌套,状态内部还可以有自己的状态机。
五、Python代码git地址
在网上找到的比较好的状态机入门代码是在github上看到的古月居的代码,在工作区间的src下打开终端执行以下命令:
git clone https://github.com/huchunxu/ros_blog_sources //克隆功能包ros_blog_sources
克隆完之后你会发现这个功能包中有很多文件夹,只需要保留samch_tutorials文件夹就好,其余没用的直接删掉。
代码导入成功!
六、大佬的SAMCH笔记和学习网址(很有用!)
因为有关状态机部分的学习资料比较少,又因为状态机部分的知识又多又精。网上很少有写的很详细的,又因为我也没有很多时间来写后续关于状态机的总结,所以给大家安利几个网址,亲测十分有用和详细!建议先看第一部分再去看第二部分。
1.第一部分
创客智造有限状态机第一讲
创客智造有限状态机第二讲
2.第二部分
大佬的博客园笔记(一)
大佬的博客园笔记(二)
大佬的博客园笔记(四)