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')