Update251203 (#233)

selfdrive/modeld/SConscript

@@ -1,11 +1,11 @@
 import os
 import glob
 
-Import('env', 'envCython', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc', 'transformations')
+Import('env', 'envCython', 'arch', 'cereal', 'messaging', 'common', 'visionipc', 'transformations')
 lenv = env.Clone()
 lenvCython = envCython.Clone()
 
-libs = [cereal, messaging, visionipc, gpucommon, common, 'capnp', 'kj', 'pthread']
+libs = [cereal, messaging, visionipc, common, 'capnp', 'kj', 'pthread']
 frameworks = []
 
 common_src = [
@@ -32,7 +32,7 @@ lenvCython.Program('models/commonmodel_pyx.so', 'models/commonmodel_pyx.pyx', LI
 tinygrad_files = ["#"+x for x in glob.glob(env.Dir("#tinygrad_repo").relpath + "/**", recursive=True, root_dir=env.Dir("#").abspath) if 'pycache' not in x]
 
 # Get model metadata
-for model_name in ['driving_vision', 'driving_policy']:
+for model_name in ['driving_vision', 'driving_policy', 'dmonitoring_model']:
   fn = File(f"models/{model_name}").abspath
   script_files = [File(Dir("#selfdrive/modeld").File("get_model_metadata.py").abspath)]
   cmd = f'python3 {Dir("#selfdrive/modeld").abspath}/get_model_metadata.py {fn}.onnx'
@@ -50,9 +50,9 @@ def tg_compile(flags, model_name):
 # Compile small models
 for model_name in ['driving_vision', 'driving_policy', 'dmonitoring_model']:
   flags = {
-    'larch64': 'DEV=QCOM',
-    'Darwin': 'DEV=CPU IMAGE=0',
-  }.get(arch, 'DEV=LLVM IMAGE=0')
+    'larch64': 'DEV=QCOM FLOAT16=1 NOLOCALS=1 IMAGE=2 JIT_BATCH_SIZE=0',
+    'Darwin': f'DEV=CPU HOME={os.path.expanduser("~")}', # tinygrad calls brew which needs a $HOME in the env
+  }.get(arch, 'DEV=CPU CPU_LLVM=1')
   tg_compile(flags, model_name)
 
 # Compile BIG model if USB GPU is available

selfdrive/modeld/dmonitoringmodeld.py

@@ -1,14 +1,9 @@
 #!/usr/bin/env python3
 import os
 from openpilot.system.hardware import TICI
+os.environ['DEV'] = 'QCOM' if TICI else 'CPU'
 from tinygrad.tensor import Tensor
 from tinygrad.dtype import dtypes
-if TICI:
-  from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address
-  os.environ['QCOM'] = '1'
-else:
-  os.environ['LLVM'] = '1'
-import math
 import time
 import pickle
 import ctypes
@@ -21,48 +16,16 @@ from cereal.messaging import PubMaster, SubMaster
 from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
 from openpilot.common.swaglog import cloudlog
 from openpilot.common.realtime import config_realtime_process
-from openpilot.common.transformations.model import dmonitoringmodel_intrinsics, DM_INPUT_SIZE
+from openpilot.common.transformations.model import dmonitoringmodel_intrinsics
 from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
 from openpilot.selfdrive.modeld.models.commonmodel_pyx import CLContext, MonitoringModelFrame
-from openpilot.selfdrive.modeld.parse_model_outputs import sigmoid
-from openpilot.system import sentry
-
-MODEL_WIDTH, MODEL_HEIGHT = DM_INPUT_SIZE
-CALIB_LEN = 3
-FEATURE_LEN = 512
-OUTPUT_SIZE = 84 + FEATURE_LEN
+from openpilot.selfdrive.modeld.parse_model_outputs import sigmoid, safe_exp
+from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address
 
 PROCESS_NAME = "selfdrive.modeld.dmonitoringmodeld"
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
 MODEL_PKL_PATH = Path(__file__).parent / 'models/dmonitoring_model_tinygrad.pkl'
-
-
-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),
-    ("features", ctypes.c_float*FEATURE_LEN)]
+METADATA_PATH = Path(__file__).parent / 'models/dmonitoring_model_metadata.pkl'
 
 
 class ModelState:
@@ -70,11 +33,14 @@ class ModelState:
   output: np.ndarray
 
   def __init__(self, cl_ctx):
-    assert ctypes.sizeof(DMonitoringModelResult) == OUTPUT_SIZE * ctypes.sizeof(ctypes.c_float)
+    with open(METADATA_PATH, 'rb') as f:
+      model_metadata = pickle.load(f)
+      self.input_shapes = model_metadata['input_shapes']
+      self.output_slices = model_metadata['output_slices']
 
     self.frame = MonitoringModelFrame(cl_ctx)
     self.numpy_inputs = {
-      'calib': np.zeros((1, CALIB_LEN), dtype=np.float32),
+      'calib': np.zeros(self.input_shapes['calib'], dtype=np.float32),
     }
 
     self.tensor_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()}
@@ -90,45 +56,53 @@ class ModelState:
     if TICI:
       # The imgs tensors are backed by opencl memory, only need init once
       if 'input_img' not in self.tensor_inputs:
-        self.tensor_inputs['input_img'] = qcom_tensor_from_opencl_address(input_img_cl.mem_address, (1, MODEL_WIDTH*MODEL_HEIGHT), dtype=dtypes.uint8)
+        self.tensor_inputs['input_img'] = qcom_tensor_from_opencl_address(input_img_cl.mem_address, self.input_shapes['input_img'], dtype=dtypes.uint8)
     else:
-      self.tensor_inputs['input_img'] = Tensor(self.frame.buffer_from_cl(input_img_cl).reshape((1, MODEL_WIDTH*MODEL_HEIGHT)), dtype=dtypes.uint8).realize()
+      self.tensor_inputs['input_img'] = Tensor(self.frame.buffer_from_cl(input_img_cl).reshape(self.input_shapes['input_img']), dtype=dtypes.uint8).realize()
 
 
-    output = self.model_run(**self.tensor_inputs).numpy().flatten()
+    output = self.model_run(**self.tensor_inputs).contiguous().realize().uop.base.buffer.numpy()
 
     t2 = time.perf_counter()
     return output, t2 - t1
 
+def slice_outputs(model_outputs, output_slices):
+  return  {k: model_outputs[np.newaxis, v] for k,v in output_slices.items()}
 
-def fill_driver_state(msg, ds_result: DriverStateResult):
-  msg.faceOrientation = list(ds_result.face_orientation)
-  msg.faceOrientationStd = [math.exp(x) for x in ds_result.face_orientation_std]
-  msg.facePosition = list(ds_result.face_position[:2])
-  msg.facePositionStd = [math.exp(x) for x in ds_result.face_position_std[:2]]
-  msg.faceProb = float(sigmoid(ds_result.face_prob))
-  msg.leftEyeProb = float(sigmoid(ds_result.left_eye_prob))
-  msg.rightEyeProb = float(sigmoid(ds_result.right_eye_prob))
-  msg.leftBlinkProb = float(sigmoid(ds_result.left_blink_prob))
-  msg.rightBlinkProb = float(sigmoid(ds_result.right_blink_prob))
-  msg.sunglassesProb = float(sigmoid(ds_result.sunglasses_prob))
-  msg.occludedProb = float(sigmoid(ds_result.occluded_prob))
-  msg.readyProb = [float(sigmoid(x)) for x in ds_result.ready_prob]
-  msg.notReadyProb = [float(sigmoid(x)) for x in ds_result.not_ready_prob]
+def parse_model_output(model_output):
+  parsed = {}
+  parsed['wheel_on_right'] = sigmoid(model_output['wheel_on_right'])
+  for ds_suffix in ['lhd', 'rhd']:
+    face_descs = model_output[f'face_descs_{ds_suffix}']
+    parsed[f'face_descs_{ds_suffix}'] = face_descs[:, :-6]
+    parsed[f'face_descs_{ds_suffix}_std'] = safe_exp(face_descs[:, -6:])
+    for key in ['face_prob', 'left_eye_prob', 'right_eye_prob','left_blink_prob', 'right_blink_prob', 'sunglasses_prob', 'using_phone_prob']:
+      parsed[f'{key}_{ds_suffix}'] = sigmoid(model_output[f'{key}_{ds_suffix}'])
+  return parsed
 
+def fill_driver_data(msg, model_output, ds_suffix):
+  msg.faceOrientation = model_output[f'face_descs_{ds_suffix}'][0, :3].tolist()
+  msg.faceOrientationStd = model_output[f'face_descs_{ds_suffix}_std'][0, :3].tolist()
+  msg.facePosition = model_output[f'face_descs_{ds_suffix}'][0, 3:5].tolist()
+  msg.facePositionStd = model_output[f'face_descs_{ds_suffix}_std'][0, 3:5].tolist()
+  msg.faceProb = model_output[f'face_prob_{ds_suffix}'][0, 0].item()
+  msg.leftEyeProb = model_output[f'left_eye_prob_{ds_suffix}'][0, 0].item()
+  msg.rightEyeProb = model_output[f'right_eye_prob_{ds_suffix}'][0, 0].item()
+  msg.leftBlinkProb = model_output[f'left_blink_prob_{ds_suffix}'][0, 0].item()
+  msg.rightBlinkProb = model_output[f'right_blink_prob_{ds_suffix}'][0, 0].item()
+  msg.sunglassesProb = model_output[f'sunglasses_prob_{ds_suffix}'][0, 0].item()
+  msg.phoneProb = model_output[f'using_phone_prob_{ds_suffix}'][0, 0].item()
 
-def get_driverstate_packet(model_output: np.ndarray, frame_id: int, location_ts: int, execution_time: float, gpu_execution_time: float):
-  model_result = ctypes.cast(model_output.ctypes.data, ctypes.POINTER(DMonitoringModelResult)).contents
+def get_driverstate_packet(model_output, frame_id: int, location_ts: int, exec_time: float, gpu_exec_time: float):
   msg = messaging.new_message('driverStateV2', valid=True)
   ds = msg.driverStateV2
   ds.frameId = frame_id
-  ds.modelExecutionTime = execution_time
-  ds.gpuExecutionTime = gpu_execution_time
-  ds.poorVisionProb = float(sigmoid(model_result.poor_vision_prob))
-  ds.wheelOnRightProb = float(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)
+  ds.modelExecutionTime = exec_time
+  ds.gpuExecutionTime = gpu_exec_time
+  ds.rawPredictions = model_output['raw_pred']
+  ds.wheelOnRightProb = model_output['wheel_on_right'][0, 0].item()
+  fill_driver_data(ds.leftDriverData, model_output, 'lhd')
+  fill_driver_data(ds.rightDriverData, model_output, 'rhd')
   return msg
 
 
@@ -153,7 +127,7 @@ def main():
   sm = SubMaster(["liveCalibration"])
   pm = PubMaster(["driverStateV2"])
 
-  calib = np.zeros(CALIB_LEN, dtype=np.float32)
+  calib = np.zeros(model.numpy_inputs['calib'].size, dtype=np.float32)
   model_transform = None
 
   while True:
@@ -172,8 +146,12 @@ def main():
     t1 = time.perf_counter()
     model_output, gpu_execution_time = model.run(buf, calib, model_transform)
     t2 = time.perf_counter()
-
-    pm.send("driverStateV2", get_driverstate_packet(model_output, vipc_client.frame_id, vipc_client.timestamp_sof, t2 - t1, gpu_execution_time))
+    raw_pred = model_output.tobytes() if SEND_RAW_PRED else b''
+    model_output = slice_outputs(model_output, model.output_slices)
+    model_output = parse_model_output(model_output)
+    model_output['raw_pred'] = raw_pred
+    msg = get_driverstate_packet(model_output, vipc_client.frame_id, vipc_client.timestamp_sof, t2 - t1, gpu_execution_time)
+    pm.send("driverStateV2", msg)
 
 
 if __name__ == "__main__":

selfdrive/modeld/fill_model_msg.py

@@ -1,6 +1,7 @@
 import os
 import capnp
 import numpy as np
+import math
 from cereal import log
 from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta
 
@@ -102,21 +103,42 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
   LINE_T_IDXS = [np.nan] * ModelConstants.IDX_N
   LINE_T_IDXS[0] = 0.0
   plan_x = net_output_data['plan'][0, :, Plan.POSITION][:, 0].tolist()
+  Tmax = ModelConstants.T_IDXS[ModelConstants.IDX_N - 1]
   for xidx in range(1, ModelConstants.IDX_N):
     tidx = 0
     # increment tidx until we find an element that's further away than the current xidx
     while tidx < ModelConstants.IDX_N - 1 and plan_x[tidx + 1] < ModelConstants.X_IDXS[xidx]:
       tidx += 1
     if tidx == ModelConstants.IDX_N - 1:
-      # if the Plan doesn't extend far enough, set plan_t to the max value (10s), then break
-      LINE_T_IDXS[xidx] = ModelConstants.T_IDXS[ModelConstants.IDX_N - 1]
-      break
+      for k in range(xidx, ModelConstants.IDX_N):
+        LINE_T_IDXS[k] = Tmax
+      break  
     # interpolate to find `t` for the current xidx
     current_x_val = plan_x[tidx]
     next_x_val = plan_x[tidx + 1]
-    p = (ModelConstants.X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val) if abs(
-      next_x_val - current_x_val) > 1e-9 else float('nan')
-    LINE_T_IDXS[xidx] = p * ModelConstants.T_IDXS[tidx + 1] + (1 - p) * ModelConstants.T_IDXS[tidx]
+
+    dx = next_x_val - current_x_val
+    if dx <= 1e-9:
+      LINE_T_IDXS[xidx] = ModelConstants.T_IDXS[tidx]
+    else:
+      p = (ModelConstants.X_IDXS[xidx] - current_x_val) / dx
+      if p < 0.0: p = 0.0                                   
+      elif p > 1.0: p = 1.0                                 
+      LINE_T_IDXS[xidx] = p * ModelConstants.T_IDXS[tidx + 1] + (1.0 - p) * ModelConstants.T_IDXS[tidx]
+
+    #p = (ModelConstants.X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val) if abs(
+    #  next_x_val - current_x_val) > 1e-9 else float('nan')
+    #LINE_T_IDXS[xidx] = p * ModelConstants.T_IDXS[tidx + 1] + (1 - p) * ModelConstants.T_IDXS[tidx]
+
+  LINE_T_IDXS = [float(Tmax if math.isnan(float(v)) else float(v)) for v in LINE_T_IDXS]
+
+  # 비내림(monotonic non-decreasing) 보정 (순수 파이썬, numpy 불사용)
+  running = LINE_T_IDXS[0]
+  for i in range(1, len(LINE_T_IDXS)):
+      if LINE_T_IDXS[i] < running:
+          LINE_T_IDXS[i] = running
+      else:
+          running = LINE_T_IDXS[i]
 
   # lane lines
   modelV2.init('laneLines', 4)

selfdrive/modeld/models/README.md

@@ -62,6 +62,5 @@ Refer to **slice_outputs** and **parse_vision_outputs/parse_policy_outputs** in
     * (deprecated) distracted probabilities: 2
     * using phone probability: 1
     * distracted probability: 1
-  * common outputs 2
-    * poor camera vision probability: 1
+  * common outputs 1
     * left hand drive probability: 1

selfdrive/modeld/models/dmonitoring_model.current

@@ -1,2 +0,0 @@
-fa69be01-b430-4504-9d72-7dcb058eb6dd
-d9fb22d1c4fa3ca3d201dbc8edf1d0f0918e53e6