ROS smach-----Concurrence state machine

1 Concurrence Outcome Map

smach Concurrence通过outcome map定义Concurrence的outcome，具体说就是基于concurrence状态机的孩子（即包含的状态）的outcome来确定concurrence的outcome。

基于outcome map如果concurrence状态机某个孩子的outcome满足，那么concurrence会有对应的outcome输出；如果没有一个孩子outcome满足map条件，则concurrence输出default outcome。

 

cc = Concurrence(outcomes = ['outcome1', 'outcome2'],
                 default_outcome = 'outcome1',
                 input_keys = ['sm_input'],
                 output_keys = ['sm_output'],
                 outcome_map = {'succeeded':{'FOO':'succeeded',
                                             'BAR':'outcome2'},
                                'outcome3':{'FOO':'outcome2'}})
with cc:
    Concurrence.add('FOO', Foo())
    Concurrence.add('BAR', Bar()

• 当孩子'FOO' 输出'succeeded' 并且孩子 'BAR'输出'outcome2'时候, 状态机Concurrence将结束并且输出 'succeeded'
• 当孩子 'FOO' 输出 'outcome2', 状态机Concurrence将结束并且输出 'outcome3',这时与孩子BAR输出无关 

2 Callbacks

如果要完全灵活掌控状态机Concurrence的outcome，可以使用提供的回调函数child_termination_cb和the outcome_cb。

# gets called when ANY child state terminates
def child_term_cb(outcome_map):

  # terminate all running states if FOO finished with outcome 'outcome3'
  if outcome_map['FOO'] == 'outcome3':
    return True

  # terminate all running states if BAR finished
  if outcome_map['BAR']:
    return True

  # in all other case, just keep running, don't terminate anything
  return False


# gets called when ALL child states are terminated
def out_cb(outcome_map):
   if outcome_map['FOO'] == 'succeeded':
      return 'outcome1'
   else:
      return 'outcome2'


# creating the concurrence state machine
sm = Concurrence(outcomes=['outcome1', 'outcome2'],
                 default_outcome='outcome1',
                 input_keys=['sm_input'],
                 output_keys=['sm_output'],
                 child_termination_cb = child_term_cb,
                 outcome_cb = out_cb)
                 
with sm:
   Concurrence.add('FOO', Foo(),
                   remapping={'foo_in':'input'})

   Concurrence.add('BAR', Bar(),
                   remapping={'bar_out':'bar_out'})

• 只要有一个孩子状态完成child_termination_cb就会被调用，在回调函数中你可以决定状态机继续运行还是结束所有在运行状态
• 当最后一个孩子状态完成时调用outcome_cb， 将返回concurrence的outcome。

3 example

#!/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'])
        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 Bar
class 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 Bas
class 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=['outcome6'])
    
    # Open the container
    with sm_top:

        smach.StateMachine.add('BAS', Bas(),
                               transitions={'outcome3':'CON'})

        # Create the sub SMACH state machine
        sm_con = smach.Concurrence(outcomes=['outcome4','outcome5'],
                                   default_outcome='outcome4',
                                   outcome_map={'outcome5':
                                       { 'FOO':'outcome2',
                                         'BAR':'outcome1'}})

        # Open the container
        with sm_con:
            # Add states to the container
            smach.Concurrence.add('FOO', Foo())
            smach.Concurrence.add('BAR', Bar())

        smach.StateMachine.add('CON', sm_con,
                               transitions={'outcome4':'CON',
                                            'outcome5':'outcome6'})

    # Execute SMACH plan
    outcome = sm_top.execute()


if __name__ == '__main__':
    main()

运行结果：

[INFO] [WallTime: 1478584578.397681] State machine starting in initial state 'BAS' with userdata:
    []
[INFO] [WallTime: 1478584578.398319] Executing state BAS
[INFO] [WallTime: 1478584578.398788] State machine transitioning 'BAS':'outcome3'-->'CON'
[INFO] [WallTime: 1478584578.399252] Concurrence starting with userdata:
    []
[INFO] [WallTime: 1478584578.401027] Executing state FOO
[INFO] [WallTime: 1478584578.401614] Executing state BAR
[INFO] [WallTime: 1478584578.402149] Concurrent state 'FOO' returned outcome 'outcome1' on termination.
[INFO] [WallTime: 1478584578.402706] Concurrent state 'BAR' returned outcome 'outcome1' on termination.
[INFO] [WallTime: 1478584578.404882] Concurrent Outcomes: {'FOO': 'outcome1', 'BAR': 'outcome1'}
[INFO] [WallTime: 1478584578.405480] State machine transitioning 'CON':'outcome4'-->'CON'
[INFO] [WallTime: 1478584578.405959] Concurrence starting with userdata:
    []
[INFO] [WallTime: 1478584578.406966] Executing state FOO
[INFO] [WallTime: 1478584578.407653] Executing state BAR
[INFO] [WallTime: 1478584578.408183] Concurrent state 'FOO' returned outcome 'outcome1' on termination.
[INFO] [WallTime: 1478584578.408720] Concurrent state 'BAR' returned outcome 'outcome1' on termination.
[INFO] [WallTime: 1478584578.410798] Concurrent Outcomes: {'FOO': 'outcome1', 'BAR': 'outcome1'}
[INFO] [WallTime: 1478584578.411496] State machine transitioning 'CON':'outcome4'-->'CON'
[INFO] [WallTime: 1478584578.412002] Concurrence starting with userdata:
    []
[INFO] [WallTime: 1478584578.413100] Executing state FOO
[INFO] [WallTime: 1478584578.413924] Executing state BAR
[INFO] [WallTime: 1478584578.414754] Concurrent state 'FOO' returned outcome 'outcome1' on termination.
[INFO] [WallTime: 1478584578.415496] Concurrent state 'BAR' returned outcome 'outcome1' on termination.
[INFO] [WallTime: 1478584578.417591] Concurrent Outcomes: {'FOO': 'outcome1', 'BAR': 'outcome1'}
[INFO] [WallTime: 1478584578.418109] State machine transitioning 'CON':'outcome4'-->'CON'
[INFO] [WallTime: 1478584578.418569] Concurrence starting with userdata:
    []
[INFO] [WallTime: 1478584578.419639] Executing state FOO
[INFO] [WallTime: 1478584578.420264] Executing state BAR
[INFO] [WallTime: 1478584578.420775] Concurrent state 'FOO' returned outcome 'outcome2' on termination.
[INFO] [WallTime: 1478584578.421183] Concurrent state 'BAR' returned outcome 'outcome1' on termination.
[INFO] [WallTime: 1478584578.423097] Concurrent Outcomes: {'FOO': 'outcome2', 'BAR': 'outcome1'}
[INFO] [WallTime: 1478584578.423607] State machine terminating 'CON':'outcome5':'outcome6'

最后一次状态转变用图可以表示为：