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

MonitorState (ROS)

Description: Monitor a state!

Tutorial Level: INTERMEDIATE

NB - This is just what I coded up while trying to understand the MonitorState. Please don't take it for best practices or anything, until JonBohren or somebody with more experience takes a look at it. -LauraLindzey

Setup

Here's an example of a simple state machine using MonitorState. In order to transition from state Foo to state Bar, send a message to the /sm_reset topic:

rostopic pub -1 /sm_reset std_msgs/Empty

The Code

   1 #!/usr/bin/env python
   2 import roslib; roslib.load_manifest('smach_MonitorState_example')
   3 import rospy
   4 import smach
   5 import smach_ros
   6 
   7 from std_msgs.msg import Empty
   8 
   9 class bar(smach.State):
  10     def __init__(self):
  11         smach.State.__init__(self, outcomes=['bar_succeeded'])
  12     def execute(self, userdata):
  13         rospy.sleep(3.0)
  14         return 'bar_succeeded'
  15 
  16 def monitor_cb(ud, msg):
  17     return False
  18 
  19 def main():
  20     rospy.init_node("monitor_example")
  21 
  22     sm = smach.StateMachine(outcomes=['DONE'])
  23     with sm:
  24         smach.StateMachine.add('FOO', smach_ros.MonitorState("/sm_reset", Empty, monitor_cb), transitions={'invalid':'BAR', 'valid':'FOO', 'preempted':'FOO'})
  25         smach.StateMachine.add('BAR',bar(), transitions={'bar_succeeded':'FOO'})
  26 
  27     sis = smach_ros.IntrospectionServer('smach_server', sm, '/SM_ROOT')
  28     sis.start()
  29     sm.execute()
  30     rospy.spin()
  31     sis.stop()
  32 
  33 if __name__=="__main__":
  34     main()

Code with Notes

   1 def monitor_cb(ud, msg):
   2     return False

Arguments are the userdata and the message. This needs to return False when we want the monitor state to terminate. In this case, the monitor state will return 'invalid'

   1     sm = smach.StateMachine(outcomes=['DONE'])
   2     with sm:
   3         smach.StateMachine.add('FOO', 
   4                                smach_ros.MonitorState("/sm_reset", 
   5                                                       Empty, 
   6                                                       monitor_cb), 
   7                                transitions={'invalid':'BAR', 
   8                                             'valid':'FOO', 
   9                                             'preempted':'FOO'})

Note that your MonitorState needs to define transitions for all three possible outcomes ('valid', 'invalid', 'preempted').

From the source code for MonitorState:

def __init__(self, topic, msg_type, cond_cb, max_checks=-1)

The max_checks argument is a limit on how many times the monitor_cb can be called before the MonitorState will return 'valid'.

Wiki: smach/Tutorials/MonitorState (last edited 2012-06-14 05:05:58 by LauraLindzey)