用ROS的smach编写一个简单的状态机例子

Posted 2020-10-25 hiram-zhang

参考：

1.《Programming Robots with ROS --A PRACTICAL INTRODUCTION TO THE ROBOT OPERATING SYSTEM》Morgan Quigley, Brian Gerkey& William D. Smart 书中第13章On Patrol

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

 

最近使用ROS进行任务（Task）执行，深切体会用传统的方法实现是极其繁杂的。比如人脸录入工作，包含人脸检测，识别，语音提示，运动控制，这些子部分基本都是通过订阅话题的回调函数中处理，之间的切换，如人脸识别，语音提示之间的切换，需要用信号量进行控制切换，当多数据融合的时候，逻辑是极其的混乱的，也容易导致冗余代码，后期的维护也是不易于维护的。深切体会到一个好的实现框架有多么的重要。正如自己的体会，PR2开发中，发现如果不用一套完整的框架去执行任务，会导致代码的维护特别的繁杂，所以大佬们才开发这套smach状态机功能包。而状态机的思想也正符合机器人的这种多数据融合，多子任务执行的问题。状态机的优点是：实现简单，易于维护，重构任务顺序容易。最近的实现证明，smach正是为任务执行量身定做的。

 

于是照《Programming Robots with ROS --A PRACTICAL INTRODUCTION TO THE ROBOT OPERATING SYSTEM》书中第13章On Patrol的例子，自己写了一个含有3个状态和2个状态触发条件的状态机，状态机框图如下：

 

python实现代码如下：

 1 #!/usr/bin/env python
 2 import rospy
 3 from smach import State, StateMachine
 4 from time import sleep
 5 
 6 class One(State):
 7   def __init__(self):
 8     State. __init__(self, outcomes=[‘go‘,‘back‘ ])
 9     
10   def execute(self, userdata):
11     print ‘one‘
12     conditions=input("input cmd:")
13     sleep(1)
14     return conditions
15     
16 class Two(State):
17   def __init__(self):
18     State. __init__(self, outcomes=[‘go‘,‘back‘  ])
19     
20   def execute(self, userdata):
21     print ‘two‘
22     conditions=input("input cmd:")
23     sleep(1)
24     return conditions
25     
26 class Three(State):
27   def __init__(self):
28     State. __init__(self, outcomes=[‘go‘,‘back‘  ])
29     
30   def execute(self, userdata):
31     print ‘three‘
32     conditions=input("input cmd:")
33     sleep(1)
34     return conditions
35  
36 if __name__ == ‘__main__‘ :
37   sm = StateMachine(outcomes=[‘go‘,‘back‘  ])
38   with sm:
39     StateMachine. add(‘ONE‘ , One(), transitions={‘go‘ : ‘TWO‘ ,‘back‘:‘THREE‘})
40     StateMachine. add(‘TWO‘ , Two(), transitions={‘go‘ : ‘THREE‘ ,‘back‘:‘ONE‘})
41     StateMachine. add(‘THREE‘ , Three(), transitions={‘go‘ : ‘ONE‘ ,‘back‘:‘TWO‘})
42     
43   sm. execute()

首先通过继承State类创建我们需要定义的状态类One,Two,Three;同时定义好状态转换的触发条件，这里设置了两个条件‘go‘,‘back‘。

然后调用创建StateMachine类的实例，并将上面定义好的状态类One,Two,Three加入到该实例中，同时设置好状态的转换关系。

 

最后让StateMachine类的实例跑起来就行了。

在终端运行状态机： $ python simple_fsm.py 

下面是运行结果，一切和设计的一样，大功告成！！