Skip to content

Commit

Permalink
Sync: commaai/openpilot:master into `sunnypilot/sunnypilot:master-n…
Browse files Browse the repository at this point in the history
…ew` (commaai#488)
  • Loading branch information
sunnyhaibin authored Dec 19, 2024
2 parents cf30110 + 7592669 commit cf74e64
Show file tree
Hide file tree
Showing 142 changed files with 265 additions and 1,554 deletions.
2 changes: 1 addition & 1 deletion README.md
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ We have detailed instructions for [how to install the harness and device in a ca
### Branches
| branch | URL | description |
|------------------|----------------------------------------|-------------------------------------------------------------------------------------|
| `release3` | openilot.comma.ai | This is openpilot's release branch. |
| `release3` | openpilot.comma.ai | This is openpilot's release branch. |
| `release3-staging` | openpilot-test.comma.ai | This is the staging branch for releases. Use it to get new releases slightly early. |
| `nightly` | openpilot-nightly.comma.ai | This is the bleeding edge development branch. Do not expect this to be stable. |
| `nightly-dev` | installer.comma.ai/commaai/nightly-dev | Same as nightly, but includes experimental development features for some cars. |
Expand Down
2 changes: 1 addition & 1 deletion msgq_repo
2 changes: 1 addition & 1 deletion panda
Submodule panda updated from 45301b to 0d4b79
3 changes: 1 addition & 2 deletions pyproject.toml
Original file line number Diff line number Diff line change
Expand Up @@ -42,8 +42,7 @@ dependencies = [

# modeld
"onnx >= 1.14.0",
"onnxruntime >=1.16.3; platform_system == 'Linux' and platform_machine == 'aarch64'",
"onnxruntime-gpu >=1.16.3; platform_system == 'Linux' and platform_machine == 'x86_64'",
"onnxruntime >=1.16.3",

# logging
"pyzmq",
Expand Down
2 changes: 1 addition & 1 deletion release/release_files.py
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@
"tools/joystick/",
"tools/longitudinal_maneuvers/",

"tinygrad_repo/openpilot/compile2.py",
"tinygrad_repo/examples/openpilot/compile3.py",
"tinygrad_repo/extra/onnx.py",
"tinygrad_repo/extra/onnx_ops.py",
"tinygrad_repo/extra/thneed.py",
Expand Down
24 changes: 11 additions & 13 deletions selfdrive/controls/lib/longitudinal_planner.py
Original file line number Diff line number Diff line change
Expand Up @@ -50,24 +50,20 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
return [a_target[0], min(a_target[1], a_x_allowed)]


def get_accel_from_plan(CP, speeds, accels):
def get_accel_from_plan(speeds, accels, action_t=DT_MDL, vEgoStopping=0.05):
if len(speeds) == CONTROL_N:
v_target_now = interp(DT_MDL, CONTROL_N_T_IDX, speeds)
a_target_now = interp(DT_MDL, CONTROL_N_T_IDX, accels)
v_now = speeds[0]
a_now = accels[0]

v_target = interp(CP.longitudinalActuatorDelay + DT_MDL, CONTROL_N_T_IDX, speeds)
if v_target != v_target_now:
a_target = 2 * (v_target - v_target_now) / CP.longitudinalActuatorDelay - a_target_now
else:
a_target = a_target_now

v_target_1sec = interp(CP.longitudinalActuatorDelay + DT_MDL + 1.0, CONTROL_N_T_IDX, speeds)
v_target = interp(action_t, CONTROL_N_T_IDX, speeds)
a_target = 2 * (v_target - v_now) / (action_t) - a_now
v_target_1sec = interp(action_t + 1.0, CONTROL_N_T_IDX, speeds)
else:
v_target = 0.0
v_target_1sec = 0.0
a_target = 0.0
should_stop = (v_target < CP.vEgoStopping and
v_target_1sec < CP.vEgoStopping)
should_stop = (v_target < vEgoStopping and
v_target_1sec < vEgoStopping)
return a_target, should_stop


Expand Down Expand Up @@ -201,7 +197,9 @@ def publish(self, sm, pm):
longitudinalPlan.longitudinalPlanSource = self.mpc.source
longitudinalPlan.fcw = self.fcw

a_target, should_stop = get_accel_from_plan(self.CP, longitudinalPlan.speeds, longitudinalPlan.accels)
action_t = self.CP.longitudinalActuatorDelay + DT_MDL
a_target, should_stop = get_accel_from_plan(longitudinalPlan.speeds, longitudinalPlan.accels,
action_t=action_t, vEgoStopping=self.CP.vEgoStopping)
longitudinalPlan.aTarget = a_target
longitudinalPlan.shouldStop = should_stop
longitudinalPlan.allowBrake = True
Expand Down
6 changes: 6 additions & 0 deletions selfdrive/debug/touch_replay.py
Original file line number Diff line number Diff line change
Expand Up @@ -38,11 +38,17 @@
if fingers[current_slot][1] != -1:
touch_points.append(fingers[current_slot].copy())

if not touch_points:
print(f'No touch events found for {route}')
quit()

unique_points, counts = np.unique(touch_points, axis=0, return_counts=True)

plt.figure(figsize=(10, 3))
plt.scatter(unique_points[:, 0], unique_points[:, 1], c=counts, s=counts * 20, edgecolors='red')
plt.colorbar()
plt.title(f'Touches for {route}')
plt.xlim(0, w)
plt.ylim(0, h)
plt.grid(True)
plt.show()
46 changes: 11 additions & 35 deletions selfdrive/modeld/SConscript
Original file line number Diff line number Diff line change
Expand Up @@ -13,20 +13,6 @@ 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":
Expand All @@ -45,34 +31,24 @@ 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)]
tinygrad_files = ["#"+x for x in glob.glob(env.Dir("#tinygrad_repo").relpath + "/**", recursive=True, root_dir=env.Dir("#").abspath) if 'pycache' not in x]

# Get model metadata
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)

# 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"

lenv.Command(fn + ".thneed", [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'

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)
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)

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'])
6 changes: 0 additions & 6 deletions selfdrive/modeld/dmonitoringmodeld
Original file line number Diff line number Diff line change
@@ -1,10 +1,4 @@
#!/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" "$@"
55 changes: 38 additions & 17 deletions selfdrive/modeld/dmonitoringmodeld.py
Original file line number Diff line number Diff line change
@@ -1,8 +1,17 @@
#!/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
Expand All @@ -13,21 +22,20 @@
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
from openpilot.common.transformations.model import dmonitoringmodel_intrinsics, DM_INPUT_SIZE
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_PATHS = {
ModelRunner.THNEED: Path(__file__).parent / 'models/dmonitoring_model.thneed',
ModelRunner.ONNX: Path(__file__).parent / 'models/dmonitoring_model.onnx'}
MODEL_PATH = Path(__file__).parent / 'models/dmonitoring_model.onnx'
MODEL_PKL_PATH = Path(__file__).parent / 'models/dmonitoring_model_tinygrad.pkl'

class DriverStateResult(ctypes.Structure):
_fields_ = [
Expand Down Expand Up @@ -58,29 +66,42 @@ 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.output = np.zeros(OUTPUT_SIZE, dtype=np.float32)
self.inputs = {
'calib': np.zeros(CALIB_LEN, dtype=np.float32)}
self.numpy_inputs = {
'calib': np.zeros((1, CALIB_LEN), dtype=np.float32),
}

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'])
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)

def run(self, buf:VisionBuf, calib:np.ndarray, transform:np.ndarray) -> tuple[np.ndarray, float]:
self.inputs['calib'][:] = calib

self.model.setInputBuffer("input_img", self.frame.prepare(buf, transform.flatten(), None).view(np.float32))
self.numpy_inputs['calib'][0,:] = calib

t1 = time.perf_counter()
self.model.execute()

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()

t2 = time.perf_counter()
return self.output, t2 - t1
return output, t2 - t1


def fill_driver_state(msg, ds_result: DriverStateResult):
Expand Down
6 changes: 0 additions & 6 deletions selfdrive/modeld/modeld
Original file line number Diff line number Diff line change
@@ -1,10 +1,4 @@
#!/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" "$@"
Loading

0 comments on commit cf74e64

Please sign in to comment.