mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-08 13:33:42 +08:00
firestar5683
80fbb4f6db
Register vehicle controls, planner modes, UI actions and Galaxy services after their providers exist. Wire the shared runtime entrypoints and cover their combined behavior. Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com> Co-authored-by: Daniel Koepping <elkoled@gmail.com> Co-authored-by: Danny <dannyirwin31@yahoo.com> Co-authored-by: DimitriChrysafis <Dimitri.chrysafis@gmail.com> Co-authored-by: Dom <168790843+firestar5683@users.noreply.github.com> Co-authored-by: Harald Schäfer <harald.the.engineer@gmail.com> Co-authored-by: Kacper Rączy <gfw.kra@gmail.com> Co-authored-by: Prabhaav Pillai <prabhaav@msu.edu> Co-authored-by: dirwin31 <83434411+dirwin31@users.noreply.github.com> Co-authored-by: firestarsdog <229254897+firestarsdog@users.noreply.github.com> Co-authored-by: stef <19478336+stefpi@users.noreply.github.com> Co-authored-by: whoisdomi <dcantu83@gmail.com> Co-authored-by: Intelli <contact@intelli.software> Co-authored-by: haraschax <6804392+haraschax@users.noreply.github.com> Co-authored-by: prabhaavp <prabhaav@msu.edu>
231 lines
12 KiB
Python
231 lines
12 KiB
Python
"""Traffic profiles composed with the real native planner and acados solver."""
|
|
|
|
import shutil
|
|
import tempfile
|
|
import unittest
|
|
from dataclasses import replace
|
|
|
|
from openpilot.cereal import log
|
|
from openpilot.common.params import Params
|
|
from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
|
|
from openpilot.starpilot.longitudinal.cruise_ceiling import CruiseCeiling, CurveCeiling
|
|
from openpilot.starpilot.longitudinal.profile_document import default_personality_profiles, profile_document
|
|
from openpilot.starpilot.longitudinal.profile_runtime import read_traffic_settings, resolve
|
|
from openpilot.starpilot.longitudinal.tests.test_cruise_ceiling import ACTIVE, V_EGO, message_bytes, messages
|
|
from opendbc.car.honda.interface import CarInterface
|
|
from opendbc.car.honda.values import CAR
|
|
|
|
|
|
class StampedMessages(dict):
|
|
logMonoTime = {'modelV2': 1_000_000_000}
|
|
|
|
|
|
class TrafficAccelerationPlannerTests(unittest.TestCase):
|
|
def setUp(self):
|
|
root = tempfile.mkdtemp(prefix='traffic-planner-')
|
|
self.addCleanup(shutil.rmtree, root, ignore_errors=True)
|
|
self.params = Params(root)
|
|
# These are planner-composition checks, independent of vehicle admission.
|
|
self.cp = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
|
|
|
def tuning(self, *, acceleration=None, braking=None):
|
|
if acceleration is not None or braking is not None:
|
|
self.params.put_bool('CustomPersonalities', True, block=True)
|
|
profiles = default_personality_profiles(False)
|
|
for category, value in (('acceleration', acceleration), ('braking', braking)):
|
|
if value is not None:
|
|
profiles['traffic'][category] = {'preset': 'custom', 'curve': [value] * 10, 'legacyActivation': True}
|
|
self.params.put('LongitudinalPersonalityProfiles', profile_document(profiles, enabled=True, global_braking_response='standard'), block=True)
|
|
target = resolve(read_traffic_settings(self.params), log.LongitudinalPersonality.standard,
|
|
V_EGO, self.cp, traffic_mode=True)
|
|
self.assertIsNotNone(target)
|
|
return target
|
|
|
|
def test_default_traffic_reaches_native_solver_without_mutating_messages(self):
|
|
target = self.tuning()
|
|
self.assertAlmostEqual(target.acceleration_max, 0.44)
|
|
self.assertFalse(target.traffic_braking_custom)
|
|
baseline = LongitudinalPlanner(self.cp, init_v=V_EGO)
|
|
traffic = LongitudinalPlanner(self.cp, init_v=V_EGO)
|
|
for _ in range(60):
|
|
sm, envelopes = messages()
|
|
sm['carControl'].longActive = True
|
|
before = message_bytes(envelopes)
|
|
baseline.update(sm)
|
|
traffic.update(sm, profile_tuning=target, traffic_mode=True)
|
|
self.assertEqual(message_bytes(envelopes), before)
|
|
self.assertEqual(traffic.mpc.solution_status, 0)
|
|
self.assertAlmostEqual(traffic.last_profile.acceleration_max, 0.44)
|
|
self.assertLessEqual(traffic.mpc.params[0, 1], 0.44 + 1e-8)
|
|
self.assertLessEqual(traffic.output_a_target, 0.44 + 1e-8)
|
|
self.assertLess(traffic.a_cruise, baseline.a_cruise)
|
|
|
|
def test_soft_saved_traffic_keeps_slc_and_force_decel_braking(self):
|
|
target = self.tuning(acceleration=0.6, braking=0.35)
|
|
self.assertAlmostEqual(target.acceleration_max, 0.6)
|
|
self.assertAlmostEqual(target.cruise_brake_magnitude, 0.35)
|
|
for force in (False, True):
|
|
with self.subTest(force=force):
|
|
baseline = LongitudinalPlanner(self.cp, init_v=V_EGO)
|
|
traffic = LongitudinalPlanner(self.cp, init_v=V_EGO)
|
|
ceiling = CruiseCeiling(15.0, ACTIVE)
|
|
for _ in range(60):
|
|
sm, _ = messages(force=force)
|
|
sm['carControl'].longActive = True
|
|
baseline.update(sm, cruise_ceiling=ceiling)
|
|
traffic.update(sm, cruise_ceiling=ceiling, profile_tuning=target, traffic_mode=True)
|
|
self.assertEqual(traffic.mpc.solution_status, 0)
|
|
self.assertLessEqual(traffic.a_cruise, baseline.a_cruise + 1e-8)
|
|
self.assertEqual(traffic.last_cruise_ceiling_status, 'force_decel' if force else 'applied')
|
|
self.assertIsNotNone(traffic.last_profile)
|
|
self.assertAlmostEqual(traffic.last_profile.cruise_brake_magnitude, 0.35)
|
|
self.assertLessEqual(traffic.a_cruise, -0.5 + 1e-8)
|
|
|
|
def test_soft_saved_traffic_preserves_lead_and_model_stop_candidates(self):
|
|
target = self.tuning(acceleration=0.6, braking=0.5)
|
|
for case in ('lead', 'e2e'):
|
|
with self.subTest(case=case):
|
|
traffic = LongitudinalPlanner(self.cp, init_v=V_EGO)
|
|
for _ in range(60):
|
|
sm, _ = messages(lead=case == 'lead', e2e=case == 'e2e')
|
|
sm['carControl'].longActive = True
|
|
traffic.update(sm, profile_tuning=target, traffic_mode=True)
|
|
self.assertEqual(traffic.mpc.solution_status, 0)
|
|
self.assertEqual(str(traffic.mpc.source), '1' if case == 'lead' else '4')
|
|
self.assertLess(traffic.output_a_target, -0.5)
|
|
if case == 'e2e':
|
|
self.assertLessEqual(traffic.output_a_target, -2.0)
|
|
self.assertTrue(traffic.output_should_stop)
|
|
|
|
def test_unknown_traffic_or_lost_long_axis_clears_applied_curve_immediately(self):
|
|
target = self.tuning()
|
|
for unknown, long_active in ((True, True), (False, False)):
|
|
with self.subTest(unknown=unknown, long_active=long_active):
|
|
traffic = LongitudinalPlanner(self.cp, init_v=V_EGO)
|
|
for _ in range(30):
|
|
sm, _ = messages()
|
|
sm['carControl'].longActive = True
|
|
traffic.update(sm, profile_tuning=target, traffic_mode=True)
|
|
self.assertAlmostEqual(traffic.last_profile.acceleration_max, 0.44)
|
|
sm, _ = messages()
|
|
sm['carControl'].longActive = long_active
|
|
traffic.update(sm, profile_tuning=target, traffic_mode=None if unknown else True)
|
|
self.assertIsNone(traffic.last_profile)
|
|
self.assertIsNone(traffic.profile_smoother.applied)
|
|
self.assertEqual(traffic.mpc.solution_status, 0)
|
|
|
|
def test_saved_traffic_braking_category_uses_fixed_baseline_for_either_lead_or_standstill(self):
|
|
for magnitude in (0.35, 2.0):
|
|
target = self.tuning(braking=magnitude)
|
|
self.assertTrue(target.traffic_braking_custom)
|
|
no_lead = LongitudinalPlanner(self.cp, init_v=V_EGO)
|
|
for _ in range(65):
|
|
sm, _ = messages()
|
|
sm['carControl'].longActive = True
|
|
sm['carState'].vCruise = 50.0
|
|
no_lead.update(sm, profile_tuning=target, traffic_mode=True)
|
|
self.assertAlmostEqual(no_lead.last_profile.cruise_brake_magnitude, magnitude)
|
|
self.assertAlmostEqual(no_lead.a_cruise, -magnitude)
|
|
for hazard in ('leadOne', 'leadTwo', 'standstill'):
|
|
with self.subTest(magnitude=magnitude, hazard=hazard):
|
|
planner = LongitudinalPlanner(self.cp, init_v=V_EGO)
|
|
baseline = LongitudinalPlanner(self.cp, init_v=V_EGO)
|
|
baseline_target = replace(target, cruise_brake_magnitude=0.42, traffic_braking_custom=False)
|
|
for _ in range(65):
|
|
sm, _ = messages(lead=hazard in ('leadOne', 'leadTwo'))
|
|
sm['carControl'].longActive = True
|
|
sm['carState'].vCruise = 50.0
|
|
if hazard == 'leadOne':
|
|
sm['radarState'].leadTwo.present = False
|
|
elif hazard == 'leadTwo':
|
|
sm['radarState'].leadOne.present = False
|
|
else:
|
|
sm['carState'].standstill = True
|
|
planner.update(sm, profile_tuning=target, traffic_mode=True)
|
|
baseline.update(sm, profile_tuning=baseline_target, traffic_mode=True)
|
|
self.assertEqual(planner.mpc.solution_status, 0)
|
|
self.assertAlmostEqual(planner.last_profile.cruise_brake_magnitude, 0.42)
|
|
self.assertAlmostEqual(planner.a_cruise, baseline.a_cruise, places=5)
|
|
self.assertAlmostEqual(planner.output_a_target, baseline.output_a_target, places=5)
|
|
if hazard != 'standstill':
|
|
self.assertLessEqual(planner.output_a_target, planner.a_cruise + 1e-8)
|
|
|
|
def test_traffic_braking_recovers_smoothly_and_unknown_clears_saved_category(self):
|
|
target = self.tuning(braking=2.0)
|
|
planner = LongitudinalPlanner(self.cp, init_v=V_EGO)
|
|
for _ in range(65):
|
|
sm, _ = messages()
|
|
sm['carControl'].longActive = True
|
|
sm['carState'].vCruise = 50.0
|
|
planner.update(sm, profile_tuning=target, traffic_mode=True)
|
|
self.assertAlmostEqual(planner.last_profile.cruise_brake_magnitude, 2.0)
|
|
sm, _ = messages(lead=True)
|
|
sm['carControl'].longActive = True
|
|
sm['carState'].vCruise = 50.0
|
|
planner.update(sm, profile_tuning=target, traffic_mode=True)
|
|
self.assertAlmostEqual(planner.last_profile.cruise_brake_magnitude, 0.42)
|
|
recovered = []
|
|
for _ in range(16):
|
|
sm, _ = messages()
|
|
sm['carControl'].longActive = True
|
|
sm['carState'].vCruise = 50.0
|
|
planner.update(sm, profile_tuning=target, traffic_mode=True)
|
|
recovered.append(planner.last_profile.cruise_brake_magnitude)
|
|
self.assertAlmostEqual(recovered[0], 0.42 + 2.0 * planner.dt)
|
|
self.assertTrue(all(a <= b for a, b in zip(recovered, recovered[1:], strict=False)))
|
|
self.assertAlmostEqual(recovered[-1], 2.0)
|
|
sm, _ = messages(lead=True)
|
|
sm['carControl'].longActive = True
|
|
planner.update(sm, profile_tuning=target, traffic_mode=True)
|
|
self.assertAlmostEqual(planner.last_profile.cruise_brake_magnitude, 0.42)
|
|
sm, _ = messages(lead=True)
|
|
sm['carControl'].longActive = True
|
|
planner.update(sm, traffic_mode=False)
|
|
self.assertGreater(planner.last_profile.cruise_brake_magnitude, 0.42)
|
|
self.assertAlmostEqual(planner.last_profile.cruise_brake_magnitude, 0.42 + 2.0 * planner.dt)
|
|
sm, _ = messages(lead=True)
|
|
sm['carControl'].longActive = True
|
|
planner.update(sm, profile_tuning=target, traffic_mode=True)
|
|
self.assertAlmostEqual(planner.last_profile.cruise_brake_magnitude, 0.42)
|
|
sm, _ = messages()
|
|
sm['carControl'].longActive = True
|
|
planner.update(sm, profile_tuning=target, traffic_mode=None)
|
|
self.assertIsNone(planner.last_profile)
|
|
self.assertIsNone(planner.profile_smoother.applied)
|
|
|
|
def test_traffic_hazard_preserves_native_stop_override_and_lower_ceilings(self):
|
|
target = self.tuning(braking=0.35)
|
|
for case in ('model_stop', 'force_decel', 'brake_override', 'slc', 'curve'):
|
|
with self.subTest(case=case):
|
|
planner = LongitudinalPlanner(self.cp, init_v=V_EGO)
|
|
baseline = LongitudinalPlanner(self.cp, init_v=V_EGO)
|
|
baseline_target = replace(target, cruise_brake_magnitude=0.42, traffic_braking_custom=False)
|
|
for _ in range(65):
|
|
sm, _ = messages(lead=True, e2e=case == 'model_stop', force=case == 'force_decel')
|
|
sm['carControl'].longActive = True
|
|
sm['carState'].vCruise = 100.0
|
|
sm['carState'].brakePressed = case == 'brake_override'
|
|
cruise_ceiling = CruiseCeiling(15.0, ACTIVE) if case == 'slc' else None
|
|
curve_ceiling = CurveCeiling(15.0, 1_000_000_000) if case == 'curve' else None
|
|
if case == 'curve':
|
|
sm = StampedMessages(sm)
|
|
planner.update(sm, profile_tuning=target, traffic_mode=True,
|
|
cruise_ceiling=cruise_ceiling, curve_ceiling=curve_ceiling)
|
|
baseline.update(sm, profile_tuning=baseline_target, traffic_mode=True,
|
|
cruise_ceiling=cruise_ceiling, curve_ceiling=curve_ceiling)
|
|
self.assertEqual(planner.mpc.solution_status, 0)
|
|
self.assertAlmostEqual(planner.output_a_target, baseline.output_a_target, places=5)
|
|
if case in ('slc', 'curve'):
|
|
self.assertLessEqual(planner.a_cruise, -0.5 + 1e-8)
|
|
elif case == 'force_decel':
|
|
self.assertLessEqual(planner.a_cruise, -0.5 + 1e-8)
|
|
elif case == 'model_stop':
|
|
self.assertEqual(str(planner.mpc.source), '4')
|
|
self.assertTrue(planner.output_should_stop)
|
|
else:
|
|
self.assertEqual(planner.last_curve_ceiling_status, 'absent')
|
|
|
|
|
|
if __name__ == '__main__':
|
|
unittest.main()
|