mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-07-23 23:02:04 +08:00
Compare commits
2 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 55bdba891b | |||
| 74bbf786e7 |
@@ -3,8 +3,6 @@
|
||||
# to move existing files into LFS:
|
||||
# git add --renormalize .
|
||||
*.onnx filter=lfs diff=lfs merge=lfs -text
|
||||
*.thneed filter=lfs diff=lfs merge=lfs -text
|
||||
*.pkl filter=lfs diff=lfs merge=lfs -text
|
||||
*.svg filter=lfs diff=lfs merge=lfs -text
|
||||
*.png filter=lfs diff=lfs merge=lfs -text
|
||||
*.gif filter=lfs diff=lfs merge=lfs -text
|
||||
|
||||
@@ -74,7 +74,6 @@ comma*.sh
|
||||
selfdrive/modeld/thneed/compile
|
||||
selfdrive/modeld/models/*.thneed
|
||||
selfdrive/modeld/models/*.pkl
|
||||
sunnypilot/modeld/thneed/compile
|
||||
|
||||
*.bz2
|
||||
*.zst
|
||||
|
||||
+1
-1
@@ -15,4 +15,4 @@
|
||||
url = https://github.com/commaai/teleoprtc
|
||||
[submodule "tinygrad"]
|
||||
path = tinygrad_repo
|
||||
url = https://github.com/commaai/tinygrad.git
|
||||
url = https://github.com/tinygrad/tinygrad.git
|
||||
@@ -396,8 +396,6 @@ SConscript(['third_party/SConscript'])
|
||||
|
||||
SConscript(['selfdrive/SConscript'])
|
||||
|
||||
SConscript(['sunnypilot/SConscript'])
|
||||
|
||||
if Dir('#tools/cabana/').exists() and GetOption('extras'):
|
||||
SConscript(['tools/replay/SConscript'])
|
||||
if arch != "larch64":
|
||||
|
||||
@@ -64,11 +64,6 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
|
||||
progress @1 :Float32;
|
||||
eta @2 :UInt32;
|
||||
}
|
||||
|
||||
enum Runner {
|
||||
snpe @0;
|
||||
tinygrad @1;
|
||||
}
|
||||
|
||||
struct ModelBundle {
|
||||
index @0 :UInt32;
|
||||
|
||||
+1
-1
@@ -212,7 +212,7 @@ std::unordered_map<std::string, uint32_t> keys = {
|
||||
|
||||
// Model Manager params
|
||||
{"ModelManager_ActiveBundle", PERSISTENT},
|
||||
{"ModelManager_DownloadIndex", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
|
||||
{"ModelManager_DownloadIndex", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION | CLEAR_ON_ONROAD_TRANSITION},
|
||||
{"ModelManager_LastSyncTime", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
|
||||
{"ModelManager_ModelsCache", PERSISTENT | BACKUP},
|
||||
|
||||
|
||||
+3
-2
@@ -42,7 +42,8 @@ dependencies = [
|
||||
|
||||
# modeld
|
||||
"onnx >= 1.14.0",
|
||||
"onnxruntime >=1.16.3",
|
||||
"onnxruntime >=1.16.3; platform_system == 'Linux' and platform_machine == 'aarch64'",
|
||||
"onnxruntime-gpu >=1.16.3; platform_system == 'Linux' and platform_machine == 'x86_64'",
|
||||
|
||||
# logging
|
||||
"pyzmq",
|
||||
@@ -137,7 +138,7 @@ allow-direct-references = true
|
||||
|
||||
[tool.pytest.ini_options]
|
||||
minversion = "6.0"
|
||||
addopts = "--ignore=openpilot/ --ignore=opendbc/ --ignore=panda/ --ignore=rednose_repo/ --ignore=tinygrad_repo/ --ignore=teleoprtc_repo/ --ignore=msgq/ --ignore=sunnypilot/tinygrad_repo/ -Werror --strict-config --strict-markers --durations=10 -n auto --dist=loadgroup"
|
||||
addopts = "--ignore=openpilot/ --ignore=opendbc/ --ignore=panda/ --ignore=rednose_repo/ --ignore=tinygrad_repo/ --ignore=teleoprtc_repo/ --ignore=msgq/ -Werror --strict-config --strict-markers --durations=10 -n auto --dist=loadgroup"
|
||||
cpp_files = "test_*"
|
||||
cpp_harness = "selfdrive/test/cpp_harness.py"
|
||||
python_files = "test_*.py"
|
||||
|
||||
@@ -91,7 +91,7 @@ whitelist = [
|
||||
"tools/joystick/",
|
||||
"tools/longitudinal_maneuvers/",
|
||||
|
||||
"tinygrad_repo/examples/openpilot/compile3.py",
|
||||
"tinygrad_repo/openpilot/compile2.py",
|
||||
"tinygrad_repo/extra/onnx.py",
|
||||
"tinygrad_repo/extra/onnx_ops.py",
|
||||
"tinygrad_repo/extra/thneed.py",
|
||||
|
||||
+35
-11
@@ -13,6 +13,20 @@ common_src = [
|
||||
"transforms/transform.cc",
|
||||
]
|
||||
|
||||
thneed_src_common = [
|
||||
"thneed/thneed_common.cc",
|
||||
"thneed/serialize.cc",
|
||||
]
|
||||
|
||||
thneed_src_qcom = thneed_src_common + ["thneed/thneed_qcom2.cc"]
|
||||
thneed_src_pc = thneed_src_common + ["thneed/thneed_pc.cc"]
|
||||
thneed_src = thneed_src_qcom if arch == "larch64" else thneed_src_pc
|
||||
|
||||
# SNPE except on Mac and ARM Linux
|
||||
snpe_lib = []
|
||||
if arch != "Darwin" and arch != "aarch64":
|
||||
common_src += ['runners/snpemodel.cc']
|
||||
snpe_lib += ['SNPE']
|
||||
|
||||
# OpenCL is a framework on Mac
|
||||
if arch == "Darwin":
|
||||
@@ -31,24 +45,34 @@ snpe_rpath_pc = f"{Dir('#').abspath}/third_party/snpe/x86_64-linux-clang"
|
||||
snpe_rpath = lenvCython['RPATH'] + [snpe_rpath_qcom if arch == "larch64" else snpe_rpath_pc]
|
||||
|
||||
cython_libs = envCython["LIBS"] + libs
|
||||
snpemodel_lib = lenv.Library('snpemodel', ['runners/snpemodel.cc'])
|
||||
commonmodel_lib = lenv.Library('commonmodel', common_src)
|
||||
|
||||
lenvCython.Program('runners/runmodel_pyx.so', 'runners/runmodel_pyx.pyx', LIBS=cython_libs, FRAMEWORKS=frameworks)
|
||||
lenvCython.Program('runners/snpemodel_pyx.so', 'runners/snpemodel_pyx.pyx', LIBS=[snpemodel_lib, snpe_lib, *cython_libs], FRAMEWORKS=frameworks, RPATH=snpe_rpath)
|
||||
lenvCython.Program('models/commonmodel_pyx.so', 'models/commonmodel_pyx.pyx', LIBS=[commonmodel_lib, *cython_libs], FRAMEWORKS=frameworks)
|
||||
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]
|
||||
|
||||
tinygrad_files = ["#"+x for x in glob.glob(env.Dir("#tinygrad_repo").relpath + "/**", recursive=True, root_dir=env.Dir("#").abspath)]
|
||||
|
||||
# Get model metadata
|
||||
fn = File("models/supercombo").abspath
|
||||
cmd = f'python3 {Dir("#selfdrive/modeld").abspath}/get_model_metadata.py {fn}.onnx'
|
||||
lenv.Command(fn + "_metadata.pkl", [fn + ".onnx"] + tinygrad_files, cmd)
|
||||
|
||||
# Compile tinygrad model
|
||||
pythonpath_string = 'PYTHONPATH="${PYTHONPATH}:' + env.Dir("#tinygrad_repo").abspath + '"'
|
||||
if arch == 'larch64':
|
||||
device_string = 'QCOM=1'
|
||||
else:
|
||||
device_string = 'CLANG=1 IMAGE=0'
|
||||
# Build thneed model
|
||||
if arch == "larch64" or GetOption('pc_thneed'):
|
||||
tinygrad_opts = []
|
||||
if not GetOption('pc_thneed'):
|
||||
# use FLOAT16 on device for speed + don't cache the CL kernels for space
|
||||
tinygrad_opts += ["FLOAT16=1", "PYOPENCL_NO_CACHE=1"]
|
||||
cmd = f"cd {Dir('#').abspath}/tinygrad_repo && " + ' '.join(tinygrad_opts) + f" python3 openpilot/compile2.py {fn}.onnx {fn}.thneed"
|
||||
|
||||
for model_name in ['supercombo', 'dmonitoring_model']:
|
||||
fn = File(f"models/{model_name}").abspath
|
||||
cmd = f'{pythonpath_string} {device_string} python3 {Dir("#tinygrad_repo").abspath}/examples/openpilot/compile3.py {fn}.onnx {fn}_tinygrad.pkl'
|
||||
lenv.Command(fn + "_tinygrad.pkl", [fn + ".onnx"] + tinygrad_files, cmd)
|
||||
lenv.Command(fn + ".thneed", [fn + ".onnx"] + tinygrad_files, cmd)
|
||||
|
||||
fn_dm = File("models/dmonitoring_model").abspath
|
||||
cmd = f"cd {Dir('#').abspath}/tinygrad_repo && " + ' '.join(tinygrad_opts) + f" python3 openpilot/compile2.py {fn_dm}.onnx {fn_dm}.thneed"
|
||||
lenv.Command(fn_dm + ".thneed", [fn_dm + ".onnx"] + tinygrad_files, cmd)
|
||||
|
||||
thneed_lib = env.SharedLibrary('thneed', thneed_src, LIBS=[gpucommon, common, 'OpenCL', 'dl'])
|
||||
thneedmodel_lib = env.Library('thneedmodel', ['runners/thneedmodel.cc'])
|
||||
lenvCython.Program('runners/thneedmodel_pyx.so', 'runners/thneedmodel_pyx.pyx', LIBS=envCython["LIBS"]+[thneedmodel_lib, thneed_lib, gpucommon, common, 'dl', 'OpenCL'])
|
||||
|
||||
@@ -1,4 +1,10 @@
|
||||
#!/usr/bin/env bash
|
||||
|
||||
DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
|
||||
cd "$DIR/../../"
|
||||
|
||||
if [ -f "$DIR/libthneed.so" ]; then
|
||||
export LD_PRELOAD="$DIR/libthneed.so"
|
||||
fi
|
||||
|
||||
exec "$DIR/dmonitoringmodeld.py" "$@"
|
||||
|
||||
@@ -1,17 +1,8 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
from openpilot.system.hardware import TICI
|
||||
if TICI:
|
||||
from tinygrad.tensor import Tensor
|
||||
from tinygrad.dtype import dtypes
|
||||
from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address
|
||||
os.environ['QCOM'] = '1'
|
||||
else:
|
||||
from openpilot.selfdrive.modeld.runners.ort_helpers import make_onnx_cpu_runner
|
||||
import gc
|
||||
import math
|
||||
import time
|
||||
import pickle
|
||||
import ctypes
|
||||
import numpy as np
|
||||
from pathlib import Path
|
||||
@@ -22,20 +13,21 @@ from cereal.messaging import PubMaster, SubMaster
|
||||
from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.common.realtime import set_realtime_priority
|
||||
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.runners import ModelRunner, Runtime
|
||||
from openpilot.selfdrive.modeld.parse_model_outputs import sigmoid
|
||||
|
||||
MODEL_WIDTH, MODEL_HEIGHT = DM_INPUT_SIZE
|
||||
CALIB_LEN = 3
|
||||
FEATURE_LEN = 512
|
||||
OUTPUT_SIZE = 84 + FEATURE_LEN
|
||||
|
||||
PROCESS_NAME = "selfdrive.modeld.dmonitoringmodeld"
|
||||
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
|
||||
MODEL_PATH = Path(__file__).parent / 'models/dmonitoring_model.onnx'
|
||||
MODEL_PKL_PATH = Path(__file__).parent / 'models/dmonitoring_model_tinygrad.pkl'
|
||||
MODEL_PATHS = {
|
||||
ModelRunner.THNEED: Path(__file__).parent / 'models/dmonitoring_model.thneed',
|
||||
ModelRunner.ONNX: Path(__file__).parent / 'models/dmonitoring_model.onnx'}
|
||||
|
||||
class DriverStateResult(ctypes.Structure):
|
||||
_fields_ = [
|
||||
@@ -66,42 +58,29 @@ class DMonitoringModelResult(ctypes.Structure):
|
||||
class ModelState:
|
||||
inputs: dict[str, np.ndarray]
|
||||
output: np.ndarray
|
||||
model: ModelRunner
|
||||
|
||||
def __init__(self, cl_ctx):
|
||||
assert ctypes.sizeof(DMonitoringModelResult) == OUTPUT_SIZE * ctypes.sizeof(ctypes.c_float)
|
||||
|
||||
self.frame = MonitoringModelFrame(cl_ctx)
|
||||
self.numpy_inputs = {
|
||||
'calib': np.zeros((1, CALIB_LEN), dtype=np.float32),
|
||||
}
|
||||
self.output = np.zeros(OUTPUT_SIZE, dtype=np.float32)
|
||||
self.inputs = {
|
||||
'calib': np.zeros(CALIB_LEN, dtype=np.float32)}
|
||||
|
||||
if TICI:
|
||||
self.tensor_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()}
|
||||
with open(MODEL_PKL_PATH, "rb") as f:
|
||||
self.model_run = pickle.load(f)
|
||||
else:
|
||||
self.onnx_cpu_runner = make_onnx_cpu_runner(MODEL_PATH)
|
||||
self.model = ModelRunner(MODEL_PATHS, self.output, Runtime.GPU, False, cl_ctx)
|
||||
self.model.addInput("input_img", None)
|
||||
self.model.addInput("calib", self.inputs['calib'])
|
||||
|
||||
def run(self, buf:VisionBuf, calib:np.ndarray, transform:np.ndarray) -> tuple[np.ndarray, float]:
|
||||
self.numpy_inputs['calib'][0,:] = calib
|
||||
self.inputs['calib'][:] = calib
|
||||
|
||||
self.model.setInputBuffer("input_img", self.frame.prepare(buf, transform.flatten(), None).view(np.float32))
|
||||
|
||||
t1 = time.perf_counter()
|
||||
|
||||
input_img_cl = self.frame.prepare(buf, transform.flatten())
|
||||
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)
|
||||
else:
|
||||
self.numpy_inputs['input_img'] = self.frame.buffer_from_cl(input_img_cl).reshape((1, MODEL_WIDTH*MODEL_HEIGHT))
|
||||
|
||||
if TICI:
|
||||
output = self.model_run(**self.tensor_inputs).numpy().flatten()
|
||||
else:
|
||||
output = self.onnx_cpu_runner.run(None, self.numpy_inputs)[0].flatten()
|
||||
|
||||
self.model.execute()
|
||||
t2 = time.perf_counter()
|
||||
return output, t2 - t1
|
||||
return self.output, t2 - t1
|
||||
|
||||
|
||||
def fill_driver_state(msg, ds_result: DriverStateResult):
|
||||
|
||||
@@ -1,4 +1,10 @@
|
||||
#!/usr/bin/env bash
|
||||
|
||||
DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
|
||||
cd "$DIR/../../"
|
||||
|
||||
if [ -f "$DIR/libthneed.so" ]; then
|
||||
export LD_PRELOAD="$DIR/libthneed.so"
|
||||
fi
|
||||
|
||||
exec "$DIR/modeld.py" "$@"
|
||||
|
||||
+27
-44
@@ -1,15 +1,5 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
from openpilot.system.hardware import TICI
|
||||
|
||||
#
|
||||
if TICI:
|
||||
from tinygrad.tensor import Tensor
|
||||
from tinygrad.dtype import dtypes
|
||||
from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address
|
||||
os.environ['QCOM'] = '1'
|
||||
else:
|
||||
from openpilot.selfdrive.modeld.runners.ort_helpers import make_onnx_cpu_runner
|
||||
import time
|
||||
import pickle
|
||||
import numpy as np
|
||||
@@ -28,19 +18,22 @@ from openpilot.common.transformations.camera import DEVICE_CAMERAS
|
||||
from openpilot.common.transformations.model import get_warp_matrix
|
||||
from openpilot.system import sentry
|
||||
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
|
||||
from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime
|
||||
from openpilot.selfdrive.modeld.parse_model_outputs import Parser
|
||||
from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.selfdrive.modeld.models.commonmodel_pyx import DrivingModelFrame, CLContext
|
||||
|
||||
|
||||
PROCESS_NAME = "selfdrive.modeld.modeld"
|
||||
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
|
||||
|
||||
MODEL_PATH = Path(__file__).parent / 'models/supercombo.onnx'
|
||||
MODEL_PKL_PATH = Path(__file__).parent / 'models/supercombo_tinygrad.pkl'
|
||||
MODEL_PATHS = {
|
||||
ModelRunner.THNEED: Path(__file__).parent / 'models/supercombo.thneed',
|
||||
ModelRunner.ONNX: Path(__file__).parent / 'models/supercombo.onnx'}
|
||||
|
||||
METADATA_PATH = Path(__file__).parent / 'models/supercombo_metadata.pkl'
|
||||
|
||||
|
||||
class FrameMeta:
|
||||
frame_id: int = 0
|
||||
timestamp_sof: int = 0
|
||||
@@ -51,39 +44,40 @@ class FrameMeta:
|
||||
self.frame_id, self.timestamp_sof, self.timestamp_eof = vipc.frame_id, vipc.timestamp_sof, vipc.timestamp_eof
|
||||
|
||||
class ModelState:
|
||||
frames: dict[str, DrivingModelFrame]
|
||||
frame: DrivingModelFrame
|
||||
wide_frame: DrivingModelFrame
|
||||
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.frames = {'input_imgs': DrivingModelFrame(context), 'big_input_imgs': DrivingModelFrame(context)}
|
||||
self.frame = DrivingModelFrame(context)
|
||||
self.wide_frame = DrivingModelFrame(context)
|
||||
self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
|
||||
self.full_features_20Hz = np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32)
|
||||
self.desire_20Hz = np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN + 1, ModelConstants.DESIRE_LEN), dtype=np.float32)
|
||||
|
||||
# img buffers are managed in openCL transform code
|
||||
self.numpy_inputs = {
|
||||
'desire': np.zeros((1, (ModelConstants.HISTORY_BUFFER_LEN+1), ModelConstants.DESIRE_LEN), dtype=np.float32),
|
||||
'traffic_convention': np.zeros((1, ModelConstants.TRAFFIC_CONVENTION_LEN), dtype=np.float32),
|
||||
'features_buffer': np.zeros((1, ModelConstants.HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32),
|
||||
self.inputs = {
|
||||
'desire': np.zeros(ModelConstants.DESIRE_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32),
|
||||
'traffic_convention': np.zeros(ModelConstants.TRAFFIC_CONVENTION_LEN, dtype=np.float32),
|
||||
'features_buffer': np.zeros(ModelConstants.HISTORY_BUFFER_LEN * ModelConstants.FEATURE_LEN, dtype=np.float32),
|
||||
}
|
||||
|
||||
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']
|
||||
net_output_size = model_metadata['output_shapes']['outputs'][1]
|
||||
self.output = np.zeros(net_output_size, dtype=np.float32)
|
||||
self.parser = Parser()
|
||||
|
||||
if TICI:
|
||||
self.tensor_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()}
|
||||
with open(MODEL_PKL_PATH, "rb") as f:
|
||||
self.model_run = pickle.load(f)
|
||||
else:
|
||||
self.onnx_cpu_runner = make_onnx_cpu_runner(MODEL_PATH)
|
||||
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 slice_outputs(self, model_outputs: np.ndarray) -> dict[str, np.ndarray]:
|
||||
parsed_model_outputs = {k: model_outputs[np.newaxis, v] for k,v in self.output_slices.items()}
|
||||
@@ -100,36 +94,24 @@ class ModelState:
|
||||
|
||||
self.desire_20Hz[:-1] = self.desire_20Hz[1:]
|
||||
self.desire_20Hz[-1] = new_desire
|
||||
self.numpy_inputs['desire'][:] = self.desire_20Hz.reshape((1,25,4,-1)).max(axis=2)
|
||||
self.inputs['desire'][:] = self.desire_20Hz.reshape((25,4,-1)).max(axis=1).flatten()
|
||||
|
||||
self.numpy_inputs['traffic_convention'][:] = inputs['traffic_convention']
|
||||
imgs_cl = {'input_imgs': self.frames['input_imgs'].prepare(buf, transform.flatten()),
|
||||
'big_input_imgs': self.frames['big_input_imgs'].prepare(wbuf, transform_wide.flatten())}
|
||||
self.inputs['traffic_convention'][:] = inputs['traffic_convention']
|
||||
|
||||
if TICI:
|
||||
# The imgs tensors are backed by opencl memory, only need init once
|
||||
for key in imgs_cl:
|
||||
if key not in self.tensor_inputs:
|
||||
self.tensor_inputs[key] = qcom_tensor_from_opencl_address(imgs_cl[key].mem_address, self.input_shapes[key], dtype=dtypes.uint8)
|
||||
else:
|
||||
for key in imgs_cl:
|
||||
self.numpy_inputs[key] = self.frames[key].buffer_from_cl(imgs_cl[key]).reshape(self.input_shapes[key])
|
||||
self.model.setInputBuffer("input_imgs", self.frame.prepare(buf, transform.flatten(), self.model.getCLBuffer("input_imgs")))
|
||||
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
|
||||
|
||||
if TICI:
|
||||
self.output = self.model_run(**self.tensor_inputs).numpy().flatten()
|
||||
else:
|
||||
self.output = self.onnx_cpu_runner.run(None, self.numpy_inputs)[0].flatten()
|
||||
|
||||
self.model.execute()
|
||||
outputs = self.parser.parse_outputs(self.slice_outputs(self.output))
|
||||
|
||||
self.full_features_20Hz[:-1] = self.full_features_20Hz[1:]
|
||||
self.full_features_20Hz[-1] = outputs['hidden_state'][0, :]
|
||||
|
||||
idxs = np.arange(-4,-100,-4)[::-1]
|
||||
self.numpy_inputs['features_buffer'][:] = self.full_features_20Hz[idxs]
|
||||
self.inputs['features_buffer'][:] = self.full_features_20Hz[idxs].flatten()
|
||||
return outputs
|
||||
|
||||
|
||||
@@ -299,6 +281,7 @@ def main(demo=False):
|
||||
pm.send('modelV2', modelv2_send)
|
||||
pm.send('drivingModelData', drivingdata_send)
|
||||
pm.send('cameraOdometry', posenet_send)
|
||||
|
||||
last_vipc_frame_id = meta_main.frame_id
|
||||
|
||||
|
||||
|
||||
@@ -7,7 +7,7 @@
|
||||
|
||||
DrivingModelFrame::DrivingModelFrame(cl_device_id device_id, cl_context context) : ModelFrame(device_id, context) {
|
||||
input_frames = std::make_unique<uint8_t[]>(buf_size);
|
||||
input_frames_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, buf_size, NULL, &err));
|
||||
//input_frames_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, buf_size, NULL, &err));
|
||||
img_buffer_20hz_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, 5*frame_size_bytes, NULL, &err));
|
||||
region.origin = 4 * frame_size_bytes;
|
||||
region.size = frame_size_bytes;
|
||||
@@ -17,7 +17,7 @@ DrivingModelFrame::DrivingModelFrame(cl_device_id device_id, cl_context context)
|
||||
init_transform(device_id, context, MODEL_WIDTH, MODEL_HEIGHT);
|
||||
}
|
||||
|
||||
cl_mem* DrivingModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection) {
|
||||
uint8_t* DrivingModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection, cl_mem* output) {
|
||||
run_transform(yuv_cl, MODEL_WIDTH, MODEL_HEIGHT, frame_width, frame_height, frame_stride, frame_uv_offset, projection);
|
||||
|
||||
for (int i = 0; i < 4; i++) {
|
||||
@@ -25,12 +25,19 @@ cl_mem* DrivingModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_hei
|
||||
}
|
||||
loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, last_img_cl);
|
||||
|
||||
copy_queue(&loadyuv, q, img_buffer_20hz_cl, input_frames_cl, 0, 0, frame_size_bytes);
|
||||
copy_queue(&loadyuv, q, last_img_cl, input_frames_cl, 0, frame_size_bytes, frame_size_bytes);
|
||||
if (output == NULL) {
|
||||
CL_CHECK(clEnqueueReadBuffer(q, img_buffer_20hz_cl, CL_TRUE, 0, frame_size_bytes, &input_frames[0], 0, nullptr, nullptr));
|
||||
CL_CHECK(clEnqueueReadBuffer(q, last_img_cl, CL_TRUE, 0, frame_size_bytes, &input_frames[MODEL_FRAME_SIZE], 0, nullptr, nullptr));
|
||||
clFinish(q);
|
||||
return &input_frames[0];
|
||||
} else {
|
||||
copy_queue(&loadyuv, q, img_buffer_20hz_cl, *output, 0, 0, frame_size_bytes);
|
||||
copy_queue(&loadyuv, q, last_img_cl, *output, 0, frame_size_bytes, frame_size_bytes);
|
||||
|
||||
// NOTE: Since thneed is using a different command queue, this clFinish is needed to ensure the image is ready.
|
||||
clFinish(q);
|
||||
return &input_frames_cl;
|
||||
// NOTE: Since thneed is using a different command queue, this clFinish is needed to ensure the image is ready.
|
||||
clFinish(q);
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
DrivingModelFrame::~DrivingModelFrame() {
|
||||
@@ -44,15 +51,16 @@ DrivingModelFrame::~DrivingModelFrame() {
|
||||
|
||||
MonitoringModelFrame::MonitoringModelFrame(cl_device_id device_id, cl_context context) : ModelFrame(device_id, context) {
|
||||
input_frames = std::make_unique<uint8_t[]>(buf_size);
|
||||
input_frame_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, buf_size, NULL, &err));
|
||||
//input_frame_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, buf_size, NULL, &err));
|
||||
|
||||
init_transform(device_id, context, MODEL_WIDTH, MODEL_HEIGHT);
|
||||
}
|
||||
|
||||
cl_mem* MonitoringModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection) {
|
||||
uint8_t* MonitoringModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection, cl_mem* output) {
|
||||
run_transform(yuv_cl, MODEL_WIDTH, MODEL_HEIGHT, frame_width, frame_height, frame_stride, frame_uv_offset, projection);
|
||||
CL_CHECK(clEnqueueReadBuffer(q, y_cl, CL_TRUE, 0, MODEL_FRAME_SIZE * sizeof(uint8_t), input_frames.get(), 0, nullptr, nullptr));
|
||||
clFinish(q);
|
||||
return &y_cl;
|
||||
//return &y_cl;
|
||||
return input_frames.get();
|
||||
}
|
||||
|
||||
MonitoringModelFrame::~MonitoringModelFrame() {
|
||||
|
||||
@@ -23,12 +23,14 @@ public:
|
||||
q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err));
|
||||
}
|
||||
virtual ~ModelFrame() {}
|
||||
virtual cl_mem* prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection) { return NULL; }
|
||||
virtual uint8_t* prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection, cl_mem* output) { return NULL; }
|
||||
/*
|
||||
uint8_t* buffer_from_cl(cl_mem *in_frames, int buffer_size) {
|
||||
CL_CHECK(clEnqueueReadBuffer(q, *in_frames, CL_TRUE, 0, buffer_size, input_frames.get(), 0, nullptr, nullptr));
|
||||
clFinish(q);
|
||||
return &input_frames[0];
|
||||
}
|
||||
*/
|
||||
|
||||
int MODEL_WIDTH;
|
||||
int MODEL_HEIGHT;
|
||||
@@ -66,7 +68,7 @@ class DrivingModelFrame : public ModelFrame {
|
||||
public:
|
||||
DrivingModelFrame(cl_device_id device_id, cl_context context);
|
||||
~DrivingModelFrame();
|
||||
cl_mem* prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection);
|
||||
uint8_t* prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection, cl_mem* output);
|
||||
|
||||
const int MODEL_WIDTH = 512;
|
||||
const int MODEL_HEIGHT = 256;
|
||||
@@ -76,7 +78,7 @@ public:
|
||||
|
||||
private:
|
||||
LoadYUVState loadyuv;
|
||||
cl_mem img_buffer_20hz_cl, last_img_cl, input_frames_cl;
|
||||
cl_mem img_buffer_20hz_cl, last_img_cl;//, input_frames_cl;
|
||||
cl_buffer_region region;
|
||||
};
|
||||
|
||||
@@ -84,7 +86,7 @@ class MonitoringModelFrame : public ModelFrame {
|
||||
public:
|
||||
MonitoringModelFrame(cl_device_id device_id, cl_context context);
|
||||
~MonitoringModelFrame();
|
||||
cl_mem* prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection);
|
||||
uint8_t* prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection, cl_mem* output);
|
||||
|
||||
const int MODEL_WIDTH = 1440;
|
||||
const int MODEL_HEIGHT = 960;
|
||||
@@ -92,5 +94,5 @@ public:
|
||||
const int buf_size = MODEL_FRAME_SIZE;
|
||||
|
||||
private:
|
||||
cl_mem input_frame_cl;
|
||||
// cl_mem input_frame_cl;
|
||||
};
|
||||
|
||||
@@ -14,8 +14,8 @@ cdef extern from "common/clutil.h":
|
||||
cdef extern from "selfdrive/modeld/models/commonmodel.h":
|
||||
cppclass ModelFrame:
|
||||
int buf_size
|
||||
unsigned char * buffer_from_cl(cl_mem*, int);
|
||||
cl_mem * prepare(cl_mem, int, int, int, int, mat3)
|
||||
# unsigned char * buffer_from_cl(cl_mem*, int);
|
||||
unsigned char * prepare(cl_mem, int, int, int, int, mat3, cl_mem*)
|
||||
|
||||
cppclass DrivingModelFrame:
|
||||
int buf_size
|
||||
|
||||
@@ -39,17 +39,24 @@ cdef class ModelFrame:
|
||||
def __dealloc__(self):
|
||||
del self.frame
|
||||
|
||||
def prepare(self, VisionBuf buf, float[:] projection):
|
||||
def prepare(self, VisionBuf buf, float[:] projection, CLMem output):
|
||||
cdef mat3 cprojection
|
||||
memcpy(cprojection.v, &projection[0], 9*sizeof(float))
|
||||
cdef cl_mem * data
|
||||
data = self.frame.prepare(buf.buf.buf_cl, buf.width, buf.height, buf.stride, buf.uv_offset, cprojection)
|
||||
return CLMem.create(data)
|
||||
cdef unsigned char * data
|
||||
if output is None:
|
||||
data = self.frame.prepare(buf.buf.buf_cl, buf.width, buf.height, buf.stride, buf.uv_offset, cprojection, NULL)
|
||||
else:
|
||||
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
|
||||
|
||||
def buffer_from_cl(self, CLMem in_frames):
|
||||
cdef unsigned char * data2
|
||||
data2 = self.frame.buffer_from_cl(in_frames.mem, self.buf_size)
|
||||
return np.asarray(<cnp.uint8_t[:self.buf_size]> data2)
|
||||
return np.asarray(<cnp.uint8_t[:self.buf_size]> data)
|
||||
# return CLMem.create(data)
|
||||
|
||||
# def buffer_from_cl(self, CLMem in_frames):
|
||||
# cdef unsigned char * data2
|
||||
# data2 = self.frame.buffer_from_cl(in_frames.mem, self.buf_size)
|
||||
# return np.asarray(<cnp.uint8_t[:self.buf_size]> data2)
|
||||
|
||||
|
||||
cdef class DrivingModelFrame(ModelFrame):
|
||||
@@ -67,4 +74,3 @@ cdef class MonitoringModelFrame(ModelFrame):
|
||||
self._frame = new cppMonitoringModelFrame(context.device_id, context.context)
|
||||
self.frame = <cppModelFrame*>(self._frame)
|
||||
self.buf_size = self._frame.buf_size
|
||||
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
import os
|
||||
from openpilot.system.hardware import TICI
|
||||
from openpilot.sunnypilot.modeld.runners.runmodel_pyx import RunModel, Runtime
|
||||
from openpilot.selfdrive.modeld.runners.runmodel_pyx import RunModel, Runtime
|
||||
assert Runtime
|
||||
|
||||
USE_THNEED = int(os.getenv('USE_THNEED', str(int(TICI))))
|
||||
@@ -13,13 +13,13 @@ class ModelRunner(RunModel):
|
||||
|
||||
def __new__(cls, paths, *args, **kwargs):
|
||||
if ModelRunner.THNEED in paths and USE_THNEED:
|
||||
from openpilot.sunnypilot.modeld.runners.thneedmodel_pyx import ThneedModel as Runner
|
||||
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.sunnypilot.modeld.runners.snpemodel_pyx import SNPEModel as Runner
|
||||
from openpilot.selfdrive.modeld.runners.snpemodel_pyx import SNPEModel as Runner
|
||||
runner_type = ModelRunner.SNPE
|
||||
elif ModelRunner.ONNX in paths:
|
||||
from openpilot.sunnypilot.modeld.runners.onnxmodel import ONNXModel as Runner
|
||||
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")
|
||||
@@ -4,8 +4,8 @@ import sys
|
||||
import numpy as np
|
||||
from typing import Any
|
||||
|
||||
from openpilot.sunnypilot.modeld.runners.runmodel_pyx import RunModel
|
||||
from openpilot.sunnypilot.modeld.runners.ort_helpers import convert_fp16_to_fp32, ORT_TYPES_TO_NP_TYPES
|
||||
from openpilot.selfdrive.modeld.runners.runmodel_pyx import RunModel
|
||||
from openpilot.selfdrive.modeld.runners.ort_helpers import convert_fp16_to_fp32, ORT_TYPES_TO_NP_TYPES
|
||||
|
||||
|
||||
def create_ort_session(path, fp16_to_fp32):
|
||||
@@ -40,7 +40,6 @@ 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()]
|
||||
@@ -64,11 +63,7 @@ class ONNXModel(RunModel):
|
||||
return None
|
||||
|
||||
def execute(self):
|
||||
# TODO-SP: The input below causes issues because its converting the input data when in reality it doesn't need conversion as it was already the target type.
|
||||
# I am leaving this comment and the input down because this needs to be looked before merging. I had similar issues when trying the tinygrad runner...
|
||||
# Also I checked to see if I found a similar change like this on thneed but I didn't find any, so probably thneed is still working fine.
|
||||
# inputs = {k: v.view(self.input_dtypes[k]) for k,v in self.inputs.items()}
|
||||
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.view(self.input_dtypes[k]) 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"
|
||||
@@ -0,0 +1,4 @@
|
||||
#pragma once
|
||||
|
||||
#include "selfdrive/modeld/runners/runmodel.h"
|
||||
#include "selfdrive/modeld/runners/snpemodel.h"
|
||||
@@ -2,7 +2,7 @@
|
||||
|
||||
from libcpp.string cimport string
|
||||
|
||||
cdef extern from "sunnypilot/modeld/runners/runmodel.h":
|
||||
cdef extern from "selfdrive/modeld/runners/runmodel.h":
|
||||
cdef int USE_CPU_RUNTIME
|
||||
cdef int USE_GPU_RUNTIME
|
||||
cdef int USE_DSP_RUNTIME
|
||||
+1
-1
@@ -4,7 +4,7 @@
|
||||
from libcpp.string cimport string
|
||||
|
||||
from .runmodel cimport USE_CPU_RUNTIME, USE_GPU_RUNTIME, USE_DSP_RUNTIME
|
||||
from sunnypilot.modeld.models.commonmodel_pyx cimport CLMem
|
||||
from selfdrive.modeld.models.commonmodel_pyx cimport CLMem
|
||||
|
||||
class Runtime:
|
||||
CPU = USE_CPU_RUNTIME
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma clang diagnostic ignored "-Wexceptions"
|
||||
|
||||
#include "sunnypilot/modeld/runners/snpemodel.h"
|
||||
#include "selfdrive/modeld/runners/snpemodel.h"
|
||||
|
||||
#include <cstring>
|
||||
#include <memory>
|
||||
@@ -15,7 +15,7 @@
|
||||
#include <SNPE/SNPEBuilder.hpp>
|
||||
#include <SNPE/SNPEFactory.hpp>
|
||||
|
||||
#include "sunnypilot/modeld/runners/runmodel.h"
|
||||
#include "selfdrive/modeld/runners/runmodel.h"
|
||||
|
||||
struct SNPEModelInput : public ModelInput {
|
||||
std::unique_ptr<zdl::DlSystem::IUserBuffer> snpe_buffer;
|
||||
@@ -4,6 +4,6 @@ from libcpp.string cimport string
|
||||
|
||||
from msgq.visionipc.visionipc cimport cl_context
|
||||
|
||||
cdef extern from "sunnypilot/modeld/runners/snpemodel.h":
|
||||
cdef extern from "selfdrive/modeld/runners/snpemodel.h":
|
||||
cdef cppclass SNPEModel:
|
||||
SNPEModel(string, float*, size_t, int, bool, cl_context)
|
||||
+3
-3
@@ -6,9 +6,9 @@ from libcpp cimport bool
|
||||
from libcpp.string cimport string
|
||||
|
||||
from .snpemodel cimport SNPEModel as cppSNPEModel
|
||||
from sunnypilot.modeld.models.commonmodel_pyx cimport CLContext
|
||||
from sunnypilot.modeld.runners.runmodel_pyx cimport RunModel
|
||||
from sunnypilot.modeld.runners.runmodel cimport RunModel as cppRunModel
|
||||
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/"
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
#include "sunnypilot/modeld/runners/thneedmodel.h"
|
||||
#include "selfdrive/modeld/runners/thneedmodel.h"
|
||||
|
||||
#include <string>
|
||||
|
||||
@@ -2,8 +2,8 @@
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "sunnypilot/modeld/runners/runmodel.h"
|
||||
#include "sunnypilot/modeld/thneed/thneed.h"
|
||||
#include "selfdrive/modeld/runners/runmodel.h"
|
||||
#include "selfdrive/modeld/thneed/thneed.h"
|
||||
|
||||
class ThneedModel : public RunModel {
|
||||
public:
|
||||
+1
-1
@@ -4,6 +4,6 @@ from libcpp.string cimport string
|
||||
|
||||
from msgq.visionipc.visionipc cimport cl_context
|
||||
|
||||
cdef extern from "sunnypilot/modeld/runners/thneedmodel.h":
|
||||
cdef extern from "selfdrive/modeld/runners/thneedmodel.h":
|
||||
cdef cppclass ThneedModel:
|
||||
ThneedModel(string, float*, size_t, int, bool, cl_context)
|
||||
+3
-3
@@ -5,9 +5,9 @@ from libcpp cimport bool
|
||||
from libcpp.string cimport string
|
||||
|
||||
from .thneedmodel cimport ThneedModel as cppThneedModel
|
||||
from sunnypilot.modeld.models.commonmodel_pyx cimport CLContext
|
||||
from sunnypilot.modeld.runners.runmodel_pyx cimport RunModel
|
||||
from sunnypilot.modeld.runners.runmodel cimport RunModel as cppRunModel
|
||||
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):
|
||||
@@ -1,8 +0,0 @@
|
||||
|
||||
from tinygrad.tensor import Tensor
|
||||
from tinygrad.helpers import to_mv
|
||||
|
||||
def qcom_tensor_from_opencl_address(opencl_address, shape, dtype):
|
||||
cl_buf_desc_ptr = to_mv(opencl_address, 8).cast('Q')[0]
|
||||
rawbuf_ptr = to_mv(cl_buf_desc_ptr, 0x100).cast('Q')[20] # offset 0xA0 is a raw gpu pointer.
|
||||
return Tensor.from_blob(rawbuf_ptr, shape, dtype=dtype, device='QCOM')
|
||||
@@ -5,7 +5,7 @@
|
||||
#include "common/util.h"
|
||||
#include "common/clutil.h"
|
||||
#include "common/swaglog.h"
|
||||
#include "sunnypilot/modeld/thneed/thneed.h"
|
||||
#include "selfdrive/modeld/thneed/thneed.h"
|
||||
using namespace json11;
|
||||
|
||||
extern map<cl_program, string> g_program_source;
|
||||
+1
-1
@@ -1,4 +1,4 @@
|
||||
#include "sunnypilot/modeld/thneed/thneed.h"
|
||||
#include "selfdrive/modeld/thneed/thneed.h"
|
||||
|
||||
#include <cassert>
|
||||
#include <cstring>
|
||||
@@ -1,4 +1,4 @@
|
||||
#include "sunnypilot/modeld/thneed/thneed.h"
|
||||
#include "selfdrive/modeld/thneed/thneed.h"
|
||||
|
||||
#include <cassert>
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
#include "sunnypilot/modeld/thneed/thneed.h"
|
||||
#include "selfdrive/modeld/thneed/thneed.h"
|
||||
|
||||
#include <dlfcn.h>
|
||||
#include <sys/mman.h>
|
||||
@@ -36,7 +36,7 @@ CPU usage budget
|
||||
TEST_DURATION = 25
|
||||
LOG_OFFSET = 8
|
||||
|
||||
MAX_TOTAL_CPU = 275. # total for all 8 cores
|
||||
MAX_TOTAL_CPU = 265. # total for all 8 cores
|
||||
PROCS = {
|
||||
# Baseline CPU usage by process
|
||||
"selfdrive.controls.controlsd": 16.0,
|
||||
@@ -50,8 +50,8 @@ PROCS = {
|
||||
"selfdrive.locationd.paramsd": 9.0,
|
||||
"./sensord": 7.0,
|
||||
"selfdrive.controls.radard": 2.0,
|
||||
"selfdrive.modeld.modeld": 22.0,
|
||||
"selfdrive.modeld.dmonitoringmodeld": 21.0,
|
||||
"selfdrive.modeld.modeld": 17.0,
|
||||
"selfdrive.modeld.dmonitoringmodeld": 11.0,
|
||||
"system.hardware.hardwared": 4.0,
|
||||
"selfdrive.locationd.calibrationd": 2.0,
|
||||
"selfdrive.locationd.torqued": 5.0,
|
||||
@@ -371,9 +371,10 @@ class TestOnroad:
|
||||
result += "------------------------------------------------\n"
|
||||
result += "----------------- Model Timing -----------------\n"
|
||||
result += "------------------------------------------------\n"
|
||||
# TODO: this went up when plannerd cpu usage increased, why?
|
||||
cfgs = [
|
||||
("modelV2", 0.045, 0.035),
|
||||
("driverStateV2", 0.045, 0.035),
|
||||
("modelV2", 0.050, 0.036),
|
||||
("driverStateV2", 0.050, 0.026),
|
||||
]
|
||||
for (s, instant_max, avg_max) in cfgs:
|
||||
ts = [getattr(m, s).modelExecutionTime for m in self.msgs[s]]
|
||||
|
||||
@@ -83,6 +83,8 @@ void SoftwarePanelSP::handleBundleDownloadProgress() {
|
||||
if (bundle.getStatus() == cereal::ModelManagerSP::DownloadStatus::DOWNLOADING) {
|
||||
currentModelLblBtn->showDescription();
|
||||
}
|
||||
|
||||
currentModelLblBtn->setEnabled(!is_onroad && !isDownloading());
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -126,6 +128,7 @@ void SoftwarePanelSP::handleCurrentModelLblBtnClicked() {
|
||||
bundleNames.append(index_to_bundle[index]);
|
||||
}
|
||||
|
||||
currentModelLblBtn->setEnabled(!is_onroad);
|
||||
currentModelLblBtn->setValue(GetActiveModelName());
|
||||
|
||||
const QString selectedBundleName = MultiOptionDialog::getSelection(
|
||||
@@ -158,7 +161,6 @@ void SoftwarePanelSP::updateLabels() {
|
||||
}
|
||||
|
||||
handleBundleDownloadProgress();
|
||||
currentModelLblBtn->setEnabled(!is_onroad && !isDownloading());
|
||||
currentModelLblBtn->setValue(GetActiveModelName());
|
||||
SoftwarePanel::updateLabels();
|
||||
}
|
||||
|
||||
@@ -24,7 +24,7 @@ private:
|
||||
const SubMaster &sm = *(uiStateSP()->sm);
|
||||
const auto model_manager = sm["modelManagerSP"].getModelManagerSP();
|
||||
|
||||
if (!model_manager.hasSelectedBundle() || !sm.updated("modelManagerSP")) {
|
||||
if (!model_manager.hasSelectedBundle()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
@@ -1 +0,0 @@
|
||||
SConscript(['modeld/SConscript'])
|
||||
@@ -1 +0,0 @@
|
||||
*_pyx.cpp
|
||||
@@ -1,58 +0,0 @@
|
||||
import glob
|
||||
|
||||
Import('env', 'envCython', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc', 'transformations')
|
||||
lenv = env.Clone()
|
||||
lenvCython = envCython.Clone()
|
||||
|
||||
libs = [cereal, messaging, visionipc, gpucommon, common, 'capnp', 'kj', 'pthread']
|
||||
frameworks = []
|
||||
|
||||
common_src = [
|
||||
"models/commonmodel.cc",
|
||||
"transforms/loadyuv.cc",
|
||||
"transforms/transform.cc",
|
||||
]
|
||||
|
||||
thneed_src_common = [
|
||||
"thneed/thneed_common.cc",
|
||||
"thneed/serialize.cc",
|
||||
]
|
||||
|
||||
thneed_src_qcom = thneed_src_common + ["thneed/thneed_qcom2.cc"]
|
||||
thneed_src_pc = thneed_src_common + ["thneed/thneed_pc.cc"]
|
||||
thneed_src = thneed_src_qcom if arch == "larch64" else thneed_src_pc
|
||||
|
||||
# SNPE except on Mac and ARM Linux
|
||||
snpe_lib = []
|
||||
if arch != "Darwin" and arch != "aarch64":
|
||||
common_src += ['runners/snpemodel.cc']
|
||||
snpe_lib += ['SNPE']
|
||||
|
||||
# OpenCL is a framework on Mac
|
||||
if arch == "Darwin":
|
||||
frameworks += ['OpenCL']
|
||||
else:
|
||||
libs += ['OpenCL']
|
||||
|
||||
# Set path definitions
|
||||
for pathdef, fn in {'TRANSFORM': 'transforms/transform.cl', 'LOADYUV': 'transforms/loadyuv.cl'}.items():
|
||||
for xenv in (lenv, lenvCython):
|
||||
xenv['CXXFLAGS'].append(f'-D{pathdef}_PATH=\\"{File(fn).abspath}\\"')
|
||||
|
||||
# Compile cython
|
||||
snpe_rpath_qcom = "/data/pythonpath/third_party/snpe/larch64"
|
||||
snpe_rpath_pc = f"{Dir('#').abspath}/third_party/snpe/x86_64-linux-clang"
|
||||
snpe_rpath = lenvCython['RPATH'] + [snpe_rpath_qcom if arch == "larch64" else snpe_rpath_pc]
|
||||
|
||||
cython_libs = envCython["LIBS"] + libs
|
||||
snpemodel_lib = lenv.Library('snpemodel', ['runners/snpemodel.cc'])
|
||||
commonmodel_lib = lenv.Library('commonmodel', common_src)
|
||||
|
||||
lenvCython.Program('runners/runmodel_pyx.so', 'runners/runmodel_pyx.pyx', LIBS=cython_libs, FRAMEWORKS=frameworks)
|
||||
lenvCython.Program('runners/snpemodel_pyx.so', 'runners/snpemodel_pyx.pyx', LIBS=[snpemodel_lib, snpe_lib, *cython_libs], FRAMEWORKS=frameworks, RPATH=snpe_rpath)
|
||||
lenvCython.Program('models/commonmodel_pyx.so', 'models/commonmodel_pyx.pyx', LIBS=[commonmodel_lib, *cython_libs], FRAMEWORKS=frameworks)
|
||||
|
||||
if arch == 'larch64' or GetOption('pc_thneed'):
|
||||
thneed_lib = env.SharedLibrary('thneed', thneed_src, LIBS=[gpucommon, common, 'OpenCL', 'dl'])
|
||||
thneedmodel_lib = env.Library('thneedmodel', ['runners/thneedmodel.cc'])
|
||||
lenvCython.Program('runners/thneedmodel_pyx.so', 'runners/thneedmodel_pyx.pyx', LIBS=envCython["LIBS"]+[thneedmodel_lib, thneed_lib, gpucommon, common, 'dl', 'OpenCL'])
|
||||
@@ -1,86 +0,0 @@
|
||||
import numpy as np
|
||||
|
||||
def index_function(idx, max_val=192, max_idx=32):
|
||||
return (max_val) * ((idx/max_idx)**2)
|
||||
|
||||
class ModelConstants:
|
||||
# time and distance indices
|
||||
IDX_N = 33
|
||||
T_IDXS = [index_function(idx, max_val=10.0) for idx in range(IDX_N)]
|
||||
X_IDXS = [index_function(idx, max_val=192.0) for idx in range(IDX_N)]
|
||||
LEAD_T_IDXS = [0., 2., 4., 6., 8., 10.]
|
||||
LEAD_T_OFFSETS = [0., 2., 4.]
|
||||
META_T_IDXS = [2., 4., 6., 8., 10.]
|
||||
|
||||
# model inputs constants
|
||||
MODEL_FREQ = 20
|
||||
FEATURE_LEN = 512
|
||||
FULL_HISTORY_BUFFER_LEN = 99
|
||||
HISTORY_BUFFER_LEN = 24
|
||||
DESIRE_LEN = 8
|
||||
TRAFFIC_CONVENTION_LEN = 2
|
||||
LAT_PLANNER_STATE_LEN = 4
|
||||
LATERAL_CONTROL_PARAMS_LEN = 2
|
||||
PREV_DESIRED_CURV_LEN = 1
|
||||
|
||||
# model outputs constants
|
||||
FCW_THRESHOLDS_5MS2 = np.array([.05, .05, .15, .15, .15], dtype=np.float32)
|
||||
FCW_THRESHOLDS_3MS2 = np.array([.7, .7], dtype=np.float32)
|
||||
FCW_5MS2_PROBS_WIDTH = 5
|
||||
FCW_3MS2_PROBS_WIDTH = 2
|
||||
|
||||
DISENGAGE_WIDTH = 5
|
||||
POSE_WIDTH = 6
|
||||
WIDE_FROM_DEVICE_WIDTH = 3
|
||||
SIM_POSE_WIDTH = 6
|
||||
LEAD_WIDTH = 4
|
||||
LANE_LINES_WIDTH = 2
|
||||
ROAD_EDGES_WIDTH = 2
|
||||
PLAN_WIDTH = 15
|
||||
DESIRE_PRED_WIDTH = 8
|
||||
LAT_PLANNER_SOLUTION_WIDTH = 4
|
||||
DESIRED_CURV_WIDTH = 1
|
||||
|
||||
NUM_LANE_LINES = 4
|
||||
NUM_ROAD_EDGES = 2
|
||||
|
||||
LEAD_TRAJ_LEN = 6
|
||||
DESIRE_PRED_LEN = 4
|
||||
|
||||
PLAN_MHP_N = 5
|
||||
LEAD_MHP_N = 2
|
||||
PLAN_MHP_SELECTION = 1
|
||||
LEAD_MHP_SELECTION = 3
|
||||
|
||||
FCW_THRESHOLD_5MS2_HIGH = 0.15
|
||||
FCW_THRESHOLD_5MS2_LOW = 0.05
|
||||
FCW_THRESHOLD_3MS2 = 0.7
|
||||
|
||||
CONFIDENCE_BUFFER_LEN = 5
|
||||
RYG_GREEN = 0.01165
|
||||
RYG_YELLOW = 0.06157
|
||||
|
||||
POLY_PATH_DEGREE = 4
|
||||
|
||||
# model outputs slices
|
||||
class Plan:
|
||||
POSITION = slice(0, 3)
|
||||
VELOCITY = slice(3, 6)
|
||||
ACCELERATION = slice(6, 9)
|
||||
T_FROM_CURRENT_EULER = slice(9, 12)
|
||||
ORIENTATION_RATE = slice(12, 15)
|
||||
|
||||
class Meta:
|
||||
ENGAGED = slice(0, 1)
|
||||
# next 2, 4, 6, 8, 10 seconds
|
||||
GAS_DISENGAGE = slice(1, 31, 6)
|
||||
BRAKE_DISENGAGE = slice(2, 31, 6)
|
||||
STEER_OVERRIDE = slice(3, 31, 6)
|
||||
HARD_BRAKE_3 = slice(4, 31, 6)
|
||||
HARD_BRAKE_4 = slice(5, 31, 6)
|
||||
HARD_BRAKE_5 = slice(6, 31, 6)
|
||||
# next 0, 2, 4, 6, 8, 10 seconds
|
||||
GAS_PRESS = slice(31, 55, 4)
|
||||
BRAKE_PRESS = slice(32, 55, 4)
|
||||
LEFT_BLINKER = slice(33, 55, 4)
|
||||
RIGHT_BLINKER = slice(34, 55, 4)
|
||||
@@ -1,237 +0,0 @@
|
||||
import os
|
||||
import capnp
|
||||
import numpy as np
|
||||
from cereal import log
|
||||
from openpilot.sunnypilot.modeld.constants import ModelConstants, Plan, Meta
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import MIN_SPEED
|
||||
|
||||
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
|
||||
|
||||
ConfidenceClass = log.ModelDataV2.ConfidenceClass
|
||||
|
||||
def curv_from_psis(psi_target, psi_rate, vego, delay):
|
||||
vego = np.clip(vego, MIN_SPEED, np.inf)
|
||||
curv_from_psi = psi_target / (vego * delay) # epsilon to prevent divide-by-zero
|
||||
return 2*curv_from_psi - psi_rate / vego
|
||||
|
||||
def get_curvature_from_plan(plan, vego, delay):
|
||||
psi_target = np.interp(delay, ModelConstants.T_IDXS, plan[:, Plan.T_FROM_CURRENT_EULER][:, 2])
|
||||
psi_rate = plan[:, Plan.ORIENTATION_RATE][0, 2]
|
||||
return curv_from_psis(psi_target, psi_rate, vego, delay)
|
||||
|
||||
class PublishState:
|
||||
def __init__(self):
|
||||
self.disengage_buffer = np.zeros(ModelConstants.CONFIDENCE_BUFFER_LEN*ModelConstants.DISENGAGE_WIDTH, dtype=np.float32)
|
||||
self.prev_brake_5ms2_probs = np.zeros(ModelConstants.FCW_5MS2_PROBS_WIDTH, dtype=np.float32)
|
||||
self.prev_brake_3ms2_probs = np.zeros(ModelConstants.FCW_3MS2_PROBS_WIDTH, dtype=np.float32)
|
||||
|
||||
def fill_xyzt(builder, t, x, y, z, x_std=None, y_std=None, z_std=None):
|
||||
builder.t = t
|
||||
builder.x = x.tolist()
|
||||
builder.y = y.tolist()
|
||||
builder.z = z.tolist()
|
||||
if x_std is not None:
|
||||
builder.xStd = x_std.tolist()
|
||||
if y_std is not None:
|
||||
builder.yStd = y_std.tolist()
|
||||
if z_std is not None:
|
||||
builder.zStd = z_std.tolist()
|
||||
|
||||
def fill_xyvat(builder, t, x, y, v, a, x_std=None, y_std=None, v_std=None, a_std=None):
|
||||
builder.t = t
|
||||
builder.x = x.tolist()
|
||||
builder.y = y.tolist()
|
||||
builder.v = v.tolist()
|
||||
builder.a = a.tolist()
|
||||
if x_std is not None:
|
||||
builder.xStd = x_std.tolist()
|
||||
if y_std is not None:
|
||||
builder.yStd = y_std.tolist()
|
||||
if v_std is not None:
|
||||
builder.vStd = v_std.tolist()
|
||||
if a_std is not None:
|
||||
builder.aStd = a_std.tolist()
|
||||
|
||||
def fill_xyz_poly(builder, degree, x, y, z):
|
||||
xyz = np.stack([x, y, z], axis=1)
|
||||
coeffs = np.polynomial.polynomial.polyfit(ModelConstants.T_IDXS, xyz, deg=degree)
|
||||
builder.xCoefficients = coeffs[:, 0].tolist()
|
||||
builder.yCoefficients = coeffs[:, 1].tolist()
|
||||
builder.zCoefficients = coeffs[:, 2].tolist()
|
||||
|
||||
def fill_lane_line_meta(builder, lane_lines, lane_line_probs):
|
||||
builder.leftY = lane_lines[1].y[0]
|
||||
builder.leftProb = lane_line_probs[1]
|
||||
builder.rightY = lane_lines[2].y[0]
|
||||
builder.rightProb = lane_line_probs[2]
|
||||
|
||||
def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._DynamicStructBuilder,
|
||||
net_output_data: dict[str, np.ndarray], v_ego: float, delay: float,
|
||||
publish_state: PublishState, vipc_frame_id: int, vipc_frame_id_extra: int,
|
||||
frame_id: int, frame_drop: float, timestamp_eof: int, model_execution_time: float,
|
||||
valid: bool) -> None:
|
||||
frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0
|
||||
frame_drop_perc = frame_drop * 100
|
||||
extended_msg.valid = valid
|
||||
base_msg.valid = valid
|
||||
|
||||
desired_curv = float(get_curvature_from_plan(net_output_data['plan'][0], v_ego, delay))
|
||||
|
||||
driving_model_data = base_msg.drivingModelData
|
||||
|
||||
driving_model_data.frameId = vipc_frame_id
|
||||
driving_model_data.frameIdExtra = vipc_frame_id_extra
|
||||
driving_model_data.frameDropPerc = frame_drop_perc
|
||||
driving_model_data.modelExecutionTime = model_execution_time
|
||||
|
||||
action = driving_model_data.action
|
||||
action.desiredCurvature = desired_curv
|
||||
|
||||
modelV2 = extended_msg.modelV2
|
||||
modelV2.frameId = vipc_frame_id
|
||||
modelV2.frameIdExtra = vipc_frame_id_extra
|
||||
modelV2.frameAge = frame_age
|
||||
modelV2.frameDropPerc = frame_drop_perc
|
||||
modelV2.timestampEof = timestamp_eof
|
||||
modelV2.modelExecutionTime = model_execution_time
|
||||
|
||||
# plan
|
||||
position = modelV2.position
|
||||
fill_xyzt(position, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.POSITION].T, *net_output_data['plan_stds'][0,:,Plan.POSITION].T)
|
||||
velocity = modelV2.velocity
|
||||
fill_xyzt(velocity, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.VELOCITY].T)
|
||||
acceleration = modelV2.acceleration
|
||||
fill_xyzt(acceleration, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ACCELERATION].T)
|
||||
orientation = modelV2.orientation
|
||||
fill_xyzt(orientation, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.T_FROM_CURRENT_EULER].T)
|
||||
orientation_rate = modelV2.orientationRate
|
||||
fill_xyzt(orientation_rate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T)
|
||||
|
||||
# temporal pose
|
||||
temporal_pose = modelV2.temporalPose
|
||||
temporal_pose.trans = net_output_data['plan'][0,0,Plan.VELOCITY].tolist()
|
||||
temporal_pose.transStd = net_output_data['plan_stds'][0,0,Plan.VELOCITY].tolist()
|
||||
temporal_pose.rot = net_output_data['plan'][0,0,Plan.ORIENTATION_RATE].tolist()
|
||||
temporal_pose.rotStd = net_output_data['plan_stds'][0,0,Plan.ORIENTATION_RATE].tolist()
|
||||
|
||||
# poly path
|
||||
poly_path = driving_model_data.path
|
||||
fill_xyz_poly(poly_path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T)
|
||||
|
||||
# lateral planning
|
||||
action = modelV2.action
|
||||
action.desiredCurvature = desired_curv
|
||||
|
||||
# times at X_IDXS according to model plan
|
||||
PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N
|
||||
PLAN_T_IDXS[0] = 0.0
|
||||
plan_x = net_output_data['plan'][0,:,Plan.POSITION][:,0].tolist()
|
||||
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
|
||||
PLAN_T_IDXS[xidx] = ModelConstants.T_IDXS[ModelConstants.IDX_N - 1]
|
||||
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')
|
||||
PLAN_T_IDXS[xidx] = p * ModelConstants.T_IDXS[tidx+1] + (1 - p) * ModelConstants.T_IDXS[tidx]
|
||||
|
||||
# lane lines
|
||||
modelV2.init('laneLines', 4)
|
||||
for i in range(4):
|
||||
lane_line = modelV2.laneLines[i]
|
||||
fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['lane_lines'][0,i,:,0], net_output_data['lane_lines'][0,i,:,1])
|
||||
modelV2.laneLineStds = net_output_data['lane_lines_stds'][0,:,0,0].tolist()
|
||||
modelV2.laneLineProbs = net_output_data['lane_lines_prob'][0,1::2].tolist()
|
||||
|
||||
lane_line_meta = driving_model_data.laneLineMeta
|
||||
fill_lane_line_meta(lane_line_meta, modelV2.laneLines, modelV2.laneLineProbs)
|
||||
|
||||
# road edges
|
||||
modelV2.init('roadEdges', 2)
|
||||
for i in range(2):
|
||||
road_edge = modelV2.roadEdges[i]
|
||||
fill_xyzt(road_edge, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['road_edges'][0,i,:,0], net_output_data['road_edges'][0,i,:,1])
|
||||
modelV2.roadEdgeStds = net_output_data['road_edges_stds'][0,:,0,0].tolist()
|
||||
|
||||
# leads
|
||||
modelV2.init('leadsV3', 3)
|
||||
for i in range(3):
|
||||
lead = modelV2.leadsV3[i]
|
||||
fill_xyvat(lead, ModelConstants.LEAD_T_IDXS, *net_output_data['lead'][0,i].T, *net_output_data['lead_stds'][0,i].T)
|
||||
lead.prob = net_output_data['lead_prob'][0,i].tolist()
|
||||
lead.probTime = ModelConstants.LEAD_T_OFFSETS[i]
|
||||
|
||||
# meta
|
||||
meta = modelV2.meta
|
||||
meta.desireState = net_output_data['desire_state'][0].reshape(-1).tolist()
|
||||
meta.desirePrediction = net_output_data['desire_pred'][0].reshape(-1).tolist()
|
||||
meta.engagedProb = net_output_data['meta'][0,Meta.ENGAGED].item()
|
||||
meta.init('disengagePredictions')
|
||||
disengage_predictions = meta.disengagePredictions
|
||||
disengage_predictions.t = ModelConstants.META_T_IDXS
|
||||
disengage_predictions.brakeDisengageProbs = net_output_data['meta'][0,Meta.BRAKE_DISENGAGE].tolist()
|
||||
disengage_predictions.gasDisengageProbs = net_output_data['meta'][0,Meta.GAS_DISENGAGE].tolist()
|
||||
disengage_predictions.steerOverrideProbs = net_output_data['meta'][0,Meta.STEER_OVERRIDE].tolist()
|
||||
disengage_predictions.brake3MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_3].tolist()
|
||||
disengage_predictions.brake4MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_4].tolist()
|
||||
disengage_predictions.brake5MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_5].tolist()
|
||||
disengage_predictions.gasPressProbs = net_output_data['meta'][0,Meta.GAS_PRESS].tolist()
|
||||
disengage_predictions.brakePressProbs = net_output_data['meta'][0,Meta.BRAKE_PRESS].tolist()
|
||||
|
||||
publish_state.prev_brake_5ms2_probs[:-1] = publish_state.prev_brake_5ms2_probs[1:]
|
||||
publish_state.prev_brake_5ms2_probs[-1] = net_output_data['meta'][0,Meta.HARD_BRAKE_5][0]
|
||||
publish_state.prev_brake_3ms2_probs[:-1] = publish_state.prev_brake_3ms2_probs[1:]
|
||||
publish_state.prev_brake_3ms2_probs[-1] = net_output_data['meta'][0,Meta.HARD_BRAKE_3][0]
|
||||
hard_brake_predicted = (publish_state.prev_brake_5ms2_probs > ModelConstants.FCW_THRESHOLDS_5MS2).all() and \
|
||||
(publish_state.prev_brake_3ms2_probs > ModelConstants.FCW_THRESHOLDS_3MS2).all()
|
||||
meta.hardBrakePredicted = hard_brake_predicted.item()
|
||||
|
||||
# confidence
|
||||
if vipc_frame_id % (2*ModelConstants.MODEL_FREQ) == 0:
|
||||
# any disengage prob
|
||||
brake_disengage_probs = net_output_data['meta'][0,Meta.BRAKE_DISENGAGE]
|
||||
gas_disengage_probs = net_output_data['meta'][0,Meta.GAS_DISENGAGE]
|
||||
steer_override_probs = net_output_data['meta'][0,Meta.STEER_OVERRIDE]
|
||||
any_disengage_probs = 1-((1-brake_disengage_probs)*(1-gas_disengage_probs)*(1-steer_override_probs))
|
||||
# independent disengage prob for each 2s slice
|
||||
ind_disengage_probs = np.r_[any_disengage_probs[0], np.diff(any_disengage_probs) / (1 - any_disengage_probs[:-1])]
|
||||
# rolling buf for 2, 4, 6, 8, 10s
|
||||
publish_state.disengage_buffer[:-ModelConstants.DISENGAGE_WIDTH] = publish_state.disengage_buffer[ModelConstants.DISENGAGE_WIDTH:]
|
||||
publish_state.disengage_buffer[-ModelConstants.DISENGAGE_WIDTH:] = ind_disengage_probs
|
||||
|
||||
score = 0.
|
||||
for i in range(ModelConstants.DISENGAGE_WIDTH):
|
||||
score += publish_state.disengage_buffer[i*ModelConstants.DISENGAGE_WIDTH+ModelConstants.DISENGAGE_WIDTH-1-i].item() / ModelConstants.DISENGAGE_WIDTH
|
||||
if score < ModelConstants.RYG_GREEN:
|
||||
modelV2.confidence = ConfidenceClass.green
|
||||
elif score < ModelConstants.RYG_YELLOW:
|
||||
modelV2.confidence = ConfidenceClass.yellow
|
||||
else:
|
||||
modelV2.confidence = ConfidenceClass.red
|
||||
|
||||
# raw prediction if enabled
|
||||
if SEND_RAW_PRED:
|
||||
modelV2.rawPredictions = net_output_data['raw_pred'].tobytes()
|
||||
|
||||
def fill_pose_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str, np.ndarray],
|
||||
vipc_frame_id: int, vipc_dropped_frames: int, timestamp_eof: int, live_calib_seen: bool) -> None:
|
||||
msg.valid = live_calib_seen & (vipc_dropped_frames < 1)
|
||||
cameraOdometry = msg.cameraOdometry
|
||||
|
||||
cameraOdometry.frameId = vipc_frame_id
|
||||
cameraOdometry.timestampEof = timestamp_eof
|
||||
|
||||
cameraOdometry.trans = net_output_data['pose'][0,:3].tolist()
|
||||
cameraOdometry.rot = net_output_data['pose'][0,3:].tolist()
|
||||
cameraOdometry.wideFromDeviceEuler = net_output_data['wide_from_device_euler'][0,:].tolist()
|
||||
cameraOdometry.roadTransformTrans = net_output_data['road_transform'][0,:3].tolist()
|
||||
cameraOdometry.transStd = net_output_data['pose_stds'][0,:3].tolist()
|
||||
cameraOdometry.rotStd = net_output_data['pose_stds'][0,3:].tolist()
|
||||
cameraOdometry.wideFromDeviceEulerStd = net_output_data['wide_from_device_euler_stds'][0,:].tolist()
|
||||
cameraOdometry.roadTransformTransStd = net_output_data['road_transform_stds'][0,:3].tolist()
|
||||
@@ -1,28 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
import sys
|
||||
import pathlib
|
||||
import onnx
|
||||
import codecs
|
||||
import pickle
|
||||
|
||||
def get_name_and_shape(value_info:onnx.ValueInfoProto) -> tuple[str, tuple[int,...]]:
|
||||
shape = tuple([int(dim.dim_value) for dim in value_info.type.tensor_type.shape.dim])
|
||||
name = value_info.name
|
||||
return name, shape
|
||||
|
||||
if __name__ == "__main__":
|
||||
model_path = pathlib.Path(sys.argv[1])
|
||||
model = onnx.load(str(model_path))
|
||||
i = [x.key for x in model.metadata_props].index('output_slices')
|
||||
output_slices = model.metadata_props[i].value
|
||||
|
||||
metadata = {}
|
||||
metadata['output_slices'] = pickle.loads(codecs.decode(output_slices.encode(), "base64"))
|
||||
metadata['input_shapes'] = dict([get_name_and_shape(x) for x in model.graph.input])
|
||||
metadata['output_shapes'] = dict([get_name_and_shape(x) for x in model.graph.output])
|
||||
|
||||
metadata_path = model_path.parent / (model_path.stem + '_metadata.pkl')
|
||||
with open(metadata_path, 'wb') as f:
|
||||
pickle.dump(metadata, f)
|
||||
|
||||
print(f'saved metadata to {metadata_path}')
|
||||
@@ -1,10 +0,0 @@
|
||||
#!/usr/bin/env bash
|
||||
|
||||
DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
|
||||
cd "$DIR/../../"
|
||||
|
||||
if [ -f "$DIR/libthneed.so" ]; then
|
||||
export LD_PRELOAD="$DIR/libthneed.so"
|
||||
fi
|
||||
|
||||
exec "$DIR/modeld.py" "$@"
|
||||
@@ -1,331 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import time
|
||||
import numpy as np
|
||||
import cereal.messaging as messaging
|
||||
from cereal import car, log
|
||||
from setproctitle import setproctitle
|
||||
from cereal.messaging import PubMaster, SubMaster
|
||||
from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
|
||||
from opendbc.car.car_helpers import get_demo_car_params
|
||||
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
|
||||
from openpilot.common.transformations.camera import DEVICE_CAMERAS
|
||||
from openpilot.common.transformations.model import get_warp_matrix
|
||||
from openpilot.system import sentry
|
||||
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
|
||||
from openpilot.sunnypilot.modeld.runners import ModelRunner, Runtime
|
||||
from openpilot.sunnypilot.modeld.parse_model_outputs import Parser
|
||||
from openpilot.sunnypilot.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState
|
||||
from openpilot.sunnypilot.modeld.constants import ModelConstants
|
||||
from openpilot.sunnypilot.modeld.models.commonmodel_pyx import DrivingModelFrame, CLContext
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.numpy_fast import interp
|
||||
|
||||
from openpilot.sunnypilot.modeld.runners.run_helpers import load_model, load_metadata, prepare_inputs
|
||||
|
||||
PROCESS_NAME = "sunnypilot.modeld.modeld"
|
||||
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
|
||||
|
||||
|
||||
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: DrivingModelFrame
|
||||
wide_frame: DrivingModelFrame
|
||||
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 = DrivingModelFrame(context)
|
||||
self.wide_frame = DrivingModelFrame(context)
|
||||
self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
|
||||
self.full_features_20Hz = np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32)
|
||||
self.desire_20Hz = np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN + 1, ModelConstants.DESIRE_LEN), dtype=np.float32)
|
||||
|
||||
model_paths = load_model()
|
||||
self.model_metadata = load_metadata()
|
||||
self.inputs = prepare_inputs(self.model_metadata)
|
||||
|
||||
self.output_slices = self.model_metadata['output_slices']
|
||||
net_output_size = self.model_metadata['output_shapes']['outputs'][1]
|
||||
self.output = np.zeros(net_output_size, dtype=np.float32)
|
||||
self.parser = Parser()
|
||||
|
||||
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)
|
||||
|
||||
num_elements = self.model_metadata['input_shapes']['features_buffer'][1]
|
||||
step_size = int(-100 / num_elements)
|
||||
self.feature_buffer_idxs = np.arange(step_size, step_size * (num_elements + 1), step_size)[::-1]
|
||||
|
||||
desired_shape = self.model_metadata["input_shapes"]["desire"][1]
|
||||
middle_dim = int(self.desire_20Hz.shape[0] / desired_shape)
|
||||
self.desire_reshape_dims = (desired_shape, middle_dim, -1)
|
||||
|
||||
def slice_outputs(self, model_outputs: np.ndarray) -> dict[str, np.ndarray]:
|
||||
parsed_model_outputs = {k: model_outputs[np.newaxis, v] for k,v in self.output_slices.items()}
|
||||
if SEND_RAW_PRED:
|
||||
parsed_model_outputs['raw_pred'] = model_outputs.copy()
|
||||
return parsed_model_outputs
|
||||
|
||||
def run(self, buf: VisionBuf, wbuf: VisionBuf, transform: np.ndarray, transform_wide: np.ndarray,
|
||||
inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None:
|
||||
# Model decides when action is completed, so desire input is just a pulse triggered on rising edge
|
||||
inputs['desire'][0] = 0
|
||||
new_desire = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0)
|
||||
self.prev_desire[:] = inputs['desire']
|
||||
|
||||
self.desire_20Hz[:-1] = self.desire_20Hz[1:]
|
||||
self.desire_20Hz[-1] = new_desire
|
||||
self.inputs['desire'][:] = self.desire_20Hz.reshape(self.desire_reshape_dims).max(axis=1).flatten()
|
||||
|
||||
for key in self.inputs:
|
||||
if key in inputs and key not in ['desire']:
|
||||
self.inputs[key][:] = inputs[key]
|
||||
|
||||
self.inputs['traffic_convention'][:] = inputs['traffic_convention']
|
||||
|
||||
self.model.setInputBuffer("input_imgs", self.frame.prepare(buf, transform.flatten(), self.model.getCLBuffer("input_imgs")))
|
||||
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()
|
||||
outputs = self.parser.parse_outputs(self.slice_outputs(self.output), self.inputs.keys())
|
||||
|
||||
self.full_features_20Hz[:-1] = self.full_features_20Hz[1:]
|
||||
self.full_features_20Hz[-1] = outputs['hidden_state'][0, :]
|
||||
|
||||
self.inputs['features_buffer'][:] = self.full_features_20Hz[self.feature_buffer_idxs].flatten()
|
||||
if "desired_curvature" in outputs:
|
||||
input_name_prev = None
|
||||
|
||||
if "prev_desired_curvs" in self.inputs.keys():
|
||||
input_name_prev = 'prev_desired_curvs'
|
||||
elif "prev_desired_curv" in self.inputs.keys():
|
||||
input_name_prev = 'prev_desired_curv'
|
||||
|
||||
if input_name_prev is not None:
|
||||
len = outputs['desired_curvature'][0].size
|
||||
self.inputs[input_name_prev][:-len] = self.inputs[input_name_prev][len:]
|
||||
self.inputs[input_name_prev][-len:] = outputs['desired_curvature'][0, :]
|
||||
|
||||
if "lat_planner_solution" in outputs:
|
||||
if "lat_planner_state" in self.inputs.keys():
|
||||
self.inputs['lat_planner_state'][2] = interp(DT_MDL, ModelConstants.T_IDXS, outputs['lat_planner_solution'][0, :, 2])
|
||||
self.inputs['lat_planner_state'][3] = interp(DT_MDL, ModelConstants.T_IDXS, outputs['lat_planner_solution'][0, :, 3])
|
||||
return outputs
|
||||
|
||||
|
||||
def main(demo=False):
|
||||
cloudlog.warning("modeld init")
|
||||
|
||||
sentry.set_tag("daemon", PROCESS_NAME)
|
||||
cloudlog.bind(daemon=PROCESS_NAME)
|
||||
setproctitle(PROCESS_NAME)
|
||||
config_realtime_process(7, 54)
|
||||
|
||||
cloudlog.warning("setting up CL context")
|
||||
cl_context = CLContext()
|
||||
cloudlog.warning("CL context ready; loading model")
|
||||
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", "drivingModelData", "cameraOdometry"])
|
||||
sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl"])
|
||||
|
||||
publish_state = PublishState()
|
||||
params = Params()
|
||||
|
||||
# setup filter to track dropped frames
|
||||
frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_FREQ)
|
||||
frame_id = 0
|
||||
last_vipc_frame_id = 0
|
||||
run_count = 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
|
||||
buf_main, buf_extra = None, None
|
||||
meta_main = FrameMeta()
|
||||
meta_extra = FrameMeta()
|
||||
|
||||
|
||||
if demo:
|
||||
CP = get_demo_car_params()
|
||||
else:
|
||||
CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
|
||||
cloudlog.info("modeld got CarParams: %s", CP.carName)
|
||||
|
||||
# TODO this needs more thought, use .2s extra for now to estimate other delays
|
||||
steer_delay = CP.steerActuatorDelay + .2
|
||||
|
||||
DH = DesireHelper()
|
||||
|
||||
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.debug("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.debug("vipc_client_extra no frame")
|
||||
continue
|
||||
|
||||
if abs(meta_main.timestamp_sof - meta_extra.timestamp_sof) > 10000000:
|
||||
cloudlog.error(f"frames out of sync! main: {meta_main.frame_id} ({meta_main.timestamp_sof / 1e9:.5f}),\
|
||||
extra: {meta_extra.frame_id} ({meta_extra.timestamp_sof / 1e9:.5f})")
|
||||
|
||||
else:
|
||||
# Use single camera
|
||||
buf_extra = buf_main
|
||||
meta_extra = meta_main
|
||||
|
||||
sm.update(0)
|
||||
desire = DH.desire
|
||||
is_rhd = sm["driverMonitoringState"].isRHD
|
||||
frame_id = sm["roadCameraState"].frameId
|
||||
v_ego = max(sm["carState"].vEgo, 0.)
|
||||
if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']:
|
||||
device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32)
|
||||
dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))]
|
||||
model_transform_main = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics if main_wide_camera else dc.fcam.intrinsics, False).astype(np.float32)
|
||||
model_transform_extra = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics, True).astype(np.float32)
|
||||
live_calib_seen = True
|
||||
|
||||
traffic_convention = np.zeros(2)
|
||||
traffic_convention[int(is_rhd)] = 1
|
||||
|
||||
vec_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
|
||||
if desire >= 0 and desire < ModelConstants.DESIRE_LEN:
|
||||
vec_desire[desire] = 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,
|
||||
}
|
||||
|
||||
if "lateral_control_params" in model.inputs.keys():
|
||||
inputs['lateral_control_params'] = np.array([sm["carState"].vEgo, steer_delay], dtype=np.float32)
|
||||
|
||||
# TODO-SP: Below should be good, but I have not tested a model with it so I can't be sure until we test it
|
||||
# if "driving_style" in model.inputs.keys():
|
||||
# inputs['driving_style'] = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], dtype=np.float32)
|
||||
#
|
||||
# if "nav_features" in model.inputs.keys():
|
||||
# inputs['nav_features'] = np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32) # Get size from shape
|
||||
#
|
||||
# if "nav_instructions" in model.inputs.keys():
|
||||
# inputs['nav_instructions'] = np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32) # Get size from shape
|
||||
|
||||
|
||||
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:
|
||||
modelv2_send = messaging.new_message('modelV2')
|
||||
drivingdata_send = messaging.new_message('drivingModelData')
|
||||
posenet_send = messaging.new_message('cameraOdometry')
|
||||
fill_model_msg(drivingdata_send, modelv2_send, model_output, v_ego, steer_delay,
|
||||
publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
|
||||
frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen)
|
||||
|
||||
desire_state = modelv2_send.modelV2.meta.desireState
|
||||
l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
|
||||
r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
|
||||
lane_change_prob = l_lane_change_prob + r_lane_change_prob
|
||||
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
|
||||
modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
|
||||
modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
|
||||
drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state
|
||||
drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction
|
||||
|
||||
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)
|
||||
|
||||
last_vipc_frame_id = meta_main.frame_id
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
import argparse
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument('--demo', action='store_true', help='A boolean for demo mode.')
|
||||
args = parser.parse_args()
|
||||
main(demo=args.demo)
|
||||
except KeyboardInterrupt:
|
||||
cloudlog.warning(f"child {PROCESS_NAME} got SIGINT")
|
||||
except Exception:
|
||||
sentry.capture_exception()
|
||||
raise
|
||||
@@ -1,62 +0,0 @@
|
||||
## Neural networks in openpilot
|
||||
To view the architecture of the ONNX networks, you can use [netron](https://netron.app/)
|
||||
|
||||
## Supercombo
|
||||
### Supercombo input format (Full size: 799906 x float32)
|
||||
* **image stream**
|
||||
* Two consecutive images (256 * 512 * 3 in RGB) recorded at 20 Hz : 393216 = 2 * 6 * 128 * 256
|
||||
* Each 256 * 512 image is represented in YUV420 with 6 channels : 6 * 128 * 256
|
||||
* Channels 0,1,2,3 represent the full-res Y channel and are represented in numpy as Y[::2, ::2], Y[::2, 1::2], Y[1::2, ::2], and Y[1::2, 1::2]
|
||||
* Channel 4 represents the half-res U channel
|
||||
* Channel 5 represents the half-res V channel
|
||||
* **wide image stream**
|
||||
* Two consecutive images (256 * 512 * 3 in RGB) recorded at 20 Hz : 393216 = 2 * 6 * 128 * 256
|
||||
* Each 256 * 512 image is represented in YUV420 with 6 channels : 6 * 128 * 256
|
||||
* Channels 0,1,2,3 represent the full-res Y channel and are represented in numpy as Y[::2, ::2], Y[::2, 1::2], Y[1::2, ::2], and Y[1::2, 1::2]
|
||||
* Channel 4 represents the half-res U channel
|
||||
* Channel 5 represents the half-res V channel
|
||||
* **desire**
|
||||
* one-hot encoded buffer to command model to execute certain actions, bit needs to be sent for the past 5 seconds (at 20FPS) : 100 * 8
|
||||
* **traffic convention**
|
||||
* one-hot encoded vector to tell model whether traffic is right-hand or left-hand traffic : 2
|
||||
* **feature buffer**
|
||||
* A buffer of intermediate features that gets appended to the current feature to form a 5 seconds temporal context (at 20FPS) : 99 * 512
|
||||
|
||||
|
||||
### Supercombo output format (Full size: XXX x float32)
|
||||
Read [here](https://github.com/commaai/openpilot/blob/90af436a121164a51da9fa48d093c29f738adf6a/selfdrive/modeld/models/driving.h#L236) for more.
|
||||
|
||||
|
||||
## Driver Monitoring Model
|
||||
* .onnx model can be run with onnx runtimes
|
||||
* .dlc file is a pre-quantized model and only runs on qualcomm DSPs
|
||||
|
||||
### input format
|
||||
* single image W = 1440 H = 960 luminance channel (Y) from the planar YUV420 format:
|
||||
* full input size is 1440 * 960 = 1382400
|
||||
* normalized ranging from 0.0 to 1.0 in float32 (onnx runner) or ranging from 0 to 255 in uint8 (snpe runner)
|
||||
* camera calibration angles (roll, pitch, yaw) from liveCalibration: 3 x float32 inputs
|
||||
|
||||
### output format
|
||||
* 84 x float32 outputs = 2 + 41 * 2 ([parsing example](https://github.com/commaai/openpilot/blob/22ce4e17ba0d3bfcf37f8255a4dd1dc683fe0c38/selfdrive/modeld/models/dmonitoring.cc#L33))
|
||||
* for each person in the front seats (2 * 41)
|
||||
* face pose: 12 = 6 + 6
|
||||
* face orientation [pitch, yaw, roll] in camera frame: 3
|
||||
* face position [dx, dy] relative to image center: 2
|
||||
* normalized face size: 1
|
||||
* standard deviations for above outputs: 6
|
||||
* face visible probability: 1
|
||||
* eyes: 20 = (8 + 1) + (8 + 1) + 1 + 1
|
||||
* eye position and size, and their standard deviations: 8
|
||||
* eye visible probability: 1
|
||||
* eye closed probability: 1
|
||||
* wearing sunglasses probability: 1
|
||||
* face occluded probability: 1
|
||||
* touching wheel probability: 1
|
||||
* paying attention probability: 1
|
||||
* (deprecated) distracted probabilities: 2
|
||||
* using phone probability: 1
|
||||
* distracted probability: 1
|
||||
* common outputs 2
|
||||
* poor camera vision probability: 1
|
||||
* left hand drive probability: 1
|
||||
@@ -1,69 +0,0 @@
|
||||
#include "sunnypilot/modeld/models/commonmodel.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
|
||||
#include "common/clutil.h"
|
||||
|
||||
DrivingModelFrame::DrivingModelFrame(cl_device_id device_id, cl_context context) : ModelFrame(device_id, context) {
|
||||
input_frames = std::make_unique<uint8_t[]>(buf_size);
|
||||
//input_frames_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, buf_size, NULL, &err));
|
||||
img_buffer_20hz_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, 5*frame_size_bytes, NULL, &err));
|
||||
region.origin = 4 * frame_size_bytes;
|
||||
region.size = frame_size_bytes;
|
||||
last_img_cl = CL_CHECK_ERR(clCreateSubBuffer(img_buffer_20hz_cl, CL_MEM_READ_WRITE, CL_BUFFER_CREATE_TYPE_REGION, ®ion, &err));
|
||||
|
||||
loadyuv_init(&loadyuv, context, device_id, MODEL_WIDTH, MODEL_HEIGHT);
|
||||
init_transform(device_id, context, MODEL_WIDTH, MODEL_HEIGHT);
|
||||
}
|
||||
|
||||
uint8_t* DrivingModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection, cl_mem* output) {
|
||||
run_transform(yuv_cl, MODEL_WIDTH, MODEL_HEIGHT, frame_width, frame_height, frame_stride, frame_uv_offset, projection);
|
||||
|
||||
for (int i = 0; i < 4; i++) {
|
||||
CL_CHECK(clEnqueueCopyBuffer(q, img_buffer_20hz_cl, img_buffer_20hz_cl, (i+1)*frame_size_bytes, i*frame_size_bytes, frame_size_bytes, 0, nullptr, nullptr));
|
||||
}
|
||||
loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, last_img_cl);
|
||||
|
||||
if (output == NULL) {
|
||||
CL_CHECK(clEnqueueReadBuffer(q, img_buffer_20hz_cl, CL_TRUE, 0, frame_size_bytes, &input_frames[0], 0, nullptr, nullptr));
|
||||
CL_CHECK(clEnqueueReadBuffer(q, last_img_cl, CL_TRUE, 0, frame_size_bytes, &input_frames[MODEL_FRAME_SIZE], 0, nullptr, nullptr));
|
||||
clFinish(q);
|
||||
return &input_frames[0];
|
||||
} else {
|
||||
copy_queue(&loadyuv, q, img_buffer_20hz_cl, *output, 0, 0, frame_size_bytes);
|
||||
copy_queue(&loadyuv, q, last_img_cl, *output, 0, frame_size_bytes, frame_size_bytes);
|
||||
|
||||
// NOTE: Since thneed is using a different command queue, this clFinish is needed to ensure the image is ready.
|
||||
clFinish(q);
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
DrivingModelFrame::~DrivingModelFrame() {
|
||||
deinit_transform();
|
||||
loadyuv_destroy(&loadyuv);
|
||||
CL_CHECK(clReleaseMemObject(img_buffer_20hz_cl));
|
||||
CL_CHECK(clReleaseMemObject(last_img_cl));
|
||||
CL_CHECK(clReleaseCommandQueue(q));
|
||||
}
|
||||
|
||||
|
||||
MonitoringModelFrame::MonitoringModelFrame(cl_device_id device_id, cl_context context) : ModelFrame(device_id, context) {
|
||||
input_frames = std::make_unique<uint8_t[]>(buf_size);
|
||||
//input_frame_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, buf_size, NULL, &err));
|
||||
|
||||
init_transform(device_id, context, MODEL_WIDTH, MODEL_HEIGHT);
|
||||
}
|
||||
uint8_t* MonitoringModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection, cl_mem* output) {
|
||||
run_transform(yuv_cl, MODEL_WIDTH, MODEL_HEIGHT, frame_width, frame_height, frame_stride, frame_uv_offset, projection);
|
||||
CL_CHECK(clEnqueueReadBuffer(q, y_cl, CL_TRUE, 0, MODEL_FRAME_SIZE * sizeof(uint8_t), input_frames.get(), 0, nullptr, nullptr));
|
||||
clFinish(q);
|
||||
//return &y_cl;
|
||||
return input_frames.get();
|
||||
}
|
||||
|
||||
MonitoringModelFrame::~MonitoringModelFrame() {
|
||||
deinit_transform();
|
||||
CL_CHECK(clReleaseCommandQueue(q));
|
||||
}
|
||||
@@ -1,98 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <cfloat>
|
||||
#include <cstdlib>
|
||||
#include <cassert>
|
||||
|
||||
#include <memory>
|
||||
|
||||
#define CL_USE_DEPRECATED_OPENCL_1_2_APIS
|
||||
#ifdef __APPLE__
|
||||
#include <OpenCL/cl.h>
|
||||
#else
|
||||
#include <CL/cl.h>
|
||||
#endif
|
||||
|
||||
#include "common/mat.h"
|
||||
#include "selfdrive/modeld/transforms/loadyuv.h"
|
||||
#include "selfdrive/modeld/transforms/transform.h"
|
||||
|
||||
class ModelFrame {
|
||||
public:
|
||||
ModelFrame(cl_device_id device_id, cl_context context) {
|
||||
q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err));
|
||||
}
|
||||
virtual ~ModelFrame() {}
|
||||
virtual uint8_t* prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection, cl_mem* output) { return NULL; }
|
||||
/*
|
||||
uint8_t* buffer_from_cl(cl_mem *in_frames, int buffer_size) {
|
||||
CL_CHECK(clEnqueueReadBuffer(q, *in_frames, CL_TRUE, 0, buffer_size, input_frames.get(), 0, nullptr, nullptr));
|
||||
clFinish(q);
|
||||
return &input_frames[0];
|
||||
}
|
||||
*/
|
||||
|
||||
int MODEL_WIDTH;
|
||||
int MODEL_HEIGHT;
|
||||
int MODEL_FRAME_SIZE;
|
||||
int buf_size;
|
||||
|
||||
protected:
|
||||
cl_mem y_cl, u_cl, v_cl;
|
||||
Transform transform;
|
||||
cl_command_queue q;
|
||||
std::unique_ptr<uint8_t[]> input_frames;
|
||||
|
||||
void init_transform(cl_device_id device_id, cl_context context, int model_width, int model_height) {
|
||||
y_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, model_width * model_height, NULL, &err));
|
||||
u_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, (model_width / 2) * (model_height / 2), NULL, &err));
|
||||
v_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, (model_width / 2) * (model_height / 2), NULL, &err));
|
||||
transform_init(&transform, context, device_id);
|
||||
}
|
||||
|
||||
void deinit_transform() {
|
||||
transform_destroy(&transform);
|
||||
CL_CHECK(clReleaseMemObject(v_cl));
|
||||
CL_CHECK(clReleaseMemObject(u_cl));
|
||||
CL_CHECK(clReleaseMemObject(y_cl));
|
||||
}
|
||||
|
||||
void run_transform(cl_mem yuv_cl, int model_width, int model_height, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection) {
|
||||
transform_queue(&transform, q,
|
||||
yuv_cl, frame_width, frame_height, frame_stride, frame_uv_offset,
|
||||
y_cl, u_cl, v_cl, model_width, model_height, projection);
|
||||
}
|
||||
};
|
||||
|
||||
class DrivingModelFrame : public ModelFrame {
|
||||
public:
|
||||
DrivingModelFrame(cl_device_id device_id, cl_context context);
|
||||
~DrivingModelFrame();
|
||||
uint8_t* prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection, cl_mem* output);
|
||||
|
||||
const int MODEL_WIDTH = 512;
|
||||
const int MODEL_HEIGHT = 256;
|
||||
const int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 / 2;
|
||||
const int buf_size = MODEL_FRAME_SIZE * 2;
|
||||
const size_t frame_size_bytes = MODEL_FRAME_SIZE * sizeof(uint8_t);
|
||||
|
||||
private:
|
||||
LoadYUVState loadyuv;
|
||||
cl_mem img_buffer_20hz_cl, last_img_cl;//, input_frames_cl;
|
||||
cl_buffer_region region;
|
||||
};
|
||||
|
||||
class MonitoringModelFrame : public ModelFrame {
|
||||
public:
|
||||
MonitoringModelFrame(cl_device_id device_id, cl_context context);
|
||||
~MonitoringModelFrame();
|
||||
uint8_t* prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection, cl_mem* output);
|
||||
|
||||
const int MODEL_WIDTH = 1440;
|
||||
const int MODEL_HEIGHT = 960;
|
||||
const int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT;
|
||||
const int buf_size = MODEL_FRAME_SIZE;
|
||||
|
||||
private:
|
||||
// cl_mem input_frame_cl;
|
||||
};
|
||||
@@ -1,26 +0,0 @@
|
||||
# distutils: language = c++
|
||||
|
||||
from msgq.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 "sunnypilot/modeld/models/commonmodel.h":
|
||||
cppclass ModelFrame:
|
||||
int buf_size
|
||||
# unsigned char * buffer_from_cl(cl_mem*, int);
|
||||
unsigned char * prepare(cl_mem, int, int, int, int, mat3, cl_mem*)
|
||||
|
||||
cppclass DrivingModelFrame:
|
||||
int buf_size
|
||||
DrivingModelFrame(cl_device_id, cl_context)
|
||||
|
||||
cppclass MonitoringModelFrame:
|
||||
int buf_size
|
||||
MonitoringModelFrame(cl_device_id, cl_context)
|
||||
@@ -1,13 +0,0 @@
|
||||
# distutils: language = c++
|
||||
|
||||
from msgq.visionipc.visionipc cimport cl_mem
|
||||
from msgq.visionipc.visionipc_pyx cimport CLContext as BaseCLContext
|
||||
|
||||
cdef class CLContext(BaseCLContext):
|
||||
pass
|
||||
|
||||
cdef class CLMem:
|
||||
cdef cl_mem * mem
|
||||
|
||||
@staticmethod
|
||||
cdef create(void*)
|
||||
@@ -1,76 +0,0 @@
|
||||
# distutils: language = c++
|
||||
# cython: c_string_encoding=ascii, language_level=3
|
||||
|
||||
import numpy as np
|
||||
cimport numpy as cnp
|
||||
from libc.string cimport memcpy
|
||||
from libc.stdint cimport uintptr_t
|
||||
|
||||
from msgq.visionipc.visionipc cimport cl_mem
|
||||
from msgq.visionipc.visionipc_pyx cimport VisionBuf, CLContext as BaseCLContext
|
||||
from sunnypilot.modeld.models.commonmodel cimport CL_DEVICE_TYPE_DEFAULT, cl_get_device_id, cl_create_context
|
||||
from sunnypilot.modeld.models.commonmodel cimport mat3, ModelFrame as cppModelFrame, DrivingModelFrame as cppDrivingModelFrame, MonitoringModelFrame as cppMonitoringModelFrame
|
||||
|
||||
|
||||
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
|
||||
|
||||
@property
|
||||
def mem_address(self):
|
||||
return <uintptr_t>(self.mem)
|
||||
|
||||
def cl_from_visionbuf(VisionBuf buf):
|
||||
return CLMem.create(<void*>&buf.buf.buf_cl)
|
||||
|
||||
|
||||
cdef class ModelFrame:
|
||||
cdef cppModelFrame * frame
|
||||
cdef int buf_size
|
||||
|
||||
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 unsigned char * data
|
||||
if output is None:
|
||||
data = self.frame.prepare(buf.buf.buf_cl, buf.width, buf.height, buf.stride, buf.uv_offset, cprojection, NULL)
|
||||
else:
|
||||
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.uint8_t[:self.buf_size]> data)
|
||||
# return CLMem.create(data)
|
||||
|
||||
# def buffer_from_cl(self, CLMem in_frames):
|
||||
# cdef unsigned char * data2
|
||||
# data2 = self.frame.buffer_from_cl(in_frames.mem, self.buf_size)
|
||||
# return np.asarray(<cnp.uint8_t[:self.buf_size]> data2)
|
||||
|
||||
|
||||
cdef class DrivingModelFrame(ModelFrame):
|
||||
cdef cppDrivingModelFrame * _frame
|
||||
|
||||
def __cinit__(self, CLContext context):
|
||||
self._frame = new cppDrivingModelFrame(context.device_id, context.context)
|
||||
self.frame = <cppModelFrame*>(self._frame)
|
||||
self.buf_size = self._frame.buf_size
|
||||
|
||||
cdef class MonitoringModelFrame(ModelFrame):
|
||||
cdef cppMonitoringModelFrame * _frame
|
||||
|
||||
def __cinit__(self, CLContext context):
|
||||
self._frame = new cppMonitoringModelFrame(context.device_id, context.context)
|
||||
self.frame = <cppModelFrame*>(self._frame)
|
||||
self.buf_size = self._frame.buf_size
|
||||
@@ -1,3 +0,0 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:0c896681fd6851de3968433e12f37834429eba265e938cf383200be3e5835cec
|
||||
size 49096168
|
||||
@@ -1,3 +0,0 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:af2cb689ec9e31292f759b561e70e4558a38f778558dff39ccff460ccafc0d52
|
||||
size 49849624
|
||||
@@ -1,3 +0,0 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:441f2865017c07ee0dfb2488c5d86aab00df7ff5c5ec163959f35c33d74b65e6
|
||||
size 594
|
||||
@@ -1,106 +0,0 @@
|
||||
import numpy as np
|
||||
from openpilot.sunnypilot.modeld.constants import ModelConstants
|
||||
|
||||
def safe_exp(x, out=None):
|
||||
# -11 is around 10**14, more causes float16 overflow
|
||||
return np.exp(np.clip(x, -np.inf, 11), out=out)
|
||||
|
||||
def sigmoid(x):
|
||||
return 1. / (1. + safe_exp(-x))
|
||||
|
||||
def softmax(x, axis=-1):
|
||||
x -= np.max(x, axis=axis, keepdims=True)
|
||||
if x.dtype == np.float32 or x.dtype == np.float64:
|
||||
safe_exp(x, out=x)
|
||||
else:
|
||||
x = safe_exp(x)
|
||||
x /= np.sum(x, axis=axis, keepdims=True)
|
||||
return x
|
||||
|
||||
class Parser:
|
||||
def __init__(self, ignore_missing=False):
|
||||
self.ignore_missing = ignore_missing
|
||||
|
||||
def check_missing(self, outs, name):
|
||||
if name not in outs and not self.ignore_missing:
|
||||
raise ValueError(f"Missing output {name}")
|
||||
return name not in outs
|
||||
|
||||
def parse_categorical_crossentropy(self, name, outs, out_shape=None):
|
||||
if self.check_missing(outs, name):
|
||||
return
|
||||
raw = outs[name]
|
||||
if out_shape is not None:
|
||||
raw = raw.reshape((raw.shape[0],) + out_shape)
|
||||
outs[name] = softmax(raw, axis=-1)
|
||||
|
||||
def parse_binary_crossentropy(self, name, outs):
|
||||
if self.check_missing(outs, name):
|
||||
return
|
||||
raw = outs[name]
|
||||
outs[name] = sigmoid(raw)
|
||||
|
||||
def parse_mdn(self, name, outs, in_N=0, out_N=1, out_shape=None):
|
||||
if self.check_missing(outs, name):
|
||||
return
|
||||
raw = outs[name]
|
||||
raw = raw.reshape((raw.shape[0], max(in_N, 1), -1))
|
||||
|
||||
n_values = (raw.shape[2] - out_N)//2
|
||||
pred_mu = raw[:,:,:n_values]
|
||||
pred_std = safe_exp(raw[:,:,n_values: 2*n_values])
|
||||
|
||||
if in_N > 1:
|
||||
weights = np.zeros((raw.shape[0], in_N, out_N), dtype=raw.dtype)
|
||||
for i in range(out_N):
|
||||
weights[:,:,i - out_N] = softmax(raw[:,:,i - out_N], axis=-1)
|
||||
|
||||
if out_N == 1:
|
||||
for fidx in range(weights.shape[0]):
|
||||
idxs = np.argsort(weights[fidx][:,0])[::-1]
|
||||
weights[fidx] = weights[fidx][idxs]
|
||||
pred_mu[fidx] = pred_mu[fidx][idxs]
|
||||
pred_std[fidx] = pred_std[fidx][idxs]
|
||||
full_shape = tuple([raw.shape[0], in_N] + list(out_shape))
|
||||
outs[name + '_weights'] = weights
|
||||
outs[name + '_hypotheses'] = pred_mu.reshape(full_shape)
|
||||
outs[name + '_stds_hypotheses'] = pred_std.reshape(full_shape)
|
||||
|
||||
pred_mu_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype)
|
||||
pred_std_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype)
|
||||
for fidx in range(weights.shape[0]):
|
||||
for hidx in range(out_N):
|
||||
idxs = np.argsort(weights[fidx,:,hidx])[::-1]
|
||||
pred_mu_final[fidx, hidx] = pred_mu[fidx, idxs[0]]
|
||||
pred_std_final[fidx, hidx] = pred_std[fidx, idxs[0]]
|
||||
else:
|
||||
pred_mu_final = pred_mu
|
||||
pred_std_final = pred_std
|
||||
|
||||
if out_N > 1:
|
||||
final_shape = tuple([raw.shape[0], out_N] + list(out_shape))
|
||||
else:
|
||||
final_shape = tuple([raw.shape[0],] + list(out_shape))
|
||||
outs[name] = pred_mu_final.reshape(final_shape)
|
||||
outs[name + '_stds'] = pred_std_final.reshape(final_shape)
|
||||
|
||||
def parse_outputs(self, outs: dict[str, np.ndarray], input_keys: [str]) -> dict[str, np.ndarray]:
|
||||
""" Parse the model outputs into a dictionary of numpy arrays. The input_keys are used to determine how the output should be parsed. """
|
||||
self.parse_mdn('plan', outs, in_N=ModelConstants.PLAN_MHP_N, out_N=ModelConstants.PLAN_MHP_SELECTION,
|
||||
out_shape=(ModelConstants.IDX_N,ModelConstants.PLAN_WIDTH))
|
||||
self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_LANE_LINES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
|
||||
self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
|
||||
self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
|
||||
self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
|
||||
self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
|
||||
self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION,
|
||||
out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH))
|
||||
if 'lat_planner_solution' in outs:
|
||||
self.parse_mdn('lat_planner_solution', outs, in_N=0, out_N=0, out_shape=(ModelConstants.IDX_N,ModelConstants.LAT_PLANNER_SOLUTION_WIDTH))
|
||||
if 'desired_curvature' in outs and "prev_desired_curv" in input_keys:
|
||||
self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(ModelConstants.DESIRED_CURV_WIDTH,))
|
||||
for k in ['lead_prob', 'lane_lines_prob', 'meta']:
|
||||
self.parse_binary_crossentropy(k, outs)
|
||||
self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))
|
||||
self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN,ModelConstants.DESIRE_PRED_WIDTH))
|
||||
return outs
|
||||
@@ -1,36 +0,0 @@
|
||||
import onnx
|
||||
import onnxruntime as ort
|
||||
import numpy as np
|
||||
import itertools
|
||||
|
||||
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(model):
|
||||
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:
|
||||
if i.op_type == 'Cast' and i.attribute[0].i == 10:
|
||||
i.attribute[0].i = 1
|
||||
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 make_onnx_cpu_runner(model_path):
|
||||
options = ort.SessionOptions()
|
||||
options.intra_op_num_threads = 4
|
||||
options.execution_mode = ort.ExecutionMode.ORT_SEQUENTIAL
|
||||
options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_ENABLE_ALL
|
||||
model_data = convert_fp16_to_fp32(onnx.load(model_path))
|
||||
return ort.InferenceSession(model_data, options, providers=['CPUExecutionProvider'])
|
||||
@@ -1,4 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include "sunnypilot/modeld/runners/runmodel.h"
|
||||
#include "sunnypilot/modeld/runners/snpemodel.h"
|
||||
@@ -1,57 +0,0 @@
|
||||
# Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
#
|
||||
# This file is part of sunnypilot and is licensed under the MIT License.
|
||||
# See the LICENSE.md file in the root directory for more details.
|
||||
|
||||
import os
|
||||
import pickle
|
||||
import numpy as np
|
||||
from pathlib import Path
|
||||
from cereal import custom
|
||||
from openpilot.sunnypilot.modeld.runners import ModelRunner
|
||||
from openpilot.sunnypilot.models.helpers import get_active_bundle
|
||||
from openpilot.system.hardware import PC
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
|
||||
USE_ONNX = os.getenv('USE_ONNX', PC)
|
||||
|
||||
CUSTOM_MODEL_PATH = Paths.model_root()
|
||||
METADATA_PATH = Path(__file__).parent / '../models/supercombo_metadata.pkl'
|
||||
|
||||
ModelManager = custom.ModelManagerSP
|
||||
|
||||
|
||||
def load_model():
|
||||
if USE_ONNX:
|
||||
model_paths = {ModelRunner.ONNX: Path(__file__).parent / '../models/supercombo.onnx'}
|
||||
elif bundle := get_active_bundle():
|
||||
drive_model = next(model for model in bundle.models if model.type == ModelManager.Type.drive)
|
||||
model_paths = {ModelRunner.THNEED: f"{CUSTOM_MODEL_PATH}/{drive_model.fileName}"}
|
||||
else:
|
||||
model_paths = {ModelRunner.THNEED: Path(__file__).parent / '../models/supercombo.thneed'}
|
||||
|
||||
return model_paths
|
||||
|
||||
|
||||
def load_metadata():
|
||||
if bundle := get_active_bundle():
|
||||
metadata_model = next(model for model in bundle.models if model.type == ModelManager.Type.metadata)
|
||||
metadata_path = f"{CUSTOM_MODEL_PATH}/{metadata_model.fileName}"
|
||||
else:
|
||||
metadata_path = METADATA_PATH
|
||||
|
||||
with open(metadata_path, 'rb') as f:
|
||||
metadata = pickle.load(f)
|
||||
|
||||
return metadata
|
||||
|
||||
|
||||
def prepare_inputs(model_metadata) -> dict[str, np.ndarray]:
|
||||
# img buffers are managed in openCL transform code so we don't pass them as inputs
|
||||
inputs: dict[str, np.ndarray] = {
|
||||
key: np.zeros(shape, dtype=np.float32).flatten() # Inputs were defined flattened back then
|
||||
for key, shape in model_metadata['input_shapes'].items()
|
||||
if key not in ['input_imgs', 'big_input_imgs']
|
||||
}
|
||||
|
||||
return inputs
|
||||
@@ -1,76 +0,0 @@
|
||||
#include "selfdrive/modeld/transforms/loadyuv.h"
|
||||
|
||||
#include <cassert>
|
||||
#include <cstdio>
|
||||
#include <cstring>
|
||||
|
||||
void loadyuv_init(LoadYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height) {
|
||||
memset(s, 0, sizeof(*s));
|
||||
|
||||
s->width = width;
|
||||
s->height = height;
|
||||
|
||||
char args[1024];
|
||||
snprintf(args, sizeof(args),
|
||||
"-cl-fast-relaxed-math -cl-denorms-are-zero "
|
||||
"-DTRANSFORMED_WIDTH=%d -DTRANSFORMED_HEIGHT=%d",
|
||||
width, height);
|
||||
cl_program prg = cl_program_from_file(ctx, device_id, LOADYUV_PATH, args);
|
||||
|
||||
s->loadys_krnl = CL_CHECK_ERR(clCreateKernel(prg, "loadys", &err));
|
||||
s->loaduv_krnl = CL_CHECK_ERR(clCreateKernel(prg, "loaduv", &err));
|
||||
s->copy_krnl = CL_CHECK_ERR(clCreateKernel(prg, "copy", &err));
|
||||
|
||||
// done with this
|
||||
CL_CHECK(clReleaseProgram(prg));
|
||||
}
|
||||
|
||||
void loadyuv_destroy(LoadYUVState* s) {
|
||||
CL_CHECK(clReleaseKernel(s->loadys_krnl));
|
||||
CL_CHECK(clReleaseKernel(s->loaduv_krnl));
|
||||
CL_CHECK(clReleaseKernel(s->copy_krnl));
|
||||
}
|
||||
|
||||
void loadyuv_queue(LoadYUVState* s, cl_command_queue q,
|
||||
cl_mem y_cl, cl_mem u_cl, cl_mem v_cl,
|
||||
cl_mem out_cl) {
|
||||
cl_int global_out_off = 0;
|
||||
|
||||
CL_CHECK(clSetKernelArg(s->loadys_krnl, 0, sizeof(cl_mem), &y_cl));
|
||||
CL_CHECK(clSetKernelArg(s->loadys_krnl, 1, sizeof(cl_mem), &out_cl));
|
||||
CL_CHECK(clSetKernelArg(s->loadys_krnl, 2, sizeof(cl_int), &global_out_off));
|
||||
|
||||
const size_t loadys_work_size = (s->width*s->height)/8;
|
||||
CL_CHECK(clEnqueueNDRangeKernel(q, s->loadys_krnl, 1, NULL,
|
||||
&loadys_work_size, NULL, 0, 0, NULL));
|
||||
|
||||
const size_t loaduv_work_size = ((s->width/2)*(s->height/2))/8;
|
||||
global_out_off += (s->width*s->height);
|
||||
|
||||
CL_CHECK(clSetKernelArg(s->loaduv_krnl, 0, sizeof(cl_mem), &u_cl));
|
||||
CL_CHECK(clSetKernelArg(s->loaduv_krnl, 1, sizeof(cl_mem), &out_cl));
|
||||
CL_CHECK(clSetKernelArg(s->loaduv_krnl, 2, sizeof(cl_int), &global_out_off));
|
||||
|
||||
CL_CHECK(clEnqueueNDRangeKernel(q, s->loaduv_krnl, 1, NULL,
|
||||
&loaduv_work_size, NULL, 0, 0, NULL));
|
||||
|
||||
global_out_off += (s->width/2)*(s->height/2);
|
||||
|
||||
CL_CHECK(clSetKernelArg(s->loaduv_krnl, 0, sizeof(cl_mem), &v_cl));
|
||||
CL_CHECK(clSetKernelArg(s->loaduv_krnl, 1, sizeof(cl_mem), &out_cl));
|
||||
CL_CHECK(clSetKernelArg(s->loaduv_krnl, 2, sizeof(cl_int), &global_out_off));
|
||||
|
||||
CL_CHECK(clEnqueueNDRangeKernel(q, s->loaduv_krnl, 1, NULL,
|
||||
&loaduv_work_size, NULL, 0, 0, NULL));
|
||||
}
|
||||
|
||||
void copy_queue(LoadYUVState* s, cl_command_queue q, cl_mem src, cl_mem dst,
|
||||
size_t src_offset, size_t dst_offset, size_t size) {
|
||||
CL_CHECK(clSetKernelArg(s->copy_krnl, 0, sizeof(cl_mem), &src));
|
||||
CL_CHECK(clSetKernelArg(s->copy_krnl, 1, sizeof(cl_mem), &dst));
|
||||
CL_CHECK(clSetKernelArg(s->copy_krnl, 2, sizeof(cl_int), &src_offset));
|
||||
CL_CHECK(clSetKernelArg(s->copy_krnl, 3, sizeof(cl_int), &dst_offset));
|
||||
const size_t copy_work_size = size/8;
|
||||
CL_CHECK(clEnqueueNDRangeKernel(q, s->copy_krnl, 1, NULL,
|
||||
©_work_size, NULL, 0, 0, NULL));
|
||||
}
|
||||
@@ -1,47 +0,0 @@
|
||||
#define UV_SIZE ((TRANSFORMED_WIDTH/2)*(TRANSFORMED_HEIGHT/2))
|
||||
|
||||
__kernel void loadys(__global uchar8 const * const Y,
|
||||
__global uchar * out,
|
||||
int out_offset)
|
||||
{
|
||||
const int gid = get_global_id(0);
|
||||
const int ois = gid * 8;
|
||||
const int oy = ois / TRANSFORMED_WIDTH;
|
||||
const int ox = ois % TRANSFORMED_WIDTH;
|
||||
|
||||
const uchar8 ys = Y[gid];
|
||||
|
||||
// 02
|
||||
// 13
|
||||
|
||||
__global uchar* outy0;
|
||||
__global uchar* outy1;
|
||||
if ((oy & 1) == 0) {
|
||||
outy0 = out + out_offset; //y0
|
||||
outy1 = out + out_offset + UV_SIZE*2; //y2
|
||||
} else {
|
||||
outy0 = out + out_offset + UV_SIZE; //y1
|
||||
outy1 = out + out_offset + UV_SIZE*3; //y3
|
||||
}
|
||||
|
||||
vstore4(ys.s0246, 0, outy0 + (oy/2) * (TRANSFORMED_WIDTH/2) + ox/2);
|
||||
vstore4(ys.s1357, 0, outy1 + (oy/2) * (TRANSFORMED_WIDTH/2) + ox/2);
|
||||
}
|
||||
|
||||
__kernel void loaduv(__global uchar8 const * const in,
|
||||
__global uchar8 * out,
|
||||
int out_offset)
|
||||
{
|
||||
const int gid = get_global_id(0);
|
||||
const uchar8 inv = in[gid];
|
||||
out[gid + out_offset / 8] = inv;
|
||||
}
|
||||
|
||||
__kernel void copy(__global uchar8 * in,
|
||||
__global uchar8 * out,
|
||||
int in_offset,
|
||||
int out_offset)
|
||||
{
|
||||
const int gid = get_global_id(0);
|
||||
out[gid + out_offset / 8] = in[gid + in_offset / 8];
|
||||
}
|
||||
@@ -1,20 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include "common/clutil.h"
|
||||
|
||||
typedef struct {
|
||||
int width, height;
|
||||
cl_kernel loadys_krnl, loaduv_krnl, copy_krnl;
|
||||
} LoadYUVState;
|
||||
|
||||
void loadyuv_init(LoadYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height);
|
||||
|
||||
void loadyuv_destroy(LoadYUVState* s);
|
||||
|
||||
void loadyuv_queue(LoadYUVState* s, cl_command_queue q,
|
||||
cl_mem y_cl, cl_mem u_cl, cl_mem v_cl,
|
||||
cl_mem out_cl);
|
||||
|
||||
|
||||
void copy_queue(LoadYUVState* s, cl_command_queue q, cl_mem src, cl_mem dst,
|
||||
size_t src_offset, size_t dst_offset, size_t size);
|
||||
@@ -1,97 +0,0 @@
|
||||
#include "selfdrive/modeld/transforms/transform.h"
|
||||
|
||||
#include <cassert>
|
||||
#include <cstring>
|
||||
|
||||
#include "common/clutil.h"
|
||||
|
||||
void transform_init(Transform* s, cl_context ctx, cl_device_id device_id) {
|
||||
memset(s, 0, sizeof(*s));
|
||||
|
||||
cl_program prg = cl_program_from_file(ctx, device_id, TRANSFORM_PATH, "");
|
||||
s->krnl = CL_CHECK_ERR(clCreateKernel(prg, "warpPerspective", &err));
|
||||
// done with this
|
||||
CL_CHECK(clReleaseProgram(prg));
|
||||
|
||||
s->m_y_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_WRITE, 3*3*sizeof(float), NULL, &err));
|
||||
s->m_uv_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_WRITE, 3*3*sizeof(float), NULL, &err));
|
||||
}
|
||||
|
||||
void transform_destroy(Transform* s) {
|
||||
CL_CHECK(clReleaseMemObject(s->m_y_cl));
|
||||
CL_CHECK(clReleaseMemObject(s->m_uv_cl));
|
||||
CL_CHECK(clReleaseKernel(s->krnl));
|
||||
}
|
||||
|
||||
void transform_queue(Transform* s,
|
||||
cl_command_queue q,
|
||||
cl_mem in_yuv, int in_width, int in_height, int in_stride, int in_uv_offset,
|
||||
cl_mem out_y, cl_mem out_u, cl_mem out_v,
|
||||
int out_width, int out_height,
|
||||
const mat3& projection) {
|
||||
const int zero = 0;
|
||||
|
||||
// sampled using pixel center origin
|
||||
// (because that's how fastcv and opencv does it)
|
||||
|
||||
mat3 projection_y = projection;
|
||||
|
||||
// in and out uv is half the size of y.
|
||||
mat3 projection_uv = transform_scale_buffer(projection, 0.5);
|
||||
|
||||
CL_CHECK(clEnqueueWriteBuffer(q, s->m_y_cl, CL_TRUE, 0, 3*3*sizeof(float), (void*)projection_y.v, 0, NULL, NULL));
|
||||
CL_CHECK(clEnqueueWriteBuffer(q, s->m_uv_cl, CL_TRUE, 0, 3*3*sizeof(float), (void*)projection_uv.v, 0, NULL, NULL));
|
||||
|
||||
const int in_y_width = in_width;
|
||||
const int in_y_height = in_height;
|
||||
const int in_y_px_stride = 1;
|
||||
const int in_uv_width = in_width/2;
|
||||
const int in_uv_height = in_height/2;
|
||||
const int in_uv_px_stride = 2;
|
||||
const int in_u_offset = in_uv_offset;
|
||||
const int in_v_offset = in_uv_offset + 1;
|
||||
|
||||
const int out_y_width = out_width;
|
||||
const int out_y_height = out_height;
|
||||
const int out_uv_width = out_width/2;
|
||||
const int out_uv_height = out_height/2;
|
||||
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 0, sizeof(cl_mem), &in_yuv)); // src
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 1, sizeof(cl_int), &in_stride)); // src_row_stride
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_y_px_stride)); // src_px_stride
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 3, sizeof(cl_int), &zero)); // src_offset
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 4, sizeof(cl_int), &in_y_height)); // src_rows
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 5, sizeof(cl_int), &in_y_width)); // src_cols
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 6, sizeof(cl_mem), &out_y)); // dst
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 7, sizeof(cl_int), &out_y_width)); // dst_row_stride
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 8, sizeof(cl_int), &zero)); // dst_offset
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 9, sizeof(cl_int), &out_y_height)); // dst_rows
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 10, sizeof(cl_int), &out_y_width)); // dst_cols
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 11, sizeof(cl_mem), &s->m_y_cl)); // M
|
||||
|
||||
const size_t work_size_y[2] = {(size_t)out_y_width, (size_t)out_y_height};
|
||||
|
||||
CL_CHECK(clEnqueueNDRangeKernel(q, s->krnl, 2, NULL,
|
||||
(const size_t*)&work_size_y, NULL, 0, 0, NULL));
|
||||
|
||||
const size_t work_size_uv[2] = {(size_t)out_uv_width, (size_t)out_uv_height};
|
||||
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_uv_px_stride)); // src_px_stride
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 3, sizeof(cl_int), &in_u_offset)); // src_offset
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 4, sizeof(cl_int), &in_uv_height)); // src_rows
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 5, sizeof(cl_int), &in_uv_width)); // src_cols
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 6, sizeof(cl_mem), &out_u)); // dst
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 7, sizeof(cl_int), &out_uv_width)); // dst_row_stride
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 8, sizeof(cl_int), &zero)); // dst_offset
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 9, sizeof(cl_int), &out_uv_height)); // dst_rows
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 10, sizeof(cl_int), &out_uv_width)); // dst_cols
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 11, sizeof(cl_mem), &s->m_uv_cl)); // M
|
||||
|
||||
CL_CHECK(clEnqueueNDRangeKernel(q, s->krnl, 2, NULL,
|
||||
(const size_t*)&work_size_uv, NULL, 0, 0, NULL));
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 3, sizeof(cl_int), &in_v_offset)); // src_ofset
|
||||
CL_CHECK(clSetKernelArg(s->krnl, 6, sizeof(cl_mem), &out_v)); // dst
|
||||
|
||||
CL_CHECK(clEnqueueNDRangeKernel(q, s->krnl, 2, NULL,
|
||||
(const size_t*)&work_size_uv, NULL, 0, 0, NULL));
|
||||
}
|
||||
@@ -1,54 +0,0 @@
|
||||
#define INTER_BITS 5
|
||||
#define INTER_TAB_SIZE (1 << INTER_BITS)
|
||||
#define INTER_SCALE 1.f / INTER_TAB_SIZE
|
||||
|
||||
#define INTER_REMAP_COEF_BITS 15
|
||||
#define INTER_REMAP_COEF_SCALE (1 << INTER_REMAP_COEF_BITS)
|
||||
|
||||
__kernel void warpPerspective(__global const uchar * src,
|
||||
int src_row_stride, int src_px_stride, int src_offset, int src_rows, int src_cols,
|
||||
__global uchar * dst,
|
||||
int dst_row_stride, int dst_offset, int dst_rows, int dst_cols,
|
||||
__constant float * M)
|
||||
{
|
||||
int dx = get_global_id(0);
|
||||
int dy = get_global_id(1);
|
||||
|
||||
if (dx < dst_cols && dy < dst_rows)
|
||||
{
|
||||
float X0 = M[0] * dx + M[1] * dy + M[2];
|
||||
float Y0 = M[3] * dx + M[4] * dy + M[5];
|
||||
float W = M[6] * dx + M[7] * dy + M[8];
|
||||
W = W != 0.0f ? INTER_TAB_SIZE / W : 0.0f;
|
||||
int X = rint(X0 * W), Y = rint(Y0 * W);
|
||||
|
||||
int sx = convert_short_sat(X >> INTER_BITS);
|
||||
int sy = convert_short_sat(Y >> INTER_BITS);
|
||||
|
||||
short sx_clamp = clamp(sx, 0, src_cols - 1);
|
||||
short sx_p1_clamp = clamp(sx + 1, 0, src_cols - 1);
|
||||
short sy_clamp = clamp(sy, 0, src_rows - 1);
|
||||
short sy_p1_clamp = clamp(sy + 1, 0, src_rows - 1);
|
||||
int v0 = convert_int(src[mad24(sy_clamp, src_row_stride, src_offset + sx_clamp*src_px_stride)]);
|
||||
int v1 = convert_int(src[mad24(sy_clamp, src_row_stride, src_offset + sx_p1_clamp*src_px_stride)]);
|
||||
int v2 = convert_int(src[mad24(sy_p1_clamp, src_row_stride, src_offset + sx_clamp*src_px_stride)]);
|
||||
int v3 = convert_int(src[mad24(sy_p1_clamp, src_row_stride, src_offset + sx_p1_clamp*src_px_stride)]);
|
||||
|
||||
short ay = (short)(Y & (INTER_TAB_SIZE - 1));
|
||||
short ax = (short)(X & (INTER_TAB_SIZE - 1));
|
||||
float taby = 1.f/INTER_TAB_SIZE*ay;
|
||||
float tabx = 1.f/INTER_TAB_SIZE*ax;
|
||||
|
||||
int dst_index = mad24(dy, dst_row_stride, dst_offset + dx);
|
||||
|
||||
int itab0 = convert_short_sat_rte( (1.0f-taby)*(1.0f-tabx) * INTER_REMAP_COEF_SCALE );
|
||||
int itab1 = convert_short_sat_rte( (1.0f-taby)*tabx * INTER_REMAP_COEF_SCALE );
|
||||
int itab2 = convert_short_sat_rte( taby*(1.0f-tabx) * INTER_REMAP_COEF_SCALE );
|
||||
int itab3 = convert_short_sat_rte( taby*tabx * INTER_REMAP_COEF_SCALE );
|
||||
|
||||
int val = v0 * itab0 + v1 * itab1 + v2 * itab2 + v3 * itab3;
|
||||
|
||||
uchar pix = convert_uchar_sat((val + (1 << (INTER_REMAP_COEF_BITS-1))) >> INTER_REMAP_COEF_BITS);
|
||||
dst[dst_index] = pix;
|
||||
}
|
||||
}
|
||||
@@ -1,25 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#define CL_USE_DEPRECATED_OPENCL_1_2_APIS
|
||||
#ifdef __APPLE__
|
||||
#include <OpenCL/cl.h>
|
||||
#else
|
||||
#include <CL/cl.h>
|
||||
#endif
|
||||
|
||||
#include "common/mat.h"
|
||||
|
||||
typedef struct {
|
||||
cl_kernel krnl;
|
||||
cl_mem m_y_cl, m_uv_cl;
|
||||
} Transform;
|
||||
|
||||
void transform_init(Transform* s, cl_context ctx, cl_device_id device_id);
|
||||
|
||||
void transform_destroy(Transform* transform);
|
||||
|
||||
void transform_queue(Transform* s, cl_command_queue q,
|
||||
cl_mem yuv, int in_width, int in_height, int in_stride, int in_uv_offset,
|
||||
cl_mem out_y, cl_mem out_u, cl_mem out_v,
|
||||
int out_width, int out_height,
|
||||
const mat3& projection);
|
||||
@@ -21,8 +21,7 @@ async def verify_file(file_path: str, expected_hash: str) -> bool:
|
||||
|
||||
return sha256_hash.hexdigest().lower() == expected_hash.lower()
|
||||
|
||||
|
||||
def get_active_bundle(params: Params = None) -> custom.ModelManagerSP.ModelBundle:
|
||||
def get_active_bundle(params: Params) -> custom.ModelManagerSP.ModelBundle:
|
||||
"""Gets the active model bundle from cache"""
|
||||
if params is None:
|
||||
params = Params()
|
||||
@@ -31,23 +30,3 @@ def get_active_bundle(params: Params = None) -> custom.ModelManagerSP.ModelBundl
|
||||
return messaging.log_from_bytes(active_bundle, custom.ModelManagerSP.ModelBundle)
|
||||
|
||||
return None
|
||||
|
||||
|
||||
def get_model_runner_by_filename(filename: str) -> custom.ModelManagerSP.Runner:
|
||||
if filename.endswith(".thneed"):
|
||||
return custom.ModelManagerSP.Runner.snpe
|
||||
|
||||
if filename.endswith("_tinygrad.pkl"):
|
||||
return custom.ModelManagerSP.Runner.tinygrad
|
||||
|
||||
|
||||
def get_active_model_runner(params: Params) -> custom.ModelManagerSP.Runner:
|
||||
"""Gets the model runner from the active model bundle. If no active bundle, returns tinygrad"""
|
||||
if params is None:
|
||||
params = Params()
|
||||
|
||||
if active_bundle := get_active_bundle(params):
|
||||
drive_model = next(model for model in active_bundle.models if model.type == custom.ModelManagerSP.Type.drive)
|
||||
return get_model_runner_by_filename(drive_model.fileName)
|
||||
|
||||
return custom.ModelManagerSP.Runner.tinygrad
|
||||
|
||||
@@ -33,7 +33,7 @@ class Proc:
|
||||
PROCS = [
|
||||
Proc(['camerad'], 1.75, msgs=['roadCameraState', 'wideRoadCameraState', 'driverCameraState']),
|
||||
Proc(['modeld'], 1.12, atol=0.2, msgs=['modelV2']),
|
||||
Proc(['dmonitoringmodeld'], 0.6, msgs=['driverStateV2']),
|
||||
Proc(['dmonitoringmodeld'], 0.65, msgs=['driverStateV2']),
|
||||
Proc(['encoderd'], 0.23, msgs=[]),
|
||||
]
|
||||
|
||||
|
||||
@@ -1,12 +1,10 @@
|
||||
import os
|
||||
import operator
|
||||
|
||||
from cereal import car, custom
|
||||
from cereal import car
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.system.hardware import PC, TICI
|
||||
from openpilot.system.manager.process import PythonProcess, NativeProcess, DaemonProcess
|
||||
|
||||
from sunnypilot.models.helpers import get_active_model_runner
|
||||
from sunnypilot.sunnylink.utils import sunnylink_need_register, sunnylink_ready, use_sunnylink_uploader
|
||||
|
||||
WEBCAM = os.getenv("USE_WEBCAM") is not None
|
||||
@@ -72,15 +70,6 @@ def use_sunnylink_uploader_shim(started, params, CP: car.CarParams) -> bool:
|
||||
"""Shim for use_sunnylink_uploader to match the process manager signature."""
|
||||
return use_sunnylink_uploader(params)
|
||||
|
||||
def is_snpe_model(started, params, CP: car.CarParams) -> bool:
|
||||
"""Check if the active model runner is SNPE."""
|
||||
# TODO-SP: I want to do a little more optimization here to only check this once when we've transitioned from offroad to onroad.
|
||||
return bool(get_active_model_runner(params) == custom.ModelManagerSP.Runner.snpe)
|
||||
|
||||
def is_stock_model(started, params, CP: car.CarParams) -> bool:
|
||||
"""Check if the active model runner is stock."""
|
||||
return not is_snpe_model(started, params, CP)
|
||||
|
||||
def or_(*fns):
|
||||
return lambda *args: operator.or_(*(fn(*args) for fn in fns))
|
||||
|
||||
@@ -97,13 +86,11 @@ procs = [
|
||||
PythonProcess("micd", "system.micd", iscar),
|
||||
PythonProcess("timed", "system.timed", always_run, enabled=not PC),
|
||||
|
||||
# TODO Make python process once TG allows opening QCOM from child proc
|
||||
NativeProcess("dmonitoringmodeld", "selfdrive/modeld", ["./dmonitoringmodeld"], driverview, enabled=(not PC or WEBCAM)),
|
||||
NativeProcess("encoderd", "system/loggerd", ["./encoderd"], only_onroad),
|
||||
NativeProcess("stream_encoderd", "system/loggerd", ["./encoderd", "--stream"], notcar),
|
||||
NativeProcess("loggerd", "system/loggerd", ["./loggerd"], logging),
|
||||
# TODO Make python process once TG allows opening QCOM from child proc
|
||||
NativeProcess("modeld", "selfdrive/modeld", ["./modeld"], and_(only_onroad, is_stock_model)),
|
||||
NativeProcess("modeld", "selfdrive/modeld", ["./modeld"], only_onroad),
|
||||
NativeProcess("sensord", "system/sensord", ["./sensord"], only_onroad, enabled=not PC),
|
||||
NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(5 if not PC else None)),
|
||||
PythonProcess("soundd", "selfdrive.ui.soundd", only_onroad),
|
||||
@@ -145,7 +132,6 @@ procs = [
|
||||
# sunnypilot
|
||||
procs += [
|
||||
PythonProcess("models_manager", "sunnypilot.models.manager", only_offroad),
|
||||
NativeProcess("modeld_snpe", "sunnypilot/modeld", ["./modeld"], and_(only_onroad, is_snpe_model)),
|
||||
]
|
||||
|
||||
if os.path.exists("./github_runner.sh"):
|
||||
|
||||
+1
-1
Submodule tinygrad_repo updated: 480e5e7a12...9dda6d260d
@@ -1234,16 +1234,27 @@ dependencies = [
|
||||
{ name = "sympy" },
|
||||
]
|
||||
wheels = [
|
||||
{ url = "https://files.pythonhosted.org/packages/95/8d/2634e2959b34aa8a0037989f4229e9abcfa484e9c228f99633b3241768a6/onnxruntime-1.20.1-cp311-cp311-macosx_13_0_universal2.whl", hash = "sha256:06bfbf02ca9ab5f28946e0f912a562a5f005301d0c419283dc57b3ed7969bb7b", size = 30998725 },
|
||||
{ url = "https://files.pythonhosted.org/packages/a5/da/c44bf9bd66cd6d9018a921f053f28d819445c4d84b4dd4777271b0fe52a2/onnxruntime-1.20.1-cp311-cp311-manylinux_2_27_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:f6243e34d74423bdd1edf0ae9596dd61023b260f546ee17d701723915f06a9f7", size = 11955227 },
|
||||
{ url = "https://files.pythonhosted.org/packages/11/ac/4120dfb74c8e45cce1c664fc7f7ce010edd587ba67ac41489f7432eb9381/onnxruntime-1.20.1-cp311-cp311-manylinux_2_27_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:5eec64c0269dcdb8d9a9a53dc4d64f87b9e0c19801d9321246a53b7eb5a7d1bc", size = 13331703 },
|
||||
{ url = "https://files.pythonhosted.org/packages/12/f1/cefacac137f7bb7bfba57c50c478150fcd3c54aca72762ac2c05ce0532c1/onnxruntime-1.20.1-cp311-cp311-win32.whl", hash = "sha256:a19bc6e8c70e2485a1725b3d517a2319603acc14c1f1a017dda0afe6d4665b41", size = 9813977 },
|
||||
{ url = "https://files.pythonhosted.org/packages/2c/2d/2d4d202c0bcfb3a4cc2b171abb9328672d7f91d7af9ea52572722c6d8d96/onnxruntime-1.20.1-cp311-cp311-win_amd64.whl", hash = "sha256:8508887eb1c5f9537a4071768723ec7c30c28eb2518a00d0adcd32c89dea3221", size = 11329895 },
|
||||
{ url = "https://files.pythonhosted.org/packages/e5/39/9335e0874f68f7d27103cbffc0e235e32e26759202df6085716375c078bb/onnxruntime-1.20.1-cp312-cp312-macosx_13_0_universal2.whl", hash = "sha256:22b0655e2bf4f2161d52706e31f517a0e54939dc393e92577df51808a7edc8c9", size = 31007580 },
|
||||
{ url = "https://files.pythonhosted.org/packages/c5/9d/a42a84e10f1744dd27c6f2f9280cc3fb98f869dd19b7cd042e391ee2ab61/onnxruntime-1.20.1-cp312-cp312-manylinux_2_27_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:f1f56e898815963d6dc4ee1c35fc6c36506466eff6d16f3cb9848cea4e8c8172", size = 11952833 },
|
||||
{ url = "https://files.pythonhosted.org/packages/47/42/2f71f5680834688a9c81becbe5c5bb996fd33eaed5c66ae0606c3b1d6a02/onnxruntime-1.20.1-cp312-cp312-manylinux_2_27_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:bb71a814f66517a65628c9e4a2bb530a6edd2cd5d87ffa0af0f6f773a027d99e", size = 13333903 },
|
||||
{ url = "https://files.pythonhosted.org/packages/c8/f1/aabfdf91d013320aa2fc46cf43c88ca0182860ff15df872b4552254a9680/onnxruntime-1.20.1-cp312-cp312-win32.whl", hash = "sha256:bd386cc9ee5f686ee8a75ba74037750aca55183085bf1941da8efcfe12d5b120", size = 9814562 },
|
||||
{ url = "https://files.pythonhosted.org/packages/dd/80/76979e0b744307d488c79e41051117634b956612cc731f1028eb17ee7294/onnxruntime-1.20.1-cp312-cp312-win_amd64.whl", hash = "sha256:19c2d843eb074f385e8bbb753a40df780511061a63f9def1b216bf53860223fb", size = 11331482 },
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "onnxruntime-gpu"
|
||||
version = "1.20.1"
|
||||
source = { registry = "https://pypi.org/simple" }
|
||||
dependencies = [
|
||||
{ name = "coloredlogs" },
|
||||
{ name = "flatbuffers" },
|
||||
{ name = "numpy" },
|
||||
{ name = "packaging" },
|
||||
{ name = "protobuf" },
|
||||
{ name = "sympy" },
|
||||
]
|
||||
wheels = [
|
||||
{ url = "https://files.pythonhosted.org/packages/e0/a5/5c2287d61f359c7342e9d59d1e3dd728a982dea85f846c7af305a801c3ca/onnxruntime_gpu-1.20.1-cp311-cp311-manylinux_2_27_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:1795e8bc6f9a1488a4d51d242edc4232a5ae60ec44ab4d4b0a7c65b3d17fcbff", size = 291519550 },
|
||||
{ url = "https://files.pythonhosted.org/packages/91/a8/6984a2fb070be372a866108e3e85c9eb6e8f0378a8567a66967d80befb75/onnxruntime_gpu-1.20.1-cp312-cp312-manylinux_2_27_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:1951f96cd534c6151721e552606d0d792ea6a4c3e57e2f10eed17cca8105e953", size = 291510989 },
|
||||
]
|
||||
|
||||
[[package]]
|
||||
@@ -1280,7 +1291,8 @@ dependencies = [
|
||||
{ name = "libusb1" },
|
||||
{ name = "numpy" },
|
||||
{ name = "onnx" },
|
||||
{ name = "onnxruntime" },
|
||||
{ name = "onnxruntime", marker = "platform_machine == 'aarch64' and platform_system == 'Linux'" },
|
||||
{ name = "onnxruntime-gpu", marker = "platform_machine == 'x86_64' and platform_system == 'Linux'" },
|
||||
{ name = "psutil" },
|
||||
{ name = "pyaudio" },
|
||||
{ name = "pycapnp" },
|
||||
@@ -1378,7 +1390,8 @@ requires-dist = [
|
||||
{ name = "natsort", marker = "extra == 'docs'" },
|
||||
{ name = "numpy", specifier = "<2.0.0" },
|
||||
{ name = "onnx", specifier = ">=1.14.0" },
|
||||
{ name = "onnxruntime", specifier = ">=1.16.3" },
|
||||
{ name = "onnxruntime", marker = "platform_machine == 'aarch64' and platform_system == 'Linux'", specifier = ">=1.16.3" },
|
||||
{ name = "onnxruntime-gpu", marker = "platform_machine == 'x86_64' and platform_system == 'Linux'", specifier = ">=1.16.3" },
|
||||
{ name = "parameterized", marker = "extra == 'dev'", specifier = ">=0.8,<0.9" },
|
||||
{ name = "pre-commit-hooks", marker = "extra == 'testing'" },
|
||||
{ name = "psutil" },
|
||||
@@ -4421,15 +4434,6 @@ wheels = [
|
||||
{ url = "https://files.pythonhosted.org/packages/a7/33/d91e003b85ff7ab227d0fff236d48c18ada2f0cd49d5e35cb514867ba609/PyQt5_sip-12.16.1-cp312-cp312-win_amd64.whl", hash = "sha256:a0f83f554727f43dfe92afbf3a8c51e83bb8b78c5f160b635d4359fad681cebe", size = 57957 },
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "pyreadline3"
|
||||
version = "3.5.4"
|
||||
source = { registry = "https://pypi.org/simple" }
|
||||
sdist = { url = "https://files.pythonhosted.org/packages/0f/49/4cea918a08f02817aabae639e3d0ac046fef9f9180518a3ad394e22da148/pyreadline3-3.5.4.tar.gz", hash = "sha256:8d57d53039a1c75adba8e50dd3d992b28143480816187ea5efbd5c78e6c885b7", size = 99839 }
|
||||
wheels = [
|
||||
{ url = "https://files.pythonhosted.org/packages/5a/dc/491b7661614ab97483abf2056be1deee4dc2490ecbf7bff9ab5cdbac86e1/pyreadline3-3.5.4-py3-none-any.whl", hash = "sha256:eaf8e6cc3c49bcccf145fc6067ba8643d1df34d604a1ec0eccbf7a18e6d3fae6", size = 83178 },
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "pyrect"
|
||||
version = "0.2.0"
|
||||
|
||||
Reference in New Issue
Block a user