art_nav: art_nav: navigator_state.py Source File

00001 #!/usr/bin/env python
00002 #
00003 # Monitor ART navigator state
00004 #
00005 #   Copyright (C) 2010 Austin Robot Technology
00006 #   License: Modified BSD Software License Agreement
00007 #
00008 # $Id: navigator_state.py 615 2010-09-24 16:07:50Z jack.oquin $
00009 
00010 import roslib;
00011 roslib.load_manifest('art_nav')
00012 
00013 import rospy
00014 from art_msgs.msg import MapID
00015 from art_msgs.msg import EstopState
00016 from art_msgs.msg import NavigatorState
00017 from art_msgs.msg import RoadState
00018 
00019 def log_state(state_msg):
00020     "log navigator state information"
00021     # TODO print state labels, not just numbers
00022     rospy.loginfo('E-stop: ' + str(state_msg.estop.state)
00023                   + ', Road: ' + str(state_msg.road.state)
00024                   + ', last way-point: ' + str(state_msg.last_waypt.seg)
00025                   + '.' + str(state_msg.last_waypt.lane)
00026                   + '.' + str(state_msg.last_waypt.pt))
00027 
00028 def test():
00029     rospy.Subscriber('navigator/state', NavigatorState, log_state)
00030     rospy.init_node('navigator_state')
00031     rospy.loginfo('monitoring navigator state')
00032     rospy.spin()
00033 
00034 if __name__ == '__main__':
00035     try:
00036         test()
00037     except rospy.ROSInterruptException: pass
00038 
00039     rospy.loginfo('navigator state monitoring completed')