Force Stop: use Vision Stop distance

openpilot/starpilot/longitudinal/force_stop.py

@@ -12,6 +12,7 @@ SIGN_CONFIRM_STEP_S = 2 * DT_MDL
 SIGN_MAX_SPEED_MPS = 17.88
 SIGN_MODEL_AGREE_M = 50.0
 SIGN_SEED_M = 27.0
+SIGN_ESTIMATE_HOLD_NS = 2_000_000_000
 SIGN_APPROACH_DECEL = 1.0
 MODEL_APPROACH_DECEL = .65
 
@@ -58,6 +59,8 @@ class StopFrame:
   resume_requested: bool = False
   perception_available: bool = True
   sign: bool = False
+  dashboard_sign: bool = False
+  vision_distance_m: float | None = None
 
 
 @dataclass(frozen=True)
@@ -98,6 +101,32 @@ class ForceStop:
     self.previously_enabled = False
     self.distance = 0.0
     self.distance_cap = 0.0
+    self._clear_sign_estimate()
+
+  def _clear_sign_estimate(self):
+    self.dash_distance = None
+    self.vision_distance = None
+    self.vision_samples = 0
+    self.sign_evidence_ns = 0
+
+  def _sign_estimate(self, frame: StopFrame, now: int, travel: float) -> float | None:
+    if self.sign_evidence_ns and now - self.sign_evidence_ns > SIGN_ESTIMATE_HOLD_NS:
+      self._clear_sign_estimate()
+    if self.dash_distance is not None:
+      self.dash_distance = max(0.0, self.dash_distance - travel)
+    if self.vision_distance is not None:
+      self.vision_distance = max(0.0, self.vision_distance - travel)
+    vision = frame.vision_distance_m
+    if vision is not None and math.isfinite(vision) and vision >= 0:
+      self.vision_samples += 1
+      self.vision_distance = vision if self.vision_distance is None else (
+        self.vision_distance + (vision - self.vision_distance) / self.vision_samples)
+    if frame.dashboard_sign or frame.sign and self.vision_distance is None:
+      self.dash_distance = SIGN_SEED_M if self.dash_distance is None else min(self.dash_distance, SIGN_SEED_M)
+    if frame.sign or vision is not None:
+      self.sign_evidence_ns = now
+    estimates = [value for value in (self.dash_distance, self.vision_distance) if value is not None]
+    return sum(estimates) / len(estimates) if estimates else None
 
   def _clear_standstill(self):
     self.standstill_since_ns = 0
@@ -127,6 +156,7 @@ class ForceStop:
       self.reset()
       return StopPlan()
     available = frame.perception_available
+    sign_estimate = self._sign_estimate(frame, now, max(0.0, speed) * dt)
     if available and (frame.light or frame.raw_model_stopped) or frame.sign:
       if not frame.standstill:
         self.stop_seen_ns = now
@@ -230,10 +260,10 @@ class ForceStop:
         elif available:
           self.distance = min(self.distance, horizon)
         self.distance_cap = max(0.0, self.distance_cap - speed * dt)
-        self.distance = min(self.distance, self.distance_cap + 15 * min(self.distance_cap / 60, 1))
-        if frame.sign and available:
-          if horizon < SIGN_MODEL_AGREE_M or self.sign_confirmed:
-            self.distance = min(self.distance, SIGN_SEED_M)
+        cap = self.distance_cap + 15 * min(self.distance_cap / 60, 1)
+        self.distance = min(self.distance, cap)
+        if sign_estimate is not None and (self.sign_confirmed or frame.sign and available and horizon < SIGN_MODEL_AGREE_M):
+          self.distance = min(sign_estimate, cap)
         effective = self.distance + offset + tune.distance_bias_m
         decel = SIGN_APPROACH_DECEL if frame.sign else MODEL_APPROACH_DECEL
         ceiling = math.sqrt(2 * decel * max(0, effective - tune.handoff_m))

openpilot/starpilot/longitudinal/force_stop_runtime.py

@@ -51,6 +51,7 @@ class ForceStopRuntime:
     self.policy = ForceStop()
     self.resume = StopResume()
     self.vision_stop = StopConfirmation()
+    self.vision_ranged_ns = 0
     self.light = StopLightDetector()
     self.lead = LeadDetector()
     self.plan = StopPlan()
@@ -66,6 +67,7 @@ class ForceStopRuntime:
 
   def reset(self):
     self.vision_stop.reset()
+    self.vision_ranged_ns = 0
     self.resume.reset()
     self.policy.reset()
     self.light.reset()
@@ -184,14 +186,19 @@ class ForceStopRuntime:
         return self.plan
       vision_sign = self.vision_stop.observe(
         read_stop_evidence(sm, now_ns=now_ns, now_boot_ns=now_boot_ns, drive_id=drive_id), now_ns=now_ns)
+      vision_fresh = bool(vision_sign and 0 <= now_ns - vision_sign <= CONTROL_STOP_MAX_AGE_NS)
+      vision_distance = None
+      if vision_fresh and vision_sign != self.vision_ranged_ns and self.vision_stop.distance_m is not None:
+        self.vision_ranged_ns = vision_sign
+        vision_distance = self.vision_stop.distance_m - float(car.vEgo) * (stamp - vision_sign) / 1e9
+      dashboard_sign = dashboard_stop_sign(sm, now_ns=now_ns, drive_id=drive_id)
       frame = StopFrame(stamp, float(car.vEgo), horizon, light_context, horizon is not None and horizon < 50,
                         bool(model.action.shouldStop), bool(car.standstill), True, bool(car.gasPressed),
                         tracked is not None and tracked.tracked is True, bool(lead.present), float(lead.dRel), float(lead.vLead),
                         raw_curve[1] if raw_curve is not None else False, curvature[0] if curvature is not None else None,
                         bool(car.leftBlinker or car.rightBlinker), float(car.steeringAngleDeg), resume_requested=resume_requested,
-                        perception_available=perception_available,
-                        sign=dashboard_stop_sign(sm, now_ns=now_ns, drive_id=drive_id) or
-                        bool(vision_sign and 0 <= now_ns - vision_sign <= CONTROL_STOP_MAX_AGE_NS))
+                        perception_available=perception_available, sign=dashboard_sign or vision_fresh,
+                        dashboard_sign=dashboard_sign, vision_distance_m=vision_distance)
       self.plan = self.policy.step(frame, vehicle_tune(str(cp.carFingerprint)), self.offset)
       if takeoff_enabled:
         self.plan = replace(self.plan, takeoff_light_observed=takeoff_observed, takeoff_light=takeoff_light)

openpilot/starpilot/longitudinal/tests/test_force_stop.py

@@ -261,6 +261,34 @@ class ForceStopTests(unittest.TestCase):
     self.assertFalse(plan.forcing)
     self.assertFalse(self.owner.sign_confirmed)
 
+  def test_vision_distance_replaces_assumed_sign_distance(self):
+    for tick in range(15):
+      plan = self.owner.step(frame(tick, horizon_m=60.), self.tune)
+    plan = self.owner.step(frame(15, horizon_m=60., sign=True, vision_distance_m=35.), self.tune)
+    self.assertTrue(self.owner.sign_confirmed)
+    self.assertAlmostEqual(plan.tracked_distance_m, 35.)
+    plan = self.owner.step(frame(16, horizon_m=60., sign=True), self.tune)
+    self.assertAlmostEqual(plan.tracked_distance_m, 34.5)
+    plan = self.owner.step(frame(17, horizon_m=60., sign=True, vision_distance_m=31.), self.tune)
+    self.assertAlmostEqual(plan.tracked_distance_m, 32.5)
+
+  def test_dashboard_and_vision_distances_are_averaged(self):
+    for tick in range(15):
+      self.owner.step(frame(tick, horizon_m=60.), self.tune)
+    plan = self.owner.step(frame(15, horizon_m=60., sign=True, dashboard_sign=True), self.tune)
+    self.assertAlmostEqual(plan.tracked_distance_m, 27.)
+    plan = self.owner.step(frame(16, horizon_m=60., sign=True, dashboard_sign=True, vision_distance_m=35.), self.tune)
+    self.assertAlmostEqual(plan.tracked_distance_m, (26.5 + 35.) / 2)
+
+  def test_sign_distance_expires_without_evidence_and_never_jumps_back(self):
+    for tick in range(15):
+      self.owner.step(frame(tick, horizon_m=60.), self.tune)
+    self.owner.step(frame(15, horizon_m=60., sign=True, vision_distance_m=30.), self.tune)
+    for tick in range(16, 70):
+      plan = self.owner.step(frame(tick, speed_mps=4., horizon_m=60., light=True), self.tune)
+      self.assertLess(plan.tracked_distance_m, 30.)
+    self.assertIsNone(self.owner.vision_distance)
+
   def test_positive_offset_keeps_stop_target_until_native_stop(self):
     for tick in range(40):
       plan = self.owner.step(frame(tick, horizon_m=12.), self.tune, 8)

openpilot/starpilot/speed_limits/vision/stop_observation.py

@@ -2,12 +2,17 @@
 from dataclasses import dataclass
 import math
 
+from openpilot.common.transformations.camera import DEVICE_CAMERAS
 from openpilot.starpilot.speed_limits.vision.observation import MODEL_ID, MAX_PAIR_SKEW_NS
 
 STOP_MAX_AGE_NS = 1_500_000_000
 CONTROL_STOP_MAX_AGE_NS = 750_000_000
 RECEIPT_MAX_AGE_NS = 1_000_000_000
 STOP_CONFIRM_CONFIDENCE = .7
+STOP_SIGN_HEIGHT_M = .762
+# Fitted on 17 confirmed sign approaches: where the car stops relative to the box range.
+STOP_RANGE_OFFSET_M = 2.0
+ROAD_FOCAL_PX = {camera.narrow_road.size: camera.narrow_road.focal_length for camera in DEVICE_CAMERAS.values()}
 
 
 @dataclass(frozen=True)
@@ -66,6 +71,16 @@ def read_stop_evidence(sm, *, now_ns: int, now_boot_ns: int, drive_id: int,
     return None
 
 
+def stop_distance(sample: StopEvidence, box: StopBox) -> float | None:
+  focal = ROAD_FOCAL_PX.get((sample.width, sample.height)) if sample.stream == 'road' else None
+  x1, y1, x2, y2 = box.bbox
+  if not focal or y2 <= y1:
+    return None
+  across = ((x1 + x2) / 2 - sample.width / 2) / focal
+  up = ((y1 + y2) / 2 - sample.height / 2) / focal
+  return focal * STOP_SIGN_HEIGHT_M / (y2 - y1) / math.sqrt(1 + across ** 2 + up ** 2) + STOP_RANGE_OFFSET_M
+
+
 def _matches(a: StopBox, b: StopBox) -> bool:
   ax, ay, ar, ab = a.bbox
   bx, by, br, bb = b.bbox
@@ -83,6 +98,7 @@ class StopConfirmation:
     self.previous: StopEvidence | None = None
     self.confirmed_ns = 0
     self.first_seen_ns = 0
+    self.distance_m: float | None = None
 
   def observe(self, sample: StopEvidence | None, *, now_ns: int) -> int:
     if sample is None or not 0 <= now_ns - sample.observed_ns < STOP_MAX_AGE_NS:
@@ -102,13 +118,15 @@ class StopConfirmation:
       self.reset()
       return 0
     candidates = tuple(box for box in sample.boxes if box.confidence >= STOP_CONFIRM_CONFIDENCE)
-    coherent = (previous is not None and 100_000_000 <= sample.observed_ns - previous.observed_ns < STOP_MAX_AGE_NS and
-                any(_matches(old, new) for old in previous.boxes if old.confidence >= STOP_CONFIRM_CONFIDENCE for new in candidates))
+    matched = tuple(new for new in candidates if previous is not None and
+                    100_000_000 <= sample.observed_ns - previous.observed_ns < STOP_MAX_AGE_NS and
+                    any(_matches(old, new) for old in previous.boxes if old.confidence >= STOP_CONFIRM_CONFIDENCE))
     if not candidates:
       self.first_seen_ns = 0
-    elif not coherent:
+    elif not matched:
       self.first_seen_ns = sample.observed_ns
     self.previous = sample
-    self.confirmed_ns = (sample.observed_ns if candidates and coherent and
+    self.confirmed_ns = (sample.observed_ns if matched and
                          sample.observed_ns - self.first_seen_ns >= 500_000_000 else 0)
+    self.distance_m = stop_distance(sample, max(matched, key=lambda box: box.confidence)) if self.confirmed_ns else None
     return self.confirmed_ns

openpilot/starpilot/speed_limits/vision/tests/test_stop_observation.py

@@ -121,6 +121,21 @@ class TestStopObservation(unittest.TestCase):
       owner.observe(evidence(), now_ns=BASE)
       self.assertEqual(owner.observe(evidence(BASE + 700_000_000, frame=15, boxes=(box,)), now_ns=BASE + 800_000_000), 0)
 
+  def test_confirmed_road_box_carries_forward_distance(self):
+    box = StopBox(.875, (1214, 400, 1294, 480))
+    for stream, expected in (('road', 27.025), ('wideRoad', None)):
+      owner = StopConfirmation()
+      owner.observe(evidence(boxes=(box,), stream=stream, width=1928, height=1208), now_ns=BASE)
+      self.assertIsNone(owner.distance_m)
+      second = evidence(BASE + 700_000_000, frame=15, boxes=(box,), stream=stream, width=1928, height=1208)
+      self.assertEqual(owner.observe(second, now_ns=BASE + 800_000_000), second.observed_ns)
+      if expected is None:
+        self.assertIsNone(owner.distance_m)
+      else:
+        self.assertAlmostEqual(owner.distance_m, expected, places=2)
+      owner.observe(None, now_ns=BASE + 900_000_000)
+      self.assertIsNone(owner.distance_m)
+
   def test_brief_warning_burst_cannot_confirm_but_sustained_sign_can(self):
     owner = StopConfirmation()
     for i in range(3):