 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. |
StateMachinePreemption
Description: Example of a state machine that implements preemption, using a concurrence container and monitor state.Tutorial Level: ADVANCED
NB - This is just what I coded up while trying to understand how to add preemption into a state machine. Please don't take it for best practices or anything, until JonBohren or somebody with more experience takes a look at it. -LauraLindzey
Contents
Setup
The goal here is to have a state within a state machine that terminates either after a calculation has been finished OR after a particular condition has been met. We use a concurrence container to join together a simple state that implements preemptionand a monitor state. The monitor state checks if a message has been received to the /sm_reset topic.
The sm starts in SETUP, transitions to FOO, and then loops between FOO and BAR. FOO is a Concurrence Container containing FOO_RESET, a MonitorState, and FOO_CALC, a simple state performing a long calculation. If the machine is in state FOO and a message is sent to /sm_reset, FOO_RESET will return, FOO_CALC will be preempted, and the maachine transitions to SETUP. Otherwise, if FOO_CALC finishes, FOO_RESET is preempted and the machine transitions to BAR.
Code
1 #!/usr/bin/env python
2 import roslib; roslib.load_manifest('smach_preemption_example')
3 import rospy
4 import smach
5 import smach_ros
6
7 from std_msgs.msg import Empty
8
9 class setup(smach.State):
10 def __init__(self):
11 smach.State.__init__(self, outcomes=['setup_done'])
12 def execute(self, userdata):
13 rospy.sleep(3.5)
14 return 'setup_done'
15
16 class foo(smach.State):
17 def __init__(self):
18 smach.State.__init__(self, outcomes=['foo_succeeded', 'preempted'])
19 def execute(self, userdata):
20 for idx in range(5):
21 if self.preempt_requested():
22 print "state foo is being preempted!!!"
23 self.service_preempt()
24 return 'preempted'
25 rospy.sleep(1.0)
26 return 'foo_succeeded'
27
28 def child_term_cb(outcome_map):
29 if outcome_map['FOO_CALC'] == 'foo_succeeded':
30 return True
31 elif outcome_map['FOO_RESET'] == 'invalid':
32 return True
33 else:
34 return False
35
36 def out_cb(outcome_map):
37 if outcome_map['FOO_RESET'] == 'invalid':
38 return 'foo_reset'
39 elif outcome_map['FOO_CALC'] == 'foo_succeeded':
40 return 'foo_done'
41 else:
42 return 'foo_reset'
43
44 def monitor_cb(ud, msg):
45 return False
46
47 def main():
48 rospy.init_node("preemption_example")
49
50 foo_concurrence = smach.Concurrence(outcomes=['foo_done', 'foo_reset'],
51 default_outcome='foo_done',
52 child_termination_cb=child_term_cb,
53 outcome_cb=out_cb)
54
55 with foo_concurrence:
56 smach.Concurrence.add('FOO_CALC', foo())
57 smach.Concurrence.add('FOO_RESET', smach_ros.MonitorState("/sm_reset", Empty, monitor_cb))
58
59 sm = smach.StateMachine(outcomes=['DONE'])
60 with sm:
61 smach.StateMachine.add('SETUP', setup(), transitions={'setup_done':'FOO'})
62 smach.StateMachine.add('FOO', foo_concurrence, transitions={'foo_done':'BAR', 'foo_reset':'SETUP'})
63 smach.StateMachine.add('BAR', foo(), transitions={'foo_succeeded':'FOO', 'preempted':'SETUP'})
64
65 sis = smach_ros.IntrospectionServer('smach_server', sm, '/SM_ROOT')
66 sis.start()
67 sm.execute()
68 rospy.spin()
69 sis.stop()
70
71 if __name__=="__main__":
72 main()
