From 472eebf9a08235d1740d91719dc5b03907f8ab89 Mon Sep 17 00:00:00 2001 From: Ruben van de Ven Date: Wed, 5 Nov 2025 10:47:43 +0100 Subject: [PATCH] Multiple changes to stage and line drawing --- trap/lines.py | 73 +++++++++++++++++++- trap/prediction_server.py | 14 ++-- trap/settings.py | 3 + trap/stage.py | 141 ++++++++++++++++++++++++++++++-------- 4 files changed, 198 insertions(+), 33 deletions(-) diff --git a/trap/lines.py b/trap/lines.py index d66cce8..49c2f20 100644 --- a/trap/lines.py +++ b/trap/lines.py @@ -5,6 +5,7 @@ import copy from dataclasses import dataclass from enum import Enum, IntEnum from functools import partial +import logging import math from pathlib import Path import time @@ -21,13 +22,16 @@ import svgpathtools from noise import snoise2 from trap import renderable_pb2 -from trap.utils import exponentialDecayRounded, inv_lerp +from trap.utils import exponentialDecay, exponentialDecayRounded, inv_lerp, relativePointToPolar, relativePolarToPoint """ See [notebook](../test_path_transforms.ipynb) for examples """ + +logger = logging.getLogger('lines') + RenderablePosition = Tuple[float,float] Coordinate = Tuple[float, float] DeltaT = float # delta_t in seconds @@ -526,6 +530,12 @@ class AppendableLineAnimator(LineAnimator): self.drawn_points.append(copy.deepcopy(self.drawn_points[-1])) idx = len(self.drawn_points) - 1 + if idx > len(target_line.points) - 1: + logger.warning("Target line shorter that appendable line, shorten") + self.drawn_points = self.drawn_points[:len(target_line)] + idx = len(self.drawn_points) - 1 + + target = target_line.points[idx] if np.isclose(self.drawn_points[-1].position, target.position, atol=.05).all(): @@ -733,6 +743,7 @@ class FadedTailLine(LineAnimator): return RenderableLine(points) + class NoiseLine(LineAnimator): """ @@ -966,7 +977,67 @@ class DashedLine(LineAnimator): color = target_line.points[0].color return RenderableLine.from_multilinestring(multilinestring, color) + + + +class RotatingLine(LineAnimator): + """ + Rotate the line around starting point towards new shape + """ + def __init__(self, target_line = None, decay_speed=16): + super().__init__(target_line) + self.decay_speed = decay_speed + self.drawn_points: List[RenderablePoint] = [] + + + + def apply(self, target_line: RenderableLine, dt: DeltaT) -> RenderableLine: + """ + warning, dashing (for now) removes all color + """ + + if len(target_line) < 2: + self.ready = True + return target_line + + origin = target_line.points[0] + + if len(self.drawn_points) < 1: + self.drawn_points = target_line.points + + diff_length = len(target_line) - len(self.drawn_points) + if diff_length < 0: # drawn points is larger + self.drawn_points = self.drawn_points[:len(target_line)] + if diff_length > 0: # target line is larger + self.drawn_points += [self.drawn_points[-1]] * diff_length + + # associated_diff = self.prediction_diffs[a] + # progress = associated_diff.nr_of_passed_points() + 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 + decay = self.decay_speed + drawn_r, drawn_angle = relativePointToPolar( origin.position, drawn_point.position) + pred_r, pred_angle = relativePointToPolar(origin.position, target_point.position) + r = exponentialDecay(drawn_r, pred_r, decay, dt) + + # make circular coordinates transition through the smaller arc + if abs(drawn_angle - pred_angle) > math.pi: + pred_angle -= math.pi * 2 + angle = exponentialDecay(drawn_angle, pred_angle, decay, dt) + x, y = relativePolarToPoint(origin, r, angle) + + r = exponentialDecay(drawn_point.color.red, target_point.color.red, decay, dt) + g = exponentialDecay(drawn_point.color.green, target_point.color.green, decay, dt) + b = exponentialDecay(drawn_point.color.blue, target_point.color.blue, decay, dt) + a = exponentialDecay(drawn_point.color.alpha, target_point.color.alpha, decay, dt) + self.drawn_points[i].position = (x, y) + self.drawn_points[i].color = SrgbaColor(r, g, b, a) + + + return RenderableLine(self.drawn_points) + IndexAndOffset = Tuple[int, float] diff --git a/trap/prediction_server.py b/trap/prediction_server.py index fde2935..4e6bf24 100644 --- a/trap/prediction_server.py +++ b/trap/prediction_server.py @@ -104,11 +104,15 @@ def get_maps_for_input(input_dict, scene, hyperparams, device): # print(scene_maps, patch_sizes, heading_angles) # print(scene_pts) - maps = scene_maps[0].get_cropped_maps_from_scene_map_batch(scene_maps, - scene_pts=torch.Tensor(scene_pts), - patch_size=patch_sizes[0], - rotation=heading_angles, - device='cpu') + try: + maps = scene_maps[0].get_cropped_maps_from_scene_map_batch(scene_maps, + scene_pts=torch.Tensor(scene_pts), + patch_size=patch_sizes[0], + rotation=heading_angles, + device='cpu') + except Exception as e: + logger.warning(f"Crash on getting maps for points: {scene_pts=} {heading_angles=} {patch_size=}") + raise e maps_dict = {node: maps[[i]].to(device) for i, node in enumerate(nodes_with_maps)} return maps_dict diff --git a/trap/settings.py b/trap/settings.py index 87ddced..bfc3342 100644 --- a/trap/settings.py +++ b/trap/settings.py @@ -41,11 +41,13 @@ class Settings(Node): with dpg.window(label="Lidar", pos=(0, 150)): 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.tracking_enabled', dpg.add_checkbox(label="tracking_enabled", default_value=self.get_setting(f'lidar.tracking_enabled', True), callback=self.on_change)) for lidar in ["192.168.0.16", "192.168.0.10"]: name = lidar.replace(".", "_") with dpg.window(label=f"Lidar {lidar}", pos=(200, 0),autosize=True): # dpg.add_text("test") # dpg.add_input_text(label="string", default_value="Quick brown fox") + self.register_setting(f'lidar.{name}.enabled', dpg.add_checkbox(label="enabled", default_value=self.get_setting(f'lidar.{name}.enabled', True), callback=self.on_change)) self.register_setting(f'lidar.{name}.rot_x', dpg.add_slider_float(label="rot_x", default_value=self.get_setting(f'lidar.{name}.rot_x', 0), max_value=math.pi * 2, callback=self.on_change)) self.register_setting(f'lidar.{name}.rot_y', dpg.add_slider_float(label="rot_y", default_value=self.get_setting(f'lidar.{name}.rot_y', 0), max_value=math.pi * 2, callback=self.on_change)) self.register_setting(f'lidar.{name}.rot_z', dpg.add_slider_float(label="rot_z", default_value=self.get_setting(f'lidar.{name}.rot_z', 0), max_value=math.pi * 2, callback=self.on_change)) @@ -87,6 +89,7 @@ class Settings(Node): def on_change(self, sender, value, user_data = None): # print(sender, app_data, user_data) setting = self.settings_fields[sender] + print(setting, value) self.settings[setting] = value self.config_sock.send_json({setting: value}) diff --git a/trap/stage.py b/trap/stage.py index 4e8dd15..a38c4b3 100644 --- a/trap/stage.py +++ b/trap/stage.py @@ -1,5 +1,6 @@ from __future__ import annotations +from abc import abstractmethod from argparse import ArgumentParser from collections import defaultdict from dataclasses import dataclass @@ -21,7 +22,7 @@ import zmq from trap.anomaly import DiffSegment, calc_anomaly, calculate_loitering_scores from trap.base import CameraAction, DataclassJSONEncoder, Frame, HomographyAction, ProjectedTrack, Track from trap.counter import CounterSender -from trap.lines import AppendableLine, AppendableLineAnimator, Coordinate, CoordinateSpace, CropAnimationLine, CropLine, DashedLine, DeltaT, FadeOutJitterLine, FadeOutLine, FadedEndsLine, FadedTailLine, LineAnimationStack, LineAnimator, NoiseLine, RenderableLayers, RenderableLine, RenderableLines, SegmentLine, SimplifyMethod, SrgbaColor, StaticLine, layers_to_message, load_lines_from_svg +from trap.lines import AppendableLine, AppendableLineAnimator, Coordinate, CoordinateSpace, CropAnimationLine, CropLine, DashedLine, DeltaT, FadeOutJitterLine, FadeOutLine, FadedEndsLine, FadedTailLine, LineAnimationStack, LineAnimator, NoiseLine, RenderableLayers, RenderableLine, RenderableLines, RotatingLine, SegmentLine, SimplifyMethod, SrgbaColor, StaticLine, layers_to_message, load_lines_from_svg from trap.node import Node from trap.track_history import TrackHistory @@ -36,7 +37,7 @@ OPTION_TRACK_NOISE = False TRACK_ASSUMED_FPS = 12 -TAKEOVER_FADEOUT = 3 + LOST_FADEOUT = 2 # seconds PREDICTION_INTERVAL: float|None = 20 # frames PREDICTION_FADE_IN: float = 3 @@ -81,13 +82,62 @@ class ScenarioScene(Enum): Time = float -class Scenario: +class PrioritySlotItem(): + TAKEOVER_FADEOUT = 3 + + def __init__(self, identifier): + self.identifier = identifier + self.start_time = 0. + self.take_over_at: Optional[Time] = None + + def take_over(self): + if self.take_over_at: + return + + self.take_over_at = time.perf_counter() + + def taken_over(self): + self.is_running = False + self.take_over_at = None + + def takenover_for(self): + if self.take_over_at: + return time.perf_counter() - self.take_over_at + return None + + def takeover_factor(self): + l = self.takenover_for() + if not l: + return 0 + return l/self.TAKEOVER_FADEOUT + + def start(self): + # change when visible + logger.info(f"Start {self.identifier}: {self.get_state_name()}") + self.start_time = time.perf_counter() + self.is_running = True + + @abstractmethod + def get_priority(self) -> int: + raise RuntimeError("Not implemented") + + @abstractmethod + def get_state_name(self) -> str: + raise RuntimeError("Not implemented") + + @abstractmethod + def can_be_taken_over(self): + raise RuntimeError("Not implemented") + + +class Scenario(PrioritySlotItem): def __init__(self, track_id): + super().__init__(track_id) self.track_id = track_id self.scene: ScenarioScene = ScenarioScene.DETECTED - self.start_time = 0. + self.current_time = 0 - self.take_over_at: Optional[Time] = None + self.track: Optional[ProjectedTrack] = None self.prediction_tracks: List[ProjectedTrack] = [] @@ -104,10 +154,14 @@ class Scenario: logger.info(f"Found {self.track_id}: {self.scene.name}") - def start(self): - # change when visible - logger.info(f"Start {self.track_id}: {self.scene.name}") - self.is_running = True + def get_state_name(self): + return self.scene.name + + def get_priority(self) -> int: + return self.scene.value.priority + + def can_be_taken_over(self): + return self.scene.value.takeover_possible def track_age(self): if not self.track: @@ -133,7 +187,7 @@ class Scenario: l = self.takenover_for() if not l: return 0 - return l/TAKEOVER_FADEOUT + return l/self.TAKEOVER_FADEOUT def lost_for(self): @@ -179,6 +233,10 @@ class Scenario: def check_loitering(self): scores = [s for s in calculate_loitering_scores(self.track, LOITERING_DURATION_TO_LINGER, LOITERING_LINGER_FACTOR, LOITERING_VELOCITY_TRESHOLD/TRACK_ASSUMED_FPS, 150)] + + if not len(scores): + logger.warning(f"No loitering score for {self.track_id}") + return False self.loitering_factor = scores[-1] if self.loitering_factor > .99: @@ -274,7 +332,7 @@ class DrawnScenario(Scenario): This distinction is only for ordering the code """ - MAX_HISTORY = 300 # points of history of trajectory to display (preventing too long lines) + MAX_HISTORY = 200 # points of history of trajectory to display (preventing too long lines) CUT_GAP = 5 # when adding a new prediction, keep the existing prediction until that point + this CUT_GAP margin def __init__(self, track_id): @@ -294,6 +352,8 @@ class DrawnScenario(Scenario): self.prediction_color = SrgbaColor(0,1,0,1) self.line_prediction = LineAnimationStack(StaticLine([], self.prediction_color)) + self.line_prediction.add(RotatingLine(self.line_prediction.tail, decay_speed=16)) + self.line_prediction.get(RotatingLine).skip = True self.line_prediction.add(SegmentLine(self.line_prediction.tail, duration=.5)) self.line_prediction.add(DashedLine(self.line_prediction.tail, t_factor=4, loop_offset=True)) self.line_prediction.get(DashedLine).skip = True @@ -451,12 +511,29 @@ class DrawnScenario(Scenario): ]), timings -class NoTracksScenario(): - def __init__(self, stage: Stage): +class NoTracksScenario(PrioritySlotItem): + TAKEOVER_FADEOUT = 1 # override default to be faster + + def __init__(self, stage: Stage, i: int): + super().__init__(f"screensaver_{i}") self.stage = stage self.line = build_line_others() + + def get_priority(self): + # super low priority + return -1 + + def can_be_taken_over(self): + return True + + def get_state_name(self): + return "previewing" + + def update(self, stage: Stage): + pass def to_renderable_lines(self, dt: DeltaT): + timings = [] lines = RenderableLines([], CoordinateSpace.WORLD) if not self.line.is_running(): track_id = random.choice(list(self.stage.history.state.tracks.keys())) @@ -464,13 +541,17 @@ class NoTracksScenario(): positions = self.stage.history.state.track_histories[track_id] self.line.root.points = positions self.line.start() + + alpha = 1 - self.takeover_factor() + self.line.get(FadeOutLine).set_alpha(alpha) lines.lines.append( self.line.as_renderable_line(dt) ) - # print(lines) + - return lines + return lines, timings + class DatasetDrawer(): def __init__(self, stage: Stage): @@ -529,7 +610,8 @@ class Stage(Node): self.auxilary = DatasetDrawer(self) - self.notrack_lines = [NoTracksScenario() for i in range(self.config.max_active_scenarios)] + # 'screensavers' + self.notrack_scenarios = [] #[NoTracksScenario(self, i) for i in range(self.config.max_active_scenarios)] @@ -591,9 +673,9 @@ class Stage(Node): # 3) determine set of pending scenarios (all except running) - pending_scenarios = [s for s in self.scenarios.values() if s not in self.active_scenarios] + pending_scenarios = [s for s in list(self.scenarios.values()) + self.notrack_scenarios if s not in self.active_scenarios] # ... highest priority first - pending_scenarios.sort(key=lambda s: s.scene.value.priority, reverse=True) + pending_scenarios.sort(key=lambda s: s.get_priority(), reverse=True) # 4) check if there's a slot free: while len(self.active_scenarios) < self.config.max_active_scenarios and len(pending_scenarios): @@ -604,15 +686,15 @@ class Stage(Node): # 5) Takeover Logic: If no space, try to replace a lower-priority active scenario # which is in a scene in which takeover is possible eligible_active_scenarios = [ - s for s in self.active_scenarios if s.scene.value.takeover_possible + s for s in self.active_scenarios if s.can_be_taken_over() ] - eligible_active_scenarios.sort(key=lambda s: s.scene.value.priority) + eligible_active_scenarios.sort(key=lambda s: s.get_priority()) if eligible_active_scenarios and pending_scenarios: lowest_priority_active = eligible_active_scenarios[0] highest_priority_waiting = pending_scenarios[0] - if highest_priority_waiting.scene.value.priority > lowest_priority_active.scene.value.priority: + if highest_priority_waiting.get_priority() > lowest_priority_active.get_priority(): # Takeover! Stop the active scenario # will be cleaned up in update() loop after animation finishes # automatically triggering the start of the highest priority scene @@ -625,26 +707,31 @@ class Stage(Node): # TODO: sometimes very slow! t1 = time.perf_counter() + training_lines = self.auxilary.to_renderable_lines(dt) + + t2 = time.perf_counter() + timings = [] for scenario in self.active_scenarios: scenario_lines, timing = scenario.to_renderable_lines(dt) lines.append_lines(scenario_lines) timings.append(timing) + if not len(self.active_scenarios): + lines = training_lines - t2 = time.perf_counter() - training_lines = self.auxilary.to_renderable_lines(dt) t2b = time.perf_counter() - rl = lines.as_simplified(SimplifyMethod.RDP, .003) # or segmentise (see shapely) + rl_scenario = lines.as_simplified(SimplifyMethod.RDP, .003) # or segmentise (see shapely) + rl_training = training_lines.as_simplified(SimplifyMethod.RDP, .003) # or segmentise (see shapely) self.counter.set("stage.lines", len(lines.lines)) self.counter.set("stage.points_orig", lines.point_count()) - self.counter.set("stage.points", rl.point_count()) + self.counter.set("stage.points", rl_scenario.point_count()) t3 = time.perf_counter() layers: RenderableLayers = { - 1: lines, + 1: rl_scenario, 2: self.debug_lines, - 3: training_lines, + 3: rl_training, } t4 = time.perf_counter()