 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 container
Description: This tutorial teaches you how to use the Iterator container.Tutorial Level: BEGINNER
Next Tutorial: Wrapping a SMACH container with actionlib
Overview
The iterator allows you to loop through a state or states until success conditions are met. This tutorial demonstrates how to use an iterator to sort a list of numbers into evens and odds.
The Code
Below is the full state machine, create a file called iterator_tutorial.py and copy the following code:
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()
The Code Explained
Start by constructing the iterator, the constructor takes the following arguments:
__init__(self, outcomes, input_keys, output_keys, it=[], it_label='it_data', exhausted_outcome='exhausted')
In this example, the outcomes now include preempted, a default outcome of the iterator. The it argument is the list of objects that will be iterated over and the it_label is key that holds the current value of the of the item in the it list. The exhausted argument should be set to the preferred state machine outcome, in this example the iterator outcome is succeeded when the iterator is done looping through the it list.
Now add a container and create the states for sorting the list into even and odd.
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'})
Add the container to the iterator:
Finish by adding the iterator to the top level state machine:
Running the Code
Make sure that a roscore is running:
$ roscore
Run the smach_viewer so you can see the results:
$ rosrun smach_viewer smach_viewer.py
Now run the state machine:
$ sudo chmod a+x iterator_tutorial.py $ python ./iterator_tutorial.py
