Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
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
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()
00046 cmd_msg.header.frame_id = '/map'
00047 cmd_msg.order.behavior.value = behavior
00048
00049
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