ROS smach-----创建层级状态机

来源：互联网发布：js小数做加减编辑：程序博客网时间：2024/05/22 23:27

1 创建几个状态

# State Foo  class Foo(smach.State):     def __init__(self, outcomes=['outcome1', 'outcome2'])          def execute(self, userdata):        return 'outcome1'  # State Bar  class Bar(smach.State):     def __init__(self, outcomes=['outcome1'])          def execute(self, userdata):        return 'outcome4'  # State Bas  class Bas(smach.State):     def __init__(self, outcomes=['outcome3'])          def execute(self, userdata):        return 'outcome3'

2 创建层级状态机

# Create the top level SMACH state machine    sm_top = smach.StateMachine(outcomes=['outcome5'])    # Open the container    with sm_top:        smach.StateMachine.add('BAS', Bas(),                               transitions={'outcome3':'SUB'})        # Create the sub SMACH state machine         sm_sub = smach.StateMachine(outcomes=['outcome4'])        # Open the container         with sm_sub:            # Add states to the container             smach.StateMachine.add('FOO', Foo(),                                   transitions={'outcome1':'BAR',                                                 'outcome2':'outcome4'})            smach.StateMachine.add('BAR', Bar(),                                   transitions={'outcome1':'FOO'})        smach.StateMachine.add('SUB', sm_sub,                               transitions={'outcome4':'outcome5'})

3 实例代码

#!/usr/bin/env pythonimport roslib; roslib.load_manifest('smach_tutorials')import rospyimport smachimport smach_ros# define state Fooclass 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')        if self.counter < 3:            self.counter += 1            return 'outcome1'        else:            return 'outcome2'# define state Barclass Bar(smach.State):    def __init__(self):        smach.State.__init__(self, outcomes=['outcome1'])    def execute(self, userdata):        rospy.loginfo('Executing state BAR')        return 'outcome1'        # define state Basclass Bas(smach.State):    def __init__(self):        smach.State.__init__(self, outcomes=['outcome3'])    def execute(self, userdata):        rospy.loginfo('Executing state BAS')        return 'outcome3'def main():    rospy.init_node('smach_example_state_machine')    # Create the top level SMACH state machine    sm_top = smach.StateMachine(outcomes=['outcome5'])        # Open the container    with sm_top:        smach.StateMachine.add('BAS', Bas(),                               transitions={'outcome3':'SUB'})        # Create the sub SMACH state machine        sm_sub = smach.StateMachine(outcomes=['outcome4'])        # Open the container        with sm_sub:            # Add states to the container            smach.StateMachine.add('FOO', Foo(),                                    transitions={'outcome1':'BAR',                                                 'outcome2':'outcome4'})            smach.StateMachine.add('BAR', Bar(),                                    transitions={'outcome1':'FOO'})        smach.StateMachine.add('SUB', sm_sub,                               transitions={'outcome4':'outcome5'})    # Execute SMACH plan    outcome = sm_top.execute()if __name__ == '__main__':    main()

运行结果

[INFO] [WallTime: 1478507071.756471] State machine starting in initial state 'BAS' with userdata: [][INFO] [WallTime: 1478507071.756705] Executing state BAS[INFO] [WallTime: 1478507071.756850] State machine transitioning 'BAS':'outcome3'-->'SUB'[INFO] [WallTime: 1478507071.757021] State machine starting in initial state 'FOO' with userdata: [][INFO] [WallTime: 1478507071.757197] Executing state FOO[INFO] [WallTime: 1478507071.757327] State machine transitioning 'FOO':'outcome1'-->'BAR'[INFO] [WallTime: 1478507071.757453] Executing state BAR[INFO] [WallTime: 1478507071.757573] State machine transitioning 'BAR':'outcome1'-->'FOO'[INFO] [WallTime: 1478507071.757696] Executing state FOO[INFO] [WallTime: 1478507071.757816] State machine transitioning 'FOO':'outcome1'-->'BAR'[INFO] [WallTime: 1478507071.757937] Executing state BAR[INFO] [WallTime: 1478507071.758055] State machine transitioning 'BAR':'outcome1'-->'FOO'[INFO] [WallTime: 1478507071.758175] Executing state FOO[INFO] [WallTime: 1478507071.758291] State machine transitioning 'FOO':'outcome1'-->'BAR'[INFO] [WallTime: 1478507071.758410] Executing state BAR[INFO] [WallTime: 1478507071.758525] State machine transitioning 'BAR':'outcome1'-->'FOO'[INFO] [WallTime: 1478507071.758645] Executing state FOO[INFO] [WallTime: 1478507071.758779] State machine terminating 'FOO':'outcome2':'outcome4'[INFO] [WallTime: 1478507071.758901] State machine terminating 'SUB':'outcome4':'outcome5'

如果用smach_viewer看图形如下：

0 0