diff --git a/supervisord.conf b/supervisord.conf index 892651d..100dcd6 100644 --- a/supervisord.conf +++ b/supervisord.conf @@ -26,10 +26,11 @@ autostart=false # command=uv run trap_video_source --homography ../DATASETS/hof3/homography.json --video-src ../DATASETS/hof3/hof3-cam-demo-twoperson.mp4 --calibration ../DATASETS/hof3/calibration.json --video-loop command=uv run trap_video_source --homography ../DATASETS/hof3-cam-baumer-cropped/homography.json --video-src gige://../DATASETS/hof3-cam-baumer-cropped/gige_config.json --calibration ../DATASETS/hof3-cam-baumer-cropped/calibration.json directory=%(here)s -directory=%(here)s [program:tracker] -command=uv run trap_tracker --smooth-tracks +# command=uv run trap_tracker --smooth-tracks +command=uv run trap_lidar --min-box-area 0 --viz --smooth-tracks +environment=DISPLAY=":0" directory=%(here)s [program:stage] @@ -37,6 +38,12 @@ directory=%(here)s command=uv run trap_stage --verbose --camera-fps 12 --homography ../DATASETS/hof3/homography.json --calibration ../DATASETS/hof3/calibration.json --cache-path /tmp/history_cache-hof3.pcl --tracker-output-dir EXPERIMENTS/raw/hof3/ directory=%(here)s +[program:settings] +command=uv run trap_settings +autostart=true +environment=DISPLAY=":0" +directory=%(here)s + [program:predictor] command=uv run trap_prediction --eval_device cuda:0 --model_dir EXPERIMENTS/models/models_20241229_21_35_13_hof3-m2-ud-split-conv12-f2.0-map-2024-12-29/ --num-samples 1 --map_encoding --eval_data_dict EXPERIMENTS/trajectron-data/hof3-m2-ud-split-nostep-conv12-f2.0-map-2024-12-29_val.pkl --prediction-horizon 120 --gmm-mode True --z-mode diff --git a/trap/lidar_tracker.py b/trap/lidar_tracker.py index e7fe56c..dd79ebb 100644 --- a/trap/lidar_tracker.py +++ b/trap/lidar_tracker.py @@ -5,6 +5,7 @@ from pathlib import Path import select import socket from typing import DefaultDict, Dict, List +import dpkt import numpy as np import open3d as o3d import shapely @@ -19,6 +20,8 @@ from trap.node import Node from bytetracker import BYTETracker import velodyne_decoder as vd +from trap.tracker import Smoother + logger = logging.getLogger('lidar') # import sort @@ -27,7 +30,82 @@ logger = logging.getLogger('lidar') # This is what is used by velodyne_decoder internally on read_pcap: ResultTuple = namedtuple("StampCloudTuple", ("stamp", "points")) -# In bag reader this is: Result = namedtuple("ResultTuple", ("stamp", "points", "topic", "frame_id")) + +def read_pcap(pcap_file, config, as_pcl_structs=False): + """ + Adapted from velodyne_decoder to handle multiple streams + """ + + + remotes = [] + decoders = defaultdict(lambda: vd.StreamDecoder(config)) + + with open(pcap_file, 'rb') as f: + pcap = dpkt.pcap.Reader(f) + + latest_result = {} + + start_t = time.perf_counter() + start_timestamp, _ = next(pcap) + + prev_data = bytes() + + for timestamp, packet in pcap: + + # first we need to figure out when to yield the data. Simulating exhausting the socket + # this should happen when the currently reading timestamp > the wall clock timediff + now_t = time.perf_counter() + + time_passed = now_t - start_t + pkg_time_passed = timestamp - start_timestamp + + if pkg_time_passed > time_passed: + if len(latest_result): + # return data if available + yield latest_result + + + # now delay processing the package until it should 'arrive' + # but, time may have passed after returning from the yield: + time_passed = time.perf_counter() - start_t + time.sleep(max(0, pkg_time_passed - time_passed)) + + latest_result = {} + + + try: + eth = dpkt.ethernet.Ethernet(packet) + if not isinstance(eth.data, dpkt.ip.IP): + continue + + ip = eth.data + if not isinstance(ip.data, dpkt.udp.UDP): + continue + + udp = ip.data + + src_ip = socket.inet_ntoa(ip.src) + src_port = udp.sport + + data = udp.data + + if len(data) != vd.PACKET_SIZE: + # other udp data + continue + + if src_ip not in remotes: + logger.info(f"New remote {src_ip}") + remotes.append(src_ip) + + result = decoders[src_ip].decode(timestamp, data, as_pcl_structs) + + + if result is not None: + latest_result[src_ip] = ResultTuple(*result) + + + except Exception as e: + logger.warning(f"Error processing packet: {e}") # like velodyne_decoder.read_pcap but with live streamed data @@ -44,7 +122,6 @@ def read_live_data(ip, port, config, as_pcl_structs=False): remotes = [] - while True: readable, _, _ = select.select([sock], [], [], 1.0) latest_result = {} @@ -121,19 +198,9 @@ def filter_pcd_points_below_z(pcd: o3d.geometry.PointCloud, z_threshold: float) -def filter_pcd_points_outside_bounds(pcd: o3d.geometry.PointCloud, polygon: shapely.Polygon) -> o3d.geometry.PointCloud: - points = np.asarray(pcd.points) - geoms = np.array([shapely.Point(p[0], p[1]) for p in points]) - - inside_idxs = shapely.contains(polygon, geoms) - return pcd.select_by_index(np.where(inside_idxs)[0]) - - - class Lidar(Node): def setup(self): - if self.config.smooth_tracks: - self.logger.warning("Smoothing not yet implemented") + self.track_sock = self.pub(self.config.zmq_trajectory_addr) self.detection_sock = self.pub(self.config.zmq_detection_addr) @@ -142,8 +209,10 @@ class Lidar(Node): if self.config.pcap and self.config.pcap.exists(): # self.sequence_generator = vd.read_pcap(self.config.pcap, config, time_range=[1761139039+100,None]) - self.sequence_generator = vd.read_pcap(self.config.pcap, config, time_range=[1761139039+30,None]) + # read_pcap(self.config.pcap) # self.sequence_generator = vd.read_pcap(self.config.pcap, config) + self.sequence_generator = read_pcap(self.config.pcap, config) + else: self.sequence_generator = read_live_data(self.config.ip, self.config.port, config) @@ -175,15 +244,20 @@ class Lidar(Node): self.map_outline_volume = o3d.visualization.SelectionPolygonVolume() self.map_outline_volume.orthogonal_axis = "Z" # extrusion direction of polygon - self.map_outline_volume.axis_min = 0 - self.map_outline_volume.axis_max = 3.5 + self.map_outline_volume.axis_min = .3 # filter from slightly above ground + self.map_outline_volume.axis_max = 2.2 polygon_points =np.array([[*p.position, 0] for p in lines[0].points]) self.map_outline_volume.bounding_polygon = o3d.utility.Vector3dVector(polygon_points) - + if self.config.smooth_tracks: + fps = 12 # TODO)) make configurable, or get from cam + logger.info(f"Smoother enabled, assuming {fps} fps") + self.smoother = Smoother(window_len=fps*5, convolution=False) + else: + logger.info("Smoother Disabled (enable with --smooth-tracks)") self.remotes = {} @@ -289,8 +363,16 @@ class Lidar(Node): frame_idx += 1 lidar_items = next(self.sequence_generator) + + + # remove some keys. Don't use dict comprehension to not copy the data unnecesarrily + remove_keys = [ip for ip in lidar_items.keys() if not self.get_setting(f"lidar.{self.ip_to_name(ip)}.enabled", True)] + for key in remove_keys: + del lidar_items[key] + current_pcd = self.build_cloud(lidar_items) + # current_pcd = filter_pcd_points_below_z(current_pcd, .1) # current_pcd = filter_raw_points_by_distance(current_pcd, 10) @@ -344,7 +426,7 @@ class Lidar(Node): stat_static = len(filtered_pcd.points) counter.set('lidar.unstatic', stat_static) - if self.room_filter.initialised: + if self.room_filter.initialised and self.get_setting('lidar.tracking_enabled',True): # filtered_pcd, _ = filtered_pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0) denoised_pcd, inlier_indexes = filtered_pcd.remove_radius_outlier(nb_points=5, radius=0.8) stat_denoise = len(filtered_pcd.points) @@ -359,7 +441,7 @@ class Lidar(Node): filtered_pcd = denoised_pcd # down sample - filtered_pcd = filtered_pcd.voxel_down_sample(voxel_size=0.01) + filtered_pcd = filtered_pcd.voxel_down_sample(voxel_size=0.08) stat_downsampled = len(filtered_pcd.points) @@ -369,19 +451,11 @@ class Lidar(Node): 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) - # points_2d = project_to_plane(pro) - # points_2d = non_ground[:, :2] - labels = cluster_2d(points_2d) + labels = cluster_2d(points_2d, .3, 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) @@ -390,10 +464,6 @@ class Lidar(Node): detections = np.ones((boxes.shape[0],boxes.shape[1]+2)); detections[:,:-2] = boxes - # detections = np.array([ - # # coords in xyxy - # box.tolist() + [1,1] for box in boxes - # ]) online_tracks = self.tracker.update(detections) self.logger.debug(f"online tracks: {[t[4] for t in online_tracks]}") @@ -476,15 +546,12 @@ class Lidar(Node): if self.config.viz: 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) live_filtered_pcd.paint_uniform_color([0.2, 0.7, 0.2]) @@ -562,7 +629,7 @@ class Lidar(Node): argparser.add_argument('--map-outlines', help='specify a map (svg-file) from which to load boundary of the map. Any points outside will be filtered (removing floor and walls)', type=Path, - default="../DATASETS/hof3/map_hof_boundaries.svg") + default="../DATASETS/hof3/map_hof_lidar_boundaries.svg") return argparser