security_vision/trap
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Lidar now reads from pcap with multiple remotes

Browse source
This commit is contained in:
Ruben van de Ven 2025-11-05 10:47:15 +01:00
parent f925f1e31a
commit c4ef961a78
2 changed files with 113 additions and 39 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

11
supervisord.conf
View file

@ -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

141
trap/lidar_tracker.py
View file

@ -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