mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-04 19:43:49 +08:00
Keep conditional experimental mode independent of planner policy
This commit is contained in:
firestar5683
@@ -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()
|
||||
@@ -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__":
|
||||
|
||||
@@ -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",
|
||||
|
||||
+1
-1
@@ -1 +1 @@
|
||||
{"protected_inventory_sha256": "a95c0e85bdc7b7a0832afceadc0f89e3565d4cd48215dbc170df594ad77fe9cb", "schema_version": 1}
|
||||
{"protected_inventory_sha256": "9d10e892ff4106a821c4c67568318d46598503096446efdb729bad3b2462d39c", "schema_version": 1}
|
||||
|
||||
Reference in New Issue
Block a user