From 4d0246997bd8147e1dd5aef328f915e54fd2aea4 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Mon, 26 Oct 2026 12:36:20 -0500 Subject: [PATCH] Restore lane centering with fresh polled state --- openpilot/selfdrive/controls/controlsd.py | 12 ++- openpilot/starpilot/lateral/lane_runtime.py | 9 +- .../lateral/tests/test_lane_runtime.py | 94 ++++++++++++++++++- prebuilt.json | 2 +- 4 files changed, 105 insertions(+), 12 deletions(-) diff --git a/openpilot/selfdrive/controls/controlsd.py b/openpilot/selfdrive/controls/controlsd.py index 11ec3864d8..bf594b7978 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 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: diff --git a/openpilot/starpilot/lateral/lane_runtime.py b/openpilot/starpilot/lateral/lane_runtime.py index 15b8edd3f7..dd35804db2 100644 --- a/openpilot/starpilot/lateral/lane_runtime.py +++ b/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' diff --git a/openpilot/starpilot/lateral/tests/test_lane_runtime.py b/openpilot/starpilot/lateral/tests/test_lane_runtime.py index 5d6eb6609a..16f893f76e 100644 --- a/openpilot/starpilot/lateral/tests/test_lane_runtime.py +++ b/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() diff --git a/prebuilt.json b/prebuilt.json index 3d3383b537..6ee392a5ce 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "ae2fef4916c78f097fd7e3760c93de71077027dc3845722e3833eb4ee8a7c4ec", "schema_version": 1} +{"protected_inventory_sha256": "42a9f58dadb8e74ac8ee516ab69fc9a162d33336baabf3a013198132c3496bec", "schema_version": 1}