From d8ca2127a3a399153bdf9cdb503aba1f1df58504 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Sun, 23 Aug 2026 14:39:08 -0700 Subject: [PATCH] Tune acceleration profiles and lead response --- .../selfdrive/controls/lib/drive_helpers.py | 5 +- .../selfdrive/controls/lib/longcontrol.py | 5 +- .../controls/lib/longitudinal_planner.py | 2 +- .../controls/tests/test_longcontrol.py | 32 +++- .../lib/accel_controller/accel_controller.py | 37 ++++- .../tests/test_accel_controller.py | 141 +++++++++++++--- .../test_accel_controller_closed_loop.py | 155 ++++++++++++++++-- .../controls/lib/lead_departure_controller.py | 3 +- .../controls/lib/longitudinal_planner.py | 23 ++- .../tests/test_lead_departure_controller.py | 51 +++++- .../tests/test_throttle_intent_arbitration.py | 43 ++++- 11 files changed, 441 insertions(+), 56 deletions(-) diff --git a/openpilot/selfdrive/controls/lib/drive_helpers.py b/openpilot/selfdrive/controls/lib/drive_helpers.py index 35ca45e56e..584d830f05 100644 --- a/openpilot/selfdrive/controls/lib/drive_helpers.py +++ b/openpilot/selfdrive/controls/lib/drive_helpers.py @@ -14,8 +14,11 @@ MAX_LATERAL_JERK = 5.0 # m/s^3 MAX_LATERAL_ACCEL_NO_ROLL = 3.0 # m/s^2 +STOPPING_SPEED = 0.25 # m/s + + def should_stop(v_ego: float, a_target: float) -> bool: - return bool(v_ego < 0.3 and a_target < 0.1) + return bool(v_ego < STOPPING_SPEED and a_target < 0.1) def clamp(val, min_val, max_val): clamped_val = float(np.clip(val, min_val, max_val)) diff --git a/openpilot/selfdrive/controls/lib/longcontrol.py b/openpilot/selfdrive/controls/lib/longcontrol.py index 3a20601c48..03149dd57b 100644 --- a/openpilot/selfdrive/controls/lib/longcontrol.py +++ b/openpilot/selfdrive/controls/lib/longcontrol.py @@ -7,6 +7,8 @@ from openpilot.selfdrive.modeld.constants import ModelConstants CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] +STOPPING_DECEL_RATE = 0.3 # m/s^2/s while trying to stop + LongCtrlState = car.CarControl.Actuators.LongControlState @@ -67,8 +69,7 @@ class LongControl: output_accel = self.last_output_accel if output_accel > self.CP.stopAccel: 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 + output_accel -= STOPPING_DECEL_RATE * DT_CTRL self.reset() else: # LongCtrlState.pid diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index 2f41c877f4..3fa1872845 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -141,7 +141,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP): output_a_target_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX, action_t=action_t) output_should_stop_mpc = should_stop(v_ego, output_a_target_mpc) - output_should_stop_mpc = self.update_lead_departure(sm, output_a_target_mpc, output_should_stop_mpc, reset_state) + output_a_target_mpc, output_should_stop_mpc = self.update_lead_departure(sm, output_a_target_mpc, output_should_stop_mpc, reset_state) output_a_target_e2e = sm['modelV2'].action.desiredAcceleration output_should_stop_e2e = sm['modelV2'].action.shouldStop diff --git a/openpilot/selfdrive/controls/tests/test_longcontrol.py b/openpilot/selfdrive/controls/tests/test_longcontrol.py index 9469545204..277076ec78 100644 --- a/openpilot/selfdrive/controls/tests/test_longcontrol.py +++ b/openpilot/selfdrive/controls/tests/test_longcontrol.py @@ -1,10 +1,40 @@ +from types import SimpleNamespace + from openpilot.common.test import OpenpilotTestCase +from openpilot.common.realtime import DT_CTRL from openpilot.cereal import custom -from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState, long_control_state_trans +from openpilot.selfdrive.controls.lib.drive_helpers import should_stop +from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState, long_control_state_trans + + +def make_stopping_control(last_output_accel): + CP = SimpleNamespace(stopAccel=-2.0, longitudinalTuning=SimpleNamespace(kiBP=[0.0], kiV=[0.0])) + control = LongControl(CP, custom.CarParamsSP.new_message()) + control.long_control_state = LongCtrlState.stopping + control.last_output_accel = last_output_accel + CS = SimpleNamespace(aEgo=0.0, brakePressed=False, cruiseState=SimpleNamespace(standstill=False), vEgo=0.0) + return control, CS class TestLongControlStateTransition(OpenpilotTestCase): + def test_stopping_threshold_boundaries(self): + assert should_stop(0.249, 0.099) + assert not should_stop(0.250, 0.099) + assert not should_stop(0.249, 0.1) + + def test_stopping_ramp_rate(self): + control, CS = make_stopping_control(-0.2) + output = control.update(True, CS, a_target=-0.2, should_stop=True, accel_limits=(-3.5, 2.0)) + + self.assertAlmostEqual(output, -0.2 - 0.3 * DT_CTRL) + + def test_stopping_does_not_release_stronger_braking(self): + control, CS = make_stopping_control(-2.2) + output = control.update(True, CS, a_target=-0.2, should_stop=True, accel_limits=(-3.5, 2.0)) + + self.assertEqual(output, -2.2) + def test_stay_stopped(self): CP_SP = custom.CarParamsSP.new_message() active = True diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py index d8160db9ee..4411985d90 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py @@ -10,15 +10,16 @@ import numpy as np from openpilot.cereal import custom from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import MAX_T, STOP_DISTANCE from openpilot.sunnypilot import get_sanitize_int_param AccelProfile = custom.LongitudinalPlanSP.AccelController.Profile MAX_ACCEL_BREAKPOINTS = [0., 3., 5., 8., 10., 25., 40.] MAX_ACCEL_PROFILES = { - AccelProfile.eco: [1.50, 1.42, 0.90, 0.52, 0.44, 0.30, 0.23], - AccelProfile.normal: [1.60, 1.48, 1.00, 0.60, 0.50, 0.36, 0.28], - AccelProfile.sport: [1.80, 1.70, 1.12, 0.70, 0.60, 0.44, 0.35], + AccelProfile.eco: [1.20, 0.95, 0.70, 0.48, 0.40, 0.28, 0.18], + AccelProfile.normal: [1.60, 1.35, 1.05, 0.65, 0.52, 0.40, 0.28], + AccelProfile.sport: [2.00, 1.90, 1.55, 1.00, 0.80, 0.60, 0.40], } TARGET_SPEED_DEADBAND = 0.2 # m/s TARGET_SPEED_APPROACH_WINDOW = 2.0 # m/s @@ -27,6 +28,8 @@ TARGET_SPEED_APPROACH_MIN_SPEED = 3.0 # m/s TARGET_SPEED_APPROACH_FULL_SPEED = 5.0 # m/s CATCHUP_ERROR_BREAKPOINTS = [TARGET_SPEED_DEADBAND, 0.5, 1.0, 2.0, 3.0, 4.0] CATCHUP_ACCEL_SCALE = [0.0, 0.3, 0.5, 0.7, 0.85, 1.0] +LEAD_COMFORT_ACCEL = -0.4 # m/s^2 +LEAD_COMFORT_JERK = 0.6 # m/s^3 class AccelController: @@ -76,3 +79,31 @@ class AccelController: adjusted_error = np.sign(speed_error) * max(0.0, abs(speed_error) - deadband) gain = 1.0 - (1.0 - TARGET_SPEED_APPROACH_GAIN) * speed_blend * target_blend return float(v_ego + gain * adjusted_error) + + def get_lead_departure_accel(self, v_ego: float, v_lead: float, a_lead: float, mpc_accel: float) -> float: + values = (v_ego, v_lead, a_lead, mpc_accel) + if not self._enabled or not all(np.isfinite(value) for value in values) or a_lead < 0.0 or mpc_accel < 0.0 or v_lead <= v_ego: + return mpc_accel + + return max(mpc_accel, min(v_lead - v_ego, self.get_max_accel(v_ego, v_lead))) + + def get_lead_accel(self, v_ego: float, d_rel: float, v_rel: float, a_lead: float, + cruise_accel: float, previous_accel: float, t_follow: float) -> float: + values = (v_ego, d_rel, v_rel, a_lead, cruise_accel, previous_accel, t_follow) + if not self._enabled or not all(np.isfinite(value) for value in values) or v_ego < TARGET_SPEED_APPROACH_FULL_SPEED or d_rel <= 0.0: + return cruise_accel + + v_lead = max(v_ego + v_rel, 0.0) + clearance = d_rel - (STOP_DISTANCE + t_follow * v_lead) + lead_lookahead = 2.0 * t_follow + projected_closing = max(-(v_rel + a_lead * lead_lookahead), 0.0) + if projected_closing <= 0.0 or clearance / projected_closing >= MAX_T: + return cruise_accel + + required_accel = a_lead - projected_closing ** 2 / (2.0 * max(clearance, STOP_DISTANCE)) + target_accel = float(np.clip(required_accel, LEAD_COMFORT_ACCEL, 0.0)) + + max_step = LEAD_COMFORT_JERK * DT_MDL + previous_comfort_accel = max(previous_accel, LEAD_COMFORT_ACCEL) + lead_accel = float(np.clip(target_accel, previous_comfort_accel - max_step, previous_comfort_accel + max_step)) + return min(cruise_accel, lead_accel) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py index 6a0f34b247..8739d46a0f 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py @@ -7,14 +7,17 @@ See the LICENSE.md file in the root directory for more details. import numpy as np +from opendbc.car.interfaces import ACCEL_MAX from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.common.test import OpenpilotTestCase from openpilot.selfdrive.controls.lib.longitudinal_planner import ( A_CRUISE_MAX_BP, A_CRUISE_MIN, J_CRUISE_VALS, get_cruise_accel, get_max_accel, ) +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import MAX_T, STOP_DISTANCE from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import ( AccelController, AccelProfile, CATCHUP_ACCEL_SCALE, CATCHUP_ERROR_BREAKPOINTS, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES, + LEAD_COMFORT_ACCEL, LEAD_COMFORT_JERK, TARGET_SPEED_APPROACH_FULL_SPEED, TARGET_SPEED_APPROACH_GAIN, TARGET_SPEED_APPROACH_MIN_SPEED, TARGET_SPEED_APPROACH_WINDOW, TARGET_SPEED_DEADBAND, ) @@ -51,38 +54,41 @@ class TestAccelController(OpenpilotTestCase): assert value <= previous[profile] previous[profile] = value - def test_normal_stays_below_stock(self): - controller = self.set_profile(AccelProfile.normal) - for speed in np.linspace(0.0, 55.0, 551): - assert controller.get_max_accel(speed) <= get_max_accel(speed) + 1e-12 + def test_profiles_stay_within_openpilot_accel_max(self): + for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport): + controller = self.set_profile(profile) + for speed in np.linspace(0.0, 55.0, 551): + assert controller.get_max_accel(speed) <= ACCEL_MAX - def test_profiles_taper_below_stock_at_road_speed(self): + def test_profiles_do_not_exceed_stock_at_road_speed(self): for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport): controller = self.set_profile(profile) for speed in np.linspace(8.0, 40.0, 321): - assert controller.get_max_accel(speed) < get_max_accel(speed) + assert controller.get_max_accel(speed) <= get_max_accel(speed) + 1e-12 - def test_launch_caps_stay_close_to_stock(self): - for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport): - controller = self.set_profile(profile) - for speed in np.linspace(0.0, 3.0, 61): - assert controller.get_max_accel(speed) >= 0.9 * get_max_accel(speed) + def test_profiles_have_material_separation(self): + controllers = [self.set_profile(profile) for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)] + for speed in MAX_ACCEL_BREAKPOINTS: + eco, normal, sport = (controller.get_max_accel(speed) for controller in controllers) + assert normal - eco >= 0.1 + assert sport - normal >= 0.1 + for speed in MAX_ACCEL_BREAKPOINTS[1:-1]: + assert controllers[2].get_max_accel(speed) - controllers[0].get_max_accel(speed) >= 0.3 - 1e-12 def test_eco_keeps_useful_road_speed_acceleration(self): controller = self.set_profile(AccelProfile.eco) for speed in np.linspace(8.0, 40.0, 321): - assert controller.get_max_accel(speed) >= 0.35 * get_max_accel(speed) - 1e-12 + assert controller.get_max_accel(speed) >= 0.25 * get_max_accel(speed) - 1e-12 - def test_profile_caps_drop_quickly_after_launch(self): - for values in MAX_ACCEL_PROFILES.values(): - assert values[2] <= 0.7 * values[1] - assert values[3] <= 0.5 * values[1] + def test_comfort_profile_caps_taper_after_launch(self): + for profile in (AccelProfile.eco, AccelProfile.normal): + values = MAX_ACCEL_PROFILES[profile] + assert values[3] <= 0.55 * values[0] - def test_sport_stays_below_reported_route_acceleration(self): + def test_sport_uses_openpilot_accel_max_at_launch(self): controller = self.set_profile(AccelProfile.sport) - route_samples = ((8.1, 0.877), (12.0, 0.858), (15.5, 0.802), (18.8, 0.750), (21.7, 0.654)) - for speed, recorded_accel in route_samples: - assert controller.get_max_accel(speed) <= 0.85 * recorded_accel + assert controller.get_max_accel(0.0) == ACCEL_MAX + assert all(controller.get_max_accel(speed) <= ACCEL_MAX for speed in np.linspace(0.0, 55.0, 551)) def test_positive_catchup_limit_is_continuous_and_monotonic(self): controller = self.set_profile(AccelProfile.normal) @@ -125,6 +131,21 @@ class TestAccelController(OpenpilotTestCase): assert np.all(np.diff(shaped_errors) >= 0.0) assert np.all(np.abs(shaped_errors) <= np.abs(errors) + 1e-12) + def test_lead_departure_accel_is_bounded_by_lead_speed_and_profile(self): + controller = self.set_profile(AccelProfile.eco) + assert controller.get_lead_departure_accel(0.0, 0.7, 0.1, 0.1) == 0.7 + assert controller.get_lead_departure_accel(0.0, 2.0, 0.1, 0.1) == MAX_ACCEL_PROFILES[AccelProfile.eco][0] + + def test_lead_departure_accel_preserves_non_acceleration_cases(self): + controller = self.set_profile(AccelProfile.normal) + assert controller.get_lead_departure_accel(0.0, 1.0, 0.1, -0.1) == -0.1 + assert controller.get_lead_departure_accel(0.0, 1.0, -0.1, 0.1) == 0.1 + assert controller.get_lead_departure_accel(1.0, 1.0, 0.1, 0.1) == 0.1 + assert controller.get_lead_departure_accel(0.0, float("nan"), 0.1, 0.1) == 0.1 + + self.params.put_bool("AccelPersonalityEnabled", False, block=True) + assert AccelController().get_lead_departure_accel(0.0, 1.0, 0.1, 0.1) == 0.1 + def test_catchup_limit_does_not_touch_launch(self): controller = self.set_profile(AccelProfile.sport) @@ -193,6 +214,86 @@ class TestAccelController(OpenpilotTestCase): controller.update() assert not controller.is_enabled() + def test_closing_lead_prevents_acceleration_rebound(self): + controller = self.set_profile(AccelProfile.normal) + previous_accel = -0.647 + samples = ( + (17.33, 120.82, -16.35, -1.70, 0.50), + (17.30, 88.70, -14.95, -0.76, -0.332), + (17.18, 103.98, -17.15, -0.24, -0.992), + ) + outputs = [] + for v_ego, d_rel, v_rel, a_lead, mpc_accel in samples: + lead_accel = controller.get_lead_accel(v_ego, d_rel, v_rel, a_lead, 0.31, previous_accel, 1.45) + previous_accel = min(lead_accel, mpc_accel) + outputs.append(previous_accel) + + assert np.allclose(outputs, [LEAD_COMFORT_ACCEL, LEAD_COMFORT_ACCEL, -0.992]) + assert all(accel <= 0.0 for accel in outputs) + + def test_lead_comfort_does_not_replay_hard_braking(self): + controller = self.set_profile(AccelProfile.normal) + output = controller.get_lead_accel(20.0, 60.0, -8.0, -1.0, 0.5, -1.77, 1.45) + assert output == LEAD_COMFORT_ACCEL + + def test_lead_approach_uses_existing_mpc_horizon(self): + controller = self.set_profile(AccelProfile.normal) + for t_follow in (1.25, 1.45, 1.75): + v_ego, v_rel, closing = 20.0, -2.0, 2.0 + boundary = STOP_DISTANCE + t_follow * (v_ego + v_rel) + closing * MAX_T + assert controller.get_lead_accel(v_ego, boundary + 1e-3, v_rel, 0.0, 0.5, 0.2, t_follow) == 0.5 + assert controller.get_lead_accel(v_ego, boundary - 1e-3, v_rel, 0.0, 0.5, 0.2, t_follow) < 0.2 + + def test_braking_lead_gently_removes_positive_acceleration(self): + controller = self.set_profile(AccelProfile.normal) + previous_accel = 0.544 + outputs = [] + for _ in range(40): + output = controller.get_lead_accel(30.56, 47.62, 5.5, -1.93, 0.544, previous_accel, 1.45) + outputs.append(output) + previous_accel = output + + max_step = LEAD_COMFORT_JERK * DT_MDL + assert np.isclose(outputs[0], 0.544 - max_step) + assert np.all(np.diff(outputs) >= -max_step - 1e-12) + assert np.isclose(outputs[-1], LEAD_COMFORT_ACCEL) + + def test_transient_lead_deceleration_is_one_small_step(self): + controller = self.set_profile(AccelProfile.normal) + first = controller.get_lead_accel(31.1, 51.3, 2.8, -2.5, 0.54, 0.4, 1.45) + second = controller.get_lead_accel(31.1, 52.0, 3.0, 0.0, 0.54, first, 1.45) + + assert np.isclose(first, 0.4 - LEAD_COMFORT_JERK * DT_MDL) + assert second == 0.54 + + def test_non_closing_and_distant_leads_do_not_change_cruise(self): + controller = self.set_profile(AccelProfile.normal) + assert controller.get_lead_accel(30.0, 52.0, 0.0, 0.0, 0.1, 0.1, 1.45) == 0.1 + assert controller.get_lead_accel(20.0, 200.0, -1.0, 0.0, 0.1, 0.1, 1.45) == 0.1 + + def test_accelerating_lead_returns_cruise(self): + controller = self.set_profile(AccelProfile.normal) + output = controller.get_lead_accel(20.0, 45.0, -1.0, 1.0, 0.3, -0.2, 1.45) + assert output == 0.3 + + def test_active_approach_cannot_rebound_from_brake_to_gas(self): + controller = self.set_profile(AccelProfile.normal) + output = controller.get_lead_accel(20.0, 30.0, -5.0, 1.0, 0.3, -0.2, 1.45) + assert -0.2 < output <= 0.0 + + def test_stock_lead_braking_always_wins(self): + controller = self.set_profile(AccelProfile.normal) + assert controller.get_lead_accel(20.0, 60.0, -8.0, -1.0, -2.0, 0.2, 1.45) == -2.0 + + def test_invalid_disabled_and_launch_cases_match_cruise(self): + controller = self.set_profile(AccelProfile.normal) + assert controller.get_lead_accel(20.0, float("nan"), -8.0, -1.0, -0.5, 0.2, 1.45) == -0.5 + assert controller.get_lead_accel(TARGET_SPEED_APPROACH_FULL_SPEED - 0.01, 20.0, -5.0, -1.0, 0.5, 0.0, 1.45) == 0.5 + + self.params.put_bool("AccelPersonalityEnabled", False, block=True) + controller = AccelController() + assert controller.get_lead_accel(20.0, 60.0, -8.0, -1.0, -0.5, 0.2, 1.45) == -0.5 + class TestPlannerIntegration(OpenpilotTestCase): def setUp(self): diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_closed_loop.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_closed_loop.py index 671445963b..22e2744a4d 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_closed_loop.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_closed_loop.py @@ -6,9 +6,11 @@ See the LICENSE.md file in the root directory for more details. """ from collections.abc import Callable +from unittest import mock import numpy as np +from openpilot.common.constants import CV from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.common.test import OpenpilotTestCase @@ -18,7 +20,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import Longi from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import ( AccelController, AccelProfile, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES, TARGET_SPEED_DEADBAND, ) -from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP +from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, PlantSP class CarParams: @@ -57,7 +59,142 @@ def run_profile(profile: int, *, enabled: bool = True, speed: float = 0.0, v_cru return rows +def run_vehicle_profile(profile: int, duration: float = 80.0): + params = Params() + params.put_bool("AccelPersonalityEnabled", True, block=True) + params.put("AccelPersonality", profile, block=True) + + plant = PlantSP(speed=0.0, actuator_model=PRIUS_TSS2_ROUTE_MODEL, run_long_control=True) + _set_mpc_acceleration(plant) + rows = [] + while plant.current_time < duration: + result = plant.step(v_cruise=25.0) + rows.append((plant.current_time, result["speed"], result["a_target"], result["actuator_command"], result["acceleration"])) + return np.asarray(rows) + + +def run_lead_comfort(*, comfort_enabled: bool, speed: float, distance_lead: float, + lead_speed_fn: Callable[[float], float], duration: float = 12.0): + params = Params() + params.put_bool("DynamicExperimentalControl", False, block=True) + params.put_bool("AccelPersonalityEnabled", True, block=True) + params.put("AccelPersonality", AccelProfile.eco, block=True) + + plant = PlantSP(lead_relevancy=True, speed=speed, distance_lead=distance_lead, only_radar=True) + plant.v_lead_prev = lead_speed_fn(0.0) + planner = plant.planner + raw_mpc, raw_cruise, adjusted_cruise, rows = [], [], [], [] + mpc_failures = 0 + original_arbitrate = planner.arbitrate_cruise_candidate + original_lead_accel = planner.accel_controller.get_lead_accel + original_mpc_reset = planner.mpc.reset + + def record_mpc_reset(*args, **kwargs): + nonlocal mpc_failures + mpc_failures += int(planner.mpc.solution_status != 0) + return original_mpc_reset(*args, **kwargs) + + def record_arbitration(sm, gated, ungated, mpc_accel, mpc_source, **kwargs): + raw_mpc.append(mpc_accel) + return original_arbitrate(sm, gated, ungated, mpc_accel, mpc_source, **kwargs) + + def record_lead_accel(v_ego, d_rel, v_rel, a_lead, a_target, previous_accel, t_follow): + raw_cruise.append(a_target) + adjusted = original_lead_accel(v_ego, d_rel, v_rel, a_lead, a_target, previous_accel, t_follow) if comfort_enabled else a_target + adjusted_cruise.append(adjusted) + return adjusted + + with ( + mock.patch.object(planner.mpc, "reset", side_effect=record_mpc_reset), + mock.patch.object(planner, "arbitrate_cruise_candidate", side_effect=record_arbitration), + mock.patch.object(planner.accel_controller, "get_lead_accel", side_effect=record_lead_accel), + ): + while plant.current_time < duration: + output = plant.step(v_lead=lead_speed_fn(plant.current_time), v_cruise=30.0) + rows.append(( + output["distance_lead"] - output["distance"], output["actuator_command"], output["a_target"], output["fcw"], planner.mpc.solution_status, + )) + + data = np.asarray(rows, dtype=float) + commands = data[:, 1] + braking_frames = np.flatnonzero(commands < -0.05) + return { + "min_gap": float(np.min(data[:, 0])), + "min_command": float(np.min(commands)), + "min_jerk": float(np.min(np.diff(commands) / DT_MDL)), + "first_brake_frame": int(braking_frames[0]) if len(braking_frames) else len(commands), + "a_targets": data[:, 2], + "fcw": bool(np.any(data[:, 3])), + "mpc_failures": mpc_failures + int(data[-1, 4] != 0), + "raw_mpc": np.asarray(raw_mpc), + "raw_cruise": np.asarray(raw_cruise), + "adjusted_cruise": np.asarray(adjusted_cruise), + } + + class TestAccelControllerClosedLoop(OpenpilotTestCase): + def test_profiles_are_immediate_smooth_and_clearly_distinct(self): + traces = {profile: run_vehicle_profile(profile) for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)} + + def crossing(trace, speed): + return float(trace[np.flatnonzero(trace[:, 1] >= speed)[0], 0]) + + time_to_20 = {profile: crossing(trace, 20.0 * CV.MPH_TO_MS) for profile, trace in traces.items()} + time_to_50 = {profile: crossing(trace, 50.0 * CV.MPH_TO_MS) for profile, trace in traces.items()} + first_motion = {profile: int(np.flatnonzero(trace[:, 1] > 0.01)[0]) for profile, trace in traces.items()} + + self.assertEqual(len(set(first_motion.values())), 1) + self.assertTrue(all(trace[0, 2] > 0.0 and trace[1, 3] > 0.0 for trace in traces.values())) + self.assertGreaterEqual(time_to_20[AccelProfile.eco] - time_to_20[AccelProfile.normal], 2.0) + self.assertGreaterEqual(time_to_20[AccelProfile.normal] - time_to_20[AccelProfile.sport], 2.0) + self.assertGreaterEqual(time_to_50[AccelProfile.eco] - time_to_50[AccelProfile.normal], 10.0) + self.assertGreaterEqual(time_to_50[AccelProfile.normal] - time_to_50[AccelProfile.sport], 10.0) + + for trace in traces.values(): + command_jerk = np.abs(np.diff(trace[:, 3])) / DT_MDL + self.assertLessEqual(float(np.max(command_jerk)), PRIUS_TSS2_ROUTE_MODEL.command_rate_limit + 1e-9) + + settled = np.flatnonzero(trace[:, 1] >= 25.0 - TARGET_SPEED_DEADBAND - 0.1) + self.assertGreater(len(settled), 0) + settled_trace = trace[settled[0]:] + self.assertGreaterEqual(float(np.min(settled_trace[:, 3])), -0.05) + self.assertGreaterEqual(float(np.min(np.diff(settled_trace[:, 1]))), -1e-8) + self.assertLessEqual(float(np.max(trace[:, 1])), 25.0 + 1e-9) + self.assertLessEqual(25.0 - float(trace[-1, 1]), TARGET_SPEED_DEADBAND + 0.02) + + def test_lead_comfort_brakes_earlier_without_stronger_peak_for_slowing_lead(self): + def slowing_lead(t: float) -> float: + return float(np.clip(20.0 - 2.5 * max(t - 2.0, 0.0), 15.0, 20.0)) + + baseline = run_lead_comfort(comfort_enabled=False, speed=20.0, distance_lead=70.0, lead_speed_fn=slowing_lead) + comfort = run_lead_comfort(comfort_enabled=True, speed=20.0, distance_lead=70.0, lead_speed_fn=slowing_lead) + + self.assertFalse(comfort["fcw"]) + self.assertLessEqual(comfort["mpc_failures"], baseline["mpc_failures"]) + self.assertGreater(comfort["min_gap"], 0.0) + self.assertGreaterEqual(comfort["min_gap"], baseline["min_gap"] - 0.1) + self.assertGreaterEqual(comfort["min_command"], baseline["min_command"] - 0.01) + self.assertGreaterEqual(comfort["min_jerk"], baseline["min_jerk"] - 0.05) + self.assertLessEqual(comfort["first_brake_frame"], baseline["first_brake_frame"]) + self.assertTrue(np.all(comfort["a_targets"] <= comfort["raw_mpc"] + 1e-9)) + self.assertTrue(np.all(comfort["adjusted_cruise"] <= comfort["raw_cruise"] + 1e-9)) + self.assertTrue(np.any(comfort["adjusted_cruise"] < comfort["raw_cruise"] - 1e-3)) + + def test_lead_comfort_preserves_stopped_lead_safety(self): + def stopped_lead(_t: float) -> float: + return 0.0 + + baseline = run_lead_comfort(comfort_enabled=False, speed=20.0, distance_lead=90.0, lead_speed_fn=stopped_lead) + comfort = run_lead_comfort(comfort_enabled=True, speed=20.0, distance_lead=90.0, lead_speed_fn=stopped_lead) + + self.assertFalse(comfort["fcw"]) + self.assertLessEqual(comfort["mpc_failures"], baseline["mpc_failures"]) + self.assertGreater(comfort["min_gap"], 0.0) + self.assertGreaterEqual(comfort["min_gap"], baseline["min_gap"] - 0.1) + self.assertGreaterEqual(comfort["min_command"], baseline["min_command"] - 0.01) + self.assertTrue(np.all(comfort["a_targets"] <= comfort["raw_mpc"] + 1e-9)) + self.assertTrue(np.all(comfort["adjusted_cruise"] <= comfort["raw_cruise"] + 1e-9)) + def test_blended_positive_model_request_uses_profile_cruise_cap(self): params = Params() params.put_bool("DynamicExperimentalControl", False, block=True) @@ -198,14 +335,10 @@ class TestAccelControllerClosedLoop(OpenpilotTestCase): self.assertEqual(len(set(first_motion.values())), 1) self.assertTrue(all(frame == stock_first_motion for frame in first_motion.values())) - for rows in traces.values(): - time_to_two = next(frame for frame, row in enumerate(rows) if row[0] >= 2.0) * DT_MDL - time_to_three = next(frame for frame, row in enumerate(rows) if row[0] >= 3.0) * DT_MDL - self.assertLessEqual(time_to_two, stock_time_to_two + DT_MDL) - self.assertLessEqual(time_to_three, stock_time_to_three + 0.1) - self.assertLessEqual(time_to_five[AccelProfile.sport], time_to_five[AccelProfile.normal]) - self.assertLessEqual(time_to_five[AccelProfile.normal], time_to_five[AccelProfile.eco]) - self.assertLessEqual(time_to_five[AccelProfile.eco], 1.25 * time_to_five[AccelProfile.normal]) + self.assertLess(stock_time_to_two, next(frame for frame, row in enumerate(traces[AccelProfile.eco]) if row[0] >= 2.0) * DT_MDL) + self.assertLess(stock_time_to_three, next(frame for frame, row in enumerate(traces[AccelProfile.eco]) if row[0] >= 3.0) * DT_MDL) + self.assertGreaterEqual(time_to_five[AccelProfile.eco] - time_to_five[AccelProfile.normal], 0.5) + self.assertGreaterEqual(time_to_five[AccelProfile.normal] - time_to_five[AccelProfile.sport], 0.5) def test_road_speed_catchup_stays_useful(self): traces = { @@ -213,8 +346,8 @@ class TestAccelControllerClosedLoop(OpenpilotTestCase): for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport) } gains = {profile: rows[-1][0] - 20.0 for profile, rows in traces.items()} - self.assertGreaterEqual(gains[AccelProfile.sport], gains[AccelProfile.normal]) - self.assertGreaterEqual(gains[AccelProfile.eco], 0.72 * gains[AccelProfile.normal]) + self.assertGreaterEqual(gains[AccelProfile.normal], 1.25 * gains[AccelProfile.eco]) + self.assertGreaterEqual(gains[AccelProfile.sport], 1.25 * gains[AccelProfile.normal]) def test_catchup_settles_inside_deadband_without_oscillation(self): target_speed = 24.0 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/lead_departure_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/lead_departure_controller.py index 3adeef15a5..504a4191c8 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/lead_departure_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/lead_departure_controller.py @@ -78,10 +78,11 @@ class LeadDepartureController: and lead.present and lead.radar and lead.radarTrackId >= 0 - and all(math.isfinite(value) for value in (lead.dRel, lead.vLeadK, lead.vRel)) + and all(math.isfinite(value) for value in (lead.dRel, lead.vLeadK, lead.vRel, lead.aLeadK)) and lead.dRel > 0.0 and lead.vLeadK >= LEAD_DEPARTURE_MIN_SPEED and lead.vRel >= LEAD_DEPARTURE_MIN_SPEED + and lead.aLeadK >= 0.0 and a_target >= 0.0 ) if not lead_valid: diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 8c44a459d7..e69a638c66 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -11,6 +11,7 @@ from openpilot.cereal import messaging, custom, log from opendbc.car import structs from openpilot.common.constants import CV from openpilot.selfdrive.car.cruise import V_CRUISE_MAX +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_T_FOLLOW from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper @@ -77,15 +78,29 @@ class LongitudinalPlannerSP: e2e: bool, force_decel: bool) -> float: finite = all(math.isfinite(value) for value in (gated, ungated, mpc_accel)) coast_gate_changed_source = gated < mpc_accel <= ungated + cruise_accel = gated if (finite and not allow_throttle and not e2e and not force_decel and self._has_valid_selected_lead(sm, mpc_source) and coast_gate_changed_source): - return ungated + cruise_accel = ungated - return gated + lead_valid = (self.accel_controller.is_enabled() and self.source == LongitudinalPlanSource.cruise + and self._has_valid_selected_lead(sm, mpc_source)) + if not lead_valid or force_decel: + return cruise_accel - def update_lead_departure(self, sm: messaging.SubMaster, a_target: float, should_stop: bool, reset: bool) -> bool: + lead = sm['radarState'].leadOne if mpc_source == MpcPlanSource.lead0 else sm['radarState'].leadTwo + return self.accel_controller.get_lead_accel( + sm['carState'].vEgo, lead.dRel, lead.vRel, lead.aLeadK, cruise_accel, self.output_a_target, + get_T_FOLLOW(sm['selfdriveState'].personality), + ) + + def update_lead_departure(self, sm: messaging.SubMaster, a_target: float, should_stop: bool, reset: bool) -> tuple[float, bool]: radar_valid = sm.valid.get('radarState', False) and getattr(sm, 'alive', {}).get('radarState', False) - return self.lead_departure_controller.update(sm, self.mpc.source, a_target, should_stop, reset, radar_valid) + should_stop = self.lead_departure_controller.update(sm, self.mpc.source, a_target, should_stop, reset, radar_valid) + if self.lead_departure_controller.active: + lead = sm['radarState'].leadOne if self.mpc.source == MpcPlanSource.lead0 else sm['radarState'].leadTwo + a_target = self.accel_controller.get_lead_departure_accel(sm['carState'].vEgo, lead.vLeadK, lead.aLeadK, a_target) + return a_target, should_stop def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]: CS = sm['carState'] diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_lead_departure_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_lead_departure_controller.py index a2583c0d3c..88336b8770 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_lead_departure_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_lead_departure_controller.py @@ -9,9 +9,11 @@ from types import SimpleNamespace from unittest import mock from openpilot.cereal import log +from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.common.test import OpenpilotTestCase from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelProfile from openpilot.sunnypilot.selfdrive.controls.lib.lead_departure_controller import LEAD_DEPARTURE_MIN_SPEED, LeadDepartureController from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, PlantSP @@ -19,8 +21,9 @@ from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRI MpcPlanSource = log.LongitudinalPlan.LongitudinalPlanSource -def make_lead(*, d_rel: float = 4.0, v_lead: float = 0.7, v_rel: float = 0.7, present: bool = True, radar: bool = True, track_id: int = 7): - return SimpleNamespace(dRel=d_rel, vLeadK=v_lead, vRel=v_rel, present=present, radar=radar, radarTrackId=track_id) +def make_lead(*, d_rel: float = 4.0, v_lead: float = 0.7, v_rel: float = 0.7, a_lead: float = 0.0, + present: bool = True, radar: bool = True, track_id: int = 7): + return SimpleNamespace(dRel=d_rel, vLeadK=v_lead, vRel=v_rel, aLeadK=a_lead, present=present, radar=radar, radarTrackId=track_id) def make_sm( @@ -58,7 +61,12 @@ def activate(controller: LeadDepartureController): assert controller.active -def run_closed_loop(controller_enabled: bool, gap: float, lead_speed, duration: float, model_should_stop: bool | None = None): +def run_closed_loop(controller_enabled: bool, gap: float, lead_speed, duration: float, model_should_stop: bool | None = None, + profile_enabled: bool = False): + params = Params() + params.put_bool("AccelPersonalityEnabled", profile_enabled, block=True) + params.put("AccelPersonality", AccelProfile.eco, block=True) + def observe_lead(_t, _name, truth): truth.update(radar=True, radarTrackId=7) return truth @@ -76,6 +84,7 @@ def run_closed_loop(controller_enabled: bool, gap: float, lead_speed, duration: e2e=model_should_stop is not None, model_action_fn=model_action if model_should_stop is not None else None, ) + plant.v_lead_prev = lead_speed(0.0) plant.planner.lead_departure_controller.enabled = controller_enabled original_update = plant.planner.update @@ -104,7 +113,8 @@ def run_closed_loop(controller_enabled: bool, gap: float, lead_speed, duration: t = plant.current_time result = plant.step(v_lead=lead_speed(t), v_cruise=8.0) rows.append( - (t, result['speed'], result['distance'], result['distance_lead'] - result['distance'], result['actuator_command'], result['should_stop'], result['fcw']) + (t, result['speed'], result['distance'], result['distance_lead'] - result['distance'], result['actuator_command'], result['should_stop'], result['fcw'], + result['a_target'], plant.applied_actuator_command) ) active.append(plant.planner.lead_departure_controller.active) @@ -181,6 +191,7 @@ class TestLeadDepartureController(OpenpilotTestCase): ('track changed', make_sm(lead_one=make_lead(track_id=9))), ('lead too slow', make_sm(lead_one=make_lead(v_lead=LEAD_DEPARTURE_MIN_SPEED - 0.01))), ('relative speed too low', make_sm(lead_one=make_lead(v_rel=LEAD_DEPARTURE_MIN_SPEED - 0.01))), + ('lead braking', make_sm(lead_one=make_lead(a_lead=-0.01))), ('gas', make_sm(gas=True)), ('brake', make_sm(brake=True)), ('override', make_sm(override=True)), @@ -255,7 +266,7 @@ class TestLeadDepartureController(OpenpilotTestCase): def lead_speed(t): return 0.0 if t < 1.0 else min(5.0, 0.8 * (t - 1.0)) - rows, active, solver_resets = run_closed_loop(True, 4.0, lead_speed, 6.0, model_should_stop=True) + rows, active, solver_resets = run_closed_loop(True, 4.0, lead_speed, 6.0, model_should_stop=True, profile_enabled=True) assert any(active) assert solver_resets == 0 @@ -270,3 +281,33 @@ class TestLeadDepartureController(OpenpilotTestCase): assert stock == controller assert not any(stock_active) and not any(controller_active) assert stock_resets == controller_resets == 0 + + def test_profile_breakaway_assist_moves_with_a_creeping_lead(self): + def lead_speed(t): + return min(1.1, 0.66 + 0.4 * t) + + baseline, baseline_active, baseline_resets = run_closed_loop(True, 2.8, lead_speed, 8.0) + assisted, assisted_active, assisted_resets = run_closed_loop(True, 2.8, lead_speed, 8.0, profile_enabled=True) + + baseline_motion = next((row[0] for row in baseline if row[1] > 0.01), float("inf")) + assisted_motion = next(row[0] for row in assisted if row[1] > 0.01) + assert any(baseline_active) and any(assisted_active) + assert baseline_resets == assisted_resets == 0 + assert assisted_motion <= 1.0 + assert baseline_motion - assisted_motion >= 1.0 + assert min(row[3] for row in assisted) >= min(row[3] for row in baseline) - 1e-9 + assert not any(row[6] for row in baseline + assisted) + assert max(abs(right[8] - left[8]) for left, right in zip(assisted, assisted[1:], strict=False)) <= 4.0 * DT_MDL + 1e-9 + + def test_departure_assist_yields_when_the_lead_brakes(self): + def lead_speed(t): + return 0.8 if t < 0.8 else max(0.0, 0.8 - 4.0 * (t - 0.8)) + + rows, active, solver_resets = run_closed_loop(True, 2.8, lead_speed, 5.0, profile_enabled=True) + + assert any(active) + assert solver_resets == 0 + assert min(row[3] for row in rows) > 2.0 + assert not any(row[6] for row in rows) + assert rows[-1][1] == 0.0 + assert min(row[7] for row in rows if row[0] >= 0.8) < 0.0 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_throttle_intent_arbitration.py b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_throttle_intent_arbitration.py index ade5f4eaea..479c56dc17 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_throttle_intent_arbitration.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_throttle_intent_arbitration.py @@ -8,34 +8,47 @@ See the LICENSE.md file in the root directory for more details. from types import SimpleNamespace from typing import cast -from openpilot.cereal import messaging, log +from openpilot.cereal import messaging, custom, log from openpilot.common.realtime import DT_MDL from openpilot.common.test import OpenpilotTestCase from openpilot.selfdrive.controls.lib.longitudinal_planner import get_coast_accel, get_cruise_accel +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP PlanSource = log.LongitudinalPlan.LongitudinalPlanSource +SunnyPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource class FakeSubMaster(dict): def __init__(self, lead_one: bool = True, lead_two: bool = False, *, valid: bool = True, alive: bool = True): + lead = {'dRel': 120.8, 'vRel': -16.4, 'aLeadK': -1.7} super().__init__(radarState=SimpleNamespace( - leadOne=SimpleNamespace(present=lead_one), - leadTwo=SimpleNamespace(present=lead_two), - )) + leadOne=SimpleNamespace(present=lead_one, **lead), + leadTwo=SimpleNamespace(present=lead_two, **lead), + ), carState=SimpleNamespace(vEgo=17.3), + selfdriveState=SimpleNamespace(personality=log.LongitudinalPersonality.standard)) self.valid = {'radarState': valid} self.alive = {'radarState': alive} def arbitrate(sm, mpc_accel=0.19, gated_cruise=-0.26, ungated_cruise=0.5, source=PlanSource.lead0, - allow_throttle=False, e2e=False, force_decel=False): - planner = object.__new__(LongitudinalPlannerSP) + allow_throttle=False, e2e=False, force_decel=False, lead_comfort=False, previous_accel=0.0): + planner = make_planner(lead_comfort, previous_accel) return planner.arbitrate_cruise_candidate( cast(messaging.SubMaster, sm), gated_cruise, ungated_cruise, mpc_accel, source, allow_throttle=allow_throttle, e2e=e2e, force_decel=force_decel, ) +def make_planner(lead_comfort: bool = False, previous_accel: float = 0.0): + planner = object.__new__(LongitudinalPlannerSP) + planner.accel_controller = AccelController() + planner.accel_controller._enabled = lead_comfort + planner.source = SunnyPlanSource.cruise + planner.output_a_target = previous_accel + return planner + + class TestThrottleIntentArbitration(OpenpilotTestCase): def test_coast_gate_cannot_add_braking_a_valid_lead_does_not_need(self): selected = arbitrate(FakeSubMaster()) @@ -53,6 +66,22 @@ class TestThrottleIntentArbitration(OpenpilotTestCase): self.assertEqual(selected, -0.26) self.assertEqual(min(selected, -0.6), -0.6) + def test_lead_comfort_is_only_an_extra_braking_candidate(self): + for sm, source in ((FakeSubMaster(), PlanSource.lead0), (FakeSubMaster(False, True), PlanSource.lead1)): + cruise = arbitrate(sm, mpc_accel=0.5, gated_cruise=0.5, ungated_cruise=0.5, source=source, + allow_throttle=True, lead_comfort=True, previous_accel=0.2) + self.assertLess(cruise, 0.2) + self.assertEqual(min(cruise, -1.2), -1.2) + + def test_lead_comfort_requires_a_live_selected_lead(self): + for sm, source in ((FakeSubMaster(valid=False), PlanSource.lead0), + (FakeSubMaster(alive=False), PlanSource.lead0), + (FakeSubMaster(lead_one=False), PlanSource.lead0), + (FakeSubMaster(lead_one=True, lead_two=False), PlanSource.lead1)): + with self.subTest(source=source): + self.assertEqual(arbitrate(sm, gated_cruise=0.5, ungated_cruise=0.5, source=source, + allow_throttle=True, lead_comfort=True), 0.5) + def test_policy_requires_live_valid_selected_lead(self): cases = ( FakeSubMaster(valid=False), @@ -90,7 +119,7 @@ class TestThrottleIntentArbitration(OpenpilotTestCase): steerRatio = 15.0 wheelbase = 2.7 - planner = object.__new__(LongitudinalPlannerSP) + planner = make_planner() sm = FakeSubMaster() v_ego = 6.9 v_cruise = 30.0