diff --git a/pyproject.toml b/pyproject.toml index 21f7a66..9788695 100644 --- a/pyproject.toml +++ b/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" diff --git a/trap/lidar_tracker.py b/trap/lidar_tracker.py index dd79ebb..477e19b 100644 --- a/trap/lidar_tracker.py +++ b/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) diff --git a/trap/prediction_server.py b/trap/prediction_server.py index 4e6bf24..2a1ae00 100644 --- a/trap/prediction_server.py +++ b/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 diff --git a/trap/settings.py b/trap/settings.py index bfc3342..65a5fc7 100644 --- a/trap/settings.py +++ b/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): diff --git a/trap/track_writer.py b/trap/track_writer.py new file mode 100644 index 0000000..6d43c6c --- /dev/null +++ b/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 + + \ No newline at end of file diff --git a/trap/tracker.py b/trap/tracker.py index 6999094..dddf61f 100644 --- a/trap/tracker.py +++ b/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