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两个回调函数,具体说明,请参考代码处的文字解释。
实现代码如下:
1 #!/usr/bin/env python 2 3 import roslib; 4 import time 5 import rospy 6 import smach 7 import smach_ros 8 from twisted.internet.defer import succeed 9 10 11 # define state Bas 12 class Bas(smach.State): 13 def __init__(self): 14 smach.State.__init__(self, outcomes=['succeeded']) 15 16 def execute(self, userdata): 17 time.sleep(4) 18 rospy.loginfo('Executing state BAS') 19 return 'succeeded' 20 21 # define state Foo 22 class Foo(smach.State): 23 def __init__(self): 24 smach.State.__init__(self, outcomes=['succeeded','preempted','aborted']) 25 self.counter = 0 26 27 def execute(self, userdata): 28 rospy.loginfo('Executing state FOO') 29 time.sleep(1) 30 print "counter:%d"%(self.counter) 31 if self.counter < 5: 32 self.counter += 1 33 time.sleep(3) 34 return 'succeeded' 35 else: 36 return 'aborted' 37 38 39 # define state Bar1 40 class Bar1(smach.State): 41 def __init__(self): 42 smach.State.__init__(self, outcomes=['succeeded','preempted','aborted']) 43 self.task_name = 'task_bar1' 44 def execute(self, userdata): 45 if self.preempt_requested():#如果暂停,则返回暂停状态 46 rospy.loginfo("Preempting %s"%(self.task_name)) 47 self.recall_preempt()#唤醒,终止暂停 48 return 'preempted' 49 time.sleep(5) 50 rospy.loginfo('Executing state BAR1') 51 return 'succeeded' 52 53 # define state Bar2 54 class Bar2(smach.State): 55 def __init__(self): 56 smach.State.__init__(self, outcomes=['succeeded','preempted','aborted']) 57 self.task_name ='bar2' 58 def execute(self, userdata): 59 if self.preempt_requested():#如果暂停,则返回暂停状态 60 rospy.loginfo("Preempting %s"%(self.task_name)) 61 self.recall_preempt()#唤醒,终止暂停 62 return 'preempted' 63 time.sleep(5) 64 rospy.loginfo('Executing state BAR2') 65 return 'succeeded' 66 67 68 # define state Bar3 69 class Bar3(smach.State): 70 def __init__(self): 71 smach.State.__init__(self, outcomes=['succeeded','preempted','aborted']) 72 self.task_name = 'task_bar3' 73 def execute(self, userdata): 74 if self.preempt_requested():#如果暂停,则返回暂停状态 75 rospy.loginfo("Preempting %s"%(self.task_name)) 76 self.recall_preempt()#唤醒,终止暂停 77 return 'preempted' 78 time.sleep(5) 79 rospy.loginfo('Executing state BAR3') 80 return 'succeeded' 81 82 # define state Charge 83 class Charge(smach.State): 84 def __init__(self): 85 smach.State.__init__(self, outcomes=['succeeded']) 86 87 def execute(self, userdata): 88 time.sleep(5) 89 rospy.loginfo('Executing state BAS') 90 return 'succeeded' 91 92 class ConcurrentExample: 93 def __init__(self): 94 rospy.init_node('smach_example_state_machine') 95 96 self.last_bar_state = None 97 # Create the top level SMACH state machine 98 self.sm_top = smach.StateMachine(outcomes=['stop']) 99 100 # Open the container 101 with self.sm_top: 102 103 smach.StateMachine.add('BAS', Bas(), 104 transitions={'succeeded':'CON'}) 105 106 # Create the sub SMACH state machine 107 self.sm_con = smach.Concurrence(outcomes=['succeeded','preempted','aborted'], 108 default_outcome='aborted', 109 #outcome_map = {'succeeded':{'FOO':'succeeded'}, 110 # 'aborted':{'FOO':'aborted'}}, 111 child_termination_cb = self.child_term_cb, 112 outcome_cb = self.out_cb 113 ) 114 115 # Open the container 116 with self.sm_con: 117 # Add states to the container 118 smach.Concurrence.add('FOO', Foo()) 119 120 self.sm_bar = smach.StateMachine(outcomes=['succeeded','preempted','aborted']) 121 with self.sm_bar: 122 smach.StateMachine.add('BAR1',Bar1(), 123 transitions={'succeeded':'BAR2','preempted':'preempted'}) 124 smach.StateMachine.add('BAR2',Bar2(), 125 transitions={'succeeded':'BAR3','preempted':'preempted'}) 126 smach.StateMachine.add('BAR3',Bar3(), 127 transitions={'succeeded':'succeeded','preempted':'preempted'}) 128 self.sm_bar.register_transition_cb(self.bar_transition_cb, cb_args=[]) 129 smach.Concurrence.add('BAR', self.sm_bar) 130 131 smach.StateMachine.add('CON', self.sm_con, 132 transitions={'succeeded':'stop', 133 'aborted':'stop', 134 'preempted':'CHARGE'}) 135 136 smach.StateMachine.add('CHARGE', Charge(), 137 transitions={'succeeded':'CON'}) 138 139 # Create and start the introspection server 140 sis = smach_ros.IntrospectionServer('server_name', self.sm_top, '/SM_ROOT') 141 sis.start() 142 143 # Execute SMACH plan 144 outcome = self.sm_top.execute() 145 rospy.spin() 146 sis.stop() 147 148 #状态之间转换的时候会调用该函数。比如BAR1转换到BAR2(或者BAR2转换到BAR3)后,执行该回调函数, 149 #那么活动的状态 active_states 是‘BAR2‘(‘BAR3‘) 150 def bar_transition_cb(self, userdata, active_states, *cb_args): 151 print active_states # 注意这里是字符串,活动状态的标识符例如‘BAR’ 152 self.last_bar_state = active_states 153 154 # gets called when ANY child state terminates, 155 # 只要Concurent下的其中一个状态完成,都会出发该回调函数 156 def child_term_cb(self, outcome_map): 157 158 # terminate all running states if FOO preempted with outcome 'succeeded' 159 if outcome_map['FOO'] == 'succeeded': 160 print "child_term_cv:FOO finished" 161 if self.last_bar_state is not None: 162 163 self.sm_bar.set_initial_state(self.last_bar_state, smach.UserData()) 164 return True 165 166 # terminate all running states if BAR preempted 167 if outcome_map['BAR']=='succeeded' or outcome_map['BAR']=='preempted': 168 print "child_term_cv:SM_BAR finished" 169 170 return True 171 172 # in all other case, just keep running, don't terminate anything 173 return False 174 175 176 # gets called when ALL child states are terminated,只要Concurrent下的状态都结束了, 177 #调用该函数.注意不是BAR下面的BAR1,BAR2,BAR3的之一完成 178 def out_cb(self, outcome_map): 179 if outcome_map['FOO'] == 'aborted': 180 print "out_cb FOO aborted" 181 return 'aborted' 182 elif outcome_map['BAR'] == 'preempted': 183 184 print "out_cb BAR preempted" 185 return 'preempted' 186 elif outcome_map['BAR'] == 'succeeded': 187 print "out_cb_BAR succeeded" 188 return 'succeeded' 189 if __name__ == '__main__': 190 ConcurrentExample()
状态机器效果图,CON下的FOO和BAR同时执行,如下所示:
CON进入暂停状态,切换到CHARGE状态下执行(绿色表示执行):
参考资料:
[1]. http://wiki.ros.org/smach/Tutorials/Concurrent%20States
[2]. ros_by_example_vol2_indigo.pdf
问题:只要BAR或FOO之一结束,就输出相应打结果,这个如何做到?还没找到方法
Make Change - Focus on Computer Vision and Pattern Recognition
版权声明:本文为博主原创文章,未经博主允许不得转载
版权声明:本文为博主原创文章,未经博主允许不得转载