From 4d804768aba3390d818048aecd7855b9a763de83 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 3 Nov 2026 11:49:49 -0600 Subject: [PATCH] Keep conditional experimental mode independent of planner policy --- .../tests/test_upstream_conditional.py | 154 ++++++++++++++++++ .../longitudinal/upstream/plannerd.py | 74 +++++++-- .../longitudinal/upstream/provenance.json | 4 +- prebuilt.json | 2 +- 4 files changed, 217 insertions(+), 17 deletions(-) create mode 100644 openpilot/starpilot/longitudinal/tests/test_upstream_conditional.py diff --git a/openpilot/starpilot/longitudinal/tests/test_upstream_conditional.py b/openpilot/starpilot/longitudinal/tests/test_upstream_conditional.py new file mode 100644 index 0000000000..b158749744 --- /dev/null +++ b/openpilot/starpilot/longitudinal/tests/test_upstream_conditional.py @@ -0,0 +1,154 @@ +"""Upstream planning policy with genuine CEM proposals and mode consumption.""" +import json +import os +import tempfile +import unittest +from unittest.mock import patch + +from opendbc.car.honda.interface import CarInterface +from opendbc.car.honda.values import CAR +from openpilot.cereal import messaging +from openpilot.common.params import Params +from openpilot.starpilot.conditional_mode.consumer import ModeConsumer +from openpilot.starpilot.conditional_mode.preferences import CEMOptions, ModeChoice, SavedPreferences, encode_preferences +from openpilot.starpilot.conditional_mode.status import settings_fingerprint +from openpilot.starpilot.conditional_mode.runtime_settings import ConditionalSettingsOwner +from openpilot.starpilot.conditional_mode.tests.test_projection import BOOT, MONO, FakeSubMaster, serialized_scene +from openpilot.starpilot.longitudinal.planner_selection import run_selected +from openpilot.starpilot.longitudinal.upstream import plannerd + + +class EndLoop(Exception): + pass + + +class TestUpstreamConditional(unittest.TestCase): + def run_loop(self, *, safe=False, stale=False, clocks=True, replay=False): + directory = self.enterContext(tempfile.TemporaryDirectory()) + params = Params(directory) + cp = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC_BOSCH) + cp.openpilotLongitudinalControl = True + params.put('CarParams', cp.to_bytes(), block=True) + params.put_bool('UseStarPilotLongitudinalPlanner', False, block=True) + params.put_bool('SafeMode', safe, block=True) + params.put_bool('ExperimentalMode', False, block=True) + params.put('ConditionalModeConfig', json.loads(encode_preferences(SavedPreferences( + mode=ModeChoice.CEM, cem=CEMOptions(speed_mps=20., stop_lights=False, lead=False, slower_lead=False)))), block=True) + settings = ConditionalSettingsOwner(params) + consumer = ModeConsumer() + results, statuses, plans, subscriptions = [], [], [], [] + + class Scene(FakeSubMaster): + def __init__(self): + super().__init__({}, MONO) + self.tick = 0 + self.frame = 0 + self.updated = {'modelV2': True} + self.experimental = False + self.now = MONO + + def all_checks(self, service_list=None): + return not stale + + def update(self): + self.tick += 1 + if self.tick > 100: + raise EndLoop + self.frame += 1 + self.now = MONO + self.tick * 50_000_000 + 1_000_000 + stamp = self.now - 1_000_000 + self.stamp(stamp) + self.payloads = serialized_scene(stamp=BOOT + stamp - MONO, speed=15. if self.tick <= 50 else 25., + lead_present=False, experimental_mode=self.experimental) + for name in ('deviceState', 'vehicleParameters'): + event = messaging.new_message(name, valid=True) + if name == 'deviceState': + event.deviceState.started = True + event.deviceState.startedMonoTime = MONO + self.payloads[name] = getattr(messaging.log_from_bytes(event.to_bytes()), name) + self.logMonoTime[name] = stamp + self.recv_time[name] = stamp / 1e9 + self.valid[name] = self.alive[name] = self.seen[name] = True + for name in ('carState', 'controlsState', 'modelV2'): + event = messaging.new_message(name, valid=True) + setattr(event, name, self.payloads[name]) + if name == 'carState': + event.carState.vCruise = 108. + elif name == 'controlsState': + event.controlsState.longControlState = 'pid' + else: + event.modelV2.action.shouldStop = False + event.modelV2.action.desiredAcceleration = -0.5 + self.payloads[name] = getattr(messaging.log_from_bytes(event.to_bytes()), name) + if stale: + self.logMonoTime['modelV2'] -= 1_000_000_000 + + sm = Scene() + + class Publisher: + def send(self, service, event): + if service == 'longitudinalPlan': + plans.append(messaging.log_from_bytes(event.to_bytes()).longitudinalPlan) + if service != 'slcState': + return + status = messaging.log_from_bytes(event.to_bytes()) + statuses.append(status) + settings.refresh(sm.now) + result = consumer.sample(status.slcState, now_ns=sm.now, now_boot_ns=BOOT + sm.now - MONO, + sample_skew_ns=1000, message_ns=sm.now, receipt_ns=sm.now, drive_id=MONO, + model_ns=sm.logMonoTime['modelV2'], car_state_ns=sm.logMonoTime['carState'], authority=not stale, + stock_experimental=False, choice=ModeChoice.CEM, settings_fingerprint=settings_fingerprint(settings.current)) + results.append((sm.tick, result)) + sm.experimental = result.experimental + + def subscribe(services, **kwargs): + subscriptions.extend(services) + return sm + + environment = {'REPLAY': '1'} if replay else {} + with patch.dict(os.environ, environment, clear=True), patch.object(plannerd, 'Params', return_value=params), \ + patch.object(plannerd, 'config_realtime_process'), patch.object(messaging, 'PubMaster', return_value=Publisher()), \ + patch.object(messaging, 'SubMaster', side_effect=subscribe), patch.object(messaging, 'sub_sock'), \ + patch.object(messaging, 'recv_one_or_none', return_value=None), \ + patch.object(plannerd, 'paired_clocks_ns', create=True, + side_effect=lambda: (sm.now, BOOT + sm.now - MONO, 1000) if clocks else None): + with self.assertRaises(EndLoop): + run_selected(params, lambda: self.fail('StarPilot planning policy selected')) + return results, statuses, plans, subscriptions + + def test_cem_switches_upstream_policy_and_returns_to_chill(self): + results, statuses, plans, subscriptions = self.run_loop() + self.assertIn('deviceState', subscriptions) + self.assertIn('longitudinalPlan', subscriptions) + self.assertEqual(len(plans), 100) + self.assertEqual(len(statuses), 100) + self.assertTrue(any(tick < 50 and result.accepted and result.experimental for tick, result in results)) + self.assertTrue(any(tick > 70 and result.accepted and not result.experimental for tick, result in results)) + self.assertTrue(any(str(plan.longitudinalPlanSource) == 'e2e' for plan in plans[:50])) + self.assertFalse(any(str(plan.longitudinalPlanSource) == 'e2e' for plan in plans[75:])) + + def test_stale_sources_never_produce_accepted_override(self): + results, statuses, plans, _ = self.run_loop(stale=True) + self.assertEqual(len(statuses), 100) + self.assertFalse(any(result.accepted for _, result in results)) + self.assertFalse(any(str(plan.longitudinalPlanSource) == 'e2e' for plan in plans)) + + def test_safe_mode_retains_upstream_without_conditional_transport(self): + results, statuses, plans, subscriptions = self.run_loop(safe=True) + self.assertFalse(results or statuses) + self.assertNotIn('deviceState', subscriptions) + self.assertEqual(len(plans), 100) + + def test_missing_live_clock_does_not_publish_conditional_proposals(self): + results, statuses, plans, _ = self.run_loop(clocks=False) + self.assertFalse(results or statuses) + self.assertEqual(len(plans), 100) + + def test_replay_never_invents_live_clock_permission(self): + results, statuses, plans, _ = self.run_loop(replay=True) + self.assertFalse(results or statuses) + self.assertEqual(len(plans), 100) + + +if __name__ == '__main__': + unittest.main() diff --git a/openpilot/starpilot/longitudinal/upstream/plannerd.py b/openpilot/starpilot/longitudinal/upstream/plannerd.py index 05d24fe771..b55b10bfa8 100755 --- a/openpilot/starpilot/longitudinal/upstream/plannerd.py +++ b/openpilot/starpilot/longitudinal/upstream/plannerd.py @@ -1,9 +1,16 @@ #!/usr/bin/env python3 +import os +from collections import deque + from opendbc.car.structs import car from openpilot.common.params import Params from openpilot.common.realtime import Priority, config_realtime_process from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.controls.lib.ldw import LaneDepartureWarning +from openpilot.selfdrive.controls.plannerd import current_manual_event, current_ui_manual_event +from openpilot.starpilot.conditional_mode.planner_host import ConditionalPlannerHost +from openpilot.starpilot.conditional_mode.projection import paired_clocks_ns +from openpilot.starpilot.feature_runtime import enabled as feature_enabled from openpilot.starpilot.longitudinal.upstream.planner import LongitudinalPlanner import openpilot.cereal.messaging as messaging @@ -18,22 +25,61 @@ def main(): ldw = LaneDepartureWarning() longitudinal_planner = LongitudinalPlanner(CP) - pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance']) - sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'vehicleParameters', 'radarState', 'modelV2', 'selfdriveState'], - poll='modelV2') + conditional = ConditionalPlannerHost(params) if feature_enabled(params, CP, 'conditional', os.environ) else None + native_inputs = ['carControl', 'carState', 'controlsState', 'vehicleParameters', 'radarState', 'modelV2', 'selfdriveState'] + optional = ['deviceState', 'longitudinalPlan', 'starpilotRadarState', 'starpilotNavigation'] if conditional is not None else [] + pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance'] + (['slcState'] if conditional is not None else [])) + sm = messaging.SubMaster(native_inputs + optional, poll='modelV2', + ignore_alive=optional, ignore_valid=optional, ignore_avg_freq=optional) + cruise_sock = messaging.sub_sock('slcCruiseEvent', conflate=False) if conditional is not None else None + action_sock = messaging.sub_sock('slcAction', conflate=False) if conditional is not None else None + pending_manual, pending_ui = deque(maxlen=64), deque(maxlen=64) + drive_start_ns = 0 - while True: - sm.update() - if sm.updated['modelV2']: - longitudinal_planner.update(sm) - longitudinal_planner.publish(sm, pm) + try: + while True: + sm.update() + if sm.updated['modelV2']: + longitudinal_planner.update(sm) + if conditional is not None and 'REPLAY' not in os.environ: + clocks = paired_clocks_ns() + if clocks is not None: + now_ns, boot_ns, skew_ns = clocks + drive_id = (int(sm['deviceState'].startedMonoTime) if sm.valid['deviceState'] and sm.alive['deviceState'] and + 0 < sm.logMonoTime['deviceState'] <= now_ns else 0) + if drive_id > 0 and drive_start_ns > 0 and drive_id != drive_start_ns: + conditional.reset() + pending_manual.clear() + pending_ui.clear() + for sock, service, kind, pending in ((cruise_sock, 'slcCruiseEvent', 'conditionalMode', pending_manual), + (action_sock, 'slcAction', 'conditionalModeCycle', pending_ui)): + for _ in range(8): + event = messaging.recv_one_or_none(sock) + if event is None: + break + if event.valid and str(getattr(event, service).kind) == kind: + pending.append(event) + proposal, snapshot = conditional.sample( + sm, CP, longitudinal_planner, now_mono_ns=now_ns, now_boot_ns=boot_ns, sample_skew_ns=skew_ns, + drive_id=drive_id, native_plan_valid=bool(sm.all_checks(native_inputs)), + manual_event=current_manual_event(pending_manual, now_ns, int(sm.logMonoTime['carState'])), + ui_event=current_ui_manual_event(pending_ui, now_ns)) + status = conditional.status.attach(None, proposal, snapshot, now_ns=now_ns, drive_id=drive_id, + model_ns=int(sm.logMonoTime['modelV2']), car_state_ns=int(sm.logMonoTime['carState'])) + pm.send('slcState', status) + if drive_id > 0: + drive_start_ns = drive_id + longitudinal_planner.publish(sm, pm) - ldw.update(sm.frame, sm['modelV2'], sm['carState'], sm['carControl']) - msg = messaging.new_message('driverAssistance') - msg.valid = sm.all_checks() - msg.driverAssistance.leftLaneDeparture = ldw.left - msg.driverAssistance.rightLaneDeparture = ldw.right - pm.send('driverAssistance', msg) + ldw.update(sm.frame, sm['modelV2'], sm['carState'], sm['carControl']) + msg = messaging.new_message('driverAssistance') + msg.valid = sm.all_checks(native_inputs) + msg.driverAssistance.leftLaneDeparture = ldw.left + msg.driverAssistance.rightLaneDeparture = ldw.right + pm.send('driverAssistance', msg) + finally: + if conditional is not None: + conditional.close() if __name__ == "__main__": diff --git a/openpilot/starpilot/longitudinal/upstream/provenance.json b/openpilot/starpilot/longitudinal/upstream/provenance.json index 3d4372e40c..bb27c54932 100644 --- a/openpilot/starpilot/longitudinal/upstream/provenance.json +++ b/openpilot/starpilot/longitudinal/upstream/provenance.json @@ -7,8 +7,8 @@ "plannerd.py": { "upstream_path": "openpilot/selfdrive/controls/plannerd.py", "upstream_sha256": "9c1591c78522e0f89794eb7301113fc616ec6ac079e576a3c9f9ebec64bcbfde", - "adapted_sha256": "d337b242b0406330e51749c9b6ffb2bfa03d64142d45561e0d2451bd0dc1855c", - "adaptation": "Only planner/MPC import paths redirected; no arithmetic/state/publisher changes" + "adapted_sha256": "0af78b0030a2219f3224e28592715a50bc5620a1e644b75c13eeb0cbeeb2afd5", + "adaptation": "Planner/MPC imports redirected; independent existing conditional-mode owner and bounded transport connected outside unchanged upstream planner/MPC arithmetic" }, "planner.py": { "upstream_path": "openpilot/selfdrive/controls/lib/longitudinal_planner.py", diff --git a/prebuilt.json b/prebuilt.json index 17665abaf2..b4e27263df 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "a95c0e85bdc7b7a0832afceadc0f89e3565d4cd48215dbc170df594ad77fe9cb", "schema_version": 1} +{"protected_inventory_sha256": "9d10e892ff4106a821c4c67568318d46598503096446efdb729bad3b2462d39c", "schema_version": 1}