ROS smach-----状态之间传递用户数据

1 定义用户数据

    有时可能需要给某个状态提供一些输入数据，而且该状态也可能需要输出一些数据给其它状态使用。这些输入和输出数据可以称为userdata 。

class Foo(smach.State):
     def __init__(self, outcomes=['outcome1', 'outcome2'],
                        input_keys=['foo_input'],
                        output_keys=['foo_output'])

     def execute(self, userdata):
        # Do something with userdata
        if userdata.foo_input == 1:
            return 'outcome1'
        else:
            userdata.foo_output = 3
            return 'outcome2'

• input_keys  列表是该状态需要输入的数据。execute方法使用 userdata结构拷贝。该状态可以读取列举的 在input_keys列表的所有userdata数据字段, 但是不能向这些字段写东西

• output_keys列表是该状态所有输出数据。该状态可以写output_keys中列举的任何userdata字段。

状态接口可以被定义，通过 outcomes,  input keys和output keys。

2 连接用户数据

    当向状态机添加状态时候，你可以映射userdata字段。例如，如果状态FOO输出数据 'foo_output', 而同时状态BAR 需要输入'bar_input', 那么你可将通过名字讲userdata相关字段连接到一起：

sm_top = smach.StateMachine(outcomes=['outcome4','outcome5'],
                          input_keys=['sm_input'],
                          output_keys=['sm_output'])
  with sm_top:
     smach.StateMachine.add('FOO', Foo(),
                            transitions={'outcome1':'BAR',
                                         'outcome2':'outcome4'},
                            remapping={'foo_input':'sm_input',
                                       'foo_output':'sm_data'})
     smach.StateMachine.add('BAR', Bar(),
                            transitions={'outcome2':'FOO'},
                            remapping={'bar_input':'sm_data',
                                       'bar_output1':'sm_output'})

字段remapping映射状态的in/output_key到状态机的userdata字段。因此，当映射 'x':'y'时候:

• x 是状态的一个input_key 或者一个output_key  ；
• y 自动成为了状态机的userdata的部分 ；

注意：如果状态和状态机中userdata名字相同，这种remapping其实是不需要的，但是为了让这种数据连接看起来更加清洗，我们还是建议使用remapping字段，即使像"remapping={'a':'a'}"。

2.1 状态之间传递数据

我们可以使用remapping机制从状态FOO向状态BAR传递数据，为此当向状态机添加这两种状态时需要使用如下remapping：

• FOO: remapping={'foo_output':'sm_user_data'}
• BAR: remapping={'bar_input':'sm_user_data'}

2.2 状态和状态机之间传递数据

从状态BAR向包含BAR的状态机传递数据

BAR: remapping={'bar_output':'sm_output'}

从状态机向包含状态FOO的状态传递数据

FOO: remapping={'foo_input':'sm_input'}

3 实例解析

 

#!/usr/bin/env python

import roslib; roslib.load_manifest('smach_tutorials')
import rospy
import smach
import smach_ros

# define state Foo
class Foo(smach.State):
    def __init__(self):
        smach.State.__init__(self, 
                             outcomes=['outcome1','outcome2'],
                             input_keys=['foo_counter_in'],
                             output_keys=['foo_counter_out'])

    def execute(self, userdata):
        rospy.loginfo('Executing state FOO')
        if userdata.foo_counter_in < 3:
            userdata.foo_counter_out = userdata.foo_counter_in + 1
            return 'outcome1'
        else:
            return 'outcome2'


# define state Bar
class Bar(smach.State):
    def __init__(self):
        smach.State.__init__(self, 
                             outcomes=['outcome1'],
                             input_keys=['bar_counter_in'])
        
    def execute(self, userdata):
        rospy.loginfo('Executing state BAR')
        rospy.loginfo('Counter = %f'%userdata.bar_counter_in)        
        return 'outcome1'
        




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

    # Create a SMACH state machine
    sm = smach.StateMachine(outcomes=['outcome4'])
    sm.userdata.sm_counter = 0

    # Open the container
    with sm:
        # Add states to the container
        smach.StateMachine.add('FOO', Foo(), 
                               transitions={'outcome1':'BAR', 
                                            'outcome2':'outcome4'},
                               remapping={'foo_counter_in':'sm_counter', 
                                          'foo_counter_out':'sm_counter'})
        smach.StateMachine.add('BAR', Bar(), 
                               transitions={'outcome1':'FOO'},
                               remapping={'bar_counter_in':'sm_counter'})


    # Execute SMACH plan
    outcome = sm.execute()


if __name__ == '__main__':
    main()

运行结果如下：

[INFO] [WallTime: 1478505239.574543] State machine starting in initial state 'FOO' with userdata: 
	['sm_counter']
[INFO] [WallTime: 1478505239.575218] Executing state FOO
[INFO] [WallTime: 1478505239.575714] State machine transitioning 'FOO':'outcome1'-->'BAR'
[INFO] [WallTime: 1478505239.576135] Executing state BAR
[INFO] [WallTime: 1478505239.576547] Counter = 1.000000
[INFO] [WallTime: 1478505239.576917] State machine transitioning 'BAR':'outcome1'-->'FOO'
[INFO] [WallTime: 1478505239.577303] Executing state FOO
[INFO] [WallTime: 1478505239.577718] State machine transitioning 'FOO':'outcome1'-->'BAR'
[INFO] [WallTime: 1478505239.578095] Executing state BAR
[INFO] [WallTime: 1478505239.578463] Counter = 2.000000
[INFO] [WallTime: 1478505239.578850] State machine transitioning 'BAR':'outcome1'-->'FOO'
[INFO] [WallTime: 1478505239.579229] Executing state FOO
[INFO] [WallTime: 1478505239.579621] State machine transitioning 'FOO':'outcome1'-->'BAR'
[INFO] [WallTime: 1478505239.580049] Executing state BAR
[INFO] [WallTime: 1478505239.580421] Counter = 3.000000
[INFO] [WallTime: 1478505239.580770] State machine transitioning 'BAR':'outcome1'-->'FOO'
[INFO] [WallTime: 1478505239.581125] Executing state FOO
[INFO] [WallTime: 1478505239.581510] State machine terminating 'FOO':'outcome2':'outcome4'