Require qualified body bounds before added lane correction

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

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

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

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'])

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

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

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256":"7a9f29aecaf71a08a0a69cc1d72d9a28bcb73b86adcf59cb2f2290fc627e6bf2","schema_version":1}
+{"protected_inventory_sha256": "7a9c16f489d35faa07afe77d4be4cdc52c9a8da4a8ff60c2d279154057c386b4", "schema_version": 1}