From 8807d791f116072f5da64e366e4d2b97372ef98c Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sun, 25 Oct 2026 20:04:27 -0500 Subject: [PATCH] Require qualified body bounds before added lane correction --- opendbc_repo/opendbc/car/hyundai/interface.py | 9 +-- openpilot/selfdrive/controls/controlsd.py | 5 +- openpilot/starpilot/lateral/lane_centering.py | 9 ++- openpilot/starpilot/lateral/lane_clearance.py | 10 ++- .../lateral/tests/test_lane_centering.py | 37 +++++++++ .../lateral/tests/test_lane_runtime.py | 79 +++++++++++++++---- prebuilt.json | 2 +- 7 files changed, 126 insertions(+), 25 deletions(-) diff --git a/opendbc_repo/opendbc/car/hyundai/interface.py b/opendbc_repo/opendbc/car/hyundai/interface.py index 9c0b43f115..4c2b906967 100644 --- a/opendbc_repo/opendbc/car/hyundai/interface.py +++ b/opendbc_repo/opendbc/car/hyundai/interface.py @@ -34,12 +34,9 @@ class CarInterface(CarInterfaceBase): RadarInterface = RadarInterface @staticmethod - def get_lateral_body_dimensions(cp): - # Hyundai's published pre-facelift IONIQ 6 body dimensions, metres. - # https://www.hyundai.news/uk/models/electrified/ioniq-6/press-kit/hyundai-ioniq-6-delivers-a-new-electric-mobility-experience.html - if cp.carFingerprint == CAR.HYUNDAI_IONIQ_6: - return 1.880, 4.855 - return None + def requires_lateral_body_dimensions(cp): + # The shared platform identifier does not establish a body/camera envelope. + return cp.carFingerprint == CAR.HYUNDAI_IONIQ_6 @staticmethod def startup_required(cp): diff --git a/openpilot/selfdrive/controls/controlsd.py b/openpilot/selfdrive/controls/controlsd.py index e6d3ed2868..605f2a9a1e 100755 --- a/openpilot/selfdrive/controls/controlsd.py +++ b/openpilot/selfdrive/controls/controlsd.py @@ -37,7 +37,7 @@ from openpilot.starpilot.lateral.lane_centering import ( LaneCenteringController, LaneCenteringInput, LaneCenteringRequest, LaneCenteringResult, ) from openpilot.starpilot.lateral.lane_feedback import feedback_message -from openpilot.starpilot.lateral.lane_clearance import current_delay, interface_dimensions +from openpilot.starpilot.lateral.lane_clearance import body_dimensions_required, current_delay, interface_dimensions from openpilot.starpilot.lateral.lane_change_preferences import LiveLaneChangePolicy from openpilot.starpilot.lateral.lane_change_smoothing import LaneChangeSmoother, limit_rate from openpilot.starpilot.lateral.lane_runtime import LaneCenteringHost, runtime_supported as lane_runtime_supported @@ -96,7 +96,8 @@ class Controls: self.curvature = 0.0 self.desired_curvature = 0.0 self.model_turn_assist = ModelTurnAssist() - self.lane_centering_controller = LaneCenteringController(body=interface_dimensions(self.CI, self.CP)) + self.lane_centering_controller = LaneCenteringController(body=interface_dimensions(self.CI, self.CP), + body_required=body_dimensions_required(self.CI, self.CP)) self.lateral_pause = LateralPause(read_lateral_pause(self.params), self.params) self.lane_change_preferences = LiveLaneChangePolicy(self.params, now_ns=0 if os.getenv('REPLAY') == '1' else time.monotonic_ns()) diff --git a/openpilot/starpilot/lateral/lane_centering.py b/openpilot/starpilot/lateral/lane_centering.py index cf52f163b1..51dd5af4e9 100644 --- a/openpilot/starpilot/lateral/lane_centering.py +++ b/openpilot/starpilot/lateral/lane_centering.py @@ -257,11 +257,12 @@ def _reader_geometry_key(model): class LaneCenteringController: - def __init__(self, *, body: BodyDimensions | None = None) -> None: + def __init__(self, *, body: BodyDimensions | None = None, body_required: bool = False) -> None: self._correction = 0.0 self._geometry_cache = None self._tracker = SingleLineTracker() self._body = body + self._body_required = body_required self._clearance_observation = None def reset(self) -> None: @@ -289,6 +290,9 @@ class LaneCenteringController: return self._result(base, reason) def guard_applied(self, model, base: float, candidate: float, measured: float, speed: float, delay) -> float: + if self._body_required and (self._body is None or not self._body.valid): + self.reset() + return base if base == candidate or self._body is None and self._tracker.path is None: return candidate checked_delay = _finite_number(delay) @@ -337,6 +341,9 @@ class LaneCenteringController: if observation.driver_override: self.reset() return self._result(base, "driver_override") + if self._body_required and (self._body is None or not self._body.valid): + self.reset() + return self._result(base, "unqualified_body") if settings.pause_on_signal and observation.turn_signal_active: self._tracker.reset() return self._fade(base, dt, "signal_release") diff --git a/openpilot/starpilot/lateral/lane_clearance.py b/openpilot/starpilot/lateral/lane_clearance.py index 958cdadfe0..0fc3449c88 100644 --- a/openpilot/starpilot/lateral/lane_clearance.py +++ b/openpilot/starpilot/lateral/lane_clearance.py @@ -30,10 +30,18 @@ def interface_dimensions(interface, cp) -> BodyDimensions | None: dimensions = provider(cp) if dimensions is None: return None - body = BodyDimensions(*dimensions) + try: + body = BodyDimensions(*dimensions) + except (TypeError, ValueError): + return None return body if body.valid else None +def body_dimensions_required(interface, cp) -> bool: + provider = getattr(interface, 'requires_lateral_body_dimensions', None) + return bool(provider(cp)) if callable(provider) else False + + def current_delay(sm, now_ns: int) -> float | None: from openpilot.cereal.services import SERVICE_LIST stamp = int(sm.logMonoTime['lateralDelay']) diff --git a/openpilot/starpilot/lateral/tests/test_lane_centering.py b/openpilot/starpilot/lateral/tests/test_lane_centering.py index bd7ec4d78a..6be7a43cf3 100644 --- a/openpilot/starpilot/lateral/tests/test_lane_centering.py +++ b/openpilot/starpilot/lateral/tests/test_lane_centering.py @@ -527,6 +527,40 @@ class TestSingleLineTracking(unittest.TestCase): class TestVehicleClearance(unittest.TestCase): + def test_required_unknown_body_preserves_nominal_and_clears_retained_state(self): + from openpilot.starpilot.lateral.lane_clearance import BodyDimensions + for body in (None, BodyDimensions(1.88, math.nan)): + with self.subTest(body=body): + controller = LaneCenteringController(body=body, body_required=True) + controller._correction = .001 + controller._tracker.path = (np.array(XS), np.full(len(XS), .3)) + source = observation(base_curvature=-.006) + result = controller.update(source) + self.assertEqual((result.candidate_curvature, result.correction, result.reason), (-.006, 0., 'unqualified_body')) + self.assertIsNone(controller._tracker.path) + self.assertIsNone(controller._geometry_cache) + controller._correction = .001 + controller._tracker.path = (np.array(XS), np.full(len(XS), .3)) + self.assertEqual(controller.guard_applied(source.model, -.006, -.005, -.006, 20., .1), -.006) + self.assertEqual(controller._correction, 0.) + self.assertIsNone(controller._tracker.path) + + def test_required_known_body_retains_prediction_and_withdraws_when_lost(self): + from openpilot.starpilot.lateral.lane_clearance import BodyDimensions + controller = LaneCenteringController(body=BodyDimensions(1.88, 4.855), body_required=True) + m = single(edge=10.) + m.roadEdges[0].y = [-10.] * len(XS) + for frame in range(30): + source = observation(m, model_sample_id=frame + 1, elapsed_seconds=.05, + measured_curvature=0., lateral_delay_s=.1) + result = controller.update(source) + self.assertGreater(result.correction, 0.) + self.assertIsNotNone(controller._tracker.path) + controller._body = None + result = controller.update(replace(source, turn_signal_active=True)) + self.assertEqual((result.candidate_curvature, result.correction), (source.base_curvature, 0.)) + self.assertIsNone(controller._tracker.path) + def test_final_command_guard_rejects_worsening_tracking_and_preserves_limits(self): from openpilot.starpilot.lateral.lane_clearance import BodyDimensions controller = LaneCenteringController(body=BodyDimensions(1.88, 4.855)) @@ -593,4 +627,7 @@ class TestVehicleClearance(unittest.TestCase): self.assertIsNone(interface_dimensions(object(), object())) for width, length in ((True, 4.8), (1.8, math.nan), (0., 4.8), (1.8, 20.)): self.assertFalse(BodyDimensions(width, length).valid) + for dimensions in ((1.88,), (1.88, 4.855, 0.), 'unknown', (1.88, math.nan)): + interface = SimpleNamespace(get_lateral_body_dimensions=lambda cp, bounds=dimensions: bounds) + self.assertIsNone(interface_dimensions(interface, object())) diff --git a/openpilot/starpilot/lateral/tests/test_lane_runtime.py b/openpilot/starpilot/lateral/tests/test_lane_runtime.py index d81d3cbcd1..3a64e1541f 100644 --- a/openpilot/starpilot/lateral/tests/test_lane_runtime.py +++ b/openpilot/starpilot/lateral/tests/test_lane_runtime.py @@ -37,6 +37,12 @@ def ioniq_candidate(*, alternate=False): return stock, build_ioniq6_hda2_long_candidate(stock, fingerprint) +def known_body_fixture(): + # Documented pre-facelift body bounds for control-path tests. This injects a + # known body; it does not certify a production VIN or firmware/body mapping. + return mock.patch.object(HyundaiCarInterface, 'get_lateral_body_dimensions', create=True, return_value=(1.88, 4.855)) + + class Frame: def __init__(self, now_ns): self.logMonoTime = dict.fromkeys(LaneCenteringHost.REQUIRED, now_ns) @@ -136,6 +142,62 @@ def feed(controls, timestamp, tick, *, active=True, enabled=True, fault=False, o class LaneRuntimeTests(unittest.TestCase): + def test_unqualified_body_through_actual_controls_preserves_nominal_native_output(self): + for identity in ('absent', 'manual', 'unknown_firmware', 'mismatched_firmware'): + with self.subTest(identity=identity), OpenpilotPrefix(), \ + mock.patch.dict(os.environ, {'LANE_CENTERING_REPLAY_RUNTIME': '0', 'REPLAY': '1', 'AOL_REPLAY_RUNTIME': '0'}), \ + mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'): + params = Params() + _, cp = ioniq_candidate() + if identity == 'manual': + cp.fingerprintSource = 'fixed' + elif identity in ('unknown_firmware', 'mismatched_firmware'): + cp.fingerprintSource = 'fw' + firmware = [{'ecu': 'fwdCamera', 'address': 0x7c4, 'brand': 'hyundai', 'fwVersion': b'UNQUALIFIED'}] + if identity == 'mismatched_firmware': + firmware.append({'ecu': 'fwdRadar', 'address': 0x7d0, 'brand': 'hyundai', + 'fwVersion': b'\xf1\x00CE__ RDR ----- 1.00 1.01 99110-KL000 '}) + cp.carFw = firmware + params.put('CarParams', cp.to_bytes(), block=True) + params.put_bool('LaneCentering', True, block=True) + params.put('LaneCenteringE2EAuthority', 0., block=True) + baseline, candidate = Controls(), Controls() + baseline.lane_centering_host = None + for tick in range(40): + now = 2_000_000_000 + tick * 10_000_000 + feed(baseline, now, tick) + feed(candidate, now, tick) + reference, _ = baseline.state_control() + command, _ = candidate.state_control() + self.assertEqual((command.latActive, command.longActive), (reference.latActive, reference.longActive)) + self.assertEqual(command.actuators.to_dict(), reference.actuators.to_dict()) + self.assertEqual(candidate.desired_curvature, baseline.desired_curvature) + self.assertEqual(candidate.lane_centering_applied, 0.) + self.assertTrue(command.latActive) + self.assertNotEqual(candidate.desired_curvature, 0.) + self.assertTrue(candidate.lane_centering_controller._body_required) + self.assertIsNone(candidate.lane_centering_controller._body) + self.assertEqual(candidate.last_lane_centering_result.reason, 'unqualified_body') + self.assertEqual(candidate.last_lane_centering_result.correction, 0.) + self.assertIsNone(candidate.lane_centering_controller._tracker.path) + + + def test_shared_platform_requires_verified_body_without_retuning_geometry(self): + from openpilot.starpilot.lateral.lane_clearance import body_dimensions_required, interface_dimensions + stock, tagged = ioniq_candidate() + for cp in (stock, tagged): + before = cp.to_dict() + self.assertTrue(body_dimensions_required(HyundaiCarInterface, cp)) + self.assertIsNone(interface_dimensions(HyundaiCarInterface, cp)) + self.assertEqual(cp.to_dict(), before) + other = cp.as_reader().as_builder() + other.carFingerprint = HyundaiCAR.HYUNDAI_IONIQ_5 + self.assertFalse(body_dimensions_required(HyundaiCarInterface, other)) + self.assertIsNone(interface_dimensions(HyundaiCarInterface, other)) + self.assertFalse(body_dimensions_required(HondaCarInterface, stock)) + self.assertIsNone(interface_dimensions(HondaCarInterface, stock)) + + def test_shared_feed_is_one_current_batch_and_delay_qualifies_at_real_rate(self): with OpenpilotPrefix(), mock.patch.dict(os.environ, {'REPLAY': '1', 'SIMULATION': '0'}), \ mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'): @@ -168,20 +230,9 @@ class LaneRuntimeTests(unittest.TestCase): feed(controls, now + tick * 10_000_000, tick, delay_fast=True) self.assertFalse(controls.sm.all_checks(['lateralDelay'])) - def test_body_dimensions_follow_selected_vehicle_without_retuning_geometry(self): - from openpilot.starpilot.lateral.lane_clearance import BodyDimensions, interface_dimensions - stock, tagged = ioniq_candidate() - for cp in (stock, tagged): - before = cp.to_dict() - self.assertEqual(interface_dimensions(HyundaiCarInterface, cp), BodyDimensions(1.88, 4.855)) - self.assertEqual(cp.to_dict(), before) - other = cp.as_reader().as_builder() - other.carFingerprint = HyundaiCAR.HYUNDAI_IONIQ_5 - self.assertIsNone(interface_dimensions(HyundaiCarInterface, other)) - self.assertIsNone(interface_dimensions(HondaCarInterface, stock)) def test_body_guard_rejects_incorrect_delay_rate_with_lateral_axis_still_active(self): - with OpenpilotPrefix(), mock.patch.dict(os.environ, {'LANE_CENTERING_REPLAY_RUNTIME': '0', 'REPLAY': '1', + with OpenpilotPrefix(), known_body_fixture(), mock.patch.dict(os.environ, {'LANE_CENTERING_REPLAY_RUNTIME': '0', 'REPLAY': '1', 'AOL_REPLAY_RUNTIME': '0'}), \ mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'): params = Params() @@ -418,7 +469,7 @@ class LaneRuntimeTests(unittest.TestCase): self.assertTrue(runtime_supported(tagged)) def test_actual_tagged_ioniq_saved_host_uses_fresh_lateral_authority(self): - with OpenpilotPrefix(), mock.patch.dict(os.environ, {'LANE_CENTERING_REPLAY_RUNTIME': '0', 'REPLAY': '1', + with OpenpilotPrefix(), known_body_fixture(), mock.patch.dict(os.environ, {'LANE_CENTERING_REPLAY_RUNTIME': '0', 'REPLAY': '1', 'AOL_REPLAY_RUNTIME': '0'}), \ mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'): params = Params() @@ -454,7 +505,7 @@ class LaneRuntimeTests(unittest.TestCase): self.assertIsNotNone(Controls().lane_centering_host) def test_tagged_ioniq_four_axes_and_stale_reset(self): - with OpenpilotPrefix(), mock.patch.dict(os.environ, {'LANE_CENTERING_REPLAY_RUNTIME': '0', 'REPLAY': '1', + with OpenpilotPrefix(), known_body_fixture(), mock.patch.dict(os.environ, {'LANE_CENTERING_REPLAY_RUNTIME': '0', 'REPLAY': '1', 'AOL_REPLAY_RUNTIME': '0'}), \ mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'): params = Params() diff --git a/prebuilt.json b/prebuilt.json index 6158bfb890..002e97702c 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256":"7a9f29aecaf71a08a0a69cc1d72d9a28bcb73b86adcf59cb2f2290fc627e6bf2","schema_version":1} +{"protected_inventory_sha256": "7a9c16f489d35faa07afe77d4be4cdc52c9a8da4a8ff60c2d279154057c386b4", "schema_version": 1}