From f22bcfee36538e44d9432fa6e895b0e76aa5a342 Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Mon, 24 Aug 2026 18:38:25 -0500 Subject: [PATCH] Stops on curves false positives --- starpilot/common/starpilot_utilities.py | 18 ++++++++++++++++++ starpilot/controls/lib/starpilot_vcruise.py | 18 +++++++++++++++++- starpilot/controls/starpilot_planner.py | 9 +++++++-- 3 files changed, 42 insertions(+), 3 deletions(-) diff --git a/starpilot/common/starpilot_utilities.py b/starpilot/common/starpilot_utilities.py index efafe8587..46f527e68 100644 --- a/starpilot/common/starpilot_utilities.py +++ b/starpilot/common/starpilot_utilities.py @@ -124,6 +124,24 @@ def calculate_lane_width(lane_line1, lane_line2, road_edge=None): return float(distance_to_lane) +def calculate_model_path_length(model_position): + # Distance the model plans to TRAVEL, measured along the path. + # + # position.x[-1] is only the forward projection of the plan's endpoint in the car's + # frame, so it collapses as soon as the path bends: on a highway sweeper it reads + # ~0.85x the real distance, on an off-ramp ~0.30x. Every "is the model planning a + # stop" test compares this against v_ego * T, so pure geometry reads as a stop. + # Measured on route 78511c37de32c375--9c33d63ad6 (32 min): 546 of 1149 false stop + # detections came from the chord collapse alone, with zero true stops depending on it + # (on a straight approach the two are identical to within 0.3%). + x = np.asarray(model_position.x, dtype=np.float64) + y = np.asarray(model_position.y, dtype=np.float64) + if x.size < 2 or y.size != x.size: + return float(x[-1]) if x.size else 0.0 + + return float(np.sum(np.hypot(np.diff(x), np.diff(y)))) + + # Credit goes to Pfeiferj! def calculate_road_curvature(modelData, v_ego): orientation_rate = np.array(modelData.orientationRate.z) diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index f583ae179..cb20351e4 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -65,6 +65,14 @@ FORCE_STOP_APPROACH_DECEL = 0.65 # m/s^2 — speed ceiling before commit. LOWER # 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 +CEM_MAX_MS = 17.88 # 40 mph — same envelope for the model path. Committing at + # ACTIVATION_M from above this needs >2.4 m/s^2, past the + # 2.3 m/s^2 line where stops start failing, and a hard + # highway-speed decel is what a driver reads as a phantom + # stop. Every real force stop in route + # 78511c37de32c375--9c33d63ad6 armed at <=39 mph; the one + # false commit (off-ramp, 5.2 s of Force Stop) armed at 48. + # The far-approach envelope bleeds speed under this first. DASH_SEED_M = 27.0 # ~88 ft — typical ADAS detection distance, used to snap # tracked length closer when dashboard confirms a sign DASH_MODEL_AGREE_M = 50.0 # m — dash arm/snap needs model_length under this; a lone dash bit @@ -375,9 +383,16 @@ class StarPilotVCruise: lead_present = (bool(getattr(lead, "status", False)) and float(getattr(lead, "dRel", float("inf"))) < lead_veto_m and float(getattr(lead, "vLead", float("inf"))) < v_ego + 2.0) + # stop_then_turn exists so a wheel already pre-wound at a stop line can't veto a + # legit Force Stop. That scene only exists at pre-wind speeds. Unscoped, the override + # made this veto unreachable: stop_then_turn is armed by stop_light_detected on the + # same frame the veto is evaluated, so it was True on 100% of the frames the veto + # was meant to gate (route 78511c37de32c375--9c33d63ad6: 1130 curved detection + # frames, curved_approach_scene fired on 0 of them). + prewind_scene = stop_then_turn and v_ego <= FORCE_STOP_TURN_VETO_MAX_SPEED curved_approach_scene = ( abs(float(getattr(self.starpilot_planner, "road_curvature", 0.0))) >= FORCE_STOP_CURVE_VETO_MAX_ROAD_CURVATURE - and not stop_then_turn + and not prewind_scene ) # CEM/model path: model predicted stop within ACTIVATION_M. @@ -397,6 +412,7 @@ class StarPilotVCruise: cem_path = (stop_light_detected and controls_enabled and starpilot_toggles.force_stops and model_length_active + and v_ego < CEM_MAX_MS and self.override_force_stop_timer <= 0 and not self.starpilot_planner.driving_in_curve and not curved_approach_scene diff --git a/starpilot/controls/starpilot_planner.py b/starpilot/controls/starpilot_planner.py index 69478f905..e844c08f9 100644 --- a/starpilot/controls/starpilot_planner.py +++ b/starpilot/controls/starpilot_planner.py @@ -20,7 +20,12 @@ from openpilot.selfdrive.controls.lib.lead_behavior import ( from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, DANGER_ZONE_COST, J_EGO_COST, STOP_DISTANCE from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import get_lead_follow_jerk_scale -from openpilot.starpilot.common.starpilot_utilities import calculate_lane_width, calculate_road_curvature, extract_curve_profile +from openpilot.starpilot.common.starpilot_utilities import ( + calculate_lane_width, + calculate_model_path_length, + calculate_road_curvature, + extract_curve_profile, +) from openpilot.starpilot.common.starpilot_variables import CRUISING_SPEED, MINIMUM_LATERAL_ACCELERATION, PLANNER_TIME, THRESHOLD from openpilot.starpilot.controls.lib.conditional_chill_mode import ConditionalChillMode from openpilot.starpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode @@ -205,8 +210,8 @@ class StarPilotPlanner: self.CS_prev_left_blinker = CS.leftBlinker self.CS_prev_right_blinker = CS.rightBlinker - self.model_length = sm["modelV2"].position.x[-1] model_position = sm["modelV2"].position + self.model_length = calculate_model_path_length(model_position) model_path_y = getattr(model_position, "y", []) if len(model_path_y) == len(model_position.x): self.lead_path_y = float(np.interp(self.lead_one.dRel, model_position.x, model_path_y))