From a0b2e71d4db10efea654dfa32b34626a5ec65fd5 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Mon, 17 Aug 2026 10:59:03 -0700 Subject: [PATCH] fix(scc): confirm curve target tightening --- .../tests/test_vision_controller.py | 102 +++++++++++++++--- .../smart_cruise_control/vision_controller.py | 51 +++++++-- 2 files changed, 130 insertions(+), 23 deletions(-) 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 4ca4cb1cf5..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 @@ -26,7 +26,12 @@ from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_con _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 @@ -176,13 +181,14 @@ class TestSmartCruiseControlVision(OpenpilotTestCase): self.scc_v.update(self.sm, True, False, 0.0, 0.0, 0.0) assert self.scc_v.state == VisionState.enabled - def test_unconfirmed_leaving_and_reentry_only_shape_speed(self): + 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) @@ -193,12 +199,15 @@ class TestSmartCruiseControlVision(OpenpilotTestCase): 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 - self.assert_approx(turning, entering) - assert 0.0 < leaving - turning <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9 + 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() @@ -271,18 +280,18 @@ class TestSmartCruiseControlVision(OpenpilotTestCase): 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_slows_after_reaching_ego_speed(self): + 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(100): - previous_v_target = self.scc_v.output_v_target + for _ in range(_RELIEF_CONFIRMATION_FRAMES + _TARGET_RELEASE_CONFIRMATION_FRAMES - 2): self.update_lat_accels(0.8, 0.8) - if previous_v_target >= self.scc_v.v_ego: - rise = self.scc_v.output_v_target - previous_v_target - assert 0.0 < rise <= _TARGET_RELEASE_RATE * DT_MDL + 1e-9 - break - else: - self.fail("curve target did not release to ego speed") + 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 @@ -354,20 +363,79 @@ class TestSmartCruiseControlVision(OpenpilotTestCase): assert self.scc_v.state == VisionState.entering assert self.scc_v.is_active - def test_sequential_curve_tightens_immediately_and_releases_bounded(self): + 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) - first_relief_v_target = self.scc_v.output_v_target assert self.scc_v.state == VisionState.entering - assert 0.0 < first_relief_v_target - restrictive_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9 + 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) - assert 0.0 <= self.scc_v.output_v_target - first_relief_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9 + 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 @@ -376,7 +444,7 @@ class TestSmartCruiseControlVision(OpenpilotTestCase): for _ in range(4): self.update_lat_accels(0.5, 1.4) - assert 0.0 < self.scc_v.output_v_target - restrictive_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9 + 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) 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 99b26ddf74..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,6 +23,7 @@ _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. @@ -30,6 +31,9 @@ _FINISH_LAT_ACC_TH = 1.1 # Lat Acc threshold to trigger the end of the turn cyc _A_LAT_REG_MAX = 2. # Maximum lateral acceleration _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 @@ -58,15 +62,50 @@ class SmartCruiseControlVision: 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 _released_v_target(self) -> float: + 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: - return demand - release_rate = _BELOW_EGO_TARGET_RELEASE_RATE if self.output_v_target < min(self.v_ego, demand) else _TARGET_RELEASE_RATE + 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: @@ -74,10 +113,10 @@ class SmartCruiseControlVision: def get_v_target_from_control(self) -> float: if self.is_active: - if self.output_v_target == V_CRUISE_UNSET: - return self._v_demand() - return self._released_v_target() + return self._filtered_v_target() + self.tighten_frames = 0 + self.release_frames = 0 return V_CRUISE_UNSET def _update_params(self) -> None: