mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-26 11:42:06 +08:00
firestar5683
160 lines
4.4 KiB
Python
160 lines
4.4 KiB
Python
from types import SimpleNamespace
|
|
|
|
import pytest
|
|
|
|
from openpilot.starpilot.common.experimental_state import CEStatus
|
|
from openpilot.starpilot.controls.lib.unified_longitudinal_intent import (
|
|
MODEL_STOP_ENTER,
|
|
UnifiedLongitudinalIntent,
|
|
)
|
|
|
|
|
|
class FakeParamsMemory:
|
|
def __init__(self):
|
|
self.values = {}
|
|
|
|
def put_int(self, key, value):
|
|
self.values[key] = value
|
|
|
|
|
|
def make_controller():
|
|
planner = SimpleNamespace(
|
|
params_memory=FakeParamsMemory(),
|
|
driving_in_curve=False,
|
|
road_curvature_detected=False,
|
|
starpilot_vcruise=SimpleNamespace(
|
|
forcing_stop=False,
|
|
stop_sign_confirmed=False,
|
|
slc=SimpleNamespace(experimental_mode=False),
|
|
),
|
|
)
|
|
return planner, UnifiedLongitudinalIntent(planner)
|
|
|
|
|
|
def make_sm(*, v_ego=10.0, should_stop=False, model_distance=100.0,
|
|
end_speed=10.0, traffic=False, lead_one=None, lead_two=None):
|
|
model = SimpleNamespace(
|
|
action=SimpleNamespace(shouldStop=should_stop),
|
|
position=SimpleNamespace(x=[0.0, model_distance]),
|
|
velocity=SimpleNamespace(x=[v_ego, end_speed]),
|
|
)
|
|
empty_lead = SimpleNamespace(status=False)
|
|
return {
|
|
"modelV2": model,
|
|
"carState": SimpleNamespace(
|
|
standstill=v_ego == 0.0,
|
|
leftBlinker=False,
|
|
rightBlinker=False,
|
|
steeringAngleDeg=0.0,
|
|
),
|
|
"radarState": SimpleNamespace(
|
|
leadOne=lead_one or empty_lead,
|
|
leadTwo=lead_two or empty_lead,
|
|
),
|
|
"starpilotCarState": SimpleNamespace(trafficModeEnabled=traffic),
|
|
}
|
|
|
|
|
|
def toggles(model_first=False):
|
|
return SimpleNamespace(longitudinal_model_preference=model_first)
|
|
|
|
|
|
def settle(controller, sm, *, v_ego=10.0, frames=20):
|
|
for _ in range(frames):
|
|
controller.update(v_ego, sm, toggles())
|
|
|
|
|
|
def test_open_road_has_no_stop_intent():
|
|
_, controller = make_controller()
|
|
sm = make_sm()
|
|
|
|
settle(controller, sm)
|
|
|
|
assert not controller.stop_detected
|
|
assert controller.status_value == CEStatus["OFF"]
|
|
|
|
|
|
def test_model_stop_is_filtered_then_latched():
|
|
_, controller = make_controller()
|
|
sm = make_sm(model_distance=5.0, end_speed=0.0)
|
|
|
|
while controller.stop_filter.x < MODEL_STOP_ENTER:
|
|
controller.update(10.0, sm, toggles())
|
|
|
|
assert controller.stop_detected
|
|
assert controller.status_value == CEStatus["STOP_LIGHT"]
|
|
|
|
|
|
def test_stop_intent_releases_with_hysteresis():
|
|
_, controller = make_controller()
|
|
stop_scene = make_sm(should_stop=True)
|
|
settle(controller, stop_scene)
|
|
assert controller.stop_detected
|
|
|
|
clear_scene = make_sm()
|
|
controller.update(10.0, clear_scene, toggles())
|
|
assert controller.stop_detected
|
|
settle(controller, clear_scene)
|
|
assert not controller.stop_detected
|
|
|
|
|
|
@pytest.mark.parametrize("hard_stop", ["forcing_stop", "stop_sign_confirmed"])
|
|
def test_explicit_stop_sources_pin_intent(hard_stop):
|
|
planner, controller = make_controller()
|
|
setattr(planner.starpilot_vcruise, hard_stop, True)
|
|
|
|
controller.update(5.0, make_sm(v_ego=5.0), toggles())
|
|
|
|
assert controller.stop_detected
|
|
assert controller.status_value == CEStatus["STOP_LIGHT"]
|
|
|
|
|
|
def test_committed_turn_vetoes_model_stop_false_positive():
|
|
planner, controller = make_controller()
|
|
planner.driving_in_curve = True
|
|
sm = make_sm(v_ego=4.0, should_stop=True)
|
|
sm["carState"].leftBlinker = True
|
|
sm["carState"].steeringAngleDeg = 60.0
|
|
|
|
settle(controller, sm, v_ego=4.0)
|
|
|
|
assert not controller.stop_detected
|
|
|
|
|
|
def test_traffic_mode_vetoes_model_stop_but_not_explicit_force_stop():
|
|
planner, controller = make_controller()
|
|
sm = make_sm(should_stop=True, traffic=True)
|
|
settle(controller, sm)
|
|
assert not controller.stop_detected
|
|
|
|
planner.starpilot_vcruise.forcing_stop = True
|
|
controller.update(10.0, sm, toggles())
|
|
assert controller.stop_detected
|
|
|
|
|
|
def test_either_credible_slow_lead_slot_sets_diagnostic_status():
|
|
_, controller = make_controller()
|
|
slow_lead = SimpleNamespace(
|
|
status=True,
|
|
dRel=25.0,
|
|
vLead=4.0,
|
|
modelProb=0.95,
|
|
radar=False,
|
|
)
|
|
sm = make_sm(v_ego=10.0, lead_two=slow_lead)
|
|
|
|
settle(controller, sm)
|
|
|
|
assert not controller.stop_detected
|
|
assert controller.status_value == CEStatus["LEAD"]
|
|
|
|
|
|
def test_model_first_preference_is_status_only_not_a_separate_controller():
|
|
planner, controller = make_controller()
|
|
|
|
controller.update(10.0, make_sm(), toggles(model_first=True))
|
|
|
|
assert not controller.stop_detected
|
|
assert controller.status_value == CEStatus["USER_OVERRIDDEN"]
|
|
assert planner.params_memory.values["CEStatus"] == CEStatus["USER_OVERRIDDEN"]
|