diff --git a/openpilot/starpilot/longitudinal/force_stop.py b/openpilot/starpilot/longitudinal/force_stop.py index 5508139bf0..8ae9c38f91 100644 --- a/openpilot/starpilot/longitudinal/force_stop.py +++ b/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)) diff --git a/openpilot/starpilot/longitudinal/force_stop_runtime.py b/openpilot/starpilot/longitudinal/force_stop_runtime.py index 720d5345d2..29ccfa326f 100644 --- a/openpilot/starpilot/longitudinal/force_stop_runtime.py +++ b/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) diff --git a/openpilot/starpilot/longitudinal/tests/test_force_stop.py b/openpilot/starpilot/longitudinal/tests/test_force_stop.py index 42c0f7a534..e97984bdcf 100644 --- a/openpilot/starpilot/longitudinal/tests/test_force_stop.py +++ b/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) diff --git a/openpilot/starpilot/speed_limits/vision/stop_observation.py b/openpilot/starpilot/speed_limits/vision/stop_observation.py index 559158906d..8d116492d9 100644 --- a/openpilot/starpilot/speed_limits/vision/stop_observation.py +++ b/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 diff --git a/openpilot/starpilot/speed_limits/vision/tests/test_stop_observation.py b/openpilot/starpilot/speed_limits/vision/tests/test_stop_observation.py index 2a3dcf78a8..034708b0c7 100644 --- a/openpilot/starpilot/speed_limits/vision/tests/test_stop_observation.py +++ b/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):