SMACH专题（一）----安装与初探

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

1、安装

　　学习smach之前需要安装这个软件，安装smach有两种方式:

　　a、使用apt-get直接安装（推荐），只需要执行如下代码：

$ sudo apt-get install ros-indigo-executive-smach

 　　b、使用源码安装，只需要在相应的工作空间的src下，通过git clone拉取下来，比如在catkin_ws/src的目录下执行如下代码：

$ git clone http://wiki.ros.org/smach/Tutorials

2、smach介绍

　smach[1]是ros中的一个实现有限状态机的开发包，在ROS任务执行中使用得比较多。一提到状态机，大家可能会想起图灵而望而生畏，别紧张，其实没那么的难。smach使用了python实现，所以要使用smach只能在python环境中使用，python使用起来很简单，如果有编程基础，看几个小时就可以上手，没编程基础的看几天也可以很容易上手。

　　smach提供了actionlib整合和smach viewer两大组件。smach viewer可以实时地查看任务执行当前的状态节点位置，是调试开发的必备工具。smach还整合了动作状态，例如定点导航。可以把话题topic，服务service转化为状态，也可以把一个类转化为状态节点。smach可以实现状态的并发执行，任务的重复执行，层次嵌套复杂的状态机（状态机也可以作为一个状态）。

　　学习smach需要有两个基础：状态机和python知识。

　　a、状态机

　　状态机就是表示有限个状态以及在这些状态之间的转移和动作等行为的数学模型。参考资料[3]，最简单的例子就是灯的开灯和关灯的例子。用开灯状态和关灯状态描述灯的状态，状态之间的转换，通过开灯动作和关灯的动作来完成，基于smach打实现如下： 

#!/usr/bin/env python

import roslib; roslib.load_manifest('smach_sample_node')
import rospy
import smach
import smach_ros
import time
# define state Foo       

# define state Bar
class Open(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['closed'])

    def execute(self, userdata):
        rospy.loginfo('Opened')
        time.sleep(2)
        rospy.loginfo('Closeing')
        return 'closed'    
    
#define state close light
class Close(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['opened'])
        
    def execute(self, userdata):
        rospy.loginfo('Closed')
        time.sleep(2)
        rospy.loginfo('Closing')
        return 'opened'


# main
def main():
    rospy.init_node('smach_example_state_machine')

    # Create a SMACH state machine
    sm_light = smach.StateMachine(outcomes=['stop', 'succee'])

    # Open the container
    with sm_light:
        # Add states to the container
        smach.StateMachine.add('OPEN', Open(), 
                               transitions={'closed':'CLOSE'})
        smach.StateMachine.add('CLOSE', Close(), 
                               transitions={'opened':'OPEN'})

    # Execute SMACH plan
    sis = smach_ros.IntrospectionServer('sm_light', sm_light, '/SM_ROOT')
    sis.start()
    # Execute SMACH plan
    outcome = sm_light.execute()

    # Wait for ctrl-c to stop the application
    rospy.spin()
    sis.stop()

if __name__ == '__main__':
    main()

  　　　　输出结果：

[INFO] [WallTime: 1453689512.106141] Closed
[INFO] [WallTime: 1453689514.108995] Closing
[INFO] [WallTime: 1453689514.109447] State machine transitioning 'CLOSE':'opened'-->'OPEN'
[INFO] [WallTime: 1453689514.109942] Opened
[INFO] [WallTime: 1453689514.420038] Closeing
[INFO] [WallTime: 1453689514.420271] State machine transitioning 'OPEN':'closed'-->'CLOSE'

 　　　　相应的状态机实时状态，如下图所示：

　　　　大家发现，基于smach实现状态机器是如此打简单，所以大胆打去弄熟它吧。

　　　　下面贴出两张ros smach viewer生成的图，直观感受下状态机的魅力。

　　　　简单有限状态机：

（来自：ros wiki）

　　　　较为复杂的有限状态机：　　　　

（来自ros wiki）

　　b、python知识

　　　网上资源特别多，这里不展开介绍了。请参考[2]

 

参考资料：

[1]. http://wiki.ros.org/smach/Tutorials

[2]. Python菜鸟教程

[3]. 有限状态机