SMACH专题（二）----Concurrent状态机

Concurrent状态机是一种同时执行多个状态的状态机。如下图所示。状态FOO和BAR同时执行，当两个状态输出的结果同时满足一个组合条件时（FOO输出outcome2,BAR输出outcome1）才会映射到状态CON的输出结果outcome4。

1、简单例子

具体地，实现代码如下：

#!/usr/bin/env python

import roslib; roslib.load_manifest('smach_example')
import time
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')
        time.sleep(1)
        if self.counter < 5:
            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):
        time.sleep(1)
        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'})
        
     # Create and start the introspection server
    sis = smach_ros.IntrospectionServer('server_name', sm_top, '/SM_ROOT')
    sis.start()

    # Execute SMACH plan
    outcome = sm_top.execute()
    rospy.spin()
    sis.stop()
if __name__ == '__main__':
    main()

2、复杂例子

　　定义复杂的输出结果条件判断，需要实现两个回调函数，如下代码的156行child_term_cb，178行的out_cb两个回调函数，具体说明，请参考代码处的文字解释。

　　实现代码如下：

#!/usr/bin/env python

import roslib;
import time
import rospy
import smach
import smach_ros
from twisted.internet.defer import succeed


# define state Bas
class Bas(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['succeeded'])

    def execute(self, userdata):
        time.sleep(4)
        rospy.loginfo('Executing state BAS')
        return 'succeeded'
    
# define state Foo
class Foo(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['succeeded','preempted','aborted'])
        self.counter = 0

    def execute(self, userdata):
        rospy.loginfo('Executing state FOO')
        time.sleep(1)
        print "counter:%d"%(self.counter)
        if self.counter < 5:
            self.counter += 1
            time.sleep(3)
            return 'succeeded'
        else:
            return 'aborted'


# define state Bar1
class Bar1(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['succeeded','preempted','aborted'])
        self.task_name = 'task_bar1'
    def execute(self, userdata):
        if self.preempt_requested():#如果暂停，则返回暂停状态
            rospy.loginfo("Preempting %s"%(self.task_name))
            self.recall_preempt()#唤醒，终止暂停
            return 'preempted'
        time.sleep(5)
        rospy.loginfo('Executing state BAR1')
        return 'succeeded'
    
# define state Bar2
class Bar2(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['succeeded','preempted','aborted'])
        self.task_name ='bar2'
    def execute(self, userdata):
        if self.preempt_requested():#如果暂停，则返回暂停状态
            rospy.loginfo("Preempting %s"%(self.task_name))
            self.recall_preempt()#唤醒，终止暂停
            return 'preempted'
        time.sleep(5)
        rospy.loginfo('Executing state BAR2')
        return 'succeeded'
    
    
# define state Bar3
class Bar3(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['succeeded','preempted','aborted'])
        self.task_name = 'task_bar3'
    def execute(self, userdata):
        if self.preempt_requested():#如果暂停，则返回暂停状态
            rospy.loginfo("Preempting %s"%(self.task_name))
            self.recall_preempt()#唤醒，终止暂停
            return 'preempted'
        time.sleep(5)
        rospy.loginfo('Executing state BAR3')
        return 'succeeded'
 
# define state Charge
class Charge(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['succeeded'])

    def execute(self, userdata):
        time.sleep(5)
        rospy.loginfo('Executing state BAS')
        return 'succeeded'

class ConcurrentExample:
    def __init__(self):
        rospy.init_node('smach_example_state_machine')
    
        self.last_bar_state = None
        # Create the top level SMACH state machine
        self.sm_top = smach.StateMachine(outcomes=['stop'])
        
        # Open the container
        with self.sm_top:
    
            smach.StateMachine.add('BAS', Bas(),
                                   transitions={'succeeded':'CON'})
    
            # Create the sub SMACH state machine
            self.sm_con = smach.Concurrence(outcomes=['succeeded','preempted','aborted'],
                                       default_outcome='aborted',
                                       #outcome_map = {'succeeded':{'FOO':'succeeded'},
                                       #                 'aborted':{'FOO':'aborted'}},
                                       child_termination_cb = self.child_term_cb,
                                       outcome_cb = self.out_cb
                                       )
    
            # Open the container
            with self.sm_con:
                # Add states to the container
                smach.Concurrence.add('FOO', Foo())
                
                self.sm_bar = smach.StateMachine(outcomes=['succeeded','preempted','aborted'])
                with self.sm_bar:
                    smach.StateMachine.add('BAR1',Bar1(),
                                           transitions={'succeeded':'BAR2','preempted':'preempted'})
                    smach.StateMachine.add('BAR2',Bar2(),
                                           transitions={'succeeded':'BAR3','preempted':'preempted'})
                    smach.StateMachine.add('BAR3',Bar3(),
                                           transitions={'succeeded':'succeeded','preempted':'preempted'})
                self.sm_bar.register_transition_cb(self.bar_transition_cb, cb_args=[])
                smach.Concurrence.add('BAR', self.sm_bar)
    
            smach.StateMachine.add('CON', self.sm_con,
                                   transitions={'succeeded':'stop',
                                                'aborted':'stop',
                                                'preempted':'CHARGE'})
            
            smach.StateMachine.add('CHARGE', Charge(),
                                   transitions={'succeeded':'CON'})
            
         # Create and start the introspection server
        sis = smach_ros.IntrospectionServer('server_name', self.sm_top, '/SM_ROOT')
        sis.start()
    
        # Execute SMACH plan
        outcome = self.sm_top.execute()
        rospy.spin()
        sis.stop()
    
    #状态之间转换的时候会调用该函数。比如BAR1转换到BAR2（或者BAR2转换到BAR3）后，执行该回调函数，
    #那么活动的状态 active_states 是‘BAR2‘（‘BAR3‘）
    def bar_transition_cb(self, userdata, active_states, *cb_args):    
        print active_states # 注意这里是字符串，活动状态的标识符例如‘BAR’
        self.last_bar_state = active_states
        
    # gets called when ANY child state terminates,
    # 只要Concurent下的其中一个状态完成，都会出发该回调函数
    def child_term_cb(self, outcome_map):
      
      # terminate all running states if FOO preempted with outcome 'succeeded'
      if outcome_map['FOO'] == 'succeeded':
        print "child_term_cv:FOO finished"
        if self.last_bar_state is not None:
            
            self.sm_bar.set_initial_state(self.last_bar_state, smach.UserData())     
        return True
          
      # terminate all running states if BAR preempted
      if outcome_map['BAR']=='succeeded' or outcome_map['BAR']=='preempted':
        print "child_term_cv:SM_BAR finished"
        
        return True
    
      # in all other case, just keep running, don't terminate anything
      return False
    
    
    # gets called when ALL child states are terminated，只要Concurrent下的状态都结束了，
    #调用该函数.注意不是BAR下面的BAR1，BAR2，BAR3的之一完成
    def out_cb(self, outcome_map):
       if outcome_map['FOO'] == 'aborted':
          print "out_cb FOO aborted"
          return 'aborted'
       elif outcome_map['BAR'] == 'preempted':
          
          print "out_cb BAR preempted"
          return 'preempted'
       elif outcome_map['BAR'] == 'succeeded':
           print "out_cb_BAR succeeded"
           return 'succeeded'
if __name__ == '__main__':
    ConcurrentExample()

　　状态机器效果图，CON下的FOO和BAR同时执行，如下所示：

　　　　CON进入暂停状态，切换到CHARGE状态下执行（绿色表示执行）：

参考资料：

[1]. http://wiki.ros.org/smach/Tutorials/Concurrent%20States

[2]. ros_by_example_vol2_indigo.pdf

问题：只要BAR或FOO之一结束，就输出相应打结果，这个如何做到？还没找到方法