default config
This commit is contained in:
Ruben van de Ven
parent
fcaa24a860
commit
f925f1e31a
4 changed files with 116 additions and 29 deletions
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
35
trap/node.py
35
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)
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue