IQ.Pilot Release Commit @ f2a861c

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,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

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)