#############################################
# Testing Static
#############################################
if len(sys.argv) < 2:
print('usage: python final.py <color>')
sys.exit()
#color = sys.argv[1]
map_struct = loadmap("maps/final.txt")
q_start = np.array([-0.70, -0., 0, 0.1, 1.5, 30])
lynx.set_pos(q_start)
#adjustable, depending on rtf
sleep(20)
[name, pose, twist] = lynx.get_object_state()
##
if (str(color).lower() == 'blue'):
T01 = np.array([[-1, 0, 0, 200], [0, -1, 0, 200], [0, 0, 1, 0],
[0, 0, 0, 1]])
elif (str(color).lower() == 'red'):
T01 = np.array([[1, 0, 0, 200], [0, 1, 0, 200], [0, 0, 1, 0],
[0, 0, 0, 1]])
else:
print("incorrect color input")
##
allotted = rospy.Duration(60)
remaining = allotted
while remaining.to_sec() > 0:
elapsed = rospy.Time.now() - start
remaining = allotted - elapsed
subprocess.call("clear") # clear the command line
print "Time Left:", max(remaining.to_sec(), 0)
sleep(.1)
# game is over
lynx.stop() # must stop before pausing physics
pause_physics_client(EmptyRequest())
subprocess.call("clear")
[names, poses, twists] = lynx.get_object_state()
score(names, poses, verbose=True) # print detailed score report
# kill student code
[kill(i) for i in range(4)]
except KeyboardInterrupt:
print("Aborted round, pausing physics and killing student code...")
pause_physics_client(EmptyRequest())
[kill(i) for i in range(4)]
lynx.stop()
