diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py index 610acd47df..21bf7d44a3 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py @@ -4,6 +4,8 @@ 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. """ + +from types import SimpleNamespace from typing import Any import numpy as np @@ -15,8 +17,23 @@ from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET from openpilot.selfdrive.modeld.constants import ModelConstants +from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, LongitudinalPlanSource from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V -from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision, _ENTERING_PRED_LAT_ACC_TH + +from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import ( + _A_LAT_REG_MAX, + _BELOW_EGO_TARGET_RELEASE_RATE, + _ENTERING_PRED_LAT_ACC_TH, + _MIN_ACTIVATION_SPEED, + _RELIEF_CONFIRMATION_FRAMES, + _TARGET_RELEASE_CONFIRMATION_FRAMES, + _TARGET_RELEASE_RATE, + _TARGET_TIGHTEN_CONFIRMATION_FRAMES, + _TARGET_TIGHTEN_RATE, + _TURNING_LAT_ACC_TH, + _URGENT_PRED_LAT_ACC_TH, + SmartCruiseControlVision, +) from openpilot.common.test import OpenpilotTestCase VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState @@ -107,7 +124,6 @@ def generate_controlsState(): class TestSmartCruiseControlVision(OpenpilotTestCase): - def setup_method(self): self.params = Params() self.reset_params() @@ -121,36 +137,377 @@ class TestSmartCruiseControlVision(OpenpilotTestCase): def reset_params(self): self.params.put_bool("SmartCruiseControlVision", True, block=True) + def assert_approx(self, actual, expected): + self.assertAlmostEqual(actual, expected, delta=max(1e-12, abs(expected) * 1e-6)) + + def set_lat_accels(self, current: float, predicted: float, v_ego: float = 20.0, model_speed: float = 20.0) -> None: + self.sm['controlsState'].curvature = current / v_ego**2 + self.sm['modelV2'].velocity.x = [model_speed] * len(ModelConstants.T_IDXS) + self.sm['modelV2'].orientationRate.z = [predicted / model_speed] * len(ModelConstants.T_IDXS) + + def update_lat_accels( + self, current: float, predicted: float, cruise: float = 30.0, a_ego: float = 0.0, v_ego: float = 20.0, model_speed: float = 20.0 + ) -> None: + self.set_lat_accels(current, predicted, v_ego, model_speed) + self.scc_v.update(self.sm, True, False, v_ego, a_ego, cruise) + + def enter_curve(self, predicted: float = 2.2) -> None: + self.update_lat_accels(0.5, predicted) + self.update_lat_accels(0.5, predicted) + assert self.scc_v.state == VisionState.entering + def test_initial_state(self): assert self.scc_v.state == VisionState.disabled assert not self.scc_v.is_active assert self.scc_v.output_v_target == V_CRUISE_UNSET - assert self.scc_v.output_a_target == 0. + assert self.scc_v.output_a_target == 0.0 def test_system_disabled(self): self.params.put_bool("SmartCruiseControlVision", False, block=True) self.scc_v.enabled = self.params.get_bool("SmartCruiseControlVision") - for _ in range(int(10. / DT_MDL)): - self.scc_v.update(self.sm, True, False, 0., 0., 0.) + for _ in range(int(10.0 / DT_MDL)): + self.scc_v.update(self.sm, True, False, 0.0, 0.0, 0.0) assert self.scc_v.state == VisionState.disabled assert not self.scc_v.is_active def test_disabled(self): - for _ in range(int(10. / DT_MDL)): - self.scc_v.update(self.sm, False, False, 0., 0., 0.) + for _ in range(int(10.0 / DT_MDL)): + self.scc_v.update(self.sm, False, False, 0.0, 0.0, 0.0) assert self.scc_v.state == VisionState.disabled def test_transition_disabled_to_enabled(self): - for _ in range(int(10. / DT_MDL)): - self.scc_v.update(self.sm, True, False, 0., 0., 0.) + for _ in range(int(10.0 / DT_MDL)): + self.scc_v.update(self.sm, True, False, 0.0, 0.0, 0.0) assert self.scc_v.state == VisionState.enabled - @parameterized.expand([ + def test_unconfirmed_release_holds_but_urgent_reentry_tightens(self): + self.enter_curve() + targets = [self.scc_v.output_v_target] + + self.update_lat_accels(2.0, 2.2, a_ego=-0.8) + assert self.scc_v.state == VisionState.turning + assert self.scc_v.output_a_target == -0.8 + turning_demand = self.scc_v._v_demand() + targets.append(self.scc_v.output_v_target) + + self.update_lat_accels(1.2, 1.2, a_ego=0.3) + assert self.scc_v.state == VisionState.leaving + assert self.scc_v.output_a_target == 0.3 + targets.append(self.scc_v.output_v_target) + + self.update_lat_accels(1.0, 3.0, a_ego=-1.2) + assert self.scc_v.state == VisionState.entering + assert self.scc_v.output_a_target == -1.2 + reentry_demand = self.scc_v._v_demand() + targets.append(self.scc_v.output_v_target) + + entering, turning, leaving, reentering = targets + assert turning < entering + self.assert_approx(turning, turning_demand) + self.assert_approx(leaving, turning) + assert reentering < leaving + self.assert_approx(reentering, reentry_demand) + + def test_new_curve_interrupts_confirmed_release_immediately(self): + self.enter_curve() + for _ in range(_RELIEF_CONFIRMATION_FRAMES + 1): + self.update_lat_accels(0.8, 0.8) + releasing_v_target = self.scc_v.output_v_target + assert self.scc_v.state == VisionState.leaving + + self.update_lat_accels(0.8, 3.0, a_ego=-0.7) + assert self.scc_v.state == VisionState.entering + assert self.scc_v.output_v_target < releasing_v_target + assert self.scc_v.output_a_target == -0.7 + + @parameterized.expand([(-2.0,), (-0.5,), (0.0,), (0.8,)]) + def test_planner_acceleration_passes_through_exactly(self, planner_accel): + self.enter_curve() + self.update_lat_accels(0.5, 2.2, a_ego=planner_accel) + assert self.scc_v.output_a_target == planner_accel + + def test_planner_acceleration_passes_through_all_states(self): + cases = ( + (False, False, 0.5, 2.2, -0.2, VisionState.disabled), + (True, False, 0.5, 0.8, 0.1, VisionState.enabled), + (True, False, 0.5, 2.2, -0.4, VisionState.entering), + (True, False, 2.0, 2.2, -0.8, VisionState.turning), + (True, False, 1.2, 1.2, 0.3, VisionState.leaving), + (True, True, 1.2, 1.2, 0.6, VisionState.overriding), + ) + for long_enabled, override, current, predicted, planner_accel, state in cases: + self.set_lat_accels(current, predicted) + self.scc_v.update(self.sm, long_enabled, override, 20.0, planner_accel, 30.0) + assert self.scc_v.state == state + assert self.scc_v.output_a_target == planner_accel + + def test_jitter_requires_confirmed_relief_then_releases_smoothly(self): + self.enter_curve() + previous_v_target = self.scc_v.output_v_target + + for frame in range(_RELIEF_CONFIRMATION_FRAMES * 2): + self.update_lat_accels(1.0, 1.05 if frame % 2 == 0 else 1.15) + assert self.scc_v.state == VisionState.entering + assert self.scc_v.output_v_target >= previous_v_target + assert self.scc_v.output_v_target - previous_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9 + previous_v_target = self.scc_v.output_v_target + + for _ in range(_RELIEF_CONFIRMATION_FRAMES): + self.update_lat_accels(1.15, 0.8) + assert self.scc_v.state == VisionState.entering + assert 0.0 <= self.scc_v.output_v_target - previous_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9 + previous_v_target = self.scc_v.output_v_target + + release_cruise = 30.0 + for _ in range(_RELIEF_CONFIRMATION_FRAMES - 1): + self.update_lat_accels(0.8, 0.8, release_cruise) + assert self.scc_v.state == VisionState.entering + assert 0.0 <= self.scc_v.output_v_target - previous_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9 + previous_v_target = self.scc_v.output_v_target + + active_v_targets = [previous_v_target] + for _ in range(int((release_cruise - previous_v_target) / (_TARGET_RELEASE_RATE * DT_MDL)) + 10): + self.update_lat_accels(0.8, 0.8, release_cruise) + if not self.scc_v.is_active: + break + assert self.scc_v.state == VisionState.leaving + assert self.scc_v.output_v_target != V_CRUISE_UNSET + active_v_targets.append(self.scc_v.output_v_target) + + assert self.scc_v.state == VisionState.enabled + assert self.scc_v.output_v_target == V_CRUISE_UNSET + self.assert_approx(active_v_targets[-1], release_cruise) + assert np.all((np.diff(active_v_targets) >= 0.0) & (np.diff(active_v_targets) <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9)) + + def test_target_release_waits_for_relief_above_ego_speed(self): + self.enter_curve() + held_v_target = self.scc_v.output_v_target + self.assert_approx(held_v_target, self.scc_v.v_ego) + + for _ in range(_RELIEF_CONFIRMATION_FRAMES + _TARGET_RELEASE_CONFIRMATION_FRAMES - 2): + self.update_lat_accels(0.8, 0.8) + self.assert_approx(self.scc_v.output_v_target, held_v_target) + + self.update_lat_accels(0.8, 0.8) + rise = self.scc_v.output_v_target - held_v_target + assert 0.0 < rise <= _TARGET_RELEASE_RATE * DT_MDL + 1e-9 + + def test_curve_target_is_independent_of_ego_speed(self): + model_speed = 24.0 + predicted_yaw_rate = 0.12 + predicted_lat_accel = model_speed * predicted_yaw_rate + expected_v_target = (_A_LAT_REG_MAX / (predicted_yaw_rate / model_speed)) ** 0.5 + targets = [] + + for v_ego in (18.0, 28.0): + controller = SmartCruiseControlVision() + self.set_lat_accels(0.5, predicted_lat_accel, v_ego, model_speed) + controller.update(self.sm, True, False, v_ego, 0.0, 30.0) + controller.update(self.sm, True, False, v_ego, 0.0, 30.0) + assert controller.state == VisionState.entering + targets.append(controller.v_target) + + self.assert_approx(targets[0], expected_v_target) + self.assert_approx(targets[1], expected_v_target) + + def test_curve_target_respects_minimum_speed_floor(self): + model_speed = 10.0 + predicted_yaw_rate = 2.0 + self.set_lat_accels(0.5, model_speed * predicted_yaw_rate, model_speed=model_speed) + self.scc_v.update(self.sm, True, False, 20.0, 0.0, 30.0) + self.scc_v.update(self.sm, True, False, 20.0, 0.0, 30.0) + + assert self.scc_v.state == VisionState.entering + assert self.scc_v.v_target < MIN_V + self.assert_approx(self.scc_v.output_v_target, MIN_V) + + @parameterized.expand( + [([], []), ([np.nan] * len(ModelConstants.T_IDXS), [np.nan] * len(ModelConstants.T_IDXS)), ([20.0] * 5, [0.1] * 3)], + names=["velocities", "yaw_rates"], + ) + def test_model_vector_edges_remain_finite(self, velocities, yaw_rates): + self.sm['modelV2'].velocity.x = velocities + self.sm['modelV2'].orientationRate.z = yaw_rates + self.scc_v.update(self.sm, True, False, 20.0, 0.0, 30.0) + self.scc_v.update(self.sm, True, False, 20.0, 0.0, 30.0) + + assert all( + np.isfinite(value) + for value in ( + self.scc_v.current_lat_acc, + self.scc_v.max_pred_lat_acc, + self.scc_v.v_target, + self.scc_v.output_v_target, + self.scc_v.output_a_target, + ) + ) + + @parameterized.expand([(5.75,), (9.9,), (_MIN_ACTIVATION_SPEED,)]) + def test_vision_control_does_not_steal_launch(self, launch_speed): + self.set_lat_accels(0.5, 3.0, launch_speed) + self.scc_v.update(self.sm, True, False, launch_speed, 0.0, 30.0) + self.scc_v.update(self.sm, True, False, launch_speed, 0.0, 30.0) + + assert launch_speed <= _MIN_ACTIVATION_SPEED + assert self.scc_v.state == VisionState.enabled + assert not self.scc_v.is_active + assert self.scc_v.output_v_target == V_CRUISE_UNSET + + def test_vision_control_can_activate_above_launch_range(self): + speed = _MIN_ACTIVATION_SPEED + 0.01 + self.set_lat_accels(0.5, 3.0, speed) + self.scc_v.update(self.sm, True, False, speed, 0.0, 30.0) + self.scc_v.update(self.sm, True, False, speed, 0.0, 30.0) + + assert self.scc_v.state == VisionState.entering + assert self.scc_v.is_active + + def test_nonurgent_activation_has_no_target_cliff(self): + v_ego = _MIN_ACTIVATION_SPEED + 0.01 + model_speed = 8.0 + self.update_lat_accels(0.5, 2.0, v_ego=v_ego, model_speed=model_speed) + self.update_lat_accels(0.5, 2.0, v_ego=v_ego, model_speed=model_speed) + + self.assert_approx(self.scc_v.v_target, 8.0) + self.assert_approx(self.scc_v.output_v_target, v_ego) + + def test_nonurgent_tightening_is_confirmed_and_rate_limited(self): + self.enter_curve() + initial_v_target = self.scc_v.output_v_target + + for _ in range(_TARGET_TIGHTEN_CONFIRMATION_FRAMES - 1): + self.update_lat_accels(0.5, 2.8) + self.assert_approx(self.scc_v.output_v_target, initial_v_target) + + self.update_lat_accels(0.5, 2.8) + drop = initial_v_target - self.scc_v.output_v_target + assert 0.0 < drop <= _TARGET_TIGHTEN_RATE * DT_MDL + 1e-9 + + def test_one_frame_curve_prediction_does_not_pulse_target(self): + self.enter_curve() + for _ in range(10): + self.update_lat_accels(0.5, 2.2) + stable_v_target = self.scc_v.output_v_target + + self.update_lat_accels(0.5, 2.8) + self.assert_approx(self.scc_v.output_v_target, stable_v_target) + self.update_lat_accels(0.5, 2.2) + + self.assert_approx(self.scc_v.output_v_target, stable_v_target) + + def test_one_frame_release_does_not_reverse_target(self): + self.enter_curve(_URGENT_PRED_LAT_ACC_TH) + stable_v_target = self.scc_v.output_v_target + + self.update_lat_accels(0.5, 2.2) + self.assert_approx(self.scc_v.output_v_target, stable_v_target) + self.update_lat_accels(0.5, _URGENT_PRED_LAT_ACC_TH) + + self.assert_approx(self.scc_v.output_v_target, stable_v_target) + + def test_urgent_predicted_curve_is_not_delayed(self): + self.enter_curve() + self.update_lat_accels(0.5, _URGENT_PRED_LAT_ACC_TH) + + self.assert_approx(self.scc_v.output_v_target, self.scc_v._v_demand()) + + def test_current_curve_is_not_delayed(self): + self.enter_curve() + self.update_lat_accels(_TURNING_LAT_ACC_TH, 2.8) + + self.assert_approx(self.scc_v.output_v_target, self.scc_v._v_demand()) + + def test_sequential_curve_confirms_release_and_tightens_urgently(self): + self.enter_curve(3.0) + for _ in range(20): + self.update_lat_accels(0.5, 3.0) + restrictive_v_target = self.scc_v.output_v_target + + self.update_lat_accels(0.5, 1.4, a_ego=0.4) + assert self.scc_v.state == VisionState.entering + self.assert_approx(self.scc_v.output_v_target, restrictive_v_target) + assert self.scc_v.output_a_target == 0.4 + + for _ in range(_TARGET_RELEASE_CONFIRMATION_FRAMES - 2): + self.update_lat_accels(0.5, 1.4) + self.assert_approx(self.scc_v.output_v_target, restrictive_v_target) + + self.update_lat_accels(0.5, 1.4) + released_v_target = self.scc_v.output_v_target + assert 0.0 < released_v_target - restrictive_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9 + + self.update_lat_accels(0.5, 3.0, a_ego=-0.6) + assert self.scc_v.state == VisionState.entering + self.assert_approx(self.scc_v.output_v_target, restrictive_v_target) + assert self.scc_v.output_a_target == -0.6 + + for _ in range(4): + self.update_lat_accels(0.5, 1.4) + self.assert_approx(self.scc_v.output_v_target, restrictive_v_target) + self.update_lat_accels(0.5, 3.0) + self.assert_approx(self.scc_v.output_v_target, restrictive_v_target) + + def test_acceleration_is_continuous_through_planner_arbitration(self): + car_control = messaging.new_message('carControl') + car_control.carControl.enabled = True + car_control.carControl.cruiseControl.override = False + self.sm['carControl'] = car_control.carControl + self.sm['carState'].vCruiseCluster = 108.0 + + planner: Any = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP) + planner.scc = SimpleNamespace( + vision=self.scc_v, + map=SimpleNamespace(output_v_target=V_CRUISE_UNSET, output_a_target=0.0), + update=lambda sm, enabled, override, v_ego, a_ego, v_cruise: self.scc_v.update(sm, enabled, override, v_ego, a_ego, v_cruise), + ) + planner.resolver = SimpleNamespace( + speed_limit_valid=False, + speed_limit_last_valid=False, + speed_limit=0.0, + speed_limit_final_last=0.0, + distance=0.0, + update=lambda _v_ego, _sm: None, + ) + planner.sla = SimpleNamespace( + output_v_target=V_CRUISE_UNSET, + output_a_target=0.0, + update=lambda *_args: None, + ) + planner.events_sp = SimpleNamespace() + + self.set_lat_accels(0.5, 2.2) + planner.update_targets(self.sm, 20.0, -0.8, 30.0) + planner.update_targets(self.sm, 20.0, -0.8, 30.0) + assert planner.source == LongitudinalPlanSource.sccVision + assert planner.output_a_target == -0.8 + + for planner_accel in (-2.0, 0.5, -0.2): + planner.update_targets(self.sm, 20.0, planner_accel, 30.0) + assert planner.source == LongitudinalPlanSource.sccVision + assert planner.output_a_target == planner_accel + + self.set_lat_accels(0.8, 0.8) + for _ in range(int(30.0 / (_TARGET_RELEASE_RATE * DT_MDL)) + 10): + planner.update_targets(self.sm, 20.0, 0.4, 30.0) + assert planner.output_a_target == 0.4 + if planner.source == LongitudinalPlanSource.cruise: + break + else: + self.fail("SCC Vision did not release to cruise") + + planner.update_targets(self.sm, 20.0, 0.4, 30.0) + assert self.scc_v.state == VisionState.enabled + assert planner.source == LongitudinalPlanSource.cruise + + @parameterized.expand( + [ ("p97_just_above_threshold", True), ("single_spike_filtered", False), ("persistent_high_values", True), - ], names=["case", "should_enter"]) + ], + names=["case", "should_enter"], + ) def test_max_pred_lat_acc_uses_p97_and_threshold(self, case, should_enter): n = len(ModelConstants.T_IDXS) th = float(_ENTERING_PRED_LAT_ACC_TH) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller_closed_loop.py b/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller_closed_loop.py new file mode 100644 index 0000000000..ffa2e6b955 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller_closed_loop.py @@ -0,0 +1,110 @@ +""" +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. +""" + +import gc +from contextlib import ExitStack +from unittest import mock + +import numpy as np + +from openpilot.common.test import OpenpilotTestCase +from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant +from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanSource +from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import _A_LAT_REG_MAX + + +def _run_constant_curve(*, scc_enabled: bool, cruise: float, duration: float = 70.0) -> dict[str, np.ndarray]: + gc.collect() + curvature = 0.005 + plant = Plant(lead_relevancy=False, speed=30.0) + planner = plant.planner + planner.dec._enabled = False + planner.scc.map.enabled = False + planner.scc.vision.enabled = scc_enabled + solver_failures = 0 + + with ExitStack() as patches: + patches.enter_context(mock.patch.object(planner.dec, "_read_params", return_value=None)) + patches.enter_context(mock.patch.object(planner.scc.map, "update_params", return_value=None)) + patches.enter_context(mock.patch.object(planner.scc.vision, "_update_params", return_value=None)) + + original_mpc_reset = planner.mpc.reset + + def record_mpc_reset(*args, **kwargs): + nonlocal solver_failures + solver_failures += int(planner.mpc.solution_status != 0) + return original_mpc_reset(*args, **kwargs) + + patches.enter_context(mock.patch.object(planner.mpc, "reset", side_effect=record_mpc_reset)) + + if scc_enabled: + original_update_calculations = planner.scc.vision._update_calculations + + def inject_constant_curvature(sm): + velocities = np.asarray(sm['modelV2'].velocity.x, dtype=float) + sm['modelV2'].orientationRate.z = (curvature * velocities).tolist() + sm['controlsState'].curvature = curvature + original_update_calculations(sm) + + patches.enter_context(mock.patch.object(planner.scc.vision, "_update_calculations", side_effect=inject_constant_curvature)) + + original_update = planner.update + + def enable_longitudinal(sm): + sm['carControl'].enabled = True + sm['carControl'].longActive = True + original_update(sm) + + patches.enter_context(mock.patch.object(planner, "update", side_effect=enable_longitudinal)) + rows = [] + while plant.current_time < duration: + output = plant.step(v_cruise=cruise) + rows.append( + ( + plant.current_time, + output['speed'], + output['should_stop'], + planner.scc.vision.is_active, + planner.source == LongitudinalPlanSource.sccVision, + planner.scc.vision.output_v_target, + ) + ) + + data = np.asarray(rows, dtype=float) + gc.collect() + return { + 'time': data[:, 0], + 'speed': data[:, 1], + 'should_stop': data[:, 2], + 'active': data[:, 3], + 'scc_source': data[:, 4], + 'target': data[:, 5], + 'solver_failures': np.asarray(solver_failures), + } + + +class TestVisionControllerClosedLoop(OpenpilotTestCase): + def test_constant_curve_recovers_like_stock_speed_cap(self): + target = (_A_LAT_REG_MAX / 0.005) ** 0.5 + scc = _run_constant_curve(scc_enabled=True, cruise=30.0) + stock = _run_constant_curve(scc_enabled=False, cruise=target) + scc_final = scc['speed'][scc['time'] >= 60.0] + stock_final = stock['speed'][stock['time'] >= 60.0] + + # The generated solver can report platform-specific failures for the + # synthetic no-lead plant. The feature must not make that stock baseline + # worse; requiring an absolute zero would hide a harness difference as a + # controller regression. + assert scc['solver_failures'] <= stock['solver_failures'] + assert not scc['should_stop'].any() + assert np.all(scc['active'][scc['time'] >= 60.0]) + assert np.all(scc['scc_source'][scc['time'] >= 60.0]) + assert np.allclose(scc['target'][scc['time'] >= 60.0], target) + assert scc_final.min() >= target - 1.0 + assert abs(scc_final.mean() - stock_final.mean()) < 0.5 + assert abs(scc_final.min() - stock_final.min()) < 1.0 + assert abs(scc_final.max() - stock_final.max()) < 1.0 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py index fb76f5b546..7916d6b3d3 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py @@ -23,25 +23,21 @@ _ENTERING_PRED_LAT_ACC_TH = 1.3 # Predicted Lat Acc threshold to trigger enteri _ABORT_ENTERING_PRED_LAT_ACC_TH = 1.1 # Predicted Lat Acc threshold to abort entering state if speed drops. _TURNING_LAT_ACC_TH = 1.6 # Lat Acc threshold to trigger turning state. +_URGENT_PRED_LAT_ACC_TH = 3. # Predicted Lat Acc threshold that requires an immediate speed reduction. _LEAVING_LAT_ACC_TH = 1.3 # Lat Acc threshold to trigger leaving turn state. _FINISH_LAT_ACC_TH = 1.1 # Lat Acc threshold to trigger the end of the turn cycle. _A_LAT_REG_MAX = 2. # Maximum lateral acceleration -_NO_OVERSHOOT_TIME_HORIZON = 4. # s. Time to use for velocity desired based on a_target when not overshooting. - -# Lookup table for the minimum smooth deceleration during the ENTERING state -# depending on the actual maximum absolute lateral acceleration predicted on the turn ahead. -_ENTERING_SMOOTH_DECEL_V = [-0.2, -1.] # min decel value allowed on ENTERING state -_ENTERING_SMOOTH_DECEL_BP = [1.3, 3.] # absolute value of lat acc ahead - -# Lookup table for the acceleration for the TURNING state -# depending on the current lateral acceleration of the vehicle. -_TURNING_ACC_V = [0.5, 0., -0.4] # acc value -_TURNING_ACC_BP = [1.5, 2.3, 3.] # absolute value of current lat acc - -_LEAVING_ACC = 0.5 # Conformable acceleration to regain speed while leaving a turn. +_RELIEF_CONFIRMATION_FRAMES = max(1, int(round(0.5 / DT_MDL))) +_TARGET_TIGHTEN_CONFIRMATION_FRAMES = max(1, int(round(0.1 / DT_MDL))) +_TARGET_RELEASE_CONFIRMATION_FRAMES = max(1, int(round(0.15 / DT_MDL))) +_TARGET_TIGHTEN_RATE = 5. # m/s^2 +_TARGET_RELEASE_RATE = 1. # m/s^2 +_BELOW_EGO_TARGET_RELEASE_RATE = 3. # m/s^2 +_MIN_PRED_SPEED = 1. # m/s +_MIN_ACTIVATION_SPEED = 10. # m/s class SmartCruiseControlVision: @@ -65,14 +61,62 @@ class SmartCruiseControlVision: self.state = VisionState.disabled self.current_lat_acc = 0. self.max_pred_lat_acc = 0. + self.relief_frames = 0 + self.tighten_frames = 0 + self.release_frames = 0 + + def _v_demand(self) -> float: + return max(MIN_V, min(self.v_target, self.v_cruise_setpoint)) + + def _curve_is_urgent(self) -> bool: + return self.current_lat_acc >= _TURNING_LAT_ACC_TH or self.max_pred_lat_acc >= _URGENT_PRED_LAT_ACC_TH + + def _filtered_v_target(self) -> float: + demand = self._v_demand() + + if self.output_v_target == V_CRUISE_UNSET: + self.tighten_frames = 0 + self.release_frames = 0 + if self._curve_is_urgent(): + return demand + return max(demand, min(self.v_ego, self.v_cruise_setpoint)) + + if demand < self.output_v_target: + self.release_frames = 0 + if self._curve_is_urgent(): + self.tighten_frames = 0 + return demand + + self.tighten_frames += 1 + if self.tighten_frames < _TARGET_TIGHTEN_CONFIRMATION_FRAMES: + return self.output_v_target + return max(demand, self.output_v_target - _TARGET_TIGHTEN_RATE * DT_MDL) + + self.tighten_frames = 0 + releasing_brake = self.output_v_target < min(self.v_ego, demand) + if not releasing_brake and self.relief_frames < _RELIEF_CONFIRMATION_FRAMES: + self.release_frames = 0 + return self.output_v_target + + if demand > self.output_v_target: + self.release_frames += 1 + if self.release_frames < _TARGET_RELEASE_CONFIRMATION_FRAMES: + return self.output_v_target + else: + self.release_frames = 0 + + release_rate = _BELOW_EGO_TARGET_RELEASE_RATE if releasing_brake else _TARGET_RELEASE_RATE + return min(demand, self.output_v_target + release_rate * DT_MDL) def get_a_target_from_control(self) -> float: - return self.a_target + return self.a_ego def get_v_target_from_control(self) -> float: if self.is_active: - return max(self.v_target, MIN_V) + self.a_target * _NO_OVERSHOOT_TIME_HORIZON + return self._filtered_v_target() + self.tighten_frames = 0 + self.release_frames = 0 return V_CRUISE_UNSET def _update_params(self) -> None: @@ -82,25 +126,27 @@ class SmartCruiseControlVision: def _update_calculations(self, sm: messaging.SubMaster) -> None: if not self.long_enabled: return - else: - rate_plan = np.array(np.abs(sm['modelV2'].orientationRate.z)) - vel_plan = np.array(sm['modelV2'].velocity.x) - self.current_lat_acc = self.v_ego ** 2 * abs(sm['controlsState'].curvature) + rate_plan = np.asarray(np.abs(sm['modelV2'].orientationRate.z), dtype=float) + vel_plan = np.asarray(sm['modelV2'].velocity.x, dtype=float) + size = min(len(rate_plan), len(vel_plan)) + rate_plan, vel_plan = rate_plan[:size], vel_plan[:size] + valid = np.isfinite(rate_plan) & np.isfinite(vel_plan) & (vel_plan >= _MIN_PRED_SPEED) - # get the maximum lat accel from the model - predicted_lat_accels = rate_plan * vel_plan - self.max_pred_lat_acc = np.percentile(predicted_lat_accels, 97) - - # get the maximum curve based on the current velocity - v_ego = max(self.v_ego, 0.1) # ensure a value greater than 0 for calculations - max_curve = self.max_pred_lat_acc / (v_ego**2) - - # Get the target velocity for the maximum curve - self.v_target = (_A_LAT_REG_MAX / max_curve) ** 0.5 + self.current_lat_acc = self.v_ego ** 2 * abs(sm['controlsState'].curvature) + self.max_pred_lat_acc = 0. + self.v_target = V_CRUISE_UNSET + if np.any(valid): + self.max_pred_lat_acc = float(np.percentile(rate_plan[valid] * vel_plan[valid], 97)) + max_pred_curvature = float(np.percentile(rate_plan[valid] / vel_plan[valid], 97)) + if max_pred_curvature > 0.: + self.v_target = min(float((_A_LAT_REG_MAX / max_pred_curvature) ** 0.5), V_CRUISE_UNSET) def _update_state_machine(self) -> tuple[bool, bool]: # ENABLED, ENTERING, TURNING, LEAVING, OVERRIDING + relief = self.current_lat_acc < _FINISH_LAT_ACC_TH and self.max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH + self.relief_frames = self.relief_frames + 1 if self.state in ACTIVE_STATES and relief else 0 + if self.state != VisionState.disabled: # longitudinal and feature disable always have priority in a non-disabled state if not self.long_enabled or not self.enabled: @@ -112,7 +158,7 @@ class SmartCruiseControlVision: # ENABLED if self.state == VisionState.enabled: # Do not enter a turn control cycle if the speed is low. - if self.v_ego <= MIN_V: + if self.v_ego <= _MIN_ACTIVATION_SPEED: pass # If significant lateral acceleration is predicted ahead, then move to Entering turn state. elif self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH: @@ -128,23 +174,26 @@ class SmartCruiseControlVision: # Transition to Turning if current lateral acceleration is over the threshold. if self.current_lat_acc >= _TURNING_LAT_ACC_TH: self.state = VisionState.turning - # Abort if the predicted lateral acceleration drops - elif self.max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH: - self.state = VisionState.enabled + # Begin releasing only after both current and predicted lateral acceleration stay clear. + elif self.relief_frames >= _RELIEF_CONFIRMATION_FRAMES: + self.state = VisionState.leaving # TURNING elif self.state == VisionState.turning: - # Transition to Leaving if current lateral acceleration drops below a threshold. + # Transition out of Turning if current lateral acceleration drops below a threshold. if self.current_lat_acc <= _LEAVING_LAT_ACC_TH: - self.state = VisionState.leaving + self.state = VisionState.entering if self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH else VisionState.leaving # LEAVING elif self.state == VisionState.leaving: # Transition back to Turning if current lateral acceleration goes back over the threshold. if self.current_lat_acc >= _TURNING_LAT_ACC_TH: self.state = VisionState.turning - # Finish if current lateral acceleration goes below a threshold. - elif self.current_lat_acc < _FINISH_LAT_ACC_TH: + # Start a new turn cycle immediately if another curve is predicted. + elif self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH: + self.state = VisionState.entering + # Finish after confirmed relief and a gradual release to the cruise setpoint. + elif self.relief_frames >= _RELIEF_CONFIRMATION_FRAMES and self.output_v_target >= self.v_cruise_setpoint: self.state = VisionState.enabled # DISABLED @@ -157,32 +206,11 @@ class SmartCruiseControlVision: enabled = self.state in ENABLED_STATES active = self.state in ACTIVE_STATES + if not active: + self.relief_frames = 0 return enabled, active - def _update_solution(self) -> float: - # DISABLED, ENABLED, OVERRIDING - if self.state not in ACTIVE_STATES: - # when not overshooting, calculate v_turn as the speed at the prediction horizon when following - # the smooth deceleration. - a_target = self.a_ego - # ENTERING - elif self.state == VisionState.entering: - # when not overshooting, target a smooth deceleration in preparation for a sharp turn to come. - a_target = np.interp(self.max_pred_lat_acc, _ENTERING_SMOOTH_DECEL_BP, _ENTERING_SMOOTH_DECEL_V) - # TURNING - elif self.state == VisionState.turning: - # When turning, we provide a target acceleration that is comfortable for the lateral acceleration felt. - a_target = np.interp(self.current_lat_acc, _TURNING_ACC_BP, _TURNING_ACC_V) - # LEAVING - elif self.state == VisionState.leaving: - # When leaving, we provide a comfortable acceleration to regain speed. - a_target = _LEAVING_ACC - else: - raise NotImplementedError(f"SCC-V state not supported: {self.state}") - - return a_target - def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise_setpoint: float) -> None: self.long_enabled = long_enabled @@ -195,7 +223,7 @@ class SmartCruiseControlVision: self._update_calculations(sm) self.is_enabled, self.is_active = self._update_state_machine() - self.a_target = self._update_solution() + self.a_target = self.a_ego self.output_v_target = self.get_v_target_from_control() self.output_a_target = self.get_a_target_from_control()