IQ.Pilot Release Commit @ bec7652

iqpilot/selfdrive/controls/lib/helpers/blinker_pause.py

@@ -1,10 +1,10 @@
 """
 Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
 """
-from cereal import car
+from iqpilot.cereal import car
 
-from openpilot.common.constants import CV
-from openpilot.common.params import Params
+from iqpilot.common.constants import CV
+from iqpilot.common.params import Params
 
 
 class SignalPauseEngine:

iqpilot/selfdrive/controls/lib/helpers/curvature.py

@@ -1,48 +0,0 @@
-"""
-Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
-"""
-
-from __future__ import annotations
-
-from numpy import clip, interp
-
-from openpilot.common.realtime import DT_MDL
-from openpilot.iqpilot.selfdrive.iqmodeld.config import ModelConstants
-from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, MAX_LATERAL_JERK, MIN_SPEED
-
-
-def _sanitize_plan(headings, curvatures):
-  valid_shape = len(headings) == CONTROL_N and len(curvatures) >= CONTROL_N
-  if valid_shape:
-    return headings, curvatures
-  placeholder = [0.0] * CONTROL_N
-  return placeholder, placeholder
-
-
-def _project_future_heading(delay_s: float, headings) -> float:
-  return float(interp(delay_s, ModelConstants.T_IDXS[:CONTROL_N], headings))
-
-
-def _convert_heading_to_curvature(projected_heading: float, speed_mps: float, current_curvature: float, delay_s: float) -> float:
-  turning_arc = projected_heading / (speed_mps * delay_s)
-  return (2.0 * turning_arc) - current_curvature
-
-
-def _limit_curvature_rate(target_curvature: float, current_curvature: float, speed_mps: float) -> float:
-  curvature_step = MAX_LATERAL_JERK / (speed_mps ** 2)
-  lower = current_curvature - (curvature_step * DT_MDL)
-  upper = current_curvature + (curvature_step * DT_MDL)
-  return float(clip(target_curvature, lower, upper))
-
-
-def solve_lag_curvature(steer_delay, v_ego, psis, curvatures):
-  headings, curvature_track = _sanitize_plan(psis, curvatures)
-  speed_mps = max(MIN_SPEED, v_ego)
-  delay_s = max(float(steer_delay), 1e-3)
-  current_curvature = float(curvature_track[0])
-  projected_heading = _project_future_heading(delay_s, headings)
-  target_curvature = _convert_heading_to_curvature(projected_heading, speed_mps, current_curvature, delay_s)
-  return _limit_curvature_rate(target_curvature, current_curvature, speed_mps)
-
-
-get_lag_adjusted_curvature = solve_lag_curvature

iqpilot/selfdrive/controls/lib/helpers/e2e_alerts.py

@@ -2,11 +2,11 @@
 Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
 """
 
-from cereal import messaging, custom
+from iqpilot.cereal import messaging, custom
 
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.iqpilot.selfdrive.selfdrived.events import IQEvents
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.selfdrived.iq_events import IQEvents
 
 PARAM_PATH = "EndToEndAlert"
 PARAM_LEAD = "EndToEndLeadAlert"

iqpilot/selfdrive/controls/lib/helpers/lane_change.py

@@ -1,10 +1,10 @@
 """
 Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
 """
-from cereal import custom, log
+from iqpilot.cereal import custom, log
 
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import DT_MDL
 
 NAV_EXIT_COMMIT_DISTANCE = 500.0  # m before a route exit to begin moving into the exit lane
 _ManeuverType = custom.IQNavState.ManeuverType

iqpilot/selfdrive/controls/lib/helpers/lane_turn.py

@@ -6,10 +6,10 @@ from __future__ import annotations
 
 from dataclasses import dataclass
 
-from cereal import custom
+from iqpilot.cereal import custom
 
-from openpilot.common.constants import CV
-from openpilot.common.params import Params
+from iqpilot.common.constants import CV
+from iqpilot.common.params import Params
 
 TurnDirection = custom.IQTurnSignalDirection
 

iqpilot/selfdrive/controls/lib/helpers/lateral_edge_guard.py

@@ -0,0 +1,182 @@
+"""
+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.
+Unavailable geometry briefly holds the last output, then fails open because a
+model dropout is not geometric evidence of a nearby edge.
+"""
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass
+from enum import IntEnum
+from typing import Any
+
+from iqpilot.cereal import custom, log
+from iqpilot.common.constants import CV
+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.
+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.
+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.
+UNAVAILABLE_HOLD_S = 0.50  # Ten model frames bridge a short model-data dropout before failing open.
+TIMER_EPSILON_S = 1e-9  # Floating-point comparison tolerance, far below one model tick.
+
+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) -> 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
+  return RoadEdgeMeasurement(
+    RoadEdgeDataState.VALID,
+    lateral_distance_m,
+    conservative_distance_m,
+    conservative_distance_m < REQUIRED_ROAD_EDGE_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) -> 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
+
+  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)
+    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 direction == LaneChangeDirection.left and self._left.blocked:
+      return LateralEdgeBlock.left
+    if direction == LaneChangeDirection.right and self._right.blocked:
+      return LateralEdgeBlock.right
+    return LateralEdgeBlock.none

iqpilot/selfdrive/controls/lib/helpers/nav_torque_pulse.py

@@ -6,8 +6,8 @@ turns and highway exits. This is a lateral-control add-on driven by iqNavState;
 it is independent of the feed-forward model and is off by default.
 """
 import numpy as np
-import cereal.messaging as messaging
-from cereal import custom
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal import custom
 
 TURN_NUDGE_TORQUE = 0.8
 EXIT_NUDGE_TORQUE = 0.6

iqpilot/selfdrive/controls/lib/helpers/tests/test_e2e_alerts.py

@@ -5,10 +5,10 @@ from types import SimpleNamespace
 
 import pytest
 
-import cereal.messaging as messaging
-from cereal import custom
-from openpilot.common.realtime import DT_MDL
-from openpilot.iqpilot.selfdrive.controls.lib.helpers.e2e_alerts import (
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal import custom
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.controls.lib.helpers.e2e_alerts import (
   EndToEndAlertEngine, CONFIRM_S, SETTLE_S, HORIZON_TAIL, PATH_SPEED_MPS, LEAD_SPEED_MPS, LEAD_GAP_M)
 
 E2E_CHIME = custom.IQOnroadEvent.EventName.e2eChime

iqpilot/selfdrive/controls/lib/helpers/tests/test_nav_torque_pulse.py

@@ -6,9 +6,9 @@ from types import SimpleNamespace
 import numpy as np
 import pytest
 
-from cereal import custom
-import openpilot.iqpilot.selfdrive.controls.lib.helpers.nav_torque_pulse as nav_pulse
-from openpilot.iqpilot.selfdrive.controls.lib.helpers.nav_torque_pulse import (
+from iqpilot.cereal import custom
+import iqpilot.selfdrive.controls.lib.helpers.nav_torque_pulse as nav_pulse
+from iqpilot.selfdrive.controls.lib.helpers.nav_torque_pulse import (
   NavTorquePulseBrain, TURN_PULSE_FRAMES, EXIT_PULSE_FRAMES)