IQ.Pilot Release Commit @ 3fe374f
This commit is contained in:
IQ.Lvbs CI [bot]
@@ -14,13 +14,11 @@ 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
|
||||
@@ -107,8 +105,6 @@ 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
|
||||
@@ -178,8 +174,6 @@ 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
|
||||
|
||||
@@ -188,7 +182,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 and not lateral_edge_blocked:
|
||||
elif allowed_to_launch and not blindspot_detected:
|
||||
self.lane_change_state = LaneChangeState.laneChangeStarting
|
||||
|
||||
def _step_lane_change_starting(self, lane_change_prob: float) -> None:
|
||||
@@ -275,8 +269,6 @@ 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)
|
||||
|
||||
@@ -1,243 +0,0 @@
|
||||
"""
|
||||
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 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. 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, replace
|
||||
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.
|
||||
# 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.
|
||||
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,
|
||||
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) -> 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:
|
||||
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 direction == LaneChangeDirection.left and self._left.blocked:
|
||||
return LateralEdgeBlock.left
|
||||
if direction == LaneChangeDirection.right and self._right.blocked:
|
||||
return LateralEdgeBlock.right
|
||||
return LateralEdgeBlock.none
|
||||
@@ -1,255 +0,0 @@
|
||||
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,
|
||||
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,
|
||||
)
|
||||
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]
|
||||
|
||||
|
||||
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
|
||||
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 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_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.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:
|
||||
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_block_debounce_rejects_a_single_clear_frame() -> None:
|
||||
guard = LateralEdgeGuard()
|
||||
blocking = edge_model(4.0)
|
||||
clear = edge_model(6.0)
|
||||
update_for(guard, blocking, BLOCK_DEBOUNCE_S - DT_MDL)
|
||||
assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none
|
||||
guard.update(clear, MIN_ACTIVE_SPEED_MPS, DT_MDL)
|
||||
update_for(guard, blocking, BLOCK_DEBOUNCE_S)
|
||||
assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left
|
||||
|
||||
|
||||
def test_clear_debounce_rejects_a_single_blocking_frame() -> None:
|
||||
guard = LateralEdgeGuard()
|
||||
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()
|
||||
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_invalid_measurement_clears_through_release_debounce() -> None:
|
||||
guard = LateralEdgeGuard()
|
||||
update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S)
|
||||
invalid = edge_model(4.0, left_std_m=MAX_VALID_ROAD_EDGE_STD_M + 0.01)
|
||||
update_for(guard, invalid, CLEAR_DEBOUNCE_S - DT_MDL)
|
||||
assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left
|
||||
guard.update(invalid, 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()
|
||||
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
|
||||
update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S, MIN_ACTIVE_SPEED_MPS)
|
||||
assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left
|
||||
|
||||
|
||||
def test_desire_helper_keeps_edge_block_out_of_blindspot_path() -> None:
|
||||
helper = DesireHelper()
|
||||
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)
|
||||
blindspot_arguments: list[bool] = []
|
||||
|
||||
def record_blindspot(blindspot_detected: bool, brake_pressed: bool) -> None:
|
||||
blindspot_arguments.append(blindspot_detected)
|
||||
|
||||
helper.alc.update_lane_change = record_blindspot
|
||||
helper.update(CarState(left_blindspot=False), True, 1.0, modeldata=edge_model(4.0))
|
||||
assert blindspot_arguments == [False]
|
||||
assert helper.lateral_edge_block == custom.IQLateralEdgeBlock.left
|
||||
assert helper.lane_change_state == log.LaneChangeState.preLaneChange
|
||||
|
||||
helper.update(CarState(left_blindspot=True), True, 1.0, modeldata=edge_model(4.0))
|
||||
assert blindspot_arguments[-1] is True
|
||||
|
||||
|
||||
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"
|
||||
|
||||
|
||||
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
|
||||
@@ -590,7 +590,6 @@ class InferenceDaemon:
|
||||
driving_msg.drivingModelData.meta.laneChangeState = self._desire_logic.lane_change_state
|
||||
driving_msg.drivingModelData.meta.laneChangeDirection = self._desire_logic.lane_change_direction
|
||||
iq_msg.iqDriveModelData.turnSignalDirection = self._desire_logic.lane_turn_direction
|
||||
iq_msg.iqDriveModelData.lateralEdgeBlock = self._desire_logic.lateral_edge_block
|
||||
|
||||
populate_odometry_message(
|
||||
pose_msg,
|
||||
|
||||
@@ -29,6 +29,7 @@ _ACTIVE_BUNDLE_KEY = "ModelManager_ActiveBundle"
|
||||
_MODELS_CACHE_KEY = "ModelManager_ModelsCache"
|
||||
_RUNNER_CACHE_KEY = "ModelRunnerTypeCache"
|
||||
_DOWNLOAD_INDEX_KEY = "ModelManager_DownloadIndex"
|
||||
_PENDING_INDEX_KEY = "ModelManager_PendingIndex"
|
||||
_PENDING_MODEL_RESTORE_FILE = "/data/k3_pending_model_restore"
|
||||
_STOCK_RUNNER = int(Runner.stock)
|
||||
_TINYGRAD_RUNNER = int(Runner.tinygrad)
|
||||
@@ -226,6 +227,7 @@ def select_default_model(params: Params = None) -> None:
|
||||
bundle_dict = _load_default_bundle_dict()
|
||||
ensure_default_model_files(bundle_dict)
|
||||
params.remove(_DOWNLOAD_INDEX_KEY)
|
||||
params.remove(_PENDING_INDEX_KEY)
|
||||
params.put(_ACTIVE_BUNDLE_KEY, bundle_dict)
|
||||
params.remove(_RUNNER_CACHE_KEY)
|
||||
params.put(_RUNNER_CACHE_KEY, _TINYGRAD_RUNNER)
|
||||
|
||||
@@ -155,14 +155,6 @@ class IQEvents(EventsBase):
|
||||
EVENTS_IQ_TYPE = dict[int, dict[str, Alert | AlertCallbackType]]
|
||||
|
||||
_GUIDANCE_EVENTS: EVENTS_IQ_TYPE = {
|
||||
EventNameIQ.lateralEdgeBlocked: {
|
||||
ET.WARNING: Alert(
|
||||
"Lane Change Blocked",
|
||||
"Road edge detected",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.prompt, .1),
|
||||
},
|
||||
|
||||
EventNameIQ.speedLimitActive: {
|
||||
ET.WARNING: speed_limit_adjust_alert,
|
||||
},
|
||||
|
||||
@@ -222,9 +222,6 @@ class SelfdriveD(GapButtonActions):
|
||||
|
||||
model_data = self._get_model_data_ext()
|
||||
model_events = []
|
||||
if model_data.lateralEdgeBlock != custom.IQLateralEdgeBlock.none:
|
||||
model_events.append(custom.IQOnroadEvent.EventName.lateralEdgeBlocked)
|
||||
|
||||
lane_turn_direction = model_data.turnSignalDirection
|
||||
if lane_turn_direction == TurnDirection.turnLeft:
|
||||
model_events.append(custom.IQOnroadEvent.EventName.modelTurnLeft)
|
||||
|
||||
Reference in New Issue
Block a user