From 5f41dc6a644cf97f0ff31974083d3f6d780b5f16 Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Sat, 1 Aug 2026 12:55:23 -0500 Subject: [PATCH] Force Stop Tweaks --- cereal/custom.capnp | 10 +++ .../lib/longitudinal_mpc_lib/long_mpc.py | 10 ++- .../controls/lib/longitudinal_planner.py | 11 ++- selfdrive/controls/radard.py | 86 ++++++++++++++++++ starpilot/controls/lib/starpilot_vcruise.py | 88 +++++++++++++++++-- starpilot/controls/starpilot_planner.py | 6 +- 6 files changed, 202 insertions(+), 9 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 5762d1644..352dbb1bc 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -224,6 +224,16 @@ struct StarPilotPlan @0xf98d843bfd7004a3 { struct StarPilotRadarState @0xb86e6369214c01c8 { leadLeft @0 :LeadData; leadRight @1 :LeadData; + adjacentStopped @2 :AdjacentStopped; + + # A vehicle in an adjacent lane that was observed MOVING and then came to rest. + # Distinct from leadLeft/leadRight, which are moving-target-only by design. + struct AdjacentStopped { + status @0 :Bool; + dRel @1 :Float32; + yRel @2 :Float32; + radarTrackId @3 :Int32 = -1; + } struct LeadData { dRel @0 :Float32; diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 2e806b88a..802397003 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -835,7 +835,8 @@ class LongitudinalMpc: def update(self, radarstate, v_cruise, x, v, a, j, danger_factor, t_follow, personality=log.LongitudinalPersonality.standard, tracking_lead=True, - optional_far_lead_comfort=True, smooth_duplicate_vision=False): + optional_far_lead_comfort=True, smooth_duplicate_vision=False, + stop_x=None): v_ego = self.x0[1] lead_one = radarstate.leadOne lead_two = radarstate.leadTwo @@ -890,6 +891,11 @@ class LongitudinalMpc: lead_0_bias, lead_1_bias = self.get_near_duplicate_lead_source_hysteresis(prev_source, lead_one, lead_two, v_ego) lead_0_obstacle = lead_0_obstacle + lead_0_bias lead_1_obstacle = lead_1_obstacle + lead_1_bias + # A forced stop is a position constraint, not a speed one. Folded in here rather than + # as a 4th column so the SOURCES[argmin] below keeps working. Lets the solver plan the + # stop directly instead of chasing a descending speed ceiling with a persistent lag. + if stop_x is not None: + cruise_obstacle = np.minimum(cruise_obstacle, stop_x) x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) candidate_source = SOURCES[np.argmin(x_obstacles[0])] sticky_source = None @@ -939,6 +945,8 @@ class LongitudinalMpc: xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1]) x = np.cumsum(np.insert(xforward, 0, x[0])) + if stop_x is not None: + cruise_target = np.minimum(cruise_target, stop_x) x_and_cruise = np.column_stack([x, cruise_target]) x = np.min(x_and_cruise, axis=1) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 02a4a115e..b18b8c466 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -3095,11 +3095,20 @@ class LongitudinalPlanner: dec_mpc_mode = self.get_mpc_mode() if not self.mlsim: self.mpc.mode = dec_mpc_mode + # Hand the forced stop to the solver as a position. The obstacle sits STOP_DISTANCE + # beyond the line because the safe-distance term already includes it — placing it on + # the line parks us short. Below that the existing v_cruise=0 path finishes the stop, + # since forcingStopLength is decaying to zero and the obstacle would land behind us. + force_stop_x = None + if sm['starpilotPlan'].forcingStop and sm['starpilotPlan'].forcingStopLength > STOP_DISTANCE: + force_stop_x = float(sm['starpilotPlan'].forcingStopLength) + STOP_DISTANCE + self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, sm['starpilotPlan'].dangerFactor, effective_t_follow, personality=personality, tracking_lead=lead_control_active, optional_far_lead_comfort=True, - smooth_duplicate_vision=nonurgent_duplicate_vision_follow and not panic_bypass) + smooth_duplicate_vision=nonurgent_duplicate_vision_follow and not panic_bypass, + stop_x=force_stop_x) self.a_desired_trajectory_full = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 3e2c58bde..f9cb6d3f3 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -30,6 +30,18 @@ RADAR_TO_CAMERA = 1.52 # RADAR is ~ 1.5m ahead from center of mesh frame G90_RADAR_LOW_SPEED_MAX_DIST = 12.0 G90_RADAR_LOW_SPEED_MAX_Y = 0.6 +# Adjacent-lane stopped-vehicle detector, used as a stop-line hint on red-light +# approaches. The qualifier is the DECELERATION HISTORY, not the current speed: roadside +# furniture and curb-parked cars never show a moving -> stopped transition, so testing +# for "anything slow in the next lane" instead would brake us early for parked cars. +ADJACENT_STOP_MOVING_V = 5.0 # m/s — must have genuinely been moving +ADJACENT_STOP_REST_V = 1.5 # m/s — and then genuinely at rest +ADJACENT_STOP_MOVING_FRAMES = 15 # 0.75 s at 20 Hz, both ways: rejects speed noise +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 + class KalmanParams: def __init__(self, dt: float): @@ -62,6 +74,11 @@ class Track: self.leadTrackID = 0 + # deceleration history for the adjacent-lane stopped-vehicle detector + self.moving_frames = 0 + self.rest_frames = 0 + self.seen_moving = False + def update(self, d_rel: float, y_rel: float, v_rel: float, v_lead: float, measured: float): # relative values, copy self.dRel = d_rel # LONG_DIST @@ -83,6 +100,21 @@ class Track: else: self.aLeadTau.update(0.0) + # Track the moving -> stopped transition. Only sustained runs count, so one noisy + # speed sample can neither arm nor trip the detector. + if self.vLead > ADJACENT_STOP_MOVING_V: + self.moving_frames += 1 + self.rest_frames = 0 + if self.moving_frames >= ADJACENT_STOP_MOVING_FRAMES: + self.seen_moving = True + elif abs(self.vLead) < ADJACENT_STOP_REST_V: + self.moving_frames = 0 + self.rest_frames += 1 + else: + # coasting between the two bands: hold state, restart both runs + self.moving_frames = 0 + self.rest_frames = 0 + self.cnt += 1 def get_RadarState(self, model_prob: float = 0.0): @@ -111,6 +143,31 @@ class Track: right_lane = np.interp(self.dRel, model_data.laneLines[2].x, model_data.laneLines[2].y) return -self.yRel > right_lane + def is_adjacent_stopped(self, model_data: capnp._DynamicStructReader): + """A neighbouring-lane vehicle that was seen moving and has now come to rest. + + Deliberately not potential_adjacent_lead, which is moving-target-only and would have + to be loosened to "anything slow" to catch these. Lane geometry mirrors it (model + y == -yRel, laneLines[1] left boundary and [2] right), plus an outer bound so + roadside returns past the neighbouring lane don't qualify. + """ + if not (self.seen_moving and self.rest_frames >= ADJACENT_STOP_REST_FRAMES): + return False + + if self.leadTrackID == self.identifier: + return False + + if not (ADJACENT_STOP_MIN_Y < abs(self.yRel) < ADJACENT_STOP_MAX_Y): + return False + + if not (0.0 < self.dRel < ADJACENT_STOP_MAX_D): + return False + + model_y = -self.yRel + left_lane = np.interp(self.dRel, model_data.laneLines[1].x, model_data.laneLines[1].y) + right_lane = np.interp(self.dRel, model_data.laneLines[2].x, model_data.laneLines[2].y) + return bool(model_y < left_lane or model_y > right_lane) + def potential_low_speed_lead(self, v_ego: float): # stop for stuff in front of you and low speed, even without model confirmation # Radar points closer than 0.75, are almost always glitches on toyota radars @@ -270,6 +327,29 @@ def get_adjacent_lead(tracks: dict[int, Track], standstill: bool, model_data: ca return lead_dict +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. + """ + if len(model_data.laneLines) < 4: + return {'status': False} + + candidates = [c for c in tracks.values() if c.is_adjacent_stopped(model_data)] + if not candidates: + return {'status': False} + + furthest = max(candidates, key=lambda c: c.dRel) + return { + 'status': True, + 'dRel': float(furthest.dRel), + 'yRel': float(furthest.yRel), + 'radarTrackId': int(furthest.identifier), + } + + class RadarD: def __init__(self, radar_ts: float = DT_MDL, delay: float = 0.0, g90_radar_filter: bool = False): self.current_time = 0.0 @@ -360,6 +440,12 @@ class RadarD: self.starpilot_radar_state.leadLeft = get_adjacent_lead(self.tracks, sm['carState'].standstill, sm['modelV2'], left=True) self.starpilot_radar_state.leadRight = get_adjacent_lead(self.tracks, sm['carState'].standstill, sm['modelV2'], left=False) + # Not gated on the adjacent-lead toggles: this is a separate signal with a separate + # consumer (Force Stop), and leaving leadLeft/leadRight untouched keeps existing + # lane-change and UI behaviour unchanged. + if self.ready: + self.starpilot_radar_state.adjacentStopped = get_adjacent_stopped(self.tracks, sm['modelV2']) + self.starpilot_toggles = get_starpilot_toggles(sm) def publish(self, pm: messaging.PubMaster): diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index de73c8eb6..f0d52dbe2 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -37,12 +37,24 @@ NAV_TURN_TARGET_SPEEDS = { # Smaller values pull speed down earlier on approach. FORCE_STOP_MODEL_APPROACH_DECEL = 0.65 FORCE_STOP_DASH_APPROACH_DECEL = 1.0 -ACTIVATION_M = 75.0 # m — CEM/model path activates when model_length < this +ACTIVATION_M = 75.0 # m — CEM/model path activates when model_length < this. + # Don't raise: forcing_stop latches until standstill, so a brief + # red-light blip at longer range commits to a stop we can't release. +ACTIVATION_HYSTERESIS_M = 8.0 # m — release margin; absorbs model_length jitter at the gate +LEAD_VETO_M = 75.0 # m — lead proximity that vetoes Force Stop (kept off ACTIVATION_M + # so raising activation can't silently widen the veto) MPC_HANDOFF_M = 6.0 # m — below this, command 0 and let MPC finish the stop +FORCE_STOP_APPROACH_DECEL = 0.75 # m/s^2 — speed ceiling before commit. LOWER = more early + # braking. Must stay above FORCE_STOP_MODEL_APPROACH_DECEL or the + # pre-commit ceiling is stricter than the stop itself. 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 +DASH_MODEL_AGREE_M = 50.0 # m — dash arm/snap needs model_length under this; a lone dash bit + # against a long model path is a phantom stop FT_TO_M = 0.3048 +ADJACENT_STOP_MIN_USE_M = 10.0 # m — inside this the MPC already owns the stop; a + # late-arriving hint could only jerk it FORCE_STOP_TURN_VETO_MAX_SPEED = 18.0 * CV.MPH_TO_MS # Real-turn steering angle. A stop-then-turn is still ~straight on approach, so a low # threshold caused legit stops to be skipped when the blinker came on early. Only suppress @@ -136,6 +148,7 @@ class StarPilotVCruise: self.override_force_stop_timer = 0 self.force_stop_timer = 0.0 + self.activation_gate_active = False self.standstill_force_stop_hold = False self.standstill_force_stop_clear_since = 0.0 self.standstill_force_stop_started_at = None @@ -190,6 +203,26 @@ class StarPilotVCruise: self.standstill_force_stop_started_at = None self.standstill_force_stop_reason = None + @staticmethod + def _get_adjacent_stop_distance(sm): + """dRel of a vehicle that decelerated to a stop in an adjacent lane, or None. + + The model's own distance runs long on a clear-lane approach; a car stopped alongside + is physically at (or just behind) the stop bar. Radar-only, so it holds for any + driving model. + """ + try: + radar_state = sm["starpilotRadarState"] + except (KeyError, IndexError, TypeError, AttributeError): + return None + + adjacent = getattr(radar_state, "adjacentStopped", None) + if adjacent is None or not getattr(adjacent, "status", False): + return None + + d_rel = float(getattr(adjacent, "dRel", 0.0)) + return d_rel if d_rel > ADJACENT_STOP_MIN_USE_M else None + @staticmethod def _nav_maneuver_target_speed(maneuver_type, maneuver_modifier): maneuver_type = str(maneuver_type or "").strip().lower() @@ -296,7 +329,7 @@ class StarPilotVCruise: # during the filter's settling window and stay committed for the whole stop. lead = self.starpilot_planner.lead_one lead_present = (bool(getattr(lead, "status", False)) - and float(getattr(lead, "dRel", float("inf"))) < ACTIVATION_M + and float(getattr(lead, "dRel", float("inf"))) < LEAD_VETO_M and float(getattr(lead, "vLead", float("inf"))) < v_ego + 2.0) curved_approach_scene = ( abs(float(getattr(self.starpilot_planner, "road_curvature", 0.0))) >= FORCE_STOP_CURVE_VETO_MAX_ROAD_CURVATURE @@ -307,9 +340,19 @@ class StarPilotVCruise: # Exclude when a lead is present (raw or filtered) — the handoff_to_stopped_lead path # in CEM can set stop_light_detected even with a lead present, which would incorrectly # activate Force Stop and stop the car far behind the lead instead of letting ACC handle it. - cem_path = (self.starpilot_planner.starpilot_cem.stop_light_detected + # Schmitt trigger: model_length jitters around ACTIVATION_M and keeps resetting + # force_stop_timer's ramp. Scoped to a detected stop so the wider release threshold + # can't leak into ordinary slow driving. + stop_light_detected = self.starpilot_planner.starpilot_cem.stop_light_detected + if self.activation_gate_active and stop_light_detected: + model_length_active = self.starpilot_planner.model_length < ACTIVATION_M + ACTIVATION_HYSTERESIS_M + else: + model_length_active = self.starpilot_planner.model_length < ACTIVATION_M + self.activation_gate_active = model_length_active and stop_light_detected + + cem_path = (stop_light_detected and controls_enabled and starpilot_toggles.force_stops - and self.starpilot_planner.model_length < ACTIVATION_M + and model_length_active and self.override_force_stop_timer <= 0 and not self.starpilot_planner.driving_in_curve and not curved_approach_scene @@ -323,6 +366,7 @@ class StarPilotVCruise: dash_active = dash_value > 0 dash_path = (dash_active and controls_enabled and starpilot_toggles.force_stops and v_ego < ADAS_MAX_MS + and self.starpilot_planner.model_length < DASH_MODEL_AGREE_M and self.override_force_stop_timer <= 0 and not self.starpilot_planner.driving_in_curve and not turn_scene_active @@ -488,11 +532,22 @@ class StarPilotVCruise: # Kinematic distance estimator (also published as forcingStopLength). # Decay one-to-one with motion, clamp by current model_length so we adopt # the model's view when it regains sight, and snap closer to DASH_SEED_M - # whenever the dashboard signal is active. + # when the dashboard signal is active and the model agrees a stop is near. self.tracked_model_length = max(self.tracked_model_length - (v_ego * DT_MDL), 0.0) self.tracked_model_length = min(self.tracked_model_length, self.starpilot_planner.model_length) if dash_active: - self.tracked_model_length = min(self.tracked_model_length, DASH_SEED_M) + if self.starpilot_planner.model_length < DASH_MODEL_AGREE_M: + self.tracked_model_length = min(self.tracked_model_length, DASH_SEED_M) + # inside the seed the model range is the better line estimate; letting it pull + # tracked back up is what keeps an early snap from parking us short of the sign + if self.starpilot_planner.model_length < DASH_SEED_M: + self.tracked_model_length = self.starpilot_planner.model_length + + # A car stopped in the next lane marks the stop bar better than the model does. + # Shortening clamp only — it can pull the stop in, never push it out. + adjacent_stop_d = self._get_adjacent_stop_distance(sm) + if adjacent_stop_d is not None: + self.tracked_model_length = min(self.tracked_model_length, adjacent_stop_d) # Kinematic profile with user offset. Positive offset shifts the perceived # line further down the road -> car rolls further before commanding 0. @@ -538,6 +593,27 @@ class StarPilotVCruise: targets.append(slc_control_target) if self.nav_turn_target > 0.0: targets.append(self.nav_turn_target) + + # Far-approach envelope: bleed speed off before commit so the car isn't still at + # cruise when the kinematic curve takes over. Same vetoes as the activation paths; + # no latch, recomputed each frame, releases on green. + if (stop_light_detected + and controls_enabled and starpilot_toggles.force_stops + and self.override_force_stop_timer <= 0 + and not self.starpilot_planner.driving_in_curve + and not curved_approach_scene + and not turn_scene_active + and not self.starpilot_planner.tracking_lead + and not lead_present): + # adjacent-stopped hint caps the model distance; shorten-only, self-clearing + approach_d = self.starpilot_planner.model_length + adjacent_stop_d = self._get_adjacent_stop_distance(sm) + if adjacent_stop_d is not None: + approach_d = min(approach_d, adjacent_stop_d) + approach_d += offset_m + if approach_d > MPC_HANDOFF_M: + targets.append(math.sqrt(2.0 * FORCE_STOP_APPROACH_DECEL * (approach_d - MPC_HANDOFF_M))) + v_cruise = min(targets) self.controls_enabled_previously = controls_enabled diff --git a/starpilot/controls/starpilot_planner.py b/starpilot/controls/starpilot_planner.py index 719a5e3dc..c712c26dc 100644 --- a/starpilot/controls/starpilot_planner.py +++ b/starpilot/controls/starpilot_planner.py @@ -30,6 +30,7 @@ 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) def _sanitize_json_value(value): @@ -282,7 +283,10 @@ class StarPilotPlanner: starpilot_plan_send.valid = sm.all_checks(service_list=["carState", "controlsState", "selfdriveState", "radarState"]) starpilotPlan = starpilot_plan_send.starpilotPlan - starpilotPlan.accelerationJerk = float(A_CHANGE_COST * self.starpilot_following.acceleration_jerk) + # While committed to a Force Stop, cut the MPC's accel-change penalty so terminal + # braking can ramp faster. 0.32 lands near 40, what long_mpc uses in blended mode. + jerk_scale = FORCE_STOP_JERK_SCALE if self.starpilot_vcruise.forcing_stop else 1.0 + starpilotPlan.accelerationJerk = float(A_CHANGE_COST * self.starpilot_following.acceleration_jerk * jerk_scale) starpilotPlan.dangerFactor = float(self.starpilot_following.danger_factor) starpilotPlan.dangerJerk = float(DANGER_ZONE_COST * self.starpilot_following.danger_jerk) starpilotPlan.speedJerk = float(J_EGO_COST * self.starpilot_following.speed_jerk)