SMACH專題（一）----安裝與初探

本文轉載自查看原文 2016-01-24 20:41 2493 ROS

　　最近使用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'