From cd20a0079bfd717244ea1765dbce88326b102335 Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Mon, 24 Aug 2026 18:10:09 -0500 Subject: [PATCH] Force Stop: fast approach --- cereal/custom.capnp | 1 + .../controls/lib/longitudinal_planner.py | 17 ++++++++- selfdrive/controls/radard.py | 18 ++++++++-- .../tests/test_longitudinal_planner.py | 1 + .../controls/tests/test_starpilot_vcruise.py | 35 +++++++++++++++++++ starpilot/common/accel_profile.py | 2 -- starpilot/controls/lib/starpilot_vcruise.py | 22 ++++++++++++ starpilot/controls/starpilot_planner.py | 10 ++++-- 8 files changed, 98 insertions(+), 8 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 01c8f4f24..3d5c1104e 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -224,6 +224,7 @@ struct StarPilotPlan @0xf98d843bfd7004a3 { cscOverridden @38 :Bool; # driver cancelled this curve with RES+ cscLearnedLatAccel @39 :Float32; # learned comfort at the current curvature, before margin cscBindingDistance @40 :Float32; # distance to the horizon point setting the target, m + approachStopLength @41 :Float32; # pre-commit distance to a detected stop, m; 0 when off } struct StarPilotRadarState @0xb86e6369214c01c8 { diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index f37ab1e5c..bfda1ad53 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -9,6 +9,7 @@ from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.starpilot.common.model_versions import is_tinygrad_model_version +from openpilot.starpilot.controls.lib.starpilot_vcruise import FT_TO_M, OFFSET_FT_MAX, OFFSET_FT_MIN from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance @@ -373,6 +374,11 @@ def get_vehicle_min_accel(CP, v_ego): # Restored planner constants retained by CEM, stop, and departure paths. A_CRUISE_MIN = -1.0 +# The stop distance runs ~9 m long through the mid-approach, which leaves the obstacle slack +# so it stays silent and deceleration sags. Multiplicative so the trim scales with what is +# left: ~13 m at 90 m, ~1.5 m at the line. Buys the 35-70 m band; the far bands are dominated +# by the over-read, so lowering this further cannot reach them. +FORCE_STOP_OBSTACLE_TRIM = 0.85 STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_SPEED = 0.25 STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_ACCEL = 0.08 STANDSTILL_LEAD_CREEP_RELEASE_MIN_GAP_MARGIN = 0.1 @@ -2196,8 +2202,17 @@ class LongitudinalPlanner: force_stop_x = None force_stop_handoff_m = get_force_stop_handoff_distance(self.CP.carFingerprint) if sm['starpilotPlan'].forcingStop and sm['starpilotPlan'].forcingStopLength > force_stop_handoff_m: + stop_length = float(sm['starpilotPlan'].forcingStopLength) + else: + # pre-commit the envelope is only a speed ceiling, which the solver tracks with a lag; + # getattr so a stale cereal build degrades to the old behaviour instead of raising + stop_length = float(getattr(sm['starpilotPlan'], 'approachStopLength', 0.0)) + if stop_length > force_stop_handoff_m: + # ForceStopDistanceOffset shifts the perceived line for the v_cruise ceiling, so it has + # to shift the obstacle too or the slider barely moves anything now that stop_x leads. + offset_ft = max(OFFSET_FT_MIN, min(OFFSET_FT_MAX, int(getattr(starpilot_toggles, 'force_stop_distance_offset', 0) or 0))) force_stop_x = ( - float(sm['starpilotPlan'].forcingStopLength) + STOP_DISTANCE + + stop_length * FORCE_STOP_OBSTACLE_TRIM + offset_ft * FT_TO_M + STOP_DISTANCE + get_force_stop_distance_bias(self.CP.carFingerprint) ) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index f9cb6d3f3..3a7efb02e 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -41,6 +41,8 @@ ADJACENT_STOP_REST_FRAMES = 15 ADJACENT_STOP_MIN_Y = 1.8 # m — inside this is our own lane ADJACENT_STOP_MAX_Y = 7.5 # m — beyond this is roadside, not an adjacent lane ADJACENT_STOP_MAX_D = 110.0 # m +ADJACENT_STOP_QUEUE_GAP_M = 5.0 # m — anything stopped beyond the furthest qualifier means + # the bar is past it too, so the hint would stop us short class KalmanParams: @@ -157,6 +159,10 @@ class Track: if self.leadTrackID == self.identifier: return False + return self.in_adjacent_lane(model_data) + + def in_adjacent_lane(self, model_data: capnp._DynamicStructReader): + """Lane geometry only, no deceleration history — also used to spot a queue ahead.""" if not (ADJACENT_STOP_MIN_Y < abs(self.yRel) < ADJACENT_STOP_MAX_Y): return False @@ -330,9 +336,10 @@ def get_adjacent_lead(tracks: dict[int, Track], standstill: bool, model_data: ca def get_adjacent_stopped(tracks: dict[int, Track], model_data: capnp._DynamicStructReader) -> dict[str, Any]: """Stop-line hint: a vehicle that decelerated to a stop in a neighbouring lane. - Takes the FARTHEST qualifying vehicle: in a queue the front car sits at the bar and the - rest are closer to us, so the nearest one underestimates the distance. The consumer only - shortens with this, so underestimating is the harmful direction. + Takes the FARTHEST qualifying vehicle, then drops the hint entirely if a queue reaches + past it. Cars already stopped when we acquire them never show the moving -> stopped + transition, so the qualifying set is biased toward the back of a line; without this the + hint marks a mid-queue bumper and stops us short of the bar. """ if len(model_data.laneLines) < 4: return {'status': False} @@ -342,6 +349,11 @@ def get_adjacent_stopped(tracks: dict[int, Track], model_data: capnp._DynamicStr return {'status': False} furthest = max(candidates, key=lambda c: c.dRel) + for c in tracks.values(): + if (c.dRel > furthest.dRel + ADJACENT_STOP_QUEUE_GAP_M and + abs(c.vLead) < ADJACENT_STOP_REST_V and + c.in_adjacent_lane(model_data)): + return {'status': False} return { 'status': True, 'dRel': float(furthest.dRel), diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 0b8887771..6119ba921 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -434,6 +434,7 @@ def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimenta forcingStop=False, redLight=False, forcingStopLength=2, + approachStopLength=0.0, ), } diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index 2c4b58632..9abbd1992 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -7,6 +7,7 @@ from openpilot.common.realtime import DT_MDL from openpilot.starpilot.common.starpilot_variables import PLANNER_TIME from openpilot.starpilot.controls.lib.curve_speed_controller import CSC_GLOW_HOLD_TIME, CSC_GLOW_ON_DELTA from openpilot.starpilot.controls.lib.starpilot_vcruise import ( + FORCE_STOP_CAP_SLACK_M, FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME, STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME, StarPilotVCruise, @@ -57,6 +58,8 @@ def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False vcruise.forcing_stop = forcing_stop vcruise.force_stop_timer = 1.0 if forcing_stop else 0.0 vcruise.tracked_model_length = 0.0 if forcing_stop else planner.model_length + # what the not-committed branch would have left behind on the frame before commit + vcruise.force_stop_distance_cap = planner.model_length return planner, vcruise @@ -753,6 +756,7 @@ def test_force_stop_reanchors_when_model_reopens_path_without_stop_action(): planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True) planner.model_length = 90.0 vcruise.tracked_model_length = 60.0 + vcruise.force_stop_distance_cap = 90.0 sm = make_sm(standstill=False) sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False)) @@ -774,6 +778,37 @@ def test_force_stop_does_not_reanchor_inside_reanchor_floor(): assert vcruise.tracked_model_length < 25.0 +def test_force_stop_reanchor_bounded_by_distance_driven(): + # The line can't recede: a ballooning horizon may not push the stop past where it was at + # commit minus the distance driven since. + planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True) + planner.model_length = 200.0 + vcruise.tracked_model_length = 60.0 + vcruise.force_stop_distance_cap = 70.0 + sm = make_sm(standstill=False) + sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False)) + + update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=10.0) + + assert vcruise.tracked_model_length <= 70.0 + FORCE_STOP_CAP_SLACK_M + assert vcruise.tracked_model_length < 100.0 # nowhere near the 200 m the horizon claimed + + +def test_force_stop_cap_slack_tapers_near_the_line(): + # Slack protects against an under-read at commit; held near the line it would just aim the + # solver that far past the stop bar. + planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True) + planner.model_length = 200.0 + vcruise.tracked_model_length = 60.0 + vcruise.force_stop_distance_cap = 12.0 + sm = make_sm(standstill=False) + sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False)) + + update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=5.0) + + assert vcruise.tracked_model_length < 12.0 + FORCE_STOP_CAP_SLACK_M / 2.0 + + def test_force_stop_does_not_reanchor_committed_model_stop(): planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True) planner.model_length = 40.0 diff --git a/starpilot/common/accel_profile.py b/starpilot/common/accel_profile.py index 8165919ce..48d81e1c0 100644 --- a/starpilot/common/accel_profile.py +++ b/starpilot/common/accel_profile.py @@ -3,8 +3,6 @@ from __future__ import annotations import math -from openpilot.selfdrive.controls.lib.longitudinal_planner import get_max_accel - ACCELERATION_PROFILES = { "STANDARD": 0, "ECO": 1, diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index 80068c10b..040abe1eb 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -62,6 +62,8 @@ LEAD_VETO_M_OVERRIDES = { } FORCE_STOP_APPROACH_DECEL = 0.65 # m/s^2 — speed ceiling before commit. LOWER = more early # braking; don't go under FORCE_STOP_MODEL_APPROACH_DECEL +# approachStopLength is published RAW: model_length converges from above, so rate-limiting +# it inward freezes it far out and the constraint never binds. Tried, measured, don't re-add. ADAS_MAX_MS = 17.88 # 40 mph — cross-street ADAS guard DASH_SEED_M = 27.0 # ~88 ft — typical ADAS detection distance, used to snap # tracked length closer when dashboard confirms a sign @@ -81,6 +83,12 @@ FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME = 4.0 FORCE_STOP_DISTANCE_REANCHOR_MIN_GAP = 3.0 # m — ignore small model-horizon noise FORCE_STOP_REANCHOR_MIN_M = 40.0 # m — inside this only ratchet down; shouldStop doesn't # assert until ~10 m, so horizon jitter would release the stop +FORCE_STOP_CAP_SLACK_M = 15.0 # m — the line can't move away, so tracked can never exceed + # what it was at commit minus distance driven. Slack covers an + # under-read at commit; without it that would stop us short. +FORCE_STOP_CAP_TAPER_M = 60.0 # m — slack fades to 0 as the cap closes. The solver aims at + # tracked, so slack held near the line is braking for a stop bar + # that far past the real one. # Knob bounds (mirror of UI slider; defense in depth) OFFSET_FT_MIN = -20 @@ -183,9 +191,11 @@ class StarPilotVCruise: self.force_stop_from_light = False self.force_stop_light_clear_since = None self.controls_enabled_previously = False + self.approach_stop_length = 0.0 # published as starpilotPlan.approachStopLength # Kinematic distance estimator. Same attribute also published as # starpilotPlan.forcingStopLength, so the existing reader keeps working. self.tracked_model_length = 0.0 + self.force_stop_distance_cap = 0.0 # odometry ceiling, re-seeded until commit self.stop_sign_confirmed = False self.stop_seen_on_approach_at = None @@ -637,6 +647,9 @@ class StarPilotVCruise: offset_ft = max(OFFSET_FT_MIN, min(OFFSET_FT_MAX, offset_ft_raw)) offset_m = offset_ft * FT_TO_M + # cleared on every path; only the far-approach envelope below republishes it + self.approach_stop_length = 0.0 + if force_standstill_enabled and not self.override_force_standstill: self.forcing_stop = True self.tracked_model_length = 0.0 @@ -669,6 +682,12 @@ class StarPilotVCruise: self.tracked_model_length = model_length else: self.tracked_model_length = min(self.tracked_model_length, model_length) + # Odometry ceiling: the line can't recede, so a re-anchor may never exceed what we + # had at commit minus what we've driven. Bounds a ballooning horizon (seen +95 m) + # that the REANCHOR_MIN floor can't catch, since that floor trusts the estimate. + self.force_stop_distance_cap = max(self.force_stop_distance_cap - (v_ego * DT_MDL), 0.0) + cap_slack = FORCE_STOP_CAP_SLACK_M * min(self.force_stop_distance_cap / FORCE_STOP_CAP_TAPER_M, 1.0) + self.tracked_model_length = min(self.tracked_model_length, self.force_stop_distance_cap + cap_slack) if dash_active: if model_length < DASH_MODEL_AGREE_M: self.tracked_model_length = min(self.tracked_model_length, DASH_SEED_M) @@ -701,6 +720,7 @@ class StarPilotVCruise: self.stop_sign_confirmed = False self.tracked_model_length = self.starpilot_planner.model_length + self.force_stop_distance_cap = self.tracked_model_length targets = [v_cruise] if self.csc_target >= CSC_MIN_SPEED: @@ -746,6 +766,8 @@ class StarPilotVCruise: adjacent_stop_d = self._get_adjacent_stop_distance(sm) if adjacent_stop_d is not None: approach_d = min(approach_d, adjacent_stop_d) + # pre-offset, so it hands off to forcingStopLength at commit without a step + self.approach_stop_length = max(approach_d, 0.0) approach_d += offset_m + force_stop_distance_bias_m if approach_d > force_stop_handoff_m: targets.append(math.sqrt(2.0 * FORCE_STOP_APPROACH_DECEL * (approach_d - force_stop_handoff_m))) diff --git a/starpilot/controls/starpilot_planner.py b/starpilot/controls/starpilot_planner.py index af009abc2..0366862ad 100644 --- a/starpilot/controls/starpilot_planner.py +++ b/starpilot/controls/starpilot_planner.py @@ -31,7 +31,10 @@ from openpilot.starpilot.controls.lib.starpilot_vcruise import StarPilotVCruise from openpilot.starpilot.controls.lib.weather_checker import WeatherChecker RADARLESS_TRACK_HOLD_TIME = 0.45 -FORCE_STOP_JERK_SCALE = 0.32 # accel-change cost multiplier while forcing_stop (125 -> ~40) +FORCE_STOP_JERK_SCALE = 0.20 # accel-change cost multiplier for the whole stop approach, + # envelope included (125 -> 25). Lower = reaches the braking + # target sooner; it does not make the target deeper. Response + # is super-linear here, so raise it if onset feels like a step. FORCE_STOP_JERK_SCALE_OVERRIDES = { # The Elantra's current force-stop ramp is smooth, but it waits too long # before building decel and then arrives at the initial brake too abruptly. @@ -307,7 +310,9 @@ class StarPilotPlanner: except (KeyError, IndexError, TypeError, AttributeError): car_params = None - if self.starpilot_vcruise.forcing_stop: + # Also while the far-approach envelope is running: at onset the ramp reaches only + # ~-0.5 m/s^2 after a second, so the first seconds of a detected red are mostly lost. + if self.starpilot_vcruise.forcing_stop or self.starpilot_vcruise.approach_stop_length > 0.0: jerk_scale = get_force_stop_jerk_scale(car_params) elif self.tracking_lead: # Elantra vision leads can hand off from cruise to lead0 while closing @@ -344,6 +349,7 @@ class StarPilotPlanner: starpilotPlan.forcingStop = self.starpilot_vcruise.forcing_stop starpilotPlan.forcingStopLength = self.starpilot_vcruise.tracked_model_length + starpilotPlan.approachStopLength = float(self.starpilot_vcruise.approach_stop_length) starpilotPlan.stopSignConfirmed = self.starpilot_vcruise.stop_sign_confirmed starpilotPlan.starpilotEvents = self.starpilot_events.events.to_msg()