IQ.Pilot Release Commit @ d2ce8a8
This commit is contained in:
IQ.Lvbs CI [bot]
373
iqpilot/selfdrive/iqmodeld/modeld_selector.py
Normal file
373
iqpilot/selfdrive/iqmodeld/modeld_selector.py
Normal file
@@ -0,0 +1,373 @@
|
||||
"""
|
||||
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
|
||||
|
||||
|
||||
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
|
||||
|
||||
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
|
||||
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
|
||||
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}")
|
||||
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")
|
||||
Reference in New Issue
Block a user