add big model fallback

openpilot/cereal/log.capnp

@@ -2347,6 +2347,12 @@ struct CameraOdometry {
   roadTransformTransStd @9 :List(Float32);
 }
 
+struct ModelOutput {
+  modelV2 @0 :ModelDataV2;
+  cameraOdometry @1 :CameraOdometry;
+  cameraOdometryValid @2 :Bool;
+}
+
 struct Sentinel {
   enum SentinelType {
     endOfSegment @0;
@@ -2542,6 +2548,8 @@ struct Event {
     driverMonitoringState @151 :DriverMonitoringState;
     livePose @129 :LivePose;
     modelV2 @75 :ModelDataV2;
+    smallModelV2 @152 :ModelOutput;
+    bigModelV2 @153 :ModelOutput;
     drivingModelData @128 :DrivingModelData;
     driverStateV2 @92 :DriverStateV2;
 

openpilot/cereal/services.py

@@ -70,6 +70,8 @@ def __init__(self, should_log: bool, frequency: float, decimation: Optional[int]
   "wideRoadCameraState": (True, 20., 20),
   "drivingModelData": (True, 20., 10),
   "modelV2": (True, 20., None, QueueSize.BIG),
+  "smallModelV2": (False, 20., None, QueueSize.BIG),
+  "bigModelV2": (False, 20., None, QueueSize.BIG),
   "managerState": (True, 2., 1),
   "qRoadEncodeIdx": (False, 20.),
   "userBookmark": (True, 0., 1),

openpilot/common/params_keys.h

@@ -129,5 +129,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"UptimeOnroad", {PERSISTENT, FLOAT, "0.0"}},
     {"UsbGpuPresent", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},
     {"UsbGpuCompiled", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},
+    {"UsbGpuActive", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}},
+    {"UsbGpuFailed", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}},
     {"Version", {PERSISTENT, STRING}},
 };

openpilot/selfdrive/modeld/bigmodeld.py

@@ -0,0 +1,19 @@
+#!/usr/bin/env python3
+import subprocess
+
+from openpilot.common.params import Params
+from openpilot.common.swaglog import cloudlog
+from openpilot.selfdrive.modeld import modeld
+
+
+def main() -> None:
+  try:
+    subprocess.run(["udevadm", "settle"], check=True)
+    modeld.main(model_name="bigModelV2")
+  except Exception:
+    cloudlog.exception("big model failed")
+    Params().put_bool("UsbGpuFailed", True, block=True)
+
+
+if __name__ == "__main__":
+  main()

openpilot/selfdrive/modeld/modeld.py

@@ -13,7 +13,7 @@
 from openpilot.common.swaglog import cloudlog
 from openpilot.common.params import Params
 from openpilot.common.filter_simple import FirstOrderFilter
-from openpilot.common.realtime import config_realtime_process, DT_MDL
+from openpilot.common.realtime import config_realtime_process, set_core_affinity, DT_MDL
 from openpilot.common.transformations.camera import DEVICE_CAMERAS
 from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
 from openpilot.common.transformations.model import get_warp_matrix
@@ -133,17 +133,28 @@ def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, np.ndarray],
     return outputs_dict
 
 
-def main(demo=False):
+def main(demo=False, model_name="modelV2"):
   cloudlog.warning("modeld init")
 
-  _present = usbgpu_present()
-  _compiled = os.path.isfile(get_manifest_path(modeld_pkl_path(usbgpu=True)))
-  USBGPU = _present and _compiled
   params = Params()
-  params.put_bool("UsbGpuPresent", _present)
-  params.put_bool("UsbGpuCompiled", _compiled)
+  if model_name == "modelV2":
+    _present = usbgpu_present()
+    _compiled = os.path.isfile(get_manifest_path(modeld_pkl_path(usbgpu=True)))
+    USBGPU = _present and _compiled
+    params.put_bool("UsbGpuPresent", _present)
+    params.put_bool("UsbGpuCompiled", _compiled)
+    if USBGPU and "REPLAY" not in os.environ and not demo:
+      model_name = "smallModelV2"
+      USBGPU = False
+  else:
+    USBGPU = model_name == "bigModelV2"
 
-  config_realtime_process(7, 54)
+  if model_name == "smallModelV2":
+    config_realtime_process([0, 1, 2, 3], 54)
+  elif model_name == "bigModelV2":
+    set_core_affinity([7])
+  else:
+    config_realtime_process(7, 54)
 
   # visionipc clients
   while True:
@@ -174,11 +185,13 @@ def main(demo=False):
   cloudlog.warning(f"models loaded in {time.monotonic() - st:.1f}s, modeld starting")
 
   # messaging
-  pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"])
+  if model_name == "modelV2":
+    pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"])
+  else:
+    pm = PubMaster([model_name])
   sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"])
 
   publish_state = PublishState()
-  params = Params()
 
   # setup filter to track dropped frames
   frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_RUN_FREQ)
@@ -286,10 +299,11 @@ def main(demo=False):
     model_output = model.run(bufs, transforms, inputs)
     mt2 = time.perf_counter()
     model_execution_time = mt2 - mt1
+    if model_name == "bigModelV2" and run_count == 1:
+      config_realtime_process(7, 54)
 
     if model_output is not None:
       modelv2_send = messaging.new_message('modelV2')
-      drivingdata_send = messaging.new_message('drivingModelData')
       posenet_send = messaging.new_message('cameraOdometry')
 
       action = get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego)
@@ -306,11 +320,20 @@ def main(demo=False):
       modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
       modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
 
-      fill_driving_model_data(drivingdata_send, modelv2_send)
       fill_pose_msg(posenet_send, model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, live_calib_seen)
-      pm.send('modelV2', modelv2_send)
-      pm.send('drivingModelData', drivingdata_send)
-      pm.send('cameraOdometry', posenet_send)
+      if model_name == "modelV2":
+        drivingdata_send = messaging.new_message('drivingModelData')
+        fill_driving_model_data(drivingdata_send, modelv2_send)
+        pm.send('modelV2', modelv2_send)
+        pm.send('drivingModelData', drivingdata_send)
+        pm.send('cameraOdometry', posenet_send)
+      else:
+        model_send = messaging.new_message(model_name, valid=modelv2_send.valid, logMonoTime=modelv2_send.logMonoTime)
+        candidate = getattr(model_send, model_name)
+        candidate.modelV2 = modelv2_send.modelV2
+        candidate.cameraOdometry = posenet_send.cameraOdometry
+        candidate.cameraOdometryValid = posenet_send.valid
+        pm.send(model_name, model_send)
     last_vipc_frame_id = meta_main.frame_id
 
 

openpilot/selfdrive/modeld/modelrouterd.py

@@ -0,0 +1,59 @@
+#!/usr/bin/env python3
+import openpilot.cereal.messaging as messaging
+from openpilot.cereal.messaging import PubMaster, SubMaster
+from openpilot.common.params import Params
+from openpilot.common.swaglog import cloudlog
+from openpilot.selfdrive.modeld.fill_model_msg import fill_driving_model_data
+
+
+def main() -> None:
+  params = Params()
+  sm = SubMaster(["smallModelV2", "bigModelV2", "carControl", "managerState"])
+  pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"])
+  model = "smallModelV2"
+  big_failed = params.get_bool("UsbGpuFailed")
+  last_frame_id = -1
+
+  while True:
+    sm.update()
+    big_available = sm.all_checks(["bigModelV2"])
+
+    not_running = False
+    if sm.updated["managerState"]:
+      proc = next(p for p in sm["managerState"].processes if p.name == "bigmodeld")
+      not_running = proc.shouldBeRunning and not proc.running
+
+    if ((model == "bigModelV2" and not big_available) or not_running) and not big_failed:
+      big_failed = True
+      params.put_bool("UsbGpuFailed", True)
+      cloudlog.event("big_model_unavailable", error=True)
+
+    if sm.updated["carControl"] and sm.all_checks(["carControl"]) and not sm["carControl"].enabled:
+      big_failed |= params.get_bool("UsbGpuFailed")
+      next_model = "bigModelV2" if big_available and not big_failed else "smallModelV2"
+      if model != next_model:
+        model = next_model
+        params.put_bool("UsbGpuActive", model == "bigModelV2", block=True)
+
+    if not sm.updated[model]:
+      continue
+
+    output = sm[model]
+    frame_id = output.modelV2.frameId
+    if frame_id <= last_frame_id:
+      continue
+
+    model_send = messaging.new_message("modelV2", valid=sm.valid[model], logMonoTime=sm.logMonoTime[model])
+    model_send.modelV2 = output.modelV2
+    drivingdata_send = messaging.new_message("drivingModelData", logMonoTime=sm.logMonoTime[model])
+    fill_driving_model_data(drivingdata_send, model_send)
+    posenet_send = messaging.new_message("cameraOdometry", valid=output.cameraOdometryValid, logMonoTime=sm.logMonoTime[model])
+    posenet_send.cameraOdometry = output.cameraOdometry
+    pm.send("modelV2", model_send)
+    pm.send("drivingModelData", drivingdata_send)
+    pm.send("cameraOdometry", posenet_send)
+    last_frame_id = frame_id
+
+
+if __name__ == "__main__":
+  main()

openpilot/system/manager/process_config.py

@@ -49,6 +49,12 @@ def not_long_maneuver(started: bool, params: Params, CP: car.CarParams) -> bool:
 def qcomgps(started: bool, params: Params, CP: car.CarParams) -> bool:
   return started and not ublox_available()
 
+def usbgpu(started: bool, params: Params, CP: car.CarParams) -> bool:
+  return started and "REPLAY" not in os.environ and params.get_bool("UsbGpuCompiled") and params.get_bool("UsbGpuPresent")
+
+def bigmodeld(started: bool, params: Params, CP: car.CarParams) -> bool:
+  return usbgpu(started, params, CP) and not params.get_bool("UsbGpuFailed")
+
 def always_run(started: bool, params: Params, CP: car.CarParams) -> bool:
   return True
 
@@ -86,6 +92,8 @@ def not_(*fns):
   PythonProcess("timed", "openpilot.system.timed", always_run, enabled=not PC),
 
   PythonProcess("modeld", "openpilot.selfdrive.modeld.modeld", only_onroad),
+  PythonProcess("bigmodeld", "openpilot.selfdrive.modeld.bigmodeld", bigmodeld),
+  PythonProcess("modelrouterd", "openpilot.selfdrive.modeld.modelrouterd", usbgpu),
   PythonProcess("dmonitoringmodeld", "openpilot.selfdrive.modeld.dmonitoringmodeld", driverview, enabled=(WEBCAM or not PC)),
 
   PythonProcess("sensord", "openpilot.system.sensord.sensord", only_onroad, enabled=not PC),