381 lines
14 KiB
Python
381 lines
14 KiB
Python
"""
|
|
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
|
"""
|
|
from __future__ import annotations
|
|
|
|
import json
|
|
import os
|
|
import threading
|
|
import time
|
|
from collections import deque
|
|
|
|
from iqpilot.cereal.messaging import PubMaster, log_from_bytes
|
|
from setproctitle import setproctitle
|
|
|
|
from iqpilot.common.filter_simple import FirstOrderFilter
|
|
from iqpilot.common.params import Params
|
|
from iqpilot.common.realtime import config_realtime_process
|
|
from iqpilot.common.swaglog import cloudlog
|
|
from iqpilot.selfdrive.iqmodeld.model_channel import BIG_CHANNEL, SMALL_CHANNEL, ModelChannel
|
|
|
|
PROCESS_NAME = "iqpilot.selfdrive.iqmodeld.modeld_selector"
|
|
|
|
BIG_MODEL_DEADLINE = float(os.getenv("IQEMAC_BIG_DEADLINE_MS", "45")) / 1000.0
|
|
BIG_MAX_LAG_FRAMES = int(os.getenv("IQEMAC_MAX_BIG_LAG_FRAMES", "6"))
|
|
BIG_FUTURE_ACCEPT = int(os.getenv("IQEMAC_BIG_FUTURE_ACCEPT", "2"))
|
|
BIG_ANCHOR_MS = float(os.getenv("IQEMAC_BIG_ANCHOR_MS", "90"))
|
|
BIG_WAIT_FLOOR_S = 0.002
|
|
BIG_WAIT_CEIL_S = float(os.getenv("IQEMAC_BIG_WAIT_CEIL_MS", "58")) / 1000.0
|
|
BIG_MISS_LIMIT = int(os.getenv("IQEMAC_BIG_MISS_LIMIT", "80"))
|
|
ACTIVATE_WINDOW = int(os.getenv("IQEMAC_ACTIVATE_WINDOW", "50"))
|
|
ACTIVATE_FRAC = float(os.getenv("IQEMAC_ACTIVATE_FRAC", "0.7"))
|
|
REARM_LIMIT = int(os.getenv("IQEMAC_REARM_LIMIT", "2"))
|
|
MODEL_FREQ = 20.0
|
|
WARMUP_FRAMES = 40
|
|
STATUS_WINDOW = int(os.getenv("IQEMAC_STATUS_EVERY", "20"))
|
|
|
|
SELECTOR_SERVICES = ["modelV2", "drivingModelData", "cameraOdometry", "iqDriveModelData"]
|
|
|
|
EMAC_STATUS_KEYS = {
|
|
"active": "MacModelActive", "failed": "MacModelFailed", "last_error": "MacModelLastError",
|
|
"latency_ms": "MacModelLatencyMs", "status": "MacModelStatus",
|
|
"reachable": "MacModelReachable", "progress": "MacModelDownloadProgress",
|
|
}
|
|
EGPU_STATUS_KEYS = {
|
|
"active": "UsbGpuActive", "failed": "UsbGpuFailed", "last_error": "UsbGpuLastError",
|
|
"latency_ms": "UsbGpuLatencyMs", "status": "UsbGpuStatus",
|
|
"reachable": "UsbGpuPresent", "progress": "UsbGpuSetupProgress",
|
|
}
|
|
|
|
|
|
def backend_status_keys(emac_enabled: bool, egpu_enabled: bool, egpu_present: bool = False) -> dict[str, str]:
|
|
from iqpilot.selfdrive.iqmodeld.egpu_helpers import resolve_backend
|
|
return EGPU_STATUS_KEYS if resolve_backend(emac_enabled, egpu_enabled, egpu_present) == "egpu" else EMAC_STATUS_KEYS
|
|
|
|
|
|
def resolve_status_keys(params):
|
|
from iqpilot.selfdrive.iqmodeld.egpu_helpers import egpu_present_consented, egpu_selected
|
|
return backend_status_keys(params.get_bool("IQEmacEnabled"), egpu_selected(params), egpu_present_consented(params))
|
|
|
|
|
|
def resolve_model_name(params, keys) -> str:
|
|
if keys is EGPU_STATUS_KEYS:
|
|
from iqpilot.selfdrive.iqmodeld.egpu_model import DEFAULT_EGPU_MODEL, resolve_egpu_model
|
|
resolved = resolve_egpu_model(params, allow_refresh=False)
|
|
return resolved["key"] if resolved else DEFAULT_EGPU_MODEL
|
|
name = params.get("IQEmacModel") or b"lebrowski"
|
|
return name.decode() if isinstance(name, bytes) else name
|
|
|
|
|
|
class AsyncParamWriter:
|
|
|
|
def __init__(self, params: Params):
|
|
self._params = params
|
|
self._pending: dict[str, object] = {}
|
|
self._lock = threading.Lock()
|
|
self._event = threading.Event()
|
|
threading.Thread(target=self._drain, daemon=True).start()
|
|
|
|
def put(self, key: str, value) -> None:
|
|
with self._lock:
|
|
self._pending[key] = value
|
|
self._event.set()
|
|
|
|
def put_bool(self, key: str, value: bool) -> None:
|
|
self.put(key, bool(value))
|
|
|
|
def _drain(self) -> None:
|
|
while True:
|
|
self._event.wait()
|
|
self._event.clear()
|
|
with self._lock:
|
|
batch, self._pending = self._pending, {}
|
|
for key, value in batch.items():
|
|
try:
|
|
if isinstance(value, bool):
|
|
self._params.put_bool(key, value)
|
|
else:
|
|
self._params.put(key, value)
|
|
except Exception:
|
|
cloudlog.exception(f"async param write failed: {key}")
|
|
|
|
|
|
def wait_for_big(big_channel, target: int, deadline: float, min_frame: int = -1,
|
|
max_lag_frames: int = BIG_MAX_LAG_FRAMES) -> tuple[dict | None, int | None]:
|
|
big_peek = None
|
|
grab_at = deadline - 0.004
|
|
while time.perf_counter() < deadline:
|
|
bfid = big_channel.peek_frame_id()
|
|
big_peek = bfid
|
|
if bfid == target - 1 and time.perf_counter() < grab_at:
|
|
time.sleep(0.0005)
|
|
continue
|
|
if bfid is not None and min_frame < bfid <= target + BIG_FUTURE_ACCEPT and target - bfid <= max_lag_frames:
|
|
got = big_channel.read()
|
|
if got is not None and got[0] == bfid:
|
|
return got[1], big_peek
|
|
break
|
|
if bfid is None or bfid <= min_frame or bfid > target + BIG_FUTURE_ACCEPT or target - bfid > max_lag_frames:
|
|
break
|
|
time.sleep(0.0005)
|
|
return None, big_peek
|
|
|
|
|
|
class BigLatch:
|
|
|
|
def __init__(self, miss_limit: int = BIG_MISS_LIMIT, activate_window: int = ACTIVATE_WINDOW,
|
|
activate_frac: float = ACTIVATE_FRAC, rearm_limit: int = REARM_LIMIT):
|
|
self.miss_limit = miss_limit
|
|
self.activate_window = activate_window
|
|
self.activate_need = int(round(activate_window * activate_frac))
|
|
self.rearm_limit = rearm_limit
|
|
self.active = False
|
|
self.done = False
|
|
self._miss = 0
|
|
self._window: deque[bool] = deque(maxlen=activate_window)
|
|
self._retires = 0
|
|
|
|
def update(self, used_big: bool) -> tuple[bool, bool]:
|
|
if self.done:
|
|
return False, False
|
|
if not self.active:
|
|
self._window.append(used_big)
|
|
if len(self._window) >= self.activate_window and sum(self._window) >= self.activate_need:
|
|
self.active = True
|
|
self._miss = 0
|
|
self._window.clear()
|
|
return True, False
|
|
if used_big:
|
|
self._miss = 0
|
|
elif self.active:
|
|
self._miss += 1
|
|
if self._miss >= self.miss_limit:
|
|
self.active = False
|
|
self._miss = 0
|
|
self._window.clear()
|
|
self._retires += 1
|
|
self.done = self._retires > self.rearm_limit
|
|
return False, True
|
|
return False, False
|
|
|
|
|
|
BIG_SOURCES = frozenset({"egpu_big", "mac_big"})
|
|
|
|
|
|
def _patch_and_send(pm: PubMaster, payload: dict, frame_drop_perc: float, selector_dropped: int,
|
|
target: int, source_lag: int, mismatch: bool | None = None) -> None:
|
|
msgs = payload["msgs"]
|
|
if mismatch is None:
|
|
mismatch = source_lag > 0
|
|
|
|
big = payload.get("source") in BIG_SOURCES
|
|
model_msg = log_from_bytes(msgs["modelV2"]).as_builder()
|
|
if mismatch:
|
|
model_msg.modelV2.frameId = target
|
|
model_msg.modelV2.frameAge = max(model_msg.modelV2.frameAge, source_lag)
|
|
model_msg.modelV2.frameDropPerc = frame_drop_perc
|
|
model_msg.modelV2.big = big
|
|
pm.send("modelV2", model_msg)
|
|
|
|
driving_msg = log_from_bytes(msgs["drivingModelData"]).as_builder()
|
|
if mismatch:
|
|
driving_msg.drivingModelData.frameId = target
|
|
driving_msg.drivingModelData.frameDropPerc = frame_drop_perc
|
|
driving_msg.drivingModelData.big = big
|
|
pm.send("drivingModelData", driving_msg)
|
|
|
|
pose_msg = log_from_bytes(msgs["cameraOdometry"]).as_builder()
|
|
if mismatch:
|
|
pose_msg.cameraOdometry.frameId = target
|
|
pose_msg.valid = bool(payload["live_calib_seen"]) and selector_dropped < 1 and not mismatch
|
|
pm.send("cameraOdometry", pose_msg)
|
|
|
|
pm.send("iqDriveModelData", msgs["iqDriveModelData"])
|
|
|
|
|
|
def _read_float(params, key: str, default: float) -> float:
|
|
v = params.get(key)
|
|
try:
|
|
return float(v) if v is not None else default
|
|
except (TypeError, ValueError):
|
|
return default
|
|
|
|
|
|
def main() -> None:
|
|
cloudlog.warning("modeld_selector init")
|
|
cloudlog.bind(daemon=PROCESS_NAME)
|
|
setproctitle(PROCESS_NAME)
|
|
config_realtime_process([0, 1, 2, 3], 54)
|
|
|
|
params = Params()
|
|
keys = resolve_status_keys(params)
|
|
pwriter = AsyncParamWriter(params)
|
|
pwriter.put_bool(keys["active"], False)
|
|
pwriter.put_bool(keys["failed"], False)
|
|
pm = PubMaster(SELECTOR_SERVICES)
|
|
|
|
small_channel: ModelChannel | None = None
|
|
big_channel: ModelChannel | None = None
|
|
latch = BigLatch()
|
|
big_used_count = 0
|
|
run_count = 0
|
|
last_published = -1
|
|
last_big_published = -1
|
|
frame_dropped_filter = FirstOrderFilter(0.0, 10.0, 1.0 / MODEL_FREQ)
|
|
recent_big = deque(maxlen=STATUS_WINDOW)
|
|
model_name = resolve_model_name(params, keys)
|
|
last_backend_check = 0.0
|
|
last_latency_ms = 0.0
|
|
last_source_lag = 0
|
|
miss_reasons = {"no_head": 0, "already_used": 0, "far_future": 0,
|
|
"too_stale": 0, "head_prev_timeout": 0, "read_race": 0}
|
|
|
|
cloudlog.warning(f"modeld_selector starting (max_big_lag_frames={BIG_MAX_LAG_FRAMES})")
|
|
while True:
|
|
if small_channel is None:
|
|
try:
|
|
small_channel = ModelChannel(SMALL_CHANNEL, create=False)
|
|
except OSError:
|
|
time.sleep(0.05)
|
|
continue
|
|
if big_channel is None:
|
|
try:
|
|
big_channel = ModelChannel(BIG_CHANNEL, create=False)
|
|
except OSError:
|
|
big_channel = None
|
|
|
|
fid = small_channel.peek_frame_id()
|
|
if fid is None or fid == last_published:
|
|
time.sleep(0.0005)
|
|
continue
|
|
if last_published >= 0 and fid < last_published - 1:
|
|
cloudlog.warning(f"modeld_selector frame reset {last_published} -> {fid}; re-arming")
|
|
last_published = -1
|
|
last_big_published = -1
|
|
big_used_count = 0
|
|
run_count = 0
|
|
latch = BigLatch()
|
|
pwriter.put_bool(keys["active"], False)
|
|
pwriter.put_bool(keys["failed"], False)
|
|
|
|
now_mono = time.monotonic()
|
|
if now_mono - last_backend_check > 1.0:
|
|
last_backend_check = now_mono
|
|
new_keys = resolve_status_keys(params)
|
|
if new_keys is not keys:
|
|
cloudlog.warning(f"modeld_selector backend changed {keys['active']} -> {new_keys['active']}; re-arming")
|
|
pwriter.put_bool(keys["active"], False)
|
|
pwriter.put_bool(keys["failed"], False)
|
|
keys = new_keys
|
|
model_name = resolve_model_name(params, keys)
|
|
recent_big.clear()
|
|
last_big_published = -1
|
|
big_used_count = 0
|
|
run_count = 0
|
|
latch = BigLatch()
|
|
pwriter.put_bool(keys["active"], False)
|
|
pwriter.put_bool(keys["failed"], False)
|
|
target = fid
|
|
t_start = time.perf_counter()
|
|
|
|
small_payload = None
|
|
got = small_channel.read()
|
|
if got is not None and got[0] == target:
|
|
small_payload = got[1]
|
|
|
|
payload = None
|
|
used_big = False
|
|
big_peek = None
|
|
if big_channel is not None and not latch.done:
|
|
deadline = t_start + BIG_MODEL_DEADLINE
|
|
sof_ns = (small_payload or {}).get("timestamp_sof")
|
|
if sof_ns:
|
|
remaining = (BIG_ANCHOR_MS / 1000.0) - (time.clock_gettime(time.CLOCK_BOOTTIME) - sof_ns / 1e9)
|
|
deadline = t_start + min(max(remaining, BIG_WAIT_FLOOR_S), BIG_WAIT_CEIL_S)
|
|
payload, big_peek = wait_for_big(big_channel, target, deadline,
|
|
last_big_published, BIG_MAX_LAG_FRAMES)
|
|
used_big = payload is not None
|
|
if not used_big:
|
|
if big_peek is None:
|
|
miss_reasons["no_head"] += 1
|
|
elif big_peek <= last_big_published:
|
|
miss_reasons["already_used"] += 1
|
|
elif big_peek > target + BIG_FUTURE_ACCEPT:
|
|
miss_reasons["far_future"] += 1
|
|
elif target - big_peek > BIG_MAX_LAG_FRAMES:
|
|
miss_reasons["too_stale"] += 1
|
|
elif big_peek == target - 1:
|
|
miss_reasons["head_prev_timeout"] += 1
|
|
else:
|
|
miss_reasons["read_race"] += 1
|
|
|
|
if payload is None:
|
|
payload = small_payload
|
|
if payload is None:
|
|
got = small_channel.read()
|
|
if got is not None and got[0] == target:
|
|
payload = got[1]
|
|
|
|
activated_now, failed_now = latch.update(used_big)
|
|
if activated_now:
|
|
pwriter.put_bool(keys["active"], True)
|
|
pwriter.put_bool(keys["failed"], False)
|
|
cloudlog.warning(f"modeld_selector switched to BIG model at frame {target}")
|
|
elif failed_now:
|
|
pwriter.put_bool(keys["active"], False)
|
|
pwriter.put_bool(keys["failed"], latch.done)
|
|
pwriter.put(keys["last_error"], "big model stalled onroad; local fallback latched"
|
|
if latch.done else "big model stalled onroad; small active, big may re-arm")
|
|
if latch.done:
|
|
cloudlog.warning(f"modeld_selector big stalled, staying on small until next ignition (frame {target})")
|
|
else:
|
|
cloudlog.warning(f"modeld_selector big stalled, small active; big may re-arm after a clean streak (frame {target})")
|
|
|
|
if payload is not None:
|
|
selector_dropped = max(0, target - last_published - 1) if last_published >= 0 else 0
|
|
frames_dropped = frame_dropped_filter.update(min(selector_dropped, 10))
|
|
if run_count < WARMUP_FRAMES:
|
|
frame_dropped_filter.x = 0.0
|
|
frames_dropped = 0.0
|
|
run_count += 1
|
|
recent_big.append(used_big)
|
|
if used_big:
|
|
big_used_count += 1
|
|
big_fid = int(payload.get("frame_id", big_peek if big_peek is not None else target))
|
|
last_big_published = min(big_fid, target)
|
|
last_latency_ms = float(payload.get("model_execution_time", 0.0)) * 1e3
|
|
source_lag = max(0, target - int(payload.get("frame_id", target)))
|
|
frame_mismatch = int(payload.get("frame_id", target)) != target
|
|
last_source_lag = source_lag
|
|
if run_count % STATUS_WINDOW == 0:
|
|
hit_rate = (sum(recent_big) / len(recent_big)) if recent_big else 0.0
|
|
pwriter.put(keys["latency_ms"], last_latency_ms)
|
|
pwriter.put(keys["status"], json.dumps({
|
|
"active": latch.active,
|
|
"failed": latch.done,
|
|
"hit_rate": round(hit_rate, 3),
|
|
"latency_ms": round(last_latency_ms, 1),
|
|
"source_lag_frames": last_source_lag,
|
|
"model": model_name,
|
|
"reachable": params.get_bool(keys["reachable"]),
|
|
"download_progress": _read_float(params, keys["progress"], 1.0),
|
|
"ts_mono": round(time.monotonic(), 1),
|
|
}))
|
|
if run_count % 100 == 0:
|
|
cloudlog.warning(f"modeld_selector misses: {miss_reasons}")
|
|
pwriter.put_bool(keys["active"], latch.active)
|
|
cloudlog.warning(f"modeld_selector: big_used={big_used_count}/{run_count} "
|
|
f"last_big_peek={big_peek} target={target} active={latch.active} "
|
|
f"max_big_lag={BIG_MAX_LAG_FRAMES}")
|
|
|
|
frame_drop_perc = 100.0 * frames_dropped / (1.0 + frames_dropped)
|
|
_patch_and_send(pm, payload, frame_drop_perc, selector_dropped, target, source_lag, frame_mismatch)
|
|
last_published = target
|
|
|
|
|
|
if __name__ == "__main__":
|
|
try:
|
|
main()
|
|
except KeyboardInterrupt:
|
|
cloudlog.warning("modeld_selector got SIGINT")
|