From 554cf9ca4a8f9127a90427bc023d06ccfe7b2652 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Sat, 25 Apr 2026 23:28:02 -0700 Subject: [PATCH 01/14] modeld: sync tinygrad --- tinygrad_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tinygrad_repo b/tinygrad_repo index 3501a71478..62ff96ba1e 160000 --- a/tinygrad_repo +++ b/tinygrad_repo @@ -1 +1 @@ -Subproject commit 3501a714785ff370cffb966a45d5f9cdf6c9ea7a +Subproject commit 62ff96ba1ebf1bea087808194b94bf25484597cb From 2898f394dd21cbd8d76780fc2d97f10f0dc8fb09 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Sat, 25 Apr 2026 23:38:31 -0700 Subject: [PATCH 02/14] bump --- tinygrad_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tinygrad_repo b/tinygrad_repo index 62ff96ba1e..8164a9c46e 160000 --- a/tinygrad_repo +++ b/tinygrad_repo @@ -1 +1 @@ -Subproject commit 62ff96ba1ebf1bea087808194b94bf25484597cb +Subproject commit 8164a9c46e4158db8a7b3b75da57d95f768591fa From 88b30e199b7e6f0967e5ca18490b8b4c3d1828fd Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Sat, 25 Apr 2026 23:47:26 -0700 Subject: [PATCH 03/14] fix to new tg --- .github/workflows/sunnypilot-build-model.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/sunnypilot-build-model.yaml b/.github/workflows/sunnypilot-build-model.yaml index ff09489b92..d7c70d886c 100644 --- a/.github/workflows/sunnypilot-build-model.yaml +++ b/.github/workflows/sunnypilot-build-model.yaml @@ -172,8 +172,8 @@ jobs: output_file="${{ env.MODELS_DIR }}/${base_name}_tinygrad.pkl" echo "Compiling: $onnx_file -> $output_file" - QCOM=1 python3 "${{ env.TINYGRAD_PATH }}/examples/openpilot/compile3.py" "$onnx_file" "$output_file" - DEV=QCOM FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 python3 "${{ env.MODELS_DIR }}/../get_model_metadata.py" "$onnx_file" || true + DEV=QCOM FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1 IMAGE=2 python3 "${{ env.TINYGRAD_PATH }}/examples/openpilot/compile3.py" "$onnx_file" "$output_file" + DEV=QCOM FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1 python3 "${{ env.MODELS_DIR }}/../get_model_metadata.py" "$onnx_file" || true done - name: Validate Model Outputs From f7511491f7c5a38903a85fa690b403389e341b51 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Sun, 26 Apr 2026 00:27:47 -0700 Subject: [PATCH 04/14] sync new modeld changes --- selfdrive/modeld/SConscript | 64 ++++-- selfdrive/modeld/compile_dm_warp.py | 54 +++++ selfdrive/modeld/compile_modeld.py | 253 ++++++++++++++++++++++++ selfdrive/modeld/compile_warp.py | 201 ------------------- selfdrive/modeld/get_model_metadata.py | 6 +- selfdrive/modeld/helpers.py | 31 +++ selfdrive/modeld/modeld.py | 157 ++++----------- selfdrive/modeld/tinygrad_helpers.py | 12 -- sunnypilot/modeld_v2/SConscript | 6 +- sunnypilot/modeld_v2/tests/test_warp.py | 7 +- sunnypilot/modeld_v2/warp.py | 59 ++++-- 11 files changed, 480 insertions(+), 370 deletions(-) create mode 100644 selfdrive/modeld/compile_dm_warp.py create mode 100644 selfdrive/modeld/compile_modeld.py delete mode 100755 selfdrive/modeld/compile_warp.py create mode 100644 selfdrive/modeld/helpers.py delete mode 100644 selfdrive/modeld/tinygrad_helpers.py diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index 544fcafc77..3c8581e898 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -1,10 +1,23 @@ import glob import json import os +from itertools import product from SCons.Script import Value from openpilot.common.file_chunker import chunk_file, get_chunk_paths +from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye +from openpilot.common.transformations.model import MEDMODEL_INPUT_SIZE, DM_INPUT_SIZE +from openpilot.selfdrive.modeld.constants import ModelConstants +from openpilot.selfdrive.modeld.helpers import CompileConfig from tinygrad import Device +CAMERA_CONFIGS = [ + (_ar_ox_fisheye.width, _ar_ox_fisheye.height), # tici: 1928x1208 + (_os_fisheye.width, _os_fisheye.height), # mici: 1344x760 +] +MODELD_CONFIGS = [CompileConfig(cam_w, cam_h, prepare_only, 'driving_') + for (cam_w, cam_h), prepare_only in product(CAMERA_CONFIGS, [True, False])] +DM_WARP_CONFIGS = [CompileConfig(cam_w, cam_h, True, 'dm_') for cam_w, cam_h in CAMERA_CONFIGS] + Import('env', 'arch') chunker_file = File("#common/file_chunker.py") lenv = env.Clone() @@ -16,18 +29,17 @@ tinygrad_files = ["#"+x for x in glob.glob(env.Dir("#tinygrad_repo").relpath + " def estimate_pickle_max_size(onnx_size): return 1.2 * onnx_size + 10 * 1024 * 1024 # 20% + 10MB is plenty -# THREADS=0 is need to prevent bug: https://github.com/tinygrad/tinygrad/issues/14689 # get fastest TG config available = set(Device.get_available_devices()) -# FIXME-SP: reset when we bump tg -if False: # 'CUDA' in available: +if 'CUDA' in available: tg_backend = 'CUDA' tg_flags = f'DEV={tg_backend}' elif 'QCOM' in available: tg_backend = 'QCOM' - tg_flags = f'DEV={tg_backend} FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0' + tg_flags = f'DEV={tg_backend} FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1' else: - tg_backend = 'CPU' if arch == 'Darwin' else 'CPU CPU_LLVM=1' # FIXME-SP: reset when we bump tg + tg_backend = 'CPU' if arch == 'Darwin' else 'CPU:LLVM' + # THREADS=0 is need to prevent bug: https://github.com/tinygrad/tinygrad/issues/14689 tg_flags = f'DEV={tg_backend} THREADS=0' def write_tg_compiled_flags(target, source, env): @@ -54,14 +66,35 @@ for model_name in ['driving_vision', 'driving_policy', 'dmonitoring_model']: image_flag = { 'larch64': 'IMAGE=2', }.get(arch, 'IMAGE=0') -script_files = [File(Dir("#selfdrive/modeld").File("compile_warp.py").abspath)] -compile_warp_cmd = f'{tg_flags} {mac_brew_string} python3 {Dir("#selfdrive/modeld").abspath}/compile_warp.py ' -from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye -warp_targets = [] -for cam in [_ar_ox_fisheye, _os_fisheye]: - w, h = cam.width, cam.height - warp_targets += [File(f"models/warp_{w}x{h}_tinygrad.pkl").abspath, File(f"models/dm_warp_{w}x{h}_tinygrad.pkl").abspath] -lenv.Command(warp_targets, tinygrad_files + script_files + [compiled_flags_node], compile_warp_cmd) +modeld_dir = Dir("#selfdrive/modeld").abspath +compile_modeld_script = [File(f"{modeld_dir}/compile_modeld.py")] +compile_dm_warp_script = [File(f"{modeld_dir}/compile_dm_warp.py")] +driving_onnx_deps = [File(f"models/{m}.onnx").abspath for m in ['driving_vision', 'driving_policy']] +driving_metadata_deps = [File(f"models/{m}_metadata.pkl").abspath for m in ['driving_vision', 'driving_policy']] + +model_w, model_h = MEDMODEL_INPUT_SIZE +frame_skip = ModelConstants.MODEL_RUN_FREQ // ModelConstants.MODEL_CONTEXT_FREQ +for cfg in MODELD_CONFIGS: + cmd = (f'{tg_flags} {mac_brew_string} {image_flag} python3 {modeld_dir}/compile_modeld.py ' + f'--model-size {model_w}x{model_h} ' + f'--nv12 {",".join(str(x) for x in cfg.nv12)} ' + f'--vision-onnx {File("models/driving_vision.onnx").abspath} ' + f'--policy-onnx {File("models/driving_policy.onnx").abspath} ' + f'--output {cfg.pkl_path} --frame-skip {frame_skip}' + + (' --prepare-only' if cfg.prepare_only else '')) + node = lenv.Command(cfg.pkl_path, tinygrad_files + compile_modeld_script + driving_onnx_deps + driving_metadata_deps + [chunker_file, compiled_flags_node], cmd) + onnx_sizes_sum = sum(os.path.getsize(f) for f in driving_onnx_deps) + chunk_targets = get_chunk_paths(cfg.pkl_path, estimate_pickle_max_size(onnx_sizes_sum)) + def do_chunk(target, source, env, pkl=cfg.pkl_path, chunks=chunk_targets): + chunk_file(pkl, chunks) + lenv.Command(chunk_targets, node, do_chunk) + +dm_w, dm_h = DM_INPUT_SIZE +for cfg in DM_WARP_CONFIGS: + cmd = (f'{tg_flags} {mac_brew_string} {image_flag} python3 {modeld_dir}/compile_dm_warp.py ' + f'--nv12 {",".join(str(x) for x in cfg.nv12)} --warp-to {dm_w}x{dm_h} ' + f'--output {cfg.pkl_path}') + lenv.Command(cfg.pkl_path, tinygrad_files + compile_dm_warp_script + compile_modeld_script + [compiled_flags_node], cmd) def tg_compile(flags, model_name): pythonpath_string = 'PYTHONPATH="${PYTHONPATH}:' + env.Dir("#tinygrad_repo").abspath + '"' @@ -82,7 +115,4 @@ def tg_compile(flags, model_name): do_chunk, ) -# Compile small models -for model_name in ['driving_vision', 'driving_policy', 'dmonitoring_model']: - tg_compile(tg_flags, model_name) - +tg_compile(tg_flags, 'dmonitoring_model') \ No newline at end of file diff --git a/selfdrive/modeld/compile_dm_warp.py b/selfdrive/modeld/compile_dm_warp.py new file mode 100644 index 0000000000..54f1c60c40 --- /dev/null +++ b/selfdrive/modeld/compile_dm_warp.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python3 +import argparse +import pickle +import time + +from tinygrad.tensor import Tensor +from tinygrad.device import Device +from tinygrad.engine.jit import TinyJit + +from openpilot.selfdrive.modeld.compile_modeld import NV12Frame, warp_perspective_tinygrad, _parse_size, _parse_nv12 + + +def make_warp_dm(nv12: NV12Frame, dm_w, dm_h): + cam_w, cam_h, stride, _, _, _ = nv12 + stride_pad = stride - cam_w + + def warp_dm(input_frame, M_inv): + M_inv = M_inv.to(Device.DEFAULT).realize() + return warp_perspective_tinygrad(input_frame[:cam_h*stride], M_inv, + (dm_w, dm_h), (cam_h, cam_w), stride_pad).reshape(-1, dm_h * dm_w) + return warp_dm + + +def compile_dm_warp(nv12: NV12Frame, dm_w, dm_h, pkl_path): + print(f"Compiling DM warp for {nv12.width}x{nv12.height} -> {dm_w}x{dm_h}...") + + warp_dm_jit = TinyJit(make_warp_dm(nv12, dm_w, dm_h), prune=True) + + for i in range(10): + frame = Tensor.randint(nv12.size, low=0, high=256, dtype='uint8').realize() + M_inv = Tensor(Tensor.randn(3, 3).mul(8).realize().numpy(), device='NPY') + Device.default.synchronize() + st = time.perf_counter() + warp_dm_jit(frame, M_inv).realize() + mt = time.perf_counter() + Device.default.synchronize() + et = time.perf_counter() + print(f" [{i+1}/10] enqueue {(mt-st)*1e3:6.2f} ms -- total {(et-st)*1e3:6.2f} ms") + + with open(pkl_path, "wb") as f: + pickle.dump(warp_dm_jit, f) + print(f" Saved to {pkl_path}") + + +if __name__ == "__main__": + p = argparse.ArgumentParser() + p.add_argument('--nv12', type=_parse_nv12, required=True, + help=f'NV12 frame layout: {",".join(NV12Frame._fields)}') + p.add_argument('--warp-to', type=_parse_size, required=True, help='DM input WxH') + p.add_argument('--output', required=True) + args = p.parse_args() + + dm_w, dm_h = args.warp_to + compile_dm_warp(args.nv12, dm_w, dm_h, args.output) \ No newline at end of file diff --git a/selfdrive/modeld/compile_modeld.py b/selfdrive/modeld/compile_modeld.py new file mode 100644 index 0000000000..d5f4417edc --- /dev/null +++ b/selfdrive/modeld/compile_modeld.py @@ -0,0 +1,253 @@ +#!/usr/bin/env python3 +import argparse +import pickle +import time +from functools import partial +from collections import namedtuple + +import numpy as np +from tinygrad.tensor import Tensor +from tinygrad.helpers import Context +from tinygrad.device import Device +from tinygrad.engine.jit import TinyJit +from tinygrad.nn.onnx import OnnxRunner + +# https://github.com/tinygrad/tinygrad/issues/15682 +from tinygrad.uop.ops import UOp, Ops +_orig = UOp.__reduce__ +UOp.__reduce__ = lambda self: (UOp.unique, ()) if self.op is Ops.UNIQUE else _orig(self) + + +NV12Frame = namedtuple("NV12Frame", ['width', 'height', 'stride', 'y_height', 'uv_height', 'size']) + +UV_SCALE_MATRIX = np.array([[0.5, 0, 0], [0, 0.5, 0], [0, 0, 1]], dtype=np.float32) +UV_SCALE_MATRIX_INV = np.linalg.inv(UV_SCALE_MATRIX) + + +def warp_perspective_tinygrad(src_flat, M_inv, dst_shape, src_shape, stride_pad): + w_dst, h_dst = dst_shape + h_src, w_src = src_shape + + x = Tensor.arange(w_dst).reshape(1, w_dst).expand(h_dst, w_dst).reshape(-1) + y = Tensor.arange(h_dst).reshape(h_dst, 1).expand(h_dst, w_dst).reshape(-1) + + # inline 3x3 matmul as elementwise to avoid reduce op (enables fusion with gather) + src_x = M_inv[0, 0] * x + M_inv[0, 1] * y + M_inv[0, 2] + src_y = M_inv[1, 0] * x + M_inv[1, 1] * y + M_inv[1, 2] + src_w = M_inv[2, 0] * x + M_inv[2, 1] * y + M_inv[2, 2] + + src_x = src_x / src_w + src_y = src_y / src_w + + x_nn_clipped = Tensor.round(src_x).clip(0, w_src - 1).cast('int') + y_nn_clipped = Tensor.round(src_y).clip(0, h_src - 1).cast('int') + idx = y_nn_clipped * (w_src + stride_pad) + x_nn_clipped + + return src_flat[idx] + + +def frames_to_tensor(frames): + H = (frames.shape[0] * 2) // 3 + W = frames.shape[1] + in_img1 = Tensor.cat(frames[0:H:2, 0::2], + frames[1:H:2, 0::2], + frames[0:H:2, 1::2], + frames[1:H:2, 1::2], + frames[H:H+H//4].reshape((H//2, W//2)), + frames[H+H//4:H+H//2].reshape((H//2, W//2)), dim=0).reshape((6, H//2, W//2)) + return in_img1 + + +def make_frame_prepare(nv12: NV12Frame, model_w, model_h): + cam_w, cam_h, stride, y_height, uv_height, _ = nv12 + uv_offset = stride * y_height + stride_pad = stride - cam_w + + def frame_prepare_tinygrad(input_frame, M_inv): + # UV_SCALE @ M_inv @ UV_SCALE_INV simplifies to elementwise scaling + M_inv_uv = M_inv * Tensor([[1.0, 1.0, 0.5], [1.0, 1.0, 0.5], [2.0, 2.0, 1.0]]) + # deinterleave NV12 UV plane (UVUV... -> separate U, V) + uv = input_frame[uv_offset:uv_offset + uv_height * stride].reshape(uv_height, stride) + with Context(SPLIT_REDUCEOP=0): + y = warp_perspective_tinygrad(input_frame[:cam_h*stride], + M_inv, (model_w, model_h), + (cam_h, cam_w), stride_pad).realize() + u = warp_perspective_tinygrad(uv[:cam_h//2, :cam_w:2].flatten(), + M_inv_uv, (model_w//2, model_h//2), + (cam_h//2, cam_w//2), 0).realize() + v = warp_perspective_tinygrad(uv[:cam_h//2, 1:cam_w:2].flatten(), + M_inv_uv, (model_w//2, model_h//2), + (cam_h//2, cam_w//2), 0).realize() + yuv = y.cat(u).cat(v).reshape((model_h * 3 // 2, model_w)) + tensor = frames_to_tensor(yuv) + return tensor + return frame_prepare_tinygrad + + +def make_input_queues(vision_input_shapes, policy_input_shapes, frame_skip): + img = vision_input_shapes['img'] # (1, 12, 128, 256) + n_frames = img[1] // 6 + img_buf_shape = (frame_skip * (n_frames - 1) + 1, 6, img[2], img[3]) + + fb = policy_input_shapes['features_buffer'] # (1, 25, 512) + dp = policy_input_shapes['desire_pulse'] # (1, 25, 8) + tc = policy_input_shapes['traffic_convention'] # (1, 2) + + npy = { + 'desire': np.zeros(dp[2], dtype=np.float32), + 'traffic_convention': np.zeros(tc, dtype=np.float32), + 'tfm': np.zeros((3, 3), dtype=np.float32), + 'big_tfm': np.zeros((3, 3), dtype=np.float32), + } + input_queues = { + 'img_q': Tensor.zeros(img_buf_shape, dtype='uint8').contiguous().realize(), + 'big_img_q': Tensor.zeros(img_buf_shape, dtype='uint8').contiguous().realize(), + 'feat_q': Tensor.zeros(frame_skip * (fb[1] - 1) + 1, fb[0], fb[2]).contiguous().realize(), + 'desire_q': Tensor.zeros(frame_skip * dp[1], dp[0], dp[2]).contiguous().realize(), + **{k: Tensor(v, device='NPY').realize() for k, v in npy.items()}, + } + return input_queues, npy + + +def shift_and_sample(buf, new_val, sample_fn): + buf.assign(buf[1:].cat(new_val, dim=0).contiguous()) + return sample_fn(buf) + + +def sample_skip(buf, frame_skip): + return buf[::frame_skip].contiguous().flatten(0, 1).unsqueeze(0) + + +def sample_desire(buf, frame_skip): + return buf.reshape(-1, frame_skip, *buf.shape[1:]).max(1).flatten(0, 1).unsqueeze(0) + + +def make_run_policy(vision_runner, policy_runner, nv12: NV12Frame, model_w, model_h, + vision_features_slice, frame_skip, prepare_only=False): + frame_prepare = make_frame_prepare(nv12, model_w, model_h) + sample_skip_fn = partial(sample_skip, frame_skip=frame_skip) + sample_desire_fn = partial(sample_desire, frame_skip=frame_skip) + + def run_policy(img_q, big_img_q, feat_q, desire_q, desire, traffic_convention, tfm, big_tfm, frame, big_frame): + tfm = tfm.to(Device.DEFAULT) + big_tfm = big_tfm.to(Device.DEFAULT) + desire = desire.to(Device.DEFAULT) + traffic_convention = traffic_convention.to(Device.DEFAULT) + Tensor.realize(tfm, big_tfm, desire, traffic_convention) + + img = shift_and_sample(img_q, frame_prepare(frame, tfm).unsqueeze(0), sample_skip_fn) + big_img = shift_and_sample(big_img_q, frame_prepare(big_frame, big_tfm).unsqueeze(0), sample_skip_fn) + + if prepare_only: + return img, big_img + + vision_out = next(iter(vision_runner({'img': img, 'big_img': big_img}).values())).cast('float32') + + new_feat = vision_out[:, vision_features_slice].reshape(1, -1).unsqueeze(0) + feat_buf = shift_and_sample(feat_q, new_feat, sample_skip_fn) + desire_buf = shift_and_sample(desire_q, desire.reshape(1, 1, -1), sample_desire_fn) + + inputs = {'features_buffer': feat_buf, 'desire_pulse': desire_buf, 'traffic_convention': traffic_convention} + policy_out = next(iter(policy_runner(inputs).values())).cast('float32') + + return vision_out, policy_out + return run_policy + + +def compile_modeld(nv12: NV12Frame, model_w, model_h, prepare_only, frame_skip, + vision_onnx, policy_onnx, pkl_path): + from get_model_metadata import metadata_path_for + + print(f"Compiling combined policy JIT for {nv12.width}x{nv12.height} (prepare_only={prepare_only})...") + + vision_runner = OnnxRunner(vision_onnx) + policy_runner = OnnxRunner(policy_onnx) + + with open(metadata_path_for(vision_onnx), 'rb') as f: + vision_metadata = pickle.load(f) + vision_features_slice = vision_metadata['output_slices']['hidden_state'] + vision_input_shapes = vision_metadata['input_shapes'] + with open(metadata_path_for(policy_onnx), 'rb') as f: + policy_input_shapes = pickle.load(f)['input_shapes'] + + _run = make_run_policy(vision_runner, policy_runner, nv12, model_w, model_h, + vision_features_slice, frame_skip, prepare_only) + run_policy_jit = TinyJit(_run, prune=True) + + N_RUNS = 3 + SEED = 42 + + def random_inputs_run_fn(fn, seed, test_val=None, test_buffers=None, expect_match=True): + input_queues, npy = make_input_queues(vision_input_shapes, policy_input_shapes, frame_skip) + np.random.seed(seed) + Tensor.manual_seed(seed) + + for i in range(N_RUNS): + frame = Tensor.randint(nv12.size, low=0, high=256, dtype='uint8').realize() + big_frame = Tensor.randint(nv12.size, low=0, high=256, dtype='uint8').realize() + for v in npy.values(): + v[:] = np.random.randn(*v.shape).astype(v.dtype) + Device.default.synchronize() + st = time.perf_counter() + outs = fn(**input_queues, frame=frame, big_frame=big_frame) + mt = time.perf_counter() + for o in outs: + # .realize() not needed once jitted, but needed for unjitted fn + o.realize() + Device.default.synchronize() + et = time.perf_counter() + print(f" [{i+1}/{N_RUNS}] enqueue {(mt-st)*1e3:6.2f} ms -- total {(et-st)*1e3:6.2f} ms") + + val = [np.copy(v.numpy()) for v in outs] + buffers = [np.copy(v.numpy().copy()) for v in input_queues.values()] + + if test_val is not None: + match = all(np.array_equal(a, b) for a, b in zip(val, test_val, strict=True)) + assert match == expect_match, f"outputs {'differ from' if expect_match else 'match'} baseline (seed={seed})" + if test_buffers is not None: + match = all(np.array_equal(a, b) for a, b in zip(buffers, test_buffers, strict=True)) + assert match == expect_match, f"buffers {'differ from' if expect_match else 'match'} baseline (seed={seed})" + return fn, val, buffers + + print('run unjitted') + _, test_val, test_buffers = random_inputs_run_fn(_run, seed=SEED) + print('capture + replay') + run_policy_jit, _, _ = random_inputs_run_fn(run_policy_jit, SEED, test_val, test_buffers) + + print('pickle round trip') + with open(pkl_path, "wb") as f: + pickle.dump(run_policy_jit, f) + print(f" Saved to {pkl_path}") + with open(pkl_path, "rb") as f: + run_policy_jit = pickle.load(f) + random_inputs_run_fn(run_policy_jit, SEED, test_val, test_buffers, expect_match=True) + random_inputs_run_fn(run_policy_jit, SEED+1, test_val, test_buffers, expect_match=False) + + +def _parse_size(s): + w, h = s.lower().split('x') + return int(w), int(h) + + +def _parse_nv12(s): + parts = s.split(',') + assert len(parts) == len(NV12Frame._fields), \ + f"--nv12 expects {','.join(NV12Frame._fields)} (got {s!r})" + return NV12Frame(*(int(x) for x in parts)) + + +if __name__ == "__main__": + p = argparse.ArgumentParser() + p.add_argument('--model-size', type=_parse_size, required=True, help='model input WxH') + p.add_argument('--nv12', type=_parse_nv12, required=True, + help=f'NV12 frame layout: {",".join(NV12Frame._fields)}') + p.add_argument('--vision-onnx', required=True) + p.add_argument('--policy-onnx', required=True) + p.add_argument('--output', required=True) + p.add_argument('--prepare-only', action='store_true') + p.add_argument('--frame-skip', type=int, required=True) + args = p.parse_args() + + model_w, model_h = args.model_size + compile_modeld(args.nv12, model_w, model_h, args.prepare_only, args.frame_skip, + args.vision_onnx, args.policy_onnx, args.output) \ No newline at end of file diff --git a/selfdrive/modeld/compile_warp.py b/selfdrive/modeld/compile_warp.py deleted file mode 100755 index 1144fc69c3..0000000000 --- a/selfdrive/modeld/compile_warp.py +++ /dev/null @@ -1,201 +0,0 @@ -#!/usr/bin/env python3 -import time -import pickle -import numpy as np -from pathlib import Path -from tinygrad.tensor import Tensor -from tinygrad.helpers import Context -from tinygrad.device import Device -from tinygrad.engine.jit import TinyJit - -from openpilot.system.camerad.cameras.nv12_info import get_nv12_info -from openpilot.common.transformations.model import MEDMODEL_INPUT_SIZE, DM_INPUT_SIZE -from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye - -MODELS_DIR = Path(__file__).parent / 'models' - -CAMERA_CONFIGS = [ - (_ar_ox_fisheye.width, _ar_ox_fisheye.height), # tici: 1928x1208 - (_os_fisheye.width, _os_fisheye.height), # mici: 1344x760 -] - -UV_SCALE_MATRIX = np.array([[0.5, 0, 0], [0, 0.5, 0], [0, 0, 1]], dtype=np.float32) -UV_SCALE_MATRIX_INV = np.linalg.inv(UV_SCALE_MATRIX) - -IMG_BUFFER_SHAPE = (30, MEDMODEL_INPUT_SIZE[1] // 2, MEDMODEL_INPUT_SIZE[0] // 2) - - -def warp_pkl_path(w, h): - return MODELS_DIR / f'warp_{w}x{h}_tinygrad.pkl' - - -def dm_warp_pkl_path(w, h): - return MODELS_DIR / f'dm_warp_{w}x{h}_tinygrad.pkl' - - -def warp_perspective_tinygrad(src_flat, M_inv, dst_shape, src_shape, stride_pad): - w_dst, h_dst = dst_shape - h_src, w_src = src_shape - - x = Tensor.arange(w_dst).reshape(1, w_dst).expand(h_dst, w_dst).reshape(-1) - y = Tensor.arange(h_dst).reshape(h_dst, 1).expand(h_dst, w_dst).reshape(-1) - - # inline 3x3 matmul as elementwise to avoid reduce op (enables fusion with gather) - src_x = M_inv[0, 0] * x + M_inv[0, 1] * y + M_inv[0, 2] - src_y = M_inv[1, 0] * x + M_inv[1, 1] * y + M_inv[1, 2] - src_w = M_inv[2, 0] * x + M_inv[2, 1] * y + M_inv[2, 2] - - src_x = src_x / src_w - src_y = src_y / src_w - - x_nn_clipped = Tensor.round(src_x).clip(0, w_src - 1).cast('int') - y_nn_clipped = Tensor.round(src_y).clip(0, h_src - 1).cast('int') - idx = y_nn_clipped * (w_src + stride_pad) + x_nn_clipped - - return src_flat[idx] - - -def frames_to_tensor(frames, model_w, model_h): - H = (frames.shape[0] * 2) // 3 - W = frames.shape[1] - in_img1 = Tensor.cat(frames[0:H:2, 0::2], - frames[1:H:2, 0::2], - frames[0:H:2, 1::2], - frames[1:H:2, 1::2], - frames[H:H+H//4].reshape((H//2, W//2)), - frames[H+H//4:H+H//2].reshape((H//2, W//2)), dim=0).reshape((6, H//2, W//2)) - return in_img1 - - -def make_frame_prepare(cam_w, cam_h, model_w, model_h): - stride, y_height, uv_height, _ = get_nv12_info(cam_w, cam_h) - uv_offset = stride * y_height - stride_pad = stride - cam_w - - def frame_prepare_tinygrad(input_frame, M_inv): - # UV_SCALE @ M_inv @ UV_SCALE_INV simplifies to elementwise scaling - M_inv_uv = M_inv * Tensor([[1.0, 1.0, 0.5], [1.0, 1.0, 0.5], [2.0, 2.0, 1.0]]) - # deinterleave NV12 UV plane (UVUV... -> separate U, V) - uv = input_frame[uv_offset:uv_offset + uv_height * stride].reshape(uv_height, stride) - with Context(SPLIT_REDUCEOP=0): - y = warp_perspective_tinygrad(input_frame[:cam_h*stride], - M_inv, (model_w, model_h), - (cam_h, cam_w), stride_pad).realize() - u = warp_perspective_tinygrad(uv[:cam_h//2, :cam_w:2].flatten(), - M_inv_uv, (model_w//2, model_h//2), - (cam_h//2, cam_w//2), 0).realize() - v = warp_perspective_tinygrad(uv[:cam_h//2, 1:cam_w:2].flatten(), - M_inv_uv, (model_w//2, model_h//2), - (cam_h//2, cam_w//2), 0).realize() - yuv = y.cat(u).cat(v).reshape((model_h * 3 // 2, model_w)) - tensor = frames_to_tensor(yuv, model_w, model_h) - return tensor - return frame_prepare_tinygrad - - -def make_update_img_input(frame_prepare, model_w, model_h): - def update_img_input_tinygrad(tensor, frame, M_inv): - M_inv = M_inv.to(Device.DEFAULT) - new_img = frame_prepare(frame, M_inv) - tensor.assign(tensor[6:].cat(new_img, dim=0).contiguous()) - return Tensor.cat(tensor[:6], tensor[-6:], dim=0).contiguous().reshape(1, 12, model_h//2, model_w//2) - return update_img_input_tinygrad - - -def make_update_both_imgs(frame_prepare, model_w, model_h): - update_img = make_update_img_input(frame_prepare, model_w, model_h) - - def update_both_imgs_tinygrad(calib_img_buffer, new_img, M_inv, - calib_big_img_buffer, new_big_img, M_inv_big): - calib_img_pair = update_img(calib_img_buffer, new_img, M_inv) - calib_big_img_pair = update_img(calib_big_img_buffer, new_big_img, M_inv_big) - return calib_img_pair, calib_big_img_pair - return update_both_imgs_tinygrad - - -def make_warp_dm(cam_w, cam_h, dm_w, dm_h): - stride, y_height, _, _ = get_nv12_info(cam_w, cam_h) - stride_pad = stride - cam_w - - def warp_dm(input_frame, M_inv): - M_inv = M_inv.to(Device.DEFAULT) - result = warp_perspective_tinygrad(input_frame[:cam_h*stride], M_inv, (dm_w, dm_h), (cam_h, cam_w), stride_pad).reshape(-1, dm_h * dm_w) - return result - return warp_dm - - -def compile_modeld_warp(cam_w, cam_h): - model_w, model_h = MEDMODEL_INPUT_SIZE - _, _, _, yuv_size = get_nv12_info(cam_w, cam_h) - - print(f"Compiling modeld warp for {cam_w}x{cam_h}...") - - frame_prepare = make_frame_prepare(cam_w, cam_h, model_w, model_h) - update_both_imgs = make_update_both_imgs(frame_prepare, model_w, model_h) - update_img_jit = TinyJit(update_both_imgs, prune=True) - - full_buffer = Tensor.zeros(IMG_BUFFER_SHAPE, dtype='uint8').contiguous().realize() - big_full_buffer = Tensor.zeros(IMG_BUFFER_SHAPE, dtype='uint8').contiguous().realize() - new_frame_np = np.random.randint(0, 256, yuv_size, dtype=np.uint8) - new_big_frame_np = np.random.randint(0, 256, yuv_size, dtype=np.uint8) - for i in range(10): - img_inputs = [full_buffer, - Tensor.from_blob(new_frame_np.ctypes.data, (yuv_size,), dtype='uint8').realize(), - Tensor(Tensor.randn(3, 3).mul(8).realize().numpy(), device='NPY')] - big_img_inputs = [big_full_buffer, - Tensor.from_blob(new_big_frame_np.ctypes.data, (yuv_size,), dtype='uint8').realize(), - Tensor(Tensor.randn(3, 3).mul(8).realize().numpy(), device='NPY')] - inputs = img_inputs + big_img_inputs - Device.default.synchronize() - - st = time.perf_counter() - _ = update_img_jit(*inputs) - mt = time.perf_counter() - Device.default.synchronize() - et = time.perf_counter() - print(f" [{i+1}/10] enqueue {(mt-st)*1e3:6.2f} ms -- total {(et-st)*1e3:6.2f} ms") - - pkl_path = warp_pkl_path(cam_w, cam_h) - with open(pkl_path, "wb") as f: - pickle.dump(update_img_jit, f) - print(f" Saved to {pkl_path}") - - jit = pickle.load(open(pkl_path, "rb")) - jit(*inputs) - - -def compile_dm_warp(cam_w, cam_h): - dm_w, dm_h = DM_INPUT_SIZE - _, _, _, yuv_size = get_nv12_info(cam_w, cam_h) - - print(f"Compiling DM warp for {cam_w}x{cam_h}...") - - warp_dm = make_warp_dm(cam_w, cam_h, dm_w, dm_h) - warp_dm_jit = TinyJit(warp_dm, prune=True) - - new_frame_np = np.random.randint(0, 256, yuv_size, dtype=np.uint8) - for i in range(10): - inputs = [Tensor.from_blob(new_frame_np.ctypes.data, (yuv_size,), dtype='uint8').realize(), - Tensor(Tensor.randn(3, 3).mul(8).realize().numpy(), device='NPY')] - Device.default.synchronize() - st = time.perf_counter() - warp_dm_jit(*inputs) - mt = time.perf_counter() - Device.default.synchronize() - et = time.perf_counter() - print(f" [{i+1}/10] enqueue {(mt-st)*1e3:6.2f} ms -- total {(et-st)*1e3:6.2f} ms") - - pkl_path = dm_warp_pkl_path(cam_w, cam_h) - with open(pkl_path, "wb") as f: - pickle.dump(warp_dm_jit, f) - print(f" Saved to {pkl_path}") - - -def run_and_save_pickle(): - for cam_w, cam_h in CAMERA_CONFIGS: - compile_modeld_warp(cam_w, cam_h) - compile_dm_warp(cam_w, cam_h) - - -if __name__ == "__main__": - run_and_save_pickle() diff --git a/selfdrive/modeld/get_model_metadata.py b/selfdrive/modeld/get_model_metadata.py index 838b1e9f40..ee08e9fb14 100755 --- a/selfdrive/modeld/get_model_metadata.py +++ b/selfdrive/modeld/get_model_metadata.py @@ -7,6 +7,10 @@ from typing import Any from tinygrad.nn.onnx import OnnxPBParser +def metadata_path_for(onnx_path) -> pathlib.Path: + p = pathlib.Path(onnx_path) + return p.parent / (p.stem + '_metadata.pkl') + class MetadataOnnxPBParser(OnnxPBParser): def _parse_ModelProto(self) -> dict: @@ -48,7 +52,7 @@ if __name__ == "__main__": 'output_shapes': dict(get_name_and_shape(x) for x in model["graph"]["output"]), } - metadata_path = model_path.parent / (model_path.stem + '_metadata.pkl') + metadata_path = metadata_path_for(model_path) with open(metadata_path, 'wb') as f: pickle.dump(metadata, f) diff --git a/selfdrive/modeld/helpers.py b/selfdrive/modeld/helpers.py new file mode 100644 index 0000000000..99c3d2ee27 --- /dev/null +++ b/selfdrive/modeld/helpers.py @@ -0,0 +1,31 @@ +import json +import os +from dataclasses import dataclass +from pathlib import Path + +from openpilot.system.camerad.cameras.nv12_info import get_nv12_info + +MODELS_DIR = Path(__file__).resolve().parent / 'models' +COMPILED_FLAGS_PATH = MODELS_DIR / 'tg_compiled_flags.json' + + +def set_tinygrad_backend_from_compiled_flags() -> None: + if os.path.isfile(COMPILED_FLAGS_PATH): + with open(COMPILED_FLAGS_PATH) as f: + os.environ['DEV'] = str(json.load(f)['DEV']) + + +@dataclass +class CompileConfig: + cam_w: int + cam_h: int + prepare_only: bool + prefix: str + + @property + def pkl_path(self): + return str(MODELS_DIR / f'{self.prefix}{"warp_" if self.prepare_only else ""}{self.cam_w}x{self.cam_h}_tinygrad.pkl') + + @property + def nv12(self): + return (self.cam_w, self.cam_h, *get_nv12_info(self.cam_w, self.cam_h)) \ No newline at end of file diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 4f89058d70..df8330af8f 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -1,12 +1,8 @@ #!/usr/bin/env python3 import os -from openpilot.selfdrive.modeld.tinygrad_helpers import MODELS_DIR, set_tinygrad_backend_from_compiled_flags +from openpilot.selfdrive.modeld.helpers import MODELS_DIR, CompileConfig, set_tinygrad_backend_from_compiled_flags set_tinygrad_backend_from_compiled_flags() -# FIXME-SP: remove once we bump tg -from openpilot.system.hardware import TICI -os.environ['DEV'] = 'QCOM' if TICI else 'CPU' - USBGPU = "USBGPU" in os.environ if USBGPU: os.environ['DEV'] = 'AMD' @@ -30,6 +26,7 @@ from openpilot.common.transformations.model import get_warp_matrix from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value, get_curvature_from_plan from openpilot.selfdrive.modeld.parse_model_outputs import Parser +from openpilot.selfdrive.modeld.compile_modeld import make_input_queues from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState from openpilot.common.file_chunker import read_file_chunked from openpilot.selfdrive.modeld.constants import ModelConstants, Plan @@ -41,17 +38,13 @@ from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase PROCESS_NAME = "selfdrive.modeld.modeld" SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') -VISION_PKL_PATH = MODELS_DIR / 'driving_vision_tinygrad.pkl' VISION_METADATA_PATH = MODELS_DIR / 'driving_vision_metadata.pkl' -POLICY_PKL_PATH = MODELS_DIR / 'driving_policy_tinygrad.pkl' POLICY_METADATA_PATH = MODELS_DIR / 'driving_policy_metadata.pkl' LAT_SMOOTH_SECONDS = 0.0 LONG_SMOOTH_SECONDS = 0.3 MIN_LAT_CONTROL_SPEED = 0.3 -IMG_QUEUE_SHAPE = (6*(ModelConstants.MODEL_RUN_FREQ//ModelConstants.MODEL_CONTEXT_FREQ + 1), 128, 256) -assert IMG_QUEUE_SHAPE[0] == 30 def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action, @@ -86,108 +79,39 @@ class FrameMeta: if vipc is not None: self.frame_id, self.timestamp_sof, self.timestamp_eof = vipc.frame_id, vipc.timestamp_sof, vipc.timestamp_eof -class InputQueues: - def __init__ (self, model_fps, env_fps, n_frames_input): - assert env_fps % model_fps == 0 - assert env_fps >= model_fps - self.model_fps = model_fps - self.env_fps = env_fps - self.n_frames_input = n_frames_input - - self.dtypes = {} - self.shapes = {} - self.q = {} - - def update_dtypes_and_shapes(self, input_dtypes, input_shapes) -> None: - self.dtypes.update(input_dtypes) - if self.env_fps == self.model_fps: - self.shapes.update(input_shapes) - else: - for k in input_shapes: - shape = list(input_shapes[k]) - if 'img' in k: - n_channels = shape[1] // self.n_frames_input - shape[1] = (self.env_fps // self.model_fps + (self.n_frames_input - 1)) * n_channels - else: - shape[1] = (self.env_fps // self.model_fps) * shape[1] - self.shapes[k] = tuple(shape) - - def reset(self) -> None: - self.q = {k: np.zeros(self.shapes[k], dtype=self.dtypes[k]) for k in self.dtypes.keys()} - - def enqueue(self, inputs:dict[str, np.ndarray]) -> None: - for k in inputs.keys(): - if inputs[k].dtype != self.dtypes[k]: - raise ValueError(f'supplied input <{k}({inputs[k].dtype})> has wrong dtype, expected {self.dtypes[k]}') - input_shape = list(self.shapes[k]) - input_shape[1] = -1 - single_input = inputs[k].reshape(tuple(input_shape)) - sz = single_input.shape[1] - self.q[k][:,:-sz] = self.q[k][:,sz:] - self.q[k][:,-sz:] = single_input - - def get(self, *names) -> dict[str, np.ndarray]: - if self.env_fps == self.model_fps: - return {k: self.q[k] for k in names} - else: - out = {} - for k in names: - shape = self.shapes[k] - if 'img' in k: - n_channels = shape[1] // (self.env_fps // self.model_fps + (self.n_frames_input - 1)) - out[k] = np.concatenate([self.q[k][:, s:s+n_channels] for s in np.linspace(0, shape[1] - n_channels, self.n_frames_input, dtype=int)], axis=1) - elif 'pulse' in k: - # any pulse within interval counts - out[k] = self.q[k].reshape((shape[0], shape[1] * self.model_fps // self.env_fps, self.env_fps // self.model_fps, -1)).max(axis=2) - else: - idxs = np.arange(-1, -shape[1], -self.env_fps // self.model_fps)[::-1] - out[k] = self.q[k][:, idxs] - return out class ModelState(ModelStateBase): - inputs: dict[str, np.ndarray] - output: np.ndarray prev_desire: np.ndarray # for tracking the rising edge of the pulse - def __init__(self): + def __init__(self, cam_w: int, cam_h: int): ModelStateBase.__init__(self) self.LAT_SMOOTH_SECONDS = LAT_SMOOTH_SECONDS + with open(VISION_METADATA_PATH, 'rb') as f: vision_metadata = pickle.load(f) self.vision_input_shapes = vision_metadata['input_shapes'] self.vision_input_names = list(self.vision_input_shapes.keys()) self.vision_output_slices = vision_metadata['output_slices'] - vision_output_size = vision_metadata['output_shapes']['outputs'][1] with open(POLICY_METADATA_PATH, 'rb') as f: policy_metadata = pickle.load(f) self.policy_input_shapes = policy_metadata['input_shapes'] self.policy_output_slices = policy_metadata['output_slices'] - policy_output_size = policy_metadata['output_shapes']['outputs'][1] self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32) - # policy inputs - self.numpy_inputs = {k: np.zeros(self.policy_input_shapes[k], dtype=np.float32) for k in self.policy_input_shapes} - self.full_input_queues = InputQueues(ModelConstants.MODEL_CONTEXT_FREQ, ModelConstants.MODEL_RUN_FREQ, ModelConstants.N_FRAMES) - for k in ['desire_pulse', 'features_buffer']: - self.full_input_queues.update_dtypes_and_shapes({k: self.numpy_inputs[k].dtype}, {k: self.numpy_inputs[k].shape}) - self.full_input_queues.reset() - - self.img_queues = {'img': Tensor.zeros(IMG_QUEUE_SHAPE, dtype='uint8').contiguous().realize(), - 'big_img': Tensor.zeros(IMG_QUEUE_SHAPE, dtype='uint8').contiguous().realize()} + self.frame_skip = ModelConstants.MODEL_RUN_FREQ // ModelConstants.MODEL_CONTEXT_FREQ + self.input_queues, self.npy = make_input_queues(self.vision_input_shapes, self.policy_input_shapes, self.frame_skip) self.full_frames : dict[str, Tensor] = {} self._blob_cache : dict[int, Tensor] = {} - self.transforms_np = {k: np.zeros((3,3), dtype=np.float32) for k in self.img_queues} - self.transforms = {k: Tensor(v, device='NPY').realize() for k, v in self.transforms_np.items()} - self.vision_output = np.zeros(vision_output_size, dtype=np.float32) - self.policy_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()} - self.policy_output = np.zeros(policy_output_size, dtype=np.float32) self.parser = Parser() - self.frame_buf_params : dict[str, tuple[int, int, int, int]] = {} - self.update_imgs = None - self.vision_run = pickle.loads(read_file_chunked(str(VISION_PKL_PATH))) - self.policy_run = pickle.loads(read_file_chunked(str(POLICY_PKL_PATH))) + self.frame_buf_params = {k: get_nv12_info(cam_w, cam_h) for k in ('img', 'big_img')} + self.run_policy = pickle.loads(read_file_chunked(CompileConfig(cam_w, cam_h, prefix='driving_', prepare_only=False).pkl_path)) + self.warp_enqueue = pickle.loads(read_file_chunked(CompileConfig(cam_w, cam_h, prefix='driving_', prepare_only=True).pkl_path)) + self.warp_enqueue( + **self.input_queues, + frame=Tensor.zeros(self.frame_buf_params['img'][3], dtype='uint8').contiguous().realize(), + big_frame=Tensor.zeros(self.frame_buf_params['big_img'][3], dtype='uint8').contiguous().realize()) def slice_outputs(self, model_outputs: np.ndarray, output_slices: dict[str, slice]) -> dict[str, np.ndarray]: parsed_model_outputs = {k: model_outputs[np.newaxis, v] for k,v in output_slices.items()} @@ -195,18 +119,6 @@ class ModelState(ModelStateBase): def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, 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_pulse'][0] = 0 - new_desire = np.where(inputs['desire_pulse'] - self.prev_desire > .99, inputs['desire_pulse'], 0) - self.prev_desire[:] = inputs['desire_pulse'] - if self.update_imgs is None: - for key in bufs.keys(): - w, h = bufs[key].width, bufs[key].height - self.frame_buf_params[key] = get_nv12_info(w, h) - warp_path = MODELS_DIR / f'warp_{w}x{h}_tinygrad.pkl' - with open(warp_path, "rb") as f: - self.update_imgs = pickle.load(f) - for key in bufs.keys(): ptr = bufs[key].data.ctypes.data yuv_size = self.frame_buf_params[key][3] @@ -215,30 +127,31 @@ class ModelState(ModelStateBase): if cache_key not in self._blob_cache: self._blob_cache[cache_key] = Tensor.from_blob(ptr, (yuv_size,), dtype='uint8') self.full_frames[key] = self._blob_cache[cache_key] - for key in bufs.keys(): - self.transforms_np[key][:,:] = transforms[key][:,:] - out = self.update_imgs(self.img_queues['img'], self.full_frames['img'], self.transforms['img'], - self.img_queues['big_img'], self.full_frames['big_img'], self.transforms['big_img']) - vision_inputs = {'img': out[0], 'big_img': out[1]} + # Model decides when action is completed, so desire input is just a pulse triggered on rising edge + inputs['desire_pulse'][0] = 0 + self.npy['desire'][:] = np.where(inputs['desire_pulse'] - self.prev_desire > .99, inputs['desire_pulse'], 0) + self.prev_desire[:] = inputs['desire_pulse'] + self.npy['traffic_convention'][:] = inputs['traffic_convention'] + self.npy['tfm'][:,:] = transforms['img'][:,:] + self.npy['big_tfm'][:,:] = transforms['big_img'][:,:] if prepare_only: + self.warp_enqueue(**self.input_queues, frame=self.full_frames['img'], big_frame=self.full_frames['big_img']) return None - self.vision_output = self.vision_run(**vision_inputs).contiguous().realize().uop.base.buffer.numpy().flatten() - vision_outputs_dict = self.parser.parse_vision_outputs(self.slice_outputs(self.vision_output, self.vision_output_slices)) + vision_output, policy_output = self.run_policy( + **self.input_queues, frame=self.full_frames['img'], big_frame=self.full_frames['big_img'] + ) - self.full_input_queues.enqueue({'features_buffer': vision_outputs_dict['hidden_state'], 'desire_pulse': new_desire}) - for k in ['desire_pulse', 'features_buffer']: - self.numpy_inputs[k][:] = self.full_input_queues.get(k)[k] - self.numpy_inputs['traffic_convention'][:] = inputs['traffic_convention'] - - self.policy_output = self.policy_run(**self.policy_inputs).contiguous().realize().uop.base.buffer.numpy().flatten() - policy_outputs_dict = self.parser.parse_policy_outputs(self.slice_outputs(self.policy_output, self.policy_output_slices)) + vision_output = vision_output.numpy().flatten() + policy_output = policy_output.numpy().flatten() + vision_outputs_dict = self.parser.parse_vision_outputs(self.slice_outputs(vision_output, self.vision_output_slices)) + policy_outputs_dict = self.parser.parse_policy_outputs(self.slice_outputs(policy_output, self.policy_output_slices)) combined_outputs_dict = {**vision_outputs_dict, **policy_outputs_dict} - if SEND_RAW_PRED: - combined_outputs_dict['raw_pred'] = np.concatenate([self.vision_output.copy(), self.policy_output.copy()]) + if SEND_RAW_PRED: + combined_outputs_dict['raw_pred'] = np.concatenate([vision_output.copy(), policy_output.copy()]) return combined_outputs_dict @@ -250,11 +163,6 @@ def main(demo=False): # also need to move the aux USB interrupts for good timings config_realtime_process(7, 54) - st = time.monotonic() - cloudlog.warning("loading model") - model = ModelState() - cloudlog.warning(f"models loaded in {time.monotonic() - st:.1f}s, modeld starting") - # visionipc clients while True: available_streams = VisionIpcClient.available_streams("camerad", block=False) @@ -278,6 +186,11 @@ def main(demo=False): 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})") + st = time.monotonic() + cloudlog.warning("loading model") + model = ModelState(vipc_client_main.width, vipc_client_main.height) + cloudlog.warning(f"models loaded in {time.monotonic() - st:.1f}s, modeld starting") + # messaging pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"]) sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"]) diff --git a/selfdrive/modeld/tinygrad_helpers.py b/selfdrive/modeld/tinygrad_helpers.py deleted file mode 100644 index 49a6ed6161..0000000000 --- a/selfdrive/modeld/tinygrad_helpers.py +++ /dev/null @@ -1,12 +0,0 @@ -import json -import os -from pathlib import Path - -MODELS_DIR = Path(__file__).parent / 'models' -COMPILED_FLAGS_PATH = MODELS_DIR / 'tg_compiled_flags.json' - - -def set_tinygrad_backend_from_compiled_flags() -> None: - if os.path.isfile(COMPILED_FLAGS_PATH): - with open(COMPILED_FLAGS_PATH) as f: - os.environ['DEV'] = str(json.load(f)['DEV']) diff --git a/sunnypilot/modeld_v2/SConscript b/sunnypilot/modeld_v2/SConscript index 8526fd2729..32a202567a 100644 --- a/sunnypilot/modeld_v2/SConscript +++ b/sunnypilot/modeld_v2/SConscript @@ -22,7 +22,7 @@ if PC: lenv.Command(outputs, inputs, cmd) tg_flags = { - 'larch64': 'DEV=QCOM FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0', + 'larch64': 'DEV=QCOM FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1', 'Darwin': f'DEV=CPU THREADS=0 HOME={os.path.expanduser("~")}', }.get(arch, 'DEV=CPU CPU_LLVM=1 THREADS=0') @@ -46,9 +46,9 @@ for model_name in ['supercombo', 'driving_vision', 'driving_off_policy', 'drivin if File(f"models/{model_name}.onnx").exists(): tg_compile(tg_flags, model_name) -script_files = [File("warp.py"), File(Dir("#selfdrive/modeld").File("compile_warp.py").abspath)] +script_files = [File("warp.py"), File(Dir("#selfdrive/modeld").File("compile_modeld.py").abspath)] pythonpath_string = 'PYTHONPATH="${PYTHONPATH}:' + env.Dir("#tinygrad_repo").abspath + ':' + env.Dir("#").abspath + '"' -compile_warp_cmd = f'{pythonpath_string} {tg_flags} python3 -m sunnypilot.modeld_v2.warp' +compile_warp_cmd = f'{pythonpath_string} {tg_flags} {image_flag} python3 -m sunnypilot.modeld_v2.warp' from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye warp_targets = [] diff --git a/sunnypilot/modeld_v2/tests/test_warp.py b/sunnypilot/modeld_v2/tests/test_warp.py index daf0dd528c..4f5ceeb773 100644 --- a/sunnypilot/modeld_v2/tests/test_warp.py +++ b/sunnypilot/modeld_v2/tests/test_warp.py @@ -2,8 +2,11 @@ import os os.environ['DEV'] = 'CPU' import pytest import numpy as np -from openpilot.selfdrive.modeld.compile_warp import get_nv12_info, CAMERA_CONFIGS -from openpilot.sunnypilot.modeld_v2.warp import Warp, MODEL_W, MODEL_H +from openpilot.sunnypilot.modeld_v2.warp import CAMERA_CONFIGS +from openpilot.system.camerad.cameras.nv12_info import get_nv12_info +from openpilot.sunnypilot.modeld_v2.warp import Warp +from openpilot.common.transformations.model import MEDMODEL_INPUT_SIZE +MODEL_W, MODEL_H = MEDMODEL_INPUT_SIZE VISION_NAME_PAIRS = [ # needed to account for supercombos input_imgs ('img', 'big_img'), diff --git a/sunnypilot/modeld_v2/warp.py b/sunnypilot/modeld_v2/warp.py index fd8be46837..61988bea89 100644 --- a/sunnypilot/modeld_v2/warp.py +++ b/sunnypilot/modeld_v2/warp.py @@ -6,29 +6,61 @@ from tinygrad.tensor import Tensor from tinygrad.engine.jit import TinyJit from tinygrad.device import Device +# https://github.com/tinygrad/tinygrad/issues/15682 +from tinygrad.uop.ops import UOp, Ops +_orig = UOp.__reduce__ +UOp.__reduce__ = lambda self: (UOp.unique, ()) if self.op is Ops.UNIQUE else _orig(self) from openpilot.system.camerad.cameras.nv12_info import get_nv12_info -from openpilot.selfdrive.modeld.compile_warp import ( - CAMERA_CONFIGS, MEDMODEL_INPUT_SIZE, make_frame_prepare, make_update_both_imgs, - warp_pkl_path, +from openpilot.selfdrive.modeld.compile_modeld import ( + NV12Frame, make_frame_prepare, ) +from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye + +CAMERA_CONFIGS = [ + (_ar_ox_fisheye.width, _ar_ox_fisheye.height), # tici: 1928x1208 + (_os_fisheye.width, _os_fisheye.height), # mici: 1344x760 +] +from openpilot.common.transformations.model import MEDMODEL_INPUT_SIZE MODELS_DIR = Path(__file__).parent / 'models' -MODEL_W, MODEL_H = MEDMODEL_INPUT_SIZE + UPSTREAM_BUFFER_LENGTH = 5 +def warp_pkl_path(cam_w, cam_h): + return MODELS_DIR / f'warp_{cam_w}x{cam_h}_tinygrad.pkl' + +def make_update_img_input(frame_prepare, model_w, model_h): + def update_img_input_tinygrad(tensor, frame, M_inv): + M_inv = M_inv.to(Device.DEFAULT) + new_img = frame_prepare(frame, M_inv) + tensor.assign(tensor[6:].cat(new_img, dim=0).contiguous()) + return Tensor.cat(tensor[:6], tensor[-6:], dim=0).contiguous().reshape(1, 12, model_h//2, model_w//2) + return update_img_input_tinygrad + +def make_update_both_imgs(frame_prepare, model_w, model_h): + update_img = make_update_img_input(frame_prepare, model_w, model_h) + + def update_both_imgs_tinygrad(calib_img_buffer, new_img, M_inv, + calib_big_img_buffer, new_big_img, M_inv_big): + calib_img_pair = update_img(calib_img_buffer, new_img, M_inv) + calib_big_img_pair = update_img(calib_big_img_buffer, new_big_img, M_inv_big) + return calib_img_pair, calib_big_img_pair + return update_both_imgs_tinygrad + def v2_warp_pkl_path(cam_w, cam_h, buffer_length): return MODELS_DIR / f'warp_{cam_w}x{cam_h}_b{buffer_length}_tinygrad.pkl' -def compile_v2_warp(cam_w, cam_h, buffer_length): +def compile_v2_warp(cam_w, cam_h, buffer_length, model_w=MEDMODEL_INPUT_SIZE[0], model_h=MEDMODEL_INPUT_SIZE[1]): _, _, _, yuv_size = get_nv12_info(cam_w, cam_h) - img_buffer_shape = (buffer_length * 6, MODEL_H // 2, MODEL_W // 2) + img_buffer_shape = (buffer_length * 6, model_h // 2, model_w // 2) print(f"Compiling v2 warp for {cam_w}x{cam_h} buffer_length={buffer_length}...") - frame_prepare = make_frame_prepare(cam_w, cam_h, MODEL_W, MODEL_H) - update_both_imgs = make_update_both_imgs(frame_prepare, MODEL_W, MODEL_H) + nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h)) + frame_prepare = make_frame_prepare(nv12, model_w, model_h) + update_both_imgs = make_update_both_imgs(frame_prepare, model_w, model_h) update_img_jit = TinyJit(update_both_imgs, prune=True) full_buffer = Tensor.zeros(img_buffer_shape, dtype='uint8').contiguous().realize() @@ -62,9 +94,11 @@ def compile_v2_warp(cam_w, cam_h, buffer_length): class Warp: - def __init__(self, buffer_length=2): + def __init__(self, buffer_length=2, model_w=MEDMODEL_INPUT_SIZE[0], model_h=MEDMODEL_INPUT_SIZE[1]): self.buffer_length = buffer_length - self.img_buffer_shape = (buffer_length * 6, MODEL_H // 2, MODEL_W // 2) + self.model_w = model_w + self.model_h = model_h + self.img_buffer_shape = (buffer_length * 6, model_h // 2, model_w // 2) self.jit_cache = {} self.full_buffers = {k: Tensor.zeros(self.img_buffer_shape, dtype='uint8').contiguous().realize() for k in ['img', 'big_img']} @@ -92,8 +126,9 @@ class Warp: with open(upstream_pkl, 'rb') as f: self.jit_cache[key] = pickle.load(f) if key not in self.jit_cache: - frame_prepare = make_frame_prepare(cam_w, cam_h, MODEL_W, MODEL_H) - update_both_imgs = make_update_both_imgs(frame_prepare, MODEL_W, MODEL_H) + nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h)) + frame_prepare = make_frame_prepare(nv12, self.model_w, self.model_h) + update_both_imgs = make_update_both_imgs(frame_prepare, self.model_w, self.model_h) self.jit_cache[key] = TinyJit(update_both_imgs, prune=True) if key not in self._nv12_cache: From b4c613680edd64a6fc15c241febb0c23e7f0e021 Mon Sep 17 00:00:00 2001 From: James Vecellio-Grant <159560811+Discountchubbs@users.noreply.github.com> Date: Sun, 26 Apr 2026 00:29:30 -0700 Subject: [PATCH 05/14] Update SConscript --- selfdrive/modeld/SConscript | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index 3c8581e898..76fef75bae 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -115,4 +115,4 @@ def tg_compile(flags, model_name): do_chunk, ) -tg_compile(tg_flags, 'dmonitoring_model') \ No newline at end of file +tg_compile(tg_flags, 'dmonitoring_model') From d85cb763047e96093b25adcbe90879034b952b3a Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Sun, 26 Apr 2026 00:32:31 -0700 Subject: [PATCH 06/14] lint --- selfdrive/modeld/compile_dm_warp.py | 2 +- selfdrive/modeld/compile_modeld.py | 2 +- selfdrive/modeld/helpers.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/modeld/compile_dm_warp.py b/selfdrive/modeld/compile_dm_warp.py index 54f1c60c40..2713cccf41 100644 --- a/selfdrive/modeld/compile_dm_warp.py +++ b/selfdrive/modeld/compile_dm_warp.py @@ -51,4 +51,4 @@ if __name__ == "__main__": args = p.parse_args() dm_w, dm_h = args.warp_to - compile_dm_warp(args.nv12, dm_w, dm_h, args.output) \ No newline at end of file + compile_dm_warp(args.nv12, dm_w, dm_h, args.output) diff --git a/selfdrive/modeld/compile_modeld.py b/selfdrive/modeld/compile_modeld.py index d5f4417edc..1e05ae30a4 100644 --- a/selfdrive/modeld/compile_modeld.py +++ b/selfdrive/modeld/compile_modeld.py @@ -250,4 +250,4 @@ if __name__ == "__main__": model_w, model_h = args.model_size compile_modeld(args.nv12, model_w, model_h, args.prepare_only, args.frame_skip, - args.vision_onnx, args.policy_onnx, args.output) \ No newline at end of file + args.vision_onnx, args.policy_onnx, args.output) diff --git a/selfdrive/modeld/helpers.py b/selfdrive/modeld/helpers.py index 99c3d2ee27..e5d731f34b 100644 --- a/selfdrive/modeld/helpers.py +++ b/selfdrive/modeld/helpers.py @@ -28,4 +28,4 @@ class CompileConfig: @property def nv12(self): - return (self.cam_w, self.cam_h, *get_nv12_info(self.cam_w, self.cam_h)) \ No newline at end of file + return (self.cam_w, self.cam_h, *get_nv12_info(self.cam_w, self.cam_h)) From 66ff8ae52cb2e05e573e19bd2fa092c7d6b13b5d Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Sun, 26 Apr 2026 00:33:45 -0700 Subject: [PATCH 07/14] shebang --- selfdrive/modeld/compile_modeld.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 selfdrive/modeld/compile_modeld.py diff --git a/selfdrive/modeld/compile_modeld.py b/selfdrive/modeld/compile_modeld.py old mode 100644 new mode 100755 From 92526b878ca7976a45db7e88bb758855878ddae7 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Sun, 26 Apr 2026 11:07:17 -0700 Subject: [PATCH 08/14] json v17 --- sunnypilot/models/fetcher.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sunnypilot/models/fetcher.py b/sunnypilot/models/fetcher.py index 0b6853da8a..4544c34deb 100644 --- a/sunnypilot/models/fetcher.py +++ b/sunnypilot/models/fetcher.py @@ -116,7 +116,7 @@ class ModelCache: class ModelFetcher: """Handles fetching and caching of model data from remote source""" - MODEL_URL = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-models/refs/heads/gh-pages/docs/driving_models_v16.json" + MODEL_URL = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-models/refs/heads/gh-pages/docs/driving_models_v17.json" def __init__(self, params: Params): self.params = params From 11ed3800bf36263425113ab3a3e6ede45f44087b Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Sun, 26 Apr 2026 11:10:17 -0700 Subject: [PATCH 09/14] ugh --- selfdrive/modeld/compile_dm_warp.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 selfdrive/modeld/compile_dm_warp.py diff --git a/selfdrive/modeld/compile_dm_warp.py b/selfdrive/modeld/compile_dm_warp.py old mode 100644 new mode 100755 From 03db277c22a5069eea3d7be66f534cc1cbdb4749 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Sun, 26 Apr 2026 11:15:34 -0700 Subject: [PATCH 10/14] dev --- sunnypilot/modeld_v2/SConscript | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sunnypilot/modeld_v2/SConscript b/sunnypilot/modeld_v2/SConscript index 32a202567a..fdaac8d084 100644 --- a/sunnypilot/modeld_v2/SConscript +++ b/sunnypilot/modeld_v2/SConscript @@ -24,7 +24,7 @@ if PC: tg_flags = { 'larch64': 'DEV=QCOM FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1', 'Darwin': f'DEV=CPU THREADS=0 HOME={os.path.expanduser("~")}', -}.get(arch, 'DEV=CPU CPU_LLVM=1 THREADS=0') +}.get(arch, 'DEV=CPU:LLVM THREADS=0') image_flag = { 'larch64': 'IMAGE=2', From ced4a664cc1e792dc92ea9abb428ff13cc071a1c Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Sun, 26 Apr 2026 11:18:51 -0700 Subject: [PATCH 11/14] use upstreams --- sunnypilot/modeld_v2/SConscript | 22 ++++++++++++++++------ 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/sunnypilot/modeld_v2/SConscript b/sunnypilot/modeld_v2/SConscript index fdaac8d084..3db7b42183 100644 --- a/sunnypilot/modeld_v2/SConscript +++ b/sunnypilot/modeld_v2/SConscript @@ -1,5 +1,6 @@ import os import glob +from tinygrad import Device Import('env', 'arch') lenv = env.Clone() @@ -21,10 +22,19 @@ if PC: if outputs: lenv.Command(outputs, inputs, cmd) -tg_flags = { - 'larch64': 'DEV=QCOM FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1', - 'Darwin': f'DEV=CPU THREADS=0 HOME={os.path.expanduser("~")}', -}.get(arch, 'DEV=CPU:LLVM THREADS=0') +available = set(Device.get_available_devices()) +if 'CUDA' in available: + tg_backend = 'CUDA' + tg_flags = f'DEV={tg_backend}' +elif 'QCOM' in available: + tg_backend = 'QCOM' + tg_flags = f'DEV={tg_backend} FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1' +else: + tg_backend = 'CPU' if arch == 'Darwin' else 'CPU:LLVM' + # THREADS=0 is need to prevent bug: https://github.com/tinygrad/tinygrad/issues/14689 + tg_flags = f'DEV={tg_backend} THREADS=0' + +mac_brew_string = f'HOME={os.path.expanduser("~")}' if arch == 'Darwin' else '' image_flag = { 'larch64': 'IMAGE=2', @@ -38,7 +48,7 @@ def tg_compile(flags, model_name): return lenv.Command( out, [fn + ".onnx"] + tinygrad_files, - f'{pythonpath_string} {flags} {image_flag} python3 {Dir("#tinygrad_repo").abspath}/examples/openpilot/compile3.py {fn}.onnx {out}' + f'{pythonpath_string} {tg_flags} {mac_brew_string} {image_flag} python3 {Dir("#tinygrad_repo").abspath}/examples/openpilot/compile3.py {fn}.onnx {out}' ) # Compile models @@ -48,7 +58,7 @@ for model_name in ['supercombo', 'driving_vision', 'driving_off_policy', 'drivin script_files = [File("warp.py"), File(Dir("#selfdrive/modeld").File("compile_modeld.py").abspath)] pythonpath_string = 'PYTHONPATH="${PYTHONPATH}:' + env.Dir("#tinygrad_repo").abspath + ':' + env.Dir("#").abspath + '"' -compile_warp_cmd = f'{pythonpath_string} {tg_flags} {image_flag} python3 -m sunnypilot.modeld_v2.warp' +compile_warp_cmd = f'{pythonpath_string} {tg_flags} {mac_brew_string} {image_flag} python3 -m sunnypilot.modeld_v2.warp' from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye warp_targets = [] From dc11e5fd84e351d85c6ea4396cef5206a71a401b Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Sun, 26 Apr 2026 12:53:45 -0700 Subject: [PATCH 12/14] sync dmonitoring too --- selfdrive/modeld/dmonitoringmodeld.py | 24 +- selfdrive/monitoring/dmonitoringd.py | 11 +- selfdrive/monitoring/helpers.py | 373 +++++++++++------------- selfdrive/monitoring/test_monitoring.py | 222 +++++--------- 4 files changed, 259 insertions(+), 371 deletions(-) diff --git a/selfdrive/modeld/dmonitoringmodeld.py b/selfdrive/modeld/dmonitoringmodeld.py index 78749b773a..18a79136ae 100755 --- a/selfdrive/modeld/dmonitoringmodeld.py +++ b/selfdrive/modeld/dmonitoringmodeld.py @@ -1,12 +1,8 @@ #!/usr/bin/env python3 import os -from openpilot.selfdrive.modeld.tinygrad_helpers import MODELS_DIR, set_tinygrad_backend_from_compiled_flags +from openpilot.selfdrive.modeld.helpers import MODELS_DIR, CompileConfig, set_tinygrad_backend_from_compiled_flags set_tinygrad_backend_from_compiled_flags() -# FIXME-SP: remove once we bump tg -from openpilot.system.hardware import TICI -os.environ['DEV'] = 'QCOM' if TICI else 'CPU' - from tinygrad.tensor import Tensor import time import pickle @@ -32,7 +28,7 @@ class ModelState: inputs: dict[str, np.ndarray] output: np.ndarray - def __init__(self): + def __init__(self, cam_w: int, cam_h: int): with open(METADATA_PATH, 'rb') as f: model_metadata = pickle.load(f) self.input_shapes = model_metadata['input_shapes'] @@ -44,22 +40,18 @@ class ModelState: self.warp_inputs_np = {'transform': np.zeros((3,3), dtype=np.float32)} self.warp_inputs = {k: Tensor(v, device='NPY') for k,v in self.warp_inputs_np.items()} - self.frame_buf_params = None + self.frame_buf_params = get_nv12_info(cam_w, cam_h) self.tensor_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()} self._blob_cache : dict[int, Tensor] = {} - self.image_warp = None self.model_run = pickle.loads(read_file_chunked(str(MODEL_PKL_PATH))) + with open(CompileConfig(cam_w, cam_h, prefix='dm_', prepare_only=True).pkl_path, "rb") as f: + self.image_warp = pickle.load(f) def run(self, buf: VisionBuf, calib: np.ndarray, transform: np.ndarray) -> tuple[np.ndarray, float]: self.numpy_inputs['calib'][0,:] = calib t1 = time.perf_counter() - if self.image_warp is None: - self.frame_buf_params = get_nv12_info(buf.width, buf.height) - warp_path = MODELS_DIR / f'dm_warp_{buf.width}x{buf.height}_tinygrad.pkl' - with open(warp_path, "rb") as f: - self.image_warp = pickle.load(f) ptr = buf.data.ctypes.data # There is a ringbuffer of imgs, just cache tensors pointing to all of them if ptr not in self._blob_cache: @@ -113,9 +105,6 @@ def get_driverstate_packet(model_output, frame_id: int, location_ts: int, exec_t def main(): config_realtime_process(7, 5) - model = ModelState() - cloudlog.warning("models loaded, dmonitoringmodeld starting") - cloudlog.warning("connecting to driver stream") vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_DRIVER, True) while not vipc_client.connect(False): @@ -123,6 +112,9 @@ def main(): assert vipc_client.is_connected() cloudlog.warning(f"connected with buffer size: {vipc_client.buffer_len}") + model = ModelState(vipc_client.width, vipc_client.height) + cloudlog.warning("models loaded, dmonitoringmodeld starting") + sm = SubMaster(["liveCalibration"]) pm = PubMaster(["driverStateV2"]) diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index 022415af6d..415853513e 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -2,7 +2,7 @@ import cereal.messaging as messaging from openpilot.common.params import Params from openpilot.common.realtime import config_realtime_process -from openpilot.selfdrive.monitoring.helpers import DriverMonitoring +from openpilot.selfdrive.monitoring.policy import DriverMonitoring def dmonitoringd_thread(): @@ -10,8 +10,7 @@ def dmonitoringd_thread(): params = Params() pm = messaging.PubMaster(['driverMonitoringState']) - sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'selfdriveState', 'modelV2', - 'carControl'], poll='driverStateV2') + sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'selfdriveState', 'modelV2'], poll='driverStateV2') DM = DriverMonitoring(rhd_saved=params.get_bool("IsRhdDetected"), always_on=params.get_bool("AlwaysOnDM")) demo_mode=False @@ -25,7 +24,7 @@ def dmonitoringd_thread(): valid = sm.all_checks() if demo_mode and sm.valid['driverStateV2']: - DM.run_step(sm, demo=demo_mode) + DM.run_step(sm, demo=True) elif valid: DM.run_step(sm, demo=demo_mode) @@ -40,8 +39,8 @@ def dmonitoringd_thread(): # save rhd virtual toggle every 5 mins if (sm['driverStateV2'].frameId % 6000 == 0 and not demo_mode and - DM.wheelpos.prob_offseter.filtered_stat.n > DM.settings._WHEELPOS_FILTER_MIN_COUNT and - DM.wheel_on_right == (DM.wheelpos.prob_offseter.filtered_stat.M > DM.settings._WHEELPOS_THRESHOLD)): + DM.wheelpos_offsetter.filtered_stat.n > DM.settings._WHEELPOS_FILTER_MIN_COUNT and + DM.wheel_on_right == (DM.wheelpos_offsetter.filtered_stat.M > DM.settings._WHEELPOS_THRESHOLD)): params.put_bool_nonblocking("IsRhdDetected", DM.wheel_on_right) def main(): diff --git a/selfdrive/monitoring/helpers.py b/selfdrive/monitoring/helpers.py index 6c81fac7f0..8e8116145e 100644 --- a/selfdrive/monitoring/helpers.py +++ b/selfdrive/monitoring/helpers.py @@ -1,18 +1,20 @@ +from collections import defaultdict from math import atan2, radians import numpy as np from cereal import car, log import cereal.messaging as messaging -from openpilot.selfdrive.selfdrived.events import Events -from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert from openpilot.common.realtime import DT_DMON from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.params import Params from openpilot.common.stat_live import RunningStatFilter from openpilot.common.transformations.camera import DEVICE_CAMERAS -from openpilot.system.hardware import HARDWARE -EventName = log.OnroadEvent.EventName +AlertLevel = log.DriverMonitoringState.AlertLevel +MonitoringPolicy = log.DriverMonitoringState.MonitoringPolicy + +def to_percent(v): + return int(min(max(v * 100., 0.), 100.)) # ****************************************************************************************** # NOTE: To fork maintainers. @@ -21,21 +23,26 @@ EventName = log.OnroadEvent.EventName # ****************************************************************************************** class DRIVER_MONITOR_SETTINGS: - def __init__(self, device_type): - self._DT_DMON = DT_DMON - # ref (page15-16): https://eur-lex.europa.eu/legal-content/EN/TXT/PDF/?uri=CELEX:42018X1947&rid=2 - self._AWARENESS_TIME = 30. # passive wheeltouch total timeout - self._AWARENESS_PRE_TIME_TILL_TERMINAL = 15. - self._AWARENESS_PROMPT_TIME_TILL_TERMINAL = 6. - self._DISTRACTED_TIME = 11. # active monitoring total timeout - self._DISTRACTED_PRE_TIME_TILL_TERMINAL = 8. - self._DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6. + def __init__(self): + # https://eur-lex.europa.eu/legal-content/EN/TXT/PDF/?uri=CELEX:42018X1947&rid=2 + self._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT = 15. + self._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT = 24. + self._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT = 30. + # https://cdn.euroncap.com/cars/assets/euro_ncap_protocol_safe_driving_driver_engagement_v11_a30e874152.pdf + self._VISION_POLICY_ALERT_1_TIMEOUT = 3. + self._VISION_POLICY_ALERT_2_TIMEOUT = 5. + self._VISION_POLICY_ALERT_3_TIMEOUT = 11. + + self._TIMEOUT_RECOVERY_FACTOR_MAX = 5. + self._TIMEOUT_RECOVERY_FACTOR_MIN = 1.25 + + self._MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts + self._MAX_TERMINAL_DURATION = int(30 / DT_DMON) # not allowed to engage after 30s of terminal alerts self._FACE_THRESHOLD = 0.7 self._EYE_THRESHOLD = 0.5 self._BLINK_THRESHOLD = 0.5 self._PHONE_THRESH = 0.5 - self._POSE_PITCH_THRESHOLD = 0.3133 self._POSE_PITCH_THRESHOLD_SLACK = 0.3237 self._POSE_PITCH_THRESHOLD_STRICT = self._POSE_PITCH_THRESHOLD @@ -56,101 +63,74 @@ class DRIVER_MONITOR_SETTINGS: self._YAW_MIN_OFFSET = -0.0246 self._DCAM_UNCERTAIN_ALERT_THRESHOLD = 0.1 - self._DCAM_UNCERTAIN_ALERT_COUNT = int(60 / self._DT_DMON) - self._DCAM_UNCERTAIN_RESET_COUNT = int(20 / self._DT_DMON) - self._POSESTD_THRESHOLD = 0.3 - self._HI_STD_FALLBACK_TIME = int(10 / self._DT_DMON) # fall back to wheel touch if model is uncertain for 10s + self._DCAM_UNCERTAIN_ALERT_COUNT = int(60 / DT_DMON) + self._DCAM_UNCERTAIN_RESET_COUNT = int(20 / DT_DMON) + self._HI_STD_THRESHOLD = 0.3 + self._HI_STD_FALLBACK_TIME = int(10 / DT_DMON) # fall back to wheel touch if model is uncertain for 10s self._DISTRACTED_FILTER_TS = 0.25 # 0.6Hz self._POSE_CALIB_MIN_SPEED = 13 # 30 mph - self._POSE_OFFSET_MIN_COUNT = int(60 / self._DT_DMON) # valid data counts before calibration completes, 1min cumulative - self._POSE_OFFSET_MAX_COUNT = int(360 / self._DT_DMON) # stop deweighting new data after 6 min, aka "short term memory" - + self._POSE_OFFSET_MIN_COUNT = int(60 / DT_DMON) # valid data counts before calibration completes, 1min cumulative + self._POSE_OFFSET_MAX_COUNT = int(360 / DT_DMON) # stop deweighting new data after 6 min, aka "short term memory" self._WHEELPOS_CALIB_MIN_SPEED = 11 self._WHEELPOS_THRESHOLD = 0.5 - self._WHEELPOS_FILTER_MIN_COUNT = int(15 / self._DT_DMON) # allow 15 seconds to converge wheel side + self._WHEELPOS_FILTER_MIN_COUNT = int(15 / DT_DMON) # allow 15 seconds to converge wheel side self._WHEELPOS_DATA_AVG = 0.03 self._WHEELPOS_DATA_VAR = 3*5.5e-5 self._WHEELPOS_MAX_COUNT = -1 - self._RECOVERY_FACTOR_MAX = 5. # relative to minus step change - self._RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change - - self._MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts - self._MAX_TERMINAL_DURATION = int(30 / self._DT_DMON) # not allowed to engage after 30s of terminal alerts - -class DistractedType: - - NOT_DISTRACTED = 0 - DISTRACTED_POSE = 1 << 0 - DISTRACTED_BLINK = 1 << 1 - DISTRACTED_PHONE = 1 << 2 - class DriverPose: def __init__(self, settings): pitch_filter_raw_priors = (settings._PITCH_NATURAL_OFFSET, settings._PITCH_NATURAL_VAR, 2) yaw_filter_raw_priors = (settings._YAW_NATURAL_OFFSET, settings._YAW_NATURAL_VAR, 2) self.yaw = 0. self.pitch = 0. - self.roll = 0. - self.yaw_std = 0. - self.pitch_std = 0. - self.roll_std = 0. - self.pitch_offseter = RunningStatFilter(raw_priors=pitch_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT) - self.yaw_offseter = RunningStatFilter(raw_priors=yaw_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT) + self.pitch_offsetter = RunningStatFilter(raw_priors=pitch_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT) + self.yaw_offsetter = RunningStatFilter(raw_priors=yaw_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT) self.calibrated = False self.low_std = True self.cfactor_pitch = 1. self.cfactor_yaw = 1. self.steer_yaw_offset = 0. -class DriverProb: - def __init__(self, raw_priors, max_trackable): - self.prob = 0. - self.prob_offseter = RunningStatFilter(raw_priors=raw_priors, max_trackable=max_trackable) - self.prob_calibrated = False - - # model output refers to center of undistorted+leveled image -EFL = 598.0 # focal length in K -cam = DEVICE_CAMERAS[("tici", "ar0231")] # corrected image has same size as raw -W, H = (cam.dcam.width, cam.dcam.height) # corrected image has same size as raw +ref_undistorted_cam = DEVICE_CAMERAS[("tici", "ar0231")].dcam +dcam_undistorted_FL = 598.0 +dcam_undistorted_W, dcam_undistorted_H = (ref_undistorted_cam.width, ref_undistorted_cam.height) -def face_orientation_from_net(angles_desc, pos_desc, rpy_calib): - # the output of these angles are in device frame - # so from driver's perspective, pitch is up and yaw is right +def face_orientation_from_model(orient_model, pos_model, rpy_calib): + pitch_model = orient_model[0] + yaw_model = orient_model[1] - pitch_net, yaw_net, roll_net = angles_desc + face_pixel_position = ((pos_model[0]+0.5)*dcam_undistorted_W, (pos_model[1]+0.5)*dcam_undistorted_H) + yaw_focal_angle = atan2(face_pixel_position[0] - dcam_undistorted_W//2, dcam_undistorted_FL) + pitch_focal_angle = atan2(face_pixel_position[1] - dcam_undistorted_H//2, dcam_undistorted_FL) - face_pixel_position = ((pos_desc[0]+0.5)*W, (pos_desc[1]+0.5)*H) - yaw_focal_angle = atan2(face_pixel_position[0] - W//2, EFL) - pitch_focal_angle = atan2(face_pixel_position[1] - H//2, EFL) + pitch = pitch_model + pitch_focal_angle + yaw = -yaw_model + yaw_focal_angle - pitch = pitch_net + pitch_focal_angle - yaw = -yaw_net + yaw_focal_angle - - # no calib for roll pitch -= rpy_calib[1] yaw -= rpy_calib[2] - return roll_net, pitch, yaw + return pitch, yaw class DriverMonitoring: def __init__(self, rhd_saved=False, settings=None, always_on=False): # init policy settings - self.settings = settings if settings is not None else DRIVER_MONITOR_SETTINGS(device_type=HARDWARE.get_device_type()) + self.settings = settings if settings is not None else DRIVER_MONITOR_SETTINGS() # init driver status wheelpos_filter_raw_priors = (self.settings._WHEELPOS_DATA_AVG, self.settings._WHEELPOS_DATA_VAR, 2) - self.wheelpos = DriverProb(raw_priors=wheelpos_filter_raw_priors, max_trackable=self.settings._WHEELPOS_MAX_COUNT) + self.wheelpos_offsetter = RunningStatFilter(raw_priors=wheelpos_filter_raw_priors, max_trackable=self.settings._WHEELPOS_MAX_COUNT) self.pose = DriverPose(settings=self.settings) self.blink_prob = 0. self.phone_prob = 0. + self.alert_level = AlertLevel.none self.always_on = always_on - self.distracted_types = [] + self.distracted_types = defaultdict(bool) self.driver_distracted = False - self.driver_distraction_filter = FirstOrderFilter(0., self.settings._DISTRACTED_FILTER_TS, self.settings._DT_DMON) + self.driver_distraction_filter = FirstOrderFilter(0., self.settings._DISTRACTED_FILTER_TS, DT_DMON) self.wheel_on_right = False self.wheel_on_right_last = None self.wheel_on_right_default = rhd_saved @@ -158,61 +138,56 @@ class DriverMonitoring: self.terminal_alert_cnt = 0 self.terminal_time = 0 self.step_change = 0. - self.active_monitoring_mode = True + self.active_policy = MonitoringPolicy.vision + self.driver_interacting = False self.is_model_uncertain = False self.hi_stds = 0 - self.threshold_pre = self.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME - self.threshold_prompt = self.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME + self.model_std_max = 0. + self.threshold_alert_1 = 0. + self.threshold_alert_2 = 0. self.dcam_uncertain_cnt = 0 - self.dcam_uncertain_alerted = False # once per drive self.dcam_reset_cnt = 0 - - self.params = Params() - self.too_distracted = self.params.get_bool("DriverTooDistracted") + self.too_distracted = Params().get_bool("DriverTooDistracted") self._reset_awareness() - self._set_timers(active_monitoring=True) - self._reset_events() + self._set_policy(MonitoringPolicy.vision) def _reset_awareness(self): self.awareness = 1. - self.awareness_active = 1. - self.awareness_passive = 1. + self.last_vision_awareness = 1. + self.last_wheeltouch_awareness = 1. - def _reset_events(self): - self.current_events = Events() - - def _set_timers(self, active_monitoring): - if self.active_monitoring_mode and self.awareness <= self.threshold_prompt: - if active_monitoring: - self.step_change = self.settings._DT_DMON / self.settings._DISTRACTED_TIME + def _set_policy(self, target_policy): + if self.active_policy == MonitoringPolicy.vision and self.awareness <= self.threshold_alert_2: + if target_policy == MonitoringPolicy.vision: + self.step_change = DT_DMON / self.settings._VISION_POLICY_ALERT_3_TIMEOUT else: self.step_change = 0. return # no exploit after orange alert elif self.awareness <= 0.: return - if active_monitoring: + if target_policy == MonitoringPolicy.vision: # when falling back from passive mode to active mode, reset awareness to avoid false alert - if not self.active_monitoring_mode: - self.awareness_passive = self.awareness - self.awareness = self.awareness_active + if self.active_policy != MonitoringPolicy.vision: + self.last_wheeltouch_awareness = self.awareness + self.awareness = self.last_vision_awareness - self.threshold_pre = self.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME - self.threshold_prompt = self.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME - self.step_change = self.settings._DT_DMON / self.settings._DISTRACTED_TIME - self.active_monitoring_mode = True + self.threshold_alert_1 = 1. - self.settings._VISION_POLICY_ALERT_1_TIMEOUT / self.settings._VISION_POLICY_ALERT_3_TIMEOUT + self.threshold_alert_2 = 1. - self.settings._VISION_POLICY_ALERT_2_TIMEOUT / self.settings._VISION_POLICY_ALERT_3_TIMEOUT + self.step_change = DT_DMON / self.settings._VISION_POLICY_ALERT_3_TIMEOUT + self.active_policy = MonitoringPolicy.vision else: - if self.active_monitoring_mode: - self.awareness_active = self.awareness - self.awareness = self.awareness_passive + if self.active_policy == MonitoringPolicy.vision: + self.last_vision_awareness = self.awareness + self.awareness = self.last_wheeltouch_awareness - self.threshold_pre = self.settings._AWARENESS_PRE_TIME_TILL_TERMINAL / self.settings._AWARENESS_TIME - self.threshold_prompt = self.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL / self.settings._AWARENESS_TIME - self.step_change = self.settings._DT_DMON / self.settings._AWARENESS_TIME - self.active_monitoring_mode = False + self.threshold_alert_1 = 1. - self.settings._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT / self.settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT + self.threshold_alert_2 = 1. - self.settings._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT / self.settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT + self.step_change = DT_DMON / self.settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT + self.active_policy = MonitoringPolicy.wheeltouch - def _set_policy(self, brake_disengage_prob, car_speed): + def _set_pose_strictness(self, brake_disengage_prob, car_speed): bp = brake_disengage_prob k1 = max(-0.00156*((car_speed-16)**2)+0.6, 0.2) bp_normal = max(min(bp / k1, 0.5),0) @@ -224,15 +199,15 @@ class DriverMonitoring: self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD def _get_distracted_types(self): - distracted_types = [] + self.distracted_types = defaultdict(bool) if not self.pose.calibrated: pitch_error = self.pose.pitch - self.settings._PITCH_NATURAL_OFFSET yaw_error = self.pose.yaw - self.settings._YAW_NATURAL_OFFSET else: - pitch_error = self.pose.pitch - min(max(self.pose.pitch_offseter.filtered_stat.mean(), + pitch_error = self.pose.pitch - min(max(self.pose.pitch_offsetter.filtered_stat.mean(), self.settings._PITCH_MIN_OFFSET), self.settings._PITCH_MAX_OFFSET) - yaw_error = self.pose.yaw - min(max(self.pose.yaw_offseter.filtered_stat.mean(), + yaw_error = self.pose.yaw - min(max(self.pose.yaw_offsetter.filtered_stat.mean(), self.settings._YAW_MIN_OFFSET), self.settings._YAW_MAX_OFFSET) pitch_error = 0 if pitch_error > 0 else abs(pitch_error) # no positive pitch limit @@ -244,28 +219,21 @@ class DriverMonitoring: pitch_threshold = self.settings._POSE_PITCH_THRESHOLD * self.pose.cfactor_pitch if self.pose.calibrated else self.settings._PITCH_NATURAL_THRESHOLD yaw_threshold = self.settings._POSE_YAW_THRESHOLD * self.pose.cfactor_yaw - if pitch_error > pitch_threshold or yaw_error > yaw_threshold: - distracted_types.append(DistractedType.DISTRACTED_POSE) - - if self.blink_prob > self.settings._BLINK_THRESHOLD: - distracted_types.append(DistractedType.DISTRACTED_BLINK) - - if self.phone_prob > self.settings._PHONE_THRESH: - distracted_types.append(DistractedType.DISTRACTED_PHONE) - - return distracted_types + self.distracted_types['pose'] = bool((pitch_error > pitch_threshold) or (yaw_error > yaw_threshold)) + self.distracted_types['eye'] = bool(self.blink_prob > self.settings._BLINK_THRESHOLD) + self.distracted_types['phone'] = bool(self.phone_prob > self.settings._PHONE_THRESH) def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged, standstill, demo_mode=False, steering_angle_deg=0.): rhd_pred = driver_state.wheelOnRightProb # calibrates only when there's movement and either face detected if car_speed > self.settings._WHEELPOS_CALIB_MIN_SPEED and (driver_state.leftDriverData.faceProb > self.settings._FACE_THRESHOLD or driver_state.rightDriverData.faceProb > self.settings._FACE_THRESHOLD): - self.wheelpos.prob_offseter.push_and_update(rhd_pred) + self.wheelpos_offsetter.push_and_update(rhd_pred) - self.wheelpos.prob_calibrated = self.wheelpos.prob_offseter.filtered_stat.n > self.settings._WHEELPOS_FILTER_MIN_COUNT + wheelpos_calibrated = self.wheelpos_offsetter.filtered_stat.n >= self.settings._WHEELPOS_FILTER_MIN_COUNT - if self.wheelpos.prob_calibrated or demo_mode: - self.wheel_on_right = self.wheelpos.prob_offseter.filtered_stat.M > self.settings._WHEELPOS_THRESHOLD + if wheelpos_calibrated or demo_mode: + self.wheel_on_right = self.wheelpos_offsetter.filtered_stat.M > self.settings._WHEELPOS_THRESHOLD else: self.wheel_on_right = self.wheel_on_right_default # use default/saved if calibration is unfinished # make sure no switching when engaged @@ -277,68 +245,57 @@ class DriverMonitoring: return self.face_detected = driver_data.faceProb > self.settings._FACE_THRESHOLD - self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_data.faceOrientation, driver_data.facePosition, cal_rpy) + self.pose.pitch, self.pose.yaw = face_orientation_from_model(driver_data.faceOrientation, driver_data.facePosition, cal_rpy) steer_d = max(abs(steering_angle_deg) - self.settings._POSE_YAW_MIN_STEER_DEG, 0.) self.pose.steer_yaw_offset = radians(steer_d) * -np.sign(steering_angle_deg) * self.settings._POSE_YAW_STEER_FACTOR if self.wheel_on_right: self.pose.yaw *= -1 self.pose.steer_yaw_offset *= -1 self.wheel_on_right_last = self.wheel_on_right - self.pose.pitch_std = driver_data.faceOrientationStd[0] - self.pose.yaw_std = driver_data.faceOrientationStd[1] - model_std_max = max(self.pose.pitch_std, self.pose.yaw_std) - self.pose.low_std = model_std_max < self.settings._POSESTD_THRESHOLD + self.model_std_max = max(driver_data.faceOrientationStd[0], driver_data.faceOrientationStd[1]) + self.pose.low_std = self.model_std_max < self.settings._HI_STD_THRESHOLD self.blink_prob = driver_data.eyesClosedProb * (driver_data.eyesVisibleProb > self.settings._EYE_THRESHOLD) self.phone_prob = driver_data.phoneProb - self.distracted_types = self._get_distracted_types() - self.driver_distracted = (DistractedType.DISTRACTED_PHONE in self.distracted_types - or DistractedType.DISTRACTED_POSE in self.distracted_types - or DistractedType.DISTRACTED_BLINK in self.distracted_types) \ - and driver_data.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std + self._get_distracted_types() + self.driver_distracted = any(self.distracted_types.values()) and driver_data.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std self.driver_distraction_filter.update(self.driver_distracted) - # update offseter - # only update when driver is actively driving the car above a certain speed + # only update offsetter when driver is actively driving the car above a certain speed if self.face_detected and car_speed > self.settings._POSE_CALIB_MIN_SPEED and self.pose.low_std and (not op_engaged or not self.driver_distracted): - self.pose.pitch_offseter.push_and_update(self.pose.pitch) - self.pose.yaw_offseter.push_and_update(self.pose.yaw) + self.pose.pitch_offsetter.push_and_update(self.pose.pitch) + self.pose.yaw_offsetter.push_and_update(self.pose.yaw) - self.pose.calibrated = self.pose.pitch_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT and \ - self.pose.yaw_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT + self.pose.calibrated = self.pose.pitch_offsetter.filtered_stat.n >= self.settings._POSE_OFFSET_MIN_COUNT and \ + self.pose.yaw_offsetter.filtered_stat.n >= self.settings._POSE_OFFSET_MIN_COUNT if self.face_detected and not self.driver_distracted: - if model_std_max > self.settings._DCAM_UNCERTAIN_ALERT_THRESHOLD: - if not standstill: - self.dcam_uncertain_cnt += 1 - self.dcam_reset_cnt = 0 + dcam_uncertain = self.model_std_max > self.settings._DCAM_UNCERTAIN_ALERT_THRESHOLD + if dcam_uncertain and not standstill: + self.dcam_uncertain_cnt += 1 + self.dcam_reset_cnt = 0 else: self.dcam_reset_cnt += 1 if self.dcam_reset_cnt > self.settings._DCAM_UNCERTAIN_RESET_COUNT: self.dcam_uncertain_cnt = 0 - self.is_model_uncertain = self.hi_stds > self.settings._HI_STD_FALLBACK_TIME - self._set_timers(self.face_detected and not self.is_model_uncertain) + self.is_model_uncertain = self.hi_stds >= self.settings._HI_STD_FALLBACK_TIME + self._set_policy(MonitoringPolicy.vision if self.face_detected and not self.is_model_uncertain else MonitoringPolicy.wheeltouch) if self.face_detected and not self.pose.low_std and not self.driver_distracted: self.hi_stds += 1 elif self.face_detected and self.pose.low_std: self.hi_stds = 0 - def _update_events(self, driver_engaged, op_engaged, standstill, wrong_gear, car_speed): - self._reset_events() - # Block engaging until ignition cycle after max number or time of distractions + def _update_events(self, driver_engaged, op_engaged, standstill, wrong_gear): + self.alert_level = AlertLevel.none + self.driver_interacting = driver_engaged + if self.terminal_alert_cnt >= self.settings._MAX_TERMINAL_ALERTS or \ self.terminal_time >= self.settings._MAX_TERMINAL_DURATION: - if not self.too_distracted: - self.params.put_bool_nonblocking("DriverTooDistracted", True) self.too_distracted = True - # Always-on distraction lockout is temporary - if self.too_distracted or (self.always_on and self.awareness <= self.threshold_prompt): - self.current_events.add(EventName.tooDistracted) - always_on_valid = self.always_on and not wrong_gear - if (driver_engaged and self.awareness > 0 and not self.active_monitoring_mode) or \ + if (self.driver_interacting and self.awareness > 0 and self.active_policy == MonitoringPolicy.wheeltouch) or \ (not always_on_valid and not op_engaged) or \ (always_on_valid and not op_engaged and self.awareness <= 0): # always reset on disengage with normal mode; disengage resets only on red if always on @@ -346,111 +303,118 @@ class DriverMonitoring: return awareness_prev = self.awareness - _reaching_pre = self.awareness - self.step_change <= self.threshold_pre - _reaching_terminal = self.awareness - self.step_change <= 0 - standstill_orange_exemption = standstill and _reaching_pre - always_on_red_exemption = always_on_valid and not op_engaged and _reaching_terminal + _reaching_alert_1 = self.awareness - self.step_change <= self.threshold_alert_1 + _reaching_alert_3 = self.awareness - self.step_change <= 0 + standstill_exemption = standstill and _reaching_alert_1 + always_on_exemption = always_on_valid and not op_engaged and _reaching_alert_3 if self.awareness > 0 and \ - ((self.driver_distraction_filter.x < 0.37 and self.face_detected and self.pose.low_std) or standstill_orange_exemption): - if driver_engaged: + ((self.driver_distraction_filter.x < 0.37 and self.face_detected and self.pose.low_std) or standstill_exemption): + if self.driver_interacting: self._reset_awareness() return # only restore awareness when paying attention and alert is not red - self.awareness = min(self.awareness + ((self.settings._RECOVERY_FACTOR_MAX-self.settings._RECOVERY_FACTOR_MIN)* - (1.-self.awareness)+self.settings._RECOVERY_FACTOR_MIN)*self.step_change, 1.) + self.awareness = min(self.awareness + ((self.settings._TIMEOUT_RECOVERY_FACTOR_MAX-self.settings._TIMEOUT_RECOVERY_FACTOR_MIN)* + (1.-self.awareness)+self.settings._TIMEOUT_RECOVERY_FACTOR_MIN)*self.step_change, 1.) if self.awareness == 1.: - self.awareness_passive = min(self.awareness_passive + self.step_change, 1.) + self.last_wheeltouch_awareness = min(self.last_wheeltouch_awareness + self.step_change, 1.) # don't display alert banner when awareness is recovering and has cleared orange - if self.awareness > self.threshold_prompt: + if self.awareness > self.threshold_alert_2: return certainly_distracted = self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected - maybe_distracted = self.hi_stds > self.settings._HI_STD_FALLBACK_TIME or not self.face_detected + maybe_distracted = self.is_model_uncertain or not self.face_detected if certainly_distracted or maybe_distracted: # should always be counting if distracted unless at standstill and reaching green # also will not be reaching 0 if DM is active when not engaged - if not (standstill_orange_exemption or always_on_red_exemption): + if not (standstill_exemption or always_on_exemption): self.awareness = max(self.awareness - self.step_change, -0.1) - alert = None if self.awareness <= 0.: - # terminal red alert: disengagement required - alert = EventName.driverDistracted3 if self.active_monitoring_mode else EventName.driverUnresponsive3 + # terminal alert: disengagement required + self.alert_level = AlertLevel.three self.terminal_time += 1 if awareness_prev > 0.: self.terminal_alert_cnt += 1 - elif self.awareness <= self.threshold_prompt: - # prompt orange alert - alert = EventName.driverDistracted2 if self.active_monitoring_mode else EventName.driverUnresponsive2 - elif self.awareness <= self.threshold_pre: - # pre green alert - alert = EventName.driverDistracted1 if self.active_monitoring_mode else EventName.driverUnresponsive1 - - if alert is not None: - self.current_events.add(alert) - - if self.dcam_uncertain_cnt > self.settings._DCAM_UNCERTAIN_ALERT_COUNT and not self.dcam_uncertain_alerted: - set_offroad_alert("Offroad_DriverMonitoringUncertain", True) - self.dcam_uncertain_alerted = True - + elif self.awareness <= self.threshold_alert_2: + self.alert_level = AlertLevel.two + elif self.awareness <= self.threshold_alert_1: + self.alert_level = AlertLevel.one def get_state_packet(self, valid=True): # build driverMonitoringState packet dat = messaging.new_message('driverMonitoringState', valid=valid) - dat.driverMonitoringState = { - "events": self.current_events.to_msg(), - "faceDetected": self.face_detected, - "isDistracted": self.driver_distracted, - "distractedType": sum(self.distracted_types), - "awarenessStatus": self.awareness, - "posePitchOffset": self.pose.pitch_offseter.filtered_stat.mean(), - "posePitchValidCount": self.pose.pitch_offseter.filtered_stat.n, - "poseYawOffset": self.pose.yaw_offseter.filtered_stat.mean(), - "poseYawValidCount": self.pose.yaw_offseter.filtered_stat.n, - "stepChange": self.step_change, - "awarenessActive": self.awareness_active, - "awarenessPassive": self.awareness_passive, - "isLowStd": self.pose.low_std, - "hiStdCount": self.hi_stds, - "isActiveMode": self.active_monitoring_mode, - "isRHD": self.wheel_on_right, - "uncertainCount": self.dcam_uncertain_cnt, - } + dm = dat.driverMonitoringState + + dm.lockout = self.too_distracted + dm.alertCountLockoutPercent = to_percent(self.terminal_alert_cnt / self.settings._MAX_TERMINAL_ALERTS) + dm.alertTimeLockoutPercent = to_percent(self.terminal_time / self.settings._MAX_TERMINAL_DURATION) + dm.alwaysOn = self.always_on + dm.alwaysOnLockout = self.always_on and self.awareness <= self.threshold_alert_2 + dm.alertLevel = self.alert_level + dm.activePolicy = self.active_policy + dm.isRHD = self.wheel_on_right + dm.rhdCalibration.calibratedPercent = to_percent(self.wheelpos_offsetter.filtered_stat.n / self.settings._WHEELPOS_FILTER_MIN_COUNT) + dm.rhdCalibration.offset = self.wheelpos_offsetter.filtered_stat.M + + dm.visionPolicyState.awarenessPercent = to_percent(self.last_vision_awareness if self.active_policy != MonitoringPolicy.vision else self.awareness) + dm.visionPolicyState.awarenessStep = self.step_change if self.active_policy == MonitoringPolicy.vision else 0. + dm.visionPolicyState.isDistracted = self.driver_distracted + dm.visionPolicyState.distractedTypes.pose = self.distracted_types['pose'] + dm.visionPolicyState.distractedTypes.eye = self.distracted_types['eye'] + dm.visionPolicyState.distractedTypes.phone = self.distracted_types['phone'] + dm.visionPolicyState.faceDetected = self.face_detected + dm.visionPolicyState.pose.pitch = self.pose.pitch + dm.visionPolicyState.pose.yaw = self.pose.yaw + dm.visionPolicyState.pose.calibrated = self.pose.calibrated + dm.visionPolicyState.pose.pitchCalib.calibratedPercent = to_percent(self.pose.pitch_offsetter.filtered_stat.n / self.settings._POSE_OFFSET_MIN_COUNT) + dm.visionPolicyState.pose.pitchCalib.offset = self.pose.pitch_offsetter.filtered_stat.M + dm.visionPolicyState.pose.yawCalib.calibratedPercent = to_percent(self.pose.yaw_offsetter.filtered_stat.n / self.settings._POSE_OFFSET_MIN_COUNT) + dm.visionPolicyState.pose.yawCalib.offset = self.pose.yaw_offsetter.filtered_stat.M + dm.visionPolicyState.pose.uncertainty = self.model_std_max + dm.visionPolicyState.wheeltouchFallbackPercent = to_percent(self.hi_stds / self.settings._HI_STD_FALLBACK_TIME) + dm.visionPolicyState.uncertainOffroadAlertPercent = to_percent(self.dcam_uncertain_cnt / self.settings._DCAM_UNCERTAIN_ALERT_COUNT) + + dm.wheeltouchPolicyState.awarenessPercent = to_percent(self.last_wheeltouch_awareness if self.active_policy == MonitoringPolicy.vision else self.awareness) + dm.wheeltouchPolicyState.awarenessStep = 0. if self.active_policy == MonitoringPolicy.vision else self.step_change + dm.wheeltouchPolicyState.driverInteracting = self.driver_interacting return dat def run_step(self, sm, demo=False): if demo: - highway_speed = 30 + car_speed = 30 enabled = True wrong_gear = False standstill = False driver_engaged = False brake_disengage_prob = 1.0 + steering_angle_deg = 0.0 rpyCalib = [0., 0., 0.] else: - highway_speed = sm['carState'].vEgo + car_speed = sm['carState'].vEgo enabled = sm['selfdriveState'].enabled or sm['carControl'].latActive wrong_gear = sm['carState'].gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low) standstill = sm['carState'].standstill driver_engaged = sm['carState'].steeringPressed or sm['carState'].gasPressed brake_disengage_prob = sm['modelV2'].meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s + steering_angle_deg = sm['carState'].steeringAngleDeg rpyCalib = sm['liveCalibration'].rpyCalib - self._set_policy( + + self._set_pose_strictness( brake_disengage_prob=brake_disengage_prob, - car_speed=highway_speed, + car_speed=car_speed, ) # Parse data from dmonitoringmodeld self._update_states( driver_state=sm['driverStateV2'], cal_rpy=rpyCalib, - car_speed=highway_speed, + car_speed=car_speed, op_engaged=enabled, standstill=standstill, demo_mode=demo, - steering_angle_deg=sm['carState'].steeringAngleDeg, + steering_angle_deg=steering_angle_deg, ) # Update distraction events @@ -459,5 +423,4 @@ class DriverMonitoring: op_engaged=enabled, standstill=standstill, wrong_gear=wrong_gear, - car_speed=highway_speed ) diff --git a/selfdrive/monitoring/test_monitoring.py b/selfdrive/monitoring/test_monitoring.py index ee6028d609..948931cb29 100644 --- a/selfdrive/monitoring/test_monitoring.py +++ b/selfdrive/monitoring/test_monitoring.py @@ -1,19 +1,17 @@ import numpy as np -import pytest -from cereal import log, car +from cereal import log from openpilot.common.realtime import DT_DMON -from openpilot.selfdrive.monitoring.helpers import DriverMonitoring, DRIVER_MONITOR_SETTINGS -from openpilot.system.hardware import HARDWARE +from openpilot.selfdrive.monitoring.policy import DriverMonitoring, DRIVER_MONITOR_SETTINGS EventName = log.OnroadEvent.EventName -dm_settings = DRIVER_MONITOR_SETTINGS(device_type=HARDWARE.get_device_type()) +dm_settings = DRIVER_MONITOR_SETTINGS() TEST_TIMESPAN = 120 # seconds -DISTRACTED_SECONDS_TO_ORANGE = dm_settings._DISTRACTED_TIME - dm_settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL + 1 -DISTRACTED_SECONDS_TO_RED = dm_settings._DISTRACTED_TIME + 1 -INVISIBLE_SECONDS_TO_ORANGE = dm_settings._AWARENESS_TIME - dm_settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL + 1 -INVISIBLE_SECONDS_TO_RED = dm_settings._AWARENESS_TIME + 1 +DISTRACTED_SECONDS_TO_ORANGE = dm_settings._VISION_POLICY_ALERT_2_TIMEOUT + 1 +DISTRACTED_SECONDS_TO_RED = dm_settings._VISION_POLICY_ALERT_3_TIMEOUT + 1 +INVISIBLE_SECONDS_TO_ORANGE = dm_settings._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT + 1 +INVISIBLE_SECONDS_TO_RED = dm_settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT + 1 def make_msg(face_detected, distracted=False, model_uncertain=False): ds = log.DriverStateV2.new_message() @@ -35,7 +33,7 @@ msg_ATTENTIVE = make_msg(True) msg_DISTRACTED = make_msg(True, distracted=True) msg_ATTENTIVE_UNCERTAIN = make_msg(True, model_uncertain=True) msg_DISTRACTED_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=True) -msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=dm_settings._POSESTD_THRESHOLD*1.5) +msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=dm_settings._HI_STD_THRESHOLD*1.5) # driver interaction with car car_interaction_DETECTED = True @@ -51,49 +49,49 @@ always_false = [False] * int(TEST_TIMESPAN / DT_DMON) class TestMonitoring: def _run_seq(self, msgs, interaction, engaged, standstill): DM = DriverMonitoring() - events = [] + alert_lvls = [] for idx in range(len(msgs)): DM._update_states(msgs[idx], [0, 0, 0], 0, engaged[idx], standstill[idx]) # cal_rpy and car_speed don't matter here # evaluate events at 10Hz for tests - DM._update_events(interaction[idx], engaged[idx], standstill[idx], 0, 0) - events.append(DM.current_events) - assert len(events) == len(msgs), f"got {len(events)} for {len(msgs)} driverState input msgs" - return events, DM + DM._update_events(interaction[idx], engaged[idx], standstill[idx], 0) + alert_lvls.append(DM.alert_level) + assert len(alert_lvls) == len(msgs), f"got {len(alert_lvls)} for {len(msgs)} driverState input msgs" + return alert_lvls, DM - def _assert_no_events(self, events): - assert all(not len(e) for e in events) # engaged, driver is attentive all the time def test_fully_aware_driver(self): - events, _ = self._run_seq(always_attentive, always_false, always_true, always_false) - self._assert_no_events(events) + alert_lvls, d_status = self._run_seq(always_attentive, always_false, always_true, always_false) + assert all(a == 0 for a in alert_lvls) + assert d_status.active_policy == log.DriverMonitoringState.MonitoringPolicy.vision # engaged, driver is distracted and does nothing def test_fully_distracted_driver(self): - events, d_status = self._run_seq(always_distracted, always_false, always_true, always_false) - assert len(events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)]) == 0 - assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL + \ - ((d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL-d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == \ - EventName.driverDistracted1 - assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL + \ - ((d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == EventName.driverDistracted2 - assert events[int((d_status.settings._DISTRACTED_TIME + \ - ((TEST_TIMESPAN-10-d_status.settings._DISTRACTED_TIME)/2))/DT_DMON)].names[0] == EventName.driverDistracted3 + alert_lvls, d_status = self._run_seq(always_distracted, always_false, always_true, always_false) + s = d_status.settings + assert alert_lvls[int(s._VISION_POLICY_ALERT_1_TIMEOUT / 2 / DT_DMON)] == 0 + assert alert_lvls[int((s._VISION_POLICY_ALERT_1_TIMEOUT + \ + (s._VISION_POLICY_ALERT_2_TIMEOUT - s._VISION_POLICY_ALERT_1_TIMEOUT) / 2) / DT_DMON)] == 1 + assert alert_lvls[int((s._VISION_POLICY_ALERT_2_TIMEOUT + \ + (s._VISION_POLICY_ALERT_3_TIMEOUT - s._VISION_POLICY_ALERT_2_TIMEOUT) / 2) / DT_DMON)] == 2 + assert alert_lvls[int((s._VISION_POLICY_ALERT_3_TIMEOUT + \ + (TEST_TIMESPAN - 10 - s._VISION_POLICY_ALERT_3_TIMEOUT) / 2) / DT_DMON)] == 3 assert isinstance(d_status.awareness, float) # engaged, no face detected the whole time, no action def test_fully_invisible_driver(self): - events, d_status = self._run_seq(always_no_face, always_false, always_true, always_false) - assert len(events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)]) == 0 - assert events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL + \ - ((d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL-d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == \ - EventName.driverUnresponsive1 - assert events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL + \ - ((d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == EventName.driverUnresponsive2 - assert events[int((d_status.settings._AWARENESS_TIME + \ - ((TEST_TIMESPAN-10-d_status.settings._AWARENESS_TIME)/2))/DT_DMON)].names[0] == EventName.driverUnresponsive3 + alert_lvls, d_status = self._run_seq(always_no_face, always_false, always_true, always_false) + s = d_status.settings + assert alert_lvls[int(s._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT / 2 / DT_DMON)] == 0 + assert alert_lvls[int((s._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT + \ + (s._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT - s._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT) / 2) / DT_DMON)] == 1 + assert alert_lvls[int((s._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT + \ + (s._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT - s._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT) / 2) / DT_DMON)] == 2 + assert alert_lvls[int((s._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT + \ + (TEST_TIMESPAN - 10 - s._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT) / 2) / DT_DMON)] == 3 + assert d_status.active_policy == log.DriverMonitoringState.MonitoringPolicy.wheeltouch # engaged, down to orange, driver pays attention, back to normal; then down to orange, driver touches wheel # - should have short orange recovery time and no green afterwards; wheel touch only recovers when paying attention @@ -104,13 +102,13 @@ class TestMonitoring: [msg_ATTENTIVE] * (int(TEST_TIMESPAN/DT_DMON)-int((DISTRACTED_SECONDS_TO_ORANGE*3+2)/DT_DMON)) interaction_vector = [car_interaction_NOT_DETECTED] * int(DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON) + \ [car_interaction_DETECTED] * (int(TEST_TIMESPAN/DT_DMON)-int(DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON)) - events, _ = self._run_seq(ds_vector, interaction_vector, always_true, always_false) - assert len(events[int(DISTRACTED_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0 - assert events[int((DISTRACTED_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0] == EventName.driverDistracted2 - assert len(events[int(DISTRACTED_SECONDS_TO_ORANGE*1.5/DT_DMON)]) == 0 - assert events[int((DISTRACTED_SECONDS_TO_ORANGE*3-0.1)/DT_DMON)].names[0] == EventName.driverDistracted2 - assert events[int((DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)].names[0] == EventName.driverDistracted2 - assert len(events[int((DISTRACTED_SECONDS_TO_ORANGE*3+2.5)/DT_DMON)]) == 0 + alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, always_true, always_false) + assert alert_lvls[int(DISTRACTED_SECONDS_TO_ORANGE*0.5/DT_DMON)] == 0 + assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2 + assert alert_lvls[int(DISTRACTED_SECONDS_TO_ORANGE*1.5/DT_DMON)] == 0 + assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE*3-0.1)/DT_DMON)] == 2 + assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)] == 2 + assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE*3+2.5)/DT_DMON)] == 0 # engaged, down to orange, driver dodges camera, then comes back still distracted, down to red, \ # driver dodges, and then touches wheel to no avail, disengages and reengages @@ -128,11 +126,11 @@ class TestMonitoring: = [True] * int(1/DT_DMON) op_vector[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+2.5)/DT_DMON):int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3)/DT_DMON)] \ = [False] * int(0.5/DT_DMON) - events, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false) - assert events[int((DISTRACTED_SECONDS_TO_ORANGE+0.5*_invisible_time)/DT_DMON)].names[0] == EventName.driverDistracted2 - assert events[int((DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)].names[0] == EventName.driverDistracted3 - assert events[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)].names[0] == EventName.driverDistracted3 - assert len(events[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3.5)/DT_DMON)]) == 0 + alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false) + assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE+0.5*_invisible_time)/DT_DMON)] == 2 + assert alert_lvls[int((DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)] == 3 + assert alert_lvls[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)] == 3 + assert alert_lvls[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3.5)/DT_DMON)] == 0 # engaged, invisible driver, down to orange, driver touches wheel; then down to orange again, driver appears # - both actions should clear the alert, but momentary appearance should not @@ -143,16 +141,16 @@ class TestMonitoring: ds_vector[int((2*INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON):int((2*INVISIBLE_SECONDS_TO_ORANGE+1+_visible_time)/DT_DMON)] = \ [msg_ATTENTIVE] * int(_visible_time/DT_DMON) interaction_vector[int((INVISIBLE_SECONDS_TO_ORANGE)/DT_DMON):int((INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON)] = [True] * int(1/DT_DMON) - events, _ = self._run_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false) - assert len(events[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0 - assert events[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0] == EventName.driverUnresponsive2 - assert len(events[int((INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)]) == 0 + alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false) + assert alert_lvls[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)] == 0 + assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2 + assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)] == 0 if _visible_time == 0.5: - assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0] == EventName.driverUnresponsive2 - assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)].names[0] == EventName.driverUnresponsive1 + assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)] == 2 + assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)] == 1 elif _visible_time == 10: - assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0] == EventName.driverUnresponsive2 - assert len(events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)]) == 0 + assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)] == 2 + assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)] == 0 # engaged, invisible driver, down to red, driver appears and then touches wheel, then disengages/reengages # - only disengage will clear the alert @@ -164,19 +162,19 @@ class TestMonitoring: ds_vector[int(INVISIBLE_SECONDS_TO_RED/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON)] = [msg_ATTENTIVE] * int(_visible_time/DT_DMON) interaction_vector[int((INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON)] = [True] * int(1/DT_DMON) op_vector[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)] = [False] * int(0.5/DT_DMON) - events, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false) - assert len(events[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0 - assert events[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0] == EventName.driverUnresponsive2 - assert events[int((INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)].names[0] == EventName.driverUnresponsive3 - assert events[int((INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)].names[0] == EventName.driverUnresponsive3 - assert events[int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)].names[0] == EventName.driverUnresponsive3 - assert len(events[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1+0.1)/DT_DMON)]) == 0 + alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false) + assert alert_lvls[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)] == 0 + assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2 + assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)] == 3 + assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)] == 3 + assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)] == 3 + assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1+0.1)/DT_DMON)] == 0 # disengaged, always distracted driver # - dm should stay quiet when not engaged def test_pure_dashcam_user(self): - events, _ = self._run_seq(always_distracted, always_false, always_false, always_false) - assert sum(len(event) for event in events) == 0 + alert_lvls, _ = self._run_seq(always_distracted, always_false, always_false, always_false) + assert all(a == 0 for a in alert_lvls) # engaged, car stops at traffic light, down to orange, no action, then car starts moving # - should only reach green when stopped, but continues counting down on launch @@ -184,11 +182,12 @@ class TestMonitoring: _redlight_time = 60 # seconds standstill_vector = always_true[:] standstill_vector[int(_redlight_time/DT_DMON):] = [False] * int((TEST_TIMESPAN-_redlight_time)/DT_DMON) - events, d_status = self._run_seq(always_distracted, always_false, always_true, standstill_vector) - assert len(events[int((_redlight_time-0.1)/DT_DMON)]) == 0 - _pre_to_prompt = d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL - d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL - assert events[int((_redlight_time+0.5)/DT_DMON)].names[0] == EventName.driverDistracted1 - assert events[int((_redlight_time+_pre_to_prompt+0.5)/DT_DMON)].names[0] == EventName.driverDistracted2 + alert_lvls, d_status = self._run_seq(always_distracted, always_false, always_true, standstill_vector) + s = d_status.settings + assert alert_lvls[int((_redlight_time-0.1)/DT_DMON)] == 0 + _alert_1_to_2 = s._VISION_POLICY_ALERT_2_TIMEOUT - s._VISION_POLICY_ALERT_1_TIMEOUT + assert alert_lvls[int((_redlight_time+0.5)/DT_DMON)] == 1 + assert alert_lvls[int((_redlight_time+_alert_1_to_2+0.5)/DT_DMON)] == 2 # engaged, distracted while moving, then car stops after reaching orange # - should reset timer to pre green at standstill @@ -196,83 +195,18 @@ class TestMonitoring: _stop_time = DISTRACTED_SECONDS_TO_ORANGE + 1 # stop 1 second after reaching orange standstill_vector = always_false[:] standstill_vector[int(_stop_time/DT_DMON):] = [True] * int((TEST_TIMESPAN-_stop_time)/DT_DMON) - events, _ = self._run_seq(always_distracted, always_false, always_true, standstill_vector) + alert_lvls, _ = self._run_seq(always_distracted, always_false, always_true, standstill_vector) # just before and briefly after stopping: orange alert; goes away quickly after stopped - assert events[int((_stop_time+0.1)/DT_DMON)].names[0] == EventName.driverDistracted2 - assert len(events[int((_stop_time+0.5)/DT_DMON)]) == 0 + assert alert_lvls[int((_stop_time+0.1)/DT_DMON)] == 2 + assert alert_lvls[int((_stop_time+0.5)/DT_DMON)] == 0 # engaged, model is somehow uncertain and driver is distracted # - should fall back to wheel touch after uncertain alert def test_somehow_indecisive_model(self): ds_vector = [msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN] * int(TEST_TIMESPAN/DT_DMON) interaction_vector = always_false[:] - events, d_status = self._run_seq(ds_vector, interaction_vector, always_true, always_false) - assert EventName.driverUnresponsive1 in \ - events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME-0.1)/DT_DMON)].names - assert EventName.driverUnresponsive2 in \ - events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names - assert EventName.driverUnresponsive3 in \ - events[int((INVISIBLE_SECONDS_TO_RED-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names - - -@pytest.mark.parametrize("enabled_state, lat_active_state, expected", [ - (False, False, False), # Both Disabled - (True, False, True), # OP Enabled, Lat Inactive - (False, True, True), # OP Disabled, Lat Active (e.g. MADS) - (True, True, True) # Both Active -]) -def test_enabled_states(enabled_state, lat_active_state, expected): - """ - Test DriverMonitoring.run_step with all 4 combinations of: - - selfdriveState.enabled (True/False) - - carControl.latActive (True/False) - """ - cs = car.CarState.new_message() - cs.vEgo = 30.0 - cs.gearShifter = car.CarState.GearShifter.drive - cs.standstill = False - cs.steeringPressed = False - cs.gasPressed = False - - ss = log.SelfdriveState.new_message() - ss.enabled = enabled_state - - cc = car.CarControl.new_message() - cc.latActive = lat_active_state - - mv2 = log.ModelDataV2.new_message() - mv2.meta.disengagePredictions.brakeDisengageProbs = [0.0] - - lc = log.LiveCalibrationData.new_message() - lc.rpyCalib = [0.0, 0.0, 0.0] - - ds = make_msg(False) - - sm = { - 'carState': cs, - 'selfdriveState': ss, - 'carControl': cc, - 'modelV2': mv2, - 'liveCalibration': lc, - 'driverStateV2': ds - } - - driver_monitoring = DriverMonitoring() - - # run_test doesn't assign enabled to a variable, so we need to spy on _update_events to see its value - captured_args = [] - original_update_events = driver_monitoring._update_events - - def spy_update_events(driver_engaged, op_engaged, standstill, wrong_gear, car_speed): - captured_args.append(op_engaged) - return original_update_events(driver_engaged, op_engaged, standstill, wrong_gear, car_speed) - - driver_monitoring._update_events = spy_update_events - - driver_monitoring.run_step(sm, demo=False) - - # Assertion - assert len(captured_args) == 1, "Expected _update_events to be called exactly once" - actual_enabled = captured_args[0] - - assert actual_enabled == expected, f"Expected op_engaged={expected}, but got {actual_enabled}" + alert_lvls, d_status = self._run_seq(ds_vector, interaction_vector, always_true, always_false) + s = d_status.settings + assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*s._HI_STD_FALLBACK_TIME-0.1)/DT_DMON)] == 1 + assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*s._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)] == 2 + assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED-1+DT_DMON*s._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)] == 3 From 41dea5d48d70b7fd47ab9a413b43aae9df9d80ce Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Sun, 26 Apr 2026 13:02:31 -0700 Subject: [PATCH 13/14] Revert "sync dmonitoring too" This reverts commit dc11e5fd84e351d85c6ea4396cef5206a71a401b. --- selfdrive/modeld/dmonitoringmodeld.py | 24 +- selfdrive/monitoring/dmonitoringd.py | 11 +- selfdrive/monitoring/helpers.py | 373 +++++++++++++----------- selfdrive/monitoring/test_monitoring.py | 222 +++++++++----- 4 files changed, 371 insertions(+), 259 deletions(-) diff --git a/selfdrive/modeld/dmonitoringmodeld.py b/selfdrive/modeld/dmonitoringmodeld.py index 18a79136ae..78749b773a 100755 --- a/selfdrive/modeld/dmonitoringmodeld.py +++ b/selfdrive/modeld/dmonitoringmodeld.py @@ -1,8 +1,12 @@ #!/usr/bin/env python3 import os -from openpilot.selfdrive.modeld.helpers import MODELS_DIR, CompileConfig, set_tinygrad_backend_from_compiled_flags +from openpilot.selfdrive.modeld.tinygrad_helpers import MODELS_DIR, set_tinygrad_backend_from_compiled_flags set_tinygrad_backend_from_compiled_flags() +# FIXME-SP: remove once we bump tg +from openpilot.system.hardware import TICI +os.environ['DEV'] = 'QCOM' if TICI else 'CPU' + from tinygrad.tensor import Tensor import time import pickle @@ -28,7 +32,7 @@ class ModelState: inputs: dict[str, np.ndarray] output: np.ndarray - def __init__(self, cam_w: int, cam_h: int): + def __init__(self): with open(METADATA_PATH, 'rb') as f: model_metadata = pickle.load(f) self.input_shapes = model_metadata['input_shapes'] @@ -40,18 +44,22 @@ class ModelState: self.warp_inputs_np = {'transform': np.zeros((3,3), dtype=np.float32)} self.warp_inputs = {k: Tensor(v, device='NPY') for k,v in self.warp_inputs_np.items()} - self.frame_buf_params = get_nv12_info(cam_w, cam_h) + self.frame_buf_params = None self.tensor_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()} self._blob_cache : dict[int, Tensor] = {} + self.image_warp = None self.model_run = pickle.loads(read_file_chunked(str(MODEL_PKL_PATH))) - with open(CompileConfig(cam_w, cam_h, prefix='dm_', prepare_only=True).pkl_path, "rb") as f: - self.image_warp = pickle.load(f) def run(self, buf: VisionBuf, calib: np.ndarray, transform: np.ndarray) -> tuple[np.ndarray, float]: self.numpy_inputs['calib'][0,:] = calib t1 = time.perf_counter() + if self.image_warp is None: + self.frame_buf_params = get_nv12_info(buf.width, buf.height) + warp_path = MODELS_DIR / f'dm_warp_{buf.width}x{buf.height}_tinygrad.pkl' + with open(warp_path, "rb") as f: + self.image_warp = pickle.load(f) ptr = buf.data.ctypes.data # There is a ringbuffer of imgs, just cache tensors pointing to all of them if ptr not in self._blob_cache: @@ -105,6 +113,9 @@ def get_driverstate_packet(model_output, frame_id: int, location_ts: int, exec_t def main(): config_realtime_process(7, 5) + model = ModelState() + cloudlog.warning("models loaded, dmonitoringmodeld starting") + cloudlog.warning("connecting to driver stream") vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_DRIVER, True) while not vipc_client.connect(False): @@ -112,9 +123,6 @@ def main(): assert vipc_client.is_connected() cloudlog.warning(f"connected with buffer size: {vipc_client.buffer_len}") - model = ModelState(vipc_client.width, vipc_client.height) - cloudlog.warning("models loaded, dmonitoringmodeld starting") - sm = SubMaster(["liveCalibration"]) pm = PubMaster(["driverStateV2"]) diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index 415853513e..022415af6d 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -2,7 +2,7 @@ import cereal.messaging as messaging from openpilot.common.params import Params from openpilot.common.realtime import config_realtime_process -from openpilot.selfdrive.monitoring.policy import DriverMonitoring +from openpilot.selfdrive.monitoring.helpers import DriverMonitoring def dmonitoringd_thread(): @@ -10,7 +10,8 @@ def dmonitoringd_thread(): params = Params() pm = messaging.PubMaster(['driverMonitoringState']) - sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'selfdriveState', 'modelV2'], poll='driverStateV2') + sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'selfdriveState', 'modelV2', + 'carControl'], poll='driverStateV2') DM = DriverMonitoring(rhd_saved=params.get_bool("IsRhdDetected"), always_on=params.get_bool("AlwaysOnDM")) demo_mode=False @@ -24,7 +25,7 @@ def dmonitoringd_thread(): valid = sm.all_checks() if demo_mode and sm.valid['driverStateV2']: - DM.run_step(sm, demo=True) + DM.run_step(sm, demo=demo_mode) elif valid: DM.run_step(sm, demo=demo_mode) @@ -39,8 +40,8 @@ def dmonitoringd_thread(): # save rhd virtual toggle every 5 mins if (sm['driverStateV2'].frameId % 6000 == 0 and not demo_mode and - DM.wheelpos_offsetter.filtered_stat.n > DM.settings._WHEELPOS_FILTER_MIN_COUNT and - DM.wheel_on_right == (DM.wheelpos_offsetter.filtered_stat.M > DM.settings._WHEELPOS_THRESHOLD)): + DM.wheelpos.prob_offseter.filtered_stat.n > DM.settings._WHEELPOS_FILTER_MIN_COUNT and + DM.wheel_on_right == (DM.wheelpos.prob_offseter.filtered_stat.M > DM.settings._WHEELPOS_THRESHOLD)): params.put_bool_nonblocking("IsRhdDetected", DM.wheel_on_right) def main(): diff --git a/selfdrive/monitoring/helpers.py b/selfdrive/monitoring/helpers.py index 8e8116145e..6c81fac7f0 100644 --- a/selfdrive/monitoring/helpers.py +++ b/selfdrive/monitoring/helpers.py @@ -1,20 +1,18 @@ -from collections import defaultdict from math import atan2, radians import numpy as np from cereal import car, log import cereal.messaging as messaging +from openpilot.selfdrive.selfdrived.events import Events +from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert from openpilot.common.realtime import DT_DMON from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.params import Params from openpilot.common.stat_live import RunningStatFilter from openpilot.common.transformations.camera import DEVICE_CAMERAS +from openpilot.system.hardware import HARDWARE -AlertLevel = log.DriverMonitoringState.AlertLevel -MonitoringPolicy = log.DriverMonitoringState.MonitoringPolicy - -def to_percent(v): - return int(min(max(v * 100., 0.), 100.)) +EventName = log.OnroadEvent.EventName # ****************************************************************************************** # NOTE: To fork maintainers. @@ -23,26 +21,21 @@ def to_percent(v): # ****************************************************************************************** class DRIVER_MONITOR_SETTINGS: - def __init__(self): - # https://eur-lex.europa.eu/legal-content/EN/TXT/PDF/?uri=CELEX:42018X1947&rid=2 - self._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT = 15. - self._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT = 24. - self._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT = 30. - # https://cdn.euroncap.com/cars/assets/euro_ncap_protocol_safe_driving_driver_engagement_v11_a30e874152.pdf - self._VISION_POLICY_ALERT_1_TIMEOUT = 3. - self._VISION_POLICY_ALERT_2_TIMEOUT = 5. - self._VISION_POLICY_ALERT_3_TIMEOUT = 11. - - self._TIMEOUT_RECOVERY_FACTOR_MAX = 5. - self._TIMEOUT_RECOVERY_FACTOR_MIN = 1.25 - - self._MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts - self._MAX_TERMINAL_DURATION = int(30 / DT_DMON) # not allowed to engage after 30s of terminal alerts + def __init__(self, device_type): + self._DT_DMON = DT_DMON + # ref (page15-16): https://eur-lex.europa.eu/legal-content/EN/TXT/PDF/?uri=CELEX:42018X1947&rid=2 + self._AWARENESS_TIME = 30. # passive wheeltouch total timeout + self._AWARENESS_PRE_TIME_TILL_TERMINAL = 15. + self._AWARENESS_PROMPT_TIME_TILL_TERMINAL = 6. + self._DISTRACTED_TIME = 11. # active monitoring total timeout + self._DISTRACTED_PRE_TIME_TILL_TERMINAL = 8. + self._DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6. self._FACE_THRESHOLD = 0.7 self._EYE_THRESHOLD = 0.5 self._BLINK_THRESHOLD = 0.5 self._PHONE_THRESH = 0.5 + self._POSE_PITCH_THRESHOLD = 0.3133 self._POSE_PITCH_THRESHOLD_SLACK = 0.3237 self._POSE_PITCH_THRESHOLD_STRICT = self._POSE_PITCH_THRESHOLD @@ -63,74 +56,101 @@ class DRIVER_MONITOR_SETTINGS: self._YAW_MIN_OFFSET = -0.0246 self._DCAM_UNCERTAIN_ALERT_THRESHOLD = 0.1 - self._DCAM_UNCERTAIN_ALERT_COUNT = int(60 / DT_DMON) - self._DCAM_UNCERTAIN_RESET_COUNT = int(20 / DT_DMON) - self._HI_STD_THRESHOLD = 0.3 - self._HI_STD_FALLBACK_TIME = int(10 / DT_DMON) # fall back to wheel touch if model is uncertain for 10s + self._DCAM_UNCERTAIN_ALERT_COUNT = int(60 / self._DT_DMON) + self._DCAM_UNCERTAIN_RESET_COUNT = int(20 / self._DT_DMON) + self._POSESTD_THRESHOLD = 0.3 + self._HI_STD_FALLBACK_TIME = int(10 / self._DT_DMON) # fall back to wheel touch if model is uncertain for 10s self._DISTRACTED_FILTER_TS = 0.25 # 0.6Hz self._POSE_CALIB_MIN_SPEED = 13 # 30 mph - self._POSE_OFFSET_MIN_COUNT = int(60 / DT_DMON) # valid data counts before calibration completes, 1min cumulative - self._POSE_OFFSET_MAX_COUNT = int(360 / DT_DMON) # stop deweighting new data after 6 min, aka "short term memory" + self._POSE_OFFSET_MIN_COUNT = int(60 / self._DT_DMON) # valid data counts before calibration completes, 1min cumulative + self._POSE_OFFSET_MAX_COUNT = int(360 / self._DT_DMON) # stop deweighting new data after 6 min, aka "short term memory" + self._WHEELPOS_CALIB_MIN_SPEED = 11 self._WHEELPOS_THRESHOLD = 0.5 - self._WHEELPOS_FILTER_MIN_COUNT = int(15 / DT_DMON) # allow 15 seconds to converge wheel side + self._WHEELPOS_FILTER_MIN_COUNT = int(15 / self._DT_DMON) # allow 15 seconds to converge wheel side self._WHEELPOS_DATA_AVG = 0.03 self._WHEELPOS_DATA_VAR = 3*5.5e-5 self._WHEELPOS_MAX_COUNT = -1 + self._RECOVERY_FACTOR_MAX = 5. # relative to minus step change + self._RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change + + self._MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts + self._MAX_TERMINAL_DURATION = int(30 / self._DT_DMON) # not allowed to engage after 30s of terminal alerts + +class DistractedType: + + NOT_DISTRACTED = 0 + DISTRACTED_POSE = 1 << 0 + DISTRACTED_BLINK = 1 << 1 + DISTRACTED_PHONE = 1 << 2 + class DriverPose: def __init__(self, settings): pitch_filter_raw_priors = (settings._PITCH_NATURAL_OFFSET, settings._PITCH_NATURAL_VAR, 2) yaw_filter_raw_priors = (settings._YAW_NATURAL_OFFSET, settings._YAW_NATURAL_VAR, 2) self.yaw = 0. self.pitch = 0. - self.pitch_offsetter = RunningStatFilter(raw_priors=pitch_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT) - self.yaw_offsetter = RunningStatFilter(raw_priors=yaw_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT) + self.roll = 0. + self.yaw_std = 0. + self.pitch_std = 0. + self.roll_std = 0. + self.pitch_offseter = RunningStatFilter(raw_priors=pitch_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT) + self.yaw_offseter = RunningStatFilter(raw_priors=yaw_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT) self.calibrated = False self.low_std = True self.cfactor_pitch = 1. self.cfactor_yaw = 1. self.steer_yaw_offset = 0. +class DriverProb: + def __init__(self, raw_priors, max_trackable): + self.prob = 0. + self.prob_offseter = RunningStatFilter(raw_priors=raw_priors, max_trackable=max_trackable) + self.prob_calibrated = False + + # model output refers to center of undistorted+leveled image -ref_undistorted_cam = DEVICE_CAMERAS[("tici", "ar0231")].dcam -dcam_undistorted_FL = 598.0 -dcam_undistorted_W, dcam_undistorted_H = (ref_undistorted_cam.width, ref_undistorted_cam.height) +EFL = 598.0 # focal length in K +cam = DEVICE_CAMERAS[("tici", "ar0231")] # corrected image has same size as raw +W, H = (cam.dcam.width, cam.dcam.height) # corrected image has same size as raw -def face_orientation_from_model(orient_model, pos_model, rpy_calib): - pitch_model = orient_model[0] - yaw_model = orient_model[1] +def face_orientation_from_net(angles_desc, pos_desc, rpy_calib): + # the output of these angles are in device frame + # so from driver's perspective, pitch is up and yaw is right - face_pixel_position = ((pos_model[0]+0.5)*dcam_undistorted_W, (pos_model[1]+0.5)*dcam_undistorted_H) - yaw_focal_angle = atan2(face_pixel_position[0] - dcam_undistorted_W//2, dcam_undistorted_FL) - pitch_focal_angle = atan2(face_pixel_position[1] - dcam_undistorted_H//2, dcam_undistorted_FL) + pitch_net, yaw_net, roll_net = angles_desc - pitch = pitch_model + pitch_focal_angle - yaw = -yaw_model + yaw_focal_angle + face_pixel_position = ((pos_desc[0]+0.5)*W, (pos_desc[1]+0.5)*H) + yaw_focal_angle = atan2(face_pixel_position[0] - W//2, EFL) + pitch_focal_angle = atan2(face_pixel_position[1] - H//2, EFL) + pitch = pitch_net + pitch_focal_angle + yaw = -yaw_net + yaw_focal_angle + + # no calib for roll pitch -= rpy_calib[1] yaw -= rpy_calib[2] - return pitch, yaw + return roll_net, pitch, yaw class DriverMonitoring: def __init__(self, rhd_saved=False, settings=None, always_on=False): # init policy settings - self.settings = settings if settings is not None else DRIVER_MONITOR_SETTINGS() + self.settings = settings if settings is not None else DRIVER_MONITOR_SETTINGS(device_type=HARDWARE.get_device_type()) # init driver status wheelpos_filter_raw_priors = (self.settings._WHEELPOS_DATA_AVG, self.settings._WHEELPOS_DATA_VAR, 2) - self.wheelpos_offsetter = RunningStatFilter(raw_priors=wheelpos_filter_raw_priors, max_trackable=self.settings._WHEELPOS_MAX_COUNT) + self.wheelpos = DriverProb(raw_priors=wheelpos_filter_raw_priors, max_trackable=self.settings._WHEELPOS_MAX_COUNT) self.pose = DriverPose(settings=self.settings) self.blink_prob = 0. self.phone_prob = 0. - self.alert_level = AlertLevel.none self.always_on = always_on - self.distracted_types = defaultdict(bool) + self.distracted_types = [] self.driver_distracted = False - self.driver_distraction_filter = FirstOrderFilter(0., self.settings._DISTRACTED_FILTER_TS, DT_DMON) + self.driver_distraction_filter = FirstOrderFilter(0., self.settings._DISTRACTED_FILTER_TS, self.settings._DT_DMON) self.wheel_on_right = False self.wheel_on_right_last = None self.wheel_on_right_default = rhd_saved @@ -138,56 +158,61 @@ class DriverMonitoring: self.terminal_alert_cnt = 0 self.terminal_time = 0 self.step_change = 0. - self.active_policy = MonitoringPolicy.vision - self.driver_interacting = False + self.active_monitoring_mode = True self.is_model_uncertain = False self.hi_stds = 0 - self.model_std_max = 0. - self.threshold_alert_1 = 0. - self.threshold_alert_2 = 0. + self.threshold_pre = self.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME + self.threshold_prompt = self.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME self.dcam_uncertain_cnt = 0 + self.dcam_uncertain_alerted = False # once per drive self.dcam_reset_cnt = 0 - self.too_distracted = Params().get_bool("DriverTooDistracted") + + self.params = Params() + self.too_distracted = self.params.get_bool("DriverTooDistracted") self._reset_awareness() - self._set_policy(MonitoringPolicy.vision) + self._set_timers(active_monitoring=True) + self._reset_events() def _reset_awareness(self): self.awareness = 1. - self.last_vision_awareness = 1. - self.last_wheeltouch_awareness = 1. + self.awareness_active = 1. + self.awareness_passive = 1. - def _set_policy(self, target_policy): - if self.active_policy == MonitoringPolicy.vision and self.awareness <= self.threshold_alert_2: - if target_policy == MonitoringPolicy.vision: - self.step_change = DT_DMON / self.settings._VISION_POLICY_ALERT_3_TIMEOUT + def _reset_events(self): + self.current_events = Events() + + def _set_timers(self, active_monitoring): + if self.active_monitoring_mode and self.awareness <= self.threshold_prompt: + if active_monitoring: + self.step_change = self.settings._DT_DMON / self.settings._DISTRACTED_TIME else: self.step_change = 0. return # no exploit after orange alert elif self.awareness <= 0.: return - if target_policy == MonitoringPolicy.vision: + if active_monitoring: # when falling back from passive mode to active mode, reset awareness to avoid false alert - if self.active_policy != MonitoringPolicy.vision: - self.last_wheeltouch_awareness = self.awareness - self.awareness = self.last_vision_awareness + if not self.active_monitoring_mode: + self.awareness_passive = self.awareness + self.awareness = self.awareness_active - self.threshold_alert_1 = 1. - self.settings._VISION_POLICY_ALERT_1_TIMEOUT / self.settings._VISION_POLICY_ALERT_3_TIMEOUT - self.threshold_alert_2 = 1. - self.settings._VISION_POLICY_ALERT_2_TIMEOUT / self.settings._VISION_POLICY_ALERT_3_TIMEOUT - self.step_change = DT_DMON / self.settings._VISION_POLICY_ALERT_3_TIMEOUT - self.active_policy = MonitoringPolicy.vision + self.threshold_pre = self.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME + self.threshold_prompt = self.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME + self.step_change = self.settings._DT_DMON / self.settings._DISTRACTED_TIME + self.active_monitoring_mode = True else: - if self.active_policy == MonitoringPolicy.vision: - self.last_vision_awareness = self.awareness - self.awareness = self.last_wheeltouch_awareness + if self.active_monitoring_mode: + self.awareness_active = self.awareness + self.awareness = self.awareness_passive - self.threshold_alert_1 = 1. - self.settings._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT / self.settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT - self.threshold_alert_2 = 1. - self.settings._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT / self.settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT - self.step_change = DT_DMON / self.settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT - self.active_policy = MonitoringPolicy.wheeltouch + self.threshold_pre = self.settings._AWARENESS_PRE_TIME_TILL_TERMINAL / self.settings._AWARENESS_TIME + self.threshold_prompt = self.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL / self.settings._AWARENESS_TIME + self.step_change = self.settings._DT_DMON / self.settings._AWARENESS_TIME + self.active_monitoring_mode = False - def _set_pose_strictness(self, brake_disengage_prob, car_speed): + def _set_policy(self, brake_disengage_prob, car_speed): bp = brake_disengage_prob k1 = max(-0.00156*((car_speed-16)**2)+0.6, 0.2) bp_normal = max(min(bp / k1, 0.5),0) @@ -199,15 +224,15 @@ class DriverMonitoring: self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD def _get_distracted_types(self): - self.distracted_types = defaultdict(bool) + distracted_types = [] if not self.pose.calibrated: pitch_error = self.pose.pitch - self.settings._PITCH_NATURAL_OFFSET yaw_error = self.pose.yaw - self.settings._YAW_NATURAL_OFFSET else: - pitch_error = self.pose.pitch - min(max(self.pose.pitch_offsetter.filtered_stat.mean(), + pitch_error = self.pose.pitch - min(max(self.pose.pitch_offseter.filtered_stat.mean(), self.settings._PITCH_MIN_OFFSET), self.settings._PITCH_MAX_OFFSET) - yaw_error = self.pose.yaw - min(max(self.pose.yaw_offsetter.filtered_stat.mean(), + yaw_error = self.pose.yaw - min(max(self.pose.yaw_offseter.filtered_stat.mean(), self.settings._YAW_MIN_OFFSET), self.settings._YAW_MAX_OFFSET) pitch_error = 0 if pitch_error > 0 else abs(pitch_error) # no positive pitch limit @@ -219,21 +244,28 @@ class DriverMonitoring: pitch_threshold = self.settings._POSE_PITCH_THRESHOLD * self.pose.cfactor_pitch if self.pose.calibrated else self.settings._PITCH_NATURAL_THRESHOLD yaw_threshold = self.settings._POSE_YAW_THRESHOLD * self.pose.cfactor_yaw - self.distracted_types['pose'] = bool((pitch_error > pitch_threshold) or (yaw_error > yaw_threshold)) - self.distracted_types['eye'] = bool(self.blink_prob > self.settings._BLINK_THRESHOLD) - self.distracted_types['phone'] = bool(self.phone_prob > self.settings._PHONE_THRESH) + if pitch_error > pitch_threshold or yaw_error > yaw_threshold: + distracted_types.append(DistractedType.DISTRACTED_POSE) + + if self.blink_prob > self.settings._BLINK_THRESHOLD: + distracted_types.append(DistractedType.DISTRACTED_BLINK) + + if self.phone_prob > self.settings._PHONE_THRESH: + distracted_types.append(DistractedType.DISTRACTED_PHONE) + + return distracted_types def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged, standstill, demo_mode=False, steering_angle_deg=0.): rhd_pred = driver_state.wheelOnRightProb # calibrates only when there's movement and either face detected if car_speed > self.settings._WHEELPOS_CALIB_MIN_SPEED and (driver_state.leftDriverData.faceProb > self.settings._FACE_THRESHOLD or driver_state.rightDriverData.faceProb > self.settings._FACE_THRESHOLD): - self.wheelpos_offsetter.push_and_update(rhd_pred) + self.wheelpos.prob_offseter.push_and_update(rhd_pred) - wheelpos_calibrated = self.wheelpos_offsetter.filtered_stat.n >= self.settings._WHEELPOS_FILTER_MIN_COUNT + self.wheelpos.prob_calibrated = self.wheelpos.prob_offseter.filtered_stat.n > self.settings._WHEELPOS_FILTER_MIN_COUNT - if wheelpos_calibrated or demo_mode: - self.wheel_on_right = self.wheelpos_offsetter.filtered_stat.M > self.settings._WHEELPOS_THRESHOLD + if self.wheelpos.prob_calibrated or demo_mode: + self.wheel_on_right = self.wheelpos.prob_offseter.filtered_stat.M > self.settings._WHEELPOS_THRESHOLD else: self.wheel_on_right = self.wheel_on_right_default # use default/saved if calibration is unfinished # make sure no switching when engaged @@ -245,57 +277,68 @@ class DriverMonitoring: return self.face_detected = driver_data.faceProb > self.settings._FACE_THRESHOLD - self.pose.pitch, self.pose.yaw = face_orientation_from_model(driver_data.faceOrientation, driver_data.facePosition, cal_rpy) + self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_data.faceOrientation, driver_data.facePosition, cal_rpy) steer_d = max(abs(steering_angle_deg) - self.settings._POSE_YAW_MIN_STEER_DEG, 0.) self.pose.steer_yaw_offset = radians(steer_d) * -np.sign(steering_angle_deg) * self.settings._POSE_YAW_STEER_FACTOR if self.wheel_on_right: self.pose.yaw *= -1 self.pose.steer_yaw_offset *= -1 self.wheel_on_right_last = self.wheel_on_right - self.model_std_max = max(driver_data.faceOrientationStd[0], driver_data.faceOrientationStd[1]) - self.pose.low_std = self.model_std_max < self.settings._HI_STD_THRESHOLD + self.pose.pitch_std = driver_data.faceOrientationStd[0] + self.pose.yaw_std = driver_data.faceOrientationStd[1] + model_std_max = max(self.pose.pitch_std, self.pose.yaw_std) + self.pose.low_std = model_std_max < self.settings._POSESTD_THRESHOLD self.blink_prob = driver_data.eyesClosedProb * (driver_data.eyesVisibleProb > self.settings._EYE_THRESHOLD) self.phone_prob = driver_data.phoneProb - self._get_distracted_types() - self.driver_distracted = any(self.distracted_types.values()) and driver_data.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std + self.distracted_types = self._get_distracted_types() + self.driver_distracted = (DistractedType.DISTRACTED_PHONE in self.distracted_types + or DistractedType.DISTRACTED_POSE in self.distracted_types + or DistractedType.DISTRACTED_BLINK in self.distracted_types) \ + and driver_data.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std self.driver_distraction_filter.update(self.driver_distracted) - # only update offsetter when driver is actively driving the car above a certain speed + # update offseter + # only update when driver is actively driving the car above a certain speed if self.face_detected and car_speed > self.settings._POSE_CALIB_MIN_SPEED and self.pose.low_std and (not op_engaged or not self.driver_distracted): - self.pose.pitch_offsetter.push_and_update(self.pose.pitch) - self.pose.yaw_offsetter.push_and_update(self.pose.yaw) + self.pose.pitch_offseter.push_and_update(self.pose.pitch) + self.pose.yaw_offseter.push_and_update(self.pose.yaw) - self.pose.calibrated = self.pose.pitch_offsetter.filtered_stat.n >= self.settings._POSE_OFFSET_MIN_COUNT and \ - self.pose.yaw_offsetter.filtered_stat.n >= self.settings._POSE_OFFSET_MIN_COUNT + self.pose.calibrated = self.pose.pitch_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT and \ + self.pose.yaw_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT if self.face_detected and not self.driver_distracted: - dcam_uncertain = self.model_std_max > self.settings._DCAM_UNCERTAIN_ALERT_THRESHOLD - if dcam_uncertain and not standstill: - self.dcam_uncertain_cnt += 1 - self.dcam_reset_cnt = 0 + if model_std_max > self.settings._DCAM_UNCERTAIN_ALERT_THRESHOLD: + if not standstill: + self.dcam_uncertain_cnt += 1 + self.dcam_reset_cnt = 0 else: self.dcam_reset_cnt += 1 if self.dcam_reset_cnt > self.settings._DCAM_UNCERTAIN_RESET_COUNT: self.dcam_uncertain_cnt = 0 - self.is_model_uncertain = self.hi_stds >= self.settings._HI_STD_FALLBACK_TIME - self._set_policy(MonitoringPolicy.vision if self.face_detected and not self.is_model_uncertain else MonitoringPolicy.wheeltouch) + self.is_model_uncertain = self.hi_stds > self.settings._HI_STD_FALLBACK_TIME + self._set_timers(self.face_detected and not self.is_model_uncertain) if self.face_detected and not self.pose.low_std and not self.driver_distracted: self.hi_stds += 1 elif self.face_detected and self.pose.low_std: self.hi_stds = 0 - def _update_events(self, driver_engaged, op_engaged, standstill, wrong_gear): - self.alert_level = AlertLevel.none - self.driver_interacting = driver_engaged - + def _update_events(self, driver_engaged, op_engaged, standstill, wrong_gear, car_speed): + self._reset_events() + # Block engaging until ignition cycle after max number or time of distractions if self.terminal_alert_cnt >= self.settings._MAX_TERMINAL_ALERTS or \ self.terminal_time >= self.settings._MAX_TERMINAL_DURATION: + if not self.too_distracted: + self.params.put_bool_nonblocking("DriverTooDistracted", True) self.too_distracted = True + # Always-on distraction lockout is temporary + if self.too_distracted or (self.always_on and self.awareness <= self.threshold_prompt): + self.current_events.add(EventName.tooDistracted) + always_on_valid = self.always_on and not wrong_gear - if (self.driver_interacting and self.awareness > 0 and self.active_policy == MonitoringPolicy.wheeltouch) or \ + if (driver_engaged and self.awareness > 0 and not self.active_monitoring_mode) or \ (not always_on_valid and not op_engaged) or \ (always_on_valid and not op_engaged and self.awareness <= 0): # always reset on disengage with normal mode; disengage resets only on red if always on @@ -303,118 +346,111 @@ class DriverMonitoring: return awareness_prev = self.awareness - _reaching_alert_1 = self.awareness - self.step_change <= self.threshold_alert_1 - _reaching_alert_3 = self.awareness - self.step_change <= 0 - standstill_exemption = standstill and _reaching_alert_1 - always_on_exemption = always_on_valid and not op_engaged and _reaching_alert_3 + _reaching_pre = self.awareness - self.step_change <= self.threshold_pre + _reaching_terminal = self.awareness - self.step_change <= 0 + standstill_orange_exemption = standstill and _reaching_pre + always_on_red_exemption = always_on_valid and not op_engaged and _reaching_terminal if self.awareness > 0 and \ - ((self.driver_distraction_filter.x < 0.37 and self.face_detected and self.pose.low_std) or standstill_exemption): - if self.driver_interacting: + ((self.driver_distraction_filter.x < 0.37 and self.face_detected and self.pose.low_std) or standstill_orange_exemption): + if driver_engaged: self._reset_awareness() return # only restore awareness when paying attention and alert is not red - self.awareness = min(self.awareness + ((self.settings._TIMEOUT_RECOVERY_FACTOR_MAX-self.settings._TIMEOUT_RECOVERY_FACTOR_MIN)* - (1.-self.awareness)+self.settings._TIMEOUT_RECOVERY_FACTOR_MIN)*self.step_change, 1.) + self.awareness = min(self.awareness + ((self.settings._RECOVERY_FACTOR_MAX-self.settings._RECOVERY_FACTOR_MIN)* + (1.-self.awareness)+self.settings._RECOVERY_FACTOR_MIN)*self.step_change, 1.) if self.awareness == 1.: - self.last_wheeltouch_awareness = min(self.last_wheeltouch_awareness + self.step_change, 1.) + self.awareness_passive = min(self.awareness_passive + self.step_change, 1.) # don't display alert banner when awareness is recovering and has cleared orange - if self.awareness > self.threshold_alert_2: + if self.awareness > self.threshold_prompt: return certainly_distracted = self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected - maybe_distracted = self.is_model_uncertain or not self.face_detected + maybe_distracted = self.hi_stds > self.settings._HI_STD_FALLBACK_TIME or not self.face_detected if certainly_distracted or maybe_distracted: # should always be counting if distracted unless at standstill and reaching green # also will not be reaching 0 if DM is active when not engaged - if not (standstill_exemption or always_on_exemption): + if not (standstill_orange_exemption or always_on_red_exemption): self.awareness = max(self.awareness - self.step_change, -0.1) + alert = None if self.awareness <= 0.: - # terminal alert: disengagement required - self.alert_level = AlertLevel.three + # terminal red alert: disengagement required + alert = EventName.driverDistracted3 if self.active_monitoring_mode else EventName.driverUnresponsive3 self.terminal_time += 1 if awareness_prev > 0.: self.terminal_alert_cnt += 1 - elif self.awareness <= self.threshold_alert_2: - self.alert_level = AlertLevel.two - elif self.awareness <= self.threshold_alert_1: - self.alert_level = AlertLevel.one + elif self.awareness <= self.threshold_prompt: + # prompt orange alert + alert = EventName.driverDistracted2 if self.active_monitoring_mode else EventName.driverUnresponsive2 + elif self.awareness <= self.threshold_pre: + # pre green alert + alert = EventName.driverDistracted1 if self.active_monitoring_mode else EventName.driverUnresponsive1 + + if alert is not None: + self.current_events.add(alert) + + if self.dcam_uncertain_cnt > self.settings._DCAM_UNCERTAIN_ALERT_COUNT and not self.dcam_uncertain_alerted: + set_offroad_alert("Offroad_DriverMonitoringUncertain", True) + self.dcam_uncertain_alerted = True + def get_state_packet(self, valid=True): # build driverMonitoringState packet dat = messaging.new_message('driverMonitoringState', valid=valid) - dm = dat.driverMonitoringState - - dm.lockout = self.too_distracted - dm.alertCountLockoutPercent = to_percent(self.terminal_alert_cnt / self.settings._MAX_TERMINAL_ALERTS) - dm.alertTimeLockoutPercent = to_percent(self.terminal_time / self.settings._MAX_TERMINAL_DURATION) - dm.alwaysOn = self.always_on - dm.alwaysOnLockout = self.always_on and self.awareness <= self.threshold_alert_2 - dm.alertLevel = self.alert_level - dm.activePolicy = self.active_policy - dm.isRHD = self.wheel_on_right - dm.rhdCalibration.calibratedPercent = to_percent(self.wheelpos_offsetter.filtered_stat.n / self.settings._WHEELPOS_FILTER_MIN_COUNT) - dm.rhdCalibration.offset = self.wheelpos_offsetter.filtered_stat.M - - dm.visionPolicyState.awarenessPercent = to_percent(self.last_vision_awareness if self.active_policy != MonitoringPolicy.vision else self.awareness) - dm.visionPolicyState.awarenessStep = self.step_change if self.active_policy == MonitoringPolicy.vision else 0. - dm.visionPolicyState.isDistracted = self.driver_distracted - dm.visionPolicyState.distractedTypes.pose = self.distracted_types['pose'] - dm.visionPolicyState.distractedTypes.eye = self.distracted_types['eye'] - dm.visionPolicyState.distractedTypes.phone = self.distracted_types['phone'] - dm.visionPolicyState.faceDetected = self.face_detected - dm.visionPolicyState.pose.pitch = self.pose.pitch - dm.visionPolicyState.pose.yaw = self.pose.yaw - dm.visionPolicyState.pose.calibrated = self.pose.calibrated - dm.visionPolicyState.pose.pitchCalib.calibratedPercent = to_percent(self.pose.pitch_offsetter.filtered_stat.n / self.settings._POSE_OFFSET_MIN_COUNT) - dm.visionPolicyState.pose.pitchCalib.offset = self.pose.pitch_offsetter.filtered_stat.M - dm.visionPolicyState.pose.yawCalib.calibratedPercent = to_percent(self.pose.yaw_offsetter.filtered_stat.n / self.settings._POSE_OFFSET_MIN_COUNT) - dm.visionPolicyState.pose.yawCalib.offset = self.pose.yaw_offsetter.filtered_stat.M - dm.visionPolicyState.pose.uncertainty = self.model_std_max - dm.visionPolicyState.wheeltouchFallbackPercent = to_percent(self.hi_stds / self.settings._HI_STD_FALLBACK_TIME) - dm.visionPolicyState.uncertainOffroadAlertPercent = to_percent(self.dcam_uncertain_cnt / self.settings._DCAM_UNCERTAIN_ALERT_COUNT) - - dm.wheeltouchPolicyState.awarenessPercent = to_percent(self.last_wheeltouch_awareness if self.active_policy == MonitoringPolicy.vision else self.awareness) - dm.wheeltouchPolicyState.awarenessStep = 0. if self.active_policy == MonitoringPolicy.vision else self.step_change - dm.wheeltouchPolicyState.driverInteracting = self.driver_interacting + dat.driverMonitoringState = { + "events": self.current_events.to_msg(), + "faceDetected": self.face_detected, + "isDistracted": self.driver_distracted, + "distractedType": sum(self.distracted_types), + "awarenessStatus": self.awareness, + "posePitchOffset": self.pose.pitch_offseter.filtered_stat.mean(), + "posePitchValidCount": self.pose.pitch_offseter.filtered_stat.n, + "poseYawOffset": self.pose.yaw_offseter.filtered_stat.mean(), + "poseYawValidCount": self.pose.yaw_offseter.filtered_stat.n, + "stepChange": self.step_change, + "awarenessActive": self.awareness_active, + "awarenessPassive": self.awareness_passive, + "isLowStd": self.pose.low_std, + "hiStdCount": self.hi_stds, + "isActiveMode": self.active_monitoring_mode, + "isRHD": self.wheel_on_right, + "uncertainCount": self.dcam_uncertain_cnt, + } return dat def run_step(self, sm, demo=False): if demo: - car_speed = 30 + highway_speed = 30 enabled = True wrong_gear = False standstill = False driver_engaged = False brake_disengage_prob = 1.0 - steering_angle_deg = 0.0 rpyCalib = [0., 0., 0.] else: - car_speed = sm['carState'].vEgo + highway_speed = sm['carState'].vEgo enabled = sm['selfdriveState'].enabled or sm['carControl'].latActive wrong_gear = sm['carState'].gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low) standstill = sm['carState'].standstill driver_engaged = sm['carState'].steeringPressed or sm['carState'].gasPressed brake_disengage_prob = sm['modelV2'].meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s - steering_angle_deg = sm['carState'].steeringAngleDeg rpyCalib = sm['liveCalibration'].rpyCalib - - self._set_pose_strictness( + self._set_policy( brake_disengage_prob=brake_disengage_prob, - car_speed=car_speed, + car_speed=highway_speed, ) # Parse data from dmonitoringmodeld self._update_states( driver_state=sm['driverStateV2'], cal_rpy=rpyCalib, - car_speed=car_speed, + car_speed=highway_speed, op_engaged=enabled, standstill=standstill, demo_mode=demo, - steering_angle_deg=steering_angle_deg, + steering_angle_deg=sm['carState'].steeringAngleDeg, ) # Update distraction events @@ -423,4 +459,5 @@ class DriverMonitoring: op_engaged=enabled, standstill=standstill, wrong_gear=wrong_gear, + car_speed=highway_speed ) diff --git a/selfdrive/monitoring/test_monitoring.py b/selfdrive/monitoring/test_monitoring.py index 948931cb29..ee6028d609 100644 --- a/selfdrive/monitoring/test_monitoring.py +++ b/selfdrive/monitoring/test_monitoring.py @@ -1,17 +1,19 @@ import numpy as np +import pytest -from cereal import log +from cereal import log, car from openpilot.common.realtime import DT_DMON -from openpilot.selfdrive.monitoring.policy import DriverMonitoring, DRIVER_MONITOR_SETTINGS +from openpilot.selfdrive.monitoring.helpers import DriverMonitoring, DRIVER_MONITOR_SETTINGS +from openpilot.system.hardware import HARDWARE EventName = log.OnroadEvent.EventName -dm_settings = DRIVER_MONITOR_SETTINGS() +dm_settings = DRIVER_MONITOR_SETTINGS(device_type=HARDWARE.get_device_type()) TEST_TIMESPAN = 120 # seconds -DISTRACTED_SECONDS_TO_ORANGE = dm_settings._VISION_POLICY_ALERT_2_TIMEOUT + 1 -DISTRACTED_SECONDS_TO_RED = dm_settings._VISION_POLICY_ALERT_3_TIMEOUT + 1 -INVISIBLE_SECONDS_TO_ORANGE = dm_settings._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT + 1 -INVISIBLE_SECONDS_TO_RED = dm_settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT + 1 +DISTRACTED_SECONDS_TO_ORANGE = dm_settings._DISTRACTED_TIME - dm_settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL + 1 +DISTRACTED_SECONDS_TO_RED = dm_settings._DISTRACTED_TIME + 1 +INVISIBLE_SECONDS_TO_ORANGE = dm_settings._AWARENESS_TIME - dm_settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL + 1 +INVISIBLE_SECONDS_TO_RED = dm_settings._AWARENESS_TIME + 1 def make_msg(face_detected, distracted=False, model_uncertain=False): ds = log.DriverStateV2.new_message() @@ -33,7 +35,7 @@ msg_ATTENTIVE = make_msg(True) msg_DISTRACTED = make_msg(True, distracted=True) msg_ATTENTIVE_UNCERTAIN = make_msg(True, model_uncertain=True) msg_DISTRACTED_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=True) -msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=dm_settings._HI_STD_THRESHOLD*1.5) +msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=dm_settings._POSESTD_THRESHOLD*1.5) # driver interaction with car car_interaction_DETECTED = True @@ -49,49 +51,49 @@ always_false = [False] * int(TEST_TIMESPAN / DT_DMON) class TestMonitoring: def _run_seq(self, msgs, interaction, engaged, standstill): DM = DriverMonitoring() - alert_lvls = [] + events = [] for idx in range(len(msgs)): DM._update_states(msgs[idx], [0, 0, 0], 0, engaged[idx], standstill[idx]) # cal_rpy and car_speed don't matter here # evaluate events at 10Hz for tests - DM._update_events(interaction[idx], engaged[idx], standstill[idx], 0) - alert_lvls.append(DM.alert_level) - assert len(alert_lvls) == len(msgs), f"got {len(alert_lvls)} for {len(msgs)} driverState input msgs" - return alert_lvls, DM + DM._update_events(interaction[idx], engaged[idx], standstill[idx], 0, 0) + events.append(DM.current_events) + assert len(events) == len(msgs), f"got {len(events)} for {len(msgs)} driverState input msgs" + return events, DM + def _assert_no_events(self, events): + assert all(not len(e) for e in events) # engaged, driver is attentive all the time def test_fully_aware_driver(self): - alert_lvls, d_status = self._run_seq(always_attentive, always_false, always_true, always_false) - assert all(a == 0 for a in alert_lvls) - assert d_status.active_policy == log.DriverMonitoringState.MonitoringPolicy.vision + events, _ = self._run_seq(always_attentive, always_false, always_true, always_false) + self._assert_no_events(events) # engaged, driver is distracted and does nothing def test_fully_distracted_driver(self): - alert_lvls, d_status = self._run_seq(always_distracted, always_false, always_true, always_false) - s = d_status.settings - assert alert_lvls[int(s._VISION_POLICY_ALERT_1_TIMEOUT / 2 / DT_DMON)] == 0 - assert alert_lvls[int((s._VISION_POLICY_ALERT_1_TIMEOUT + \ - (s._VISION_POLICY_ALERT_2_TIMEOUT - s._VISION_POLICY_ALERT_1_TIMEOUT) / 2) / DT_DMON)] == 1 - assert alert_lvls[int((s._VISION_POLICY_ALERT_2_TIMEOUT + \ - (s._VISION_POLICY_ALERT_3_TIMEOUT - s._VISION_POLICY_ALERT_2_TIMEOUT) / 2) / DT_DMON)] == 2 - assert alert_lvls[int((s._VISION_POLICY_ALERT_3_TIMEOUT + \ - (TEST_TIMESPAN - 10 - s._VISION_POLICY_ALERT_3_TIMEOUT) / 2) / DT_DMON)] == 3 + events, d_status = self._run_seq(always_distracted, always_false, always_true, always_false) + assert len(events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)]) == 0 + assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL + \ + ((d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL-d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == \ + EventName.driverDistracted1 + assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL + \ + ((d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == EventName.driverDistracted2 + assert events[int((d_status.settings._DISTRACTED_TIME + \ + ((TEST_TIMESPAN-10-d_status.settings._DISTRACTED_TIME)/2))/DT_DMON)].names[0] == EventName.driverDistracted3 assert isinstance(d_status.awareness, float) # engaged, no face detected the whole time, no action def test_fully_invisible_driver(self): - alert_lvls, d_status = self._run_seq(always_no_face, always_false, always_true, always_false) - s = d_status.settings - assert alert_lvls[int(s._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT / 2 / DT_DMON)] == 0 - assert alert_lvls[int((s._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT + \ - (s._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT - s._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT) / 2) / DT_DMON)] == 1 - assert alert_lvls[int((s._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT + \ - (s._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT - s._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT) / 2) / DT_DMON)] == 2 - assert alert_lvls[int((s._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT + \ - (TEST_TIMESPAN - 10 - s._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT) / 2) / DT_DMON)] == 3 - assert d_status.active_policy == log.DriverMonitoringState.MonitoringPolicy.wheeltouch + events, d_status = self._run_seq(always_no_face, always_false, always_true, always_false) + assert len(events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)]) == 0 + assert events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL + \ + ((d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL-d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == \ + EventName.driverUnresponsive1 + assert events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL + \ + ((d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == EventName.driverUnresponsive2 + assert events[int((d_status.settings._AWARENESS_TIME + \ + ((TEST_TIMESPAN-10-d_status.settings._AWARENESS_TIME)/2))/DT_DMON)].names[0] == EventName.driverUnresponsive3 # engaged, down to orange, driver pays attention, back to normal; then down to orange, driver touches wheel # - should have short orange recovery time and no green afterwards; wheel touch only recovers when paying attention @@ -102,13 +104,13 @@ class TestMonitoring: [msg_ATTENTIVE] * (int(TEST_TIMESPAN/DT_DMON)-int((DISTRACTED_SECONDS_TO_ORANGE*3+2)/DT_DMON)) interaction_vector = [car_interaction_NOT_DETECTED] * int(DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON) + \ [car_interaction_DETECTED] * (int(TEST_TIMESPAN/DT_DMON)-int(DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON)) - alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, always_true, always_false) - assert alert_lvls[int(DISTRACTED_SECONDS_TO_ORANGE*0.5/DT_DMON)] == 0 - assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2 - assert alert_lvls[int(DISTRACTED_SECONDS_TO_ORANGE*1.5/DT_DMON)] == 0 - assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE*3-0.1)/DT_DMON)] == 2 - assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)] == 2 - assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE*3+2.5)/DT_DMON)] == 0 + events, _ = self._run_seq(ds_vector, interaction_vector, always_true, always_false) + assert len(events[int(DISTRACTED_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0 + assert events[int((DISTRACTED_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0] == EventName.driverDistracted2 + assert len(events[int(DISTRACTED_SECONDS_TO_ORANGE*1.5/DT_DMON)]) == 0 + assert events[int((DISTRACTED_SECONDS_TO_ORANGE*3-0.1)/DT_DMON)].names[0] == EventName.driverDistracted2 + assert events[int((DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)].names[0] == EventName.driverDistracted2 + assert len(events[int((DISTRACTED_SECONDS_TO_ORANGE*3+2.5)/DT_DMON)]) == 0 # engaged, down to orange, driver dodges camera, then comes back still distracted, down to red, \ # driver dodges, and then touches wheel to no avail, disengages and reengages @@ -126,11 +128,11 @@ class TestMonitoring: = [True] * int(1/DT_DMON) op_vector[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+2.5)/DT_DMON):int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3)/DT_DMON)] \ = [False] * int(0.5/DT_DMON) - alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false) - assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE+0.5*_invisible_time)/DT_DMON)] == 2 - assert alert_lvls[int((DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)] == 3 - assert alert_lvls[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)] == 3 - assert alert_lvls[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3.5)/DT_DMON)] == 0 + events, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false) + assert events[int((DISTRACTED_SECONDS_TO_ORANGE+0.5*_invisible_time)/DT_DMON)].names[0] == EventName.driverDistracted2 + assert events[int((DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)].names[0] == EventName.driverDistracted3 + assert events[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)].names[0] == EventName.driverDistracted3 + assert len(events[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3.5)/DT_DMON)]) == 0 # engaged, invisible driver, down to orange, driver touches wheel; then down to orange again, driver appears # - both actions should clear the alert, but momentary appearance should not @@ -141,16 +143,16 @@ class TestMonitoring: ds_vector[int((2*INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON):int((2*INVISIBLE_SECONDS_TO_ORANGE+1+_visible_time)/DT_DMON)] = \ [msg_ATTENTIVE] * int(_visible_time/DT_DMON) interaction_vector[int((INVISIBLE_SECONDS_TO_ORANGE)/DT_DMON):int((INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON)] = [True] * int(1/DT_DMON) - alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false) - assert alert_lvls[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)] == 0 - assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2 - assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)] == 0 + events, _ = self._run_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false) + assert len(events[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0 + assert events[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0] == EventName.driverUnresponsive2 + assert len(events[int((INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)]) == 0 if _visible_time == 0.5: - assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)] == 2 - assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)] == 1 + assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0] == EventName.driverUnresponsive2 + assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)].names[0] == EventName.driverUnresponsive1 elif _visible_time == 10: - assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)] == 2 - assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)] == 0 + assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0] == EventName.driverUnresponsive2 + assert len(events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)]) == 0 # engaged, invisible driver, down to red, driver appears and then touches wheel, then disengages/reengages # - only disengage will clear the alert @@ -162,19 +164,19 @@ class TestMonitoring: ds_vector[int(INVISIBLE_SECONDS_TO_RED/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON)] = [msg_ATTENTIVE] * int(_visible_time/DT_DMON) interaction_vector[int((INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON)] = [True] * int(1/DT_DMON) op_vector[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)] = [False] * int(0.5/DT_DMON) - alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false) - assert alert_lvls[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)] == 0 - assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2 - assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)] == 3 - assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)] == 3 - assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)] == 3 - assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1+0.1)/DT_DMON)] == 0 + events, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false) + assert len(events[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0 + assert events[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0] == EventName.driverUnresponsive2 + assert events[int((INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)].names[0] == EventName.driverUnresponsive3 + assert events[int((INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)].names[0] == EventName.driverUnresponsive3 + assert events[int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)].names[0] == EventName.driverUnresponsive3 + assert len(events[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1+0.1)/DT_DMON)]) == 0 # disengaged, always distracted driver # - dm should stay quiet when not engaged def test_pure_dashcam_user(self): - alert_lvls, _ = self._run_seq(always_distracted, always_false, always_false, always_false) - assert all(a == 0 for a in alert_lvls) + events, _ = self._run_seq(always_distracted, always_false, always_false, always_false) + assert sum(len(event) for event in events) == 0 # engaged, car stops at traffic light, down to orange, no action, then car starts moving # - should only reach green when stopped, but continues counting down on launch @@ -182,12 +184,11 @@ class TestMonitoring: _redlight_time = 60 # seconds standstill_vector = always_true[:] standstill_vector[int(_redlight_time/DT_DMON):] = [False] * int((TEST_TIMESPAN-_redlight_time)/DT_DMON) - alert_lvls, d_status = self._run_seq(always_distracted, always_false, always_true, standstill_vector) - s = d_status.settings - assert alert_lvls[int((_redlight_time-0.1)/DT_DMON)] == 0 - _alert_1_to_2 = s._VISION_POLICY_ALERT_2_TIMEOUT - s._VISION_POLICY_ALERT_1_TIMEOUT - assert alert_lvls[int((_redlight_time+0.5)/DT_DMON)] == 1 - assert alert_lvls[int((_redlight_time+_alert_1_to_2+0.5)/DT_DMON)] == 2 + events, d_status = self._run_seq(always_distracted, always_false, always_true, standstill_vector) + assert len(events[int((_redlight_time-0.1)/DT_DMON)]) == 0 + _pre_to_prompt = d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL - d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL + assert events[int((_redlight_time+0.5)/DT_DMON)].names[0] == EventName.driverDistracted1 + assert events[int((_redlight_time+_pre_to_prompt+0.5)/DT_DMON)].names[0] == EventName.driverDistracted2 # engaged, distracted while moving, then car stops after reaching orange # - should reset timer to pre green at standstill @@ -195,18 +196,83 @@ class TestMonitoring: _stop_time = DISTRACTED_SECONDS_TO_ORANGE + 1 # stop 1 second after reaching orange standstill_vector = always_false[:] standstill_vector[int(_stop_time/DT_DMON):] = [True] * int((TEST_TIMESPAN-_stop_time)/DT_DMON) - alert_lvls, _ = self._run_seq(always_distracted, always_false, always_true, standstill_vector) + events, _ = self._run_seq(always_distracted, always_false, always_true, standstill_vector) # just before and briefly after stopping: orange alert; goes away quickly after stopped - assert alert_lvls[int((_stop_time+0.1)/DT_DMON)] == 2 - assert alert_lvls[int((_stop_time+0.5)/DT_DMON)] == 0 + assert events[int((_stop_time+0.1)/DT_DMON)].names[0] == EventName.driverDistracted2 + assert len(events[int((_stop_time+0.5)/DT_DMON)]) == 0 # engaged, model is somehow uncertain and driver is distracted # - should fall back to wheel touch after uncertain alert def test_somehow_indecisive_model(self): ds_vector = [msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN] * int(TEST_TIMESPAN/DT_DMON) interaction_vector = always_false[:] - alert_lvls, d_status = self._run_seq(ds_vector, interaction_vector, always_true, always_false) - s = d_status.settings - assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*s._HI_STD_FALLBACK_TIME-0.1)/DT_DMON)] == 1 - assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*s._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)] == 2 - assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED-1+DT_DMON*s._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)] == 3 + events, d_status = self._run_seq(ds_vector, interaction_vector, always_true, always_false) + assert EventName.driverUnresponsive1 in \ + events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME-0.1)/DT_DMON)].names + assert EventName.driverUnresponsive2 in \ + events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names + assert EventName.driverUnresponsive3 in \ + events[int((INVISIBLE_SECONDS_TO_RED-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names + + +@pytest.mark.parametrize("enabled_state, lat_active_state, expected", [ + (False, False, False), # Both Disabled + (True, False, True), # OP Enabled, Lat Inactive + (False, True, True), # OP Disabled, Lat Active (e.g. MADS) + (True, True, True) # Both Active +]) +def test_enabled_states(enabled_state, lat_active_state, expected): + """ + Test DriverMonitoring.run_step with all 4 combinations of: + - selfdriveState.enabled (True/False) + - carControl.latActive (True/False) + """ + cs = car.CarState.new_message() + cs.vEgo = 30.0 + cs.gearShifter = car.CarState.GearShifter.drive + cs.standstill = False + cs.steeringPressed = False + cs.gasPressed = False + + ss = log.SelfdriveState.new_message() + ss.enabled = enabled_state + + cc = car.CarControl.new_message() + cc.latActive = lat_active_state + + mv2 = log.ModelDataV2.new_message() + mv2.meta.disengagePredictions.brakeDisengageProbs = [0.0] + + lc = log.LiveCalibrationData.new_message() + lc.rpyCalib = [0.0, 0.0, 0.0] + + ds = make_msg(False) + + sm = { + 'carState': cs, + 'selfdriveState': ss, + 'carControl': cc, + 'modelV2': mv2, + 'liveCalibration': lc, + 'driverStateV2': ds + } + + driver_monitoring = DriverMonitoring() + + # run_test doesn't assign enabled to a variable, so we need to spy on _update_events to see its value + captured_args = [] + original_update_events = driver_monitoring._update_events + + def spy_update_events(driver_engaged, op_engaged, standstill, wrong_gear, car_speed): + captured_args.append(op_engaged) + return original_update_events(driver_engaged, op_engaged, standstill, wrong_gear, car_speed) + + driver_monitoring._update_events = spy_update_events + + driver_monitoring.run_step(sm, demo=False) + + # Assertion + assert len(captured_args) == 1, "Expected _update_events to be called exactly once" + actual_enabled = captured_args[0] + + assert actual_enabled == expected, f"Expected op_engaged={expected}, but got {actual_enabled}" From 271ed5e091efc600c20b762e1eeffdc9e4cf5a02 Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Sun, 26 Apr 2026 13:03:04 -0700 Subject: [PATCH 14/14] dm --- selfdrive/modeld/dmonitoringmodeld.py | 24 ++++++++---------------- 1 file changed, 8 insertions(+), 16 deletions(-) diff --git a/selfdrive/modeld/dmonitoringmodeld.py b/selfdrive/modeld/dmonitoringmodeld.py index 78749b773a..18a79136ae 100755 --- a/selfdrive/modeld/dmonitoringmodeld.py +++ b/selfdrive/modeld/dmonitoringmodeld.py @@ -1,12 +1,8 @@ #!/usr/bin/env python3 import os -from openpilot.selfdrive.modeld.tinygrad_helpers import MODELS_DIR, set_tinygrad_backend_from_compiled_flags +from openpilot.selfdrive.modeld.helpers import MODELS_DIR, CompileConfig, set_tinygrad_backend_from_compiled_flags set_tinygrad_backend_from_compiled_flags() -# FIXME-SP: remove once we bump tg -from openpilot.system.hardware import TICI -os.environ['DEV'] = 'QCOM' if TICI else 'CPU' - from tinygrad.tensor import Tensor import time import pickle @@ -32,7 +28,7 @@ class ModelState: inputs: dict[str, np.ndarray] output: np.ndarray - def __init__(self): + def __init__(self, cam_w: int, cam_h: int): with open(METADATA_PATH, 'rb') as f: model_metadata = pickle.load(f) self.input_shapes = model_metadata['input_shapes'] @@ -44,22 +40,18 @@ class ModelState: self.warp_inputs_np = {'transform': np.zeros((3,3), dtype=np.float32)} self.warp_inputs = {k: Tensor(v, device='NPY') for k,v in self.warp_inputs_np.items()} - self.frame_buf_params = None + self.frame_buf_params = get_nv12_info(cam_w, cam_h) self.tensor_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()} self._blob_cache : dict[int, Tensor] = {} - self.image_warp = None self.model_run = pickle.loads(read_file_chunked(str(MODEL_PKL_PATH))) + with open(CompileConfig(cam_w, cam_h, prefix='dm_', prepare_only=True).pkl_path, "rb") as f: + self.image_warp = pickle.load(f) def run(self, buf: VisionBuf, calib: np.ndarray, transform: np.ndarray) -> tuple[np.ndarray, float]: self.numpy_inputs['calib'][0,:] = calib t1 = time.perf_counter() - if self.image_warp is None: - self.frame_buf_params = get_nv12_info(buf.width, buf.height) - warp_path = MODELS_DIR / f'dm_warp_{buf.width}x{buf.height}_tinygrad.pkl' - with open(warp_path, "rb") as f: - self.image_warp = pickle.load(f) ptr = buf.data.ctypes.data # There is a ringbuffer of imgs, just cache tensors pointing to all of them if ptr not in self._blob_cache: @@ -113,9 +105,6 @@ def get_driverstate_packet(model_output, frame_id: int, location_ts: int, exec_t def main(): config_realtime_process(7, 5) - model = ModelState() - cloudlog.warning("models loaded, dmonitoringmodeld starting") - cloudlog.warning("connecting to driver stream") vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_DRIVER, True) while not vipc_client.connect(False): @@ -123,6 +112,9 @@ def main(): assert vipc_client.is_connected() cloudlog.warning(f"connected with buffer size: {vipc_client.buffer_len}") + model = ModelState(vipc_client.width, vipc_client.height) + cloudlog.warning("models loaded, dmonitoringmodeld starting") + sm = SubMaster(["liveCalibration"]) pm = PubMaster(["driverStateV2"])