Separate tracker writer compatible with either tracker (lidar or camera) mode

pyproject.toml

@@ -59,6 +59,7 @@ model_train = "trap.models.train:train"
 trap_video_source = "trap.frame_emitter:FrameEmitter.parse_and_start"
 trap_video_writer = "trap.frame_writer:FrameWriter.parse_and_start"
 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_prediction = "trap.prediction_server:PredictionServer.parse_and_start"

trap/lidar_tracker.py

@@ -455,10 +455,14 @@ class Lidar(Node):
 
                 points_2d = project_to_xy(filtered_pcd)
                 
-                labels = cluster_2d(points_2d, .3, min_samples=8)
+                labels = cluster_2d(
+                    points_2d, 
+                    self.get_setting('lidar.eps', .3), 
+                    self.get_setting('lidar.min_samples', 8)
+                    )
                 
                 # boxes, centroids = get_cluster_boxes(points_2d, labels, min_area= 0.3*0.3)
-                boxes, centroids = get_cluster_boxes(points_2d, labels, min_area= self.config.min_box_area)
+                boxes, centroids = get_cluster_boxes(points_2d, labels, min_area= self.get_setting('lidar.min_box_area', .1))
 
                 # append confidence and class (placeholders)
                 
@@ -471,6 +475,8 @@ class Lidar(Node):
                 # active_stracks = [track for track in self.tracker.tracked_stracks if track.is_activated]
                 active_stracks = [track for track in self.tracker.tracked_stracks if track.is_activated]
                 detections = [Detection.from_bytetrack(track, frame_idx) for track in active_stracks]
+
+                counter.set('detections', len(detections))
                 
                 self.detection_sock.send_pyobj(detections)
 
@@ -504,6 +510,8 @@ class Lidar(Node):
 
                 if self.config.smooth_tracks:
                     frame = self.smoother.smooth_frame_tracks(frame)
+
+                counter.set('tracks', len(active_tracks))
                 
                 self.track_sock.send_pyobj(frame)
                 

trap/prediction_server.py

@@ -111,6 +111,7 @@ def get_maps_for_input(input_dict, scene, hyperparams, device):
                                                                 rotation=heading_angles,
                                                                 device='cpu')
     except Exception as e:
+        print(scene_maps)
         logger.warning(f"Crash on getting maps for points: {scene_pts=} {heading_angles=} {patch_size=}")
         raise e
 

trap/settings.py

@@ -42,6 +42,10 @@ class Settings(Node):
             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))
+            self.register_setting(f'lidar.eps', dpg.add_slider_float(label="DBSCAN epsilon", default_value=self.get_setting(f'lidar.eps', 0.3), max_value=1, callback=self.on_change))
+            self.register_setting(f'lidar.min_samples', dpg.add_slider_int(label="DBSCAN min_samples", default_value=self.get_setting(f'lidar.min_samples', 8), max_value=30, callback=self.on_change))
+            self.register_setting(f'lidar.min_box_area', dpg.add_slider_float(label="DBSCAN min_samples", default_value=self.get_setting(f'lidar.min_box_area', .1), min_value=0, max_value=1, 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):

trap/track_writer.py

@@ -0,0 +1,58 @@
+# used for "Forward Referencing of type annotations"
+from __future__ import annotations
+
+from argparse import ArgumentParser
+from pathlib import Path
+
+import zmq
+
+from trap.frame_emitter import Frame
+from trap.node import Node
+from trap.tracker import TrainingDataWriter
+
+
+class TrackWriter(Node):
+    def setup(self):
+        self.track_sock = self.sub(self.config.zmq_trajectory_addr)
+
+
+    def run(self):
+        with TrainingDataWriter(self.config.output_dir) as writer:
+            try:
+                while self.run_loop():
+                    zmq_ev = self.track_sock.poll(timeout=1000)
+                    if not zmq_ev:
+                        # when no data comes in, loop so that is_running is checked
+                        continue
+                
+                    try:
+                        frame: Frame = self.track_sock.recv_pyobj()
+                        writer.add(frame, frame.tracks.values())
+                        
+                        self.logger.debug(f"write frame {frame.time:.3f} with {len(frame.tracks)} tracks")
+
+                    except zmq.ZMQError as e:
+                        
+                        pass
+            except KeyboardInterrupt as e:
+                print('stopping on interrupt')
+                    
+        self.logger.info('Stopping')
+
+
+
+    @classmethod
+    def arg_parser(cls):
+        argparser = ArgumentParser()
+        argparser.add_argument('--zmq-trajectory-addr',
+                        help='Manually specity communication addr for the trajectory messages',
+                        type=str,
+                        default="ipc:///tmp/feeds_traj")
+        argparser.add_argument("--output-dir",
+                    help="Directory to save the video in",
+                    required=True,
+                    default=Path("EXPERIMENTS/raw/hof-lidar"),
+                    type=Path)
+        return argparser
+        
+    

trap/tracker.py

@@ -295,7 +295,7 @@ def rewrite_raw_track_files(path: Path):
         with file.open('w') as target_fp:
             
             for i in range(line_nrs):
-                line = sources.readline()
+                line = sources.readline().rstrip()
                 current_file = sources.current_file
                 if prev_file != current_file:
                     offset: int = max_track_id