From f925f1e31a5ae1569061f0df953f4880cbe26e3c Mon Sep 17 00:00:00 2001 From: Ruben van de Ven Date: Tue, 4 Nov 2025 13:50:16 +0100 Subject: [PATCH] default config --- pyproject.toml | 2 +- trap/lidar_tracker.py | 60 ++++++++++++++++++++++++++++--------------- trap/node.py | 35 +++++++++++++++++++++++-- trap/settings.py | 48 ++++++++++++++++++++++++++++++---- 4 files changed, 116 insertions(+), 29 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index a5e33b3..21f7a66 100644 --- a/pyproject.toml +++ b/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" diff --git a/trap/lidar_tracker.py b/trap/lidar_tracker.py index 2c48544..e7fe56c 100644 --- a/trap/lidar_tracker.py +++ b/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: + if self.config.viz and len(dropped_indexes): 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 - - - print(f"after static object removal: {stat_static} / after outlier removal: {stat_denoise} / after_downsample: {stat_downsampled}") + + counter.set('lidar.downsampled', 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,10 +475,15 @@ class LidarTracker(Node): if self.config.viz: - if len(filtered_pcds): - dropped_pcd = o3d.geometry.concatenate_point_clouds(filtered_pcds) - live_pcd.points = dropped_pcd.points - live_pcd.colors = dropped_pcd.colors + if len(dropped_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) diff --git a/trap/node.py b/trap/node.py index 391c731..1a32d21 100644 --- a/trap/node.py +++ b/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,7 +50,18 @@ 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 @@ -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) diff --git a/trap/settings.py b/trap/settings.py index 4dfd093..87ddced 100644 --- a/trap/settings.py +++ b/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,13 +16,16 @@ 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.config_init_sock.close() # setup by default for all nodes, but we want to publish + self.config_init_sock = self.pull(self.config.zmq_config_init_addr) self.settings_fields = {} - self.settings = {} + 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()