IQ.Pilot Release Commit @ f2a861c

iqpilot/selfdrive/iqmodeld/daemon.py

@@ -1,60 +1,62 @@
 #!/usr/bin/env python3
 from __future__ import annotations
 
+import sys
 import time
 from dataclasses import dataclass
 from typing import Any
 
-import cereal.messaging as messaging
+import iqpilot.cereal.messaging as messaging
 import numpy as np
 
-from cereal import car, custom, log
-from cereal.messaging import PubMaster, SubMaster
-from msgq.visionipc import VisionBuf, VisionIpcClient, VisionStreamType
+from iqpilot.cereal import car, custom, log
+from iqpilot.cereal.messaging import PubMaster, SubMaster
+from iqpilot.cereal.visionipc import VisionStreamType
+from msgq.visionipc import VisionBuf, VisionIpcClient
 from iqdbc.car.car_helpers import get_demo_car_params
 from setproctitle import setproctitle
 
-from openpilot.common.filter_simple import FirstOrderFilter
-from openpilot.common.iq_perf import PerfSample, PerfTraceEmitter, PerfTraceRing
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL, config_realtime_process
-from openpilot.common.swaglog import cloudlog
-from openpilot.common.transformations.camera import DEVICE_CAMERAS
-from openpilot.common.transformations.model import get_warp_matrix
-from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
-from openpilot.selfdrive.controls.lib.drive_helpers import (
+from iqpilot.common.filter_simple import FirstOrderFilter
+from iqpilot.common.iq_perf import PerfSample, PerfTraceEmitter, PerfTraceRing
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import DT_MDL, config_realtime_process
+from iqpilot.common.swaglog import cloudlog
+from iqpilot.common.transformations.camera import DEVICE_CAMERAS
+from iqpilot.common.transformations.model import get_warp_matrix
+from iqpilot.selfdrive.controls.lib.desire_helper import DesireHelper
+from iqpilot.selfdrive.controls.lib.drive_helpers import (
   MODEL_SMOOTHING_MAX_TOTAL_SEC,
   dynamic_lat_smooth_extra_seconds,
   get_accel_from_plan,
   smooth_value,
 )
-from openpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy
-from openpilot.system import sentry
+from iqpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy
+from iqpilot.system import sentry
 
-from openpilot.iqpilot.common.steer_delay import resolve_steer_delay
-from openpilot.iqpilot.selfdrive.iqmodeld.models.helpers import get_active_bundle
-from openpilot.iqpilot.selfdrive.iqmodeld.models.inference_state import InferenceStateBase
-from openpilot.iqpilot.selfdrive.iqmodeld.models.runners.model_runner import get_model_runner
-from openpilot.iqpilot.selfdrive.iqmodeld.camera import CameraOffsetHelper
-from openpilot.iqpilot.selfdrive.iqmodeld.config import Plan
-from openpilot.iqpilot.selfdrive.iqmodeld.messaging import (
+from iqpilot.common.steer_delay import lateral_action_delay
+from iqpilot.selfdrive.iqmodeld.models.helpers import get_active_bundle
+from iqpilot.selfdrive.iqmodeld.models.inference_state import InferenceStateBase
+from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import get_model_runner
+from iqpilot.selfdrive.iqmodeld.camera import CameraOffsetHelper
+from iqpilot.selfdrive.iqmodeld.config import Plan
+from iqpilot.selfdrive.iqmodeld.messaging import (
   DrivePacketMemory,
   pick_curvature,
   populate_drive_messages,
   populate_odometry_message,
 )
-from openpilot.iqpilot.selfdrive.iqmodeld.metadata import select_meta_layout
+from iqpilot.selfdrive.iqmodeld.metadata import select_meta_layout
 
 try:
-  from openpilot.iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx import RoadProjector, WarpContext
+  from iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx import RoadProjector, WarpContext
 except ModuleNotFoundError:
   class WarpContext:
     def __init__(self, *args, **kwargs):
-      raise ModuleNotFoundError("openpilot.iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx is not built")
+      raise ModuleNotFoundError("iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx is not built")
 
   class RoadProjector:
     def __init__(self, *args, **kwargs):
-      raise ModuleNotFoundError("openpilot.iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx is not built")
+      raise ModuleNotFoundError("iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx is not built")
 
 
 PROCESS_NAME = "iqpilot.selfdrive.iqmodeld.daemon"
@@ -62,6 +64,9 @@ IQP_NAV_MODEL_INFLUENCE_ENABLED = False
 TurnDirection = custom.IQTurnSignalDirection
 IQMODEL_EVAL_WARN_US = int(DT_MDL * 1_000_000)
 IQMODEL_EVAL_ERROR_US = IQMODEL_EVAL_WARN_US * 2
+_FRAME_STARVED_BACKOFF_POLLS = 5
+_FRAME_STARVED_BACKOFF_SECONDS = 0.005
+_FRAME_STARVED_LOG_EVERY = 200
 
 
 def _plan_y_std_1s(outputs: dict[str, np.ndarray]) -> float:
@@ -422,12 +427,12 @@ class CalibrationAtlas:
     self._offset_tuner.set_offset(offset_value)
 
   def refresh(self, sm: SubMaster, main_is_wide: bool, dual_camera: bool) -> tuple[np.ndarray, np.ndarray, bool]:
-    if not (sm.seen["liveCalibration"] and sm.seen["roadCameraState"] and sm.seen["deviceState"]):
+    if not (sm.seen["extrinsicsCalibration"] and sm.seen["roadCameraState"] and sm.seen["deviceState"]):
       return self.main_warp, self.extra_warp, self.ready
 
-    rpy = get_calibrated_rpy(sm["liveCalibration"])
+    rpy = get_calibrated_rpy(sm["extrinsicsCalibration"])
     if rpy is None:
-      live_calib = sm["liveCalibration"]
+      live_calib = sm["extrinsicsCalibration"]
       if len(live_calib.rpyCalib) == 3:
         rpy = np.array(live_calib.rpyCalib, dtype=np.float32)
       else:
@@ -467,7 +472,7 @@ class FrameDropMeter:
 
 
 class InferenceDaemon:
-  def __init__(self, demo: bool = False):
+  def __init__(self, demo: bool = False, channel_path: str | None = None):
     cloudlog.warning("iqmodeld init")
     sentry.set_tag("daemon", PROCESS_NAME)
     cloudlog.bind(daemon=PROCESS_NAME)
@@ -481,11 +486,18 @@ class InferenceDaemon:
     self._meta_layout = select_meta_layout()
     cloudlog.warning("models loaded, iqmodeld starting")
 
+    self._channel = None
+    if channel_path is not None:
+      from iqpilot.selfdrive.iqmodeld.model_channel import ModelChannel
+      self._channel = ModelChannel(channel_path, create=True)
+
     self._cameras = CameraIngress(self._gpu)
-    self._pub = PubMaster(["modelV2", "drivingModelData", "cameraOdometry", "iqDriveModelData", "iqPerfTrace"])
+    pub_services = ["iqPerfTrace"] if self._channel is not None else [
+      "modelV2", "drivingModelData", "cameraOdometry", "iqDriveModelData", "iqPerfTrace"]
+    self._pub = PubMaster(pub_services)
     self._sub = SubMaster([
-      "deviceState", "carState", "roadCameraState", "liveCalibration",
-      "driverMonitoringState", "carControl", "liveDelay", "iqNavState", "radarState",
+      "deviceState", "carState", "roadCameraState", "extrinsicsCalibration",
+      "driverMonitoringState", "carControl", "lateralDelay", "iqNavState", "radarState",
     ])
     self._message_memory = DrivePacketMemory()
     self._params = Params()
@@ -509,7 +521,13 @@ class InferenceDaemon:
   def _refresh_tunables(self, tick: int) -> None:
     if tick % 60 != 0:
       return
-    self._runtime.lat_delay = resolve_steer_delay(self._params, self._sub["liveDelay"].lateralDelay)
+    from iqpilot.selfdrive.iqmodeld.egpu_helpers import egpu_selected
+    big_enabled = self._params.get_bool("IQEmacEnabled") or egpu_selected(self._params)
+    if big_enabled != (self._channel is not None):
+      # publish mode is fixed at startup: staying up would fight the selector for modelV2
+      cloudlog.warning("iqmodeld: big backend toggled, restarting to switch publish mode")
+      sys.exit(0)
+    self._runtime.lat_delay = lateral_action_delay(self._params, self._car_params, self._sub["lateralDelay"].lateralDelay)
     self._runtime.PLANPLUS_CONTROL = self._params.get("PlanplusControl", return_default=True)
     self._runtime.model_smoothing_max_extra_sec = _model_lat_smooth_max_sec(self._params)
     self._warps.set_offset(self._params.get("CameraOffset", return_default=True))
@@ -586,6 +604,7 @@ 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,
@@ -596,6 +615,22 @@ class InferenceDaemon:
       live_calib_seen,
     )
 
+    if self._channel is not None:
+      self._channel.write(main_stamp.frame_id, {
+        "source": "small",
+        "frame_id": main_stamp.frame_id,
+        "timestamp_sof": int(main_stamp.timestamp_sof),
+        "live_calib_seen": bool(live_calib_seen),
+        "model_execution_time": float(execution_time),
+        "msgs": {
+          "modelV2": model_msg.to_bytes(),
+          "drivingModelData": driving_msg.to_bytes(),
+          "cameraOdometry": pose_msg.to_bytes(),
+          "iqDriveModelData": iq_msg.to_bytes(),
+        },
+      })
+      return
+
     self._pub.send("modelV2", model_msg)
     self._pub.send("drivingModelData", driving_msg)
     self._pub.send("cameraOdometry", pose_msg)
@@ -603,12 +638,21 @@ class InferenceDaemon:
 
   def serve(self) -> None:
     tick = 0
+    starved_polls = 0
     while True:
       frame_pair = self._cameras.pull()
       if frame_pair is None:
-        cloudlog.debug("visionipc frame missing")
+        starved_polls += 1
+        if starved_polls >= _FRAME_STARVED_BACKOFF_POLLS:
+          time.sleep(_FRAME_STARVED_BACKOFF_SECONDS)
+        if starved_polls % _FRAME_STARVED_LOG_EVERY == 0:
+          cloudlog.error(f"visionipc delivered no frames for {starved_polls} polls; model is not running")
         continue
 
+      if starved_polls:
+        cloudlog.warning(f"visionipc recovered after {starved_polls} frameless polls")
+        starved_polls = 0
+
       main_buf, extra_buf, main_stamp, extra_stamp = frame_pair
       self._sub.update(0)
       self._refresh_tunables(tick)
@@ -682,8 +726,15 @@ class InferenceDaemon:
       tick += 1
 
 
-def main(demo: bool = False):
-  InferenceDaemon(demo=demo).serve()
+def main(demo: bool = False, channel_path: str | None = "auto"):
+  if channel_path == "auto":
+    channel_path = None
+    params = Params()
+    from iqpilot.selfdrive.iqmodeld.egpu_helpers import egpu_selected
+    if params.get_bool("IQEmacEnabled") or egpu_selected(params):
+      from iqpilot.selfdrive.iqmodeld.model_channel import SMALL_CHANNEL
+      channel_path = SMALL_CHANNEL
+  InferenceDaemon(demo=demo, channel_path=channel_path).serve()
 
 
 __all__ = [