Restore lane centering with fresh polled state

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 body_dimensions_required, current_delay, interface_dimensions
+from openpilot.starpilot.lateral.lane_clearance import 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
@@ -58,7 +58,9 @@ ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys())
 
 
 class Controls:
-  def __init__(self) -> None:
+  def __init__(self, *, predict_lane_clearance: bool = False) -> None:
+    if type(predict_lane_clearance) is not bool:
+      raise TypeError("Invalid lane-clearance selection")
     self.params = Params()
     cloudlog.info("controlsd is waiting for CarParams")
     self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams)
@@ -96,8 +98,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),
-                                                          body_required=body_dimensions_required(self.CI, self.CP))
+    self.lane_centering_controller = (LaneCenteringController(body=interface_dimensions(self.CI, self.CP), body_required=True)
+                                      if predict_lane_clearance else LaneCenteringController())
     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())
@@ -573,7 +575,7 @@ class Controls:
     feedback = feedback_message(
       self.last_lane_centering_result, self.lane_centering_applied, CC.latActive,
       model_mono_time=self.sm.logMonoTime['modelV2'], car_control_mono_time=cc_send.logMonoTime,
-      valid=bool(CS.canValid and self.sm.all_checks(['modelV2'])), inactive_reason=self.lane_centering_inactive_reason)
+      valid=bool(CS.canValid and self.sm.all_checks(['modelV2'])), inactive_reason=getattr(self, 'lane_centering_inactive_reason', 'inactive'))
     automatic = getattr(self, 'global_learning', None)
     if automatic is not None:
       try:

openpilot/starpilot/lateral/lane_runtime.py

@@ -51,6 +51,7 @@ class LaneCenteringHost:
   REFRESH_NS = 1_000_000_000
   MAX_TICK_NS = 50_000_000
   REQUIRED = ('carState', 'selfdriveState', 'modelV2', 'vehicleParameters')
+  FREQUENCY_CHECKED = ('carState', 'modelV2', 'vehicleParameters')
 
   def __init__(self, params):
     self.params = params
@@ -81,7 +82,10 @@ class LaneCenteringHost:
     if sm['carState'].canTimeout:
       self.last_refusal = 'host_can_timeout'
       return None
-    if not sm.all_checks(list(self.REQUIRED)):
+    # The polled subscriber's mean frequency measures this consumer loop.
+    # Its source still requires a live, valid snapshot within the bound below.
+    if not (sm.all_checks(list(self.FREQUENCY_CHECKED)) and
+            sm.all_alive(['selfdriveState']) and sm.all_valid(['selfdriveState'])):
       self.last_refusal = self._unhealthy_reason(sm)
       return None
     for name in self.REQUIRED:
@@ -102,7 +106,8 @@ class LaneCenteringHost:
     for method, reason in (('all_alive', 'not_alive'), ('all_freq_ok', 'frequency'), ('all_valid', 'invalid')):
       check = getattr(sm, method, None)
       if check is not None:
-        for name in self.REQUIRED:
+        names = self.FREQUENCY_CHECKED if method == 'all_freq_ok' else self.REQUIRED
+        for name in names:
           if not check([name]):
             return f'host_{name}_{reason}'
     return 'host_checks'

openpilot/starpilot/lateral/tests/test_lane_runtime.py

@@ -52,6 +52,12 @@ class Frame:
   def all_checks(self, _services):
     return self.checked
 
+  def all_alive(self, _services):
+    return True
+
+  def all_valid(self, _services):
+    return True
+
   def __getitem__(self, _service):
     return self.state
 
@@ -156,7 +162,7 @@ class LaneRuntimeTests(unittest.TestCase):
       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, candidate = Controls(), Controls(predict_lane_clearance=True)
       baseline.lane_centering_host = None
       body = candidate.lane_centering_controller._body
       self.assertIsNotNone(body)
@@ -218,7 +224,7 @@ class LaneRuntimeTests(unittest.TestCase):
         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, candidate = Controls(), Controls(predict_lane_clearance=True)
         baseline.lane_centering_host = None
         for tick in range(40):
           now = 2_000_000_000 + tick * 10_000_000
@@ -297,7 +303,7 @@ class LaneRuntimeTests(unittest.TestCase):
       params.put('CarParams', tagged.to_bytes(), block=True)
       params.put_bool('LaneCentering', True, block=True)
       params.put('LaneCenteringE2EAuthority', 0., block=True)
-      controls = Controls()
+      controls = Controls(predict_lane_clearance=True)
       for tick in range(35):
         feed(controls, 2_000_000_000 + tick * 10_000_000, tick)
         command, _ = controls.state_control()
@@ -533,7 +539,7 @@ class LaneRuntimeTests(unittest.TestCase):
       stock, tagged = ioniq_candidate()
       params.put('LaneCenteringE2EAuthority', 0.0, block=True)
       params.put('CarParams', tagged.to_bytes(), block=True)
-      disabled = Controls()
+      disabled = Controls(predict_lane_clearance=True)
       self.assertIsNotNone(disabled.lane_centering_host)
       frame = Frame(1_000_000_000)
       self.assertIsNone(disabled.lane_centering_host.sample(frame, now_ns=1_000_000_000,
@@ -637,6 +643,8 @@ class LaneRuntimeTests(unittest.TestCase):
       sm.data['carState'] = car_state.as_reader().carState
       for flag, reason in (('alive', 'not_alive'), ('freq_ok', 'frequency'), ('valid', 'invalid')):
         for service in host.REQUIRED:
+          if flag == 'freq_ok' and service == 'selfdriveState':
+            continue
           with self.subTest(flag=flag, service=service):
             getattr(sm, flag)[service] = False
             self.assertIsNone(sample(sm))
@@ -690,6 +698,8 @@ class LaneRuntimeTests(unittest.TestCase):
         controls.state_control()
       for flag, reason in (('alive', 'not_alive'), ('freq_ok', 'frequency'), ('valid', 'invalid')):
         for service in LaneCenteringHost.REQUIRED:
+          if flag == 'freq_ok' and service == 'selfdriveState':
+            continue
           tick += 1
           feed(controls, 1_000_000_000 + tick * 10_000_000, tick)
           getattr(controls.sm, flag)[service] = False
@@ -755,6 +765,82 @@ class LaneRuntimeTests(unittest.TestCase):
                 self.assertIsNone(candidate.lane_centering_controller._tracker.path)
 
 
+  def test_fresh_polled_state_with_slow_consumer_keeps_actual_centering(self):
+    with OpenpilotPrefix(), mock.patch.dict(os.environ, {'AOL_REPLAY_RUNTIME': '0', 'REPLAY': '1'}), \
+         mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'):
+      params = Params()
+      publish_corolla_params(params)
+      params.put_bool('LaneCentering', True, block=True)
+      params.put('LaneCenteringE2EAuthority', 0., block=True)
+      reference, candidate = Controls(), Controls()
+      for tick in range(100):
+        now = 1_000_000_000 + tick * 10_000_000
+        feed(reference, now, tick)
+        feed(candidate, now, tick)
+        if tick >= 30:
+          candidate.sm.freq_ok['selfdriveState'] = False
+        nominal, _ = reference.state_control()
+        command, _ = candidate.state_control()
+        self.assertEqual(command.to_dict(), nominal.to_dict())
+        if tick >= 30:
+          self.assertIsNotNone(candidate.last_lane_centering_result)
+          self.assertGreater(candidate.lane_centering_applied, 0.)
+      for age in (25_000_001, -1):
+        tick += 1
+        now = 1_000_000_000 + tick * 10_000_000
+        feed(candidate, now, tick)
+        candidate.sm.freq_ok['selfdriveState'] = False
+        candidate.sm.logMonoTime['selfdriveState'] = now - age
+        candidate.state_control()
+        self.assertIsNone(candidate.last_lane_centering_result)
+        self.assertEqual(candidate.lane_centering_applied, 0.)
+        self.assertEqual(candidate.lane_centering_controller._correction, 0.)
+      for flag in ('alive', 'valid'):
+        tick += 1
+        now = 1_000_000_000 + tick * 10_000_000
+        feed(candidate, now, tick)
+        candidate.sm.freq_ok['selfdriveState'] = False
+        getattr(candidate.sm, flag)['selfdriveState'] = False
+        candidate.state_control()
+        self.assertIsNone(candidate.last_lane_centering_result)
+        self.assertEqual(candidate.lane_centering_applied, 0.)
+        getattr(candidate.sm, flag)['selfdriveState'] = True
+
+
+  def test_ordinary_centering_and_explicit_prediction_on_close_observed_edge(self):
+    from opendbc.car.hyundai.tests.test_lane_body import identity
+    with 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()
+      metadata = identity()
+      cp.carVin, cp.carFw = metadata.carVin, [vars(fw) for fw in metadata.carFw]
+      params.put('CarParams', cp.to_bytes(), block=True)
+      params.put_bool('LaneCentering', True, block=True)
+      params.put('LaneCenteringE2EAuthority', 0., block=True)
+      ordinary = Controls()
+      m = model(left=-1.2, right=1.8)
+      m.init('roadEdges', 2)
+      for edge, side in zip(m.roadEdges, (-1., 1.), strict=True):
+        edge.x, edge.y = list(range(51)), [2.5 * side] * 51
+      m.roadEdgeStds = [.17, 1.]
+      with mock.patch(f'{__name__}.model', return_value=m):
+        for tick in range(80):
+          feed(ordinary, 1_000_000_000 + tick * 10_000_000, tick)
+          ordinary.state_control()
+      self.assertGreater(ordinary.lane_centering_applied, 0.)
+      self.assertIsNone(ordinary.lane_centering_controller._body)
+      predicted = Controls(predict_lane_clearance=True)
+      self.assertIsNotNone(predicted.lane_centering_controller._body)
+      with mock.patch(f'{__name__}.model', return_value=m):
+        for tick in range(80):
+          feed(predicted, 1_000_000_000 + tick * 10_000_000, tick)
+          predicted.state_control()
+      self.assertEqual(predicted.last_lane_centering_result.reason, 'clearance_limited')
+      self.assertEqual(predicted.lane_centering_applied, 0.)
+      self.assertEqual(predicted.CP.to_dict(), ordinary.CP.to_dict())
+
   def test_saved_settings_bounds_and_corruption(self):
     with OpenpilotPrefix():
       params = Params()

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "ae2fef4916c78f097fd7e3760c93de71077027dc3845722e3833eb4ee8a7c4ec", "schema_version": 1}
+{"protected_inventory_sha256": "42a9f58dadb8e74ac8ee516ab69fc9a162d33336baabf3a013198132c3496bec", "schema_version": 1}