forked from IQ.Lvbs/IQ.Pilot
IQ.Pilot Release Commit @ 3fe374f
This commit is contained in:
IQ.Lvbs CI [bot]
@@ -322,8 +322,6 @@ struct IQOnroadEvent @0xf4621d3ee9233bc9 {
|
||||
# camera hardware
|
||||
wideCamFaulty @32;
|
||||
|
||||
# lane-change safety
|
||||
lateralEdgeBlocked @33;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -503,15 +501,8 @@ enum IQTurnSignalDirection {
|
||||
turnRight @2;
|
||||
}
|
||||
|
||||
enum IQLateralEdgeBlock {
|
||||
none @0;
|
||||
left @1;
|
||||
right @2;
|
||||
}
|
||||
|
||||
struct IQDriveModelData @0xcdf0f7f14f46cb86 {
|
||||
turnSignalDirection @0 :IQTurnSignalDirection;
|
||||
lateralEdgeBlock @1 :IQLateralEdgeBlock;
|
||||
}
|
||||
|
||||
enum NavDirection {
|
||||
|
||||
@@ -78,7 +78,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"JoystickAolRequest", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, STRING}},
|
||||
{"Konn3ktSshKeys", {PERSISTENT, STRING}},
|
||||
{"Konn3ktBleTransportEnabled", {PERSISTENT, BOOL, "1"}},
|
||||
{"Konn3ktLibdatachannelWebRTC", {PERSISTENT, BOOL, "0"}},
|
||||
{"LanguageSetting", {PERSISTENT, STRING, "en"}},
|
||||
{"LastAthenaPingTime", {CLEAR_ON_MANAGER_START, INT}},
|
||||
{"LastGPSPosition", {PERSISTENT, STRING}},
|
||||
@@ -219,6 +218,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"ModelManager_ActiveBundle", {PERSISTENT, JSON}},
|
||||
{"ModelManager_ClearCache", {CLEAR_ON_MANAGER_START, BOOL}},
|
||||
{"ModelManager_DownloadIndex", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, INT, "-1"}},
|
||||
{"ModelManager_PendingIndex", {PERSISTENT, INT, "-1"}},
|
||||
{"IQModelFavorites", {PERSISTENT, STRING}},
|
||||
{"ModelManager_LastSyncTime", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, INT, "0"}},
|
||||
{"ModelManager_ModelsCache", {PERSISTENT, JSON}},
|
||||
|
||||
@@ -66,10 +66,6 @@
|
||||
- Added Always-On Lateral support through compatible Hyundai LFA buttons.
|
||||
- Added an optional mode that pauses steering torque when the driver takes the wheel and resumes after release.
|
||||
|
||||
#### Lane Changes
|
||||
|
||||
- Added a model-based road-edge guard that blocks unsafe lane changes with an alert.
|
||||
|
||||
#### Lateral Tuning
|
||||
|
||||
- Added configurable steering smoothing, slew limiting, and curvature lookahead.
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -26,6 +26,22 @@ from iqpilot.system.version import get_build_metadata
|
||||
from iqpilot.system.hardware.hw import Paths
|
||||
|
||||
|
||||
MODELD_WATCHDOG_TIMEOUT = 30.0
|
||||
|
||||
|
||||
def update_modeld_watchdog(deadline: float | None, started: bool, model_updated: bool, process, now: float) -> float | None:
|
||||
running = process.proc is not None and process.proc.is_alive()
|
||||
if not started or not running:
|
||||
return None
|
||||
if deadline is None or model_updated:
|
||||
return now + MODELD_WATCHDOG_TIMEOUT
|
||||
if now >= deadline:
|
||||
cloudlog.error("iqmodeld is alive but not publishing modelV2; restarting")
|
||||
process.restart()
|
||||
return now + MODELD_WATCHDOG_TIMEOUT
|
||||
return deadline
|
||||
|
||||
|
||||
def manager_init() -> None:
|
||||
heal_param_perms()
|
||||
save_bootlog()
|
||||
@@ -55,16 +71,22 @@ def manager_init() -> None:
|
||||
params.put_bool("RecordFront", True)
|
||||
|
||||
# set unset params to their default value
|
||||
initialized_defaults = {}
|
||||
for k in params.all_keys():
|
||||
default_value = params.get_default_value(k)
|
||||
if default_value is not None and params.get(k) is None:
|
||||
params.put(k, default_value)
|
||||
if default_value is not None:
|
||||
initialized_defaults[k] = params.get(k)
|
||||
|
||||
try:
|
||||
from iqpilot.selfdrive.iqmodeld.models.helpers import seed_default_bundle_if_unset
|
||||
seed_default_bundle_if_unset(params)
|
||||
except Exception:
|
||||
cloudlog.exception("failed to seed default model bundle")
|
||||
for k, value in initialized_defaults.items():
|
||||
if value is not None and params.get(k) is None:
|
||||
params.put(k, value)
|
||||
|
||||
# Create folders needed for msgq
|
||||
try:
|
||||
@@ -142,7 +164,7 @@ def manager_thread() -> None:
|
||||
ignore.append("pandad")
|
||||
ignore += [x for x in os.getenv("BLOCK", "").split(",") if len(x) > 0]
|
||||
|
||||
sm = messaging.SubMaster(['deviceState', 'carParams', 'pandaStates'], poll='deviceState')
|
||||
sm = messaging.SubMaster(['deviceState', 'carParams', 'pandaStates', 'modelV2'], poll='deviceState')
|
||||
pm = messaging.PubMaster(['managerState'])
|
||||
|
||||
write_onroad_params(False, params)
|
||||
@@ -151,6 +173,7 @@ def manager_thread() -> None:
|
||||
started_prev = False
|
||||
ignition_prev = False
|
||||
running_prev = None
|
||||
modeld_deadline = None
|
||||
|
||||
while True:
|
||||
sm.update(1000)
|
||||
@@ -178,6 +201,9 @@ def manager_thread() -> None:
|
||||
ignition_prev = ignition
|
||||
|
||||
ensure_running(managed_processes.values(), started, params=params, CP=sm['carParams'], not_run=ignore)
|
||||
modeld_deadline = update_modeld_watchdog(
|
||||
modeld_deadline, started, sm.updated['modelV2'], managed_processes['iqmodeld'], time.monotonic()
|
||||
)
|
||||
|
||||
# print only on change (reprinting every loop floods the shared tmux); always logged
|
||||
procs = [p for p in managed_processes.values() if p.proc]
|
||||
|
||||
@@ -42,6 +42,31 @@ class TestManager:
|
||||
assert proc.bundle == "iqpilot_model_selector_private"
|
||||
assert proc.entry == "iqpilot_private.models.manager"
|
||||
|
||||
def test_modeld_watchdog_restarts_stalled_process(self, mocker):
|
||||
proc = mocker.Mock()
|
||||
proc.proc.is_alive.return_value = True
|
||||
|
||||
deadline = manager.update_modeld_watchdog(None, True, False, proc, 10.0)
|
||||
assert deadline == 10.0 + manager.MODELD_WATCHDOG_TIMEOUT
|
||||
assert manager.update_modeld_watchdog(deadline, True, False, proc, deadline - 0.1) == deadline
|
||||
proc.restart.assert_not_called()
|
||||
|
||||
next_deadline = manager.update_modeld_watchdog(deadline, True, False, proc, deadline)
|
||||
proc.restart.assert_called_once_with()
|
||||
assert next_deadline == deadline + manager.MODELD_WATCHDOG_TIMEOUT
|
||||
|
||||
def test_modeld_watchdog_tracks_output_and_resets(self, mocker):
|
||||
proc = mocker.Mock()
|
||||
proc.proc.is_alive.return_value = True
|
||||
|
||||
deadline = manager.update_modeld_watchdog(20.0, True, True, proc, 15.0)
|
||||
assert deadline == 15.0 + manager.MODELD_WATCHDOG_TIMEOUT
|
||||
assert manager.update_modeld_watchdog(deadline, False, False, proc, 16.0) is None
|
||||
|
||||
proc.proc.is_alive.return_value = False
|
||||
assert manager.update_modeld_watchdog(deadline, True, False, proc, 16.0) is None
|
||||
proc.restart.assert_not_called()
|
||||
|
||||
def test_bundle_process_stops_with_sigterm(self, mocker):
|
||||
proc = BundleProcess("test", "bundle", "entry", lambda *_: True)
|
||||
proc.proc = mocker.Mock(exitcode=None, pid=123)
|
||||
|
||||
@@ -4,7 +4,7 @@ import sys
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
PACKAGE_NAMES = ("msgq", "iqdbc", "panda", "teleoprtc", "tinygrad")
|
||||
PACKAGE_NAMES = ("msgq", "iqdbc", "panda", "tinygrad")
|
||||
root = Path(sys.argv[1]).resolve()
|
||||
text = (root / "pyproject.toml").read_text()
|
||||
for name in PACKAGE_NAMES:
|
||||
|
||||
@@ -11,7 +11,7 @@ import sys
|
||||
from importlib import metadata
|
||||
from pathlib import Path
|
||||
|
||||
PACKAGES = ("iqdbc", "msgq", "panda", "teleoprtc", "tinygrad")
|
||||
PACKAGES = ("iqdbc", "msgq", "panda", "tinygrad")
|
||||
|
||||
missing = []
|
||||
for name in PACKAGES:
|
||||
|
||||
47
iqpilot/system/runtime_wheel_requirements.py
Normal file
47
iqpilot/system/runtime_wheel_requirements.py
Normal file
@@ -0,0 +1,47 @@
|
||||
#!/usr/bin/env python3
|
||||
import hashlib
|
||||
import sys
|
||||
import tomllib
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
PACKAGE_NAMES = ("libdatachannel-py",)
|
||||
PACKAGE_IMPORTS = ("libdatachannel",)
|
||||
PACKAGE_WHEELS = {
|
||||
"libdatachannel-py": "libdatachannel_py-2026.1.0.dev2-cp312-cp312-manylinux_2_35_aarch64.whl",
|
||||
}
|
||||
|
||||
|
||||
def locked_packages(root: Path) -> dict[str, dict]:
|
||||
lock = tomllib.loads((root / "uv.lock").read_text())
|
||||
packages = {package["name"]: package for package in lock["package"] if package["name"] in PACKAGE_NAMES}
|
||||
if packages.keys() != set(PACKAGE_NAMES):
|
||||
raise RuntimeError("missing locked runtime wheel")
|
||||
return packages
|
||||
|
||||
|
||||
def requirements(root: Path) -> list[str]:
|
||||
packages = locked_packages(root)
|
||||
return [f"{name}=={packages[name]['version']}" for name in PACKAGE_NAMES]
|
||||
|
||||
|
||||
def install_sources(root: Path) -> list[str]:
|
||||
packages = locked_packages(root)
|
||||
sources = []
|
||||
for name in PACKAGE_NAMES:
|
||||
filename = PACKAGE_WHEELS[name]
|
||||
wheel = root / "artifacts" / "runtime_wheels" / filename
|
||||
locked_wheel = next((item for item in packages[name]["wheels"] if item["url"].endswith(f"/{filename}")), None)
|
||||
if locked_wheel is None or not wheel.is_file():
|
||||
raise RuntimeError(f"missing vendored runtime wheel for {name}")
|
||||
expected = locked_wheel["hash"].removeprefix("sha256:")
|
||||
if hashlib.sha256(wheel.read_bytes()).hexdigest() != expected:
|
||||
raise RuntimeError(f"invalid vendored runtime wheel for {name}")
|
||||
sources.append(str(wheel))
|
||||
return sources
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
root = Path(sys.argv[1]).resolve()
|
||||
values = install_sources(root) if "--sources" in sys.argv[2:] else requirements(root)
|
||||
print("\n".join(values))
|
||||
31
iqpilot/system/runtime_wheels_verify.py
Normal file
31
iqpilot/system/runtime_wheels_verify.py
Normal file
@@ -0,0 +1,31 @@
|
||||
#!/usr/bin/env python3
|
||||
import csv
|
||||
import sys
|
||||
from importlib import metadata
|
||||
from pathlib import Path
|
||||
|
||||
from iqpilot.system.runtime_wheel_requirements import PACKAGE_NAMES
|
||||
|
||||
|
||||
missing = []
|
||||
for name in PACKAGE_NAMES:
|
||||
try:
|
||||
dist = metadata.distribution(name)
|
||||
except metadata.PackageNotFoundError:
|
||||
missing.append(f"{name}: not installed")
|
||||
continue
|
||||
record = dist.read_text("RECORD")
|
||||
if not record:
|
||||
missing.append(f"{name}: no RECORD")
|
||||
continue
|
||||
base = Path(str(dist._path)).parent
|
||||
for row in csv.reader(record.splitlines()):
|
||||
if not row or row[0].endswith((".pyc", "/")):
|
||||
continue
|
||||
if not (base / row[0]).exists():
|
||||
missing.append(f"{name}: {row[0]}")
|
||||
|
||||
if missing:
|
||||
print("\n".join(missing[:20]), file=sys.stderr)
|
||||
print(f"runtime wheels damaged: {len(missing)} missing files", file=sys.stderr)
|
||||
sys.exit(1)
|
||||
@@ -1,105 +0,0 @@
|
||||
import asyncio
|
||||
import fractions
|
||||
|
||||
import aiortc
|
||||
import av
|
||||
import numpy as np
|
||||
|
||||
from iqpilot.cereal import messaging
|
||||
|
||||
|
||||
WEBRTC_AUDIO_SERVICE = "webrtcAudioData"
|
||||
WEBRTC_AUDIO_PTIME = 0.020
|
||||
|
||||
|
||||
class AudioInputStreamTrack(aiortc.mediastreams.AudioStreamTrack):
|
||||
"""Device microphone -> WebRTC, sourced from micd's `rawAudioData` cereal stream.
|
||||
|
||||
micd owns the ALSA capture device, so opening it again via PyAudio fails with a host error
|
||||
('audio in use', PortAudio errno -9999). Instead we consume micd's already-published int16 mono
|
||||
PCM and repacketize it into WebRTC audio frames — no device contention, and it works whenever micd
|
||||
is running. Reading one message per recv() paces playout to micd's real-time publish rate.
|
||||
"""
|
||||
def __init__(self, rate: int = 16000, channels: int = 1):
|
||||
super().__init__()
|
||||
self.rate = rate
|
||||
self.channels = channels
|
||||
# conflate=False: keep audio continuous (don't drop buffered samples) for clean playback.
|
||||
self._sock = messaging.sub_sock("rawAudioData", conflate=False)
|
||||
self._start: float | None = None
|
||||
self.pts = 0
|
||||
self.enabled = True
|
||||
self._audio_buffer = bytearray()
|
||||
|
||||
def enable(self, enabled: bool) -> None:
|
||||
self.enabled = enabled
|
||||
|
||||
async def _fill_audio_buffer(self, target_bytes: int) -> None:
|
||||
deadline = asyncio.get_running_loop().time() + WEBRTC_AUDIO_PTIME
|
||||
while len(self._audio_buffer) < target_bytes:
|
||||
msg = messaging.recv_one_or_none(self._sock)
|
||||
if msg is not None:
|
||||
audio = msg.rawAudioData
|
||||
rate = int(audio.sampleRate) or self.rate
|
||||
if rate != self.rate:
|
||||
self.rate = rate
|
||||
self._audio_buffer.clear()
|
||||
self._start = None
|
||||
self.pts = 0
|
||||
self._audio_buffer.extend(bytes(audio.data))
|
||||
continue
|
||||
if asyncio.get_running_loop().time() >= deadline:
|
||||
break
|
||||
await asyncio.sleep(0.005)
|
||||
|
||||
async def _next_audio_data(self) -> tuple[bytes, int]:
|
||||
samples = max(1, int(WEBRTC_AUDIO_PTIME * self.rate))
|
||||
target_bytes = samples * 2
|
||||
await self._fill_audio_buffer(target_bytes)
|
||||
|
||||
if len(self._audio_buffer) >= target_bytes:
|
||||
data = bytes(self._audio_buffer[:target_bytes])
|
||||
del self._audio_buffer[:target_bytes]
|
||||
else:
|
||||
data = bytes(self._audio_buffer)
|
||||
self._audio_buffer.clear()
|
||||
data += bytes(target_bytes - len(data))
|
||||
|
||||
return data, self.rate
|
||||
|
||||
async def _pace(self, pts: int, sample_rate: int) -> None:
|
||||
if self._start is None:
|
||||
self._start = asyncio.get_running_loop().time()
|
||||
return
|
||||
|
||||
wait = self._start + (pts / sample_rate) - asyncio.get_running_loop().time()
|
||||
if wait > 0:
|
||||
await asyncio.sleep(wait)
|
||||
|
||||
async def recv(self):
|
||||
while True:
|
||||
if not self.enabled:
|
||||
break
|
||||
data, sample_rate = await self._next_audio_data()
|
||||
if data:
|
||||
samples = len(data) // 2
|
||||
pts = self.pts
|
||||
self.pts += samples
|
||||
await self._pace(pts, sample_rate)
|
||||
|
||||
frame = av.AudioFrame(format="s16", layout="mono", samples=samples)
|
||||
frame.planes[0].update(data)
|
||||
frame.pts = pts
|
||||
frame.sample_rate = sample_rate
|
||||
frame.time_base = fractions.Fraction(1, sample_rate)
|
||||
return frame
|
||||
|
||||
samples_per_frame = max(1, int(WEBRTC_AUDIO_PTIME * self.rate))
|
||||
samples = np.zeros((1, samples_per_frame), dtype=np.int16)
|
||||
frame = av.AudioFrame.from_ndarray(samples, format='s16', layout='mono')
|
||||
frame.sample_rate = self.rate
|
||||
frame.time_base = fractions.Fraction(1, self.rate)
|
||||
frame.pts = self.pts
|
||||
self.pts += frame.samples
|
||||
await self._pace(frame.pts, self.rate)
|
||||
return frame
|
||||
@@ -7,17 +7,19 @@ import av
|
||||
|
||||
from iqpilot.cereal import messaging
|
||||
from iqpilot.selfdrive.ui.soundd import SAMPLE_RATE as SOUND_SAMPLE_RATE
|
||||
from iqpilot.system.webrtc.device.audio import WEBRTC_AUDIO_PTIME, WEBRTC_AUDIO_SERVICE
|
||||
|
||||
|
||||
WEBRTC_AUDIO_SERVICE = "webrtcAudioData"
|
||||
WEBRTC_AUDIO_PTIME = 0.020
|
||||
|
||||
|
||||
class AudioInputOpusProducer:
|
||||
"""Micd PCM -> 48 kHz Opus payloads for libdatachannel's RTP packetizer."""
|
||||
|
||||
def __init__(self) -> None:
|
||||
self._sock = messaging.sub_sock("rawAudioData", conflate=False)
|
||||
self._pcm = bytearray()
|
||||
self._source_rate = 16_000
|
||||
self._next_pts = 0
|
||||
self._packet_pts = 0
|
||||
self._pending: deque[tuple[bytes, int]] = deque()
|
||||
self._enabled = True
|
||||
self._resampler = av.AudioResampler(format="fltp", layout="mono", rate=48_000)
|
||||
@@ -65,13 +67,22 @@ class AudioInputOpusProducer:
|
||||
frame.time_base = Fraction(1, 48_000)
|
||||
self._next_pts += frame.samples
|
||||
for packet in self._encoder.encode(frame):
|
||||
self._pending.append((bytes(packet), int(packet.pts or 0)))
|
||||
self._pending.append((bytes(packet), self._packet_pts))
|
||||
self._packet_pts += int(packet.duration or frame.samples)
|
||||
return self._pending.popleft()
|
||||
|
||||
|
||||
class IncomingOpusCerealProxy:
|
||||
"""libdatachannel Opus payloads -> soundd-compatible PCM cereal messages."""
|
||||
class DebugAudioOpusProducer(AudioInputOpusProducer):
|
||||
async def _read_pcm_frame(self) -> av.AudioFrame:
|
||||
samples = int(WEBRTC_AUDIO_PTIME * self._source_rate)
|
||||
await asyncio.sleep(WEBRTC_AUDIO_PTIME)
|
||||
frame = av.AudioFrame(format="s16", layout="mono", samples=samples)
|
||||
frame.planes[0].update(bytes(samples * 2))
|
||||
frame.sample_rate = self._source_rate
|
||||
return frame
|
||||
|
||||
|
||||
class IncomingOpusCerealProxy:
|
||||
def __init__(self, track) -> None:
|
||||
self._loop = asyncio.get_running_loop()
|
||||
self._queue: asyncio.Queue[bytes] = asyncio.Queue(maxsize=32)
|
||||
@@ -4,7 +4,7 @@ import struct
|
||||
import time
|
||||
|
||||
import av
|
||||
from iqpilot.system.webrtc.teleoprtc_ldc.tracks import TiciVideoStreamTrack
|
||||
from iqpilot.system.webrtc.rtc.tracks import TiciVideoStreamTrack
|
||||
|
||||
from iqpilot.cereal import messaging
|
||||
from iqpilot.common.params import Params
|
||||
@@ -272,3 +272,40 @@ class LiveStreamVideoStreamTrack(TiciVideoStreamTrack):
|
||||
|
||||
def codec_preference(self) -> str | None:
|
||||
return "H264"
|
||||
|
||||
|
||||
class DebugVideoStreamTrack(TiciVideoStreamTrack):
|
||||
def __init__(self, camera_type: str):
|
||||
super().__init__(camera_type, 0.05)
|
||||
self._codec = av.CodecContext.create("libx264", "w")
|
||||
self._codec.width = 640
|
||||
self._codec.height = 480
|
||||
self._codec.pix_fmt = "yuv420p"
|
||||
self._codec.time_base = self._time_base
|
||||
self._codec.framerate = 20
|
||||
self._codec.options = {"preset": "ultrafast", "tune": "zerolatency"}
|
||||
self._codec.open()
|
||||
self._pts = 0
|
||||
self.timing_sei_enabled = False
|
||||
|
||||
async def recv(self):
|
||||
await asyncio.sleep(self._dt)
|
||||
frame = av.VideoFrame(self._codec.width, self._codec.height, "yuv420p")
|
||||
frame.planes[0].update(bytes(frame.planes[0].buffer_size))
|
||||
for plane in frame.planes[1:]:
|
||||
plane.update(bytes([128]) * plane.buffer_size)
|
||||
frame.pts = self._pts
|
||||
self._pts += int(self._dt * self._clock_rate)
|
||||
packets = self._codec.encode(frame)
|
||||
if not packets:
|
||||
return await self.recv()
|
||||
packet = av.Packet(b"".join(bytes(encoded) for encoded in packets))
|
||||
packet.pts = frame.pts
|
||||
packet.dts = frame.pts
|
||||
packet.time_base = self._time_base
|
||||
packet.duration = int(self._dt * self._clock_rate)
|
||||
return packet
|
||||
|
||||
def switch_camera(self, camera_type: str) -> None:
|
||||
if camera_type not in LiveStreamVideoStreamTrack.livestream_camera_to_sock_mapping:
|
||||
raise ValueError(f"Unknown camera {camera_type}")
|
||||
@@ -1,273 +0,0 @@
|
||||
import asyncio
|
||||
import logging
|
||||
import struct
|
||||
import time
|
||||
|
||||
import av
|
||||
from teleoprtc.tracks import TiciVideoStreamTrack
|
||||
|
||||
from iqpilot.cereal import messaging
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.common.realtime import DT_MDL, DT_DMON
|
||||
|
||||
# Arbitrary 16-byte UUID identifying konn3kt frame-timing SEI messages. When timing
|
||||
# telemetry is enabled, each frame carries a user_data_unregistered SEI NAL with four
|
||||
# big-endian doubles (ms): encode duration, IPC/queue delay, host transit, and the
|
||||
# device wall clock. The client decodes these to compute true glass-to-glass latency.
|
||||
TIMING_SEI_UUID = bytes([
|
||||
0xa5, 0xe0, 0xc4, 0xa4, 0x5b, 0x6e, 0x4e, 0x1e,
|
||||
0x9c, 0x7e, 0x12, 0x34, 0x56, 0x78, 0x9a, 0xbc,
|
||||
])
|
||||
# Annex-B start code + SEI NAL (type 6) + user_data_unregistered (type 5) + payload size
|
||||
# (0x30 = 48 bytes = 16 UUID + 32 data). Trailing 0x80 is the RBSP stop bit.
|
||||
_SEI_PREFIX = b'\x00\x00\x00\x01\x06\x05\x30' + TIMING_SEI_UUID
|
||||
|
||||
|
||||
class LiveStreamVideoStreamTrack(TiciVideoStreamTrack):
|
||||
livestream_camera_to_sock_mapping = {
|
||||
"driver": "livestreamDriverEncodeData",
|
||||
"wideRoad": "livestreamWideRoadEncodeData",
|
||||
"road": "livestreamRoadEncodeData",
|
||||
}
|
||||
main_camera_to_sock_mapping = {
|
||||
"driver": "driverEncodeData",
|
||||
"wideRoad": "wideRoadEncodeData",
|
||||
"road": "roadEncodeData",
|
||||
}
|
||||
|
||||
# Number of live tracks still waiting for their first keyframe. The on-demand
|
||||
# keyframe request (LivestreamRequestKeyframe) is a single global param honored by
|
||||
# every encoder, so with multiple concurrent tracks (dual-camera PiP) we must not
|
||||
# clear it until *all* tracks have received an IDR — otherwise the first track to
|
||||
# get its keyframe clears the request and starves the others (black feed).
|
||||
_kf_pending_count = 0
|
||||
|
||||
def __init__(self, camera_type: str):
|
||||
dt = DT_DMON if camera_type == "driver" else DT_MDL
|
||||
super().__init__(camera_type, dt)
|
||||
|
||||
self._params = Params()
|
||||
self._camera_type = camera_type
|
||||
self._candidate_topics = [
|
||||
self.main_camera_to_sock_mapping[camera_type],
|
||||
self.livestream_camera_to_sock_mapping[camera_type],
|
||||
]
|
||||
self._socks = {topic: messaging.sub_sock(topic, conflate=True) for topic in self._candidate_topics}
|
||||
self._active_topic = self._preferred_topics()[0]
|
||||
self._pts = 0
|
||||
self._t0_ns = time.monotonic_ns()
|
||||
self._cached_header: bytes = b""
|
||||
self._sent_keyframe = False
|
||||
self._kf_requested = False # whether this track counts toward _kf_pending_count
|
||||
self._frame_count = 0
|
||||
self._last_frame_time = 0.0
|
||||
self._last_preference_refresh = 0.0
|
||||
# Tracks how long the H264 livestream feed has been silent, to gate the last-resort main-feed
|
||||
# fallback (see recv) without flapping between sources frame-by-frame.
|
||||
self._live_silent_since: float | None = None
|
||||
# Opt-in glass-to-glass latency telemetry (toggled by the client over the data channel).
|
||||
self.timing_sei_enabled = False
|
||||
self._logger = logging.getLogger("LiveStreamVideoStreamTrack")
|
||||
|
||||
# Ask the encoder for an immediate IDR so the stream starts fast instead of waiting up to a full
|
||||
# GOP for the next periodic keyframe (encoderd honors LivestreamRequestKeyframe per-frame).
|
||||
self._mark_keyframe_needed()
|
||||
|
||||
def _request_keyframe(self, enabled: bool) -> None:
|
||||
try:
|
||||
self._params.put_bool("LivestreamRequestKeyframe", enabled)
|
||||
except Exception:
|
||||
self._logger.exception("failed to set LivestreamRequestKeyframe")
|
||||
|
||||
def _mark_keyframe_needed(self) -> None:
|
||||
"""This track needs (another) keyframe: keep the global request asserted."""
|
||||
if not self._kf_requested:
|
||||
LiveStreamVideoStreamTrack._kf_pending_count += 1
|
||||
self._kf_requested = True
|
||||
self._request_keyframe(True)
|
||||
|
||||
def request_keyframe(self) -> None:
|
||||
self._mark_keyframe_needed()
|
||||
|
||||
def _mark_keyframe_received(self) -> None:
|
||||
"""This track got its keyframe; only clear the global request once no track needs one."""
|
||||
if self._kf_requested:
|
||||
self._kf_requested = False
|
||||
LiveStreamVideoStreamTrack._kf_pending_count = max(0, LiveStreamVideoStreamTrack._kf_pending_count - 1)
|
||||
if LiveStreamVideoStreamTrack._kf_pending_count == 0:
|
||||
self._request_keyframe(False)
|
||||
|
||||
def stop(self):
|
||||
# Release our pending-keyframe hold so a torn-down track that never received an
|
||||
# IDR doesn't pin LivestreamRequestKeyframe True forever (continuous keyframes).
|
||||
if getattr(self, "_kf_requested", False):
|
||||
self._kf_requested = False
|
||||
LiveStreamVideoStreamTrack._kf_pending_count = max(0, LiveStreamVideoStreamTrack._kf_pending_count - 1)
|
||||
try:
|
||||
super().stop()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
def switch_camera(self, camera_type: str) -> None:
|
||||
"""Repoint this track at a different camera without renegotiating the peer connection.
|
||||
|
||||
Lets a single video track back the whole Live View — the client flips cameras over the
|
||||
data channel and we swap the source here, instead of uplinking every camera at once."""
|
||||
if camera_type not in self.livestream_camera_to_sock_mapping:
|
||||
self._logger.warning("[%s] ignoring switch to unknown camera %s", self._id, camera_type)
|
||||
return
|
||||
if camera_type == self._camera_type:
|
||||
return
|
||||
self._logger.info("[%s] switching camera %s -> %s", self._id, self._camera_type, camera_type)
|
||||
self._camera_type = camera_type
|
||||
self._candidate_topics = [
|
||||
self.main_camera_to_sock_mapping[camera_type],
|
||||
self.livestream_camera_to_sock_mapping[camera_type],
|
||||
]
|
||||
self._socks = {topic: messaging.sub_sock(topic, conflate=True) for topic in self._candidate_topics}
|
||||
self._active_topic = self._preferred_topics()[0]
|
||||
# Force a fresh keyframe/header before emitting frames from the new source, and ask the encoder
|
||||
# for an immediate IDR so the camera switch isn't stalled waiting for the next periodic keyframe.
|
||||
self._cached_header = b""
|
||||
self._sent_keyframe = False
|
||||
self._last_preference_refresh = 0.0
|
||||
self._live_silent_since = None
|
||||
self._mark_keyframe_needed()
|
||||
|
||||
def _preferred_topics(self) -> list[str]:
|
||||
# WebRTC currently forces H.264. The dedicated livestream topics are the H.264 feeds,
|
||||
# while the main encode topics are the full-resolution HEVC recordings. Prefer the
|
||||
# livestream feeds both onroad and offroad, and keep the main topics only as fallback.
|
||||
return [
|
||||
self.livestream_camera_to_sock_mapping[self._camera_type],
|
||||
self.main_camera_to_sock_mapping[self._camera_type],
|
||||
]
|
||||
|
||||
def _reset_decoder_state(self, topic: str) -> None:
|
||||
if topic == self._active_topic:
|
||||
return
|
||||
self._logger.info("[%s] switching video source from %s to %s", self._id, self._active_topic, topic)
|
||||
self._active_topic = topic
|
||||
self._cached_header = b""
|
||||
self._sent_keyframe = False
|
||||
|
||||
def _timing_sei(self, evta, log_mono_time: int) -> bytes:
|
||||
"""Build a timing SEI NAL from encode metadata, or empty bytes when disabled."""
|
||||
if not self.timing_sei_enabled:
|
||||
return b""
|
||||
idx = evta.idx
|
||||
return _SEI_PREFIX + struct.pack(
|
||||
'>4d',
|
||||
(idx.timestampEof - idx.timestampSof) / 1e6, # encode duration (ms)
|
||||
(log_mono_time - idx.timestampEof) / 1e6, # IPC/queue delay (ms)
|
||||
(time.monotonic_ns() - log_mono_time) / 1e6, # host transit so far (ms)
|
||||
time.time() * 1000, # device wall clock (ms) # noqa: TID251
|
||||
) + b'\x80'
|
||||
|
||||
def _is_keyframe(self, data: bytes) -> bool:
|
||||
"""Check if H.264 NAL unit contains an IDR keyframe (NAL type 5)."""
|
||||
i = 0
|
||||
while i < len(data) - 4:
|
||||
# Look for Annex B start codes: 0x000001 or 0x00000001
|
||||
if data[i:i+3] == b'\x00\x00\x01':
|
||||
nal_type = data[i+3] & 0x1f
|
||||
if nal_type == 5: # IDR slice
|
||||
return True
|
||||
i += 3
|
||||
elif data[i:i+4] == b'\x00\x00\x00\x01':
|
||||
nal_type = data[i+4] & 0x1f
|
||||
if nal_type == 5: # IDR slice
|
||||
return True
|
||||
i += 4
|
||||
else:
|
||||
i += 1
|
||||
return False
|
||||
|
||||
async def recv(self):
|
||||
while True:
|
||||
now = time.monotonic()
|
||||
# Resolve topics each iteration: a camera switch (different async task) can rebuild self._socks
|
||||
# across the await below, so a value cached before the loop would index a stale key (KeyError).
|
||||
live_topic = self.livestream_camera_to_sock_mapping[self._camera_type]
|
||||
main_topic = self.main_camera_to_sock_mapping[self._camera_type]
|
||||
# Lock onto the dedicated H264 livestream feed. Onroad the HEVC main feed also publishes at
|
||||
# 20fps; eagerly preferring whichever socket had a frame ready raced frame-by-frame, reset the
|
||||
# decoder every frame, and (the track is negotiated H264) shoved HEVC garbage into the stream —
|
||||
# the onroad choppiness. Only fall back to the main feed as a last resort after a long
|
||||
# livestream silence (e.g. stream_encoderd still spinning up), and snap back when it returns.
|
||||
msg = messaging.recv_one_or_none(self._socks[live_topic])
|
||||
if msg is not None:
|
||||
self._reset_decoder_state(live_topic)
|
||||
self._last_frame_time = now
|
||||
self._live_silent_since = None
|
||||
break
|
||||
|
||||
if self._live_silent_since is None:
|
||||
self._live_silent_since = now
|
||||
elif now - self._live_silent_since > 3.0:
|
||||
maybe_msg = messaging.recv_one_or_none(self._socks[main_topic])
|
||||
if maybe_msg is not None:
|
||||
self._reset_decoder_state(main_topic)
|
||||
self._last_frame_time = now
|
||||
msg = maybe_msg
|
||||
break
|
||||
|
||||
await asyncio.sleep(0.005)
|
||||
|
||||
evta = getattr(msg, msg.which())
|
||||
|
||||
header = bytes(evta.header)
|
||||
data = bytes(evta.data)
|
||||
self._frame_count += 1
|
||||
|
||||
# Cache SPS/PPS header when it arrives
|
||||
if header:
|
||||
self._cached_header = header
|
||||
self._logger.debug(f"[{self._id}] cached SPS/PPS header ({len(header)} bytes)")
|
||||
|
||||
# CRITICAL: Cannot decode without SPS/PPS. Wait for it.
|
||||
if not self._cached_header:
|
||||
self._logger.debug(f"[{self._id}] frame {self._frame_count}: no SPS/PPS yet, skipping")
|
||||
return await self.recv()
|
||||
|
||||
is_keyframe = self._is_keyframe(data)
|
||||
|
||||
# Wait for first keyframe before sending any frames
|
||||
# Browser decoder needs IDR to initialize properly
|
||||
if not self._sent_keyframe:
|
||||
if not is_keyframe:
|
||||
self._logger.debug(f"[{self._id}] frame {self._frame_count}: waiting for keyframe")
|
||||
return await self.recv()
|
||||
self._sent_keyframe = True
|
||||
# Got the IDR we asked for — stop nagging the encoder, but only once every
|
||||
# concurrent track has its keyframe (multi-track PiP shares the global param).
|
||||
self._mark_keyframe_received()
|
||||
self._logger.info(f"[{self._id}] first keyframe received, starting stream")
|
||||
|
||||
# Optional timing SEI NAL, inserted before the slice data (and after SPS/PPS on keyframes).
|
||||
sei_nal = self._timing_sei(evta, msg.logMonoTime)
|
||||
|
||||
# Prepend SPS/PPS header to keyframes (required by some decoders)
|
||||
# For non-keyframes, header is optional but safe to include
|
||||
if is_keyframe:
|
||||
payload = self._cached_header + sei_nal + data
|
||||
else:
|
||||
payload = sei_nal + data
|
||||
|
||||
self._pts = ((time.monotonic_ns() - self._t0_ns) * self._clock_rate) // 1_000_000_000
|
||||
|
||||
packet = av.Packet(payload)
|
||||
packet.time_base = self._time_base
|
||||
packet.pts = int(self._pts)
|
||||
packet.dts = int(self._pts)
|
||||
packet.duration = int(self._dt * self._clock_rate)
|
||||
|
||||
if is_keyframe:
|
||||
packet.is_keyframe = True
|
||||
|
||||
self.log_debug("track sending frame %s (keyframe=%s, size=%d)", self._pts, is_keyframe, len(payload))
|
||||
|
||||
return packet
|
||||
|
||||
def codec_preference(self) -> str | None:
|
||||
return "H264"
|
||||
@@ -1,4 +1,4 @@
|
||||
# ruff: noqa: TID251, UP006, UP035
|
||||
# ruff: noqa: UP006, UP035
|
||||
|
||||
import abc
|
||||
from typing import Dict, List, Optional
|
||||
@@ -1,4 +1,4 @@
|
||||
# ruff: noqa: TID251, UP006, UP035
|
||||
# ruff: noqa: UP006, UP035
|
||||
|
||||
import abc
|
||||
import asyncio
|
||||
@@ -14,6 +14,7 @@ from libdatachannel import (
|
||||
Candidate,
|
||||
Description,
|
||||
FrameInfo,
|
||||
H264RtpDepacketizer,
|
||||
H264RtpPacketizer,
|
||||
IceServer,
|
||||
NalUnit,
|
||||
@@ -100,7 +101,6 @@ class WebRTCBaseStream(abc.ABC):
|
||||
|
||||
@staticmethod
|
||||
def _make_ice_servers(servers: Optional[List[dict]]) -> List[IceServer]:
|
||||
"""Preserve Konn3kt's authenticated STUN/TURN configuration in libdatachannel."""
|
||||
parsed: List[IceServer] = []
|
||||
for server in servers or []:
|
||||
urls = server.get("urls", []) if isinstance(server, dict) else []
|
||||
@@ -119,7 +119,9 @@ class WebRTCBaseStream(abc.ABC):
|
||||
except Exception:
|
||||
logging.getLogger("WebRTCStream").warning("Ignoring invalid ICE server %r", url, exc_info=True)
|
||||
# A supplied list is authoritative, including LAN-only sessions where relay is deliberately filtered.
|
||||
return parsed or [IceServer("stun:stun.l.google.com:19302")]
|
||||
if servers is not None:
|
||||
return parsed
|
||||
return [IceServer("stun:stun.l.google.com:19302")]
|
||||
|
||||
def _log_debug(self, msg: Any, *args):
|
||||
self.logger.debug(f"{type(self)}() {msg}", *args)
|
||||
@@ -144,13 +146,14 @@ class WebRTCBaseStream(abc.ABC):
|
||||
media = Description.Video(camera_type, Description.Direction.RecvOnly)
|
||||
media.add_h264_codec(96)
|
||||
track = self.peer_connection.add_track(media)
|
||||
track.set_media_handler(OpusRtpDepacketizer())
|
||||
track.set_media_handler(H264RtpDepacketizer())
|
||||
self._consumer_tracks.append(track)
|
||||
self.incoming_camera_tracks[camera_type] = track
|
||||
if self.expected_incoming_audio:
|
||||
media = Description.Audio("audio", Description.Direction.RecvOnly)
|
||||
media.add_opus_codec(111)
|
||||
track = self.peer_connection.add_track(media)
|
||||
track.set_media_handler(OpusRtpDepacketizer())
|
||||
self._consumer_tracks.append(track)
|
||||
self.incoming_audio_tracks.append(track)
|
||||
|
||||
@@ -170,7 +173,7 @@ class WebRTCBaseStream(abc.ABC):
|
||||
def _make_video_media(self, track: TiciVideoStreamTrack, remote_sdp: str) -> Tuple[Description.Video, int, int, str]:
|
||||
mid, payload_type = self._find_offer_video(remote_sdp)
|
||||
ssrc = random.randint(1, 0xFFFFFFFF)
|
||||
cname = f"teleoprtc-{random.getrandbits(32):08x}"
|
||||
cname = f"iqpilot-video-{random.getrandbits(32):08x}"
|
||||
stream_id = f"stream-{random.getrandbits(32):08x}"
|
||||
media = Description.Video(mid, Description.Direction.SendOnly)
|
||||
media.add_h264_codec(payload_type)
|
||||
@@ -193,7 +196,7 @@ class WebRTCBaseStream(abc.ABC):
|
||||
def _make_audio_media(self, remote_sdp: str) -> Tuple[Description.Audio, int, int, str]:
|
||||
mid, payload_type = self._find_offer_audio(remote_sdp)
|
||||
ssrc = random.randint(1, 0xFFFFFFFF)
|
||||
cname = f"teleoprtc-audio-{random.getrandbits(32):08x}"
|
||||
cname = f"iqpilot-audio-{random.getrandbits(32):08x}"
|
||||
direction = Description.Direction.SendRecv if self.expected_incoming_audio else Description.Direction.SendOnly
|
||||
media = Description.Audio(mid, direction)
|
||||
media.add_opus_codec(payload_type)
|
||||
@@ -352,7 +355,6 @@ class WebRTCBaseStream(abc.ABC):
|
||||
self.incoming_message_handlers.append(message_handler)
|
||||
|
||||
def add_ice_candidate(self, candidate: dict) -> None:
|
||||
"""Accept post-offer browser candidates for Konn3kt's existing trickle endpoint."""
|
||||
candidate_sdp = str(candidate.get("candidate") or "") if isinstance(candidate, dict) else ""
|
||||
if not candidate_sdp:
|
||||
return
|
||||
@@ -32,6 +32,7 @@ class TiciVideoStreamTrack:
|
||||
def __init__(self, camera_type: str, dt: float, time_base: fractions.Fraction = VIDEO_TIME_BASE, clock_rate: int = VIDEO_CLOCK_RATE):
|
||||
assert camera_type in ["driver", "wideRoad", "road"]
|
||||
self._id: str = video_track_id(camera_type, str(uuid.uuid4()))
|
||||
self._dt = dt
|
||||
self._time_base: fractions.Fraction = time_base
|
||||
self._clock_rate: int = clock_rate
|
||||
self._logger = logging.getLogger("WebRTCStream")
|
||||
@@ -14,7 +14,6 @@ from iqpilot.system.webrtc.webrtcd import CerealIncomingMessageProxy, CerealOutg
|
||||
|
||||
|
||||
def _default_route_ip() -> str | None:
|
||||
"""Use the interface the kernel will actually use for Internet/relay media."""
|
||||
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
try:
|
||||
sock.connect(("8.8.8.8", 53))
|
||||
@@ -25,9 +24,7 @@ def _default_route_ip() -> str | None:
|
||||
sock.close()
|
||||
|
||||
|
||||
class LibdatachannelBitrateController:
|
||||
"""Loss-driven bitrate control using native RTCP receiver reports."""
|
||||
|
||||
class LivestreamBitrateController:
|
||||
bitrates = [500_000, 1_500_000, int(os.environ.get("STREAM_BITRATE", 5_000_000))]
|
||||
label_to_bitrate = {"low": bitrates[0], "med": bitrates[1], "high": bitrates[2]}
|
||||
sample_interval = 0.2
|
||||
@@ -103,45 +100,45 @@ class LibdatachannelBitrateController:
|
||||
self._publish(self.bitrates[self._level])
|
||||
|
||||
|
||||
class StreamSessionLibdatachannel:
|
||||
"""IQ.Pilot's libdatachannel stream path. It preserves Konn3kt signalling and controls."""
|
||||
|
||||
shared_pub_master = DynamicPubMaster([])
|
||||
class StreamSession:
|
||||
shared_pub_master: DynamicPubMaster | None = None
|
||||
|
||||
def __init__(self, sdp: str, cameras: list[str], incoming_services: list[str], outgoing_services: list[str],
|
||||
ice_servers: list[dict[str, Any]] | None = None, debug_mode: bool = False, ui_stream: bool = False):
|
||||
from iqpilot.system.webrtc.device.video_ldc import LiveStreamVideoStreamTrack
|
||||
from iqpilot.system.webrtc.teleoprtc_ldc.builder import WebRTCAnswerBuilder
|
||||
from iqpilot.system.webrtc.teleoprtc_ldc.info import parse_info_from_offer
|
||||
from iqpilot.system.webrtc.device.native_audio import AudioInputOpusProducer, DebugAudioOpusProducer
|
||||
from iqpilot.system.webrtc.device.native_video import DebugVideoStreamTrack, LiveStreamVideoStreamTrack
|
||||
from iqpilot.system.webrtc.rtc.builder import WebRTCAnswerBuilder
|
||||
from iqpilot.system.webrtc.rtc.info import parse_info_from_offer
|
||||
|
||||
config = parse_info_from_offer(sdp)
|
||||
if len(cameras) != config.n_expected_camera_tracks:
|
||||
raise ValueError("Incoming stream has misconfigured number of video tracks")
|
||||
if debug_mode:
|
||||
raise ValueError("libdatachannel debug tracks are not supported")
|
||||
|
||||
builder = WebRTCAnswerBuilder(sdp, bind_address=_default_route_ip(), ice_servers=ice_servers or [])
|
||||
self.video_tracks = [LiveStreamVideoStreamTrack(camera) for camera in cameras]
|
||||
video_track_type = DebugVideoStreamTrack if debug_mode else LiveStreamVideoStreamTrack
|
||||
self.video_tracks = [video_track_type(camera) for camera in cameras]
|
||||
for camera, track in zip(cameras, self.video_tracks, strict=True):
|
||||
builder.add_video_stream(camera, track)
|
||||
|
||||
# The browser uses a single sendrecv audio m-line. libdatachannel's Python binding
|
||||
# currently cannot negotiate that bidirectional track reliably, so this experimental
|
||||
# transport deliberately remains video/control-only. The default aiortc path keeps
|
||||
# both audio directions until the native duplex path passes the same integration test.
|
||||
self.audio_output = None
|
||||
audio_track_type = DebugAudioOpusProducer if debug_mode else AudioInputOpusProducer
|
||||
self.audio_output = audio_track_type() if config.expected_audio_track else None
|
||||
if self.audio_output is not None:
|
||||
builder.add_audio_stream(self.audio_output)
|
||||
if config.incoming_audio_track:
|
||||
builder.offer_to_receive_audio_stream()
|
||||
self.stream = builder.stream()
|
||||
|
||||
self.identifier = str(uuid.uuid4())
|
||||
self.incoming_bridge_services = incoming_services
|
||||
self.incoming_bridge = CerealIncomingMessageProxy(self.shared_pub_master) if incoming_services else None
|
||||
if incoming_services and self.shared_pub_master is None:
|
||||
StreamSession.shared_pub_master = DynamicPubMaster([])
|
||||
self.incoming_bridge = CerealIncomingMessageProxy(self.shared_pub_master) if self.shared_pub_master is not None and incoming_services else None
|
||||
self.outgoing_bridge = CerealOutgoingMessageProxy(messaging.SubMaster(outgoing_services)) if outgoing_services else None
|
||||
self.outgoing_bridge_runner = CerealProxyRunner(self.outgoing_bridge) if self.outgoing_bridge is not None else None
|
||||
self.ui_stream_requested = ui_stream
|
||||
self.ui_stream_runner: CerealProxyRunner | None = None
|
||||
self.audio_input_proxy = None
|
||||
self.audio_recv_requested = False
|
||||
self.bitrate_controller = LibdatachannelBitrateController(self.stream.get_receiver_report_stats)
|
||||
self.audio_recv_requested = config.incoming_audio_track
|
||||
self.bitrate_controller = LivestreamBitrateController(self.stream.get_receiver_report_stats)
|
||||
self.run_task: asyncio.Task | None = None
|
||||
self._cleanup_lock = asyncio.Lock()
|
||||
self._cleanup_done = False
|
||||
@@ -216,7 +213,7 @@ class StreamSessionLibdatachannel:
|
||||
if self.ui_stream_requested:
|
||||
self.set_ui_stream(True)
|
||||
if self.audio_recv_requested and self.stream.has_incoming_audio_track():
|
||||
from iqpilot.system.webrtc.device.audio_ldc import IncomingOpusCerealProxy
|
||||
from iqpilot.system.webrtc.device.native_audio import IncomingOpusCerealProxy
|
||||
self.audio_input_proxy = IncomingOpusCerealProxy(self.stream.get_incoming_audio_track())
|
||||
self.audio_input_proxy.start()
|
||||
self.bitrate_controller.start()
|
||||
96
iqpilot/system/webrtc/tests/test_native_session.py
Normal file
96
iqpilot/system/webrtc/tests/test_native_session.py
Normal file
@@ -0,0 +1,96 @@
|
||||
from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
|
||||
from iqpilot.system.webrtc import session as session_module
|
||||
|
||||
|
||||
class FakeStream:
|
||||
def get_receiver_report_stats(self):
|
||||
return {}
|
||||
|
||||
|
||||
class FakeBuilder:
|
||||
instance = None
|
||||
|
||||
def __init__(self, sdp, bind_address=None, ice_servers=None):
|
||||
self.sdp = sdp
|
||||
self.bind_address = bind_address
|
||||
self.ice_servers = ice_servers
|
||||
self.video = []
|
||||
self.audio = []
|
||||
self.receive_audio = False
|
||||
self.result = FakeStream()
|
||||
FakeBuilder.instance = self
|
||||
|
||||
def add_video_stream(self, camera, track):
|
||||
self.video.append((camera, track))
|
||||
|
||||
def add_audio_stream(self, track):
|
||||
self.audio.append(track)
|
||||
|
||||
def offer_to_receive_audio_stream(self):
|
||||
self.receive_audio = True
|
||||
|
||||
def stream(self):
|
||||
return self.result
|
||||
|
||||
|
||||
class FakeVideoTrack:
|
||||
def __init__(self, camera):
|
||||
self.camera = camera
|
||||
self.timing_sei_enabled = False
|
||||
self.switched = None
|
||||
|
||||
def switch_camera(self, camera):
|
||||
self.switched = camera
|
||||
|
||||
|
||||
class FakeAudioProducer:
|
||||
def __init__(self):
|
||||
self.enabled = True
|
||||
|
||||
def enable(self, enabled):
|
||||
self.enabled = enabled
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def native_session(mocker):
|
||||
config = SimpleNamespace(
|
||||
n_expected_camera_tracks=1,
|
||||
expected_audio_track=True,
|
||||
incoming_audio_track=True,
|
||||
incoming_datachannel=True,
|
||||
)
|
||||
mocker.patch("iqpilot.system.webrtc.rtc.info.parse_info_from_offer", return_value=config)
|
||||
mocker.patch("iqpilot.system.webrtc.rtc.builder.WebRTCAnswerBuilder", FakeBuilder)
|
||||
mocker.patch("iqpilot.system.webrtc.device.native_video.LiveStreamVideoStreamTrack", FakeVideoTrack)
|
||||
mocker.patch("iqpilot.system.webrtc.device.native_audio.AudioInputOpusProducer", FakeAudioProducer)
|
||||
mocker.patch.object(session_module, "_default_route_ip", return_value="192.0.2.1")
|
||||
mocker.patch.object(session_module, "Params", return_value=mocker.Mock())
|
||||
return session_module.StreamSession(
|
||||
"offer", ["road"], [], [], [{"urls": "turn:example.com"}], ui_stream=False,
|
||||
)
|
||||
|
||||
|
||||
def test_native_session_builds_duplex_audio(native_session):
|
||||
builder = FakeBuilder.instance
|
||||
assert builder is not None
|
||||
assert builder.bind_address == "192.0.2.1"
|
||||
assert builder.ice_servers == [{"urls": "turn:example.com"}]
|
||||
assert [camera for camera, _ in builder.video] == ["road"]
|
||||
assert builder.audio == [native_session.audio_output]
|
||||
assert builder.receive_audio
|
||||
assert native_session.audio_recv_requested
|
||||
|
||||
|
||||
def test_native_session_controls(native_session, mocker):
|
||||
native_session.bitrate_controller = mocker.Mock()
|
||||
native_session.message_handler('{"type":"timingSei","enabled":true}')
|
||||
assert native_session.video_tracks[0].timing_sei_enabled
|
||||
native_session.message_handler('{"type":"switchCamera","camera":"driver"}')
|
||||
assert native_session.video_tracks[0].switched == "driver"
|
||||
native_session.message_handler('{"type":"setAudioEnabled","enabled":false}')
|
||||
assert not native_session.audio_output.enabled
|
||||
native_session.message_handler('{"type":"setQuality","quality":"low"}')
|
||||
native_session.bitrate_controller.set_quality.assert_called_once_with("low")
|
||||
116
iqpilot/system/webrtc/tests/test_rtc.py
Normal file
116
iqpilot/system/webrtc/tests/test_rtc.py
Normal file
@@ -0,0 +1,116 @@
|
||||
import pytest
|
||||
|
||||
from iqpilot.system.webrtc.rtc.info import parse_info_from_offer
|
||||
from iqpilot.system.webrtc.rtc.stream import WebRTCBaseStream
|
||||
from iqpilot.system.webrtc.rtc.tracks import TiciTrackWrapper, TiciVideoStreamTrack, parse_video_track_id, video_track_id
|
||||
|
||||
|
||||
def sdp_with_media(media):
|
||||
mids = " ".join(str(i) for i in range(len(media)))
|
||||
sections = []
|
||||
for index, (kind, direction) in enumerate(media):
|
||||
if kind == "video":
|
||||
payload, mapping = "96", "H264/90000"
|
||||
protocol = "UDP/TLS/RTP/SAVPF"
|
||||
elif kind == "audio":
|
||||
payload, mapping = "111", "opus/48000/2"
|
||||
protocol = "UDP/TLS/RTP/SAVPF"
|
||||
else:
|
||||
sections.extend([
|
||||
"m=application 9 UDP/DTLS/SCTP webrtc-datachannel",
|
||||
"c=IN IP4 0.0.0.0",
|
||||
f"a=mid:{index}",
|
||||
"a=sctp-port:5000",
|
||||
])
|
||||
continue
|
||||
sections.extend([
|
||||
f"m={kind} 9 {protocol} {payload}",
|
||||
"c=IN IP4 0.0.0.0",
|
||||
f"a=mid:{index}",
|
||||
f"a={direction}",
|
||||
f"a=rtpmap:{payload} {mapping}",
|
||||
"a=rtcp-mux",
|
||||
])
|
||||
lines = [
|
||||
"v=0",
|
||||
"o=- 1 1 IN IP4 0.0.0.0",
|
||||
"s=-",
|
||||
"t=0 0",
|
||||
f"a=group:BUNDLE {mids}",
|
||||
*sections,
|
||||
]
|
||||
return "\r\n".join(lines) + "\r\n"
|
||||
|
||||
|
||||
@pytest.mark.parametrize("direction,expected_outgoing,expected_incoming", [
|
||||
("recvonly", True, False),
|
||||
("sendonly", False, True),
|
||||
("sendrecv", True, True),
|
||||
("inactive", False, False),
|
||||
])
|
||||
def test_audio_directions(direction, expected_outgoing, expected_incoming):
|
||||
info = parse_info_from_offer(sdp_with_media([("audio", direction)]))
|
||||
assert info.expected_audio_track == expected_outgoing
|
||||
assert info.incoming_audio_track == expected_incoming
|
||||
|
||||
|
||||
def test_video_and_data_channel_metadata():
|
||||
info = parse_info_from_offer(sdp_with_media([
|
||||
("video", "recvonly"),
|
||||
("video", "sendrecv"),
|
||||
("application", "sendrecv"),
|
||||
]))
|
||||
assert info.n_expected_camera_tracks == 2
|
||||
assert info.incoming_datachannel
|
||||
|
||||
|
||||
def test_explicit_empty_ice_servers_disable_defaults():
|
||||
assert WebRTCBaseStream._make_ice_servers([]) == []
|
||||
|
||||
|
||||
def test_default_ice_server():
|
||||
servers = WebRTCBaseStream._make_ice_servers(None)
|
||||
assert len(servers) == 1
|
||||
assert servers[0].hostname == "stun.l.google.com"
|
||||
assert servers[0].port == 19302
|
||||
|
||||
|
||||
def test_authenticated_ice_servers():
|
||||
servers = WebRTCBaseStream._make_ice_servers([{
|
||||
"urls": ["turn:relay.example.com:3478", "stun:stun.example.com:3478"],
|
||||
"username": "user",
|
||||
"credential": "secret",
|
||||
}])
|
||||
assert [(server.hostname, server.port) for server in servers] == [
|
||||
("relay.example.com", 3478),
|
||||
("stun.example.com", 3478),
|
||||
]
|
||||
assert all(server.username == "user" and server.password == "secret" for server in servers)
|
||||
|
||||
|
||||
def test_track_id_roundtrip():
|
||||
assert parse_video_track_id(video_track_id("driver", "track")) == ("driver", "track")
|
||||
|
||||
|
||||
def test_invalid_track_id():
|
||||
with pytest.raises(ValueError):
|
||||
parse_video_track_id("driver")
|
||||
|
||||
|
||||
def test_track_wrapper_preserves_camera():
|
||||
class Track:
|
||||
kind = "video"
|
||||
id = "source"
|
||||
|
||||
async def recv(self):
|
||||
return b"frame"
|
||||
|
||||
wrapper = TiciTrackWrapper("road", Track())
|
||||
assert parse_video_track_id(wrapper.id)[0] == "road"
|
||||
wrapper.stop()
|
||||
assert wrapper.readyState == "ended"
|
||||
|
||||
|
||||
def test_track_stores_frame_period():
|
||||
track = TiciVideoStreamTrack("wideRoad", 0.05)
|
||||
assert track._dt == 0.05
|
||||
79
iqpilot/system/webrtc/tests/test_rtc_integration.py
Normal file
79
iqpilot/system/webrtc/tests/test_rtc_integration.py
Normal file
@@ -0,0 +1,79 @@
|
||||
import asyncio
|
||||
import os
|
||||
|
||||
import pytest
|
||||
|
||||
from iqpilot.system.webrtc.rtc import WebRTCOfferBuilder
|
||||
from iqpilot.system.webrtc.rtc.stream import RTCSessionDescription
|
||||
from iqpilot.system.webrtc.session import StreamSession
|
||||
|
||||
|
||||
@pytest.mark.asyncio
|
||||
async def test_native_video_audio_and_data_channel():
|
||||
if not os.environ.get("CI"):
|
||||
return
|
||||
|
||||
answer_session = None
|
||||
video_received = asyncio.Event()
|
||||
audio_received = asyncio.Event()
|
||||
|
||||
async def connect(offer):
|
||||
nonlocal answer_session
|
||||
answer_session = StreamSession(offer.sdp, offer.video, [], [], [], debug_mode=True)
|
||||
answer = await answer_session.get_answer()
|
||||
answer_session.start()
|
||||
return RTCSessionDescription(answer.sdp, answer.type)
|
||||
|
||||
builder = WebRTCOfferBuilder(connect, ice_servers=[])
|
||||
builder.offer_to_receive_video_stream("road")
|
||||
builder.offer_to_receive_audio_stream()
|
||||
builder.add_messaging()
|
||||
stream = builder.stream()
|
||||
|
||||
try:
|
||||
await asyncio.wait_for(stream.start(), 10)
|
||||
stream.get_incoming_video_track("road").on_frame(lambda *_: video_received.set())
|
||||
stream.get_incoming_audio_track().on_frame(lambda *_: audio_received.set())
|
||||
await asyncio.wait_for(stream.wait_for_connection(), 10)
|
||||
await asyncio.wait_for(video_received.wait(), 10)
|
||||
await asyncio.wait_for(audio_received.wait(), 10)
|
||||
stream.get_messaging_channel().send('{"type":"timingSei","enabled":true}')
|
||||
await asyncio.sleep(0.1)
|
||||
assert answer_session is not None
|
||||
assert answer_session.video_tracks[0].timing_sei_enabled
|
||||
finally:
|
||||
await stream.stop()
|
||||
if answer_session is not None:
|
||||
await answer_session.stop_async()
|
||||
|
||||
|
||||
@pytest.mark.asyncio
|
||||
async def test_native_duplex_audio_negotiation():
|
||||
if not os.environ.get("CI"):
|
||||
return
|
||||
|
||||
answer_session = None
|
||||
|
||||
async def connect(offer):
|
||||
nonlocal answer_session
|
||||
sendrecv_offer = offer.sdp.replace("a=recvonly", "a=sendrecv", 1)
|
||||
answer_session = StreamSession(sendrecv_offer, offer.video, [], [], [], debug_mode=True)
|
||||
answer = await answer_session.get_answer()
|
||||
answer_session.start()
|
||||
return RTCSessionDescription(answer.sdp, answer.type)
|
||||
|
||||
builder = WebRTCOfferBuilder(connect, ice_servers=[])
|
||||
builder.offer_to_receive_audio_stream()
|
||||
stream = builder.stream()
|
||||
|
||||
try:
|
||||
await asyncio.wait_for(stream.start(), 10)
|
||||
await asyncio.wait_for(stream.wait_for_connection(), 10)
|
||||
assert stream.has_incoming_audio_track()
|
||||
assert answer_session is not None
|
||||
assert answer_session.audio_recv_requested
|
||||
assert answer_session.audio_output is not None
|
||||
finally:
|
||||
await stream.stop()
|
||||
if answer_session is not None:
|
||||
await answer_session.stop_async()
|
||||
@@ -1,19 +1,14 @@
|
||||
import asyncio
|
||||
import gc
|
||||
import json
|
||||
import time
|
||||
# for aiortc and its dependencies
|
||||
import warnings
|
||||
warnings.filterwarnings("ignore", category=DeprecationWarning)
|
||||
warnings.filterwarnings("ignore", category=RuntimeWarning) # TODO: remove this when google-crc32c publish a python3.12 wheel
|
||||
|
||||
from aiortc import RTCDataChannel
|
||||
from aiortc.mediastreams import VIDEO_CLOCK_RATE, VIDEO_TIME_BASE
|
||||
import capnp
|
||||
from iqpilot.cereal import messaging, log
|
||||
|
||||
from iqpilot.system.webrtc.webrtcd import CerealOutgoingMessageProxy, CerealIncomingMessageProxy
|
||||
from iqpilot.system.webrtc.device.video import LiveStreamVideoStreamTrack
|
||||
from iqpilot.system.webrtc.device.audio import AudioInputStreamTrack
|
||||
from iqpilot.system.webrtc.device.native_audio import AudioInputOpusProducer, DebugAudioOpusProducer
|
||||
from iqpilot.system.webrtc.device.native_video import DebugVideoStreamTrack, LiveStreamVideoStreamTrack
|
||||
from iqpilot.system.webrtc.rtc.tracks import VIDEO_TIME_BASE
|
||||
|
||||
|
||||
class TestStreamSession:
|
||||
@@ -23,6 +18,7 @@ class TestStreamSession:
|
||||
def teardown_method(self):
|
||||
self.loop.stop()
|
||||
self.loop.close()
|
||||
gc.collect()
|
||||
|
||||
def test_outgoing_proxy(self, mocker):
|
||||
test_msg = log.Event.new_message()
|
||||
@@ -32,12 +28,12 @@ class TestStreamSession:
|
||||
expected_dict = {"type": "customReservedRawData0", "logMonoTime": 123, "valid": True, "data": "test"}
|
||||
expected_json = json.dumps(expected_dict).encode()
|
||||
|
||||
channel = mocker.Mock(spec=RTCDataChannel)
|
||||
mocked_submaster = messaging.SubMaster(["customReservedRawData0"])
|
||||
def mocked_update(t):
|
||||
mocked_submaster.update_msgs(0, [test_msg])
|
||||
|
||||
mocker.patch.object(messaging.SubMaster, "update", side_effect=mocked_update)
|
||||
channel = mocker.Mock()
|
||||
mocked_submaster = mocker.MagicMock()
|
||||
mocked_submaster.updated = {"customReservedRawData0": True}
|
||||
mocked_submaster.logMonoTime = {"customReservedRawData0": 123}
|
||||
mocked_submaster.valid = {"customReservedRawData0": True}
|
||||
mocked_submaster.__getitem__.return_value = test_msg.customReservedRawData0
|
||||
proxy = CerealOutgoingMessageProxy(mocked_submaster)
|
||||
proxy.add_channel(channel)
|
||||
|
||||
@@ -47,9 +43,9 @@ class TestStreamSession:
|
||||
|
||||
def test_incoming_proxy(self, mocker):
|
||||
tested_msgs = [
|
||||
{"type": "customReservedRawData0", "data": "test"}, # primitive
|
||||
{"type": "can", "data": [{"address": 0, "dat": "", "src": 0}]}, # list
|
||||
{"type": "testJoystick", "data": {"axes": [0, 0], "buttons": [False]}}, # dict
|
||||
{"type": "customReservedRawData0", "data": "test"},
|
||||
{"type": "can", "data": [{"address": 0, "dat": "", "src": 0}]},
|
||||
{"type": "testJoystick", "data": {"axes": [0, 0], "buttons": [False]}},
|
||||
]
|
||||
|
||||
mocked_pubmaster = mocker.MagicMock(spec=messaging.PubMaster)
|
||||
@@ -72,20 +68,15 @@ class TestStreamSession:
|
||||
fake_msg.livestreamDriverEncodeData.header = b"header"
|
||||
fake_msg.livestreamDriverEncodeData.data = b"\x00\x00\x00\x01\x65"
|
||||
|
||||
mocker.patch("iqpilot.system.webrtc.device.video.messaging.recv_one_or_none", return_value=fake_msg)
|
||||
mocker.patch("iqpilot.system.webrtc.device.native_video.messaging.sub_sock", return_value=mocker.Mock())
|
||||
mocker.patch("iqpilot.system.webrtc.device.native_video.messaging.recv_one_or_none", return_value=fake_msg)
|
||||
track = LiveStreamVideoStreamTrack("driver")
|
||||
|
||||
assert track.id.startswith("driver")
|
||||
assert track.codec_preference() == "H264"
|
||||
|
||||
for i in range(5):
|
||||
packet = self.loop.run_until_complete(track.recv())
|
||||
assert packet.time_base == VIDEO_TIME_BASE
|
||||
if i == 0:
|
||||
start_ns = time.monotonic_ns()
|
||||
start_pts = packet.pts
|
||||
assert abs(i + packet.pts - (start_pts + (((time.monotonic_ns() - start_ns) * VIDEO_CLOCK_RATE) // 1_000_000_000))) < 450 #5ms
|
||||
assert packet.size == len(b"header\x00\x00\x00\x01\x65")
|
||||
packet = self.loop.run_until_complete(track.recv())
|
||||
assert packet.time_base == VIDEO_TIME_BASE
|
||||
assert packet.pts is not None
|
||||
assert packet.size == len(b"header\x00\x00\x00\x01\x65")
|
||||
|
||||
def test_input_audio_track(self, mocker):
|
||||
packet_time, rate = 0.02, 16000
|
||||
@@ -93,11 +84,27 @@ class TestStreamSession:
|
||||
fake_msg = messaging.new_message("rawAudioData")
|
||||
fake_msg.rawAudioData.data = b"\x00" * 2 * sample_count
|
||||
fake_msg.rawAudioData.sampleRate = rate
|
||||
mocker.patch("iqpilot.system.webrtc.device.audio.messaging.recv_one_or_none", return_value=fake_msg)
|
||||
track = AudioInputStreamTrack(rate=rate)
|
||||
mocker.patch("iqpilot.system.webrtc.device.native_audio.messaging.sub_sock", return_value=mocker.Mock())
|
||||
track = AudioInputOpusProducer()
|
||||
track._source_rate = rate
|
||||
mocker.patch("iqpilot.system.webrtc.device.native_audio.messaging.recv_one_or_none", return_value=fake_msg)
|
||||
|
||||
for i in range(5):
|
||||
frame = self.loop.run_until_complete(track.recv())
|
||||
assert frame.rate == rate
|
||||
assert frame.samples == sample_count
|
||||
assert frame.pts == i * sample_count
|
||||
packet = self.loop.run_until_complete(track.recv())
|
||||
assert packet is not None
|
||||
payload, pts = packet
|
||||
assert payload
|
||||
assert pts >= 0
|
||||
|
||||
def test_debug_video_track(self):
|
||||
track = DebugVideoStreamTrack("road")
|
||||
packet = self.loop.run_until_complete(track.recv())
|
||||
assert packet.size > 0
|
||||
assert packet.pts == 0
|
||||
|
||||
def test_debug_audio_track(self):
|
||||
track = DebugAudioOpusProducer()
|
||||
packet = self.loop.run_until_complete(track.recv())
|
||||
assert packet is not None
|
||||
payload, pts = packet
|
||||
assert payload
|
||||
assert pts == 0
|
||||
|
||||
@@ -1,6 +1,9 @@
|
||||
import asyncio
|
||||
import json
|
||||
import math
|
||||
|
||||
import pytest
|
||||
|
||||
from iqpilot.cereal import log, messaging
|
||||
from iqpilot.system.webrtc.ui_stream import (
|
||||
UI_STREAM_SERVICES,
|
||||
@@ -270,11 +273,13 @@ class TestUIStreamFrame:
|
||||
|
||||
|
||||
class TestSessionWiring:
|
||||
def test_set_ui_stream_control_message(self):
|
||||
import asyncio
|
||||
@pytest.mark.asyncio
|
||||
async def test_set_ui_stream_control_message(self, mocker):
|
||||
import logging
|
||||
from types import SimpleNamespace
|
||||
from iqpilot.system.webrtc.webrtcd import StreamSession
|
||||
from iqpilot.system.webrtc.session import StreamSession
|
||||
|
||||
mocker.patch("iqpilot.system.webrtc.ui_stream.messaging.SubMaster", return_value=FakeSubMaster(make_readers()))
|
||||
|
||||
session = StreamSession.__new__(StreamSession)
|
||||
session.logger = logging.getLogger("webrtcd")
|
||||
@@ -287,14 +292,11 @@ class TestSessionWiring:
|
||||
get_messaging_channel=lambda: channel,
|
||||
)
|
||||
|
||||
async def go():
|
||||
await session.message_handler(b'{"type":"setUiStream","enabled":true}')
|
||||
assert session.ui_stream_runner is not None
|
||||
await asyncio.sleep(0.05)
|
||||
await session.message_handler(b'{"type":"setUiStream","enabled":false}')
|
||||
assert session.ui_stream_runner is None
|
||||
|
||||
asyncio.run(go())
|
||||
session.message_handler(b'{"type":"setUiStream","enabled":true}')
|
||||
assert session.ui_stream_runner is not None
|
||||
await asyncio.sleep(0.05)
|
||||
session.message_handler(b'{"type":"setUiStream","enabled":false}')
|
||||
assert session.ui_stream_runner is None
|
||||
|
||||
assert len(channel.sent) >= 1
|
||||
frame = json.loads(channel.sent[0])
|
||||
|
||||
@@ -1,16 +1,33 @@
|
||||
import pytest
|
||||
import asyncio
|
||||
import json
|
||||
# for aiortc and its dependencies
|
||||
import warnings
|
||||
warnings.filterwarnings("ignore", category=DeprecationWarning)
|
||||
warnings.filterwarnings("ignore", category=RuntimeWarning) # TODO: remove this when google-crc32c publish a python3.12 wheel
|
||||
from types import SimpleNamespace
|
||||
|
||||
from iqpilot.system.webrtc.webrtcd import get_stream
|
||||
|
||||
import aiortc
|
||||
from teleoprtc import WebRTCOfferBuilder
|
||||
from parameterized import parameterized_class
|
||||
import pytest
|
||||
|
||||
from iqpilot.system.webrtc.webrtcd import add_ice, get_stream
|
||||
|
||||
|
||||
class FakeSession:
|
||||
instances = []
|
||||
|
||||
def __init__(self, identifier="session"):
|
||||
self.identifier = identifier
|
||||
self.started = False
|
||||
self.stopped = False
|
||||
self.candidates = []
|
||||
self.instances.append(self)
|
||||
|
||||
async def get_answer(self):
|
||||
return SimpleNamespace(sdp="answer", type="answer")
|
||||
|
||||
def start(self):
|
||||
self.started = True
|
||||
|
||||
async def stop_async(self):
|
||||
self.stopped = True
|
||||
|
||||
async def add_ice_candidate(self, candidate):
|
||||
self.candidates.append(candidate)
|
||||
|
||||
|
||||
@parameterized_class(("in_services", "out_services"), [
|
||||
@@ -21,44 +38,38 @@ from parameterized import parameterized_class
|
||||
])
|
||||
@pytest.mark.asyncio
|
||||
class TestWebrtcdProc:
|
||||
async def assertCompletesWithTimeout(self, awaitable, timeout=10):
|
||||
try:
|
||||
async with asyncio.timeout(timeout):
|
||||
await awaitable
|
||||
except TimeoutError:
|
||||
pytest.fail("Timeout while waiting for awaitable to complete")
|
||||
|
||||
async def test_webrtcd(self, mocker):
|
||||
mock_request = mocker.MagicMock()
|
||||
async def connect(offer):
|
||||
body = {'sdp': offer.sdp, 'cameras': offer.video, 'bridge_services_in': self.in_services, 'bridge_services_out': self.out_services}
|
||||
mock_request.json.side_effect = mocker.AsyncMock(return_value=body)
|
||||
response = await get_stream(mock_request)
|
||||
response_json = json.loads(response.text)
|
||||
return aiortc.RTCSessionDescription(**response_json)
|
||||
session = FakeSession()
|
||||
mocker.patch("iqpilot.system.webrtc.webrtcd._new_stream_session", return_value=session)
|
||||
request = mocker.MagicMock()
|
||||
request.app = {"streams": {}, "debug": False}
|
||||
request.json = mocker.AsyncMock(return_value={
|
||||
"sdp": "offer",
|
||||
"cameras": ["road"],
|
||||
"bridge_services_in": self.in_services,
|
||||
"bridge_services_out": self.out_services,
|
||||
})
|
||||
|
||||
builder = WebRTCOfferBuilder(connect, ice_servers=[])
|
||||
builder.offer_to_receive_video_stream("road")
|
||||
builder.offer_to_receive_audio_stream()
|
||||
if len(self.in_services) > 0 or len(self.out_services) > 0:
|
||||
builder.add_messaging()
|
||||
response = await get_stream(request)
|
||||
|
||||
stream = builder.stream()
|
||||
assert response.status == 200
|
||||
assert json.loads(response.text) == {"sdp": "answer", "type": "answer"}
|
||||
assert request.app["streams"] == {session.identifier: session}
|
||||
assert session.started
|
||||
|
||||
await self.assertCompletesWithTimeout(stream.start())
|
||||
await self.assertCompletesWithTimeout(stream.wait_for_connection())
|
||||
async def test_replaces_session_and_routes_ice(self, mocker):
|
||||
previous = FakeSession("previous")
|
||||
session = FakeSession("current")
|
||||
mocker.patch("iqpilot.system.webrtc.webrtcd._new_stream_session", return_value=session)
|
||||
request = mocker.MagicMock()
|
||||
request.app = {"streams": {previous.identifier: previous}, "debug": False}
|
||||
request.json = mocker.AsyncMock(return_value={"sdp": "offer", "cameras": ["road"]})
|
||||
|
||||
assert stream.has_incoming_video_track("road")
|
||||
assert stream.has_incoming_audio_track()
|
||||
assert stream.has_messaging_channel() == (len(self.in_services) > 0 or len(self.out_services) > 0)
|
||||
response = await get_stream(request)
|
||||
|
||||
video_track, audio_track = stream.get_incoming_video_track("road"), stream.get_incoming_audio_track()
|
||||
await self.assertCompletesWithTimeout(video_track.recv())
|
||||
await self.assertCompletesWithTimeout(audio_track.recv())
|
||||
|
||||
await self.assertCompletesWithTimeout(stream.stop())
|
||||
|
||||
# cleanup, very implementation specific, test may break if it changes
|
||||
assert mock_request.app["streams"].__setitem__.called, "Implementation changed, please update this test"
|
||||
_, session = mock_request.app["streams"].__setitem__.call_args.args
|
||||
await self.assertCompletesWithTimeout(session.post_run_cleanup())
|
||||
assert response.status == 200
|
||||
assert previous.stopped
|
||||
request.json = mocker.AsyncMock(return_value={"candidate": {"candidate": "candidate:1"}})
|
||||
ice_response = await add_ice(request)
|
||||
assert ice_response.status == 200
|
||||
assert session.candidates == [{"candidate": "candidate:1"}]
|
||||
|
||||
@@ -3,52 +3,27 @@
|
||||
import argparse
|
||||
import asyncio
|
||||
import json
|
||||
import os
|
||||
import uuid
|
||||
import logging
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Any, TYPE_CHECKING
|
||||
|
||||
from iqpilot.common.params import Params
|
||||
from typing import Any
|
||||
|
||||
import warnings
|
||||
warnings.filterwarnings("ignore", category=DeprecationWarning)
|
||||
warnings.filterwarnings("ignore", category=RuntimeWarning)
|
||||
|
||||
import capnp
|
||||
import aiortc.rtcrtpsender
|
||||
from aiohttp import web
|
||||
from aiortc.rtp import RTCP_PSFB_APP, RtcpPsfbPacket, unpack_remb_fci
|
||||
if TYPE_CHECKING:
|
||||
from aiortc.rtcdatachannel import RTCDataChannel
|
||||
|
||||
from iqpilot.system.webrtc.schema import generate_field
|
||||
from iqpilot.cereal import messaging, log
|
||||
|
||||
|
||||
_handle_rtcp_packet = aiortc.rtcrtpsender.RTCRtpSender._handle_rtcp_packet
|
||||
|
||||
|
||||
async def _handle_rtcp_packet_with_remb(self, packet):
|
||||
if isinstance(packet, RtcpPsfbPacket) and packet.fmt == RTCP_PSFB_APP:
|
||||
try:
|
||||
bitrate, ssrcs = unpack_remb_fci(packet.fci)
|
||||
if getattr(self, "_ssrc", None) in ssrcs:
|
||||
self._remb_bitrate = bitrate
|
||||
except ValueError:
|
||||
pass
|
||||
return await _handle_rtcp_packet(self, packet)
|
||||
|
||||
|
||||
aiortc.rtcrtpsender.RTCRtpSender._handle_rtcp_packet = _handle_rtcp_packet_with_remb
|
||||
|
||||
|
||||
class CerealOutgoingMessageProxy:
|
||||
def __init__(self, sm: messaging.SubMaster):
|
||||
self.sm = sm
|
||||
self.channels: list[RTCDataChannel] = []
|
||||
self.channels: list[Any] = []
|
||||
|
||||
def add_channel(self, channel: 'RTCDataChannel'):
|
||||
def add_channel(self, channel: Any):
|
||||
self.channels.append(channel)
|
||||
|
||||
def to_json(self, msg_content: Any):
|
||||
@@ -114,43 +89,6 @@ class AsyncTaskRunner:
|
||||
self.task = None
|
||||
|
||||
|
||||
class IncomingAudioCerealProxy(AsyncTaskRunner):
|
||||
def __init__(self, track: Any):
|
||||
super().__init__()
|
||||
from av.audio.resampler import AudioResampler
|
||||
from iqpilot.selfdrive.ui.soundd import SAMPLE_RATE as SOUND_SAMPLE_RATE
|
||||
from iqpilot.system.webrtc.device.audio import WEBRTC_AUDIO_SERVICE
|
||||
|
||||
self.track = track
|
||||
self.service = WEBRTC_AUDIO_SERVICE
|
||||
self.pm = messaging.PubMaster([self.service])
|
||||
self.resampler = AudioResampler(format="s16", layout="mono", rate=SOUND_SAMPLE_RATE)
|
||||
|
||||
def _publish(self, frame: Any) -> None:
|
||||
data = frame.to_ndarray().tobytes()
|
||||
if not data:
|
||||
return
|
||||
|
||||
msg = messaging.new_message(self.service, valid=True)
|
||||
msg.webrtcAudioData.data = data
|
||||
msg.webrtcAudioData.sampleRate = frame.sample_rate
|
||||
self.pm.send(self.service, msg)
|
||||
|
||||
async def run(self):
|
||||
from aiortc.mediastreams import MediaStreamError
|
||||
|
||||
while True:
|
||||
try:
|
||||
frame = await self.track.recv()
|
||||
for resampled_frame in self.resampler.resample(frame):
|
||||
self._publish(resampled_frame)
|
||||
except MediaStreamError:
|
||||
break
|
||||
except Exception:
|
||||
self.logger.exception("Incoming audio cereal proxy failure")
|
||||
await asyncio.sleep(0.1)
|
||||
|
||||
|
||||
class CerealProxyRunner:
|
||||
def __init__(self, proxy: CerealOutgoingMessageProxy):
|
||||
self.proxy = proxy
|
||||
@@ -169,110 +107,14 @@ class CerealProxyRunner:
|
||||
self.task = None
|
||||
|
||||
async def run(self):
|
||||
from aiortc.exceptions import InvalidStateError
|
||||
|
||||
while True:
|
||||
try:
|
||||
self.proxy.update()
|
||||
except InvalidStateError:
|
||||
self.logger.warning("Cereal outgoing proxy invalid state (connection closed)")
|
||||
break
|
||||
except Exception:
|
||||
self.logger.exception("Cereal outgoing proxy failure")
|
||||
await asyncio.sleep(0.01)
|
||||
|
||||
|
||||
class LivestreamBitrateController:
|
||||
"""Adaptive bitrate for the livestream encoder using browser REMB feedback."""
|
||||
|
||||
# Match comma's rung choices more closely. A steadier capped stream tends to look better than
|
||||
# an occasionally-higher bitrate stream that induces queueing, jitter, and frame pacing swings.
|
||||
bitrates = [500_000, 1_500_000, int(os.environ.get("STREAM_BITRATE", 5_000_000))]
|
||||
label_to_bitrate = {"low": bitrates[0], "med": bitrates[1], "high": bitrates[-1]}
|
||||
|
||||
sample_interval = 1.0
|
||||
lower_factor = 0.9
|
||||
probe_after = 10
|
||||
settle_samples = 3
|
||||
|
||||
def __init__(self, peer_connection: Any):
|
||||
self.pc = peer_connection
|
||||
self.params = Params()
|
||||
self.task: asyncio.Task | None = None
|
||||
|
||||
# Start conservative and probe UP only when REMB proves headroom. Previously this started at
|
||||
# the top rung (5 Mbps); with no REMB feedback (e.g. transport-cc-only receivers, or a flaky
|
||||
# uplink that never delivers RTCP), _bandwidth_estimate() returns None and run() hits
|
||||
# `if estimate is None: continue` — so the level never moves and the encoder stays pinned at
|
||||
# 5 Mbps, flooding a marginal cellular uplink until webrtcd's send buffer balloons and trips the
|
||||
# device's lowMemory soft-disable. The med rung is carriable on typical cellular; healthy links
|
||||
# with working REMB still probe up to high within ~probe_after seconds.
|
||||
self.level = min(1, len(self.bitrates) - 1)
|
||||
self.stable = 0
|
||||
self.settle = 0
|
||||
self._auto = True
|
||||
self.current_bitrate = self.bitrates[self.level]
|
||||
self._publish(self.bitrates[self.level])
|
||||
|
||||
def start(self):
|
||||
if self.task is None:
|
||||
self.task = asyncio.create_task(self.run())
|
||||
|
||||
def stop(self):
|
||||
if self.task is not None and not self.task.done():
|
||||
self.task.cancel()
|
||||
self.task = None
|
||||
|
||||
async def run(self):
|
||||
while True:
|
||||
await asyncio.sleep(self.sample_interval)
|
||||
if not self._auto:
|
||||
continue
|
||||
estimate = self._bandwidth_estimate()
|
||||
if estimate is None:
|
||||
continue
|
||||
|
||||
if self.settle > 0:
|
||||
self.settle -= 1
|
||||
continue
|
||||
|
||||
if estimate < self.bitrates[self.level] * self.lower_factor:
|
||||
while self.level > 0 and estimate < self.bitrates[self.level] * self.lower_factor:
|
||||
self.level -= 1
|
||||
self.stable = 0
|
||||
self._publish(self.bitrates[self.level])
|
||||
elif self.level < len(self.bitrates) - 1:
|
||||
self.stable += 1
|
||||
if self.stable >= self.probe_after:
|
||||
self.level += 1
|
||||
self.stable = 0
|
||||
self.settle = self.settle_samples
|
||||
self._publish(self.bitrates[self.level])
|
||||
else:
|
||||
self.stable = 0
|
||||
|
||||
def _bandwidth_estimate(self) -> int | None:
|
||||
estimate = None
|
||||
for sender in self.pc.getSenders():
|
||||
bitrate = getattr(sender, "_remb_bitrate", None)
|
||||
if bitrate is not None:
|
||||
estimate = bitrate if estimate is None else min(estimate, bitrate)
|
||||
return estimate
|
||||
|
||||
def set_quality(self, quality: str):
|
||||
if quality in self.label_to_bitrate:
|
||||
self._auto = False
|
||||
self._publish(self.label_to_bitrate[quality])
|
||||
elif quality == "auto":
|
||||
self._auto = True
|
||||
|
||||
def _publish(self, bitrate: int):
|
||||
# Param is registered as INT — must pass a Python int, not str. Passing str throws
|
||||
# TypeError in Params.put (type mismatch) and crashes StreamSession.__init__ → HTTP 500.
|
||||
self.current_bitrate = int(bitrate)
|
||||
self.params.put("LivestreamEncoderBitrate", int(bitrate))
|
||||
|
||||
|
||||
class DynamicPubMaster(messaging.PubMaster):
|
||||
def __init__(self, *args, **kwargs):
|
||||
super().__init__(*args, **kwargs)
|
||||
@@ -285,239 +127,11 @@ class DynamicPubMaster(messaging.PubMaster):
|
||||
self.sock[service] = messaging.pub_sock(service)
|
||||
|
||||
|
||||
class StreamSession:
|
||||
shared_pub_master = DynamicPubMaster([])
|
||||
|
||||
def __init__(self, sdp: str, cameras: list[str], incoming_services: list[str], outgoing_services: list[str],
|
||||
ice_servers: list[dict[str, Any]] | None = None, debug_mode: bool = False, ui_stream: bool = False):
|
||||
from aiortc.mediastreams import VideoStreamTrack, AudioStreamTrack
|
||||
from iqpilot.system.webrtc.device.video import LiveStreamVideoStreamTrack
|
||||
from iqpilot.system.webrtc.device.audio import AudioInputStreamTrack
|
||||
from teleoprtc import WebRTCAnswerBuilder
|
||||
from teleoprtc.info import parse_info_from_offer
|
||||
|
||||
config = parse_info_from_offer(sdp)
|
||||
builder = WebRTCAnswerBuilder(sdp, ice_servers=ice_servers or [])
|
||||
|
||||
assert len(cameras) == config.n_expected_camera_tracks, "Incoming stream has misconfigured number of video tracks"
|
||||
self.video_tracks: list[LiveStreamVideoStreamTrack] = []
|
||||
for cam in cameras:
|
||||
track = LiveStreamVideoStreamTrack(cam) if not debug_mode else VideoStreamTrack()
|
||||
if isinstance(track, LiveStreamVideoStreamTrack):
|
||||
self.video_tracks.append(track)
|
||||
builder.add_video_stream(cam, track)
|
||||
# Audio init may fail if openpilot is using the audio subsystem - skip gracefully
|
||||
if config.expected_audio_track:
|
||||
try:
|
||||
self.audio_input_track = AudioInputStreamTrack() if not debug_mode else AudioStreamTrack()
|
||||
builder.add_audio_stream(self.audio_input_track)
|
||||
self.audio_send_enabled = True
|
||||
except Exception as e:
|
||||
logging.warning(f"Could not init audio input (audio in use?): {e}")
|
||||
if config.incoming_audio_track:
|
||||
builder.offer_to_receive_audio_stream()
|
||||
|
||||
self.stream = builder.stream()
|
||||
self.identifier = str(uuid.uuid4())
|
||||
|
||||
self.incoming_bridge: CerealIncomingMessageProxy | None = None
|
||||
self.incoming_bridge_services = incoming_services
|
||||
self.outgoing_bridge: CerealOutgoingMessageProxy | None = None
|
||||
self.outgoing_bridge_runner: CerealProxyRunner | None = None
|
||||
if len(incoming_services) > 0:
|
||||
self.incoming_bridge = CerealIncomingMessageProxy(self.shared_pub_master)
|
||||
if len(outgoing_services) > 0:
|
||||
self.outgoing_bridge = CerealOutgoingMessageProxy(messaging.SubMaster(outgoing_services))
|
||||
self.outgoing_bridge_runner = CerealProxyRunner(self.outgoing_bridge)
|
||||
|
||||
self.ui_stream_requested = ui_stream
|
||||
self.ui_stream_runner: CerealProxyRunner | None = None
|
||||
|
||||
self.incoming_audio_proxy: IncomingAudioCerealProxy | None = None
|
||||
self.audio_input_track: AudioInputStreamTrack | AudioStreamTrack | None = None
|
||||
self.audio_send_enabled = False
|
||||
self.audio_recv_requested = bool(config.incoming_audio_track)
|
||||
self.audio_send_requested = bool(config.expected_audio_track)
|
||||
self.run_task: asyncio.Task | None = None
|
||||
# Adaptive bitrate controller for the livestream encoder (no-op in debug mode).
|
||||
self.bitrate_controller: LivestreamBitrateController | None = None
|
||||
if not debug_mode and len(self.video_tracks) > 0:
|
||||
self.bitrate_controller = LivestreamBitrateController(self.stream.peer_connection)
|
||||
self.logger = logging.getLogger("webrtcd")
|
||||
self.logger.info("New stream session (%s), cameras %s, audio in %s out %s, incoming services %s, outgoing services %s",
|
||||
self.identifier, cameras, config.incoming_audio_track, config.expected_audio_track, incoming_services, outgoing_services)
|
||||
|
||||
def start(self):
|
||||
self.run_task = asyncio.create_task(self.run())
|
||||
|
||||
async def stop_async(self):
|
||||
if self.run_task is not None and not self.run_task.done():
|
||||
self.run_task.cancel()
|
||||
try:
|
||||
await self.run_task
|
||||
except asyncio.CancelledError:
|
||||
pass
|
||||
except Exception:
|
||||
self.logger.exception("Stream session stop task failure")
|
||||
self.run_task = None
|
||||
await self.post_run_cleanup()
|
||||
|
||||
def stop(self):
|
||||
# Backwards-compatible sync wrapper. Prefer `await stop_async()` from async contexts.
|
||||
try:
|
||||
loop = asyncio.get_running_loop()
|
||||
# If we're already in an event loop, schedule async shutdown and return.
|
||||
loop.create_task(self.stop_async())
|
||||
return
|
||||
except RuntimeError:
|
||||
pass
|
||||
asyncio.run(self.stop_async())
|
||||
|
||||
async def get_answer(self):
|
||||
return await self.stream.start()
|
||||
|
||||
async def message_handler(self, message: bytes):
|
||||
# Control messages are handled in-process and don't require an incoming cereal bridge.
|
||||
try:
|
||||
payload = json.loads(message) if isinstance(message, (bytes, str)) else None
|
||||
except (ValueError, TypeError):
|
||||
payload = None
|
||||
if isinstance(payload, dict) and payload.get("type") == "timingSei":
|
||||
enabled = bool(payload.get("enabled", False))
|
||||
for track in self.video_tracks:
|
||||
track.timing_sei_enabled = enabled
|
||||
self.logger.info("timing SEI %s", "enabled" if enabled else "disabled")
|
||||
return
|
||||
if isinstance(payload, dict) and payload.get("type") == "setQuality":
|
||||
if self.bitrate_controller is not None:
|
||||
quality = str(payload.get("quality", "auto"))
|
||||
self.bitrate_controller.set_quality(quality)
|
||||
self.logger.info("livestream quality set to %s", quality)
|
||||
return
|
||||
if isinstance(payload, dict) and payload.get("type") == "setAudioEnabled":
|
||||
enabled = bool(payload.get("enabled", True))
|
||||
if hasattr(self.audio_input_track, "enable"):
|
||||
self.audio_input_track.enable(enabled)
|
||||
self.audio_send_enabled = enabled
|
||||
self.logger.info("livestream audio send %s", "enabled" if enabled else "disabled")
|
||||
return
|
||||
if isinstance(payload, dict) and payload.get("type") == "setUiStream":
|
||||
enabled = bool(payload.get("enabled", False))
|
||||
self.set_ui_stream(enabled)
|
||||
self.logger.info("ui stream %s", "enabled" if enabled else "disabled")
|
||||
return
|
||||
if isinstance(payload, dict) and payload.get("type") == "switchCamera":
|
||||
camera = str(payload.get("camera", ""))
|
||||
# Single-track model: repoint the (one) video track at the requested camera.
|
||||
for track in self.video_tracks:
|
||||
track.switch_camera(camera)
|
||||
return
|
||||
|
||||
if self.incoming_bridge is None:
|
||||
return
|
||||
try:
|
||||
self.incoming_bridge.send(message)
|
||||
except Exception:
|
||||
self.logger.exception("Cereal incoming proxy failure")
|
||||
|
||||
def set_ui_stream(self, enabled: bool):
|
||||
if enabled:
|
||||
if self.ui_stream_runner is not None or not self.stream.has_messaging_channel():
|
||||
return
|
||||
from iqpilot.system.webrtc.ui_stream import UIStreamMessageProxy
|
||||
bitrate_getter = None
|
||||
if self.bitrate_controller is not None:
|
||||
controller = self.bitrate_controller
|
||||
|
||||
def bitrate_getter():
|
||||
return controller.current_bitrate
|
||||
proxy = UIStreamMessageProxy(bitrate_getter=bitrate_getter)
|
||||
proxy.add_channel(self.stream.get_messaging_channel())
|
||||
self.ui_stream_runner = CerealProxyRunner(proxy)
|
||||
self.ui_stream_runner.start()
|
||||
elif self.ui_stream_runner is not None:
|
||||
self.ui_stream_runner.stop()
|
||||
self.ui_stream_runner = None
|
||||
|
||||
async def add_ice_candidate(self, cand: Any):
|
||||
"""Add a trickled ICE candidate from the client to the live peer connection."""
|
||||
if not isinstance(cand, dict):
|
||||
return
|
||||
cand_str = cand.get("candidate") or ""
|
||||
if not cand_str:
|
||||
return # end-of-candidates marker; aiortc needs no explicit signal
|
||||
try:
|
||||
from aiortc.sdp import candidate_from_sdp
|
||||
sdp_str = cand_str.split(":", 1)[-1] if cand_str.startswith("candidate:") else cand_str
|
||||
ice = candidate_from_sdp(sdp_str)
|
||||
ice.sdpMid = cand.get("sdpMid")
|
||||
ice.sdpMLineIndex = cand.get("sdpMLineIndex")
|
||||
await self.stream.peer_connection.addIceCandidate(ice)
|
||||
except Exception:
|
||||
self.logger.exception("Failed to add ICE candidate")
|
||||
|
||||
async def run(self):
|
||||
try:
|
||||
await self.stream.wait_for_connection()
|
||||
if self.stream.has_messaging_channel():
|
||||
# Always install the handler so control messages (e.g. timing SEI toggle) work
|
||||
# even when no incoming cereal bridge service was requested.
|
||||
self.stream.set_message_handler(self.message_handler)
|
||||
if self.incoming_bridge is not None:
|
||||
await self.shared_pub_master.add_services_if_needed(self.incoming_bridge_services)
|
||||
if self.outgoing_bridge_runner is not None:
|
||||
channel = self.stream.get_messaging_channel()
|
||||
self.outgoing_bridge_runner.proxy.add_channel(channel)
|
||||
self.outgoing_bridge_runner.start()
|
||||
if self.ui_stream_requested:
|
||||
self.set_ui_stream(True)
|
||||
if self.audio_recv_requested and self.stream.has_incoming_audio_track():
|
||||
track = self.stream.get_incoming_audio_track(buffered=False)
|
||||
self.incoming_audio_proxy = IncomingAudioCerealProxy(track)
|
||||
self.incoming_audio_proxy.start()
|
||||
self.logger.info("Stream session (%s) incoming audio proxy started", self.identifier)
|
||||
else:
|
||||
self.logger.info("Stream session (%s) no incoming audio track from client", self.identifier)
|
||||
if self.bitrate_controller is not None:
|
||||
self.bitrate_controller.start()
|
||||
self.logger.info(
|
||||
"Stream session (%s) audio state send_requested=%s send_enabled=%s recv_requested=%s recv_active=%s",
|
||||
self.identifier,
|
||||
self.audio_send_requested,
|
||||
self.audio_send_enabled,
|
||||
self.audio_recv_requested,
|
||||
self.incoming_audio_proxy is not None,
|
||||
)
|
||||
self.logger.info("Stream session (%s) connected", self.identifier)
|
||||
|
||||
await self.stream.wait_for_disconnection()
|
||||
await self.post_run_cleanup()
|
||||
|
||||
self.logger.info("Stream session (%s) ended", self.identifier)
|
||||
except Exception:
|
||||
self.logger.exception("Stream session failure")
|
||||
|
||||
async def post_run_cleanup(self):
|
||||
if self.bitrate_controller is not None:
|
||||
self.bitrate_controller.stop()
|
||||
await self.stream.stop()
|
||||
if self.ui_stream_runner is not None:
|
||||
self.ui_stream_runner.stop()
|
||||
self.ui_stream_runner = None
|
||||
if self.outgoing_bridge is not None:
|
||||
self.outgoing_bridge_runner.stop()
|
||||
if self.incoming_audio_proxy is not None:
|
||||
await self.incoming_audio_proxy.stop()
|
||||
|
||||
|
||||
def _is_retryable_stream_error(e: Exception) -> bool:
|
||||
# Transient failures seen during answer generation: SDP/candidate parse issues
|
||||
# (typically browser mDNS .local host candidates aiortc can't resolve) and
|
||||
# socket-level hiccups while gathering. Anything else is a real error.
|
||||
return isinstance(e, (ValueError, OSError))
|
||||
|
||||
|
||||
async def _cleanup_failed_session(session: 'StreamSession | None', logger: logging.Logger) -> None:
|
||||
async def _cleanup_failed_session(session: Any | None, logger: logging.Logger) -> None:
|
||||
if session is None:
|
||||
return
|
||||
try:
|
||||
@@ -543,15 +157,7 @@ class StreamRequestBody:
|
||||
|
||||
|
||||
def _new_stream_session(offer_sdp: str, body: StreamRequestBody, debug_mode: bool):
|
||||
if Params().get_bool("Konn3ktLibdatachannelWebRTC"):
|
||||
try:
|
||||
from iqpilot.system.webrtc.webrtcd_ldc import StreamSessionLibdatachannel
|
||||
return StreamSessionLibdatachannel(
|
||||
offer_sdp, body.cameras, body.bridge_services_in, body.bridge_services_out, body.iceServers, debug_mode,
|
||||
ui_stream=body.ui_stream,
|
||||
)
|
||||
except Exception:
|
||||
logging.getLogger("webrtcd").exception("libdatachannel unavailable; falling back to aiortc")
|
||||
from iqpilot.system.webrtc.session import StreamSession
|
||||
return StreamSession(
|
||||
offer_sdp, body.cameras, body.bridge_services_in, body.bridge_services_out, body.iceServers, debug_mode,
|
||||
ui_stream=body.ui_stream,
|
||||
@@ -561,7 +167,7 @@ def _new_stream_session(offer_sdp: str, body: StreamRequestBody, debug_mode: boo
|
||||
async def get_stream(request: 'web.Request'):
|
||||
stream_dict, debug_mode = request.app['streams'], request.app['debug']
|
||||
logger = logging.getLogger("webrtcd")
|
||||
session: StreamSession | None = None
|
||||
session: Any | None = None
|
||||
try:
|
||||
raw_body = await request.json()
|
||||
body = StreamRequestBody(**raw_body)
|
||||
|
||||
Reference in New Issue
Block a user