参考：
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
下面是运行结果，一切和设计的一样，大功告成！！
