(!) 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

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()

Code with Notes

Wiki: mysmach/Tutorials/State Machine Preemption with MonitorState (last edited 2015-01-31 08:33:54 by hongming wang)