From 56a3f5887d4fca0561bd4e4109b2074cbea50ca2 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Thu, 6 Aug 2026 10:31:52 -0700 Subject: [PATCH] long: preserve smooth stoppingdeacelrate. --- .../selfdrive/controls/lib/longcontrol.py | 5 +- .../test_longcontrol_vehicle_interfaces.py | 220 +++++++++++++----- .../selfdrive/controls/lib/longcontrol.py | 16 ++ .../test/longitudinal_maneuvers/plant.py | 9 +- .../tests/test_plant_sp.py | 23 +- 5 files changed, 197 insertions(+), 76 deletions(-) create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/longcontrol.py diff --git a/openpilot/selfdrive/controls/lib/longcontrol.py b/openpilot/selfdrive/controls/lib/longcontrol.py index e3b612e79f..14e58921ba 100644 --- a/openpilot/selfdrive/controls/lib/longcontrol.py +++ b/openpilot/selfdrive/controls/lib/longcontrol.py @@ -4,6 +4,7 @@ from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N from openpilot.common.pid import PIDController from openpilot.selfdrive.modeld.constants import ModelConstants +from openpilot.sunnypilot.selfdrive.controls.lib.longcontrol import LongControlSP CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] @@ -39,7 +40,7 @@ def long_control_state_trans(CP_SP, active, long_control_state, return long_control_state -class LongControl: +class LongControl(LongControlSP): def __init__(self, CP, CP_SP): self.CP = CP self.CP_SP = CP_SP @@ -66,7 +67,7 @@ class LongControl: elif self.long_control_state == LongCtrlState.stopping: output_accel = self.last_output_accel - if output_accel > self.CP.stopAccel: + if output_accel > self.CP.stopAccel and not LongControlSP.should_hold_stopping(self, CS, a_target): output_accel = min(output_accel, 0.0) # TODO: can we just go straight to stopAccel? output_accel -= 1.0 * DT_CTRL # m/s^2/s while trying to stop diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_longcontrol_vehicle_interfaces.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_longcontrol_vehicle_interfaces.py index 407a15dffe..4128b91065 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_longcontrol_vehicle_interfaces.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_longcontrol_vehicle_interfaces.py @@ -6,78 +6,180 @@ from opendbc.car.ford.values import CAR as FORD from opendbc.car.gm.values import CAR as GM from opendbc.car.honda.values import CAR as HONDA from opendbc.car.hyundai.values import CAR as HYUNDAI +from opendbc.car.rivian.values import CAR as RIVIAN from opendbc.car.toyota.values import CAR as TOYOTA -from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan -from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState, long_control_state_trans +from opendbc.car.volkswagen.values import CAR as VOLKSWAGEN +from openpilot.selfdrive.controls.lib.drive_helpers import should_stop +from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState +from openpilot.sunnypilot.selfdrive.controls.lib.longcontrol import LongControlSP -VEHICLES = [ - pytest.param(TOYOTA.TOYOTA_RAV4_TSS2, (True, False, 0.0, -2.0, 0.25, 0.25, 0.3), id="toyota-rav4-tss2"), - pytest.param(HONDA.HONDA_ACCORD, (True, False, 0.0, -2.0, 0.5, 0.5, 0.8), id="honda-accord"), - pytest.param(GM.CHEVROLET_BOLT_EUV, (True, False, 0.0, -2.0, 0.25, 0.25, 2.0), id="gm-bolt-euv"), - pytest.param(HYUNDAI.HYUNDAI_SONATA, (True, True, 1.0, -2.0, 0.1, 0.5, 0.8), id="hyundai-sonata"), - pytest.param(FORD.FORD_ESCAPE_MK4, (True, False, 0.0, -2.0, 0.5, 0.5, 0.8), id="ford-escape"), -] +VEHICLES = (TOYOTA.TOYOTA_RAV4_TSS2, HONDA.HONDA_ACCORD, HONDA.HONDA_CIVIC_2022, GM.CHEVROLET_BOLT_EUV, + HYUNDAI.HYUNDAI_SONATA, FORD.FORD_ESCAPE_MK4, VOLKSWAGEN.VOLKSWAGEN_ARTEON_MK1, RIVIAN.RIVIAN_R1) +ROUTE_STOP_ONSETS = ( + (0.290, -0.497, -0.270, -0.302), (0.464, -0.223, -0.264, -0.292), (0.467, -0.582, -0.316, -0.359), + (0.530, -0.311, -0.309, -0.333), (0.581, -0.467, -0.312, -0.352), (0.398, -0.557, -0.311, -0.348), + (0.517, -0.290, -0.301, -0.327), (0.312, -0.420, -0.271, -0.304), (0.474, -0.509, -0.303, -0.347), + (0.241, -0.554, -0.573, -0.617), (0.292, -0.154, -0.302, -0.326), +) def get_car_params(candidate): fingerprint = gen_empty_fingerprint() interface = interfaces[candidate] CP = interface.get_params(candidate, fingerprint, [], True, False, False) - CP_SP = interface.get_params_sp(CP, candidate, fingerprint, [], True, False, False) - return CP, CP_SP + return CP, interface.get_params_sp(CP, candidate, fingerprint, [], True, False, False) -@pytest.mark.parametrize(("candidate", "expected"), VEHICLES) -def test_real_vehicle_longcontrol_stop_and_start(candidate, expected): +def make_car_state(v_ego=0.2, a_ego=0.0, standstill=False): + return structs.CarState(vEgo=float(v_ego), aEgo=float(a_ego), standstill=standstill) + + +def make_control(candidate, initial_accel=-0.33): CP, CP_SP = get_car_params(candidate) - expected_long, expected_starting, *expected_tuning = expected - - assert CP.openpilotLongitudinalControl is expected_long - assert CP.startingState is expected_starting - assert (CP.startAccel, CP.stopAccel, CP.vEgoStarting, CP.vEgoStopping, CP.stoppingDecelRate) == pytest.approx(expected_tuning) - - stop_speeds = [CP.vEgoStopping - 0.01] * 2 - drive_speeds = [CP.vEgoStopping + 0.01] * 2 - _, should_stop = get_accel_from_plan(stop_speeds, [0.0, 0.0], [0.0, 1.0], vEgoStopping=CP.vEgoStopping) - _, should_drive = get_accel_from_plan(drive_speeds, [0.0, 0.0], [0.0, 1.0], vEgoStopping=CP.vEgoStopping) - assert should_stop - assert not should_drive - - departure_state = long_control_state_trans( - CP, - CP_SP, - True, - LongCtrlState.stopping, - CP.vEgoStarting - 0.01, - should_drive, - brake_pressed=False, - cruise_standstill=False, - ) - assert departure_state == (LongCtrlState.starting if CP.startingState else LongCtrlState.pid) - assert ( - long_control_state_trans( - CP, - CP_SP, - True, - departure_state, - CP.vEgoStarting + 0.01, - should_drive, - brake_pressed=False, - cruise_standstill=False, - ) - == LongCtrlState.pid - ) - - CS = structs.CarState() - CS.vEgo = 0.0 - CS.aEgo = 0.0 control = LongControl(CP, CP_SP) + control.long_control_state = LongCtrlState.pid + control.last_output_accel = initial_accel + return CP, control - stopping_accel = control.update(True, CS, 0.0, should_stop, (-3.0, 2.0)) + +def stock_stopping_output(output_accel, stop_accel): + return min(output_accel, 0.0) - DT_CTRL if output_accel > stop_accel else output_accel + + +def test_stop_threshold_remains_unchanged(): + assert should_stop(0.24, 0.0) + assert not should_stop(0.26, 0.0) + assert not should_stop(0.24, 0.1) + + +def test_longcontrol_uses_sunnypilot_extension(): + assert LongControl.__bases__ == (LongControlSP,) + + +@pytest.mark.parametrize("candidate", VEHICLES) +@pytest.mark.parametrize(("v_ego", "a_ego", "a_target", "initial_accel"), ROUTE_STOP_ONSETS) +def test_logged_stop_onsets_hold_the_existing_brake(candidate, v_ego, a_ego, a_target, initial_accel): + _, control = make_control(candidate, initial_accel) + output = control.update(True, make_car_state(v_ego, a_ego), a_target, True, (-3.5, 2.0)) assert control.long_control_state == LongCtrlState.stopping - assert stopping_accel == pytest.approx(-CP.stoppingDecelRate * DT_CTRL) + assert output == pytest.approx(initial_accel) - departure_accel = control.update(True, CS, 0.0, should_drive, (-3.0, 2.0)) - assert control.long_control_state == departure_state - assert departure_accel == pytest.approx(CP.startAccel) + +@pytest.mark.parametrize("candidate", VEHICLES) +def test_urgent_braking_matches_the_stock_ramp(candidate): + CP, control = make_control(candidate) + CS = make_car_state(0.8, -0.1) + output = control.last_output_accel + + for _ in range(round(1.0 / DT_CTRL)): + output = control.update(True, CS, -3.0, True, (-3.5, 2.0)) + + expected = -0.33 + for _ in range(round(1.0 / DT_CTRL)): + expected = stock_stopping_output(expected, CP.stopAccel) + assert output == pytest.approx(expected) + + +@pytest.mark.parametrize("candidate", VEHICLES) +def test_stronger_planner_brake_matches_the_stock_ramp(candidate): + CP, control = make_control(candidate) + outputs = [control.update(True, make_car_state(0.3, -0.3), -1.0, True, (-3.5, 2.0)) for _ in range(10)] + expected = [] + output = -0.33 + for _ in range(10): + output = stock_stopping_output(output, CP.stopAccel) + expected.append(output) + assert outputs == pytest.approx(expected) + + +@pytest.mark.parametrize("candidate", VEHICLES) +def test_insufficient_deceleration_uses_the_stock_ramp_immediately(candidate): + CP, control = make_control(candidate) + output = control.update(True, make_car_state(0.6, -0.1), -0.1, True, (-3.5, 2.0)) + assert output == pytest.approx(stock_stopping_output(-0.33, CP.stopAccel)) + + +def test_deceleration_noise_cannot_release_the_brake(): + _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2) + outputs = [control.update(True, make_car_state(0.3, -0.3 if frame % 2 else 0.0), -0.1, True, (-3.5, 2.0)) for frame in range(40)] + assert all(current <= previous for previous, current in zip(outputs[:-1], outputs[1:], strict=True)) + + +def test_planner_noise_cannot_release_the_brake(): + _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2) + outputs = [control.update(True, make_car_state(0.3, -0.3), -1.0 if frame % 2 else -0.1, True, (-3.5, 2.0)) for frame in range(40)] + assert all(current <= previous for previous, current in zip(outputs[:-1], outputs[1:], strict=True)) + + +@pytest.mark.parametrize(("v_ego", "a_ego", "a_target"), ((float("nan"), -0.3, -0.1), (0.3, float("nan"), -0.1), (0.3, -0.3, float("nan")))) +def test_invalid_state_uses_the_stock_ramp(v_ego, a_ego, a_target): + CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2) + output = control.update(True, make_car_state(v_ego, a_ego), a_target, True, (-3.5, 2.0)) + assert output == pytest.approx(stock_stopping_output(-0.33, CP.stopAccel)) + + +@pytest.mark.parametrize(("speed", "initial_accel", "grade_accel", "actuator_lag"), ( + (0.24, 0.0, 0.0, 0.15), (0.464, -0.223, 0.0, 0.25), (0.53, -0.31, 0.0, 0.35), + (0.24, 0.0, 0.49, 0.15), (0.53, -0.31, 0.49, 0.25), (0.6, -0.3, 0.49, 0.35), (0.6, -0.3, 0.49, 0.5), +)) +def test_smooth_stop_distance_is_bounded(speed, initial_accel, grade_accel, actuator_lag): + _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, initial_accel) + applied_accel = initial_accel + distance = 0.0 + + for _ in range(round(4.0 / DT_CTRL)): + command = control.update(True, make_car_state(speed, applied_accel), -0.1, True, (-3.5, 2.0)) + applied_accel += DT_CTRL / actuator_lag * (command + grade_accel - applied_accel) + speed = max(0.0, speed + applied_accel * DT_CTRL) + distance += speed * DT_CTRL + if speed == 0.0: + break + + assert speed == 0.0 + assert distance < 1.0 + + +@pytest.mark.parametrize("candidate", VEHICLES) +def test_standstill_uses_the_stock_ramp(candidate): + CP, control = make_control(candidate) + CS = make_car_state(0.0, 0.0, standstill=True) + outputs = [control.update(True, CS, 0.0, True, (-3.5, 2.0)) for _ in range(round(2.0 / DT_CTRL))] + expected = -0.33 + for _ in range(round(2.0 / DT_CTRL)): + expected = stock_stopping_output(expected, CP.stopAccel) + assert outputs[0] == pytest.approx(stock_stopping_output(-0.33, CP.stopAccel)) + assert outputs[-1] == pytest.approx(expected) + + +@pytest.mark.parametrize(("v_ego", "a_ego", "standstill"), ((0.6, -0.1, False), (0.0, 0.0, True))) +def test_stopping_never_releases_a_stronger_command(v_ego, a_ego, standstill): + _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -3.0) + output = control.update(True, make_car_state(v_ego, a_ego, standstill), 0.0, True, (-3.5, 2.0)) + assert output == pytest.approx(-3.0) + + +def test_reported_standstill_while_moving_can_hold_the_brake(): + _, control = make_control(GM.CHEVROLET_BOLT_EUV) + output = control.update(True, make_car_state(0.3, -0.3, standstill=True), -0.1, True, (-3.5, 2.0)) + assert output == pytest.approx(-0.33) + + +def test_stopping_removes_positive_acceleration_immediately(): + _, control = make_control(HYUNDAI.HYUNDAI_SONATA, 0.2) + output = control.update(True, make_car_state(0.2, -0.2), -0.1, True, (-3.5, 2.0)) + assert output == pytest.approx(-DT_CTRL) + + +def test_rollback_uses_the_stock_ramp(): + CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2) + output = control.update(True, make_car_state(-0.1, 0.1), -0.1, True, (-3.5, 2.0)) + assert output == pytest.approx(stock_stopping_output(-0.33, CP.stopAccel)) + + +def test_departure_uses_the_stock_pid_path(): + _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2) + control.long_control_state = LongCtrlState.stopping + output = control.update(True, make_car_state(0.0), 0.6, False, (-3.5, 2.0)) + assert control.long_control_state == LongCtrlState.pid + assert output > 0.0 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longcontrol.py b/openpilot/sunnypilot/selfdrive/controls/lib/longcontrol.py new file mode 100644 index 0000000000..43d91f3cd3 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/longcontrol.py @@ -0,0 +1,16 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" + +STOPPING_DISTANCE = 0.75 +STOPPED_SPEED = 0.02 +STOPPING_TIME = 2.5 + + +class LongControlSP: + def should_hold_stopping(self, CS, a_target: float) -> bool: + return (self.last_output_accel <= 0.0 and a_target >= self.last_output_accel and CS.vEgo > STOPPED_SPEED and CS.aEgo < 0.0 + and CS.vEgo <= -CS.aEgo * STOPPING_TIME and CS.vEgo ** 2 <= -2.0 * CS.aEgo * STOPPING_DISTANCE) diff --git a/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py index 9970c689c3..7786483603 100644 --- a/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py @@ -14,8 +14,7 @@ from typing import Any import numpy as np -from openpilot.cereal import log -import cereal.messaging as messaging +from openpilot.cereal import log, messaging from openpilot.common.realtime import DT_MDL, Ratekeeper from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState @@ -251,14 +250,10 @@ class PlantSP(Plant): "dRel": float(d_rel), "yRel": 0.0, "vRel": float(v_rel), - "aRel": float(a_lead - self.acceleration), "vLead": float(v_lead), - "dPath": 0.0, - "vLat": 0.0, "vLeadK": float(v_lead), "aLeadK": float(a_lead), - "fcw": False, - "status": bool(status), + "present": bool(status), # TODO use real radard logic for this "aLeadTau": float(_LEAD_ACCEL_TAU), "modelProb": float(prob_lead), diff --git a/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py index c002dda313..cbcaa11cbb 100644 --- a/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py +++ b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py @@ -1,5 +1,6 @@ from collections.abc import Callable import math +from typing import cast import pytest @@ -14,15 +15,19 @@ def departing_lead(current_time: float) -> float: return 0.0 if current_time < 1.0 else min(2.0, 2.0 * (current_time - 1.0)) +def stopped_lead(_current_time: float) -> float: + return 0.0 + + PARITY_SCENARIOS = { - "approach_stopped_lead": dict(lead_relevancy=True, speed=15.0, distance_lead=60.0, v_cruise=20.0, v_lead=0.0, steps=80), - "stop_then_depart": dict(lead_relevancy=True, speed=0.0, distance_lead=6.0, v_cruise=8.0, v_lead=departing_lead, steps=120), + "approach_stopped_lead": {"lead_relevancy": True, "speed": 15.0, "distance_lead": 60.0, "v_cruise": 20.0, "v_lead": stopped_lead, "steps": 80}, + "stop_then_depart": {"lead_relevancy": True, "speed": 0.0, "distance_lead": 6.0, "v_cruise": 8.0, "v_lead": departing_lead, "steps": 120}, } -def _drive(cls, *, v_cruise: float, v_lead: float | Callable[[float], float], steps: int, **kwargs): +def _drive(cls, *, v_cruise: float, v_lead: Callable[[float], float], steps: int, **kwargs): plant = cls(**kwargs) - plant.v_lead_prev = float(v_lead(0.0)) if callable(v_lead) else float(v_lead) + plant.v_lead_prev = v_lead(0.0) solver_failures = 0 original_reset = plant.planner.mpc.reset @@ -35,16 +40,18 @@ def _drive(cls, *, v_cruise: float, v_lead: float | Callable[[float], float], st plant.planner.mpc.reset = counting_reset results = [] for _ in range(steps): - lead_speed = float(v_lead(plant.current_time)) if callable(v_lead) else v_lead + lead_speed = v_lead(plant.current_time) result = plant.step(v_lead=lead_speed, v_cruise=v_cruise) results.append((result, plant.planner.mpc.source, plant.planner.output_a_target)) return results, solver_failures @pytest.mark.parametrize("scenario", PARITY_SCENARIOS, ids=list(PARITY_SCENARIOS)) -def test_plant_sp_matches_stock_plant_on_shared_kwargs(scenario): +def test_plant_sp_matches_stock_plant_on_shared_kwargs(scenario: str): kwargs = dict(PARITY_SCENARIOS[scenario]) - v_cruise, v_lead, steps = kwargs.pop("v_cruise"), kwargs.pop("v_lead"), kwargs.pop("steps") + v_cruise = cast(float, kwargs.pop("v_cruise")) + v_lead = cast(Callable[[float], float], kwargs.pop("v_lead")) + steps = cast(int, kwargs.pop("steps")) stock_results, stock_failures = _drive(Plant, v_cruise=v_cruise, v_lead=v_lead, steps=steps, **kwargs) sp_results, sp_failures = _drive(PlantSP, v_cruise=v_cruise, v_lead=v_lead, steps=steps, **kwargs) @@ -91,7 +98,7 @@ def test_full_lead_observation_is_independent_from_truth(): "vLeadK": 5.5, "aLeadK": -1.25, "aLeadTau": 0.7, - "status": True, + "present": True, "modelProb": 0.9, "radarTrackId": 42, }