Multiple changes to stage and line drawing

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]
 

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,
+    try:
-                                                               scene_pts=torch.Tensor(scene_pts),
+        maps = scene_maps[0].get_cropped_maps_from_scene_map_batch(scene_maps,
-                                                               patch_size=patch_sizes[0],
+                                                                scene_pts=torch.Tensor(scene_pts),
-                                                               rotation=heading_angles,
+                                                                patch_size=patch_sizes[0],
-                                                               device='cpu')
+                                                                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

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

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):
+    def get_state_name(self):
-        # change when visible
+        return self.scene.name
-        logger.info(f"Start {self.track_id}: {self.scene.name}")
+    
-        self.is_running = True
+    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():
+class NoTracksScenario(PrioritySlotItem):
-    def __init__(self, stage: Stage):
+    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()