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/radard.py b/selfdrive/controls/radard.py index 3e2c58bde..3ba0364af 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -30,6 +30,22 @@ 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: a target +# that was seen moving and then came to rest is self-identifying, while roadside +# furniture and curb-parked cars never show that transition. Simply accepting "anything +# slow in the next lane" would pick up parked cars and brake us early. +# Measured over 39 red-light approaches (Desktop/stops corpus): present on ~10% of stops, +# dRel reads a mean 6.3 m SHORT of the true stop line (worst case 13.7 m short, a car +# queued behind the bar), and first appears a median 53 m out. +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 +78,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 +104,21 @@ class Track: else: self.aLeadTau.update(0.0) + # Track the moving -> stopped transition. Only a sustained run counts, so a single + # 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 +147,33 @@ 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 the same test as potential_adjacent_lead, which is moving-target-only + and would have to be loosened to "anything slow" to catch these — that would also + catch curb-parked cars. Requiring the moving -> stopped transition keeps the signal + self-validating. Lane geometry mirrors potential_adjacent_lead (model y == -yRel, + laneLines[1] the left boundary and laneLines[2] the 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 +333,31 @@ 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, not the nearest. In a queue the front car sits + at the stop bar and the ones behind it are progressively closer to us, so the nearest + candidate systematically underestimates the distance to the line. Since the consumer + only ever uses this to SHORTEN its estimate, underestimating is the harmful direction — + picking the farthest degrades toward doing nothing instead. + """ + 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 +448,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 the existing + # lane-change and UI behaviour exactly as it was. + 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 7dd483b16..346dd4fec 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -54,6 +54,16 @@ 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 FT_TO_M = 0.3048 +# Distance below which the adjacent-stopped hint is ignored — inside this the MPC and the +# dash/model paths already own the stop, and a late-arriving hint could only jerk it. +ADJACENT_STOP_MIN_USE_M = 10.0 +# NOTE: model_length is long-biased (+12.3 m) and swings ~15 m within one approach, so +# filtering it before the sqrt below looks attractive. It was tried and measured over 39 +# stops and it is WORSE — mean ceiling error vs the ideal went 1.20 -> 1.22 m/s (median), +# 1.31 (lowpass 0.5 s), 1.57 (lowpass 1.5 s), 6.91 (odometry decay + only-shorten). The +# sqrt compresses distance error hard, so +12 m of distance is only ~1.1 m/s of ceiling, +# and model_length is a fast-moving signal — any lag a filter adds costs more than the +# noise it removes. Leave the raw value alone. 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 @@ -202,6 +212,28 @@ 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. + + On a clear-lane red-light approach the model's own distance runs long, while a car + stopped alongside is physically at (or just behind) the stop bar. Measured over 39 + approaches: present on ~10% of stops, reads a mean 6.3 m short of the true line, and + arrives a median 53 m out — inside the armed window. Radar-only, so this is + independent of whichever driving model is loaded. + """ + 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() @@ -516,6 +548,14 @@ class StarPilotVCruise: if dash_active: self.tracked_model_length = min(self.tracked_model_length, DASH_SEED_M) + # A car that decelerated to a stop in the next lane is a physical marker for the + # stop bar, and a far better one than the model's own estimate. Applied as one + # more shortening clamp, which is the direction this estimator already only + # moves in — 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. effective_d = self.tracked_model_length + offset_m