Lidar now reads from pcap with multiple remotes

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
 

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_min = .3 # filter from slightly above ground
-                self.map_outline_volume.axis_max = 3.5
+                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)
+                labels = cluster_2d(points_2d, .3, min_samples=8)
-                
-                # non_ground = remove_ground(pcd)
-                # points_2d = project_to_plane(pro)
-                # points_2d = non_ground[:, :2]
-                labels = cluster_2d(points_2d)
                 
                 # 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