def record(r_led, g_led, event_manager, processing_complete, processed_frames): print('recording') g_led.Update(100, force=True) r_led.Update(1, force=True) processing_complete.wait() processing_complete.clear() event_manager.update(recording) errors = 0 while True: # wait here until all frames have been processed and sent to server try: # get the processed frames from queue n_frame, ball_candidates = processed_frames.get_nowait() # print(f"rec: {n_frame}") message = sf.MyMessage(c.TYPE_BALLS, (n_frame, ball_candidates)) if not sf.send_message(client.client_socket, message, c.CLIENT): errors += 1 print(f"error: {errors}") # if processing complete and no more data to send to server except queue.Empty: if processing_complete.is_set() and (processed_frames.qsize() == 0): event_manager.clear() break # if there were no transmission errors send True, else send False if errors == 0: message = sf.MyMessage(c.TYPE_DONE, True) else: message = sf.MyMessage(c.TYPE_DONE, False) sf.send_message(client.client_socket, message, c.CLIENT) return
def stream(r_led, g_led, event_manager, processing_complete, processed_frames): print('recording stream') message_list = [] g_led.Update(1, force=True) r_led.Update(1, force=True) processing_complete.wait() processing_complete.clear() event_manager.update(record_stream) errors = 0 while True: try: # get the frames from queue n_frame, frame_buf = processed_frames.get_nowait() if event_manager.record_stream.is_set(): print(f"calib: {n_frame}") # y_data is a numpy array hxw with 8 bit greyscale brightness values y_data = np.frombuffer(frame_buf, dtype=np.float32, count=w * h).reshape( (h, w)).astype(np.uint8) message = sf.MyMessage(c.TYPE_IMG, (n_frame, y_data)) if not sf.send_message(client.client_socket, message, c.CLIENT): errors += 1 print(f"error: {errors}") message_list.extend(client.read_all_server_messages()) for message in message_list: if message['data'].type == c.TYPE_DONE: event_manager.clear() except queue.Empty: if not event_manager.record_stream.is_set( ) and processed_frames.qsize( ) == 0: # if the recording has finished print('stream done') processing_complete.set() break # if there were no transmission errors send True, else send False if errors == 0: message = sf.MyMessage(c.TYPE_DONE, True) else: message = sf.MyMessage(c.TYPE_DONE, False) sf.send_message(client.client_socket, message, c.CLIENT)
def initialise_picamera(self): ''' Sends message to both clients to start the picameras Waits for confirmation of camera startup Return: True if cameras started False: Cameras not started ''' print('Initialising cameras') message = sf.MyMessage(c.TYPE_START_CAM, None) setup_complete = {} for client in self.client_names: self.send_to_client(client, message) setup_complete[client] = False while True: message_list = self.read_client_messages() for message in message_list: check_task_trigger(setup_complete, c.TYPE_START_CAM, message) if check_task_complete(setup_complete): print('Cameras initialised') return True time.sleep(0.001)
def shutdown(): print('shutting down') shut_obj = sf.MyMessage(c.TYPE_SHUTDOWN, None) send_to_client(c.LEFT_CLIENT, shut_obj) send_to_client(c.RIGHT_CLIENT, shut_obj) while True: time.sleep(2) sys.exit()
def record(stereo_calib = None, record_time = c.REC_T): if stereo_calib is None: print('stereo cameras must first be calibrated') return False message_list = [] pos = 0 left_done = False right_done = False right_frames = 0 left_frames = 0 rec_obj = sf.MyMessage(c.TYPE_REC, record_time) send_to_client(c.LEFT_CLIENT, rec_obj) send_to_client(c.RIGHT_CLIENT, rec_obj) left_candidates = record_time*100*[None] right_candidates = record_time*100*[None] while True: message_list.extend(read_all_client_messages()) while pos < len(message_list): if message_list[pos]['data'].type == c.TYPE_BALLS: if message_list[pos]['client'] == c.LEFT_CLIENT: f = int(message_list[pos]['data'].message[0]) rectify_points(message_list[pos]['data'].message[1], stereo_calib.cameraMatrix1, stereo_calib.distCoeffs1, stereo_calib.R1, stereo_calib.P1) left_candidates[f] = message_list[pos]['data'].message[1] left_frames+=1 elif message_list[pos]['client'] == c.RIGHT_CLIENT: f = int(message_list[pos]['data'].message[0]) rectify_points(message_list[pos]['data'].message[1], stereo_calib.cameraMatrix2, stereo_calib.distCoeffs2, stereo_calib.R2, stereo_calib.P2) right_candidates[f] = message_list[pos]['data'].message[1] right_frames+=1 elif message_list[pos]['data'].type == c.TYPE_DONE: if message_list[pos]['client'] == c.LEFT_CLIENT: left_done = True elif message_list[pos]['client'] == c.RIGHT_CLIENT: right_done = True else: print(f"unknown message format for recording: {message_list[pos]['data'].type}") pos+=1 if left_done and right_done: print('recording finished') # -- Make both lists the same length -- # min_length = min(left_frames, right_frames) left_candidates = left_candidates[:min_length] right_candidates = right_candidates[:min_length] print(f"frames captured: {min_length}") return left_candidates, right_candidates
def send_to_server(self, message_type, message_data): ''' Send message to the server given the message type and data Return True if sent successfully ''' message = sf.MyMessage(message_type, message_data) sf.send_message(self.socket, message, c.CLIENT) return True
def stream(self, stream_t, show=False, save=False): ''' Sends a message to both clients to start a stream for stream_t seconds ''' stream_t = convert_string_to_float(stream_t) if stream_t is None: return False print(f"Streaming for {stream_t} seconds") message = sf.MyMessage(c.TYPE_STREAM, (stream_t, )) stream_complete = {} for client in self.client_names: self.send_to_client(client, message) stream_complete[client] = False img_queue = mp.Queue() img_play = mp.Event() image_process = mp.Process(target=image_viewer, args=(img_queue, img_play, c.STREAM_DELTA_T)) image_process.start() img_play.set() img_dict = dict.fromkeys(self.client_names) for key in img_dict.keys(): img_dict[key] = {} cur_frame = 0 while True: message_list = self.read_client_messages() for message in message_list: if not check_task_trigger(stream_complete, c.TYPE_DONE, message) \ and check_img(message): if save: save_img(message) if show: add_to_img_dict(img_dict, message) img = None img = combine_img(img_dict, cur_frame) if img is not None: show_img(img, img_queue) cur_frame += c.STREAM_IMG_DELTA if check_task_complete(stream_complete) and img_queue.qsize() == 0: print('Stream complete') cv2.destroyAllWindows() image_process.terminate() return True time.sleep(0.001)
def shutdown(self): ''' Sends a shutdown command to both clients then shuts down the server Return: ''' print('Shutting down...') message = sf.MyMessage(c.TYPE_SHUTDOWN, None) for client_name in self.client_names: self.send_to_client(client_name, message) sys.exit()
def record(self, record_t): ''' Sends a message to both clients to start a recording for record_t seconds ''' record_t = convert_string_to_float(record_t) if record_t is not None and isinstance(record_t, float): if record_t > 0 and record_t <= c.REC_T_MAX: record_t = record_t else: print(f"ERROR: {record_t}s is too long for recording") record_t = c.REC_T_MAX else: return False print(f"Recording for {record_t} seconds") message = sf.MyMessage(c.TYPE_RECORD, (record_t, )) recording_complete = {} for client in self.client_names: self.send_to_client(client, message) recording_complete[client] = False ball_candidate_dict = dict.fromkeys(self.client_names) for key in ball_candidate_dict.keys(): ball_candidate_dict[key] = {} while True: message_list = self.read_client_messages() for message in message_list: if not check_task_trigger(recording_complete, c.TYPE_DONE, message): check_ball_cand(message, ball_candidate_dict, self.stereo_calib) if check_task_complete(recording_complete): print('Recording complete') with open("ball_dict.pkl", "wb") as file: pickle.dump(ball_candidate_dict, file) points_3d = tr.triangulate_points(ball_candidate_dict, self.stereo_calib) plot_points_3d(points_3d) np.save("points_3d.npy", points_3d) trajectory = analyse_trajectory(points_3d) if trajectory is not None: plot_trajectory(trajectory, save=False) return True time.sleep(0.001)
def stream(run_time = c.CALIB_T, calibrate = False, display = False, timeout = False): message_list = [] left_stream_imgs = [] right_stream_imgs = [] left_chessboards = [] right_chessboards = [] chessboards = mp.Queue() chessboard_searchers = [] pos = 0 left_done = False right_done = False disp_n_frame = 0 cal_n_frame = 0 img_size = None done = False stopframe = int(run_time*c.FRAMERATE) chessboards_found = mp.Value('i',0) rec_obj = sf.MyMessage(c.TYPE_STREAM, c.CALIB_IMG_DELAY) send_to_client(c.LEFT_CLIENT, rec_obj) send_to_client(c.RIGHT_CLIENT, rec_obj) if calibrate: # load camera intrinsic calibration data left_cal, right_cal = s_cal.load_calibs() while True: message_list.extend(read_all_client_messages()) if len(message_list) > 0: while pos < len(message_list): # when the clients send an image during calibration if (message_list[pos]['data'].type == c.TYPE_IMG) and not done: n_frame = message_list[pos]['data'].message[0] print(n_frame) y_data = message_list[pos]['data'].message[1] if img_size is None: (h,w) = y_data.shape[:2] img_size = (w,h) # add the img to the corresponding calibration img list if message_list[pos]['client'] == c.LEFT_CLIENT: left_stream_imgs.append((n_frame, y_data)) elif message_list[pos]['client'] == c.RIGHT_CLIENT: right_stream_imgs.append((n_frame, y_data)) # cv2.imwrite(f"{message_list[pos]['client']}{n_frame}.png",y_data) if display: if (len(left_stream_imgs) > disp_n_frame) and (len(right_stream_imgs) > disp_n_frame): ########## FOR TESTING ############## os.chdir(c.IMG_P) cv2.imwrite(f"l_{(disp_n_frame):04d}.png",left_stream_imgs[disp_n_frame][1]) cv2.imwrite(f"r_{(disp_n_frame):04d}.png",right_stream_imgs[disp_n_frame][1]) os.chdir(c.ROOT_P) ########## FOR TESTING ############## disp_frame = cv2.hconcat([left_stream_imgs[disp_n_frame][1],right_stream_imgs[disp_n_frame][1]]) cv2.imshow(f"stream", disp_frame) print(disp_n_frame) cv2.waitKey(100) if left_stream_imgs[disp_n_frame][0] >=stopframe and timeout: done_obj = sf.MyMessage(c.TYPE_DONE, 1) send_to_client(c.LEFT_CLIENT, done_obj) send_to_client(c.RIGHT_CLIENT, done_obj) done = True cv2.destroyAllWindows() disp_n_frame += 1 # look for chessboards if calibrate: if (len(left_stream_imgs) > cal_n_frame) and (len(right_stream_imgs) > cal_n_frame): chessboard_search = mp.Process(target = search_for_chessboards, args=(chessboards_found, chessboards, [left_stream_imgs[cal_n_frame]], [right_stream_imgs[cal_n_frame]])) chessboard_search.start() chessboard_searchers.append(chessboard_search) cal_n_frame += 1 if chessboards_found.value >= c.MIN_PATTERNS: done_obj = sf.MyMessage(c.TYPE_DONE, 1) send_to_client(c.LEFT_CLIENT, done_obj) send_to_client(c.RIGHT_CLIENT, done_obj) if display: done = True cv2.destroyAllWindows() # when both clients send the done message, they are finished collecting frames elif (message_list[pos]['data'].type == c.TYPE_DONE): if message_list[pos]['client'] == c.LEFT_CLIENT: left_done = True elif message_list[pos]['client'] == c.RIGHT_CLIENT: right_done = True if left_done and right_done: if calibrate and chessboards_found.value >= c.MIN_PATTERNS: # for searcher in chessboard_searchers: # searcher.join() while True: try: left_chessboard, right_chessboard = chessboards.get_nowait() left_chessboards.append(left_chessboard) right_chessboards.append(right_chessboard) except queue.Empty: if chessboards.qsize() == 0: break # # check all chessboards are valid in both images s_cal.validate_chessboards(left_chessboards, right_chessboards) # calibrated stereo cameras RMS, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = s_cal.calibrate_stereo( left_chessboards, right_chessboards, left_cal, right_cal, img_size) # obtain stereo rectification projection matrices R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, img_size, R, T) # save all calibration params to object stereo_calib = s_cal.StereoCal(RMS, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F, R1, R2, P1, P2, Q, validPixROI1, validPixROI2) s_cal.save_stereo_calib(stereo_calib) print(f'calibration complete, rms: {RMS}') return stereo_calib else: return None pos += 1