diff --git a/pyproject.toml b/pyproject.toml index 9788695..18a4324 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -16,7 +16,7 @@ dependencies = [ "gdown>=4.7.1,<5", "pandas-helper-calc", "tsmoothie>=1.0.5,<2", - "pyglet>=2.0.15,<3", + "pyglet>=2.1.8,<3", "pyglet-cornerpin>=0.3.0,<0.4", "opencv-python", "setproctitle>=1.3.3,<2", @@ -62,6 +62,7 @@ trap_tracker = "trap.tracker:Tracker.parse_and_start" trap_track_writer = "trap.track_writer:TrackWriter.parse_and_start" trap_lidar = "trap.lidar_tracker:Lidar.parse_and_start" trap_stage = "trap.stage:Stage.parse_and_start" +trap_render_stage = "trap.stage_renderer:StageRenderer.parse_and_start" trap_prediction = "trap.prediction_server:PredictionServer.parse_and_start" trap_render_cv = "trap.cv_renderer:CvRenderer.parse_and_start" trap_monitor = "trap.monitor:Monitor.parse_and_start" # migrate timer diff --git a/supervisord.conf b/supervisord.conf index c3d0e51..7f32236 100644 --- a/supervisord.conf +++ b/supervisord.conf @@ -35,7 +35,7 @@ directory=%(here)s autostart=false [program:lidar] -command=uv run trap_lidar --min-box-area 0.1 --viz --smooth-tracks +command=uv run trap_lidar --min-box-area 0.1 --viz environment=DISPLAY=":0" directory=%(here)s autostart=false @@ -82,7 +82,7 @@ autostart=false stopwaitsecs=60 [program:laserspace] -command=cargo run --release tcp://127.0.0.1:99174 +command=cargo run --release tcp://127.0.0.1:99174 ../trap/SETTINGS/2025-11-dortmund/laserspace.json directory=%(here)s/../laserspace environment=DISPLAY=":0" autostart=false diff --git a/trap/lidar_tracker.py b/trap/lidar_tracker.py index fe729e7..70455e3 100644 --- a/trap/lidar_tracker.py +++ b/trap/lidar_tracker.py @@ -222,6 +222,8 @@ class Lidar(Node): self.sequence_generator = read_live_data(self.config.ip, self.config.port, config) self.tracker = BYTETracker(frame_rate=ASSUMED_FPS) + + self.tracks: DefaultDict[str, Track] = defaultdict(lambda: Track()) @@ -257,7 +259,11 @@ class Lidar(Node): if len(lines) > 1: logger.warning("Multiple lines in outline file, using first only") - polygon_points =np.array([[*p.position, 0] for p in lines[0].points]) + axis_min = .3 + axis_max = 2.2 + + + polygon_points =np.array([[*p.position, axis_min] for p in lines[0].points]) # self.map_outline = shapely.Polygon([p.position for p in lines[0].points]) boundary_lines = [[i, (i+1) % len(lines[0].points)] for i in range(len(lines[0].points))] line_set = o3d.geometry.LineSet( @@ -272,8 +278,8 @@ class Lidar(Node): self.map_outline_volume = o3d.visualization.SelectionPolygonVolume() self.map_outline_volume.orthogonal_axis = "Z" # extrusion direction of polygon - self.map_outline_volume.axis_min = .3 # filter from slightly above ground - self.map_outline_volume.axis_max = 2.2 + self.map_outline_volume.axis_min = axis_min # filter from slightly above ground + self.map_outline_volume.axis_max = axis_max @@ -283,7 +289,7 @@ class Lidar(Node): if self.config.smooth_tracks: # TODO)) make configurable logger.info(f"Smoother enabled, assuming {ASSUMED_FPS} fps") - self.smoother = Smoother(window_len=ASSUMED_FPS*5, convolution=False) + self.smoother = Smoother(window_len=int(ASSUMED_FPS*.6), convolution=True) else: logger.info("Smoother Disabled (enable with --smooth-tracks)") @@ -389,9 +395,16 @@ class Lidar(Node): counter = CounterSender() frame_idx = 0 - while self.run_loop(): + kalmain_init_pos =self.tracker.kalman_filter._std_weight_position + kalmain_init_vel =self.tracker.kalman_filter._std_weight_velocity + + # limit at lidar framefrate to avoid flickering if multiple lidars are connected + while self.run_loop_capped_fps(12): frame_idx += 1 + self.tracker.kalman_filter._std_weight_position = kalmain_init_pos * self.get_setting('lidar.kalman_factor', 1.3) + self.tracker.kalman_filter._std_weight_velocity = kalmain_init_vel * self.get_setting('lidar.kalman_factor', 1.3) + lidar_items = next(self.sequence_generator) @@ -457,24 +470,26 @@ class Lidar(Node): stat_static = len(filtered_pcd.points) counter.set('lidar.unstatic', stat_static) + timers=[] if self.room_filter.initialised and self.get_setting('lidar.tracking_enabled',True): # filtered_pcd, _ = filtered_pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0) + timers.append(('a', time.perf_counter())) denoised_pcd, inlier_indexes = filtered_pcd.remove_radius_outlier(nb_points=5, radius=0.8) stat_denoise = len(filtered_pcd.points) counter.set('lidar.denoised', stat_denoise) - + timers.append(('denoise', time.perf_counter())) if self.config.viz: outlier_pcd = filtered_pcd.select_by_index(inlier_indexes) outlier_pcd.paint_uniform_color((1,0,0)) dropped_pcds.append(outlier_pcd) - + timers.append(('viz', time.perf_counter())) filtered_pcd = denoised_pcd # down sample - filtered_pcd = filtered_pcd.voxel_down_sample(voxel_size=0.08) + filtered_pcd = filtered_pcd.voxel_down_sample(voxel_size=0.04) stat_downsampled = len(filtered_pcd.points) - + timers.append(('downsample', time.perf_counter())) if self.config.viz: @@ -482,9 +497,9 @@ class Lidar(Node): counter.set('lidar.downsampled', stat_downsampled) - points_2d = project_to_xy(filtered_pcd) + timers.append(('project', time.perf_counter())) # with open('/tmp/points.pcl', 'wb') as fp: # pickle.dump(points_2d, fp) @@ -492,10 +507,13 @@ class Lidar(Node): clusters = self.cluster_2d( points_2d, ) + timers.append(('cluster2d', time.perf_counter())) # print(len(clusters)) # boxes, centroids = get_cluster_boxes(points_2d, labels, min_area= 0.3*0.3) - boxes, centroids = get_cluster_boxes(clusters, min_area= self.get_setting('lidar.min_box_area', .1)) + boxes, centroids = get_cluster_boxes(clusters, min_area= self.get_setting('lidar.min_box_area', .1), max_area= self.get_setting('lidar.max_box_area', 5)) + + timers.append(('boxes', time.perf_counter())) # print(centroids) # append confidence and class (placeholders) @@ -504,6 +522,8 @@ class Lidar(Node): detections[:,:-2] = boxes online_tracks = self.tracker.update(detections) + + timers.append(('update', time.perf_counter())) self.logger.debug(f"online tracks: {[t[4] for t in online_tracks]}") removed_tracks = self.tracker.removed_stracks # active_stracks = [track for track in self.tracker.tracked_stracks if track.is_activated] @@ -511,9 +531,11 @@ class Lidar(Node): detections = [Detection.from_bytetrack(track, frame_idx) for track in active_stracks] counter.set('detections', len(detections)) + timers.append(('tracks', time.perf_counter())) self.detection_sock.send_pyobj(detections) + timers.append(('sent', time.perf_counter())) for detection in detections: track = self.tracks[detection.track_id] @@ -526,6 +548,8 @@ class Lidar(Node): for t in removed_tracks: if t.track_id in self.tracks: + if t.is_activated: + self.logger.info(f"Lost track: {t.track_id}") del self.tracks[t.track_id] # TODO: fix this oddity: # else: @@ -537,6 +561,7 @@ class Lidar(Node): active_track_ids = [d.track_id for d in detections] active_tracks = {t.track_id: t.get_with_interpolated_history() for t in self.tracks.values() if t.track_id in active_track_ids} # active_tracks = displacement_filter.apply_to_dict(active_tracks, frame.camera)# a filter to remove just detecting static objects + timers.append(('interpolated', time.perf_counter())) # frame = Frame(frame_idx, None, time.time(), self.tracks, camera.H, camera) frame = Frame(frame_idx, None, time.time(), active_tracks, @@ -545,9 +570,13 @@ class Lidar(Node): if self.config.smooth_tracks: frame = self.smoother.smooth_frame_tracks(frame) + timers.append(('smooth', time.perf_counter())) + counter.set('tracks', len(active_tracks)) self.track_sock.send_pyobj(frame) + + timers.append(('sent', time.perf_counter())) @@ -567,6 +596,12 @@ class Lidar(Node): for line_set in line_sets: vis.add_geometry(line_set, False) + + timers.append(('viz2', time.perf_counter())) + total_time = timers[-1][1]-timers[0][1] + for t0, t1 in zip(timers, timers[1:]): + difftime = t1[1]-t0[1] + counter.set(f'tracker.timer.{t1[0]}', difftime/total_time) elif self.config.viz: print('initializing') if hasattr(self.room_filter, 'scan_counter'): @@ -641,7 +676,8 @@ class Lidar(Node): argparser.add_argument('--ip', help='IP of this computer on which to listen for IP packets broadcast by lidar (so NOT the ip of the Lidar itself)', type=str, - default="192.168.0.70") + # default="192.168.0.70") + default="0.0.0.0") argparser.add_argument('--port', help='Port of the incomming ip packets', type=int, @@ -854,7 +890,7 @@ def split_cluster_by_convex_hull(points, max_hull_area): -def get_cluster_boxes(clusters, min_area = 0): +def get_cluster_boxes(clusters, min_area = 0, max_area=5): if not len(clusters): return np.empty([0,4]), np.empty([0,2]) @@ -865,7 +901,7 @@ def get_cluster_boxes(clusters, min_area = 0): x_max, y_max = cluster.max(axis=0) area = (x_max-x_min) * (y_max - y_min) - if area < min_area: + if area < min_area or area > max_area: logger.warning(f"Dropping box {area} ") continue diff --git a/trap/lines.py b/trap/lines.py index 2bcf474..5b6e56d 100644 --- a/trap/lines.py +++ b/trap/lines.py @@ -59,7 +59,7 @@ class SrgbaColor(): return math.isclose(self.red, other.red) and math.isclose(self.green, other.green) and math.isclose(self.blue, other.blue) and math.isclose(self.alpha, other.alpha) def as_array(self): - return [self.red, self.green, self.blue, self.alpha] + return np.array([self.red, self.green, self.blue, self.alpha]) @dataclass @@ -510,6 +510,9 @@ class LineAnimator(StaticLine): self.start_t = time.perf_counter() return True + + def is_started(self): + return bool(self.start_t) def is_running(self): # when ready, consider not running @@ -536,9 +539,10 @@ class AppendableLineAnimator(LineAnimator): def apply(self, target_line, dt: DeltaT) -> RenderableLine: - if len(target_line) == 0: + if len(target_line) < 2: + return target_line # nothing to draw yet - return RenderableLine([]) + # return RenderableLine([]) @@ -1061,7 +1065,8 @@ class DashedLine(LineAnimator): dash = shapely.ops.substring(line, pos, end) segments.append(dash) pos += dash_len + gap_len - + + segments = [segment for segment in segments if isinstance(segment, shapely.geometry.LineString)] # TODO: return all color together with the points return shapely.geometry.MultiLineString(segments) @@ -1210,6 +1215,8 @@ class RotatingLine(LineAnimator): # progress = associated_diff.nr_of_passed_points() is_ready: List[bool] = [] + # target_point = target_line.points[-1] + for i, (target_point, drawn_point) in enumerate(zip(target_line.points, list(self.drawn_points))): # TODO: this should be done in polar space starting from origin (i.e. self.drawn_posision[-1]) decay = max(3, (18/i) if i else 10) # points further away move with more delay @@ -1220,6 +1227,7 @@ class RotatingLine(LineAnimator): r = exponentialDecay(drawn_r, pred_r, decay, dt) # make circular coordinates transition through the smaller arc + # TODO 20251108 bring this back, but calculated for the whole line: if abs(drawn_angle - pred_angle) > math.pi: pred_angle -= math.pi * 2 angle = exponentialDecay(drawn_angle, pred_angle, decay, dt) @@ -1626,4 +1634,35 @@ def layers_to_message(layers: RenderableLayers): # print( t2-t1,t3-t2) - return s \ No newline at end of file + return s + +def message_to_layers(s: str) -> RenderableLayers: + """Decode the protobuf""" + pb_layers = renderable_pb2.RenderableLayers() + pb_layers.ParseFromString(s) + + + layers = {} + + for n, pb_lines in pb_layers.layers.items(): + lines = [] + for pb_line in pb_lines.lines: + points = [] + for pb_point in pb_line.points: + color = SrgbaColor( + pb_point.color.red, + pb_point.color.green, + pb_point.color.blue, + pb_point.color.alpha, + ) + + point = RenderablePoint( + (pb_point.position.x, pb_point.position.y), + color + ) + points.append(point) + + lines.append(RenderableLine(points)) + layers[n] = RenderableLines(lines, CoordinateSpace.WORLD) + + return layers \ No newline at end of file diff --git a/trap/node.py b/trap/node.py index 1a32d21..49e5108 100644 --- a/trap/node.py +++ b/trap/node.py @@ -72,13 +72,15 @@ class Node(): return self.is_running.is_set() def check_config(self): - try: - config = self.config_sock.recv_json(zmq.NOBLOCK) - for field, value in config.items(): - self.settings[field] = value - except zmq.ZMQError as e: - # no msgs - pass + while True: + try: + config = self.config_sock.recv_json(zmq.NOBLOCK) + + for field, value in config.items(): + self.settings[field] = value + except zmq.ZMQError as e: + # no msgs + break def get_setting(self, name: str, default: Any): if name in self.settings: diff --git a/trap/prediction_server.py b/trap/prediction_server.py index ef56d5b..2275388 100644 --- a/trap/prediction_server.py +++ b/trap/prediction_server.py @@ -449,7 +449,7 @@ class PredictionServer(Node): # print('preds', len(predictions[0][0])) if not len(history) or np.isnan(history[-1]).any(): - logger.warning(f'skip for no history @ {ts_key} [{len(prediction_dict)=}, {len(histories_dict)=}, {len(futures_dict)=}]') + logger.warning(f'skip for no history for {node} @ {ts_key} [{len(prediction_dict)=}, {len(histories_dict)=}, {len(futures_dict)=}]') # logger.info(f"{preds=}") continue diff --git a/trap/process_data.py b/trap/process_data.py index b33a271..c7868f9 100644 --- a/trap/process_data.py +++ b/trap/process_data.py @@ -84,7 +84,7 @@ class TrackIteration: for n in range(noisy_variations+1): for f in range(offset_variations+1): iterations.append(TrackIteration(smooth, sample_step_size, i, noisy=bool(n), offset=bool(f))) - if toggle_smooth: + if smooth and toggle_smooth: iterations.append(TrackIteration(not smooth, sample_step_size, i, noisy=bool(n), offset=bool(f))) return iterations @@ -345,16 +345,18 @@ def process_data(src_dir: Path, dst_dir: Path, name: str, smooth_tracks: bool, n # print(f"Non-Linear: {nl}") print(f"error: {skipped_for_error}, used: {created}") print("Run with") + target_model_dir = (dst_dir / "../models/").resolve() + target_config = (dst_dir / "../trajectron.json").resolve() # set eval_every very high, because we're not interested in theoretical evaluations, and we don't mind overfitting print(f""" uv run trajectron_train --eval_every 200 \\ --train_data_dict {names['train'].name} \\ --eval_data_dict {names['val'].name} \\ --offline_scene_graph no --preprocess_workers 8 \\ - --log_dir EXPERIMENTS/models \\ + --log_dir {target_model_dir} \\ --log_tag _{name} \\ --train_epochs 100 \\ - --conf EXPERIMENTS/config.json \\ + --conf {target_config} \\ --data_dir {dst_dir} \\ {"--map_encoding" if map_img_path else ""} """) diff --git a/trap/settings.py b/trap/settings.py index 219865c..b759696 100644 --- a/trap/settings.py +++ b/trap/settings.py @@ -38,14 +38,25 @@ class Settings(Node): dpg.add_text(f"Settings from {self.config.settings_file}") dpg.add_button(label="Save", callback=self.save) + with dpg.window(label="Renderer", pos=(0, 600)): + for i in range(8) : + self.register_setting(f'stagerenderer.layer.{i}', dpg.add_checkbox(label=f"layer {i}", default_value=self.get_setting(f'stagerenderer.layer.{i}', True), callback=self.on_change)) + self.register_setting(f'stagerenderer.scale', dpg.add_slider_float(label="scale", default_value=self.get_setting(f'stagerenderer.scale', 1), max_value=3, callback=self.on_change)) + self.register_setting(f'stagerenderer.dx', dpg.add_slider_int(label="dx", default_value=self.get_setting(f'stagerenderer.dx', 0), min_value=-300, max_value=300, callback=self.on_change)) + self.register_setting(f'stagerenderer.dy', dpg.add_slider_int(label="dy", default_value=self.get_setting(f'stagerenderer.dy', 0), min_value=-300, max_value=300, callback=self.on_change)) + self.register_setting(f'stagerenderer.fade', dpg.add_slider_float(label="fade factor", default_value=self.get_setting(f'stagerenderer.fade', 0.27), max_value=1, callback=self.on_change)) + with dpg.window(label="Stage", pos=(150, 0)): self.register_setting(f'stage.fps', dpg.add_slider_int(label="FPS cap", default_value=self.get_setting(f'stage.fps', 30), callback=self.on_change)) self.register_setting(f'stage.prediction_interval', dpg.add_slider_int(label="prediction interval", default_value=self.get_setting('stage.prediction_interval', 18), callback=self.on_change)) + self.register_setting(f'stage.loitering_animation', dpg.add_checkbox(label="loitering_animation", default_value=self.get_setting('stage.loitering_animation', True), callback=self.on_change)) with dpg.window(label="Lidar", pos=(0, 100), autosize=True): self.register_setting(f'lidar.crop_map_boundaries', dpg.add_checkbox(label="crop_map_boundaries", default_value=self.get_setting(f'lidar.crop_map_boundaries', True), callback=self.on_change)) self.register_setting(f'lidar.viz_cropping', dpg.add_checkbox(label="viz_cropping", default_value=self.get_setting(f'lidar.viz_cropping', True), callback=self.on_change)) + # self.register_setting(f'lidar.voxel_downsample', dpg.add_checkbox(label="voxel_downsample", default_value=self.get_setting(f'lidar.voxel_downsample', True), callback=self.on_change)) self.register_setting(f'lidar.tracking_enabled', dpg.add_checkbox(label="tracking_enabled", default_value=self.get_setting(f'lidar.tracking_enabled', True), callback=self.on_change)) + self.register_setting(f'lidar.kalman_factor', dpg.add_slider_float(label="kalman_factor", default_value=self.get_setting(f'lidar.kalman_factor', 1.3), max_value=3, callback=self.on_change)) dpg.add_separator(label="Clustering") @@ -60,8 +71,9 @@ class Settings(Node): dpg.add_separator(label="Cluster filter") self.register_setting(f'lidar.min_box_area', dpg.add_slider_float(label="min_box_area", default_value=self.get_setting(f'lidar.min_box_area', .1), min_value=0, max_value=1, callback=self.on_change)) + self.register_setting(f'lidar.max_box_area', dpg.add_slider_float(label="max_box_area", default_value=self.get_setting(f'lidar.max_box_area', 5), min_value=.5, max_value=10, callback=self.on_change)) - for i, lidar in enumerate(["192.168.0.16", "192.168.0.10"]): + for i, lidar in enumerate(["192.168.1.16", "192.168.0.10"]): name = lidar.replace(".", "_") with dpg.window(label=f"Lidar {lidar}", pos=(i * 300, 450),autosize=True): # dpg.add_text("test") diff --git a/trap/stage.py b/trap/stage.py index 52dd348..2248fa1 100644 --- a/trap/stage.py +++ b/trap/stage.py @@ -119,6 +119,9 @@ class PrioritySlotItem(): self.start_time = time.perf_counter() self.is_running = True + def running_for(self): + return time.perf_counter() - self.start_time + @abstractmethod def get_priority(self) -> int: raise RuntimeError("Not implemented") @@ -161,7 +164,11 @@ class Scenario(PrioritySlotItem): return self.scene.name def get_priority(self) -> int: - return self.scene.value.priority + # newer higher prio + distance = 0 + if self.track and len(self.track.projected_history) > 5: + distance = np.linalg.norm(self.track.projected_history[-1] - self.track.projected_history[0]) + return (self.scene.value.priority, distance) def can_be_taken_over(self): if self.scene.value.takeover_possible: @@ -221,11 +228,12 @@ class Scenario(PrioritySlotItem): def set_scene(self, scene: ScenarioScene): if self.scene is scene: - return + return False logger.info(f"Changing scene for {self.track_id}: {self.scene.name} -> {scene.name}") self.scene = scene self.state_change_at = time.perf_counter() + return True def update_state(self): self.check_lost() or self.check_loitering() or self.check_track() @@ -252,10 +260,10 @@ class Scenario(PrioritySlotItem): def check_track(self): predictions = len(self.prediction_tracks) - if predictions == 1: + if predictions and self.running_for() < 20: self.set_scene(ScenarioScene.FIRST_PREDICTION) return True - if predictions > 30: + if predictions and self.running_for() > 120: self.set_scene(ScenarioScene.PLAY) return True if predictions: @@ -350,7 +358,7 @@ class DrawnScenario(Scenario): history_color = SrgbaColor(1.,0.,1.,1.) history = StaticLine([], history_color) self.line_history = LineAnimationStack(history) - self.line_history.add(AppendableLineAnimator(self.line_history.tail, draw_decay_speed=120)) + self.line_history.add(AppendableLineAnimator(self.line_history.tail, draw_decay_speed=120, transition_in_on_init=False)) self.line_history.add(CropLine(self.line_history.tail, self.MAX_HISTORY)) self.line_history.add(SimplifyLine(self.line_history.tail, 0.003)) # Simplify before effects, so they don't distort self.line_history.add(FadedTailLine(self.line_history.tail, TRACK_FADE_AFTER_DURATION * TRACK_ASSUMED_FPS, TRACK_END_FADE)) @@ -394,10 +402,12 @@ class DrawnScenario(Scenario): if self.track: self.line_history.root.points = self.track.projected_history - lf = self.lost_factor() - self.line_history.get(FadeOutJitterLine).set_alpha(1-lf) - self.line_prediction.get(FadeOutLine).set_alpha(1-lf) - self.line_history.get(NoiseLine).amplitude = lf * 1.8 + lost_factor = self.lost_factor() # fade out when lost + start_factor = 0#1 - min(1, self.running_for()) # fade in when starting + # print(start_factor) + self.line_history.get(FadeOutJitterLine).set_alpha(1- lost_factor - start_factor) + self.line_prediction.get(FadeOutLine).set_alpha(1-lost_factor) + self.line_history.get(NoiseLine).amplitude = lost_factor * 1.8 if len(self.prediction_tracks): # now_p = np.array(self.line_history.root.points[-1]) @@ -408,49 +418,50 @@ class DrawnScenario(Scenario): # TODO: only when animation is ready for it? or collect lines - if not self.active_ptrack: - # draw the first prediction - self.active_ptrack = self.prediction_tracks[-1] - self.line_prediction.root.points = self.active_ptrack._track.predictions[0] + if self.is_running: + if not self.active_ptrack: + # draw the first prediction + self.active_ptrack = self.prediction_tracks[-1] + self.line_prediction.root.points = self.active_ptrack._track.predictions[0] - self.line_prediction.start() # reset positions - - elif self.active_ptrack._track.updated_at < self.prediction_tracks[-1]._track.updated_at: - # stale prediction - # switch only if drawing animation is ready - # if self.line_prediction.is_ready(): - self.active_ptrack = self.prediction_tracks[-1] - self.line_prediction.root.points = self.active_ptrack._track.predictions[0] - if self.line_prediction.is_ready() and self.line_prediction.get(DashedLine).skip == True: - self.line_prediction.get(SegmentLine).skip = True - self.line_prediction.get(DashedLine).skip = False - self.line_prediction.start() # reset positions - - # self.line_prediction.get(SegmentLine).anim_f = partial(SegmentLine.anim_arrive, length=.3) - # self.line_prediction.get(SegmentLine).duration = .5 - # self.line_prediction.get(DashedLine).skip = True - # # print('restart') - # self.line_prediction.start() # reset positions - # # print(self.line_prediction.get(SegmentLine).running_for()) - # else: - # if self.line_prediction.is_ready(): - # # little hack: check is dashedline skips, to only run this once per animation: - # if self.line_prediction.get(DashedLine).skip: - # # no new yet, but ready with anim, start stage 2 - # self.line_prediction.get(SegmentLine).anim_f = partial(SegmentLine.anim_grow) - # self.line_prediction.get(SegmentLine).duration = 1 - # # self.line_prediction.get(SegmentLine).skip = True - # self.line_prediction.get(DashedLine).skip = False - # self.line_prediction.start() - # elif self.line_prediction.get(SegmentLine).duration != 2: # hack to only play once - # self.line_prediction.get(SegmentLine).anim_f = partial(SegmentLine.anim_grow, reverse=True) - # self.line_prediction.get(SegmentLine).duration = 2 - # self.line_prediction.get(SegmentLine).start() - - if self.active_ptrack: - # TODO: this should crop by distance/lenght - self.line_prediction.get(CropLine).start_offset = self.track._track.frame_index - self.active_ptrack._track.frame_index + + elif self.active_ptrack._track.updated_at < self.prediction_tracks[-1]._track.updated_at: + # stale prediction + # switch only if drawing animation is ready + # if self.line_prediction.is_ready(): + self.active_ptrack = self.prediction_tracks[-1] + self.line_prediction.root.points = self.active_ptrack._track.predictions[0] + if self.line_prediction.is_ready() and self.line_prediction.get(DashedLine).skip == True: + self.line_prediction.get(SegmentLine).skip = True + self.line_prediction.get(DashedLine).skip = False + + self.line_prediction.start() # reset positions + + # self.line_prediction.get(SegmentLine).anim_f = partial(SegmentLine.anim_arrive, length=.3) + # self.line_prediction.get(SegmentLine).duration = .5 + # self.line_prediction.get(DashedLine).skip = True + # # print('restart') + # self.line_prediction.start() # reset positions + # # print(self.line_prediction.get(SegmentLine).running_for()) + # else: + # if self.line_prediction.is_ready(): + # # little hack: check is dashedline skips, to only run this once per animation: + # if self.line_prediction.get(DashedLine).skip: + # # no new yet, but ready with anim, start stage 2 + # self.line_prediction.get(SegmentLine).anim_f = partial(SegmentLine.anim_grow) + # self.line_prediction.get(SegmentLine).duration = 1 + # # self.line_prediction.get(SegmentLine).skip = True + # self.line_prediction.get(DashedLine).skip = False + # self.line_prediction.start() + # elif self.line_prediction.get(SegmentLine).duration != 2: # hack to only play once + # self.line_prediction.get(SegmentLine).anim_f = partial(SegmentLine.anim_grow, reverse=True) + # self.line_prediction.get(SegmentLine).duration = 2 + # self.line_prediction.get(SegmentLine).start() + + if self.active_ptrack: + # TODO: this should crop by distance/lenght + self.line_prediction.get(CropLine).start_offset = self.track._track.frame_index - self.active_ptrack._track.frame_index @@ -459,7 +470,7 @@ class DrawnScenario(Scenario): # special case: LOITERING - if self.scene is ScenarioScene.LOITERING: # or self.state_change_at: + if self.stage.get_setting('stage.loitering_animation', True) and self.scene is ScenarioScene.LOITERING: # or self.state_change_at: # logger.info('loitering') transition = min(1, (time.perf_counter() - self.state_change_at)/1.4) # print('loitering factor', transition) @@ -555,7 +566,15 @@ class DrawnScenario(Scenario): prediction_line, others_line ]), timings - + + def set_scene(self, scene): + """Create log message for the auxilary interface + """ + original = self.scene.name + changed = super().set_scene(scene) + if changed: + self.stage.log_sock.send_string(f"Change {self.track_id}: {original} -> {self.scene.name}") + return changed class NoTracksScenario(PrioritySlotItem): TAKEOVER_FADEOUT = 1 # override default to be faster @@ -567,7 +586,7 @@ class NoTracksScenario(PrioritySlotItem): def get_priority(self): # super low priority - return -1 + return (-1, -1) def can_be_taken_over(self): return True @@ -599,6 +618,21 @@ class NoTracksScenario(PrioritySlotItem): return lines, timings +class DebugDrawer(): + def __init__(self, stage: Stage): + self.stage = stage + + def to_renderable_lines(self, dt: DeltaT): + lines = RenderableLines([], CoordinateSpace.WORLD) + past_color = SrgbaColor(1,0,1,1) + future_color = SrgbaColor(0,1,0,1) + for scenario in self.stage.scenarios.values(): + lines.append(StaticLine(scenario.track.projected_history, past_color).as_renderable_line(dt)) + if scenario.active_ptrack: + lines.append(StaticLine(scenario.active_ptrack._track.predictions[0], future_color).as_renderable_line(dt)) + return lines + + class DatasetDrawer(): def __init__(self, stage: Stage): self.stage = stage @@ -649,6 +683,8 @@ class Stage(Node): self.prediction_sock = self.sub(self.config.zmq_prediction_addr) self.detection_sock = self.sub(self.config.zmq_detection_addr) self.stage_sock = self.pub(self.config.zmq_stage_addr) + self.log_sock = self.push(self.config.zmq_log_addr) + # self.stage_py_sock = self.pub(self.config.zmq_stage_py_addr) self.counter = CounterSender() @@ -659,6 +695,7 @@ class Stage(Node): self.history = TrackHistory(self.config.tracker_output_dir, self.config.camera, self.config.cache_path) self.auxilary = DatasetDrawer(self) + self.debug_drawer = DebugDrawer(self) # 'screensavers' self.notrack_scenarios = [] #[NoTracksScenario(self, i) for i in range(self.config.max_active_scenarios)] @@ -758,6 +795,7 @@ class Stage(Node): # TODO: sometimes very slow! t1 = time.perf_counter() training_lines = self.auxilary.to_renderable_lines(dt) + all_active_tracks = self.debug_drawer.to_renderable_lines(dt) t2 = time.perf_counter() @@ -782,6 +820,7 @@ class Stage(Node): 1: lines, 2: self.debug_lines, 3: training_lines, + 4: all_active_tracks, } t4 = time.perf_counter() @@ -791,6 +830,7 @@ class Stage(Node): t5 = time.perf_counter() self.stage_sock.send(msg) + # self.stage_sock.send_pyobj(layers) # self.stage_sock.send_json(obj=layers, cls=DataclassJSONEncoder) @@ -831,6 +871,14 @@ class Stage(Node): help='Manually specity communication addr for the stage messages (the rendered lines)', type=str, default="tcp://0.0.0.0:99174") + argparser.add_argument('--zmq-log-addr', + help='Manually specity communication addr for the log messages', + type=str, + default="tcp://0.0.0.0:99188") + argparser.add_argument('--zmq-stage-py-addr', + help='Sometimes there is no need for protobuf', + type=str, + default="ipc:///tmp/feeds_stage") argparser.add_argument('--debug-map', help='specify a map (svg-file) from which to load lines which will be overlayed', type=str, diff --git a/trap/stage_renderer.py b/trap/stage_renderer.py new file mode 100644 index 0000000..ed01d76 --- /dev/null +++ b/trap/stage_renderer.py @@ -0,0 +1,255 @@ + + +from argparse import ArgumentParser +from collections import deque +import math +from typing import List +import numpy as np +import pyglet +from torch import mul +import zmq +from trap.lines import RenderableLayers, message_to_layers +from trap.node import Node + +BG_COLOR = (0,0,0) +class StageRenderer(Node): + def setup(self): + # self.prediction_sock = self.sub(self.config.zmq_prediction_addr) + # self.tracker_sock = self.sub(self.config.zmq_trajectory_addr) + # self.detector_sock = self.sub(self.config.zmq_detection_addr) + # self.frame_sock = self.sub(self.config.zmq_frame_addr) + self.stage_sock = self.sub(self.config.zmq_stage_addr) + self.log_sock = self.pull(self.config.zmq_log_addr) + + # setup pyglet: + display = pyglet.display.get_display() + screens = display.get_screens() + + # use configured montior, fall back to whatever is available + self.screen = sorted(screens, reverse=True, key=lambda s: s.get_monitor_name() == self.config.monitor)[0] + + if self.screen.get_monitor_name() != self.config.monitor: + self.logger.warning(f"Not displaying on configured monitor. {self.screen.get_monitor_name()} instead of {self.config.monitor}") + + # print(self.screen.get_modes()) + + + config = pyglet.gl.Config(sample_buffers=1, samples=4) + + # when screen is in portrait, window mode here expects still (larger x smaller) number. + # self.window.get_size() will be reported properly + wh = sorted((self.screen.width, self.screen.height), reverse=self.config.fullscreen) + + self.window = pyglet.window.Window(width=wh[0], height=wh[1], config=config, fullscreen=self.config.fullscreen, screen=self.screen) + self.window.set_exclusive_keyboard(True) + self.window.set_exclusive_keyboard(False) + self.window.set_exclusive_mouse(True) + self.window.set_exclusive_mouse(False) + + # self.window.set_size(1080, 1920) + + window_size = self.window.get_size() + print(window_size) + self.window.set_handler('on_draw', self.on_draw) + # self.window.set_handler('on_close', self.on_close) + + # pyglet.gl.glClearColor(81./255, 20/255, 46./255, 0) + pyglet.gl.glClearColor(0/255, 0/255, 255/255, 0) + 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.current_layers: RenderableLayers = {} + + self.lines: List[pyglet.shapes.Line] = [] + self.lines_batch = pyglet.graphics.Batch() + self.text = pyglet.text.document.FormattedDocument("") + self.text_batch = pyglet.graphics.Batch() + self.text_layout = pyglet.text.layout.TextLayout( + self.text, 20, 350, + width=self.window.get_size()[1], + height=self.window.get_size()[0] // 3, + multiline=True, wrap_lines=False, batch=self.text_batch) + + max_len = 30 + self.log_msgs = deque([], maxlen=max_len) + self.log_msgs.extend(["..."] * max_len) + + + translate = (10,-400) + # scale = 5 + + smallest_dimension = min(self.window.get_size()) + max_x = 16.3 + max_y = 14.3 + scale = min(smallest_dimension / max_x, smallest_dimension/max_y) + + padding = 40 + + self.logger.info(f"Use {scale=}") + + + self.transform = np.array([ + [scale, 0,translate[0]], + [0,-scale,window_size[1]], + [0,0,1] + ]) + + self.bg_image = pyglet.image.load(self.config.floorplan) + scale = (window_size[0] - padding*2) / (self.bg_image.width) + print('image_scale', scale, self.bg_image.width, self.bg_image.height) + # self.bg_image.height = int(self.bg_image.height / 3) + # self.bg_image.width = int(self.bg_image.width / 3) + img_y = window_size[1]-int(self.bg_image.height*scale)-padding*2 + self.bg_sprite = pyglet.sprite.Sprite(img=self.bg_image, x=padding, y=img_y) + self.bg_sprite.scale = scale + + + clear_area = img_y + self.clear_transparent = pyglet.shapes.Rectangle(0, window_size[1]-clear_area, window_size[0], clear_area, color=(*BG_COLOR,255//70)) + self.clear_fully= pyglet.shapes.Rectangle(0, 0, window_size[0], window_size[1]-clear_area, color=(*BG_COLOR,255)) + + + def check_running(self, dt): + if not self.run_loop(): + self.window.close() + self.event_loop.exit() + + def run(self): + self.event_loop = pyglet.app.EventLoop() + pyglet.clock.schedule_interval(self.check_running, 0.1) + # pyglet.clock.schedule(self.receive) + self.event_loop.run() + + + def receive(self, dt): + try: + msg = self.stage_sock.recv(zmq.NOBLOCK) + self.current_layers = message_to_layers(msg) + self.update_lines() + except zmq.ZMQError as e: + # idx = frame.index if frame else "NONE" + # logger.debug(f"reuse video frame {idx}") + pass + + while True: + try: + log_msg = self.log_sock.recv_string(zmq.NOBLOCK) + self.log_msgs.append(log_msg) + except zmq.ZMQError as e: + # idx = frame.index if frame else "NONE" + # logger.debug(f"reuse video frame {idx}") + break + self.update_msgs() + + + def update_lines(self): + """ + Render the renderable lines of selected layers + """ + + additional_scale = self.get_setting('stagerenderer.scale', 1) + dx = self.get_setting('stagerenderer.dx', 0) + dy = self.get_setting('stagerenderer.dy', 0) + transform = self.transform.copy() + transform[0][0] *= additional_scale + transform[1][1] *= additional_scale + transform[0][2] += dx + transform[1][2] += dy + + i = -1 + for nr, lines in self.current_layers.items(): + + if not self.get_setting(f'stagerenderer.layer.{nr}', True): + continue + + + for line in lines.lines: + for p1, p2 in zip(line.points, line.points[1:]): + i += 1 + pp1 = np.array([p1.position[0], p1.position[1], 1]) + pp2 = np.array([p2.position[0], p2.position[1], 1]) + + pos1 = (transform@pp1)[:2].astype(int) + pos2 = (transform@pp2)[:2].astype(int) + + color = (p2.color.as_array()*255).astype(int) + + if i < len(self.lines): + print('reuse') + shape = self.lines[i] + shape.x = pos1[0] + shape.y = pos1[1] + shape.x2 = pos2[0] + shape.y2 = pos2[1] + shape.color = color + + self.lines.append(pyglet.shapes.Line(pos1[0], pos1[1], + pos2[0], + pos2[1], + 3, + color, + batch=self.lines_batch)) + + print(len(self.lines), i) + too_many = len(self.lines) - 1 - i + if too_many > 0: + print('del', too_many) + for j in reversed(range(i, i+too_many)): + self.lines[i].delete() + del self.lines[i] + + + def update_msgs(self): + text = "\n".join(self.log_msgs) + self.text.text = text + self.text.set_style(0, len(self.text.text), dict( + font_name='Arial', # change to a font installed on your system + font_size=18, + color=(255, 255, 255, 255), + )) + + + def on_draw(self): + self.receive(.1) + self.window.clear() + # self.clear_transparent.color = (*BG_COLOR, int(255*self.get_setting('stagerenderer.fade', .27))) + # self.clear_transparent.draw() + # self.clear_fully.draw() + self.fps_display.draw() + + # self.bg_sprite.draw() + + # self.lines_batch.draw() + # self.text_batch.draw() + + + @classmethod + def arg_parser(cls): + render_parser = ArgumentParser() + + render_parser.add_argument('--zmq-stage-addr', + help='Manually specity communication addr for the stage messages (the rendered lines)', + type=str, + default="tcp://0.0.0.0:99174") + render_parser.add_argument('--zmq-log-addr', + help='Manually specity communication addr for the log messages', + type=str, + default="tcp://0.0.0.0:99188") + + render_parser.add_argument("--fullscreen", + help="Set Window full screen", + action='store_true') + + render_parser.add_argument('--floorplan', + help='specify a map (png-file) onto which overlayed', + type=str, + default="SETTINGS/2025-11-dortmund/space/floorplan.png") + render_parser.add_argument('--monitor', + help='Specify a screen on which to output (eg. HDMI-1)', + type=str, + default="HDMI-1") + return render_parser + diff --git a/trap/tracker.py b/trap/tracker.py index 977e6f5..e1cd680 100644 --- a/trap/tracker.py +++ b/trap/tracker.py @@ -833,7 +833,8 @@ class Smoother(TrackPointFilter): else: # "Unlike Kalman filtering, which focuses on predicting and updating the current state using historical measurements, Kalman smoothing enhances the accuracy of past state values" # see https://medium.com/@shahalkp1/kalman-smoothing-using-tsmoothie-0175260464e5 - self.smoother = KalmanSmoother(component='level_trend', component_noise={'level':0.02, 'season': .01, 'trend':0.02},n_seasons = 2, copy=None) + # self.smoother = KalmanSmoother(component='level_trend', component_noise={'level':0.02, 'season': .01, 'trend':0.02},n_seasons = 2, copy=False) + self.smoother = KalmanSmoother(component='level', component_noise={'level':0.01},observation_noise=.3, n_seasons = 0, copy=False) diff --git a/uv.lock b/uv.lock index 5c9d0b8..c4a8417 100644 --- a/uv.lock +++ b/uv.lock @@ -1889,11 +1889,11 @@ wheels = [ [[package]] name = "pyglet" -version = "2.1.3" +version = "2.1.11" source = { registry = "https://pypi.org/simple" } -sdist = { url = "https://files.pythonhosted.org/packages/08/90/7f8a8d939dbf8f6875b957540cc3091e936e41c4ac8f190a9517589678f8/pyglet-2.1.3.tar.gz", hash = "sha256:9a2c3c84228402ea7103443ac8a52060cc1c91419951ec1105845ce30fed2ce8", size = 6515859 } +sdist = { url = "https://files.pythonhosted.org/packages/e3/6b/84c397a74cd33eb377168c682e9e3d6b90c1c10c661e11ea5b397ac8497c/pyglet-2.1.11.tar.gz", hash = "sha256:8285d0af7d0ab443232a81df4d941e0d5c48c18a23ec770b3e5c59a222f5d56e", size = 6594448 } wheels = [ - { url = "https://files.pythonhosted.org/packages/83/d6/41208b6741e732a7faf160e89346a17e81b14899bd7ae90058da858083d6/pyglet-2.1.3-py3-none-any.whl", hash = "sha256:5a7eaf35869ecf7451fb49cc064c4c0e9a118eaa5e771667c607125b13f85e33", size = 962091 }, + { url = "https://files.pythonhosted.org/packages/77/a2/2b09fbff0eedbe44fbf164b321439a38f7c5568d8b754aa197ee45886431/pyglet-2.1.11-py3-none-any.whl", hash = "sha256:fa0f4fdf366cfc5040aeb462416910b0db2fa374b7d620b7a432178ca3fa8af1", size = 1032213 }, ] [[package]] @@ -2825,7 +2825,7 @@ requires-dist = [ { name = "opencv-python", path = "opencv_python-4.10.0.84-cp310-cp310-linux_x86_64.whl" }, { name = "pandas-helper-calc", git = "https://github.com/scls19fr/pandas-helper-calc" }, { name = "py-to-proto", specifier = ">=0.6.0" }, - { name = "pyglet", specifier = ">=2.0.15,<3" }, + { name = "pyglet", specifier = ">=2.1.8,<3" }, { name = "pyglet-cornerpin", specifier = ">=0.3.0,<0.4" }, { name = "python-statemachine", specifier = ">=2.5.0" }, { name = "pyusb", specifier = ">=1.3.1,<2" },