fix(hud): playback ground speed + distinct goals-reached count

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):

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()

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()

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

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()

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