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

stage rendering to auxilary screen

Browse source
This commit is contained in:
Ruben van de Ven 2025-11-12 13:35:59 +01:00
parent a387cae62c
commit 509ad16733
12 changed files with 488 additions and 92 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

3
pyproject.toml
View file

@ -16,7 +16,7 @@ dependencies = [
"gdown>=4.7.1,<5",
"pandas-helper-calc",
"tsmoothie>=1.0.5,<2",
"pyglet>=2.0.15,<3",
"pyglet>=2.1.8,<3",
"pyglet-cornerpin>=0.3.0,<0.4",
"opencv-python",
"setproctitle>=1.3.3,<2",
@ -62,6 +62,7 @@ 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_render_stage = "trap.stage_renderer:StageRenderer.parse_and_start"
trap_prediction = "trap.prediction_server:PredictionServer.parse_and_start"
trap_render_cv = "trap.cv_renderer:CvRenderer.parse_and_start"
trap_monitor = "trap.monitor:Monitor.parse_and_start" # migrate timer

4
supervisord.conf
View file

@ -35,7 +35,7 @@ directory=%(here)s
autostart=false
[program:lidar]
command=uv run trap_lidar --min-box-area 0.1 --viz --smooth-tracks
command=uv run trap_lidar --min-box-area 0.1 --viz
environment=DISPLAY=":0"
directory=%(here)s
autostart=false
@ -82,7 +82,7 @@ autostart=false
stopwaitsecs=60
[program:laserspace]
command=cargo run --release tcp://127.0.0.1:99174
command=cargo run --release tcp://127.0.0.1:99174 ../trap/SETTINGS/2025-11-dortmund/laserspace.json
directory=%(here)s/../laserspace
environment=DISPLAY=":0"
autostart=false

64
trap/lidar_tracker.py
View file

@ -222,6 +222,8 @@ class Lidar(Node):
self.sequence_generator = read_live_data(self.config.ip, self.config.port, config)
self.tracker = BYTETracker(frame_rate=ASSUMED_FPS)
self.tracks: DefaultDict[str, Track] = defaultdict(lambda: Track())
@ -257,7 +259,11 @@ class Lidar(Node):
if len(lines) > 1:
logger.warning("Multiple lines in outline file, using first only")
polygon_points =np.array([[*p.position, 0] for p in lines[0].points])
axis_min = .3
axis_max = 2.2
polygon_points =np.array([[*p.position, axis_min] for p in lines[0].points])
# self.map_outline = shapely.Polygon([p.position for p in lines[0].points])
boundary_lines = [[i, (i+1) % len(lines[0].points)] for i in range(len(lines[0].points))]
line_set = o3d.geometry.LineSet(
@ -272,8 +278,8 @@ 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 = .3 # filter from slightly above ground
self.map_outline_volume.axis_max = 2.2
self.map_outline_volume.axis_min = axis_min # filter from slightly above ground
self.map_outline_volume.axis_max = axis_max
@ -283,7 +289,7 @@ class Lidar(Node):
if self.config.smooth_tracks:
# TODO)) make configurable
logger.info(f"Smoother enabled, assuming {ASSUMED_FPS} fps")
self.smoother = Smoother(window_len=ASSUMED_FPS*5, convolution=False)
self.smoother = Smoother(window_len=int(ASSUMED_FPS*.6), convolution=True)
else:
logger.info("Smoother Disabled (enable with --smooth-tracks)")
@ -389,9 +395,16 @@ class Lidar(Node):
counter = CounterSender()
frame_idx = 0
while self.run_loop():
kalmain_init_pos =self.tracker.kalman_filter._std_weight_position
kalmain_init_vel =self.tracker.kalman_filter._std_weight_velocity
# limit at lidar framefrate to avoid flickering if multiple lidars are connected
while self.run_loop_capped_fps(12):
frame_idx += 1
self.tracker.kalman_filter._std_weight_position = kalmain_init_pos * self.get_setting('lidar.kalman_factor', 1.3)
self.tracker.kalman_filter._std_weight_velocity = kalmain_init_vel * self.get_setting('lidar.kalman_factor', 1.3)
lidar_items = next(self.sequence_generator)
@ -457,24 +470,26 @@ class Lidar(Node):
stat_static = len(filtered_pcd.points)
counter.set('lidar.unstatic', stat_static)
timers=[]
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)
timers.append(('a', time.perf_counter()))
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)
timers.append(('denoise', time.perf_counter()))
if self.config.viz:
outlier_pcd = filtered_pcd.select_by_index(inlier_indexes)
outlier_pcd.paint_uniform_color((1,0,0))
dropped_pcds.append(outlier_pcd)
timers.append(('viz', time.perf_counter()))
filtered_pcd = denoised_pcd
# down sample
filtered_pcd = filtered_pcd.voxel_down_sample(voxel_size=0.08)
filtered_pcd = filtered_pcd.voxel_down_sample(voxel_size=0.04)
stat_downsampled = len(filtered_pcd.points)
timers.append(('downsample', time.perf_counter()))
if self.config.viz:
@ -482,9 +497,9 @@ class Lidar(Node):
counter.set('lidar.downsampled', stat_downsampled)
points_2d = project_to_xy(filtered_pcd)
timers.append(('project', time.perf_counter()))
# with open('/tmp/points.pcl', 'wb') as fp:
# pickle.dump(points_2d, fp)
@ -492,10 +507,13 @@ class Lidar(Node):
clusters = self.cluster_2d(
points_2d,
)
timers.append(('cluster2d', time.perf_counter()))
# print(len(clusters))
# boxes, centroids = get_cluster_boxes(points_2d, labels, min_area= 0.3*0.3)
boxes, centroids = get_cluster_boxes(clusters, min_area= self.get_setting('lidar.min_box_area', .1))
boxes, centroids = get_cluster_boxes(clusters, min_area= self.get_setting('lidar.min_box_area', .1), max_area= self.get_setting('lidar.max_box_area', 5))
timers.append(('boxes', time.perf_counter()))
# print(centroids)
# append confidence and class (placeholders)
@ -504,6 +522,8 @@ class Lidar(Node):
detections[:,:-2] = boxes
online_tracks = self.tracker.update(detections)
timers.append(('update', time.perf_counter()))
self.logger.debug(f"online tracks: {[t[4] for t in online_tracks]}")
removed_tracks = self.tracker.removed_stracks
# active_stracks = [track for track in self.tracker.tracked_stracks if track.is_activated]
@ -511,9 +531,11 @@ class Lidar(Node):
detections = [Detection.from_bytetrack(track, frame_idx) for track in active_stracks]
counter.set('detections', len(detections))
timers.append(('tracks', time.perf_counter()))
self.detection_sock.send_pyobj(detections)
timers.append(('sent', time.perf_counter()))
for detection in detections:
track = self.tracks[detection.track_id]
@ -526,6 +548,8 @@ class Lidar(Node):
for t in removed_tracks:
if t.track_id in self.tracks:
if t.is_activated:
self.logger.info(f"Lost track: {t.track_id}")
del self.tracks[t.track_id]
# TODO: fix this oddity:
# else:
@ -537,6 +561,7 @@ class Lidar(Node):
active_track_ids = [d.track_id for d in detections]
active_tracks = {t.track_id: t.get_with_interpolated_history() for t in self.tracks.values() if t.track_id in active_track_ids}
# active_tracks = displacement_filter.apply_to_dict(active_tracks, frame.camera)# a filter to remove just detecting static objects
timers.append(('interpolated', time.perf_counter()))
# frame = Frame(frame_idx, None, time.time(), self.tracks, camera.H, camera)
frame = Frame(frame_idx, None, time.time(), active_tracks,
@ -545,9 +570,13 @@ class Lidar(Node):
if self.config.smooth_tracks:
frame = self.smoother.smooth_frame_tracks(frame)
timers.append(('smooth', time.perf_counter()))
counter.set('tracks', len(active_tracks))
self.track_sock.send_pyobj(frame)
timers.append(('sent', time.perf_counter()))
@ -567,6 +596,12 @@ class Lidar(Node):
for line_set in line_sets:
vis.add_geometry(line_set, False)
timers.append(('viz2', time.perf_counter()))
total_time = timers[-1][1]-timers[0][1]
for t0, t1 in zip(timers, timers[1:]):
difftime = t1[1]-t0[1]
counter.set(f'tracker.timer.{t1[0]}', difftime/total_time)
elif self.config.viz:
print('initializing')
if hasattr(self.room_filter, 'scan_counter'):
@ -641,7 +676,8 @@ class Lidar(Node):
argparser.add_argument('--ip',
help='IP of this computer on which to listen for IP packets broadcast by lidar (so NOT the ip of the Lidar itself)',
type=str,
default="192.168.0.70")
# default="192.168.0.70")
default="0.0.0.0")
argparser.add_argument('--port',
help='Port of the incomming ip packets',
type=int,
@ -854,7 +890,7 @@ def split_cluster_by_convex_hull(points, max_hull_area):
def get_cluster_boxes(clusters, min_area = 0):
def get_cluster_boxes(clusters, min_area = 0, max_area=5):
if not len(clusters):
return np.empty([0,4]), np.empty([0,2])
@ -865,7 +901,7 @@ def get_cluster_boxes(clusters, min_area = 0):
x_max, y_max = cluster.max(axis=0)
area = (x_max-x_min) * (y_max - y_min)
if area < min_area:
if area < min_area or area > max_area:
logger.warning(f"Dropping box {area} ")
continue

49
trap/lines.py
View file

@ -59,7 +59,7 @@ class SrgbaColor():
return math.isclose(self.red, other.red) and math.isclose(self.green, other.green) and math.isclose(self.blue, other.blue) and math.isclose(self.alpha, other.alpha)
def as_array(self):
return [self.red, self.green, self.blue, self.alpha]
return np.array([self.red, self.green, self.blue, self.alpha])
@dataclass
@ -510,6 +510,9 @@ class LineAnimator(StaticLine):
self.start_t = time.perf_counter()
return True
def is_started(self):
return bool(self.start_t)
def is_running(self):
# when ready, consider not running
@ -536,9 +539,10 @@ class AppendableLineAnimator(LineAnimator):
def apply(self, target_line, dt: DeltaT) -> RenderableLine:
if len(target_line) == 0:
if len(target_line) < 2:
return target_line
# nothing to draw yet
return RenderableLine([])
# return RenderableLine([])
@ -1061,7 +1065,8 @@ class DashedLine(LineAnimator):
dash = shapely.ops.substring(line, pos, end)
segments.append(dash)
pos += dash_len + gap_len
segments = [segment for segment in segments if isinstance(segment, shapely.geometry.LineString)]
# TODO: return all color together with the points
return shapely.geometry.MultiLineString(segments)
@ -1210,6 +1215,8 @@ class RotatingLine(LineAnimator):
# progress = associated_diff.nr_of_passed_points()
is_ready: List[bool] = []
# target_point = target_line.points[-1]
for i, (target_point, drawn_point) in enumerate(zip(target_line.points, list(self.drawn_points))):
# TODO: this should be done in polar space starting from origin (i.e. self.drawn_posision[-1])
decay = max(3, (18/i) if i else 10) # points further away move with more delay
@ -1220,6 +1227,7 @@ class RotatingLine(LineAnimator):
r = exponentialDecay(drawn_r, pred_r, decay, dt)
# make circular coordinates transition through the smaller arc
# TODO 20251108 bring this back, but calculated for the whole line:
if abs(drawn_angle - pred_angle) > math.pi:
pred_angle -= math.pi * 2
angle = exponentialDecay(drawn_angle, pred_angle, decay, dt)
@ -1626,4 +1634,35 @@ def layers_to_message(layers: RenderableLayers):
# print( t2-t1,t3-t2)
return s
return s
def message_to_layers(s: str) -> RenderableLayers:
"""Decode the protobuf"""
pb_layers = renderable_pb2.RenderableLayers()
pb_layers.ParseFromString(s)
layers = {}
for n, pb_lines in pb_layers.layers.items():
lines = []
for pb_line in pb_lines.lines:
points = []
for pb_point in pb_line.points:
color = SrgbaColor(
pb_point.color.red,
pb_point.color.green,
pb_point.color.blue,
pb_point.color.alpha,
)
point = RenderablePoint(
(pb_point.position.x, pb_point.position.y),
color
)
points.append(point)
lines.append(RenderableLine(points))
layers[n] = RenderableLines(lines, CoordinateSpace.WORLD)
return layers

16
trap/node.py
View file

@ -72,13 +72,15 @@ class Node():
return self.is_running.is_set()
def check_config(self):
try:
config = self.config_sock.recv_json(zmq.NOBLOCK)
for field, value in config.items():
self.settings[field] = value
except zmq.ZMQError as e:
# no msgs
pass
while True:
try:
config = self.config_sock.recv_json(zmq.NOBLOCK)
for field, value in config.items():
self.settings[field] = value
except zmq.ZMQError as e:
# no msgs
break
def get_setting(self, name: str, default: Any):
if name in self.settings:

2
trap/prediction_server.py
View file

@ -449,7 +449,7 @@ class PredictionServer(Node):
# print('preds', len(predictions[0][0]))
if not len(history) or np.isnan(history[-1]).any():
logger.warning(f'skip for no history @ {ts_key} [{len(prediction_dict)=}, {len(histories_dict)=}, {len(futures_dict)=}]')
logger.warning(f'skip for no history for {node} @ {ts_key} [{len(prediction_dict)=}, {len(histories_dict)=}, {len(futures_dict)=}]')
# logger.info(f"{preds=}")
continue

8
trap/process_data.py
View file

@ -84,7 +84,7 @@ class TrackIteration:
for n in range(noisy_variations+1):
for f in range(offset_variations+1):
iterations.append(TrackIteration(smooth, sample_step_size, i, noisy=bool(n), offset=bool(f)))
if toggle_smooth:
if smooth and toggle_smooth:
iterations.append(TrackIteration(not smooth, sample_step_size, i, noisy=bool(n), offset=bool(f)))
return iterations
@ -345,16 +345,18 @@ def process_data(src_dir: Path, dst_dir: Path, name: str, smooth_tracks: bool, n
# print(f"Non-Linear: {nl}")
print(f"error: {skipped_for_error}, used: {created}")
print("Run with")
target_model_dir = (dst_dir / "../models/").resolve()
target_config = (dst_dir / "../trajectron.json").resolve()
# set eval_every very high, because we're not interested in theoretical evaluations, and we don't mind overfitting
print(f"""
uv run trajectron_train --eval_every 200 \\
--train_data_dict {names['train'].name} \\
--eval_data_dict {names['val'].name} \\
--offline_scene_graph no --preprocess_workers 8 \\
--log_dir EXPERIMENTS/models \\
--log_dir {target_model_dir} \\
--log_tag _{name} \\
--train_epochs 100 \\
--conf EXPERIMENTS/config.json \\
--conf {target_config} \\
--data_dir {dst_dir} \\
{"--map_encoding" if map_img_path else ""}
""")

14
trap/settings.py
View file

@ -38,14 +38,25 @@ class Settings(Node):
dpg.add_text(f"Settings from {self.config.settings_file}")
dpg.add_button(label="Save", callback=self.save)
with dpg.window(label="Renderer", pos=(0, 600)):
for i in range(8) :
self.register_setting(f'stagerenderer.layer.{i}', dpg.add_checkbox(label=f"layer {i}", default_value=self.get_setting(f'stagerenderer.layer.{i}', True), callback=self.on_change))
self.register_setting(f'stagerenderer.scale', dpg.add_slider_float(label="scale", default_value=self.get_setting(f'stagerenderer.scale', 1), max_value=3, callback=self.on_change))
self.register_setting(f'stagerenderer.dx', dpg.add_slider_int(label="dx", default_value=self.get_setting(f'stagerenderer.dx', 0), min_value=-300, max_value=300, callback=self.on_change))
self.register_setting(f'stagerenderer.dy', dpg.add_slider_int(label="dy", default_value=self.get_setting(f'stagerenderer.dy', 0), min_value=-300, max_value=300, callback=self.on_change))
self.register_setting(f'stagerenderer.fade', dpg.add_slider_float(label="fade factor", default_value=self.get_setting(f'stagerenderer.fade', 0.27), max_value=1, callback=self.on_change))
with dpg.window(label="Stage", pos=(150, 0)):
self.register_setting(f'stage.fps', dpg.add_slider_int(label="FPS cap", default_value=self.get_setting(f'stage.fps', 30), callback=self.on_change))
self.register_setting(f'stage.prediction_interval', dpg.add_slider_int(label="prediction interval", default_value=self.get_setting('stage.prediction_interval', 18), callback=self.on_change))
self.register_setting(f'stage.loitering_animation', dpg.add_checkbox(label="loitering_animation", default_value=self.get_setting('stage.loitering_animation', True), callback=self.on_change))
with dpg.window(label="Lidar", pos=(0, 100), autosize=True):
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.voxel_downsample', dpg.add_checkbox(label="voxel_downsample", default_value=self.get_setting(f'lidar.voxel_downsample', 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.kalman_factor', dpg.add_slider_float(label="kalman_factor", default_value=self.get_setting(f'lidar.kalman_factor', 1.3), max_value=3, callback=self.on_change))
dpg.add_separator(label="Clustering")
@ -60,8 +71,9 @@ class Settings(Node):
dpg.add_separator(label="Cluster filter")
self.register_setting(f'lidar.min_box_area', dpg.add_slider_float(label="min_box_area", default_value=self.get_setting(f'lidar.min_box_area', .1), min_value=0, max_value=1, callback=self.on_change))
self.register_setting(f'lidar.max_box_area', dpg.add_slider_float(label="max_box_area", default_value=self.get_setting(f'lidar.max_box_area', 5), min_value=.5, max_value=10, callback=self.on_change))
for i, lidar in enumerate(["192.168.0.16", "192.168.0.10"]):
for i, lidar in enumerate(["192.168.1.16", "192.168.0.10"]):
name = lidar.replace(".", "_")
with dpg.window(label=f"Lidar {lidar}", pos=(i * 300, 450),autosize=True):
# dpg.add_text("test")

154
trap/stage.py
View file

@ -119,6 +119,9 @@ class PrioritySlotItem():
self.start_time = time.perf_counter()
self.is_running = True
def running_for(self):
return time.perf_counter() - self.start_time
@abstractmethod
def get_priority(self) -> int:
raise RuntimeError("Not implemented")
@ -161,7 +164,11 @@ class Scenario(PrioritySlotItem):
return self.scene.name
def get_priority(self) -> int:
return self.scene.value.priority
# newer higher prio
distance = 0
if self.track and len(self.track.projected_history) > 5:
distance = np.linalg.norm(self.track.projected_history[-1] - self.track.projected_history[0])
return (self.scene.value.priority, distance)
def can_be_taken_over(self):
if self.scene.value.takeover_possible:
@ -221,11 +228,12 @@ class Scenario(PrioritySlotItem):
def set_scene(self, scene: ScenarioScene):
if self.scene is scene:
return
return False
logger.info(f"Changing scene for {self.track_id}: {self.scene.name} -> {scene.name}")
self.scene = scene
self.state_change_at = time.perf_counter()
return True
def update_state(self):
self.check_lost() or self.check_loitering() or self.check_track()
@ -252,10 +260,10 @@ class Scenario(PrioritySlotItem):
def check_track(self):
predictions = len(self.prediction_tracks)
if predictions == 1:
if predictions and self.running_for() < 20:
self.set_scene(ScenarioScene.FIRST_PREDICTION)
return True
if predictions > 30:
if predictions and self.running_for() > 120:
self.set_scene(ScenarioScene.PLAY)
return True
if predictions:
@ -350,7 +358,7 @@ class DrawnScenario(Scenario):
history_color = SrgbaColor(1.,0.,1.,1.)
history = StaticLine([], history_color)
self.line_history = LineAnimationStack(history)
self.line_history.add(AppendableLineAnimator(self.line_history.tail, draw_decay_speed=120))
self.line_history.add(AppendableLineAnimator(self.line_history.tail, draw_decay_speed=120, transition_in_on_init=False))
self.line_history.add(CropLine(self.line_history.tail, self.MAX_HISTORY))
self.line_history.add(SimplifyLine(self.line_history.tail, 0.003)) # Simplify before effects, so they don't distort
self.line_history.add(FadedTailLine(self.line_history.tail, TRACK_FADE_AFTER_DURATION * TRACK_ASSUMED_FPS, TRACK_END_FADE))
@ -394,10 +402,12 @@ class DrawnScenario(Scenario):
if self.track:
self.line_history.root.points = self.track.projected_history
lf = self.lost_factor()
self.line_history.get(FadeOutJitterLine).set_alpha(1-lf)
self.line_prediction.get(FadeOutLine).set_alpha(1-lf)
self.line_history.get(NoiseLine).amplitude = lf * 1.8
lost_factor = self.lost_factor() # fade out when lost
start_factor = 0#1 - min(1, self.running_for()) # fade in when starting
# print(start_factor)
self.line_history.get(FadeOutJitterLine).set_alpha(1- lost_factor - start_factor)
self.line_prediction.get(FadeOutLine).set_alpha(1-lost_factor)
self.line_history.get(NoiseLine).amplitude = lost_factor * 1.8
if len(self.prediction_tracks):
# now_p = np.array(self.line_history.root.points[-1])
@ -408,49 +418,50 @@ class DrawnScenario(Scenario):
# TODO: only when animation is ready for it? or collect lines
if not self.active_ptrack:
# draw the first prediction
self.active_ptrack = self.prediction_tracks[-1]
self.line_prediction.root.points = self.active_ptrack._track.predictions[0]
if self.is_running:
if not self.active_ptrack:
# draw the first prediction
self.active_ptrack = self.prediction_tracks[-1]
self.line_prediction.root.points = self.active_ptrack._track.predictions[0]
self.line_prediction.start() # reset positions
elif self.active_ptrack._track.updated_at < self.prediction_tracks[-1]._track.updated_at:
# stale prediction
# switch only if drawing animation is ready
# if self.line_prediction.is_ready():
self.active_ptrack = self.prediction_tracks[-1]
self.line_prediction.root.points = self.active_ptrack._track.predictions[0]
if self.line_prediction.is_ready() and self.line_prediction.get(DashedLine).skip == True:
self.line_prediction.get(SegmentLine).skip = True
self.line_prediction.get(DashedLine).skip = False
self.line_prediction.start() # reset positions
# self.line_prediction.get(SegmentLine).anim_f = partial(SegmentLine.anim_arrive, length=.3)
# self.line_prediction.get(SegmentLine).duration = .5
# self.line_prediction.get(DashedLine).skip = True
# # print('restart')
# self.line_prediction.start() # reset positions
# # print(self.line_prediction.get(SegmentLine).running_for())
# else:
# if self.line_prediction.is_ready():
# # little hack: check is dashedline skips, to only run this once per animation:
# if self.line_prediction.get(DashedLine).skip:
# # no new yet, but ready with anim, start stage 2
# self.line_prediction.get(SegmentLine).anim_f = partial(SegmentLine.anim_grow)
# self.line_prediction.get(SegmentLine).duration = 1
# # self.line_prediction.get(SegmentLine).skip = True
# self.line_prediction.get(DashedLine).skip = False
# self.line_prediction.start()
# elif self.line_prediction.get(SegmentLine).duration != 2: # hack to only play once
# self.line_prediction.get(SegmentLine).anim_f = partial(SegmentLine.anim_grow, reverse=True)
# self.line_prediction.get(SegmentLine).duration = 2
# self.line_prediction.get(SegmentLine).start()
if self.active_ptrack:
# TODO: this should crop by distance/lenght
self.line_prediction.get(CropLine).start_offset = self.track._track.frame_index - self.active_ptrack._track.frame_index
elif self.active_ptrack._track.updated_at < self.prediction_tracks[-1]._track.updated_at:
# stale prediction
# switch only if drawing animation is ready
# if self.line_prediction.is_ready():
self.active_ptrack = self.prediction_tracks[-1]
self.line_prediction.root.points = self.active_ptrack._track.predictions[0]
if self.line_prediction.is_ready() and self.line_prediction.get(DashedLine).skip == True:
self.line_prediction.get(SegmentLine).skip = True
self.line_prediction.get(DashedLine).skip = False
self.line_prediction.start() # reset positions
# self.line_prediction.get(SegmentLine).anim_f = partial(SegmentLine.anim_arrive, length=.3)
# self.line_prediction.get(SegmentLine).duration = .5
# self.line_prediction.get(DashedLine).skip = True
# # print('restart')
# self.line_prediction.start() # reset positions
# # print(self.line_prediction.get(SegmentLine).running_for())
# else:
# if self.line_prediction.is_ready():
# # little hack: check is dashedline skips, to only run this once per animation:
# if self.line_prediction.get(DashedLine).skip:
# # no new yet, but ready with anim, start stage 2
# self.line_prediction.get(SegmentLine).anim_f = partial(SegmentLine.anim_grow)
# self.line_prediction.get(SegmentLine).duration = 1
# # self.line_prediction.get(SegmentLine).skip = True
# self.line_prediction.get(DashedLine).skip = False
# self.line_prediction.start()
# elif self.line_prediction.get(SegmentLine).duration != 2: # hack to only play once
# self.line_prediction.get(SegmentLine).anim_f = partial(SegmentLine.anim_grow, reverse=True)
# self.line_prediction.get(SegmentLine).duration = 2
# self.line_prediction.get(SegmentLine).start()
if self.active_ptrack:
# TODO: this should crop by distance/lenght
self.line_prediction.get(CropLine).start_offset = self.track._track.frame_index - self.active_ptrack._track.frame_index
@ -459,7 +470,7 @@ class DrawnScenario(Scenario):
# special case: LOITERING
if self.scene is ScenarioScene.LOITERING: # or self.state_change_at:
if self.stage.get_setting('stage.loitering_animation', True) and self.scene is ScenarioScene.LOITERING: # or self.state_change_at:
# logger.info('loitering')
transition = min(1, (time.perf_counter() - self.state_change_at)/1.4)
# print('loitering factor', transition)
@ -555,7 +566,15 @@ class DrawnScenario(Scenario):
prediction_line,
others_line
]), timings
def set_scene(self, scene):
"""Create log message for the auxilary interface
"""
original = self.scene.name
changed = super().set_scene(scene)
if changed:
self.stage.log_sock.send_string(f"Change {self.track_id}: {original} -> {self.scene.name}")
return changed
class NoTracksScenario(PrioritySlotItem):
TAKEOVER_FADEOUT = 1 # override default to be faster
@ -567,7 +586,7 @@ class NoTracksScenario(PrioritySlotItem):
def get_priority(self):
# super low priority
return -1
return (-1, -1)
def can_be_taken_over(self):
return True
@ -599,6 +618,21 @@ class NoTracksScenario(PrioritySlotItem):
return lines, timings
class DebugDrawer():
def __init__(self, stage: Stage):
self.stage = stage
def to_renderable_lines(self, dt: DeltaT):
lines = RenderableLines([], CoordinateSpace.WORLD)
past_color = SrgbaColor(1,0,1,1)
future_color = SrgbaColor(0,1,0,1)
for scenario in self.stage.scenarios.values():
lines.append(StaticLine(scenario.track.projected_history, past_color).as_renderable_line(dt))
if scenario.active_ptrack:
lines.append(StaticLine(scenario.active_ptrack._track.predictions[0], future_color).as_renderable_line(dt))
return lines
class DatasetDrawer():
def __init__(self, stage: Stage):
self.stage = stage
@ -649,6 +683,8 @@ class Stage(Node):
self.prediction_sock = self.sub(self.config.zmq_prediction_addr)
self.detection_sock = self.sub(self.config.zmq_detection_addr)
self.stage_sock = self.pub(self.config.zmq_stage_addr)
self.log_sock = self.push(self.config.zmq_log_addr)
# self.stage_py_sock = self.pub(self.config.zmq_stage_py_addr)
self.counter = CounterSender()
@ -659,6 +695,7 @@ class Stage(Node):
self.history = TrackHistory(self.config.tracker_output_dir, self.config.camera, self.config.cache_path)
self.auxilary = DatasetDrawer(self)
self.debug_drawer = DebugDrawer(self)
# 'screensavers'
self.notrack_scenarios = [] #[NoTracksScenario(self, i) for i in range(self.config.max_active_scenarios)]
@ -758,6 +795,7 @@ class Stage(Node):
# TODO: sometimes very slow!
t1 = time.perf_counter()
training_lines = self.auxilary.to_renderable_lines(dt)
all_active_tracks = self.debug_drawer.to_renderable_lines(dt)
t2 = time.perf_counter()
@ -782,6 +820,7 @@ class Stage(Node):
1: lines,
2: self.debug_lines,
3: training_lines,
4: all_active_tracks,
}
t4 = time.perf_counter()
@ -791,6 +830,7 @@ class Stage(Node):
t5 = time.perf_counter()
self.stage_sock.send(msg)
# self.stage_sock.send_pyobj(layers)
# self.stage_sock.send_json(obj=layers, cls=DataclassJSONEncoder)
@ -831,6 +871,14 @@ class Stage(Node):
help='Manually specity communication addr for the stage messages (the rendered lines)',
type=str,
default="tcp://0.0.0.0:99174")
argparser.add_argument('--zmq-log-addr',
help='Manually specity communication addr for the log messages',
type=str,
default="tcp://0.0.0.0:99188")
argparser.add_argument('--zmq-stage-py-addr',
help='Sometimes there is no need for protobuf',
type=str,
default="ipc:///tmp/feeds_stage")
argparser.add_argument('--debug-map',
help='specify a map (svg-file) from which to load lines which will be overlayed',
type=str,

255
trap/stage_renderer.py Normal file
View file

@ -0,0 +1,255 @@
from argparse import ArgumentParser
from collections import deque
import math
from typing import List
import numpy as np
import pyglet
from torch import mul
import zmq
from trap.lines import RenderableLayers, message_to_layers
from trap.node import Node
BG_COLOR = (0,0,0)
class StageRenderer(Node):
def setup(self):
# self.prediction_sock = self.sub(self.config.zmq_prediction_addr)
# self.tracker_sock = self.sub(self.config.zmq_trajectory_addr)
# self.detector_sock = self.sub(self.config.zmq_detection_addr)
# self.frame_sock = self.sub(self.config.zmq_frame_addr)
self.stage_sock = self.sub(self.config.zmq_stage_addr)
self.log_sock = self.pull(self.config.zmq_log_addr)
# setup pyglet:
display = pyglet.display.get_display()
screens = display.get_screens()
# use configured montior, fall back to whatever is available
self.screen = sorted(screens, reverse=True, key=lambda s: s.get_monitor_name() == self.config.monitor)[0]
if self.screen.get_monitor_name() != self.config.monitor:
self.logger.warning(f"Not displaying on configured monitor. {self.screen.get_monitor_name()} instead of {self.config.monitor}")
# print(self.screen.get_modes())
config = pyglet.gl.Config(sample_buffers=1, samples=4)
# when screen is in portrait, window mode here expects still (larger x smaller) number.
# self.window.get_size() will be reported properly
wh = sorted((self.screen.width, self.screen.height), reverse=self.config.fullscreen)
self.window = pyglet.window.Window(width=wh[0], height=wh[1], config=config, fullscreen=self.config.fullscreen, screen=self.screen)
self.window.set_exclusive_keyboard(True)
self.window.set_exclusive_keyboard(False)
self.window.set_exclusive_mouse(True)
self.window.set_exclusive_mouse(False)
# self.window.set_size(1080, 1920)
window_size = self.window.get_size()
print(window_size)
self.window.set_handler('on_draw', self.on_draw)
# self.window.set_handler('on_close', self.on_close)
# pyglet.gl.glClearColor(81./255, 20/255, 46./255, 0)
pyglet.gl.glClearColor(0/255, 0/255, 255/255, 0)
self.fps_display = pyglet.window.FPSDisplay(window=self.window, color=(255,255,255,255))
self.fps_display.label.x = self.window.width - 50
self.fps_display.label.y = self.window.height - 17
self.fps_display.label.bold = False
self.fps_display.label.font_size = 10
self.current_layers: RenderableLayers = {}
self.lines: List[pyglet.shapes.Line] = []
self.lines_batch = pyglet.graphics.Batch()
self.text = pyglet.text.document.FormattedDocument("")
self.text_batch = pyglet.graphics.Batch()
self.text_layout = pyglet.text.layout.TextLayout(
self.text, 20, 350,
width=self.window.get_size()[1],
height=self.window.get_size()[0] // 3,
multiline=True, wrap_lines=False, batch=self.text_batch)
max_len = 30
self.log_msgs = deque([], maxlen=max_len)
self.log_msgs.extend(["..."] * max_len)
translate = (10,-400)
# scale = 5
smallest_dimension = min(self.window.get_size())
max_x = 16.3
max_y = 14.3
scale = min(smallest_dimension / max_x, smallest_dimension/max_y)
padding = 40
self.logger.info(f"Use {scale=}")
self.transform = np.array([
[scale, 0,translate[0]],
[0,-scale,window_size[1]],
[0,0,1]
])
self.bg_image = pyglet.image.load(self.config.floorplan)
scale = (window_size[0] - padding*2) / (self.bg_image.width)
print('image_scale', scale, self.bg_image.width, self.bg_image.height)
# self.bg_image.height = int(self.bg_image.height / 3)
# self.bg_image.width = int(self.bg_image.width / 3)
img_y = window_size[1]-int(self.bg_image.height*scale)-padding*2
self.bg_sprite = pyglet.sprite.Sprite(img=self.bg_image, x=padding, y=img_y)
self.bg_sprite.scale = scale
clear_area = img_y
self.clear_transparent = pyglet.shapes.Rectangle(0, window_size[1]-clear_area, window_size[0], clear_area, color=(*BG_COLOR,255//70))
self.clear_fully= pyglet.shapes.Rectangle(0, 0, window_size[0], window_size[1]-clear_area, color=(*BG_COLOR,255))
def check_running(self, dt):
if not self.run_loop():
self.window.close()
self.event_loop.exit()
def run(self):
self.event_loop = pyglet.app.EventLoop()
pyglet.clock.schedule_interval(self.check_running, 0.1)
# pyglet.clock.schedule(self.receive)
self.event_loop.run()
def receive(self, dt):
try:
msg = self.stage_sock.recv(zmq.NOBLOCK)
self.current_layers = message_to_layers(msg)
self.update_lines()
except zmq.ZMQError as e:
# idx = frame.index if frame else "NONE"
# logger.debug(f"reuse video frame {idx}")
pass
while True:
try:
log_msg = self.log_sock.recv_string(zmq.NOBLOCK)
self.log_msgs.append(log_msg)
except zmq.ZMQError as e:
# idx = frame.index if frame else "NONE"
# logger.debug(f"reuse video frame {idx}")
break
self.update_msgs()
def update_lines(self):
"""
Render the renderable lines of selected layers
"""
additional_scale = self.get_setting('stagerenderer.scale', 1)
dx = self.get_setting('stagerenderer.dx', 0)
dy = self.get_setting('stagerenderer.dy', 0)
transform = self.transform.copy()
transform[0][0] *= additional_scale
transform[1][1] *= additional_scale
transform[0][2] += dx
transform[1][2] += dy
i = -1
for nr, lines in self.current_layers.items():
if not self.get_setting(f'stagerenderer.layer.{nr}', True):
continue
for line in lines.lines:
for p1, p2 in zip(line.points, line.points[1:]):
i += 1
pp1 = np.array([p1.position[0], p1.position[1], 1])
pp2 = np.array([p2.position[0], p2.position[1], 1])
pos1 = (transform@pp1)[:2].astype(int)
pos2 = (transform@pp2)[:2].astype(int)
color = (p2.color.as_array()*255).astype(int)
if i < len(self.lines):
print('reuse')
shape = self.lines[i]
shape.x = pos1[0]
shape.y = pos1[1]
shape.x2 = pos2[0]
shape.y2 = pos2[1]
shape.color = color
self.lines.append(pyglet.shapes.Line(pos1[0], pos1[1],
pos2[0],
pos2[1],
3,
color,
batch=self.lines_batch))
print(len(self.lines), i)
too_many = len(self.lines) - 1 - i
if too_many > 0:
print('del', too_many)
for j in reversed(range(i, i+too_many)):
self.lines[i].delete()
del self.lines[i]
def update_msgs(self):
text = "\n".join(self.log_msgs)
self.text.text = text
self.text.set_style(0, len(self.text.text), dict(
font_name='Arial', # change to a font installed on your system
font_size=18,
color=(255, 255, 255, 255),
))
def on_draw(self):
self.receive(.1)
self.window.clear()
# self.clear_transparent.color = (*BG_COLOR, int(255*self.get_setting('stagerenderer.fade', .27)))
# self.clear_transparent.draw()
# self.clear_fully.draw()
self.fps_display.draw()
# self.bg_sprite.draw()
# self.lines_batch.draw()
# self.text_batch.draw()
@classmethod
def arg_parser(cls):
render_parser = ArgumentParser()
render_parser.add_argument('--zmq-stage-addr',
help='Manually specity communication addr for the stage messages (the rendered lines)',
type=str,
default="tcp://0.0.0.0:99174")
render_parser.add_argument('--zmq-log-addr',
help='Manually specity communication addr for the log messages',
type=str,
default="tcp://0.0.0.0:99188")
render_parser.add_argument("--fullscreen",
help="Set Window full screen",
action='store_true')
render_parser.add_argument('--floorplan',
help='specify a map (png-file) onto which overlayed',
type=str,
default="SETTINGS/2025-11-dortmund/space/floorplan.png")
render_parser.add_argument('--monitor',
help='Specify a screen on which to output (eg. HDMI-1)',
type=str,
default="HDMI-1")
return render_parser

3
trap/tracker.py
View file

@ -833,7 +833,8 @@ class Smoother(TrackPointFilter):
else:
# "Unlike Kalman filtering, which focuses on predicting and updating the current state using historical measurements, Kalman smoothing enhances the accuracy of past state values"
# see https://medium.com/@shahalkp1/kalman-smoothing-using-tsmoothie-0175260464e5
self.smoother = KalmanSmoother(component='level_trend', component_noise={'level':0.02, 'season': .01, 'trend':0.02},n_seasons = 2, copy=None)
# self.smoother = KalmanSmoother(component='level_trend', component_noise={'level':0.02, 'season': .01, 'trend':0.02},n_seasons = 2, copy=False)
self.smoother = KalmanSmoother(component='level', component_noise={'level':0.01},observation_noise=.3, n_seasons = 0, copy=False)

8
uv.lock
View file

@ -1889,11 +1889,11 @@ wheels = [
[[package]]
name = "pyglet"
version = "2.1.3"
version = "2.1.11"
source = { registry = "https://pypi.org/simple" }
sdist = { url = "https://files.pythonhosted.org/packages/08/90/7f8a8d939dbf8f6875b957540cc3091e936e41c4ac8f190a9517589678f8/pyglet-2.1.3.tar.gz", hash = "sha256:9a2c3c84228402ea7103443ac8a52060cc1c91419951ec1105845ce30fed2ce8", size = 6515859 }
sdist = { url = "https://files.pythonhosted.org/packages/e3/6b/84c397a74cd33eb377168c682e9e3d6b90c1c10c661e11ea5b397ac8497c/pyglet-2.1.11.tar.gz", hash = "sha256:8285d0af7d0ab443232a81df4d941e0d5c48c18a23ec770b3e5c59a222f5d56e", size = 6594448 }
wheels = [
{ url = "https://files.pythonhosted.org/packages/83/d6/41208b6741e732a7faf160e89346a17e81b14899bd7ae90058da858083d6/pyglet-2.1.3-py3-none-any.whl", hash = "sha256:5a7eaf35869ecf7451fb49cc064c4c0e9a118eaa5e771667c607125b13f85e33", size = 962091 },
{ url = "https://files.pythonhosted.org/packages/77/a2/2b09fbff0eedbe44fbf164b321439a38f7c5568d8b754aa197ee45886431/pyglet-2.1.11-py3-none-any.whl", hash = "sha256:fa0f4fdf366cfc5040aeb462416910b0db2fa374b7d620b7a432178ca3fa8af1", size = 1032213 },
]
[[package]]
@ -2825,7 +2825,7 @@ requires-dist = [
{ name = "opencv-python", path = "opencv_python-4.10.0.84-cp310-cp310-linux_x86_64.whl" },
{ name = "pandas-helper-calc", git = "https://github.com/scls19fr/pandas-helper-calc" },
{ name = "py-to-proto", specifier = ">=0.6.0" },
{ name = "pyglet", specifier = ">=2.0.15,<3" },
{ name = "pyglet", specifier = ">=2.1.8,<3" },
{ name = "pyglet-cornerpin", specifier = ">=0.3.0,<0.4" },
{ name = "python-statemachine", specifier = ">=2.5.0" },
{ name = "pyusb", specifier = ">=1.3.1,<2" },