default config

pyproject.toml

@@ -59,7 +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_lidar = "trap.lidar_tracker:LidarTracker.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_render_cv = "trap.cv_renderer:CvRenderer.parse_and_start"

trap/lidar_tracker.py

@@ -13,6 +13,7 @@ from scipy.spatial.distance import cdist
 import logging
 import time
 from trap.base import Camera, Detection, Frame, Track, UndistortedCamera
+from trap.counter import CounterSender
 from trap.lines import load_lines_from_svg
 from trap.node import Node
 from bytetracker import BYTETracker
@@ -129,7 +130,7 @@ def filter_pcd_points_outside_bounds(pcd: o3d.geometry.PointCloud, polygon: shap
 
 
 
-class LidarTracker(Node):
+class Lidar(Node):
     def setup(self):
         if self.config.smooth_tracks:
             self.logger.warning("Smoothing not yet implemented")
@@ -281,6 +282,8 @@ class LidarTracker(Node):
 
         camera = UndistortedCamera()
 
+        counter = CounterSender()
+
         frame_idx = 0
         while self.run_loop():
             frame_idx += 1
@@ -291,13 +294,18 @@ class LidarTracker(Node):
             # current_pcd = filter_pcd_points_below_z(current_pcd, .1)
             # current_pcd = filter_raw_points_by_distance(current_pcd, 10)
 
-            filtered_pcds: List[o3d.geometry.PointCloud] = []
+            dropped_pcds: List[o3d.geometry.PointCloud] = []
 
             updated_live_pcd = False
+
+            stat_orig = len(current_pcd.points)
+
+            counter.set('lidar.orig', stat_orig)
+
             if self.map_outline_volume:
                 # t0 = time.perf_counter()
                 if self.get_setting('lidar.crop_map_boundaries', True):
-                    if not self.get_setting('lidar.viz_cropping', False):
+                    if not self.get_setting('lidar.viz_cropping', True):
                         current_pcd = self.map_outline_volume.crop_point_cloud(current_pcd)
                     else: # some more effort to visualise what falls outside
                         cropping_indexes = self.map_outline_volume.crop_in_polygon(current_pcd)
@@ -315,54 +323,59 @@ class LidarTracker(Node):
 
                         cropped_pcd = current_pcd.select_by_index(cropping_indexes, invert=True)
                         cropped_pcd.paint_uniform_color((0,0,1))
-                        filtered_pcds.append(cropped_pcd)
+                        dropped_pcds.append(cropped_pcd)
                         
 
                         # still crop         
                         current_pcd = current_pcd.select_by_index(cropping_indexes)
                     
-
+            stat_crop = len(current_pcd.points)  
+            counter.set('lidar.crop', stat_crop)
 
             # a = time.perf_counter()
             filtered_pcd, dropped_indexes = self.room_filter.remove_static(current_pcd)
 
 
-
+            if self.config.viz and len(dropped_indexes):
-            if self.config.viz:
                 static_pcd = current_pcd.select_by_index(dropped_indexes)
                 static_pcd.paint_uniform_color((.5,.5,.5))
-                filtered_pcds.append(static_pcd)
+                dropped_pcds.append(static_pcd)
 
             stat_static = len(filtered_pcd.points)
+            counter.set('lidar.unstatic', stat_static)
 
             if self.room_filter.initialised:
-
                 # filtered_pcd, _ = filtered_pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)
-                filtered_pcd, inlier_indexes = filtered_pcd.remove_radius_outlier(nb_points=5, radius=0.2)
+                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)
 
                 if self.config.viz:
-                    outlier_pcd = filtered_pcd.select_by_index(inlier_indexes, invert=True)
+                    outlier_pcd = filtered_pcd.select_by_index(inlier_indexes)
                     outlier_pcd.paint_uniform_color((1,0,0))
-                    filtered_pcds.append(outlier_pcd)
+                    dropped_pcds.append(outlier_pcd)
 
 
+                filtered_pcd = denoised_pcd
 
                 # down sample
-                filtered_pcd = filtered_pcd.voxel_down_sample(voxel_size=0.05)
+                filtered_pcd = filtered_pcd.voxel_down_sample(voxel_size=0.01)
                 stat_downsampled = len(filtered_pcd.points)
 
+
                 if self.config.viz:
                     
                     live_filtered_pcd.points = filtered_pcd.points
 
                                                 
+                counter.set('lidar.downsampled', stat_downsampled)
             
-                print(f"after static object removal: {stat_static} / after outlier removal: {stat_denoise} / after_downsample: {stat_downsampled}")
+                # print(f"in: {stat_orig} / crop: {stat_crop} / static object removal: {stat_static} / outlier removal: {stat_denoise} / downsample: {stat_downsampled}")
 
                 # pcd.points = filtered_pcd.points           
 
                 points_2d = project_to_xy(filtered_pcd)
+                
                 # points_2d = project_to_plane(filtered_pcd, ground_normal)
                 
                 # non_ground = remove_ground(pcd)
@@ -443,11 +456,11 @@ class LidarTracker(Node):
                     for line_set in line_sets:
                         vis.add_geometry(line_set, False)
             elif self.config.viz:
+
                 line_sets = []
                 live_filtered_pcd.points = o3d.utility.Vector3dVector(np.empty([0,3]))
             
 
-
             # tracks = tracker.update(centroids)
 
 
@@ -462,11 +475,16 @@ class LidarTracker(Node):
 
             if self.config.viz:
 
-                if len(filtered_pcds):
+                if len(dropped_pcds):
-                    dropped_pcd = o3d.geometry.concatenate_point_clouds(filtered_pcds)
+                    print(dropped_pcds)
+                    dropped_pcd = sum(dropped_pcds, o3d.geometry.PointCloud())
+                else:
+                    dropped_pcd = o3d.geometry.PointCloud()
                 live_pcd.points = dropped_pcd.points
                 live_pcd.colors = dropped_pcd.colors
 
+                print(live_pcd)
+
                 color = [0.5, 0.5, 0.7] if self.room_filter.initialised else [0.7, 0.7, 0.7]
                 # live_pcd.paint_uniform_color(color)
                 live_filtered_pcd.paint_uniform_color([0.2, 0.7, 0.2])

trap/node.py

@@ -5,7 +5,7 @@ import multiprocessing
 from multiprocessing.synchronize import Event as BaseEvent
 from argparse import ArgumentParser, Namespace
 import time
-from typing import Optional
+from typing import Any, Optional
 
 import zmq
 
@@ -15,6 +15,8 @@ from trap.timer import Timer
 
 class Node():
     def __init__(self, config: Namespace, is_running: BaseEvent, fps_counter: CounterFpsSender):
+        self.node_id = self.__class__.__name__.lower()
+
         self.config = config
         self.is_running = is_running
         self.fps_counter = fps_counter
@@ -26,7 +28,9 @@ class Node():
         self.dt_since_last_tick = 0
 
         self.config_sock = self.sub(self.config.zmq_config_addr)
+        self.config_init_sock = self.push(self.config.zmq_config_init_addr) # a sending sub
         self.settings = defaultdict(None)
+        self.refresh_settings()
 
         self.setup()
 
@@ -46,8 +50,19 @@ class Node():
         raise RuntimeError("Not implemented run()")
     
     def stop(self):
+        """
+        Called when runloop is stopped. Override to clean up what was initiated in start() and run() methods
+        """
         pass
 
+    def refresh_settings(self):
+        try:
+            self.config_init_sock.send_string(self.node_id, zmq.NOBLOCK)
+        except Exception as e:
+            self.logger.warning('No settings socket available')
+            self.logger.exception(e)
+        
+    
     def run_loop(self):
         """Use in run(), to check if it should keep looping
         Takes care of tick()'ing the iterations/second counter
@@ -65,7 +80,7 @@ class Node():
             # no msgs
             pass
     
-    def get_setting(self, name: str, default):
+    def get_setting(self, name: str, default: Any):
         if name in self.settings:
             return self.settings[name]
         return default
@@ -119,6 +134,10 @@ class Node():
                         help='Manually specity communication addr for the config messages',
                         type=str,
                         default="ipc:///tmp/feeds_config")
+        parser.add_argument('--zmq-config-init-addr',
+                        help='Manually specity communication addr for req-rep config messages',
+                        type=str,
+                        default="ipc:///tmp/feeds_config_rr")
         return parser
 
         
@@ -137,6 +156,18 @@ class Node():
         sock.bind(addr)
         return sock
 
+    def push(self, addr: str):
+        "push-pull pair"
+        sock = self.zmq_context.socket(zmq.PUSH)
+        sock.connect(addr)
+        return sock
+    
+    def pull(self, addr: str):
+        "Push-pull pair"
+        sock = self.zmq_context.socket(zmq.PULL)
+        sock.bind(addr)
+        return sock
+
     @classmethod
     def start(cls, config: Namespace, is_running: BaseEvent, timer_counter: Optional[Timer]):
         instance = cls(config, is_running, timer_counter)

trap/settings.py

@@ -2,6 +2,9 @@ from argparse import ArgumentParser
 import json
 import math
 from pathlib import Path
+from typing import Any, Dict
+
+import zmq
 from trap.node import Node
 
 import dearpygui.dearpygui    as dpg
@@ -13,12 +16,15 @@ class Settings(Node):
 
     """
     def setup(self):
+        
         self.config_sock.close() # setup by default for all nodes, but we want to publish
         self.config_sock = self.pub(self.config.zmq_config_addr)
         
-        self.settings_fields = {}
+        self.config_init_sock.close() # setup by default for all nodes, but we want to publish
-        self.settings = {}
+        self.config_init_sock = self.pull(self.config.zmq_config_init_addr)
 
+        self.settings_fields = {}
+        self.settings: Dict[str, Any] = {}
 
 
         self.load()
@@ -47,6 +53,8 @@ class Settings(Node):
                 self.register_setting(f'lidar.{name}.trans_y', dpg.add_slider_float(label="trans_y", default_value=self.get_setting(f'lidar.{name}.trans_y', 0), min_value=-15, max_value=15, callback=self.on_change))
                 self.register_setting(f'lidar.{name}.trans_z', dpg.add_slider_float(label="trans_z", default_value=self.get_setting(f'lidar.{name}.trans_z', 0), min_value=-15, max_value=15, callback=self.on_change))
 
+        self.send_for_prefix("") # spread the defaults
+
         dpg.show_viewport()
     
     def stop(self):
@@ -58,6 +66,21 @@ class Settings(Node):
         # override node function to disable it
         pass
 
+    
+    def refresh_settings(self):
+        # override node function to disable it
+        pass
+
+
+
+    def get_setting(self, name: str, default: Any):
+        """
+        Automatically configure the value with the default when requesting it
+        """
+        r = super().get_setting(name, default)
+        self.settings[name] = r
+        return r
+
     def register_setting(self, name: str, field: int):
         self.settings_fields[field] = name
     
@@ -67,15 +90,19 @@ class Settings(Node):
         self.settings[setting] = value
         self.config_sock.send_json({setting: value})
 
+    def send_for_prefix(self, prefix: str):
+        self.config_sock.send_json(self.get_by_prefix(prefix))
             
     def save(self):
         with self.config.settings_file.open('w') as fp:
             self.logger.info(f"Save to {self.config.settings_file}")
             json.dump(self.settings, fp)
 
+    def get_by_prefix(self, prefix: str) -> Dict[str, Any]:
+        return {key: value for key, value in self.settings.items() if key.startswith(prefix)}
         
 
-    def load(self):
+    def load(self) -> Dict[str, Any]:
         if not self.config.settings_file.exists():
             self.logger.info(f"No config at {self.config.settings_file}")
             return {}
@@ -87,6 +114,17 @@ class Settings(Node):
     def run(self):
         # below replaces, start_dearpygui()
         while self.run_loop() and dpg.is_dearpygui_running():
+
+            # 1) receive init requests
+            try:
+                init_msg = self.config_init_sock.recv_string(zmq.NOBLOCK)
+                self.logger.info(f"Send init for {init_msg}")
+                print('init', init_msg)
+                self.send_for_prefix(init_msg)
+            except zmq.ZMQError as e:
+                # no msgs
+                pass
+
             dpg.render_dearpygui_frame()