From 52eb586c65a39ea1e1ff259378bafcfe66cbbfef Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Mon, 24 Aug 2026 20:03:54 -0500 Subject: [PATCH] Revert "Stops on curves false positives" This reverts commit f22bcfee36538e44d9432fa6e895b0e76aa5a342. --- starpilot/common/starpilot_utilities.py | 18 ------------------ starpilot/controls/lib/starpilot_vcruise.py | 18 +----------------- starpilot/controls/starpilot_planner.py | 9 ++------- 3 files changed, 3 insertions(+), 42 deletions(-) diff --git a/starpilot/common/starpilot_utilities.py b/starpilot/common/starpilot_utilities.py index 46f527e68..efafe8587 100644 --- a/starpilot/common/starpilot_utilities.py +++ b/starpilot/common/starpilot_utilities.py @@ -124,24 +124,6 @@ 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 cb20351e4..f583ae179 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -65,14 +65,6 @@ 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 @@ -383,16 +375,9 @@ 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 prewind_scene + and not stop_then_turn ) # CEM/model path: model predicted stop within ACTIVATION_M. @@ -412,7 +397,6 @@ 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 e844c08f9..69478f905 100644 --- a/starpilot/controls/starpilot_planner.py +++ b/starpilot/controls/starpilot_planner.py @@ -20,12 +20,7 @@ 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_model_path_length, - calculate_road_curvature, - extract_curve_profile, -) +from openpilot.starpilot.common.starpilot_utilities import calculate_lane_width, 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 @@ -210,8 +205,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))