ubuntu16.04でSMACH ロボットの状態遷移を可視化

ロボットの状態遷移ツール

ロボットが複数のタスクをしたり、複数人でロボットを開発するときにはこのような状態遷移の可視化ツールがあると便利。

http://wiki.ros.org/ja/smach

http://wiki.ros.org/smach_viewer

1 . 依存パッケージのインストール（そのままだとpython関係でエラーが出る)　　

$sudo add-apt-repository ppa:nilarimogard/webupd8    
$sudo apt-get install python-wxgtk2.8

2 . git clone 　　

$cd ~/catkin_ws/src/    
$git clone https://github.com/ros-visualization/executive_smach_visualization.git
$git clone https://github.com/ros/executive_smach.git

3 . サンプルコード　　

#!/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')
        rospy.sleep(1)
        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')
        rospy.sleep(1)
        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('server_name', sm, '/SM_ROOT')
    sis.start()

    # Execute SMACH plan
    outcome = sm.execute()
    rospy.spin()


if __name__ == '__main__':
    main()

何をしているのかの詳細はここがわかりやすい。

smach/Tutorials/Getting Started - ROS Wiki

4 . viewer の起動　　

rosrun smach_viewer smach_viewer.py 状態遷移の状態確認