dragonpilot beta3

date: 2023-10-09T10:55:55
commit: 91b6e3aecd7170f24bccacb10c515ec281c30295

selfdrive/modeld/.gitignore

@@ -0,0 +1 @@
+*_pyx.cpp

selfdrive/modeld/dmonitoringmodeld

@@ -1,12 +0,0 @@
-#!/bin/sh
-
-DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
-cd $DIR
-
-if [ -f /TICI ]; then
-  export LD_LIBRARY_PATH="/usr/lib/aarch64-linux-gnu:/data/pythonpath/third_party/snpe/larch64:$LD_LIBRARY_PATH"
-  export ADSP_LIBRARY_PATH="/data/pythonpath/third_party/snpe/dsp/"
-else
-  export LD_LIBRARY_PATH="$DIR/../../third_party/snpe/x86_64-linux-clang:$DIR/../../openpilot/third_party/snpe/x86_64:$LD_LIBRARY_PATH"
-fi
-exec ./_dmonitoringmodeld

selfdrive/modeld/dmonitoringmodeld.py

@@ -0,0 +1,157 @@
+#!/usr/bin/env python3
+import os
+import gc
+import math
+import time
+import ctypes
+import numpy as np
+from pathlib import Path
+from typing import Tuple, Dict
+
+from cereal import messaging
+from cereal.messaging import PubMaster, SubMaster
+from cereal.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
+from openpilot.system.swaglog import cloudlog
+from openpilot.common.params import Params
+from openpilot.common.realtime import set_realtime_priority
+from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime
+from openpilot.selfdrive.modeld.models.commonmodel_pyx import sigmoid
+
+CALIB_LEN = 3
+REG_SCALE = 0.25
+MODEL_WIDTH = 1440
+MODEL_HEIGHT = 960
+OUTPUT_SIZE = 84
+SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
+MODEL_PATHS = {
+  ModelRunner.SNPE: Path(__file__).parent / 'models/dmonitoring_model_q.dlc',
+  ModelRunner.ONNX: Path(__file__).parent / 'models/dmonitoring_model.onnx'}
+
+class DriverStateResult(ctypes.Structure):
+  _fields_ = [
+    ("face_orientation", ctypes.c_float*3),
+    ("face_position", ctypes.c_float*3),
+    ("face_orientation_std", ctypes.c_float*3),
+    ("face_position_std", ctypes.c_float*3),
+    ("face_prob", ctypes.c_float),
+    ("_unused_a", ctypes.c_float*8),
+    ("left_eye_prob", ctypes.c_float),
+    ("_unused_b", ctypes.c_float*8),
+    ("right_eye_prob", ctypes.c_float),
+    ("left_blink_prob", ctypes.c_float),
+    ("right_blink_prob", ctypes.c_float),
+    ("sunglasses_prob", ctypes.c_float),
+    ("occluded_prob", ctypes.c_float),
+    ("ready_prob", ctypes.c_float*4),
+    ("not_ready_prob", ctypes.c_float*2)]
+
+class DMonitoringModelResult(ctypes.Structure):
+  _fields_ = [
+    ("driver_state_lhd", DriverStateResult),
+    ("driver_state_rhd", DriverStateResult),
+    ("poor_vision_prob", ctypes.c_float),
+    ("wheel_on_right_prob", ctypes.c_float)]
+
+class ModelState:
+  inputs: Dict[str, np.ndarray]
+  output: np.ndarray
+  model: ModelRunner
+
+  def __init__(self):
+    assert ctypes.sizeof(DMonitoringModelResult) == OUTPUT_SIZE * ctypes.sizeof(ctypes.c_float)
+    self.output = np.zeros(OUTPUT_SIZE, dtype=np.float32)
+    self.inputs = {
+      'input_img': np.zeros(MODEL_HEIGHT * MODEL_WIDTH, dtype=np.uint8),
+      'calib': np.zeros(CALIB_LEN, dtype=np.float32)}
+
+    self.model = ModelRunner(MODEL_PATHS, self.output, Runtime.DSP, True, None)
+    self.model.addInput("input_img", None)
+    self.model.addInput("calib", self.inputs['calib'])
+
+  def run(self, buf:VisionBuf, calib:np.ndarray) -> Tuple[np.ndarray, float]:
+    self.inputs['calib'][:] = calib
+
+    v_offset = buf.height - MODEL_HEIGHT
+    h_offset = (buf.width - MODEL_WIDTH) // 2
+    buf_data = buf.data.reshape(-1, buf.stride)
+    input_data = self.inputs['input_img'].reshape(MODEL_HEIGHT, MODEL_WIDTH)
+    input_data[:] = buf_data[v_offset:v_offset+MODEL_HEIGHT, h_offset:h_offset+MODEL_WIDTH]
+
+    t1 = time.perf_counter()
+    self.model.setInputBuffer("input_img", self.inputs['input_img'].view(np.float32))
+    self.model.execute()
+    t2 = time.perf_counter()
+    return self.output, t2 - t1
+
+
+def fill_driver_state(msg, ds_result: DriverStateResult):
+  msg.faceOrientation = [x * REG_SCALE for x in ds_result.face_orientation]
+  msg.faceOrientationStd = [math.exp(x) for x in ds_result.face_orientation_std]
+  msg.facePosition = [x * REG_SCALE for x in ds_result.face_position[:2]]
+  msg.facePositionStd = [math.exp(x) for x in ds_result.face_position_std[:2]]
+  msg.faceProb = sigmoid(ds_result.face_prob)
+  msg.leftEyeProb = sigmoid(ds_result.left_eye_prob)
+  msg.rightEyeProb = sigmoid(ds_result.right_eye_prob)
+  msg.leftBlinkProb = sigmoid(ds_result.left_blink_prob)
+  msg.rightBlinkProb = sigmoid(ds_result.right_blink_prob)
+  msg.sunglassesProb = sigmoid(ds_result.sunglasses_prob)
+  msg.occludedProb = sigmoid(ds_result.occluded_prob)
+  msg.readyProb = [sigmoid(x) for x in ds_result.ready_prob]
+  msg.notReadyProb = [sigmoid(x) for x in ds_result.not_ready_prob]
+
+def get_driverstate_packet(model_output: np.ndarray, frame_id: int, location_ts: int, execution_time: float, dsp_execution_time: float):
+  model_result = ctypes.cast(model_output.ctypes.data, ctypes.POINTER(DMonitoringModelResult)).contents
+  msg = messaging.new_message('driverStateV2')
+  ds = msg.driverStateV2
+  ds.frameId = frame_id
+  ds.modelExecutionTime = execution_time
+  ds.dspExecutionTime = dsp_execution_time
+  ds.poorVisionProb = sigmoid(model_result.poor_vision_prob)
+  ds.wheelOnRightProb = sigmoid(model_result.wheel_on_right_prob)
+  ds.rawPredictions = model_output.tobytes() if SEND_RAW_PRED else b''
+  fill_driver_state(ds.leftDriverData, model_result.driver_state_lhd)
+  fill_driver_state(ds.rightDriverData, model_result.driver_state_rhd)
+  return msg
+
+
+def main():
+  gc.disable()
+  set_realtime_priority(1)
+
+  model = ModelState()
+  cloudlog.warning("models loaded, dmonitoringmodeld starting")
+  Params().put_bool("DmModelInitialized", True)
+
+  cloudlog.warning("connecting to driver stream")
+  vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_DRIVER, True)
+  while not vipc_client.connect(False):
+    time.sleep(0.1)
+  assert vipc_client.is_connected()
+  cloudlog.warning(f"connected with buffer size: {vipc_client.buffer_len}")
+
+  sm = SubMaster(["liveCalibration"])
+  pm = PubMaster(["driverStateV2"])
+
+  calib = np.zeros(CALIB_LEN, dtype=np.float32)
+  # last = 0
+
+  while True:
+    buf = vipc_client.recv()
+    if buf is None:
+      continue
+
+    sm.update(0)
+    if sm.updated["liveCalibration"]:
+      calib[:] = np.array(sm["liveCalibration"].rpyCalib)
+
+    t1 = time.perf_counter()
+    model_output, dsp_execution_time = model.run(buf, calib)
+    t2 = time.perf_counter()
+
+    pm.send("driverStateV2", get_driverstate_packet(model_output, vipc_client.frame_id, vipc_client.timestamp_sof, t2 - t1, dsp_execution_time))
+    # print("dmonitoring process: %.2fms, from last %.2fms\n" % (t2 - t1, t1 - last))
+    # last = t1
+
+
+if __name__ == "__main__":
+  main()

selfdrive/modeld/modeld

@@ -1,11 +1,10 @@
-#!/bin/sh
+#!/usr/bin/env bash
 
 DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
-cd $DIR
+cd "$DIR/../../"
 
-if [ -f /TICI ]; then
-  export LD_LIBRARY_PATH="/usr/lib/aarch64-linux-gnu:/data/pythonpath/third_party/snpe/larch64:$LD_LIBRARY_PATH"
-else
-  export LD_LIBRARY_PATH="$DIR/../../third_party/snpe/x86_64-linux-clang:$DIR/../../openpilot/third_party/snpe/x86_64:$LD_LIBRARY_PATH"
+if [ -f "$DIR/libthneed.so" ]; then
+  export LD_PRELOAD="$DIR/libthneed.so"
 fi
-exec ./_modeld
+
+exec "$DIR/modeld.py"

selfdrive/modeld/modeld.py

@@ -0,0 +1,261 @@
+#!/usr/bin/env python3
+import sys
+import time
+import numpy as np
+from pathlib import Path
+from typing import Dict, Optional
+from setproctitle import setproctitle
+from cereal.messaging import PubMaster, SubMaster
+from cereal.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
+from openpilot.system.swaglog import cloudlog
+from openpilot.common.params import Params
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.realtime import config_realtime_process
+from openpilot.common.transformations.model import get_warp_matrix
+from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime
+from openpilot.selfdrive.modeld.models.commonmodel_pyx import ModelFrame, CLContext
+from openpilot.selfdrive.modeld.models.driving_pyx import (
+  PublishState, create_model_msg, create_pose_msg,
+  FEATURE_LEN, HISTORY_BUFFER_LEN, DESIRE_LEN, TRAFFIC_CONVENTION_LEN, NAV_FEATURE_LEN, NAV_INSTRUCTION_LEN,
+  OUTPUT_SIZE, NET_OUTPUT_SIZE, MODEL_FREQ)
+
+MODEL_PATHS = {
+  ModelRunner.THNEED: Path(__file__).parent / 'models/supercombo.thneed',
+  ModelRunner.ONNX: Path(__file__).parent / 'models/supercombo.onnx'}
+
+class FrameMeta:
+  frame_id: int = 0
+  timestamp_sof: int = 0
+  timestamp_eof: int = 0
+
+  def __init__(self, vipc=None):
+    if vipc is not None:
+      self.frame_id, self.timestamp_sof, self.timestamp_eof = vipc.frame_id, vipc.timestamp_sof, vipc.timestamp_eof
+
+class ModelState:
+  frame: ModelFrame
+  wide_frame: ModelFrame
+  inputs: Dict[str, np.ndarray]
+  output: np.ndarray
+  prev_desire: np.ndarray  # for tracking the rising edge of the pulse
+  model: ModelRunner
+
+  def __init__(self, context: CLContext):
+    self.frame = ModelFrame(context)
+    self.wide_frame = ModelFrame(context)
+    self.prev_desire = np.zeros(DESIRE_LEN, dtype=np.float32)
+    self.output = np.zeros(NET_OUTPUT_SIZE, dtype=np.float32)
+    self.inputs = {
+      'desire': np.zeros(DESIRE_LEN * (HISTORY_BUFFER_LEN+1), dtype=np.float32),
+      'traffic_convention': np.zeros(TRAFFIC_CONVENTION_LEN, dtype=np.float32),
+      'nav_features': np.zeros(NAV_FEATURE_LEN, dtype=np.float32),
+      'nav_instructions': np.zeros(NAV_INSTRUCTION_LEN, dtype=np.float32),
+      'features_buffer': np.zeros(HISTORY_BUFFER_LEN * FEATURE_LEN, dtype=np.float32),
+    }
+
+    self.model = ModelRunner(MODEL_PATHS, self.output, Runtime.GPU, False, context)
+    self.model.addInput("input_imgs", None)
+    self.model.addInput("big_input_imgs", None)
+    for k,v in self.inputs.items():
+      self.model.addInput(k, v)
+
+  def run(self, buf: VisionBuf, wbuf: VisionBuf, transform: np.ndarray, transform_wide: np.ndarray,
+                inputs: Dict[str, np.ndarray], prepare_only: bool) -> Optional[np.ndarray]:
+    # Model decides when action is completed, so desire input is just a pulse triggered on rising edge
+    inputs['desire'][0] = 0
+    self.inputs['desire'][:-DESIRE_LEN] = self.inputs['desire'][DESIRE_LEN:]
+    self.inputs['desire'][-DESIRE_LEN:] = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0)
+    self.prev_desire[:] = inputs['desire']
+
+    self.inputs['traffic_convention'][:] = inputs['traffic_convention']
+    self.inputs['nav_features'][:] = inputs['nav_features']
+    self.inputs['nav_instructions'][:] = inputs['nav_instructions']
+    # self.inputs['driving_style'][:] = inputs['driving_style']
+
+    # if getCLBuffer is not None, frame will be None
+    self.model.setInputBuffer("input_imgs", self.frame.prepare(buf, transform.flatten(), self.model.getCLBuffer("input_imgs")))
+    if wbuf is not None:
+      self.model.setInputBuffer("big_input_imgs", self.wide_frame.prepare(wbuf, transform_wide.flatten(), self.model.getCLBuffer("big_input_imgs")))
+
+    if prepare_only:
+      return None
+
+    self.model.execute()
+    self.inputs['features_buffer'][:-FEATURE_LEN] = self.inputs['features_buffer'][FEATURE_LEN:]
+    self.inputs['features_buffer'][-FEATURE_LEN:] = self.output[OUTPUT_SIZE:OUTPUT_SIZE+FEATURE_LEN]
+    return self.output
+
+
+def main():
+  cloudlog.bind(daemon="selfdrive.modeld.modeld")
+  setproctitle("selfdrive.modeld.modeld")
+  config_realtime_process(7, 54)
+
+  cl_context = CLContext()
+  model = ModelState(cl_context)
+  cloudlog.warning("models loaded, modeld starting")
+
+  # visionipc clients
+  while True:
+    available_streams = VisionIpcClient.available_streams("camerad", block=False)
+    if available_streams:
+      use_extra_client = VisionStreamType.VISION_STREAM_WIDE_ROAD in available_streams and VisionStreamType.VISION_STREAM_ROAD in available_streams
+      main_wide_camera = VisionStreamType.VISION_STREAM_ROAD not in available_streams
+      break
+    time.sleep(.1)
+
+  vipc_client_main_stream = VisionStreamType.VISION_STREAM_WIDE_ROAD if main_wide_camera else VisionStreamType.VISION_STREAM_ROAD
+  vipc_client_main = VisionIpcClient("camerad", vipc_client_main_stream, True, cl_context)
+  vipc_client_extra = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_WIDE_ROAD, False, cl_context)
+  cloudlog.warning(f"vision stream set up, main_wide_camera: {main_wide_camera}, use_extra_client: {use_extra_client}")
+
+  while not vipc_client_main.connect(False):
+    time.sleep(0.1)
+  while use_extra_client and not vipc_client_extra.connect(False):
+    time.sleep(0.1)
+
+  cloudlog.warning(f"connected main cam with buffer size: {vipc_client_main.buffer_len} ({vipc_client_main.width} x {vipc_client_main.height})")
+  if use_extra_client:
+    cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})")
+
+  # messaging
+  pm = PubMaster(["modelV2", "cameraOdometry"])
+  sm = SubMaster(["lateralPlan", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction"])
+
+  state = PublishState()
+  params = Params()
+
+  # setup filter to track dropped frames
+  frame_dropped_filter = FirstOrderFilter(0., 10., 1. / MODEL_FREQ)
+  frame_id = 0
+  last_vipc_frame_id = 0
+  run_count = 0
+  # last = 0.0
+
+  model_transform_main = np.zeros((3, 3), dtype=np.float32)
+  model_transform_extra = np.zeros((3, 3), dtype=np.float32)
+  live_calib_seen = False
+  driving_style = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], dtype=np.float32)
+  nav_features = np.zeros(NAV_FEATURE_LEN, dtype=np.float32)
+  nav_instructions = np.zeros(NAV_INSTRUCTION_LEN, dtype=np.float32)
+  buf_main, buf_extra = None, None
+  meta_main = FrameMeta()
+  meta_extra = FrameMeta()
+
+  while True:
+    # Keep receiving frames until we are at least 1 frame ahead of previous extra frame
+    while meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000:
+      buf_main = vipc_client_main.recv()
+      meta_main = FrameMeta(vipc_client_main)
+      if buf_main is None:
+        break
+
+    if buf_main is None:
+      cloudlog.error("vipc_client_main no frame")
+      continue
+
+    if use_extra_client:
+      # Keep receiving extra frames until frame id matches main camera
+      while True:
+        buf_extra = vipc_client_extra.recv()
+        meta_extra = FrameMeta(vipc_client_extra)
+        if buf_extra is None or meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000:
+          break
+
+      if buf_extra is None:
+        cloudlog.error("vipc_client_extra no frame")
+        continue
+
+      if abs(meta_main.timestamp_sof - meta_extra.timestamp_sof) > 10000000:
+        cloudlog.error("frames out of sync! main: {} ({:.5f}), extra: {} ({:.5f})".format(
+          meta_main.frame_id, meta_main.timestamp_sof / 1e9,
+          meta_extra.frame_id, meta_extra.timestamp_sof / 1e9))
+
+    else:
+      # Use single camera
+      buf_extra = buf_main
+      meta_extra = meta_main
+
+    # TODO: path planner timeout?
+    sm.update(0)
+    desire = sm["lateralPlan"].desire.raw
+    is_rhd = sm["driverMonitoringState"].isRHD
+    frame_id = sm["roadCameraState"].frameId
+    if sm.updated["liveCalibration"]:
+      device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32)
+      model_transform_main = get_warp_matrix(device_from_calib_euler, main_wide_camera, False).astype(np.float32)
+      model_transform_extra = get_warp_matrix(device_from_calib_euler, True, True).astype(np.float32)
+      live_calib_seen = True
+
+    traffic_convention = np.zeros(2)
+    traffic_convention[int(is_rhd)] = 1
+
+    vec_desire = np.zeros(DESIRE_LEN, dtype=np.float32)
+    if desire >= 0 and desire < DESIRE_LEN:
+      vec_desire[desire] = 1
+
+    # Enable/disable nav features
+    timestamp_llk = sm["navModel"].locationMonoTime
+    nav_valid = sm.valid["navModel"] # and (nanos_since_boot() - timestamp_llk < 1e9)
+    nav_enabled = nav_valid # and params.get_bool("ExperimentalMode")
+
+    if not nav_enabled:
+      nav_features[:] = 0
+      nav_instructions[:] = 0
+
+    if nav_enabled and sm.updated["navModel"]:
+      nav_features = np.array(sm["navModel"].features)
+
+    if nav_enabled and sm.updated["navInstruction"]:
+      nav_instructions[:] = 0
+      for maneuver in sm["navInstruction"].allManeuvers:
+        distance_idx = 25 + int(maneuver.distance / 20)
+        direction_idx = 0
+        if maneuver.modifier in ("left", "slight left", "sharp left"):
+          direction_idx = 1
+        if maneuver.modifier in ("right", "slight right", "sharp right"):
+          direction_idx = 2
+        if 0 <= distance_idx < 50:
+          nav_instructions[distance_idx*3 + direction_idx] = 1
+
+    # tracked dropped frames
+    vipc_dropped_frames = max(0, meta_main.frame_id - last_vipc_frame_id - 1)
+    frames_dropped = frame_dropped_filter.update(min(vipc_dropped_frames, 10))
+    if run_count < 10: # let frame drops warm up
+      frame_dropped_filter.x = 0.
+      frames_dropped = 0.
+    run_count = run_count + 1
+
+    frame_drop_ratio = frames_dropped / (1 + frames_dropped)
+    prepare_only = vipc_dropped_frames > 0
+    if prepare_only:
+      cloudlog.error(f"skipping model eval. Dropped {vipc_dropped_frames} frames")
+
+    inputs:Dict[str, np.ndarray] = {
+      'desire': vec_desire,
+      'traffic_convention': traffic_convention,
+      'driving_style': driving_style,
+      'nav_features': nav_features,
+      'nav_instructions': nav_instructions}
+
+    mt1 = time.perf_counter()
+    model_output = model.run(buf_main, buf_extra, model_transform_main, model_transform_extra, inputs, prepare_only)
+    mt2 = time.perf_counter()
+    model_execution_time = mt2 - mt1
+
+    if model_output is not None:
+      pm.send("modelV2", create_model_msg(model_output, state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio,
+                                          meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, live_calib_seen))
+      pm.send("cameraOdometry", create_pose_msg(model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, live_calib_seen))
+
+    # print("model process: %.2fms, from last %.2fms, vipc_frame_id %u, frame_id, %u, frame_drop %.3f" %
+    #   ((mt2 - mt1)*1000, (mt1 - last)*1000, meta_extra.frame_id, frame_id, frame_drop_ratio))
+    # last = mt1
+    last_vipc_frame_id = meta_main.frame_id
+
+
+if __name__ == "__main__":
+  try:
+    main()
+  except KeyboardInterrupt:
+    sys.exit()

selfdrive/modeld/models/commonmodel.pxd

@@ -0,0 +1,20 @@
+# distutils: language = c++
+
+from cereal.visionipc.visionipc cimport cl_device_id, cl_context, cl_mem
+
+cdef extern from "common/mat.h":
+  cdef struct mat3:
+    float v[9]
+
+cdef extern from "common/clutil.h":
+  cdef unsigned long CL_DEVICE_TYPE_DEFAULT
+  cl_device_id cl_get_device_id(unsigned long)
+  cl_context cl_create_context(cl_device_id)
+
+cdef extern from "selfdrive/modeld/models/commonmodel.h":
+  float sigmoid(float)
+
+  cppclass ModelFrame:
+    int buf_size
+    ModelFrame(cl_device_id, cl_context)
+    float * prepare(cl_mem, int, int, int, int, mat3, cl_mem*)

selfdrive/modeld/models/commonmodel_pyx.pxd

@@ -0,0 +1,13 @@
+# distutils: language = c++
+
+from cereal.visionipc.visionipc cimport cl_mem
+from cereal.visionipc.visionipc_pyx cimport CLContext as BaseCLContext
+
+cdef class CLContext(BaseCLContext):
+  pass
+
+cdef class CLMem:
+  cdef cl_mem * mem;
+
+  @staticmethod
+  cdef create(void*)

selfdrive/modeld/models/commonmodel_pyx.pyx

@@ -0,0 +1,43 @@
+# distutils: language = c++
+# cython: c_string_encoding=ascii
+
+import numpy as np
+cimport numpy as cnp
+from libc.string cimport memcpy
+
+from cereal.visionipc.visionipc cimport cl_mem
+from cereal.visionipc.visionipc_pyx cimport VisionBuf, CLContext as BaseCLContext
+from .commonmodel cimport CL_DEVICE_TYPE_DEFAULT, cl_get_device_id, cl_create_context
+from .commonmodel cimport mat3, sigmoid as cppSigmoid, ModelFrame as cppModelFrame
+
+def sigmoid(x):
+  return cppSigmoid(x)
+
+cdef class CLContext(BaseCLContext):
+  def __cinit__(self):
+    self.device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT)
+    self.context = cl_create_context(self.device_id)
+
+cdef class CLMem:
+  @staticmethod
+  cdef create(void * cmem):
+    mem = CLMem()
+    mem.mem = <cl_mem*> cmem
+    return mem
+
+cdef class ModelFrame:
+  cdef cppModelFrame * frame
+
+  def __cinit__(self, CLContext context):
+    self.frame = new cppModelFrame(context.device_id, context.context)
+
+  def __dealloc__(self):
+    del self.frame
+
+  def prepare(self, VisionBuf buf, float[:] projection, CLMem output):
+    cdef mat3 cprojection
+    memcpy(cprojection.v, &projection[0], 9*sizeof(float))
+    cdef float * data = self.frame.prepare(buf.buf.buf_cl, buf.width, buf.height, buf.stride, buf.uv_offset, cprojection, output.mem)
+    if not data:
+      return None
+    return np.asarray(<cnp.float32_t[:self.frame.buf_size]> data)

selfdrive/modeld/models/dmonitoring.h

@@ -1,50 +0,0 @@
-#pragma once
-
-#include <vector>
-
-#include "cereal/messaging/messaging.h"
-#include "common/util.h"
-#include "selfdrive/modeld/models/commonmodel.h"
-#include "selfdrive/modeld/runners/run.h"
-
-#define CALIB_LEN 3
-
-#define OUTPUT_SIZE 84
-#define REG_SCALE 0.25f
-
-typedef struct DriverStateResult {
-  float face_orientation[3];
-  float face_orientation_std[3];
-  float face_position[2];
-  float face_position_std[2];
-  float face_prob;
-  float left_eye_prob;
-  float right_eye_prob;
-  float left_blink_prob;
-  float right_blink_prob;
-  float sunglasses_prob;
-  float occluded_prob;
-  float ready_prob[4];
-  float not_ready_prob[2];
-} DriverStateResult;
-
-typedef struct DMonitoringModelResult {
-  DriverStateResult driver_state_lhd;
-  DriverStateResult driver_state_rhd;
-  float poor_vision_prob;
-  float wheel_on_right_prob;
-  float dsp_execution_time;
-} DMonitoringModelResult;
-
-typedef struct DMonitoringModelState {
-  RunModel *m;
-  float output[OUTPUT_SIZE];
-  std::vector<uint8_t> net_input_buf;
-  float calib[CALIB_LEN];
-} DMonitoringModelState;
-
-void dmonitoring_init(DMonitoringModelState* s);
-DMonitoringModelResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, int stride, int uv_offset, float *calib);
-void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringModelResult &model_res, float execution_time, kj::ArrayPtr<const float> raw_pred);
-void dmonitoring_free(DMonitoringModelState* s);
-

selfdrive/modeld/models/driving.h

@@ -4,25 +4,18 @@
 #include <memory>
 
 #include "cereal/messaging/messaging.h"
-#include "cereal/visionipc/visionipc_client.h"
-#include "common/mat.h"
 #include "common/modeldata.h"
 #include "common/util.h"
 #include "selfdrive/modeld/models/commonmodel.h"
-#include "selfdrive/modeld/models/nav.h"
 #include "selfdrive/modeld/runners/run.h"
 
-// gate this here
-#define TEMPORAL
-#define DESIRE
-#define TRAFFIC_CONVENTION
-#define NAV
-
-constexpr int FEATURE_LEN = 128;
+constexpr int FEATURE_LEN = 512;
 constexpr int HISTORY_BUFFER_LEN = 99;
 constexpr int DESIRE_LEN = 8;
 constexpr int DESIRE_PRED_LEN = 4;
 constexpr int TRAFFIC_CONVENTION_LEN = 2;
+constexpr int NAV_FEATURE_LEN = 256;
+constexpr int NAV_INSTRUCTION_LEN = 150;
 constexpr int DRIVING_STYLE_LEN = 12;
 constexpr int MODEL_FREQ = 20;
 
@@ -31,10 +24,8 @@ constexpr int BLINKER_LEN = 6;
 constexpr int META_STRIDE = 7;
 
 constexpr int PLAN_MHP_N = 5;
-
 constexpr int LEAD_MHP_N = 2;
 constexpr int LEAD_TRAJ_LEN = 6;
-constexpr int LEAD_PRED_DIM = 4;
 constexpr int LEAD_MHP_SELECTION = 3;
 // Padding to get output shape as multiple of 4
 constexpr int PAD_SIZE = 2;
@@ -253,49 +244,14 @@ struct ModelOutput {
 };
 
 constexpr int OUTPUT_SIZE = sizeof(ModelOutput) / sizeof(float);
-
-#ifdef TEMPORAL
-  constexpr int TEMPORAL_SIZE = HISTORY_BUFFER_LEN * FEATURE_LEN;
-#else
-  constexpr int TEMPORAL_SIZE = 0;
-#endif
 constexpr int NET_OUTPUT_SIZE = OUTPUT_SIZE + FEATURE_LEN + PAD_SIZE;
 
-// TODO: convert remaining arrays to std::array and update model runners
-struct ModelState {
-  ModelFrame *frame = nullptr;
-  ModelFrame *wide_frame = nullptr;
-  std::array<float, HISTORY_BUFFER_LEN * FEATURE_LEN> feature_buffer = {};
-  std::array<float, NET_OUTPUT_SIZE> output = {};
-  std::unique_ptr<RunModel> m;
-#ifdef DESIRE
-  float prev_desire[DESIRE_LEN] = {};
-  float pulse_desire[DESIRE_LEN*(HISTORY_BUFFER_LEN+1)] = {};
-#endif
-#ifdef TRAFFIC_CONVENTION
-  float traffic_convention[TRAFFIC_CONVENTION_LEN] = {};
-#endif
-#ifdef DRIVING_STYLE
-  float driving_style[DRIVING_STYLE_LEN] = {};
-#endif
-#ifdef NAV
-  float nav_features[NAV_FEATURE_LEN] = {};
-  float nav_instructions[NAV_INSTRUCTION_LEN] = {};
-#endif
-};
-
 struct PublishState {
   std::array<float, DISENGAGE_LEN * DISENGAGE_LEN> disengage_buffer = {};
   std::array<float, 5> prev_brake_5ms2_probs = {};
   std::array<float, 3> prev_brake_3ms2_probs = {};
 };
 
-void model_init(ModelState* s, cl_device_id device_id, cl_context context);
-ModelOutput *model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* buf_wide, const mat3 &transform, const mat3 &transform_wide,
-                              float *desire_in, bool is_rhd, float *driving_style, float *nav_features, float *nav_instructions, bool prepare_only);
-void model_free(ModelState* s);
-void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
-                   const ModelOutput &net_outputs, ModelState &s, PublishState &ps, uint64_t timestamp_eof, uint64_t timestamp_llk,
-                   float model_execution_time, const bool nav_enabled, const bool valid);
-void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames,
-                     const ModelOutput &net_outputs, uint64_t timestamp_eof, const bool valid);
+void fill_model_msg(MessageBuilder &msg, float *net_output_data, PublishState &ps, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
+                    uint64_t timestamp_eof, uint64_t timestamp_llk, float model_execution_time, const bool nav_enabled, const bool valid);
+void fill_pose_msg(MessageBuilder &msg, float *net_outputs, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, uint64_t timestamp_eof, const bool valid);

selfdrive/modeld/models/driving.pxd

@@ -0,0 +1,25 @@
+# distutils: language = c++
+
+from libcpp cimport bool
+from libc.stdint cimport uint32_t, uint64_t
+
+cdef extern from "cereal/messaging/messaging.h":
+  cdef cppclass MessageBuilder:
+    size_t getSerializedSize()
+    int serializeToBuffer(unsigned char *, size_t)
+
+cdef extern from "selfdrive/modeld/models/driving.h":
+  cdef int FEATURE_LEN
+  cdef int HISTORY_BUFFER_LEN
+  cdef int DESIRE_LEN
+  cdef int TRAFFIC_CONVENTION_LEN
+  cdef int DRIVING_STYLE_LEN
+  cdef int NAV_FEATURE_LEN
+  cdef int NAV_INSTRUCTION_LEN
+  cdef int OUTPUT_SIZE
+  cdef int NET_OUTPUT_SIZE
+  cdef int MODEL_FREQ
+  cdef struct PublishState: pass
+
+  void fill_model_msg(MessageBuilder, float *, PublishState, uint32_t, uint32_t, uint32_t, float, uint64_t, uint64_t, float, bool, bool)
+  void fill_pose_msg(MessageBuilder, float *, uint32_t, uint32_t, uint64_t, bool)

selfdrive/modeld/models/driving_pyx.pyx

@@ -0,0 +1,52 @@
+# distutils: language = c++
+# cython: c_string_encoding=ascii
+
+import numpy as np
+cimport numpy as cnp
+from libcpp cimport bool
+from libc.string cimport memcpy
+from libc.stdint cimport uint32_t, uint64_t
+
+from .commonmodel cimport mat3
+from .driving cimport FEATURE_LEN as CPP_FEATURE_LEN, HISTORY_BUFFER_LEN as CPP_HISTORY_BUFFER_LEN, DESIRE_LEN as CPP_DESIRE_LEN, \
+                      TRAFFIC_CONVENTION_LEN as CPP_TRAFFIC_CONVENTION_LEN, DRIVING_STYLE_LEN as CPP_DRIVING_STYLE_LEN, \
+                      NAV_FEATURE_LEN as CPP_NAV_FEATURE_LEN, NAV_INSTRUCTION_LEN as CPP_NAV_INSTRUCTION_LEN, \
+                      OUTPUT_SIZE as CPP_OUTPUT_SIZE, NET_OUTPUT_SIZE as CPP_NET_OUTPUT_SIZE, MODEL_FREQ as CPP_MODEL_FREQ
+from .driving cimport MessageBuilder, PublishState as cppPublishState
+from .driving cimport fill_model_msg, fill_pose_msg
+
+FEATURE_LEN = CPP_FEATURE_LEN
+HISTORY_BUFFER_LEN = CPP_HISTORY_BUFFER_LEN
+DESIRE_LEN = CPP_DESIRE_LEN
+TRAFFIC_CONVENTION_LEN = CPP_TRAFFIC_CONVENTION_LEN
+DRIVING_STYLE_LEN = CPP_DRIVING_STYLE_LEN
+NAV_FEATURE_LEN = CPP_NAV_FEATURE_LEN
+NAV_INSTRUCTION_LEN = CPP_NAV_INSTRUCTION_LEN
+OUTPUT_SIZE = CPP_OUTPUT_SIZE
+NET_OUTPUT_SIZE = CPP_NET_OUTPUT_SIZE
+MODEL_FREQ = CPP_MODEL_FREQ
+
+cdef class PublishState:
+  cdef cppPublishState state
+
+def create_model_msg(float[:] model_outputs, PublishState ps, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
+                     uint64_t timestamp_eof, uint64_t timestamp_llk, float model_execution_time, bool nav_enabled, bool valid):
+  cdef MessageBuilder msg
+  fill_model_msg(msg, &model_outputs[0], ps.state, vipc_frame_id, vipc_frame_id_extra, frame_id, frame_drop,
+                 timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, valid)
+
+  output_size = msg.getSerializedSize()
+  output_data = bytearray(output_size)
+  cdef unsigned char * output_ptr = output_data
+  assert msg.serializeToBuffer(output_ptr, output_size) > 0, "output buffer is too small to serialize"
+  return bytes(output_data)
+
+def create_pose_msg(float[:] model_outputs, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, uint64_t timestamp_eof, bool valid):
+  cdef MessageBuilder msg
+  fill_pose_msg(msg, &model_outputs[0], vipc_frame_id, vipc_dropped_frames, timestamp_eof, valid)
+
+  output_size = msg.getSerializedSize()
+  output_data = bytearray(output_size)
+  cdef unsigned char * output_ptr = output_data
+  assert msg.serializeToBuffer(output_ptr, output_size) > 0, "output buffer is too small to serialize"
+  return bytes(output_data)

selfdrive/modeld/models/nav.h

@@ -1,57 +0,0 @@
-#pragma once
-
-#include "cereal/messaging/messaging.h"
-#include "cereal/visionipc/visionipc_client.h"
-#include "common/util.h"
-#include "common/modeldata.h"
-#include "selfdrive/modeld/models/commonmodel.h"
-#include "selfdrive/modeld/runners/run.h"
-
-constexpr int NAV_INPUT_SIZE = 256*256;
-constexpr int NAV_FEATURE_LEN = 256;
-constexpr int NAV_INSTRUCTION_LEN = 150;
-constexpr int NAV_DESIRE_LEN = 32;
-
-struct NavModelOutputXY {
-  float x;
-  float y;
-};
-static_assert(sizeof(NavModelOutputXY) == sizeof(float)*2);
-
-struct NavModelOutputPlan {
-  std::array<NavModelOutputXY, TRAJECTORY_SIZE> mean;
-  std::array<NavModelOutputXY, TRAJECTORY_SIZE> std;
-};
-static_assert(sizeof(NavModelOutputPlan) == sizeof(NavModelOutputXY)*TRAJECTORY_SIZE*2);
-
-struct NavModelOutputDesirePrediction {
-  std::array<float, NAV_DESIRE_LEN> values;
-};
-static_assert(sizeof(NavModelOutputDesirePrediction) == sizeof(float)*NAV_DESIRE_LEN);
-
-struct NavModelOutputFeatures {
-  std::array<float, NAV_FEATURE_LEN> values;
-};
-static_assert(sizeof(NavModelOutputFeatures) == sizeof(float)*NAV_FEATURE_LEN);
-
-struct NavModelResult {
-  const NavModelOutputPlan plan;
-  const NavModelOutputDesirePrediction desire_pred;
-  const NavModelOutputFeatures features;
-  float dsp_execution_time;
-};
-static_assert(sizeof(NavModelResult) == sizeof(NavModelOutputPlan) + sizeof(NavModelOutputDesirePrediction) + sizeof(NavModelOutputFeatures) + sizeof(float));
-
-constexpr int NAV_OUTPUT_SIZE = sizeof(NavModelResult) / sizeof(float);
-constexpr int NAV_NET_OUTPUT_SIZE = NAV_OUTPUT_SIZE - 1;
-
-struct NavModelState {
-  RunModel *m;
-  uint8_t net_input_buf[NAV_INPUT_SIZE];
-  float output[NAV_OUTPUT_SIZE];
-};
-
-void navmodel_init(NavModelState* s);
-NavModelResult* navmodel_eval_frame(NavModelState* s, VisionBuf* buf);
-void navmodel_publish(PubMaster &pm, VisionIpcBufExtra &extra, const NavModelResult &model_res, float execution_time, bool route_valid);
-void navmodel_free(NavModelState* s);

selfdrive/modeld/navmodeld

@@ -1,12 +0,0 @@
-#!/bin/sh
-
-DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
-cd $DIR
-
-if [ -f /TICI ]; then
-  export LD_LIBRARY_PATH="/usr/lib/aarch64-linux-gnu:/data/pythonpath/third_party/snpe/larch64:$LD_LIBRARY_PATH"
-  export ADSP_LIBRARY_PATH="/data/pythonpath/third_party/snpe/dsp/"
-else
-  export LD_LIBRARY_PATH="$DIR/../../third_party/snpe/x86_64-linux-clang:$DIR/../../openpilot/third_party/snpe/x86_64:$LD_LIBRARY_PATH"
-fi
-exec ./_navmodeld

selfdrive/modeld/navmodeld.py

@@ -0,0 +1,118 @@
+#!/usr/bin/env python3
+import gc
+import math
+import time
+import ctypes
+import numpy as np
+from pathlib import Path
+from typing import Tuple, Dict
+
+from cereal import messaging
+from cereal.messaging import PubMaster, SubMaster
+from cereal.visionipc import VisionIpcClient, VisionStreamType
+from openpilot.system.swaglog import cloudlog
+from openpilot.common.params import Params
+from openpilot.common.realtime import set_realtime_priority
+from openpilot.selfdrive.modeld.constants import IDX_N
+from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime
+
+NAV_INPUT_SIZE = 256*256
+NAV_FEATURE_LEN = 256
+NAV_DESIRE_LEN = 32
+NAV_OUTPUT_SIZE = 2*2*IDX_N + NAV_DESIRE_LEN + NAV_FEATURE_LEN
+MODEL_PATHS = {
+  ModelRunner.SNPE: Path(__file__).parent / 'models/navmodel_q.dlc',
+  ModelRunner.ONNX: Path(__file__).parent / 'models/navmodel.onnx'}
+
+class NavModelOutputXY(ctypes.Structure):
+  _fields_ = [
+    ("x", ctypes.c_float),
+    ("y", ctypes.c_float)]
+
+class NavModelOutputPlan(ctypes.Structure):
+  _fields_ = [
+    ("mean", NavModelOutputXY*IDX_N),
+    ("std", NavModelOutputXY*IDX_N)]
+
+class NavModelResult(ctypes.Structure):
+  _fields_ = [
+    ("plan", NavModelOutputPlan),
+    ("desire_pred", ctypes.c_float*NAV_DESIRE_LEN),
+    ("features", ctypes.c_float*NAV_FEATURE_LEN)]
+
+class ModelState:
+  inputs: Dict[str, np.ndarray]
+  output: np.ndarray
+  model: ModelRunner
+
+  def __init__(self):
+    assert ctypes.sizeof(NavModelResult) == NAV_OUTPUT_SIZE * ctypes.sizeof(ctypes.c_float)
+    self.output = np.zeros(NAV_OUTPUT_SIZE, dtype=np.float32)
+    self.inputs = {'input_img': np.zeros(NAV_INPUT_SIZE, dtype=np.uint8)}
+    self.model = ModelRunner(MODEL_PATHS, self.output, Runtime.DSP, True, None)
+    self.model.addInput("input_img", None)
+
+  def run(self, buf:np.ndarray) -> Tuple[np.ndarray, float]:
+    self.inputs['input_img'][:] = buf
+
+    t1 = time.perf_counter()
+    self.model.setInputBuffer("input_img", self.inputs['input_img'].view(np.float32))
+    self.model.execute()
+    t2 = time.perf_counter()
+    return self.output, t2 - t1
+
+def get_navmodel_packet(model_output: np.ndarray, valid: bool, frame_id: int, location_ts: int, execution_time: float, dsp_execution_time: float):
+  model_result = ctypes.cast(model_output.ctypes.data, ctypes.POINTER(NavModelResult)).contents
+  msg = messaging.new_message('navModel')
+  msg.valid = valid
+  msg.navModel.frameId = frame_id
+  msg.navModel.locationMonoTime = location_ts
+  msg.navModel.modelExecutionTime = execution_time
+  msg.navModel.dspExecutionTime = dsp_execution_time
+  msg.navModel.features = model_result.features[:]
+  msg.navModel.desirePrediction = model_result.desire_pred[:]
+  msg.navModel.position.x = [p.x for p in model_result.plan.mean]
+  msg.navModel.position.y = [p.y for p in model_result.plan.mean]
+  msg.navModel.position.xStd = [math.exp(p.x) for p in model_result.plan.std]
+  msg.navModel.position.yStd = [math.exp(p.y) for p in model_result.plan.std]
+  return msg
+
+
+def main():
+  gc.disable()
+  set_realtime_priority(1)
+
+  # there exists a race condition when two processes try to create a
+  # SNPE model runner at the same time, wait for dmonitoringmodeld to finish
+  cloudlog.warning("waiting for dmonitoringmodeld to initialize")
+  if not Params().get_bool("DmModelInitialized", True):
+    return
+
+  model = ModelState()
+  cloudlog.warning("models loaded, navmodeld starting")
+
+  vipc_client = VisionIpcClient("navd", VisionStreamType.VISION_STREAM_MAP, True)
+  while not vipc_client.connect(False):
+    time.sleep(0.1)
+  assert vipc_client.is_connected()
+  cloudlog.warning(f"connected with buffer size: {vipc_client.buffer_len}")
+
+  sm = SubMaster(["navInstruction"])
+  pm = PubMaster(["navModel"])
+
+  while True:
+    buf = vipc_client.recv()
+    if buf is None:
+      continue
+
+    sm.update(0)
+    t1 = time.perf_counter()
+    model_output, dsp_execution_time = model.run(buf.data[:buf.uv_offset])
+    t2 = time.perf_counter()
+
+    valid = vipc_client.valid and sm.valid["navInstruction"]
+    pm.send("navModel", get_navmodel_packet(model_output, valid, vipc_client.frame_id, vipc_client.timestamp_sof, t2 - t1, dsp_execution_time))
+
+
+if __name__ == "__main__":
+  main()

selfdrive/modeld/runners/__init__.py

@@ -0,0 +1,27 @@
+import os
+from openpilot.system.hardware import TICI
+from openpilot.selfdrive.modeld.runners.runmodel_pyx import RunModel, Runtime
+assert Runtime
+
+USE_THNEED = int(os.getenv('USE_THNEED', str(int(TICI))))
+USE_SNPE = int(os.getenv('USE_SNPE', str(int(TICI))))
+
+class ModelRunner(RunModel):
+  THNEED = 'THNEED'
+  SNPE = 'SNPE'
+  ONNX = 'ONNX'
+
+  def __new__(cls, paths, *args, **kwargs):
+    if ModelRunner.THNEED in paths and USE_THNEED:
+      from openpilot.selfdrive.modeld.runners.thneedmodel_pyx import ThneedModel as Runner
+      runner_type = ModelRunner.THNEED
+    elif ModelRunner.SNPE in paths and USE_SNPE:
+      from openpilot.selfdrive.modeld.runners.snpemodel_pyx import SNPEModel as Runner
+      runner_type = ModelRunner.SNPE
+    elif ModelRunner.ONNX in paths:
+      from openpilot.selfdrive.modeld.runners.onnxmodel import ONNXModel as Runner
+      runner_type = ModelRunner.ONNX
+    else:
+      raise Exception("Couldn't select a model runner, make sure to pass at least one valid model path")
+
+    return Runner(str(paths[runner_type]), *args, **kwargs)

selfdrive/modeld/runners/onnxmodel.py

@@ -0,0 +1,93 @@
+import onnx
+import itertools
+import os
+import sys
+import numpy as np
+from typing import Tuple, Dict, Union, Any
+
+from openpilot.selfdrive.modeld.runners.runmodel_pyx import RunModel
+
+ORT_TYPES_TO_NP_TYPES = {'tensor(float16)': np.float16, 'tensor(float)': np.float32, 'tensor(uint8)': np.uint8}
+
+def attributeproto_fp16_to_fp32(attr):
+  float32_list = np.frombuffer(attr.raw_data, dtype=np.float16)
+  attr.data_type = 1
+  attr.raw_data = float32_list.astype(np.float32).tobytes()
+
+def convert_fp16_to_fp32(path):
+  model = onnx.load(path)
+  for i in model.graph.initializer:
+    if i.data_type == 10:
+      attributeproto_fp16_to_fp32(i)
+  for i in itertools.chain(model.graph.input, model.graph.output):
+    if i.type.tensor_type.elem_type == 10:
+      i.type.tensor_type.elem_type = 1
+  for i in model.graph.node:
+    for a in i.attribute:
+      if hasattr(a, 't'):
+        if a.t.data_type == 10:
+          attributeproto_fp16_to_fp32(a.t)
+  return model.SerializeToString()
+
+def create_ort_session(path, fp16_to_fp32):
+  os.environ["OMP_NUM_THREADS"] = "4"
+  os.environ["OMP_WAIT_POLICY"] = "PASSIVE"
+
+  import onnxruntime as ort
+  print("Onnx available providers: ", ort.get_available_providers(), file=sys.stderr)
+  options = ort.SessionOptions()
+  options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_DISABLE_ALL
+
+  provider: Union[str, Tuple[str, Dict[Any, Any]]]
+  if 'OpenVINOExecutionProvider' in ort.get_available_providers() and 'ONNXCPU' not in os.environ:
+    provider = 'OpenVINOExecutionProvider'
+  elif 'CUDAExecutionProvider' in ort.get_available_providers() and 'ONNXCPU' not in os.environ:
+    options.intra_op_num_threads = 2
+    provider = ('CUDAExecutionProvider', {'cudnn_conv_algo_search': 'DEFAULT'})
+  else:
+    options.intra_op_num_threads = 2
+    options.execution_mode = ort.ExecutionMode.ORT_SEQUENTIAL
+    options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_ENABLE_ALL
+    provider = 'CPUExecutionProvider'
+
+  model_data = convert_fp16_to_fp32(path) if fp16_to_fp32 else path
+  print("Onnx selected provider: ", [provider], file=sys.stderr)
+  ort_session = ort.InferenceSession(model_data, options, providers=[provider])
+  print("Onnx using ", ort_session.get_providers(), file=sys.stderr)
+  return ort_session
+
+
+class ONNXModel(RunModel):
+  def __init__(self, path, output, runtime, use_tf8, cl_context):
+    self.inputs = {}
+    self.output = output
+    self.use_tf8 = use_tf8
+
+    self.session = create_ort_session(path, fp16_to_fp32=True)
+    self.input_names = [x.name for x in self.session.get_inputs()]
+    self.input_shapes = {x.name: [1, *x.shape[1:]] for x in self.session.get_inputs()}
+    self.input_dtypes = {x.name: ORT_TYPES_TO_NP_TYPES[x.type] for x in self.session.get_inputs()}
+
+    # run once to initialize CUDA provider
+    if "CUDAExecutionProvider" in self.session.get_providers():
+      self.session.run(None, {k: np.zeros(self.input_shapes[k], dtype=self.input_dtypes[k]) for k in self.input_names})
+    print("ready to run onnx model", self.input_shapes, file=sys.stderr)
+
+  def addInput(self, name, buffer):
+    assert name in self.input_names
+    self.inputs[name] = buffer
+
+  def setInputBuffer(self, name, buffer):
+    assert name in self.inputs
+    self.inputs[name] = buffer
+
+  def getCLBuffer(self, name):
+    return None
+
+  def execute(self):
+    inputs = {k: (v.view(np.uint8) / 255. if self.use_tf8 and k == 'input_img' else v) for k,v in self.inputs.items()}
+    inputs = {k: v.reshape(self.input_shapes[k]).astype(self.input_dtypes[k]) for k,v in inputs.items()}
+    outputs = self.session.run(None, inputs)
+    assert len(outputs) == 1, "Only single model outputs are supported"
+    self.output[:] = outputs[0]
+    return self.output

selfdrive/modeld/runners/run.h

@@ -1,10 +1,4 @@
 #pragma once
 
-#include "runmodel.h"
-#include "snpemodel.h"
-
-#if defined(USE_THNEED)
-#include "thneedmodel.h"
-#elif defined(USE_ONNX_MODEL)
-#include "onnxmodel.h"
-#endif
+#include "selfdrive/modeld/runners/runmodel.h"
+#include "selfdrive/modeld/runners/snpemodel.h"

selfdrive/modeld/runners/runmodel.h

@@ -8,6 +8,10 @@
 #include "common/clutil.h"
 #include "common/swaglog.h"
 
+#define USE_CPU_RUNTIME 0
+#define USE_GPU_RUNTIME 1
+#define USE_DSP_RUNTIME 2
+
 struct ModelInput {
   const std::string name;
   float *buffer;

selfdrive/modeld/runners/runmodel.pxd

@@ -0,0 +1,14 @@
+# distutils: language = c++
+
+from libcpp.string cimport string
+
+cdef extern from "selfdrive/modeld/runners/runmodel.h":
+  cdef int USE_CPU_RUNTIME
+  cdef int USE_GPU_RUNTIME
+  cdef int USE_DSP_RUNTIME
+
+  cdef cppclass RunModel:
+    void addInput(string, float*, int)
+    void setInputBuffer(string, float*, int)
+    void * getCLBuffer(string)
+    void execute()

selfdrive/modeld/runners/runmodel_pyx.pxd

@@ -0,0 +1,6 @@
+# distutils: language = c++
+
+from .runmodel cimport RunModel as cppRunModel
+
+cdef class RunModel:
+  cdef cppRunModel * model

selfdrive/modeld/runners/runmodel_pyx.pyx

@@ -0,0 +1,38 @@
+# distutils: language = c++
+# cython: c_string_encoding=ascii
+
+from libcpp.string cimport string
+from libc.string cimport memcpy
+
+from .runmodel cimport USE_CPU_RUNTIME, USE_GPU_RUNTIME, USE_DSP_RUNTIME
+from selfdrive.modeld.models.commonmodel_pyx cimport CLMem
+
+class Runtime:
+  CPU = USE_CPU_RUNTIME
+  GPU = USE_GPU_RUNTIME
+  DSP = USE_DSP_RUNTIME
+
+cdef class RunModel:
+  def __dealloc__(self):
+    del self.model
+
+  def addInput(self, string name, float[:] buffer):
+    if buffer is not None:
+      self.model.addInput(name, &buffer[0], len(buffer))
+    else:
+      self.model.addInput(name, NULL, 0)
+
+  def setInputBuffer(self, string name, float[:] buffer):
+    if buffer is not None:
+      self.model.setInputBuffer(name, &buffer[0], len(buffer))
+    else:
+      self.model.setInputBuffer(name, NULL, 0)
+
+  def getCLBuffer(self, string name):
+    cdef void * cl_buf = self.model.getCLBuffer(name)
+    if not cl_buf:
+      return None
+    return CLMem.create(cl_buf)
+
+  def execute(self):
+    self.model.execute()

selfdrive/modeld/runners/snpemodel.h

@@ -1,6 +1,10 @@
 #pragma once
 #pragma clang diagnostic ignored "-Wdeprecated-declarations"
 
+#include <memory>
+#include <string>
+#include <utility>
+
 #include <DlContainer/IDlContainer.hpp>
 #include <DlSystem/DlError.hpp>
 #include <DlSystem/ITensor.hpp>
@@ -13,14 +17,6 @@
 
 #include "selfdrive/modeld/runners/runmodel.h"
 
-#define USE_CPU_RUNTIME 0
-#define USE_GPU_RUNTIME 1
-#define USE_DSP_RUNTIME 2
-
-#ifdef USE_THNEED
-#include "selfdrive/modeld/thneed/thneed.h"
-#endif
-
 struct SNPEModelInput : public ModelInput {
   std::unique_ptr<zdl::DlSystem::IUserBuffer> snpe_buffer;
 
@@ -37,11 +33,6 @@ public:
   void addInput(const std::string name, float *buffer, int size);
   void execute();
 
-#ifdef USE_THNEED
-  std::unique_ptr<Thneed> thneed;
-  bool thneed_recorded = false;
-#endif
-
 private:
   std::string model_data;
 

selfdrive/modeld/runners/snpemodel.pxd

@@ -0,0 +1,9 @@
+# distutils: language = c++
+
+from libcpp.string cimport string
+
+from cereal.visionipc.visionipc cimport cl_context
+
+cdef extern from "selfdrive/modeld/runners/snpemodel.h":
+  cdef cppclass SNPEModel:
+    SNPEModel(string, float*, size_t, int, bool, cl_context)

selfdrive/modeld/runners/snpemodel_pyx.pyx

@@ -0,0 +1,17 @@
+# distutils: language = c++
+# cython: c_string_encoding=ascii
+
+import os
+from libcpp cimport bool
+from libcpp.string cimport string
+
+from .snpemodel cimport SNPEModel as cppSNPEModel
+from selfdrive.modeld.models.commonmodel_pyx cimport CLContext
+from selfdrive.modeld.runners.runmodel_pyx cimport RunModel
+from selfdrive.modeld.runners.runmodel cimport RunModel as cppRunModel
+
+os.environ['ADSP_LIBRARY_PATH'] = "/data/pythonpath/third_party/snpe/dsp/"
+
+cdef class SNPEModel(RunModel):
+  def __cinit__(self, string path, float[:] output, int runtime, bool use_tf8, CLContext context):
+    self.model = <cppRunModel *> new cppSNPEModel(path, &output[0], len(output), runtime, use_tf8, context.context)

selfdrive/modeld/runners/thneedmodel.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#include <string>
+
 #include "selfdrive/modeld/runners/runmodel.h"
 #include "selfdrive/modeld/thneed/thneed.h"
 

selfdrive/modeld/runners/thneedmodel.pxd

@@ -0,0 +1,9 @@
+# distutils: language = c++
+
+from libcpp.string cimport string
+
+from cereal.visionipc.visionipc cimport cl_context
+
+cdef extern from "selfdrive/modeld/runners/thneedmodel.h":
+  cdef cppclass ThneedModel:
+    ThneedModel(string, float*, size_t, int, bool, cl_context)

selfdrive/modeld/runners/thneedmodel_pyx.pyx

@@ -0,0 +1,14 @@
+# distutils: language = c++
+# cython: c_string_encoding=ascii
+
+from libcpp cimport bool
+from libcpp.string cimport string
+
+from .thneedmodel cimport ThneedModel as cppThneedModel
+from selfdrive.modeld.models.commonmodel_pyx cimport CLContext
+from selfdrive.modeld.runners.runmodel_pyx cimport RunModel
+from selfdrive.modeld.runners.runmodel cimport RunModel as cppRunModel
+
+cdef class ThneedModel(RunModel):
+  def __cinit__(self, string path, float[:] output, int runtime, bool use_tf8, CLContext context):
+    self.model = <cppRunModel *> new cppThneedModel(path, &output[0], len(output), runtime, use_tf8, context.context)

selfdrive/modeld/thneed/thneed.h

@@ -12,7 +12,7 @@
 
 #include <CL/cl.h>
 
-#include "msm_kgsl.h"
+#include "third_party/linux/include/msm_kgsl.h"
 
 using namespace std;