From a50ab450a567a4cdee4b3e40b063cf3d80f91f65 Mon Sep 17 00:00:00 2001 From: Joe Rivera Date: Mon, 27 Jul 2026 00:32:00 -0500 Subject: [PATCH] fix(hud): report playback ground speed + count distinct goals reached MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - speed: the HUD showed the sim-time speed (~77 m/s), not the pace the viewer sees. Now reported as the on-screen ground speed — frame displacement x frame rate in the video capture, and frame displacement / dt in the live demos (MetricsPublisher.publish(m, dt)). - reached: NavStats.goals_reached counted every per-frame reached flag (so a 4-goal drive read 'reached 6'). Now counts DISTINCT goal indices via a set. Added a test. --- grl_snam/demos/_common.py | 11 +++++++++-- grl_snam/demos/austin_freedrive.py | 2 +- grl_snam/demos/austin_learned.py | 2 +- grl_snam/metrics.py | 9 +++++++-- grl_snam/tools/capture.py | 3 +++ tests/test_nav.py | 11 +++++++++++ 6 files changed, 32 insertions(+), 6 deletions(-) diff --git a/grl_snam/demos/_common.py b/grl_snam/demos/_common.py index 186253b..22f03dd 100644 --- a/grl_snam/demos/_common.py +++ b/grl_snam/demos/_common.py @@ -143,8 +143,15 @@ def __init__(self, app, pycvc, base="grl_snam.metrics", print_every=45): self._base = base self._print_every = print_every self._n = 0 - - def publish(self, m: NavMetrics) -> None: + self._prev = None + + def publish(self, m: NavMetrics, dt: float | None = None) -> None: + # report the on-screen ground speed (frame displacement / dt), not the sim-time + # speed, so the HUD matches what the viewer sees. + if dt and dt > 0 and self._prev is not None: + dx, dy = m.x - self._prev[0], m.y - self._prev[1] + m.speed_mps = (dx * dx + dy * dy) ** 0.5 / dt + self._prev = (m.x, m.y) self._n += 1 for k, v in m.as_dict().items(): if isinstance(v, bool): diff --git a/grl_snam/demos/austin_freedrive.py b/grl_snam/demos/austin_freedrive.py index 9b8a508..be8b047 100644 --- a/grl_snam/demos/austin_freedrive.py +++ b/grl_snam/demos/austin_freedrive.py @@ -114,5 +114,5 @@ def step(dt: float) -> None: _S["lab"].node("agent0").setTransform(_S["vpose"].update(x, y, dt)) eye, tgt, up = _S["chase"].update((x, y, _S["sample"](x, y)), dt) _S["cam"].look(eye, tgt, up) - _S["metrics"].publish(m) + _S["metrics"].publish(m, dt) _S["lab"].pump() diff --git a/grl_snam/demos/austin_learned.py b/grl_snam/demos/austin_learned.py index d4a2771..8d72b35 100644 --- a/grl_snam/demos/austin_learned.py +++ b/grl_snam/demos/austin_learned.py @@ -208,5 +208,5 @@ def step(dt: float) -> None: _S["lab"].node("agent0").setTransform(_S["vpose"].update(m.x, m.y, dt)) eye, tgt, up = _S["chase"].update((m.x, m.y, _S["sample"](m.x, m.y)), dt) _S["cam"].look(eye, tgt, up) - _S["metrics"].publish(m) + _S["metrics"].publish(m, dt) _S["lab"].pump() diff --git a/grl_snam/metrics.py b/grl_snam/metrics.py index 14c686d..9fd3231 100644 --- a/grl_snam/metrics.py +++ b/grl_snam/metrics.py @@ -50,17 +50,17 @@ class NavStats: steps: int = 0 penetration_steps: int = 0 - goals_reached: int = 0 min_clearance_m: float = field(default=1e9) total_path_m: float = 0.0 _prev: tuple | None = None + _reached: set = field(default_factory=set) def update(self, m: NavMetrics) -> None: self.steps += 1 if m.inside_building: self.penetration_steps += 1 if m.reached: - self.goals_reached += 1 + self._reached.add(m.goal_index) # count DISTINCT goals, not per-frame reached flags self.min_clearance_m = min(self.min_clearance_m, m.clearance_m) if self._prev is not None: dx, dy = m.x - self._prev[0], m.y - self._prev[1] @@ -71,6 +71,11 @@ def update(self, m: NavMetrics) -> None: def penetration_pct(self) -> float: return 100.0 * self.penetration_steps / max(1, self.steps) + @property + def goals_reached(self) -> int: + """Distinct goals reached over the drive (by goal index).""" + return len(self._reached) + def hud_lines(m: NavMetrics, stats: NavStats | None = None) -> list[str]: """Human-readable HUD lines for a metrics snapshot (used as on-frame overlay text diff --git a/grl_snam/tools/capture.py b/grl_snam/tools/capture.py index 89e804b..c015cf1 100644 --- a/grl_snam/tools/capture.py +++ b/grl_snam/tools/capture.py @@ -314,6 +314,9 @@ def shaded_cone(height, rad, col, down=True): camera.SetFocalPoint(px + head[0] * ahead, py + head[1] * ahead, zt - 2.0) camera.SetViewUp(0, 0, 1) if hud_actor is not None: + # report the PLAYBACK ground speed (matches what the viewer sees), not the + # sim-time speed: frame-to-frame displacement x frame rate. + m.speed_mps = float(nn) * fps stats.update(m) hud_actor.SetInput("\n".join(hud_lines(m, stats))) rw.Render() diff --git a/tests/test_nav.py b/tests/test_nav.py index a822cb9..5d2f575 100644 --- a/tests/test_nav.py +++ b/tests/test_nav.py @@ -61,3 +61,14 @@ def test_hud_lines_and_stats(): assert any("coeffs" in ln for ln in lines) assert any("reached" in ln for ln in lines) assert stats.steps == 1 + + +def test_goals_reached_counts_distinct_not_per_frame(): + stats = NavStats() + # goal 0 reported reached over 3 frames, then goal 1 over 2 frames -> 2 distinct, not 5 + for _ in range(3): + stats.update(NavMetrics(goal_index=0, reached=True)) + for _ in range(2): + stats.update(NavMetrics(goal_index=1, reached=True)) + assert stats.goals_reached == 2 + assert stats.steps == 5