00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 import roslib;
00013 roslib.load_manifest('art_nav')
00014
00015 import rospy
00016
00017 from art_msgs.msg import ArtHertz
00018 from art_msgs.msg import Behavior
00019 from art_msgs.msg import EstopState
00020 from art_msgs.msg import MapID
00021 from art_msgs.msg import NavigatorCommand
00022 from art_msgs.msg import NavigatorState
00023 from art_msgs.msg import RoadState
00024
00025 last_order = None
00026
00027 def log_cmd(cmd):
00028 global last_order
00029 if not last_order:
00030 rospy.loginfo("first navigator command received")
00031 last_order = cmd.order
00032 rospy.loginfo('order ' + str(cmd.order.behavior.value) + ' received')
00033
00034 def log_state(state_msg):
00035 rospy.logdebug('publishing ' + str(state_msg.header.seq))
00036
00037 def estop_state_changes(state_msg):
00038 "do some E-stop state transitions, returns updated state message"
00039 if last_order.behavior.value == Behavior.Run:
00040 if state_msg.estop.state != EstopState.Run:
00041 state_msg.estop.state = EstopState.Run
00042 rospy.loginfo('entering E-stop Run state')
00043 elif last_order.behavior.value == Behavior.Pause:
00044 if state_msg.estop.state != EstopState.Pause:
00045 state_msg.estop.state = EstopState.Pause
00046 rospy.loginfo('entering E-stop Pause state')
00047 elif (last_order.behavior.value == Behavior.Quit
00048 or last_order.behavior.value == Behavior.Abort):
00049 if state_msg.estop.state != EstopState.Done:
00050 state_msg.estop.state = EstopState.Done
00051 rospy.loginfo('entering E-stop Done state')
00052 return state_msg
00053
00054 def road_state_changes(state_msg):
00055 """do some road state transitions when running
00056 returns updated state message"""
00057 rospy.loginfo('Running, checking behavior')
00058 if last_order.behavior.value == Behavior.Initialize:
00059 if state_msg.road.state == RoadState.Init:
00060 state_msg.road.state = RoadState.Follow
00061
00062 state_msg.last_waypt.seg = 1
00063 state_msg.last_waypt.lane = 1
00064 state_msg.last_waypt.pt = 1
00065 rospy.loginfo('entering Road Follow state')
00066 return state_msg
00067
00068 def test():
00069 topic = rospy.Publisher('navigator/state', NavigatorState)
00070 rospy.Subscriber('navigator/cmd', NavigatorCommand, log_cmd)
00071 rospy.init_node('test_commander')
00072
00073 rospy.loginfo('starting commander test')
00074
00075 state_msg = NavigatorState()
00076 state_msg.header.frame_id = '/map'
00077
00078 while not rospy.is_shutdown():
00079
00080 state_msg.header.stamp = rospy.Time.now()
00081
00082
00083 if last_order:
00084 state_msg = estop_state_changes(state_msg)
00085 if state_msg.estop.state == EstopState.Run:
00086 state_msg = road_state_changes(state_msg)
00087 state_msg.last_order = last_order
00088
00089 log_state(state_msg)
00090 topic.publish(state_msg)
00091
00092 rospy.sleep(1.0/ArtHertz.NAVIGATOR)
00093
00094 if __name__ == '__main__':
00095 try:
00096 test()
00097 except rospy.ROSInterruptException: pass
00098
00099 rospy.loginfo('commander test completed')