IQ.Pilot Release Commit @ cd83f5a

2026-08-23 11:32:03 -05:00
parent 58039e647c
commit a80e124cb8
116 changed files with 1657 additions and 4066 deletions
--- a/iqpilot/selfdrive/controls/lib/helpers/lateral_edge_guard.py
+++ b/iqpilot/selfdrive/controls/lib/helpers/lateral_edge_guard.py
@@ -2,14 +2,16 @@
 Lateral Edge Guard uses the model's lateral road-edge geometry to withhold lane
 changes that lack room for a target lane. The model standard deviation remains
 in metres: measurements above the validity limit are rejected, while valid
-measurements use a two-sigma lower confidence bound for conservative clearance.
+measurements use a one-sigma lower confidence bound for conservative clearance.
 Unavailable geometry briefly holds the last output, then fails open because a
-model dropout is not geometric evidence of a nearby edge.
+model dropout is not geometric evidence of a nearby edge. A visible outer lane
+line on the target side is direct evidence that a lane exists and overrides the
+edge-distance inference.
 """
 from __future__ import annotations

 import math
-from dataclasses import dataclass
+from dataclasses import dataclass, replace
 from enum import IntEnum
 from typing import Any

@@ -20,13 +22,22 @@ from iqpilot.common.swaglog import cloudlog

 MIN_ACTIVE_SPEED_MPS = 20.0 * CV.MPH_TO_MS  # Matches the lane-change speed gate and excludes parking manoeuvres.
 MAX_VALID_ROAD_EDGE_STD_M = 1.0  # A 2-sigma bound beyond 2 m cannot distinguish an adjacent 3.5 m lane reliably.
-EDGE_CONFIDENCE_SIGMA = 2.0  # 97.7% one-sided confidence under the model's Gaussian uncertainty assumption.
+# roadEdgeStd describes a single edge point, but it is applied to a 5-40 m minimum that already absorbs the
+# spatial worst case; 1 sigma covers ~1.1x the measured p99 frame-to-frame spread of that minimum, 2 sigma 2.2x.
+EDGE_CONFIDENCE_SIGMA = 1.0
 ROAD_EDGE_LOOKAHEAD_MIN_M = 5.0  # Ignore near-field edge points dominated by vehicle-body perspective.
 ROAD_EDGE_LOOKAHEAD_MAX_M = 40.0  # Covers about 2 s at the 20 m/s model-training reference speed.
 LANE_CENTER_OFFSET_M = 3.5  # Typical freeway lane width and the target-centre lateral displacement.
 # CarParams exposes neither width nor track; 0.95 m is half of an assumed conservative 1.90 m body width.
 VEHICLE_LATERAL_HALF_WIDTH_M = 1.90 / 2.0
 EDGE_CLEARANCE_MARGIN_M = 0.25  # Additional lateral separation between the vehicle body and detected road edge.
+ADJACENT_LANE_LINE_PROB = 0.5
+EGO_LANE_LINE_PROB_MIN = 0.5
+MIN_MEASURED_LANE_WIDTH_M = 2.5
+MAX_MEASURED_LANE_WIDTH_M = 4.5
+# modelV2 lane lines are ordered outer-left, ego-left, ego-right, outer-right.
+OUTER_LANE_LINE_INDEX = (0, 3)
+EGO_LANE_LINE_INDEX = (1, 2)
 REQUIRED_ROAD_EDGE_DISTANCE_M = LANE_CENTER_OFFSET_M + VEHICLE_LATERAL_HALF_WIDTH_M + EDGE_CLEARANCE_MARGIN_M
 BLOCK_DEBOUNCE_S = 0.30  # Six model frames reject a transient close-edge prediction before blocking.
 CLEAR_DEBOUNCE_S = 0.50  # Ten model frames make release slower than assertion for conservative hysteresis.
@@ -60,7 +71,8 @@ class _SideState:
  fallback_reported: bool = False


-def evaluate_road_edge(edge: Any, std_m: Any, direction: int) -> RoadEdgeMeasurement:
+def evaluate_road_edge(edge: Any, std_m: Any, direction: int,
+                       lane_width_m: float = LANE_CENTER_OFFSET_M) -> RoadEdgeMeasurement:
  if edge is None or std_m is None:
    return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)

@@ -103,11 +115,12 @@ def evaluate_road_edge(edge: Any, std_m: Any, direction: int) -> RoadEdgeMeasure
    return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)

  conservative_distance_m = lateral_distance_m - EDGE_CONFIDENCE_SIGMA * std
+  required_distance_m = lane_width_m + VEHICLE_LATERAL_HALF_WIDTH_M + EDGE_CLEARANCE_MARGIN_M
  return RoadEdgeMeasurement(
    RoadEdgeDataState.VALID,
    lateral_distance_m,
    conservative_distance_m,
-    conservative_distance_m < REQUIRED_ROAD_EDGE_DISTANCE_M,
+    conservative_distance_m < required_distance_m,
  )


@@ -160,12 +173,60 @@ class LateralEdgeGuard:
    except (AttributeError, TypeError):
      return None, None

+  @staticmethod
+  def _lane_line_prob(modeldata: Any, index: int) -> float | None:
+    if modeldata is None:
+      return None
+    try:
+      probs = modeldata.laneLineProbs
+      if len(probs) <= index:
+        return None
+      value = float(probs[index])
+    except (AttributeError, TypeError, IndexError, ValueError):
+      return None
+    return value if math.isfinite(value) else None
+
+  @classmethod
+  def _adjacent_lane_visible(cls, modeldata: Any, side_index: int) -> bool:
+    prob = cls._lane_line_prob(modeldata, OUTER_LANE_LINE_INDEX[side_index])
+    return prob is not None and prob > ADJACENT_LANE_LINE_PROB
+
+  @classmethod
+  def _measured_lane_width(cls, modeldata: Any) -> float:
+    left_prob = cls._lane_line_prob(modeldata, EGO_LANE_LINE_INDEX[0])
+    right_prob = cls._lane_line_prob(modeldata, EGO_LANE_LINE_INDEX[1])
+    if left_prob is None or right_prob is None:
+      return LANE_CENTER_OFFSET_M
+    if left_prob <= EGO_LANE_LINE_PROB_MIN or right_prob <= EGO_LANE_LINE_PROB_MIN:
+      return LANE_CENTER_OFFSET_M
+    try:
+      lines = modeldata.laneLines
+      left_y = float(lines[EGO_LANE_LINE_INDEX[0]].y[0])
+      right_y = float(lines[EGO_LANE_LINE_INDEX[1]].y[0])
+    except (AttributeError, TypeError, IndexError, ValueError):
+      return LANE_CENTER_OFFSET_M
+    width = abs(right_y - left_y)
+    if not math.isfinite(width):
+      return LANE_CENTER_OFFSET_M
+    return min(max(width, MIN_MEASURED_LANE_WIDTH_M), MAX_MEASURED_LANE_WIDTH_M)
+
+  @staticmethod
+  def _apply_lane_evidence(measurement: RoadEdgeMeasurement, lane_visible: bool) -> RoadEdgeMeasurement:
+    if lane_visible and measurement.state == RoadEdgeDataState.VALID and measurement.should_block:
+      return replace(measurement, should_block=False)
+    return measurement
+
  def update(self, modeldata: Any, v_ego_mps: float, dt_s: float) -> None:
    dt = max(float(dt_s), 0.0)
    left_edge, left_std = self._model_side(modeldata, 0)
    right_edge, right_std = self._model_side(modeldata, 1)
-    self.left_measurement = evaluate_road_edge(left_edge, left_std, LaneChangeDirection.left)
-    self.right_measurement = evaluate_road_edge(right_edge, right_std, LaneChangeDirection.right)
+    lane_width_m = self._measured_lane_width(modeldata)
+    self.left_measurement = self._apply_lane_evidence(
+      evaluate_road_edge(left_edge, left_std, LaneChangeDirection.left, lane_width_m),
+      self._adjacent_lane_visible(modeldata, 0))
+    self.right_measurement = self._apply_lane_evidence(
+      evaluate_road_edge(right_edge, right_std, LaneChangeDirection.right, lane_width_m),
+      self._adjacent_lane_visible(modeldata, 1))
    speed_active = math.isfinite(v_ego_mps) and v_ego_mps >= MIN_ACTIVE_SPEED_MPS
    self._left, left_fallback = step_side_guard(self._left, self.left_measurement, speed_active, dt)
    self._right, right_fallback = step_side_guard(self._right, self.right_measurement, speed_active, dt)
--- a/iqpilot/selfdrive/controls/lib/tests/test_lateral_edge_guard.py
+++ b/iqpilot/selfdrive/controls/lib/tests/test_lateral_edge_guard.py
@@ -9,12 +9,16 @@ from iqpilot.common.realtime import DT_MDL
 from iqpilot.selfdrive.controls.lib.desire_helper import DesireHelper
 from iqpilot.selfdrive.controls.lib.helpers.lane_change import AutoLaneChangeMode
 from iqpilot.selfdrive.controls.lib.helpers.lateral_edge_guard import (
+  ADJACENT_LANE_LINE_PROB,
  BLOCK_DEBOUNCE_S,
  CLEAR_DEBOUNCE_S,
  MAX_VALID_ROAD_EDGE_STD_M,
  MIN_ACTIVE_SPEED_MPS,
  REQUIRED_ROAD_EDGE_DISTANCE_M,
  UNAVAILABLE_HOLD_S,
+  LANE_CENTER_OFFSET_M,
+  MAX_MEASURED_LANE_WIDTH_M,
+  MIN_MEASURED_LANE_WIDTH_M,
  LateralEdgeGuard,
  RoadEdgeDataState,
  evaluate_road_edge,
@@ -35,6 +39,25 @@ class ModelData:
  roadEdgeStds: list[float]


+@dataclass
+class LaneModelData:
+  roadEdges: list[Edge]
+  roadEdgeStds: list[float]
+  laneLines: list[Edge]
+  laneLineProbs: list[float]
+
+
+def lane_model(left_distance_m: float = 4.0, outer_prob: float = 0.0,
+               ego_width_m: float = 3.5, ego_prob: float = 0.9) -> LaneModelData:
+  xs = [5.0, 20.0, 40.0]
+  base = edge_model(left_distance_m, left_distance_m)
+  half = ego_width_m / 2.0
+  lines = [Edge(xs, [-(half + 3.0)] * 3), Edge(xs, [-half] * 3),
+           Edge(xs, [half] * 3), Edge(xs, [half + 3.0] * 3)]
+  return LaneModelData(base.roadEdges, base.roadEdgeStds, lines,
+                       [outer_prob, ego_prob, ego_prob, outer_prob])
+
+
 class CarState:
  def __init__(self, left_blindspot: bool = False) -> None:
    self.vEgo = MIN_ACTIVE_SPEED_MPS + 1.0
@@ -86,11 +109,15 @@ def test_unavailable_and_invalid_are_distinct() -> None:
  assert invalid.should_block is None


-def test_two_sigma_bound_uses_std_in_metres() -> None:
+def test_one_sigma_bound_uses_std_in_metres() -> None:
  measurement = evaluate_road_edge(edge_model(5.0).roadEdges[0], 0.2, log.LaneChangeDirection.left)
  assert measurement.lateral_distance_m == 5.0
-  assert measurement.conservative_distance_m == 4.6
-  assert measurement.should_block is True
+  assert measurement.conservative_distance_m == 4.8
+  assert measurement.should_block is False
+
+  blocking = evaluate_road_edge(edge_model(4.5).roadEdges[0], 0.2, log.LaneChangeDirection.left)
+  assert blocking.conservative_distance_m == 4.3
+  assert blocking.should_block is True


 def test_distance_threshold_on_either_side() -> None:
@@ -193,3 +220,36 @@ def test_published_edge_block_maps_to_distinct_event_and_alert() -> None:
  alert = EVENTS_IQ[event_name][ET.WARNING]
  assert alert.alert_text_1 == "Lane Change Blocked"
  assert alert.alert_text_2 == "Road edge detected"
+
+
+def test_visible_outer_lane_line_overrides_edge_block() -> None:
+  blocking = lane_model(4.0, outer_prob=0.0)
+  guard = LateralEdgeGuard()
+  update_for(guard, blocking, BLOCK_DEBOUNCE_S)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) != custom.IQLateralEdgeBlock.none
+
+  guard = LateralEdgeGuard()
+  update_for(guard, lane_model(4.0, outer_prob=ADJACENT_LANE_LINE_PROB + 0.2), BLOCK_DEBOUNCE_S * 4)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none
+
+
+def test_outer_lane_line_below_threshold_still_blocks() -> None:
+  guard = LateralEdgeGuard()
+  update_for(guard, lane_model(4.0, outer_prob=ADJACENT_LANE_LINE_PROB - 0.1), BLOCK_DEBOUNCE_S)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) != custom.IQLateralEdgeBlock.none
+
+
+def test_narrow_measured_lane_relaxes_required_distance() -> None:
+  narrow = evaluate_road_edge(edge_model(4.3).roadEdges[0], 0.0, log.LaneChangeDirection.left, 3.0)
+  wide = evaluate_road_edge(edge_model(4.3).roadEdges[0], 0.0, log.LaneChangeDirection.left, LANE_CENTER_OFFSET_M)
+  assert narrow.should_block is False
+  assert wide.should_block is True
+
+
+def test_measured_lane_width_is_clamped_and_falls_back() -> None:
+  assert LateralEdgeGuard._measured_lane_width(None) == LANE_CENTER_OFFSET_M
+  assert LateralEdgeGuard._measured_lane_width(edge_model(4.0)) == LANE_CENTER_OFFSET_M
+  assert LateralEdgeGuard._measured_lane_width(lane_model(4.0, ego_prob=0.1)) == LANE_CENTER_OFFSET_M
+  assert LateralEdgeGuard._measured_lane_width(lane_model(4.0, ego_width_m=9.0)) == MAX_MEASURED_LANE_WIDTH_M
+  assert LateralEdgeGuard._measured_lane_width(lane_model(4.0, ego_width_m=0.5)) == MIN_MEASURED_LANE_WIDTH_M
+  assert LateralEdgeGuard._measured_lane_width(lane_model(4.0, ego_width_m=3.2)) == 3.2