def hyperloop():
imageFolder = None
imageNum = 0
logger = Logger('relog')
logger.setLogLevel('debug')
logger.info('Started replay')
state = State()
for p in sys.argv:
if (os.path.isdir(p)):
imageFolder = p
elif (p.isdigit()):
imageNum = int(p)
elif (p == "-lap"):
state.Approaching = Signal.LAP
elif (p == "-up"):
state.Approaching = Signal.UPPER
elif (p == "-lo"):
state.Approaching = Signal.LOWER
elif (p == "-s"):
state.Approaching = Signal.UPPER
if (state.Approaching != Signal.LAP):
state.setStopSignal(1)
camera = Camera(None, True)
if imageFolder:
# program loop
files = sorted_aphanumeric(os.listdir(imageFolder))
while True:
try:
file = os.path.join(imageFolder, files[imageNum])
logger.info("[" + str(imageNum) + "] Loaded file: " + file)
image = cv2.imread(file, 1)
camera.capture(image)
key = cv2.waitKey(0) & 0xFF
if key == ord("n"):
# cv2.destroyAllWindows()
if (imageNum + 1 < len(files)):
imageNum += 1
elif key == ord("b"):
# cv2.destroyAllWindows()
if (imageNum - 1 >= 0):
imageNum -= 1
elif key == ord('q'):
break
except KeyboardInterrupt:
break
except Exception as e:
logger.logError(e)
traceback.print_exc(limit=3, file=sys.stdout)
logger.info('Stopped')
def startConsole(directory=None, camera_address=None):
if directory is None:
should_save = False
else:
should_save = True
if not os.path.isdir(directory):
os.makedirs(directory)
dir_positive = os.path.join(directory, 'positives')
if not os.path.isdir(dir_positive):
os.makedirs(dir_positive)
files_positive = glob.glob(os.path.join(dir_positive, '*.png'))
if files_positive:
files_positive = [
os.path.split(file)[1] for file in files_positive
]
files_positive = [
int(os.path.splitext(file)[0]) for file in files_positive
]
id_positive = sorted(files_positive)[-1] + 1
else:
id_positive = 0
dir_negative = os.path.join(directory, 'negatives')
if not os.path.isdir(dir_negative):
os.makedirs(dir_negative)
files_negative = glob.glob(os.path.join(dir_negative, '*.png'))
if files_negative:
files_negative = [
os.path.split(file)[1] for file in files_negative
]
files_negative = [
int(os.path.splitext(file)[0]) for file in files_negative
]
id_negative = sorted(files_negative)[-1] + 1
else:
id_negative = 0
print("Starting positive id from %i and negative id from %i." %
(id_positive, id_negative))
cam = Camera()
if camera_address is None:
send_command = False
cam.setSize(160, 120)
cam.capture()
else:
send_command = True
connection = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
connection.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
connection.settimeout(100)
connection.connect(camera_address)
# cam.receive(connection) # TODO: refactore camera module
pygame.init()
fpsClock = pygame.time.Clock()
# http://www.pygame.org/docs/ref/display.html#pygame.display.set_mode
surface = pygame.display.set_mode((width, height), 0, 0)
pygame.display.set_caption('Control Console')
pygame.mouse.set_visible(1)
background = pygame.Surface(surface.get_size())
background.fill((20, 20, 20))
# test=pygame.image.load("test.png")
rect = pygame.Rect(0, 0, width, height)
border_time = 0.
forward = 0.
turn = 0.
keep_running = True
while keep_running:
# surface.blit(background,(0,0))
# frame_received = cam.getFrame()
# frame = cv2.resize(frame_received, (width, height))
# frame = cv2.cvtColor(frame, cv2.COLOR_GRAY2RGB)
# frame = np.rot90(frame)
# frame = pygame.surfarray.make_surface(frame)
# surface.blit(frame, (0, 0))
reward = 0
events = pygame.event.get()
for event in events:
if event.type == QUIT:
keep_running = False
print('Exiting!!!')
if event.type == KEYDOWN:
if (event.key == K_q):
print('q')
if event.key == K_EQUALS:
border_time = time()
border_color = [0, 255, 0]
reward = 1
if should_save:
writeFrame(
frame_received,
os.path.join(dir_positive, '%i' % id_positive))
id_positive += 1
elif event.key == K_MINUS:
border_time = time()
border_color = [255, 0, 0]
reward = -1
if should_save:
writeFrame(
frame_received,
os.path.join(dir_negative, '%i' % id_negative))
id_negative += 1
if event.key == K_ESCAPE:
keep_running = False
print('Exiting!!!')
if time() - border_time < .2:
pygame.draw.rect(surface, border_color, rect, border)
keys_pressed = pygame.key.get_pressed()
if keys_pressed[K_RIGHT]:
turn = runningMean(1, turn, agility)
elif keys_pressed[K_LEFT]:
turn = runningMean(-1, turn, agility)
else:
turn = runningMean(0, turn, agility)
if keys_pressed[K_UP]:
forward = runningMean(1, forward, agility)
elif keys_pressed[K_DOWN]:
forward = runningMean(-1, forward, agility)
else:
forward = runningMean(0, forward, agility)
if abs(turn) < .1:
_turn = 0.
else:
_turn = turn
if abs(forward) < .1:
_forward = 0.
else:
_forward = forward
message = struct.pack(">cffc", "s", _forward, _turn, "e")
# print(struct.unpack(">cffc",message))
if send_command:
connection.send(message)
# if forward or turn or reward:
print("Positives: %i | Negatives: %i" % (id_positive, id_negative))
print("%+i | %+ 1.3f | %+ 1.3f" % (reward, forward, turn))
stdout.write("\033[F\033[K")
stdout.write("\033[F\033[K")
pygame.display.flip() # or pygame.display.update()
fpsClock.tick(FPS)
pygame.quit()
# cmara setup
cam = Camera()
if remote_camera:
# connect to body
connection = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
connection.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
connection.settimeout(100)
connection.connect(body_address)
# receive stream
cam.receive(connection)
else:
cam.setSize(width, height)
cam.capture()
image_sample = cam.getFrame()
if len(image_sample.shape) == 2:
image_depth = 1
else:
image_depth = 3
print("Camera image depth is", image_depth)
# initialize model
data_input = tf.placeholder(tf.float32, shape=(
None, height, width, image_depth), name="data_input")
net = Encoder([height, width, image_depth])
if randomized_input:
randomized = data_input + tf.random_normal([1, 120, 160, 1], stddev=.05)
output = net.model(randomized)
def hyperloop():
logger = Logger()
logger.setLogLevel('info')
logger.info('Started')
state = State()
for p in sys.argv:
if (p == "--standalone" or p == "-s"):
state.Standalone = True
logger.info("Standalone mode activated")
elif (p == "--nocamera" or p == "-n"):
state.NoImageTransfer = True
logger.info("Camera image transfer X11 disabled")
elif (p == "--record" or p == "-r"):
state.RecordImage = True
logger.info("Record mode activated")
elif (p == "--measure" or p == "-m"):
state.MeasureMode = True
logger.info("Measure mode activated")
elif (p == "--invert" or p == "-i"):
state.InvertCamera = True
logger.info("Inverted camera activated")
vs = PiVideoStream(resolution=(480, 368), framerate=32).start()
piCamera = vs.camera
piCamera.exposure_mode = 'sports'
piCamera.ISO = 1600
camera = Camera(vs, not state.NoImageTransfer)
# camera warmup
camera.warmup()
communication = Communication(logger)
acceleration = Acceleration(logger)
# reads any input incoming over UART / i2c / GPIO
communication.readThreadStart()
# measure acceleration
acceleration.measureThreadStart()
fps = FPS().start()
# program loop
while True:
try:
if ((not state.Stopped and state.Loaded) or state.Standalone):
# if (state.StopSignalNum == 0 or (state.Approaching and not state.StopSignalNum == 0) or state.Standalone):
# capture image from videostream and analyze
camera.capture()
fps.update()
if (state.StopSignalNum == 0 and state.LapSignalCount < 2 and not state.Approaching == Signal.UPPER):
communication.sendSpeedPercent(25)
state.Approaching = Signal.UPPER
logger.info("Approaching upper signal")
# if we found our stop signal, announce it
elif (state.StopSignalNum != 0 and not state.StopSignalAnnounced):
communication.sendSpeedPercent(100)
communication.buzzSignalNumber(state.StopSignalNum)
state.setStopSignalAnnounced(True)
state.Approaching = Signal.LAP
logger.info("Approaching lap signal")
# if we passed the lap signal twice, deccelerate to X percent
elif (state.StopSignalAnnounced and state.LapSignalCount >= 2 and not state.Approaching == Signal.LOWER):
communication.sendSpeedPercent(25)
state.Approaching = Signal.LOWER
logger.info("Approaching lower signal")
elif (state.StopSignalAnnounced and state.StopSignalNum == state.CurrentNum and not state.ApproachStop):
communication.sendApproachStop()
communication.sendSpeedPercent(25)
state.ApproachStop = True
logger.info("Approaching stop")
if (cv2.waitKey(1) & 0xFF) == ord('q'):
break
except KeyboardInterrupt:
break
except Exception as e:
logger.logError(e)
traceback.print_exc(limit=3, file=sys.stdout)
fps.stop()
communication.sendStartStop('stop')
time.sleep(1)
logger.info('FPS: ' + str(fps.fps()))
logger.info('Aborting running threads')
communication.readThreadStop()
acceleration.measureThreadStop()
logger.info('Stopped')
