mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-22 07:23:53 +08:00
firestarsdog
270 lines
10 KiB
Python
270 lines
10 KiB
Python
import math
|
|
import sys
|
|
from enum import IntEnum
|
|
from types import ModuleType, SimpleNamespace
|
|
|
|
import pytest
|
|
|
|
from openpilot.starpilot.common.accel_profile import ACCELERATION_PROFILES, DECELERATION_PROFILES, get_accel_profile_curve_values
|
|
from openpilot.starpilot.common.longitudinal_personality_profiles import default_personality_profiles, profile_document
|
|
|
|
|
|
def _module(name, **attributes):
|
|
module = ModuleType(name)
|
|
for key, value in attributes.items():
|
|
setattr(module, key, value)
|
|
return module
|
|
|
|
|
|
class _Params:
|
|
def __init__(self, *args, **kwargs):
|
|
self.writes = []
|
|
|
|
def put_nonblocking(self, key, value):
|
|
self.writes.append((key, value))
|
|
|
|
def put_bool(self, key, value):
|
|
self.writes.append((key, value))
|
|
|
|
|
|
sys.modules["openpilot.common.constants"] = _module(
|
|
"openpilot.common.constants", CV=SimpleNamespace(KPH_TO_MS=1 / 3.6, MPH_TO_MS=0.44704),
|
|
)
|
|
sys.modules["openpilot.common.params"] = _module("openpilot.common.params", Params=_Params)
|
|
sys.modules["openpilot.selfdrive.car.cruise"] = _module(
|
|
"openpilot.selfdrive.car.cruise", V_CRUISE_MAX=145, V_CRUISE_UNSET=255,
|
|
)
|
|
sys.modules["openpilot.selfdrive.controls.lib.longitudinal_planner"] = _module(
|
|
"openpilot.selfdrive.controls.lib.longitudinal_planner", A_CRUISE_MIN=-1.0, get_max_accel=lambda _v_ego: 2.0,
|
|
)
|
|
sys.modules["openpilot.starpilot.controls.lib.starpilot_vcruise"] = _module(
|
|
"openpilot.starpilot.controls.lib.starpilot_vcruise",
|
|
get_active_slc_control_target=lambda enabled, set_speed_limit, target, offset, overridden_speed, *_args, **_kwargs: (
|
|
float(overridden_speed or target) + float(offset) if enabled and set_speed_limit else 0.0
|
|
),
|
|
)
|
|
|
|
from openpilot.starpilot.controls.lib.starpilot_acceleration import StarPilotAcceleration
|
|
|
|
|
|
class Personality(IntEnum):
|
|
aggressive = 0
|
|
standard = 1
|
|
relaxed = 2
|
|
|
|
|
|
def _sm(*, traffic=False, personality=Personality.standard, lead=False, force_decel=False):
|
|
lead_state = SimpleNamespace(status=lead, vLead=0.0, aLeadK=-1.0 if lead else 0.0, dRel=10.0 if lead else 1000.0)
|
|
return {
|
|
"carControl": SimpleNamespace(orientationNED=[0.0, 0.0, 0.0]),
|
|
"carState": SimpleNamespace(vCruise=80.0, vEgoCluster=0.0, standstill=False),
|
|
"controlsState": SimpleNamespace(forceDecel=force_decel),
|
|
"radarState": SimpleNamespace(leadOne=lead_state, leadTwo=SimpleNamespace(status=False, vLead=0.0, aLeadK=0.0, dRel=1000.0)),
|
|
"selfdriveState": SimpleNamespace(personality=personality),
|
|
"starpilotCarState": SimpleNamespace(
|
|
ecoGear=False, forceCoast=False, pulseAndGlide=False, sportGear=False, trafficModeEnabled=traffic,
|
|
),
|
|
}
|
|
|
|
|
|
def _planner(v_cruise=20.0):
|
|
return SimpleNamespace(
|
|
starpilot_cem=SimpleNamespace(stop_light_detected=False),
|
|
starpilot_following=SimpleNamespace(disable_throttle=False),
|
|
starpilot_vcruise=SimpleNamespace(slc_target=0.0, slc_offset=0.0, slc=SimpleNamespace(overridden_speed=0.0), forcing_stop=False),
|
|
starpilot_weather=SimpleNamespace(weather_id=0, reduce_acceleration=0.0),
|
|
v_cruise=v_cruise,
|
|
)
|
|
|
|
|
|
def _toggles(document):
|
|
return SimpleNamespace(
|
|
acceleration_profile=ACCELERATION_PROFILES["STANDARD"],
|
|
custom_accel_profile=False,
|
|
custom_accel_profile_breakpoints=[0.0, 5.0, 10.0, 15.0, 20.0, 25.0, 40.0],
|
|
custom_accel_profile_values=[],
|
|
custom_personalities=True,
|
|
deceleration_profile=DECELERATION_PROFILES["STANDARD"],
|
|
ev_tuning=False,
|
|
longitudinal_personality_profiles=document,
|
|
map_acceleration=False,
|
|
map_deceleration=False,
|
|
personality_ev_tuning=False,
|
|
personality_truck_tuning=False,
|
|
pulse_glide_speed_delta=0.0,
|
|
redneck_cruise=False,
|
|
set_speed_limit=False,
|
|
set_speed_offset=0.0,
|
|
speed_limit_controller=False,
|
|
truck_tuning=False,
|
|
)
|
|
|
|
|
|
def _document(*, enabled=True):
|
|
return profile_document(default_personality_profiles(False), enabled=enabled)
|
|
|
|
|
|
@pytest.mark.parametrize(
|
|
("profile", "personality", "traffic_mode", "acceleration", "braking"),
|
|
[
|
|
("aggressive", Personality.aggressive, False, 1.10, 0.60),
|
|
("standard", Personality.standard, False, 1.25, 0.75),
|
|
("relaxed", Personality.relaxed, False, 1.40, 0.90),
|
|
("traffic", Personality.aggressive, True, 1.55, 1.05),
|
|
],
|
|
)
|
|
def test_real_enum_shaped_personality_selects_each_explicit_profile_override(
|
|
profile, personality, traffic_mode, acceleration, braking,
|
|
):
|
|
document = _document()
|
|
document["profiles"][profile]["acceleration"] = {"preset": "custom", "curve": [acceleration] * 10}
|
|
document["profiles"][profile]["braking"] = {"preset": "custom", "curve": [braking] * 10}
|
|
controller = StarPilotAcceleration(_planner(v_cruise=5.0))
|
|
|
|
controller.update(10.0, _sm(traffic=traffic_mode, personality=personality), _toggles(document))
|
|
|
|
assert controller.max_accel == pytest.approx(acceleration)
|
|
assert controller.min_accel == pytest.approx(-braking)
|
|
|
|
|
|
def test_master_toggle_disables_profile_document_overrides():
|
|
document = _document()
|
|
document["profiles"]["standard"]["acceleration"] = {"preset": "custom", "curve": [1.25] * 10}
|
|
toggles = _toggles(document)
|
|
toggles.custom_personalities = False
|
|
|
|
controller = StarPilotAcceleration(_planner())
|
|
controller.update(0.0, _sm(personality=Personality.standard), toggles)
|
|
|
|
assert controller.max_accel == pytest.approx(2.0)
|
|
|
|
|
|
@pytest.mark.parametrize(
|
|
("profile", "personality", "traffic_mode", "legacy_max_accel", "legacy_min_accel"),
|
|
[
|
|
("traffic", Personality.aggressive, True, 1.1, -0.35),
|
|
("aggressive", Personality.aggressive, False, 2.0, -1.0),
|
|
("standard", Personality.standard, False, 2.0, -1.0),
|
|
("relaxed", Personality.relaxed, False, 2.0, -1.0),
|
|
],
|
|
)
|
|
def test_disabled_active_profile_keeps_legacy_acceleration_path(
|
|
profile, personality, traffic_mode, legacy_max_accel, legacy_min_accel,
|
|
):
|
|
document = _document()
|
|
document["profiles"][profile]["acceleration"] = {"preset": "custom", "curve": [1.25] * 10}
|
|
document["profiles"][profile]["braking"] = {"preset": "custom", "curve": [0.75] * 10}
|
|
toggles = _toggles(document)
|
|
setattr(toggles, f"{profile}_personality_profile", False)
|
|
controller = StarPilotAcceleration(_planner())
|
|
|
|
controller.update(0.0, _sm(traffic=traffic_mode, personality=personality), toggles)
|
|
|
|
assert controller.max_accel == pytest.approx(legacy_max_accel)
|
|
assert controller.min_accel == pytest.approx(legacy_min_accel)
|
|
|
|
|
|
def test_detected_truck_curve_is_used_without_enabling_legacy_truck_tuning():
|
|
document = _document()
|
|
document["profiles"]["standard"]["acceleration"] = {"preset": "sport_plus", "curve": []}
|
|
toggles = _toggles(document)
|
|
toggles.personality_truck_tuning = True
|
|
|
|
controller = StarPilotAcceleration(_planner())
|
|
controller.update(0.0, _sm(personality=Personality.standard), toggles)
|
|
|
|
expected = get_accel_profile_curve_values(ACCELERATION_PROFILES["SPORT_PLUS"], False, True)[0]
|
|
assert controller.max_accel == pytest.approx(expected)
|
|
|
|
|
|
def test_fresh_profile_defaults_keep_legacy_acceleration_and_braking():
|
|
document = _document()
|
|
toggles = _toggles(document)
|
|
toggles.custom_accel_profile = True
|
|
toggles.custom_accel_profile_values = [3.0] * 7
|
|
controller = StarPilotAcceleration(_planner())
|
|
|
|
controller.update(0.0, _sm(personality=Personality.aggressive), toggles)
|
|
assert controller.max_accel == pytest.approx(3.0)
|
|
assert controller.min_accel == pytest.approx(-1.0)
|
|
|
|
controller.update(0.0, _sm(traffic=True), toggles)
|
|
assert controller.max_accel == pytest.approx(1.1)
|
|
assert controller.min_accel == pytest.approx(-0.35)
|
|
|
|
|
|
def test_absent_disabled_malformed_partial_wrong_version_and_nonfinite_use_legacy_path():
|
|
candidates = [None, {}, _document(enabled=False), _document(), _document(), _document()]
|
|
candidates[3]["schemaVersion"] = 99
|
|
del candidates[4]["profiles"]["standard"]["braking"]
|
|
candidates[5]["profiles"]["standard"]["acceleration"] = {"preset": "custom", "curve": [math.nan] * 10}
|
|
|
|
for candidate in candidates:
|
|
toggles = _toggles(candidate)
|
|
toggles.custom_accel_profile = True
|
|
toggles.custom_accel_profile_values = [3.0] * 7
|
|
controller = StarPilotAcceleration(_planner())
|
|
controller.update(0.0, _sm(), toggles)
|
|
assert controller.max_accel == pytest.approx(3.0)
|
|
assert controller.min_accel == pytest.approx(-1.0)
|
|
|
|
|
|
def test_map_gear_force_coast_and_weather_precedence_remains_explicit():
|
|
document = _document()
|
|
document["profiles"]["standard"]["acceleration"] = {"preset": "custom", "curve": [3.5] * 10}
|
|
document["profiles"]["standard"]["braking"] = {"preset": "custom", "curve": [2.0] * 10}
|
|
toggles = _toggles(document)
|
|
toggles.map_acceleration = True
|
|
toggles.map_deceleration = True
|
|
sm = _sm()
|
|
sm["starpilotCarState"].ecoGear = True
|
|
planner = _planner()
|
|
planner.starpilot_weather.weather_id = 1
|
|
planner.starpilot_weather.reduce_acceleration = 0.25
|
|
controller = StarPilotAcceleration(planner)
|
|
|
|
controller.update(0.0, sm, toggles)
|
|
assert controller.max_accel == pytest.approx(1.5 * 0.75)
|
|
assert controller.min_accel == pytest.approx(-0.5)
|
|
assert all(key != "LongitudinalPersonalityProfiles" for key, _value in controller.params.writes)
|
|
|
|
sm["starpilotCarState"].forceCoast = True
|
|
controller.update(0.0, sm, toggles)
|
|
assert controller.min_accel == pytest.approx(-0.5)
|
|
|
|
|
|
def test_custom_acceleration_and_braking_use_the_selected_twenty_mph_point():
|
|
document = _document()
|
|
document["profiles"]["standard"]["acceleration"] = {
|
|
"preset": "custom", "curve": [1.0 + 0.1 * index for index in range(10)],
|
|
}
|
|
document["profiles"]["standard"]["braking"] = {
|
|
"preset": "custom", "curve": [0.75 + 0.1 * index for index in range(10)],
|
|
}
|
|
controller = StarPilotAcceleration(_planner(v_cruise=5.0))
|
|
|
|
controller.update(20.0 * 0.44704, _sm(), _toggles(document))
|
|
|
|
assert controller.max_accel == pytest.approx(1.2)
|
|
assert controller.min_accel == pytest.approx(-0.95)
|
|
|
|
|
|
def test_custom_braking_only_shapes_explicit_cruise_deceleration_and_never_reduces_hazard_authority():
|
|
document = _document()
|
|
document["profiles"]["standard"]["braking"] = {"preset": "custom", "curve": [0.5] * 10}
|
|
toggles = _toggles(document)
|
|
controller = StarPilotAcceleration(_planner(v_cruise=30.0))
|
|
|
|
controller.update(10.0, _sm(lead=False), toggles)
|
|
assert controller.min_accel <= -1.0
|
|
|
|
controller = StarPilotAcceleration(_planner(v_cruise=5.0))
|
|
controller.update(10.0, _sm(lead=False), toggles)
|
|
assert controller.min_accel == pytest.approx(-0.5)
|
|
|
|
controller.update(10.0, _sm(lead=True), toggles)
|
|
assert controller.min_accel <= -1.0
|
|
|
|
controller.update(10.0, _sm(force_decel=True), toggles)
|
|
assert controller.min_accel <= -1.0
|