SMACH专题（三）----几种State类型

 1 #!/usr/bin/env python
 2 # license removed for brevity
 3 
 4 import rospy
 5 from std_msgs.msg import String
 6 import math
 7 import random
 8 from smach import CBState, StateMachine
 9 from smach_ros import IntrospectionServer
10 import smach
11 
12 @smach.cb_interface(
13                 output_keys=[‘xyz‘],
14                 outcomes=[‘outcome1‘,‘outcome2‘])
15 def fun_cb(ud,x0, x, y, z):
16     ‘‘‘
17     callback function, which is called in CBState way, it seems to be used by
18     function, but the class‘funtion
19     ‘‘‘
20     #x0 is the value of  cb_args=[10], equal to 10
21     dist = math.sqrt(x0*x0+x*x+y*y+z*z+100*random.random())
22     ud.xyz = dist
23     if dist > 10:
24         return ‘outcome1‘
25     else:
26         return ‘outcome2‘
27         
28 class Test_CbState():
29     def __init__(self):
30         rospy.init_node(‘smach_example_callback_state‘)
31         self.sample_name = "cbstate"  
32     
33     def test(self):
34         ‘‘‘
35         test Callback State Machine
36         ‘‘‘
37         self.sm = StateMachine(outcomes = [‘outcome3‘])
38         with self.sm:
39             StateMachine.add(‘Test_CB‘,CBState(fun_cb,
40                                                cb_args=[10],
41                                                cb_kwargs={‘x‘:1,‘y‘:2,‘z‘:3}),
42                                                {‘outcome1‘:‘outcome3‘,‘outcome2‘:‘Test_CB‘})
43         sis = IntrospectionServer(‘test_cb_state‘, self.sm, ‘/SM_ROOT‘)
44         sis.start()
45         
46         self.sm.execute(‘Test_CB‘)
47         
48         rospy.spin()
49         sis.stop()
50         
51 if __name__ == ‘__main__‘:
52     ‘‘‘
53     main funtion
54     ‘‘‘
55     test_sm_cbstate = Test_CbState()
56     test_sm_cbstate.test()
57