diff --git a/openpilot/starpilot/conditional_mode/projection.py b/openpilot/starpilot/conditional_mode/projection.py index 15ce3cddc9..e92c28e4f0 100644 --- a/openpilot/starpilot/conditional_mode/projection.py +++ b/openpilot/starpilot/conditional_mode/projection.py @@ -25,7 +25,7 @@ from openpilot.starpilot.conditional_mode.stop import StopFrame, StopLead, StopL from openpilot.starpilot.conditional_mode.turn_scene import committed_turn_scene from openpilot.starpilot.model_geometry import ( STANDSTILL_MODEL_PREFIX_NOISE_M as STANDSTILL_MODEL_PREFIX_NOISE_M, STANDSTILL_SPEED_NOISE_MPS, normalized_origin, - standstill_clear_horizon as _standstill_clear_horizon, + standstill_clear_horizon as _standstill_clear_horizon, standstill_release_speed, ) @@ -449,9 +449,8 @@ class SceneProjector: horizon = _horizon(model) stop_horizon = horizon model_should_stop = _boolean(_field(_field(model, 'action'), 'shouldStop')) - if (horizon is None and self.stop_detector.committed and model_should_stop is False and - speed is not None and speed <= STANDSTILL_SPEED_NOISE_MPS and - _field(car, 'standstill') is True and _number(_field(car, 'vEgoRaw'), high=80.0) == 0.0): + if (horizon is None and self.stop_detector.committed and model_should_stop is False and speed is not None and + standstill_release_speed(_field(car, 'standstill'), _field(car, 'vEgo'), _field(car, 'vEgoRaw'))): # A stationary model can backstep millimeters near its origin before # projecting forward. Use that endpoint only to clear an existing stop; # Curve geometry and cold stop acquisition retain strict validation. diff --git a/openpilot/starpilot/conditional_mode/tests/test_stop_commit_transport.py b/openpilot/starpilot/conditional_mode/tests/test_stop_commit_transport.py index da6e33a3c1..adea5558ff 100644 --- a/openpilot/starpilot/conditional_mode/tests/test_stop_commit_transport.py +++ b/openpilot/starpilot/conditional_mode/tests/test_stop_commit_transport.py @@ -231,8 +231,8 @@ class TestStopCommitTransport(unittest.TestCase): cases = ((RED_97_X, RED_97_V, False, 0., 0.), (CLEAR_97_X, CLEAR_97_V, True, 0., 0.), (material_tail, CLEAR_97_V, False, 0., 0.), - (CLEAR_97_X, CLEAR_97_V, False, .02, 0.), - (CLEAR_97_X, CLEAR_97_V, False, 0., .01)) + (CLEAR_97_X, CLEAR_97_V, False, .06, 0.), + (CLEAR_97_X, CLEAR_97_V, False, 0., .06)) for position, velocity, should_stop, speed, raw in cases: for _ in range(16): _, result = self.sample(standstill=True, ego_speed=speed, raw_speed=raw, diff --git a/openpilot/starpilot/lead_tracking.py b/openpilot/starpilot/lead_tracking.py index ac84eb101b..9197c5d019 100644 --- a/openpilot/starpilot/lead_tracking.py +++ b/openpilot/starpilot/lead_tracking.py @@ -12,7 +12,7 @@ import math import numpy as np from openpilot.common.realtime import DT_MDL -from openpilot.starpilot.model_geometry import STANDSTILL_SPEED_NOISE_MPS, normalized_origin, standstill_clear_horizon +from openpilot.starpilot.model_geometry import STANDSTILL_RELEASE_SPEED_MPS, normalized_origin, standstill_clear_horizon STOP_DISTANCE_M = 6.0 @@ -159,7 +159,7 @@ class LeadDetector: prob = _number(_get(radar_lead, 'modelProb'), 0.0, 1.0) y_rel = _number(_get(radar_lead, 'yRel'), -20.0, 20.0) path = _path_at_distance(model, distance) if model_fresh and distance is not None else None - if path is None and model_fresh and standstill is True and speed is not None and speed <= STANDSTILL_SPEED_NOISE_MPS: + if path is None and model_fresh and standstill is True and speed is not None and speed <= STANDSTILL_RELEASE_SPEED_MPS: # Only retain a stopped track; these coordinates never enter interpolation. horizon = standstill_clear_horizon(model) try: diff --git a/openpilot/starpilot/longitudinal/force_stop_runtime.py b/openpilot/starpilot/longitudinal/force_stop_runtime.py index c712554ced..d51b3794d5 100644 --- a/openpilot/starpilot/longitudinal/force_stop_runtime.py +++ b/openpilot/starpilot/longitudinal/force_stop_runtime.py @@ -10,7 +10,7 @@ from openpilot.starpilot.conditional_mode.stop import ( ) from openpilot.starpilot.lead_tracking import LeadDetector from openpilot.starpilot.longitudinal.force_stop import ForceStop, StopFrame, StopPlan, vehicle_tune -from openpilot.starpilot.model_geometry import STANDSTILL_SPEED_NOISE_MPS, road_curvature, standstill_clear_horizon +from openpilot.starpilot.model_geometry import road_curvature, standstill_clear_horizon, standstill_release_speed from openpilot.starpilot.saved_source import read_saved from openpilot.starpilot.longitudinal.stop_resume import StopResume @@ -22,7 +22,7 @@ SOURCE_AGE_NS = 150_000_000 def standstill_takeoff_light(model, car) -> bool | None: """Fresh takeoff metadata only; Force Stop keeps its strict spatial policy.""" - if car.standstill is not True or not 0.0 <= car.vEgo <= STANDSTILL_SPEED_NOISE_MPS: + if not standstill_release_speed(car.standstill, car.vEgo, car.vEgoRaw): return None should_stop = model.action.shouldStop if type(should_stop) is not bool: diff --git a/openpilot/starpilot/longitudinal/tests/test_lead_takeoff.py b/openpilot/starpilot/longitudinal/tests/test_lead_takeoff.py index eec0075de3..17c6628aa7 100644 --- a/openpilot/starpilot/longitudinal/tests/test_lead_takeoff.py +++ b/openpilot/starpilot/longitudinal/tests/test_lead_takeoff.py @@ -215,6 +215,17 @@ def test_takeoff_light_carrier_rejects_corrupt_stop_preferences(tmp_path): assert path.read_bytes() == b'{' +@pytest.mark.parametrize('standstill,speed,raw,expected', [ + (True, -0.0008, 0., False), (True, .044, .044, False), (True, .06, .06, None), + (True, -0.0008, .02, None), (True, -.02, 0., None), (False, 0., 0., None)]) +def test_takeoff_metadata_accepts_recorded_standstill_speed_noise(standstill, speed, raw, expected): + from openpilot.starpilot.longitudinal.force_stop_runtime import standstill_takeoff_light + xs = [70. * index / 32 for index in range(33)] + xs[1:3] = [0.0005, 0.0002] + model = NS(position=NS(x=xs), action=NS(shouldStop=False)) + assert standstill_takeoff_light(model, NS(standstill=standstill, vEgo=speed, vEgoRaw=raw)) is expected + + @pytest.mark.parametrize('stop,horizon,expected', [(False, 70., False), (True, 70., True), (False, 53.9, True)]) def test_actual_takeoff_metadata_uses_fresh_prefix_without_force_stop_authority(tmp_path, stop, horizon, expected): from openpilot.common.params import Params diff --git a/openpilot/starpilot/model_geometry.py b/openpilot/starpilot/model_geometry.py index 3cd8632329..5e85da262b 100644 --- a/openpilot/starpilot/model_geometry.py +++ b/openpilot/starpilot/model_geometry.py @@ -4,6 +4,7 @@ import math ORIGIN_ROUNDOFF_M = 1e-6 STANDSTILL_SPEED_NOISE_MPS = 0.01 +STANDSTILL_RELEASE_SPEED_MPS = 0.05 STANDSTILL_MODEL_PREFIX_NOISE_M = 0.01 @@ -14,6 +15,12 @@ def normalized_origin(distances: tuple) -> tuple: return distances +def standstill_release_speed(standstill: object, v_ego: object, v_ego_raw: object) -> bool: + speed = _finite(v_ego, low=-STANDSTILL_SPEED_NOISE_MPS, high=STANDSTILL_RELEASE_SPEED_MPS) + raw = _finite(v_ego_raw, low=0.0, high=STANDSTILL_RELEASE_SPEED_MPS) + return standstill is True and speed is not None and raw is not None and (speed >= 0.0 or raw == 0.0) + + def _finite(value: object, *, low: float, high: float) -> float | None: if isinstance(value, bool) or not isinstance(value, (int, float)): return None diff --git a/openpilot/starpilot/tests/test_lead_tracking.py b/openpilot/starpilot/tests/test_lead_tracking.py index b27ca6014c..69bafb5ea1 100644 --- a/openpilot/starpilot/tests/test_lead_tracking.py +++ b/openpilot/starpilot/tests/test_lead_tracking.py @@ -193,7 +193,7 @@ class TestLeadDetector(unittest.TestCase): self.assertFalse(detector.tracked) def test_stopped_track_retains_only_fresh_bounded_origin_geometry(self): - for variation in ('clear', 'stale', 'moving', 'bad_y', 'tail_backstep', 'flag_speed'): + for variation in ('clear', 'quantized', 'stale', 'moving', 'bad_y', 'tail_backstep', 'flag_speed'): detector = LeadDetector() for tick in range(15): step(detector, tick, lead=wire_lead(distance=5, speed=0), speed=2.) @@ -207,9 +207,9 @@ class TestLeadDetector(unittest.TestCase): event.modelV2.position.y = [float('nan') if variation == 'bad_y' else 0.] * 33 model = messaging.log_from_bytes(event.to_bytes()).modelV2 observation = step(detector, 15, model=model, lead=wire_lead(distance=5, speed=.8), - speed=.5 if variation in ('moving', 'flag_speed') else 0., + speed={'moving': .5, 'flag_speed': .5, 'quantized': .044}.get(variation, 0.), standstill=variation != 'moving', model_fresh=variation != 'stale') - if variation == 'clear': + if variation in ('clear', 'quantized'): self.assertTrue(observation.tracked) else: self.assertIsNone(observation.tracked)