test_estop.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # ART navigator unit test for E-stop state
00004 #
00005 #   Copyright (C) 2010 Austin Robot Technology
00006 #   License: Modified BSD Software License Agreement
00007 #
00008 # $Id: test_estop.py 864 2010-11-26 19:45:58Z jack.oquin $
00009 
00010 # make print compatible with python 2.6 and 3.0
00011 from __future__ import print_function
00012 
00013 import roslib;
00014 roslib.load_manifest('art_nav')
00015 
00016 import rospy
00017 from art_msgs.msg import Behavior
00018 from art_msgs.msg import EstopState
00019 from art_msgs.msg import MapID
00020 from art_msgs.msg import NavigatorCommand
00021 from art_msgs.msg import NavigatorState
00022 
00023 import sys
00024 
00025 last_state_ = None
00026 
00027 def log_state(state_msg):
00028     "log navigator state information"
00029     global last_state_
00030     # TODO print state labels, not just numbers
00031     rospy.loginfo('E-stop: ' + str(state_msg.estop.state)
00032                   + ' Road: ' + str(state_msg.road.state)
00033                   + ' last way-point: ' + str(state_msg.last_waypt.seg)
00034                   + '.' + str(state_msg.last_waypt.lane)
00035                   + '.' + str(state_msg.last_waypt.pt))
00036     last_state_ = state_msg
00037 
00038 def estop(behavior, new_state):
00039     cmd = rospy.Publisher('navigator/cmd', NavigatorCommand)
00040     rospy.Subscriber('navigator/state', NavigatorState, log_state)
00041     rospy.init_node('estop')
00042     rospy.loginfo('sending behavior: ' + str(behavior))
00043     rospy.loginfo('setting E-stop state to: ' + str(new_state))
00044 
00045     cmd_msg = NavigatorCommand()     # navigator command msg
00046     cmd_msg.header.frame_id = '/map'
00047     cmd_msg.order.behavior.value = behavior
00048 
00049     # keep sending this behavior until requesting state reached
00050     while not rospy.is_shutdown():
00051         if last_state_ and last_state_.estop.state == new_state:
00052             break
00053         cmd_msg.header.stamp = rospy.Time.now()
00054         cmd.publish(cmd_msg)
00055         rospy.sleep(0.2)
00056 
00057     rospy.loginfo('E-stop state set to ' + str(new_state))
00058 
00059 def usage():
00060     "print usage message"
00061     print('usage: estop.py <state>')
00062     print('')
00063     print('options')
00064     print('    p    Pause')
00065     print('    q    Quit (Done)')
00066     print('    r    Run')
00067     sys.exit(9)
00068 
00069 if __name__ == '__main__':
00070     if len(sys.argv) != 2:
00071         print('exactly one parameter expected')
00072         usage()
00073 
00074     option = sys.argv[1]
00075     if option == 'p':
00076         new_state = EstopState.Pause
00077         behavior = Behavior.Pause
00078     elif option == 'r':
00079         new_state = EstopState.Run
00080         behavior = Behavior.Run
00081     elif option == 'q':
00082         new_state = EstopState.Done
00083         behavior = Behavior.Quit
00084     else:
00085         print('unknown parameter:', option)
00086         usage()
00087 
00088     try:
00089         estop(behavior, new_state)
00090     except rospy.ROSInterruptException: pass


art_nav
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Fri Jan 3 2014 11:08:43