 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Iterator容器
Description: 本教程将教会你如何使用Iterator容器。Tutorial Level: BEGINNER
Next Tutorial: 使用actionlib封装SMACH容器
概述
迭代器(iterator)允许你循环遍历一个状态或直到满足成功条件的状态为止。本教程将演示如何使用迭代器将数字列表排序为偶数(evens)和奇数(odds)。
代码
下面是完整的状态机，创建一个名为iterator_tutorial.py的文件并复制下列代码:
2 import roslib; roslib.load_manifest('smach')
3 roslib.load_manifest('smach_ros')
4 import rospy
5
6 import smach
7 from smach import Iterator, StateMachine, CBState
8 from smach_ros import ConditionState, IntrospectionServer
9
10 def construct_sm():
11 sm = StateMachine(outcomes = ['succeeded','aborted','preempted'])
12 sm.userdata.nums = range(25, 88, 3)
13 sm.userdata.even_nums = []
14 sm.userdata.odd_nums = []
15 with sm:
16 tutorial_it = Iterator(outcomes = ['succeeded','preempted','aborted'],
17 input_keys = ['nums', 'even_nums', 'odd_nums'],
18 it = lambda: range(0, len(sm.userdata.nums)),
19 output_keys = ['even_nums', 'odd_nums'],
20 it_label = 'index',
21 exhausted_outcome = 'succeeded')
22 with tutorial_it:
23 container_sm = StateMachine(outcomes = ['succeeded','preempted','aborted','continue'],
24 input_keys = ['nums', 'index', 'even_nums', 'odd_nums'],
25 output_keys = ['even_nums', 'odd_nums'])
26 with container_sm:
27 #test wether even or odd
28 StateMachine.add('EVEN_OR_ODD',
29 ConditionState(cond_cb = lambda ud:ud.nums[ud.index]%2,
30 input_keys=['nums', 'index']),
31 {'true':'ODD',
32 'false':'EVEN' })
33 #add even state
34 @smach.cb_interface(input_keys=['nums', 'index', 'even_nums'],
35 output_keys=['odd_nums'],
36 outcomes=['succeeded'])
37 def even_cb(ud):
38 ud.even_nums.append(ud.nums[ud.index])
39 return 'succeeded'
40 StateMachine.add('EVEN', CBState(even_cb),
41 {'succeeded':'continue'})
42 #add odd state
43 @smach.cb_interface(input_keys=['nums', 'index', 'odd_nums'],
44 output_keys=['odd_nums'],
45 outcomes=['succeeded'])
46 def odd_cb(ud):
47 ud.odd_nums.append(ud.nums[ud.index])
48 return 'succeeded'
49 StateMachine.add('ODD', CBState(odd_cb),
50 {'succeeded':'continue'})
51 #close container_sm
52 Iterator.set_contained_state('CONTAINER_STATE',
53 container_sm,
54 loop_outcomes=['continue'])
55 #close the tutorial_it
56 StateMachine.add('TUTORIAL_IT',tutorial_it,
57 {'succeeded':'succeeded',
58 'aborted':'aborted'})
59 return sm
60
61 def main():
62 rospy.init_node("iterator_tutorial")
63 sm_iterator = construct_sm()
64
65 # Run state machine introspection server for smach viewer
66 intro_server = IntrospectionServer('iterator_tutorial',sm_iterator,'/ITERATOR_TUTORIAL')
67 intro_server.start()
68
69
70 outcome = sm_iterator.execute()
71
72 rospy.spin()
73 intro_server.stop()
74
75 if __name__ == "__main__":
76 main()
代码解释
首先构造迭代器，构造函数采用以下参数:
__init__(self, outcomes, input_keys, output_keys, it=[], it_label='it_data', exhausted_outcome='exhausted')
在本例中，结果现在包括抢占，迭代器的默认结果。it参数是被迭代的对象列表，it_label是保存it列表中项目当前值的键。exhausted参数应该设置为首选的状态机结果，在这个例子中，迭代器的结果是succeeded，迭代器在it列表中循环。
现在添加一个容器，并创建用于将列表排序为偶数和奇数的状态。
24 with tutorial_it:
25 container_sm = StateMachine(outcomes = ['succeeded','preempted','aborted','continue'],
26 input_keys = ['nums', 'index', 'even_nums', 'odd_nums'],
27 output_keys = ['even_nums', 'odd_nums'])
28 with container_sm:
29 #test wether even or odd
30 StateMachine.add('EVEN_OR_ODD',
31 ConditionState(cond_cb = lambda ud:ud.nums[ud.index]%2,
32 input_keys=['nums', 'index']),
33 {'true':'ODD',
34 'false':'EVEN' })
35 #add even state
36 @smach.cb_interface(input_keys=['nums', 'index', 'even_nums'],
37 output_keys=['odd_nums'],
38 outcomes=['succeeded'])
39 def even_cb(ud):
40 ud.even_nums.append(ud.nums[ud.index])
41 return 'succeeded'
42 StateMachine.add('EVEN', CBState(even_cb),
43 {'succeeded':'continue'})
44 #add odd state
45 @smach.cb_interface(input_keys=['nums', 'index', 'odd_nums'],
46 output_keys=['odd_nums'],
47 outcomes=['succeeded'])
48 def odd_cb(ud):
49 ud.odd_nums.append(ud.nums[ud.index])
50 return 'succeeded'
51 StateMachine.add('ODD', CBState(odd_cb),
52 {'succeeded':'continue'})
添加容器到迭代器:
最后，将迭代器添加到顶层状态机:
运行代码
确保roscore正在运行:
$ roscore
运行smach_viewer，这样可以看到结果：
$ rosrun smach_viewer smach_viewer.py
现在运行状态机：
$ sudo chmod a+x iterator_tutorial.py $ python ./iterator_tutorial.py
