IQ.Pilot Release Commit @ dab4674

2026-08-30 14:49:29 -05:00
parent d42df282ae
commit c9629a3603
19 changed files with 812 additions and 66 deletions
--- a/iqpilot/selfdrive/controls/lib/desire_helper.py
+++ b/iqpilot/selfdrive/controls/lib/desire_helper.py
@@ -14,11 +14,13 @@ from iqpilot.selfdrive.controls.lib.helpers.lane_change import (
  AutoLaneChangeMode,
  NavExitLaneChangeController,
 )
+from iqpilot.selfdrive.controls.lib.helpers.lateral_edge_guard import LateralEdgeGuard
 from iqpilot.selfdrive.controls.lib.helpers.lane_turn import IQNavTurnController

 LaneChangeState = log.LaneChangeState
 LaneChangeDirection = log.LaneChangeDirection
 TurnDirection = custom.IQTurnSignalDirection
+LateralEdgeBlock = custom.IQLateralEdgeBlock
 NavManeuverPhase = custom.IQNavState.ManeuverPhase

 LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
@@ -105,6 +107,8 @@ class DesireHelper:
    self.alc = IQLaneSwapController(self)
    self.lane_turn_controller = IQNavTurnController(self)
    self.nav_exit = NavExitLaneChangeController(_read_enable_bsm())
+    self.lateral_edge_guard = LateralEdgeGuard()
+    self.lateral_edge_block = LateralEdgeBlock.none
    self.lane_turn_direction = TurnDirection.none
    self.nav_turn_direction = TurnDirection.none
    self.turn_desire_stop_timer = 0.0
@@ -174,6 +178,8 @@ class DesireHelper:
  def _step_pre_lane_change(self, one_blinker: bool, nav_exit_active: bool, below_speed: bool) -> None:
    self._refresh_requested_direction(one_blinker, nav_exit_active)
    blindspot_detected = _blindspot_matches(self._last_carstate, self.lane_change_direction)
+    self.lateral_edge_block = self.lateral_edge_guard.block_for_direction(self.lane_change_direction)
+    lateral_edge_blocked = self.lateral_edge_block != LateralEdgeBlock.none
    steering_ready = _steering_nudge_matches(self._last_carstate, self.lane_change_direction)
    nav_auto_start = nav_exit_active and self.nav_exit.auto_allowed

@@ -182,7 +188,7 @@ class DesireHelper:

    if (not (one_blinker or nav_exit_active)) or below_speed:
      self._clear_lane_change()
-    elif allowed_to_launch and not blindspot_detected:
+    elif allowed_to_launch and not blindspot_detected and not lateral_edge_blocked:
      self.lane_change_state = LaneChangeState.laneChangeStarting

  def _step_lane_change_starting(self, lane_change_prob: float) -> None:
@@ -269,6 +275,8 @@ class DesireHelper:

  def update(self, carstate, lateral_active, lane_change_prob, nav_state=None, modeldata=None, radar_state=None):
    self._last_carstate = carstate
+    self.lateral_edge_guard.update(modeldata, carstate.vEgo, DT_MDL)
+    self.lateral_edge_block = LateralEdgeBlock.none
    one_blinker = carstate.leftBlinker != carstate.rightBlinker
    below_speed = carstate.vEgo < LANE_CHANGE_SPEED_MIN
    nav_exit_active = self._refresh_turn_overrides(carstate, nav_state)
--- a/iqpilot/selfdrive/controls/lib/helpers/lateral_edge_guard.py
+++ b/iqpilot/selfdrive/controls/lib/helpers/lateral_edge_guard.py
@@ -0,0 +1,268 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass, replace
+from enum import IntEnum
+from typing import Any
+
+from iqpilot.cereal import custom, log
+from iqpilot.common.constants import CV
+from iqpilot.common.params import Params
+from iqpilot.common.swaglog import cloudlog
+
+
+MIN_ACTIVE_SPEED_MPS = 20.0 * CV.MPH_TO_MS
+MAX_VALID_ROAD_EDGE_STD_M = 1.0
+EDGE_CONFIDENCE_SIGMA = 1.0
+ROAD_EDGE_LOOKAHEAD_MIN_M = 5.0
+ROAD_EDGE_LOOKAHEAD_MAX_M = 40.0
+LANE_CENTER_OFFSET_M = 3.5
+VEHICLE_LATERAL_HALF_WIDTH_M = 1.90 / 2.0
+EDGE_CLEARANCE_MARGIN_M = 0.25
+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
+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
+CLEAR_DEBOUNCE_S = 0.50
+UNAVAILABLE_HOLD_S = 0.50
+TIMER_EPSILON_S = 1e-9
+PARAM_REFRESH_FRAMES = 50
+
+LaneChangeDirection = log.LaneChangeDirection
+LateralEdgeBlock = custom.IQLateralEdgeBlock
+
+
+class RoadEdgeDataState(IntEnum):
+  VALID = 0
+  UNAVAILABLE = 1
+  INVALID = 2
+
+
+@dataclass(frozen=True, slots=True)
+class RoadEdgeMeasurement:
+  state: RoadEdgeDataState
+  lateral_distance_m: float | None = None
+  conservative_distance_m: float | None = None
+  should_block: bool | None = None
+
+
+@dataclass(frozen=True, slots=True)
+class _SideState:
+  blocked: bool = False
+  block_timer_s: float = 0.0
+  clear_timer_s: float = 0.0
+  unavailable_timer_s: float = 0.0
+  fallback_reported: bool = False
+
+
+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)
+
+  try:
+    xs = edge.x
+    ys = edge.y
+    count = len(xs)
+    y_count = len(ys)
+  except (AttributeError, TypeError):
+    return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+
+  if count == 0 or y_count != count:
+    return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+
+  try:
+    std = float(std_m)
+  except (TypeError, ValueError):
+    return RoadEdgeMeasurement(RoadEdgeDataState.INVALID)
+  if not math.isfinite(std) or std < 0.0 or std > MAX_VALID_ROAD_EDGE_STD_M:
+    return RoadEdgeMeasurement(RoadEdgeDataState.INVALID)
+
+  lateral_distance_m: float | None = None
+  for idx in range(count):
+    try:
+      x_m = float(xs[idx])
+      y_m = float(ys[idx])
+    except (IndexError, TypeError, ValueError):
+      return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+    if not math.isfinite(x_m) or not math.isfinite(y_m):
+      return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+    if not ROAD_EDGE_LOOKAHEAD_MIN_M <= x_m <= ROAD_EDGE_LOOKAHEAD_MAX_M:
+      continue
+    if ((direction == LaneChangeDirection.left and y_m >= 0.0) or
+        (direction == LaneChangeDirection.right and y_m <= 0.0)):
+      return RoadEdgeMeasurement(RoadEdgeDataState.INVALID)
+    distance_m = abs(y_m)
+    lateral_distance_m = distance_m if lateral_distance_m is None else min(lateral_distance_m, distance_m)
+
+  if lateral_distance_m is None:
+    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_distance_m,
+  )
+
+
+def step_side_guard(state: _SideState, measurement: RoadEdgeMeasurement, speed_active: bool,
+                    dt_s: float) -> tuple[_SideState, bool]:
+  if not speed_active:
+    return _SideState(), False
+
+  if measurement.state == RoadEdgeDataState.UNAVAILABLE:
+    unavailable_timer_s = state.unavailable_timer_s + dt_s
+    if unavailable_timer_s < UNAVAILABLE_HOLD_S - TIMER_EPSILON_S:
+      return _SideState(state.blocked, unavailable_timer_s=unavailable_timer_s,
+                        fallback_reported=state.fallback_reported), False
+    fallback_started = not state.fallback_reported
+    return _SideState(unavailable_timer_s=unavailable_timer_s, fallback_reported=True), fallback_started
+
+  should_block = bool(measurement.should_block) if measurement.state == RoadEdgeDataState.VALID else False
+  if should_block == state.blocked:
+    return _SideState(blocked=state.blocked), False
+
+  if should_block:
+    block_timer_s = state.block_timer_s + dt_s
+    if block_timer_s >= BLOCK_DEBOUNCE_S - TIMER_EPSILON_S:
+      return _SideState(blocked=True), False
+    return _SideState(block_timer_s=block_timer_s), False
+
+  clear_timer_s = state.clear_timer_s + dt_s
+  if clear_timer_s >= CLEAR_DEBOUNCE_S - TIMER_EPSILON_S:
+    return _SideState(), False
+  return _SideState(blocked=True, clear_timer_s=clear_timer_s), False
+
+
+class LateralEdgeGuard:
+  def __init__(self, enabled: bool | None = None) -> None:
+    self._params = Params() if enabled is None else None
+    self._param_refresh_frame = 0
+    self.enabled = self._read_enabled() if enabled is None else enabled
+    self._active = False
+    self._left = _SideState()
+    self._right = _SideState()
+    self.left_measurement = RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+    self.right_measurement = RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+
+  def _read_enabled(self) -> bool:
+    try:
+      return bool(self._params and self._params.get_bool("IQEdgeGuard"))
+    except Exception:
+      return False
+
+  def _refresh_enabled(self) -> None:
+    if self._params is not None and self._param_refresh_frame % PARAM_REFRESH_FRAMES == 0:
+      self.enabled = self._read_enabled()
+    self._param_refresh_frame += 1
+
+  def _reset(self) -> None:
+    self._left = _SideState()
+    self._right = _SideState()
+    self.left_measurement = RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+    self.right_measurement = RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+
+  @staticmethod
+  def _model_side(modeldata: Any, side_index: int) -> tuple[Any | None, Any | None]:
+    if modeldata is None:
+      return None, None
+    try:
+      edges = modeldata.roadEdges
+      stds = modeldata.roadEdgeStds
+      if len(edges) <= side_index or len(stds) <= side_index:
+        return None, None
+      return edges[side_index], stds[side_index]
+    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:
+    self._refresh_enabled()
+    if not self.enabled:
+      if self._active:
+        self._reset()
+      self._active = False
+      return
+    if not self._active:
+      self._reset()
+      self._active = True
+
+    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)
+    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)
+    if left_fallback:
+      cloudlog.warning(f"lateral edge guard: left road edge unavailable for {UNAVAILABLE_HOLD_S:.2f} s; falling back to not blocking")
+    if right_fallback:
+      cloudlog.warning(f"lateral edge guard: right road edge unavailable for {UNAVAILABLE_HOLD_S:.2f} s; falling back to not blocking")
+
+  def block_for_direction(self, direction: int) -> custom.IQLateralEdgeBlock:
+    if not self.enabled:
+      return LateralEdgeBlock.none
+    if direction == LaneChangeDirection.left and self._left.blocked:
+      return LateralEdgeBlock.left
+    if direction == LaneChangeDirection.right and self._right.blocked:
+      return LateralEdgeBlock.right
+    return LateralEdgeBlock.none
--- a/iqpilot/selfdrive/controls/lib/tests/test_lateral_edge_guard.py
+++ b/iqpilot/selfdrive/controls/lib/tests/test_lateral_edge_guard.py
@@ -0,0 +1,236 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+import math
+
+from iqpilot.cereal import custom, log
+import iqpilot.cereal.messaging as messaging
+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,
+  LANE_CENTER_OFFSET_M,
+  MAX_MEASURED_LANE_WIDTH_M,
+  MAX_VALID_ROAD_EDGE_STD_M,
+  MIN_ACTIVE_SPEED_MPS,
+  MIN_MEASURED_LANE_WIDTH_M,
+  REQUIRED_ROAD_EDGE_DISTANCE_M,
+  UNAVAILABLE_HOLD_S,
+  LateralEdgeGuard,
+  RoadEdgeDataState,
+  evaluate_road_edge,
+)
+from iqpilot.selfdrive.selfdrived.iq_events import EVENTS_IQ, ET
+from iqpilot.selfdrive.selfdrived.selfdrived import SelfdriveD
+
+
+@dataclass
+class Edge:
+  x: list[float]
+  y: list[float]
+
+
+@dataclass
+class ModelData:
+  roadEdges: list[Edge]
+  roadEdgeStds: list[float]
+
+
+@dataclass
+class LaneModelData:
+  roadEdges: list[Edge]
+  roadEdgeStds: list[float]
+  laneLines: list[Edge]
+  laneLineProbs: list[float]
+
+
+class CarState:
+  def __init__(self, left_blindspot: bool = False) -> None:
+    self.vEgo = MIN_ACTIVE_SPEED_MPS + 1.0
+    self.leftBlinker = True
+    self.rightBlinker = False
+    self.leftBlindspot = left_blindspot
+    self.rightBlindspot = False
+    self.steeringPressed = True
+    self.steeringTorque = 1.0
+    self.brakePressed = False
+    self.standstill = False
+
+
+def edge_model(left_distance_m: float = 6.0, right_distance_m: float = 6.0,
+               left_std_m: float = 0.0, right_std_m: float = 0.0) -> ModelData:
+  xs = [5.0, 20.0, 40.0]
+  return ModelData(
+    [Edge(xs, [-left_distance_m] * len(xs)), Edge(xs, [right_distance_m] * len(xs))],
+    [left_std_m, right_std_m],
+  )
+
+
+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])
+
+
+def cycles(duration_s: float) -> int:
+  return math.ceil(duration_s / DT_MDL)
+
+
+def update_for(guard: LateralEdgeGuard, modeldata: ModelData | None, duration_s: float,
+               speed_mps: float = MIN_ACTIVE_SPEED_MPS) -> None:
+  for _ in range(cycles(duration_s)):
+    guard.update(modeldata, speed_mps, DT_MDL)
+
+
+def test_valid_geometry_blocks_and_clear_geometry_does_not_block() -> None:
+  blocked = evaluate_road_edge(edge_model(4.0).roadEdges[0], 0.2, log.LaneChangeDirection.left)
+  clear = evaluate_road_edge(edge_model(6.0).roadEdges[0], 0.2, log.LaneChangeDirection.left)
+  assert blocked.state == RoadEdgeDataState.VALID
+  assert blocked.should_block is True
+  assert clear.state == RoadEdgeDataState.VALID
+  assert clear.should_block is False
+
+
+def test_unavailable_and_invalid_are_distinct() -> None:
+  unavailable = evaluate_road_edge(Edge([5.0], []), 0.2, log.LaneChangeDirection.left)
+  invalid = evaluate_road_edge(edge_model().roadEdges[0], MAX_VALID_ROAD_EDGE_STD_M + 0.01,
+                               log.LaneChangeDirection.left)
+  assert unavailable.state == RoadEdgeDataState.UNAVAILABLE
+  assert unavailable.lateral_distance_m is None
+  assert invalid.state == RoadEdgeDataState.INVALID
+  assert invalid.should_block is None
+
+
+def test_distance_threshold_on_either_side() -> None:
+  epsilon_m = 0.001
+  for direction, edge_index in ((log.LaneChangeDirection.left, 0), (log.LaneChangeDirection.right, 1)):
+    below = edge_model(REQUIRED_ROAD_EDGE_DISTANCE_M - epsilon_m, REQUIRED_ROAD_EDGE_DISTANCE_M - epsilon_m)
+    above = edge_model(REQUIRED_ROAD_EDGE_DISTANCE_M + epsilon_m, REQUIRED_ROAD_EDGE_DISTANCE_M + epsilon_m)
+    assert evaluate_road_edge(below.roadEdges[edge_index], 0.0, direction).should_block is True
+    assert evaluate_road_edge(above.roadEdges[edge_index], 0.0, direction).should_block is False
+
+
+def test_disabled_guard_never_blocks() -> None:
+  guard = LateralEdgeGuard(enabled=False)
+  update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S * 2)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none
+
+
+def test_enabled_guard_blocks_after_debounce() -> None:
+  guard = LateralEdgeGuard(enabled=True)
+  update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left
+
+
+def test_parameter_refresh_controls_guard() -> None:
+  class EdgeGuardParams:
+    enabled = True
+
+    def get_bool(self, key: str) -> bool:
+      assert key == "IQEdgeGuard"
+      return self.enabled
+
+  params = EdgeGuardParams()
+  guard = LateralEdgeGuard(enabled=False)
+  guard._params = params
+  guard._param_refresh_frame = 0
+  update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left
+
+  params.enabled = False
+  guard._param_refresh_frame = 50
+  guard.update(edge_model(4.0), MIN_ACTIVE_SPEED_MPS, DT_MDL)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none
+
+
+def test_clear_debounce_rejects_a_single_blocking_frame() -> None:
+  guard = LateralEdgeGuard(enabled=True)
+  update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S)
+  update_for(guard, edge_model(6.0), CLEAR_DEBOUNCE_S - DT_MDL)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left
+  guard.update(edge_model(4.0), MIN_ACTIVE_SPEED_MPS, DT_MDL)
+  update_for(guard, edge_model(6.0), CLEAR_DEBOUNCE_S)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none
+
+
+def test_unavailable_holds_then_falls_back_to_not_blocking() -> None:
+  guard = LateralEdgeGuard(enabled=True)
+  update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S)
+  update_for(guard, None, UNAVAILABLE_HOLD_S - DT_MDL)
+  assert guard.left_measurement.state == RoadEdgeDataState.UNAVAILABLE
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left
+  guard.update(None, MIN_ACTIVE_SPEED_MPS, DT_MDL)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none
+
+
+def test_speed_gate_is_inactive_below_threshold() -> None:
+  guard = LateralEdgeGuard(enabled=True)
+  update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S, MIN_ACTIVE_SPEED_MPS - 0.01)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none
+
+
+def test_visible_outer_lane_line_overrides_edge_block() -> None:
+  guard = LateralEdgeGuard(enabled=True)
+  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_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
+
+
+def test_desire_helper_blocks_only_when_edge_guard_is_enabled() -> None:
+  helper = DesireHelper()
+  helper.lateral_edge_guard = LateralEdgeGuard(enabled=True)
+  helper.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
+  helper.lane_change_state = log.LaneChangeState.preLaneChange
+  helper.lane_change_direction = log.LaneChangeDirection.left
+  update_for(helper.lateral_edge_guard, edge_model(4.0), BLOCK_DEBOUNCE_S)
+  helper.update(CarState(), True, 1.0, modeldata=edge_model(4.0))
+  assert helper.lateral_edge_block == custom.IQLateralEdgeBlock.left
+  assert helper.lane_change_state == log.LaneChangeState.preLaneChange
+
+  helper.lateral_edge_guard = LateralEdgeGuard(enabled=False)
+  helper.update(CarState(), True, 1.0, modeldata=edge_model(4.0))
+  assert helper.lateral_edge_block == custom.IQLateralEdgeBlock.none
+  assert helper.lane_change_state == log.LaneChangeState.laneChangeStarting
+
+
+def test_published_edge_block_maps_to_distinct_event_and_alert() -> None:
+  message = messaging.new_message("iqDriveModelData")
+  message.iqDriveModelData.lateralEdgeBlock = custom.IQLateralEdgeBlock.right
+
+  class SubMaster:
+    updated = {"iqDriveModelData": True}
+
+    def __getitem__(self, service: str):
+      assert service == "iqDriveModelData"
+      return message.iqDriveModelData
+
+  selfdrived = SelfdriveD.__new__(SelfdriveD)
+  selfdrived.sm = SubMaster()
+  selfdrived._cached_model_event_names = ()
+  selfdrived._refresh_cached_model_events()
+
+  event_name = custom.IQOnroadEvent.EventName.lateralEdgeBlocked
+  assert selfdrived._cached_model_event_names == (event_name,)
+  alert = EVENTS_IQ[event_name][ET.WARNING]
+  assert alert.alert_text_1 == "Lane Change Blocked"
+  assert alert.alert_text_2 == "Road edge detected"