mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-09 22:13:43 +08:00
firestar5683
14cb60b8e5
Implement acceleration profiles, FollowJerk, force-stop and lead-departure owners. Keep selected acceleration limits and hazard arbitration explicit at their shared boundaries.
286 lines
16 KiB
Python
286 lines
16 KiB
Python
"""Actual SLC reducers compose into an optional native planner ceiling."""
|
|
|
|
import unittest
|
|
from dataclasses import replace
|
|
|
|
from openpilot.starpilot.speed_limits import acceptance as acc
|
|
from openpilot.starpilot.speed_limits import action_arbitration as arb
|
|
from openpilot.starpilot.speed_limits import composition as comp
|
|
from openpilot.starpilot.speed_limits import lead_relaxation as lr
|
|
from openpilot.starpilot.speed_limits import selection as sel
|
|
from openpilot.starpilot.speed_limits import speed_domain as sd
|
|
|
|
SESSION = "composed-drive"
|
|
ACTIVE = acc.Authority(acc.Mode.LONGITUDINAL_ONLY, acc.LongitudinalOwner.SYSTEM, False, True, False, False)
|
|
POLICY = acc.Policy(confirm_lower=False, confirm_higher=False, fallback_previous=True)
|
|
SELECT = sel.SelectionPolicy(sel.SelectionMode.ORDERED, (sel.Source.DASHBOARD, sel.Source.MAP), False)
|
|
OFFSET = sd.OffsetSchedule((sd.OffsetBand(0.0, None, 2.0),))
|
|
LEAD_POLICY = lr.Policy(20.0 * 0.44704, 30.0, 1.2, 0.35, 0.25, 0.001, 0.05,
|
|
tuple(v * 0.44704 for v in (0.0, 5.0, 10.0, 15.0)), (0.7, 0.9, 1.15, 1.35))
|
|
NO_LEAD = lr.LeadEvidence(lr.LeadKind.ABSENT)
|
|
HOST = comp.HostState(True, True, True, False, False, 100.0, 100.0)
|
|
|
|
|
|
def observations(speed: float | None = 15.0, *, kind: acc.ObservationKind = acc.ObservationKind.VALID):
|
|
if kind is acc.ObservationKind.VALID:
|
|
assert speed is not None
|
|
candidate_speed = speed if speed is not None else 0.0
|
|
dashboard = acc.Observation(kind, acc.Candidate(sel.Source.DASHBOARD.value,
|
|
acc.ObservationIdentity(acc.IdentityKind.GEOGRAPHIC, value="road"), candidate_speed)
|
|
if kind is acc.ObservationKind.VALID else None)
|
|
return {sel.Source.DASHBOARD: dashboard,
|
|
sel.Source.MAP: acc.Observation(acc.ObservationKind.ABSENT),
|
|
sel.Source.VISION: acc.Observation(acc.ObservationKind.ABSENT),
|
|
sel.Source.ONLINE: acc.Observation(acc.ObservationKind.ABSENT)}
|
|
|
|
|
|
def frame(now: int, *, speed: float | None = 15.0, source_kind: acc.ObservationKind = acc.ObservationKind.VALID,
|
|
authority: acc.Authority = ACTIVE, host: comp.HostState = HOST,
|
|
policy: acc.Policy = POLICY, offset: sd.OffsetSchedule = OFFSET,
|
|
selected_kind: sd.DomainStatus = sd.DomainStatus.VALID,
|
|
ego_kind: sd.DomainStatus = sd.DomainStatus.VALID,
|
|
ego_raw: float = 20.0, ego_cluster: float = 20.0,
|
|
lead: lr.LeadEvidence = NO_LEAD, action: acc.DriverAction | None = None) -> comp.Frame:
|
|
ego_pair = (sd.SpeedPair(ego_kind, ego_raw, ego_cluster, SESSION, now) if ego_kind is sd.DomainStatus.VALID
|
|
else sd.SpeedPair(ego_kind))
|
|
return comp.Frame(now, observations(speed, kind=source_kind), SELECT, authority, policy, offset,
|
|
selected_kind, ego_pair, False, lead, LEAD_POLICY, host, action=action)
|
|
|
|
|
|
class CompositionTests(unittest.TestCase):
|
|
def setUp(self):
|
|
self.state = comp.new_session(SESSION)
|
|
|
|
def run_frame(self, current: comp.Frame) -> comp.Result:
|
|
result = comp.step(self.state, current)
|
|
self.state = result.state
|
|
return result
|
|
|
|
def test_real_chain_applies_offset_and_ego_delta_once(self):
|
|
result = self.run_frame(frame(0, ego_raw=20.0, ego_cluster=21.0))
|
|
self.assertFalse(result.errors)
|
|
self.assertEqual(result.selection.selected_source, sel.Source.DASHBOARD)
|
|
self.assertEqual(result.acceptance.control_target_mps, 15.0)
|
|
self.assertEqual(result.domain.context.offset_mps, 2.0)
|
|
self.assertEqual(result.coordinate.cap_mps, 16.0)
|
|
self.assertEqual(result.relaxation.cap_mps, 16.0)
|
|
self.assertEqual(result.ceiling.speed_mps, 16.0)
|
|
self.assertEqual(result.ceiling.authority, ACTIVE)
|
|
self.assertEqual(result.state.control_generation, 1)
|
|
|
|
def test_unknown_stale_source_and_qualified_absent_fallback(self):
|
|
self.run_frame(frame(0))
|
|
absent = self.run_frame(frame(1, source_kind=acc.ObservationKind.ABSENT))
|
|
self.assertEqual(absent.acceptance.basis, "previous_accepted")
|
|
self.assertEqual(absent.relaxation.status, "fallback_source")
|
|
self.assertEqual(absent.ceiling.speed_mps, 17.0)
|
|
for now, kind in ((2, acc.ObservationKind.UNKNOWN), (3, acc.ObservationKind.STALE)):
|
|
result = self.run_frame(frame(now, source_kind=kind))
|
|
self.assertIsNone(result.ceiling)
|
|
self.assertIsNone(result.state.lead_prior)
|
|
|
|
def test_uninitialized_driver_and_missing_coordinates_suppress(self):
|
|
unset = self.run_frame(frame(0, host=replace(HOST, driver_v_cruise_kph=255.0)))
|
|
self.assertIsNone(unset.ceiling)
|
|
self.assertIsNone(unset.state.lead_prior)
|
|
self.assertEqual(unset.acceptance.control_target_mps, 15.0)
|
|
missing = self.run_frame(frame(1, selected_kind=sd.DomainStatus.STALE))
|
|
self.assertIsNone(missing.ceiling)
|
|
self.assertIsNone(missing.state.lead_prior)
|
|
resumed = self.run_frame(frame(2))
|
|
self.assertEqual(resumed.ceiling.speed_mps, 17.0)
|
|
|
|
def test_four_modes_stock_and_host_loss(self):
|
|
cases = (
|
|
acc.Authority(acc.Mode.OFF, acc.LongitudinalOwner.NONE, False, False, False, True),
|
|
acc.Authority(acc.Mode.LATERAL_ONLY, acc.LongitudinalOwner.NONE, True, False, False, False),
|
|
acc.Authority(acc.Mode.LONGITUDINAL_ONLY, acc.LongitudinalOwner.STOCK, False, False, True, False),
|
|
ACTIVE,
|
|
acc.Authority(acc.Mode.COMBINED, acc.LongitudinalOwner.SYSTEM, True, True, False, False),
|
|
)
|
|
for authority in cases:
|
|
with self.subTest(authority=authority):
|
|
self.state = comp.new_session(SESSION)
|
|
host = HOST if authority.can_control else replace(HOST, system_long_available=False, long_control_active=False)
|
|
result = self.run_frame(frame(0, authority=authority, host=host))
|
|
self.assertEqual(result.ceiling is not None, authority.can_control)
|
|
self.state = comp.new_session(SESSION)
|
|
self.run_frame(frame(0))
|
|
lost = self.run_frame(frame(1, authority=replace(ACTIVE, longitudinal_active=False),
|
|
host=replace(HOST, long_control_active=False)))
|
|
self.assertIsNone(lost.ceiling)
|
|
self.assertIsNone(lost.state.lead_prior)
|
|
self.assertFalse(lost.state.reset_required)
|
|
resumed = self.run_frame(frame(2))
|
|
self.assertEqual(resumed.state.control_generation, 2)
|
|
|
|
def test_host_authority_disagreement_does_not_advance_acceptance_time(self):
|
|
self.run_frame(frame(0))
|
|
old_acc = self.state.acceptance
|
|
mismatch = self.run_frame(frame(100_000_000, host=replace(HOST, long_control_active=False)))
|
|
self.assertIsNone(mismatch.ceiling)
|
|
self.assertTrue(mismatch.errors)
|
|
self.assertTrue(mismatch.state.reset_required)
|
|
self.assertEqual(mismatch.state.acceptance, old_acc)
|
|
self.assertIsNone(mismatch.state.lead_prior)
|
|
|
|
def test_late_error_preserves_consumed_action_receipt_and_history(self):
|
|
pending_policy = replace(POLICY, confirm_lower=True)
|
|
self.run_frame(frame(0, speed=25.0))
|
|
pending = self.run_frame(frame(1, speed=15.0, policy=pending_policy))
|
|
self.assertIsNotNone(pending.acceptance.state.pending)
|
|
action = acc.DriverAction(SESSION, 1, pending.acceptance.state.pending.decision_id, acc.ActionKind.ACCEPT)
|
|
bad_offset = sd.OffsetSchedule(())
|
|
failed = self.run_frame(frame(2, speed=15.0, policy=pending_policy, action=action, offset=bad_offset))
|
|
self.assertIsNone(failed.ceiling)
|
|
self.assertTrue(failed.errors)
|
|
self.assertTrue(failed.state.reset_required)
|
|
self.assertTrue(failed.acceptance.action_receipt.consumed)
|
|
self.assertEqual(failed.acceptance.state.accepted.candidate.speed_mps, 15.0)
|
|
self.assertEqual(failed.state.acceptance, failed.acceptance.state)
|
|
|
|
def test_rejected_lower_limit_survives_absent_fallback_and_reconnect(self):
|
|
confirmation = replace(POLICY, confirm_lower=True)
|
|
initial = self.run_frame(frame(0, speed=25.0, policy=confirmation))
|
|
self.assertEqual(initial.ceiling.speed_mps, 27.0)
|
|
pending = self.run_frame(frame(1, speed=15.0, policy=confirmation))
|
|
assert pending.acceptance.state.pending is not None
|
|
reject = acc.DriverAction(SESSION, 1, pending.acceptance.state.pending.decision_id, acc.ActionKind.REJECT)
|
|
rejected = self.run_frame(frame(2, speed=15.0, policy=confirmation, action=reject))
|
|
absent = self.run_frame(frame(3, source_kind=acc.ObservationKind.ABSENT, policy=confirmation))
|
|
reconnected = self.run_frame(frame(4, speed=15.0, policy=confirmation))
|
|
self.assertTrue(rejected.acceptance.action_receipt.consumed)
|
|
self.assertEqual(absent.acceptance.basis, "previous_accepted")
|
|
self.assertEqual(reconnected.acceptance.basis, "rejected")
|
|
for result in (rejected, absent, reconnected):
|
|
self.assertFalse(result.errors)
|
|
self.assertEqual(result.acceptance.state.accepted.candidate.speed_mps, 25.0)
|
|
self.assertEqual(result.domain.context.accepted_raw_mps, 25.0)
|
|
self.assertEqual(result.ceiling.speed_mps, 27.0)
|
|
|
|
def test_reversed_monotonic_clock_latches_without_ceiling(self):
|
|
first = self.run_frame(frame(10))
|
|
self.assertIsNotNone(first.ceiling)
|
|
prior_acceptance = self.state.acceptance
|
|
reversed_time = self.run_frame(frame(9))
|
|
self.assertEqual(reversed_time.status, "invalid_frame")
|
|
self.assertTrue(reversed_time.state.reset_required)
|
|
self.assertIsNone(reversed_time.ceiling)
|
|
self.assertIsNone(reversed_time.state.lead_prior)
|
|
self.assertEqual(reversed_time.state.acceptance, prior_acceptance)
|
|
latched = self.run_frame(frame(11))
|
|
self.assertEqual(latched.status, "session_reset_required")
|
|
self.assertIsNone(latched.ceiling)
|
|
|
|
def test_actual_causal_ledger_change_retains_raw_driver_intent(self):
|
|
first = self.run_frame(frame(0))
|
|
context = first.override.context
|
|
assert context is not None
|
|
ledger = arb.step(arb.new_ledger(SESSION),
|
|
arb.BeginAction(SESSION, 1, arb.Origin.DRIVER_CRUISE, context.context_id), now_ns=0).state
|
|
ledger = arb.step(ledger, arb.ResolveAction(SESSION, 1, arb.Disposition.DRIVER_INTENT), now_ns=0).state
|
|
changed = arb.step(ledger, arb.CruiseChange(SESSION, 1, 1, 100.0 / 3.6, 110.0 / 3.6), now_ns=1)
|
|
second_frame = replace(frame(1, host=replace(HOST, driver_v_cruise_kph=110.0, selected_cluster_kph=110.0)),
|
|
classified_cruise=changed)
|
|
second = self.run_frame(second_frame)
|
|
self.assertEqual(second.override.event_receipt.status, "driver_intent")
|
|
self.assertTrue(second.override.event_receipt.consumed)
|
|
assert second.override.state.persistent_selected_mps is not None
|
|
self.assertAlmostEqual(second.override.state.persistent_selected_mps, 110.0 / 3.6)
|
|
third = self.run_frame(frame(2, host=replace(HOST, driver_v_cruise_kph=105.0, selected_cluster_kph=106.0)))
|
|
assert third.override.state.persistent_selected_mps is not None
|
|
self.assertAlmostEqual(third.override.state.persistent_selected_mps, 110.0 / 3.6)
|
|
self.assertGreater(third.ceiling.speed_mps, 110.0 / 3.6) # retained raw intent plus current cluster delta
|
|
|
|
def test_old_ledger_result_does_not_become_a_current_speed_action(self):
|
|
first = self.run_frame(frame(0))
|
|
context = first.override.context
|
|
assert context is not None
|
|
ledger = arb.step(arb.new_ledger(SESSION),
|
|
arb.BeginAction(SESSION, 1, arb.Origin.DRIVER_CRUISE, context.context_id), now_ns=0).state
|
|
ledger = arb.step(ledger, arb.ResolveAction(SESSION, 1, arb.Disposition.DRIVER_INTENT), now_ns=0).state
|
|
old = arb.step(ledger, arb.CruiseChange(SESSION, 1, 1, 100.0 / 3.6, 110.0 / 3.6), now_ns=1)
|
|
rejected = self.run_frame(replace(frame(2, host=replace(HOST, driver_v_cruise_kph=110.0)),
|
|
classified_cruise=old))
|
|
self.assertTrue(rejected.errors)
|
|
self.assertIsNone(rejected.ceiling)
|
|
self.assertEqual(rejected.state.acceptance.last_timestamp_ns, 0)
|
|
|
|
def test_lower_accepted_cap_eases_then_force_stop_clears_continuity(self):
|
|
lead = lr.LeadEvidence(lr.LeadKind.VALID, True, True, 80.0, 23.0, 0.0)
|
|
self.run_frame(frame(0, speed=25.0, ego_raw=27.0, ego_cluster=27.0, lead=lead))
|
|
lower = self.run_frame(frame(50_000_000, speed=20.0, ego_raw=27.0, ego_cluster=27.0, lead=lead))
|
|
self.assertEqual(lower.relaxation.status, "eased")
|
|
assert lower.ceiling is not None and lower.coordinate is not None and lower.coordinate.cap_mps is not None
|
|
self.assertGreater(lower.ceiling.speed_mps, lower.coordinate.cap_mps)
|
|
stopped = self.run_frame(frame(100_000_000, speed=20.0, ego_raw=27.0, ego_cluster=27.0,
|
|
host=replace(HOST, force_stop_active=True), lead=lead))
|
|
self.assertIsNone(stopped.ceiling)
|
|
self.assertIsNone(stopped.state.lead_prior)
|
|
resumed = self.run_frame(frame(150_000_000, speed=20.0, ego_raw=27.0, ego_cluster=27.0, lead=lead))
|
|
self.assertEqual(resumed.ceiling.speed_mps, 22.0)
|
|
self.assertEqual(resumed.state.control_generation, 2)
|
|
|
|
def test_actual_native_planner_default_and_optional_cap_transitions(self):
|
|
from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
|
|
from openpilot.starpilot.longitudinal.tests.test_cruise_ceiling import V_EGO, message_bytes, messages, snapshot
|
|
from opendbc.car.honda.interface import CarInterface
|
|
from opendbc.car.honda.values import CAR
|
|
|
|
cp = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
|
for scenario in ("ordinary", "lead", "e2e", "force"):
|
|
with self.subTest(scenario=scenario):
|
|
self.state = comp.new_session(SESSION)
|
|
default = LongitudinalPlanner(cp, init_v=V_EGO)
|
|
optional = LongitudinalPlanner(cp, init_v=V_EGO)
|
|
deviations = []
|
|
last = None
|
|
for n in range(40):
|
|
active = n >= 12
|
|
host = replace(HOST, force_decel=scenario == "force" and active)
|
|
current = frame(n * 50_000_000, source_kind=(acc.ObservationKind.VALID if active else
|
|
acc.ObservationKind.UNKNOWN), host=host,
|
|
ego_raw=20.0, ego_cluster=21.0)
|
|
result = self.run_frame(current)
|
|
conditions = {"lead": scenario == "lead" and active, "e2e": scenario == "e2e" and active,
|
|
"force": scenario == "force" and active}
|
|
left, left_envelopes = messages(**conditions)
|
|
right, right_envelopes = messages(**conditions)
|
|
assert current.ego_pair.raw_mps is not None and current.ego_pair.cluster_mps is not None
|
|
assert current.host.selected_cluster_kph is not None
|
|
for sm in (left, right):
|
|
sm["carState"].vEgo = current.ego_pair.raw_mps
|
|
sm["carState"].vEgoCluster = current.ego_pair.cluster_mps
|
|
sm["carState"].vCruise = current.host.driver_v_cruise_kph
|
|
sm["carState"].vCruiseCluster = current.host.selected_cluster_kph
|
|
self.assertAlmostEqual(sm["carState"].vEgo, current.ego_pair.raw_mps)
|
|
self.assertAlmostEqual(sm["carState"].vEgoCluster, current.ego_pair.cluster_mps)
|
|
self.assertAlmostEqual(sm["carState"].vCruise, current.host.driver_v_cruise_kph)
|
|
self.assertAlmostEqual(sm["carState"].vCruiseCluster, current.host.selected_cluster_kph)
|
|
before = message_bytes((*left_envelopes, *right_envelopes))
|
|
default.update(left)
|
|
optional.update(right, cruise_ceiling=result.ceiling)
|
|
self.assertEqual(before, message_bytes((*left_envelopes, *right_envelopes)))
|
|
self.assertEqual(default.mpc.solution_status, 0)
|
|
self.assertEqual(optional.mpc.solution_status, 0)
|
|
if snapshot(default) != snapshot(optional):
|
|
deviations.append(n)
|
|
if not active or scenario == "force":
|
|
self.assertIsNone(result.ceiling)
|
|
self.assertEqual(snapshot(default), snapshot(optional))
|
|
last = (snapshot(default), snapshot(optional), optional.last_cruise_ceiling_status)
|
|
if scenario == "force":
|
|
self.assertEqual(deviations, [])
|
|
self.assertEqual(last[2], "absent")
|
|
else:
|
|
self.assertTrue(deviations)
|
|
self.assertEqual(last[2], "applied")
|
|
if scenario == "lead":
|
|
self.assertEqual(last[1][2], "1")
|
|
elif scenario == "e2e":
|
|
self.assertEqual(last[1][2], "4")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
unittest.main()
|