diff --git a/trap/renderer.py b/trap/renderer.py index 57ee5fa..58d98ae 100644 --- a/trap/renderer.py +++ b/trap/renderer.py @@ -134,7 +134,7 @@ class DrawnTrack: if ci >= len(self.shapes): # TODO: add color2 - line = self.renderer.gradientLine(x, y, x2, y2, 2, color, color, batch=self.renderer.batch_anim) + line = self.renderer.gradientLine(x, y, x2, y2, 3, color, color, batch=self.renderer.batch_anim) line.opacity = 5 self.shapes.append(line) @@ -143,7 +143,7 @@ class DrawnTrack: line.x, line.y = x, y line.x2, line.y2 = x2, y2 line.color = color - line.opacity = int(exponentialDecay(line.opacity, 255, 3, dt)) + line.opacity = int(exponentialDecay(line.opacity, 180, 3, dt)) # TODO: basically a duplication of the above, do this smarter? # TODO: add intermediate segment @@ -183,9 +183,9 @@ class DrawnTrack: decay = (16/ci) if ci else 16 half = len(drawn_predictions) / 2 if ci < half: - target_opacity = 255 + target_opacity = 180 else: - target_opacity = (1 - ((ci - half) / half)) * 255 + target_opacity = (1 - ((ci - half) / half)) * 180 line.opacity = int(exponentialDecay(line.opacity, target_opacity, decay, dt)) @@ -266,11 +266,16 @@ class Renderer: self.window.set_handler('on_close', self.on_close) pyglet.gl.glClearColor(81./255, 20/255, 46./255, 0) - self.fps_display = pyglet.window.FPSDisplay(window=self.window) + self.fps_display = pyglet.window.FPSDisplay(window=self.window, color=(255,255,255,255)) + self.fps_display.label.x = self.window.width - 50 + self.fps_display.label.y = self.window.height - 17 + self.fps_display.label.bold = False + self.fps_display.label.font_size = 10 self.drawn_tracks: dict[str, DrawnTrack] = {} + self.first_time: float|None = None self.frame: Frame|None= None self.prediction_frame: Frame|None = None @@ -281,6 +286,8 @@ class Renderer: self.init_shapes() + self.init_labels() + def init_shapes(self): ''' @@ -327,9 +334,66 @@ class Renderer: self.gradientLine = GradientLine + def init_labels(self): + base_color = (255,)*4 + info_color = (255,255,0, 255) + + options = [] + for option in ['prediction_horizon','num_samples','full_dist','gmm_mode','z_mode', 'model_dir']: + options.append(f"{option}: {self.config.__dict__[option]}") + + self.labels = { + 'waiting': pyglet.text.Label("Waiting for prediction"), + 'frame_idx': pyglet.text.Label("", x=20, y=self.window.height - 17, color=base_color, batch=self.batch_overlay), + 'frame_time': pyglet.text.Label("t", x=120, y=self.window.height - 17, color=base_color, batch=self.batch_overlay), + 'pred_idx': pyglet.text.Label("", x=90, y=self.window.height - 17, color=info_color, batch=self.batch_overlay), + 'pred_time': pyglet.text.Label("", x=200, y=self.window.height - 17, color=info_color, batch=self.batch_overlay), + 'track_len': pyglet.text.Label("", x=500, y=self.window.height - 17, color=base_color, batch=self.batch_overlay), + 'options1': pyglet.text.Label(options.pop(-1), x=20, y=30, color=base_color, batch=self.batch_overlay), + 'options2': pyglet.text.Label(" | ".join(options), x=20, y=10, color=base_color, batch=self.batch_overlay), + + } + + def refresh_labels(self, dt: float): + """Every frame""" + + if self.frame: + self.labels['frame_idx'].text = f"{self.frame.index:06d}" + self.labels['frame_time'].text = f"{self.frame.time - self.first_time:.3f}s" + + if self.prediction_frame: + self.labels['pred_idx'].text = f"{self.prediction_frame.index - self.frame.index}" + self.labels['pred_time'].text = f"{self.prediction_frame.time - time.time():.3f}s" + self.labels['track_len'].text = f"{len(self.prediction_frame.tracks)} tracks" + + + # cv2.putText(img, f"{frame.index:06d}", (20,17), cv2.FONT_HERSHEY_PLAIN, 1, base_color, 1) + # cv2.putText(img, f"{frame.time - first_time:.3f}s", (120,17), cv2.FONT_HERSHEY_PLAIN, 1, base_color, 1) + + # if prediction_frame: + # # render Δt and Δ frames + # cv2.putText(img, f"{prediction_frame.index - frame.index}", (90,17), cv2.FONT_HERSHEY_PLAIN, 1, info_color, 1) + # cv2.putText(img, f"{prediction_frame.time - time.time():.2f}s", (200,17), cv2.FONT_HERSHEY_PLAIN, 1, info_color, 1) + # cv2.putText(img, f"{len(prediction_frame.tracks)} tracks", (500,17), cv2.FONT_HERSHEY_PLAIN, 1, base_color, 1) + # cv2.putText(img, f"h: {np.average([len(t.history or []) for t in prediction_frame.tracks.values()]):.2f}", (580,17), cv2.FONT_HERSHEY_PLAIN, 1, info_color, 1) + # cv2.putText(img, f"ph: {np.average([len(t.predictor_history or []) for t in prediction_frame.tracks.values()]):.2f}", (660,17), cv2.FONT_HERSHEY_PLAIN, 1, info_color, 1) + # cv2.putText(img, f"p: {np.average([len(t.predictions or []) for t in prediction_frame.tracks.values()]):.2f}", (740,17), cv2.FONT_HERSHEY_PLAIN, 1, info_color, 1) + + # options = [] + # for option in ['prediction_horizon','num_samples','full_dist','gmm_mode','z_mode', 'model_dir']: + # options.append(f"{option}: {config.__dict__[option]}") + + + # cv2.putText(img, options.pop(-1), (20,img.shape[0]-30), cv2.FONT_HERSHEY_PLAIN, 1, base_color, 1) + # cv2.putText(img, " | ".join(options), (20,img.shape[0]-10), cv2.FONT_HERSHEY_PLAIN, 1, base_color, 1) + + + def check_frames(self, dt): try: self.frame: Frame = self.frame_sock.recv_pyobj(zmq.NOBLOCK) + if not self.first_time: + self.first_time = self.frame.time img = cv2.GaussianBlur(self.frame.img, (15, 15), 0) img = cv2.flip(cv2.cvtColor(img, cv2.COLOR_BGR2RGB), 0) img = pyglet.image.ImageData(self.frame_size[0], self.frame_size[1], 'RGB', img.tobytes()) @@ -384,8 +448,11 @@ class Renderer: for track_id, track in self.drawn_tracks.items(): track.update_drawn_positions(dt) - self.shape1 = shapes.Circle(700, 150, 100, color=(50, 0, 30), batch=self.batch_anim) - self.shape3 = shapes.Circle(800, 150, 100, color=(100, 225, 30), batch=self.batch_anim) + + self.refresh_labels(dt) + + # self.shape1 = shapes.Circle(700, 150, 100, color=(50, 0, 30), batch=self.batch_anim) + # self.shape3 = shapes.Circle(800, 150, 100, color=(100, 225, 30), batch=self.batch_anim) pass def on_draw(self): diff --git a/trap/timer.py b/trap/timer.py new file mode 100644 index 0000000..dc4131d --- /dev/null +++ b/trap/timer.py @@ -0,0 +1,54 @@ +import time +from multiprocessing.sharedctypes import RawValue, Value, Array +from ctypes import c_double + + +class Timer(): + """ + Measure 2 independent things: the freuency of tic, and the duration of tic->toc + Note that indeed these don't need to be equal + """ + def __init__(self) -> None: + self.last_tic = RawValue(c_double, -1) + self.last_toc = RawValue(c_double, -1) + self.fps = RawValue(c_double, -1) + self.processing_duration = RawValue(c_double, -1) + + self.smoothing = .1 + + def tic(self): + now = time.time() + if self.last_tic is None: + + self.last_tic = now + return + + duration = now - self.last_tic + self.last_tic = now + + current_fps = 1 / duration + if not self.fps: + self.fps = current_fps + else: + self.fps = self.fps * (1-self.smoothing) + current_fps * self.smoothing + + def toc(self): + self.last_toc = time.time() + duration = self.last_toc - self.last_tic + + self.processing_duration = self.processing_duration * (1-self.smoothing) + duration * self.smoothing + + + @property + def fps(self): + + pass + +class TimerCollection(): + def __init__(self) -> None: + self._timers = set() + + def print(self)->str: + print('Update', end='\r') + + \ No newline at end of file