mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-16 06:42:12 +08:00
dm: sync new tinygrad driver monitoring stack from SP-Dev-Dom-fixed
This commit is contained in:
firestar5683
@@ -4,10 +4,8 @@ from openpilot.system.hardware import TICI
|
||||
os.environ['DEV'] = 'QCOM' if TICI else 'CPU'
|
||||
from tinygrad.tensor import Tensor
|
||||
from tinygrad.dtype import dtypes
|
||||
import math
|
||||
import time
|
||||
import pickle
|
||||
import ctypes
|
||||
import numpy as np
|
||||
from pathlib import Path
|
||||
|
||||
@@ -16,114 +14,99 @@ from cereal.messaging import PubMaster, SubMaster
|
||||
from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.common.realtime import config_realtime_process
|
||||
from openpilot.common.transformations.model import dmonitoringmodel_intrinsics, DM_INPUT_SIZE
|
||||
from openpilot.common.transformations.model import dmonitoringmodel_intrinsics
|
||||
from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
|
||||
try:
|
||||
from openpilot.selfdrive.modeld.models.commonmodel_pyx import CLContext, MonitoringModelFrame
|
||||
except ImportError:
|
||||
from openpilot.frogpilot.tinygrad_modeld.models.commonmodel_pyx import CLContext, MonitoringModelFrame
|
||||
from openpilot.frogpilot.tinygrad_modeld.parse_model_outputs import sigmoid
|
||||
from openpilot.frogpilot.tinygrad_modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address
|
||||
|
||||
MODEL_WIDTH, MODEL_HEIGHT = DM_INPUT_SIZE
|
||||
CALIB_LEN = 3
|
||||
FEATURE_LEN = 512
|
||||
OUTPUT_SIZE = 84 + FEATURE_LEN
|
||||
from openpilot.selfdrive.modeld.parse_model_outputs import sigmoid, safe_exp
|
||||
from openpilot.selfdrive.modeld.models.commonmodel_pyx import CLContext, MonitoringModelFrame
|
||||
from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address
|
||||
|
||||
PROCESS_NAME = "frogpilot.tinygrad_modeld.dmonitoringmodeld"
|
||||
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
|
||||
MODEL_PKL_PATH = Path(__file__).resolve().parents[2] / "selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl"
|
||||
|
||||
|
||||
class DriverStateResult(ctypes.Structure):
|
||||
_fields_ = [
|
||||
("face_orientation", ctypes.c_float*3),
|
||||
("face_position", ctypes.c_float*3),
|
||||
("face_orientation_std", ctypes.c_float*3),
|
||||
("face_position_std", ctypes.c_float*3),
|
||||
("face_prob", ctypes.c_float),
|
||||
("_unused_a", ctypes.c_float*8),
|
||||
("left_eye_prob", ctypes.c_float),
|
||||
("_unused_b", ctypes.c_float*8),
|
||||
("right_eye_prob", ctypes.c_float),
|
||||
("left_blink_prob", ctypes.c_float),
|
||||
("right_blink_prob", ctypes.c_float),
|
||||
("sunglasses_prob", ctypes.c_float),
|
||||
("occluded_prob", ctypes.c_float),
|
||||
("ready_prob", ctypes.c_float*4),
|
||||
("not_ready_prob", ctypes.c_float*2)]
|
||||
|
||||
|
||||
class DMonitoringModelResult(ctypes.Structure):
|
||||
_fields_ = [
|
||||
("driver_state_lhd", DriverStateResult),
|
||||
("driver_state_rhd", DriverStateResult),
|
||||
("poor_vision_prob", ctypes.c_float),
|
||||
("wheel_on_right_prob", ctypes.c_float),
|
||||
("features", ctypes.c_float*FEATURE_LEN)]
|
||||
METADATA_PATH = Path(__file__).parent / "models/dmonitoring_model_metadata.pkl"
|
||||
|
||||
|
||||
class ModelState:
|
||||
inputs: dict[str, np.ndarray]
|
||||
output: np.ndarray
|
||||
|
||||
def __init__(self, cl_ctx):
|
||||
assert ctypes.sizeof(DMonitoringModelResult) == OUTPUT_SIZE * ctypes.sizeof(ctypes.c_float)
|
||||
def __init__(self, cl_ctx: CLContext):
|
||||
with open(METADATA_PATH, 'rb') as f:
|
||||
model_metadata = pickle.load(f)
|
||||
self.input_shapes = model_metadata['input_shapes']
|
||||
self.output_slices = model_metadata['output_slices']
|
||||
|
||||
self.frame = MonitoringModelFrame(cl_ctx)
|
||||
self.numpy_inputs = {
|
||||
'calib': np.zeros((1, CALIB_LEN), dtype=np.float32),
|
||||
'calib': np.zeros(self.input_shapes['calib'], dtype=np.float32),
|
||||
}
|
||||
|
||||
self.tensor_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()}
|
||||
self.tensor_inputs = {k: Tensor(v, device='NPY').realize() for k, v in self.numpy_inputs.items()}
|
||||
with open(MODEL_PKL_PATH, "rb") as f:
|
||||
self.model_run = pickle.load(f)
|
||||
|
||||
def run(self, buf: VisionBuf, calib: np.ndarray, transform: np.ndarray) -> tuple[np.ndarray, float]:
|
||||
self.numpy_inputs['calib'][0,:] = calib
|
||||
self.numpy_inputs['calib'][0, :] = calib
|
||||
|
||||
t1 = time.perf_counter()
|
||||
|
||||
input_img_cl = self.frame.prepare(buf, transform.flatten())
|
||||
if TICI:
|
||||
# The imgs tensors are backed by opencl memory, only need init once
|
||||
if 'input_img' not in self.tensor_inputs:
|
||||
self.tensor_inputs['input_img'] = qcom_tensor_from_opencl_address(input_img_cl.mem_address, (1, MODEL_WIDTH*MODEL_HEIGHT), dtype=dtypes.uint8)
|
||||
self.tensor_inputs['input_img'] = qcom_tensor_from_opencl_address(
|
||||
input_img_cl.mem_address, self.input_shapes['input_img'], dtype=dtypes.uint8
|
||||
)
|
||||
else:
|
||||
self.tensor_inputs['input_img'] = Tensor(self.frame.buffer_from_cl(input_img_cl).reshape((1, MODEL_WIDTH*MODEL_HEIGHT)), dtype=dtypes.uint8).realize()
|
||||
self.tensor_inputs['input_img'] = Tensor(
|
||||
self.frame.buffer_from_cl(input_img_cl).reshape(self.input_shapes['input_img']), dtype=dtypes.uint8
|
||||
).realize()
|
||||
|
||||
|
||||
output = self.model_run(**self.tensor_inputs).contiguous().realize().uop.base.buffer.numpy()
|
||||
output = self.model_run(**self.tensor_inputs).contiguous().realize().uop.base.buffer.numpy().flatten()
|
||||
|
||||
t2 = time.perf_counter()
|
||||
return output, t2 - t1
|
||||
|
||||
|
||||
def fill_driver_state(msg, ds_result: DriverStateResult):
|
||||
msg.faceOrientation = list(ds_result.face_orientation)
|
||||
msg.faceOrientationStd = [math.exp(x) for x in ds_result.face_orientation_std]
|
||||
msg.facePosition = list(ds_result.face_position[:2])
|
||||
msg.facePositionStd = [math.exp(x) for x in ds_result.face_position_std[:2]]
|
||||
msg.faceProb = float(sigmoid(ds_result.face_prob))
|
||||
msg.leftEyeProb = float(sigmoid(ds_result.left_eye_prob))
|
||||
msg.rightEyeProb = float(sigmoid(ds_result.right_eye_prob))
|
||||
msg.leftBlinkProb = float(sigmoid(ds_result.left_blink_prob))
|
||||
msg.rightBlinkProb = float(sigmoid(ds_result.right_blink_prob))
|
||||
msg.sunglassesProb = float(sigmoid(ds_result.sunglasses_prob))
|
||||
msg.phoneProb = 0.0
|
||||
def slice_outputs(model_outputs, output_slices):
|
||||
return {k: model_outputs[np.newaxis, v] for k, v in output_slices.items()}
|
||||
|
||||
|
||||
def get_driverstate_packet(model_output: np.ndarray, frame_id: int, location_ts: int, execution_time: float, gpu_execution_time: float):
|
||||
model_result = ctypes.cast(model_output.ctypes.data, ctypes.POINTER(DMonitoringModelResult)).contents
|
||||
def parse_model_output(model_output):
|
||||
parsed = {}
|
||||
parsed['wheel_on_right'] = sigmoid(model_output['wheel_on_right'])
|
||||
for ds_suffix in ['lhd', 'rhd']:
|
||||
face_descs = model_output[f'face_descs_{ds_suffix}']
|
||||
parsed[f'face_descs_{ds_suffix}'] = face_descs[:, :-6]
|
||||
parsed[f'face_descs_{ds_suffix}_std'] = safe_exp(face_descs[:, -6:])
|
||||
for key in ['face_prob', 'left_eye_prob', 'right_eye_prob', 'left_blink_prob', 'right_blink_prob', 'sunglasses_prob', 'using_phone_prob']:
|
||||
parsed[f'{key}_{ds_suffix}'] = sigmoid(model_output[f'{key}_{ds_suffix}'])
|
||||
return parsed
|
||||
|
||||
|
||||
def fill_driver_data(msg, model_output, ds_suffix):
|
||||
msg.faceOrientation = model_output[f'face_descs_{ds_suffix}'][0, :3].tolist()
|
||||
msg.faceOrientationStd = model_output[f'face_descs_{ds_suffix}_std'][0, :3].tolist()
|
||||
msg.facePosition = model_output[f'face_descs_{ds_suffix}'][0, 3:5].tolist()
|
||||
msg.facePositionStd = model_output[f'face_descs_{ds_suffix}_std'][0, 3:5].tolist()
|
||||
msg.faceProb = model_output[f'face_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.leftEyeProb = model_output[f'left_eye_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.rightEyeProb = model_output[f'right_eye_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.leftBlinkProb = model_output[f'left_blink_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.rightBlinkProb = model_output[f'right_blink_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.sunglassesProb = model_output[f'sunglasses_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.phoneProb = model_output[f'using_phone_prob_{ds_suffix}'][0, 0].item()
|
||||
|
||||
|
||||
def get_driverstate_packet(model_output, frame_id: int, location_ts: int, exec_time: float, gpu_exec_time: float):
|
||||
msg = messaging.new_message('driverStateV2', valid=True)
|
||||
ds = msg.driverStateV2
|
||||
ds.frameId = frame_id
|
||||
ds.modelExecutionTime = execution_time
|
||||
ds.gpuExecutionTime = gpu_execution_time
|
||||
ds.wheelOnRightProb = float(sigmoid(model_result.wheel_on_right_prob))
|
||||
ds.rawPredictions = model_output.tobytes() if SEND_RAW_PRED else b''
|
||||
fill_driver_state(ds.leftDriverData, model_result.driver_state_lhd)
|
||||
fill_driver_state(ds.rightDriverData, model_result.driver_state_rhd)
|
||||
ds.modelExecutionTime = exec_time
|
||||
ds.gpuExecutionTime = gpu_exec_time
|
||||
ds.rawPredictions = model_output['raw_pred']
|
||||
ds.wheelOnRightProb = model_output['wheel_on_right'][0, 0].item()
|
||||
fill_driver_data(ds.leftDriverData, model_output, 'lhd')
|
||||
fill_driver_data(ds.rightDriverData, model_output, 'rhd')
|
||||
return msg
|
||||
|
||||
|
||||
@@ -144,7 +127,7 @@ def main():
|
||||
sm = SubMaster(["liveCalibration"])
|
||||
pm = PubMaster(["driverStateV2"])
|
||||
|
||||
calib = np.zeros(CALIB_LEN, dtype=np.float32)
|
||||
calib = np.zeros(model.numpy_inputs['calib'].size, dtype=np.float32)
|
||||
model_transform = None
|
||||
|
||||
while True:
|
||||
@@ -163,8 +146,12 @@ def main():
|
||||
t1 = time.perf_counter()
|
||||
model_output, gpu_execution_time = model.run(buf, calib, model_transform)
|
||||
t2 = time.perf_counter()
|
||||
|
||||
pm.send("driverStateV2", get_driverstate_packet(model_output, vipc_client.frame_id, vipc_client.timestamp_sof, t2 - t1, gpu_execution_time))
|
||||
raw_pred = model_output.tobytes() if SEND_RAW_PRED else b''
|
||||
model_output = slice_outputs(model_output, model.output_slices)
|
||||
model_output = parse_model_output(model_output)
|
||||
model_output['raw_pred'] = raw_pred
|
||||
msg = get_driverstate_packet(model_output, vipc_client.frame_id, vipc_client.timestamp_sof, t2 - t1, gpu_execution_time)
|
||||
pm.send("driverStateV2", msg)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
@@ -139,21 +139,14 @@ class ModelState:
|
||||
model_id_raw = _get_param_str(params, "Model")
|
||||
if not model_id_raw:
|
||||
model_id_raw = _get_param_str(params, "DrivingModel", "sc")
|
||||
model_id = model_id_raw.strip() or "sc"
|
||||
model_id_lower = model_id.lower()
|
||||
if model_id_lower == "sc2":
|
||||
model_id = "sc"
|
||||
elif model_id_lower == "sc":
|
||||
model_id = "sc"
|
||||
elif model_id_lower == "bd2":
|
||||
model_id = "bd2"
|
||||
model_id = (model_id_raw.strip() or "sc").lower()
|
||||
|
||||
model_version = _get_param_str(params, "ModelVersion")
|
||||
if not model_version:
|
||||
model_version = _get_param_str(params, "DrivingModelVersion")
|
||||
|
||||
model_dir = MODELS_PATH
|
||||
use_builtin_model = model_id in ("sc", "bd2")
|
||||
use_builtin_model = model_id == "sc"
|
||||
model_download_id = model_id
|
||||
if use_builtin_model and (model_id_raw != model_id or _get_param_str(params, "DrivingModel") != model_id):
|
||||
params.put("Model", model_id)
|
||||
|
||||
@@ -25,7 +25,6 @@ MODEL_PKL_PATH = Path(__file__).parent / 'models/dmonitoring_model_tinygrad.pkl'
|
||||
METADATA_PATH = Path(__file__).parent / 'models/dmonitoring_model_metadata.pkl'
|
||||
MODELS_DIR = Path(__file__).parent / 'models'
|
||||
|
||||
|
||||
class ModelState:
|
||||
inputs: dict[str, np.ndarray]
|
||||
output: np.ndarray
|
||||
@@ -46,42 +45,8 @@ class ModelState:
|
||||
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._warp_rebuild_attempted: set[tuple[int, int]] = set()
|
||||
self._warp_backend_rebuild_attempted: set[tuple[int, int]] = set()
|
||||
self.model_run = pickle.loads(read_file_chunked(str(MODEL_PKL_PATH)))
|
||||
|
||||
def _load_or_rebuild_dm_warp(self, width: int, height: int):
|
||||
warp_path = MODELS_DIR / f'dm_warp_{width}x{height}_tinygrad.pkl'
|
||||
resolution_key = (width, height)
|
||||
|
||||
def load_warp():
|
||||
with open(warp_path, "rb") as f:
|
||||
return pickle.load(f)
|
||||
|
||||
try:
|
||||
return load_warp()
|
||||
except Exception as error:
|
||||
if resolution_key in self._warp_rebuild_attempted:
|
||||
raise
|
||||
|
||||
self._warp_rebuild_attempted.add(resolution_key)
|
||||
cloudlog.exception(f"Failed to load DM warp artifact {warp_path}: {error}")
|
||||
cloudlog.warning(f"Rebuilding DM warp artifact for {width}x{height}")
|
||||
|
||||
try:
|
||||
warp_path.unlink(missing_ok=True)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
from openpilot.selfdrive.modeld.compile_warp import compile_dm_warp
|
||||
compile_dm_warp(width, height)
|
||||
|
||||
try:
|
||||
return load_warp()
|
||||
except Exception as retry_error:
|
||||
cloudlog.exception(f"Reload failed after rebuilding {warp_path}: {retry_error}")
|
||||
raise
|
||||
|
||||
def run(self, buf: VisionBuf, calib: np.ndarray, transform: np.ndarray) -> tuple[np.ndarray, float]:
|
||||
self.numpy_inputs['calib'][0,:] = calib
|
||||
|
||||
@@ -89,33 +54,16 @@ class ModelState:
|
||||
|
||||
if self.image_warp is None:
|
||||
self.frame_buf_params = get_nv12_info(buf.width, buf.height)
|
||||
self.image_warp = self._load_or_rebuild_dm_warp(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:
|
||||
self._blob_cache[ptr] = Tensor.from_blob(ptr, (self.frame_buf_params[3],), dtype='uint8')
|
||||
|
||||
self.warp_inputs_np['transform'][:] = transform[:]
|
||||
resolution_key = (buf.width, buf.height)
|
||||
try:
|
||||
self.tensor_inputs['input_img'] = self.image_warp(self._blob_cache[ptr], self.warp_inputs['transform']).realize()
|
||||
except AssertionError as error:
|
||||
# Handle runtime backend mismatch (e.g. CPU-captured warp artifact on QCOM device).
|
||||
if "args mismatch in JIT" not in str(error) or resolution_key in self._warp_backend_rebuild_attempted:
|
||||
raise
|
||||
|
||||
self._warp_backend_rebuild_attempted.add(resolution_key)
|
||||
cloudlog.warning(f"DM warp JIT backend mismatch for {buf.width}x{buf.height}; rebuilding artifact for active backend")
|
||||
warp_path = MODELS_DIR / f'dm_warp_{buf.width}x{buf.height}_tinygrad.pkl'
|
||||
try:
|
||||
warp_path.unlink(missing_ok=True)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
from openpilot.selfdrive.modeld.compile_warp import compile_dm_warp
|
||||
compile_dm_warp(buf.width, buf.height)
|
||||
self.image_warp = self._load_or_rebuild_dm_warp(buf.width, buf.height)
|
||||
self.tensor_inputs['input_img'] = self.image_warp(self._blob_cache[ptr], self.warp_inputs['transform']).realize()
|
||||
self.tensor_inputs['input_img'] = self.image_warp(self._blob_cache[ptr], self.warp_inputs['transform']).realize()
|
||||
|
||||
output = self.model_run(**self.tensor_inputs).contiguous().realize().uop.base.buffer.numpy().flatten()
|
||||
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -48,16 +48,11 @@ def dmonitoringd_thread():
|
||||
DM.always_on = params.get_bool("AlwaysOnDM")
|
||||
demo_mode = params.get_bool("IsDriverViewEnabled") and sm["carState"].gearShifter != GearShifter.reverse
|
||||
|
||||
# save rhd virtual toggle every 5 mins, but only with clear confidence.
|
||||
# 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):
|
||||
wheelpos_mean = DM.wheelpos.prob_offseter.filtered_stat.M
|
||||
save_rhd = DM.settings._WHEELPOS_THRESHOLD_ENTER_RHD + DM.settings._WHEELPOS_SAVE_MARGIN
|
||||
save_lhd = DM.settings._WHEELPOS_THRESHOLD_ENTER_LHD - DM.settings._WHEELPOS_SAVE_MARGIN
|
||||
if wheelpos_mean >= save_rhd:
|
||||
params.put_bool_nonblocking("IsRhdDetected", True)
|
||||
elif wheelpos_mean <= save_lhd:
|
||||
params.put_bool_nonblocking("IsRhdDetected", False)
|
||||
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():
|
||||
dmonitoringd_thread()
|
||||
|
||||
@@ -35,11 +35,7 @@ class DRIVER_MONITOR_SETTINGS:
|
||||
self._EYE_THRESHOLD = 0.65
|
||||
self._SG_THRESHOLD = 0.9
|
||||
self._BLINK_THRESHOLD = 0.865
|
||||
|
||||
self._PHONE_THRESH = 0.75 if device_type == 'mici' else 0.4
|
||||
self._PHONE_THRESH2 = 15.0
|
||||
self._PHONE_MAX_OFFSET = 0.06
|
||||
self._PHONE_MIN_OFFSET = 0.025
|
||||
self._PHONE_THRESH = 0.5
|
||||
|
||||
self._POSE_PITCH_THRESHOLD = 0.3133
|
||||
self._POSE_PITCH_THRESHOLD_SLACK = 0.3237
|
||||
@@ -50,6 +46,8 @@ class DRIVER_MONITOR_SETTINGS:
|
||||
self._PITCH_NATURAL_OFFSET = 0.011 # initial value before offset is learned
|
||||
self._PITCH_NATURAL_THRESHOLD = 0.449
|
||||
self._YAW_NATURAL_OFFSET = 0.075 # initial value before offset is learned
|
||||
self._PITCH_NATURAL_VAR = 3*0.01
|
||||
self._YAW_NATURAL_VAR = 3*0.05
|
||||
self._PITCH_MAX_OFFSET = 0.124
|
||||
self._PITCH_MIN_OFFSET = -0.0881
|
||||
self._YAW_MAX_OFFSET = 0.289
|
||||
@@ -70,18 +68,9 @@ class DRIVER_MONITOR_SETTINGS:
|
||||
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_THRESHOLD_ENTER_RHD = self._WHEELPOS_THRESHOLD
|
||||
self._WHEELPOS_THRESHOLD_ENTER_LHD = self._WHEELPOS_THRESHOLD
|
||||
self._WHEELPOS_SAVE_MARGIN = 0.0
|
||||
self._WHEELPOS_STARTUP_OVERRIDE_RHD = 0.55
|
||||
self._WHEELPOS_STARTUP_OVERRIDE_LHD = 0.45
|
||||
|
||||
# C4 (mici) has shown borderline wheel-side probabilities around 0.5x.
|
||||
# Use hysteresis and stricter persistence thresholds to avoid false RHD latching.
|
||||
if device_type == 'mici':
|
||||
self._WHEELPOS_THRESHOLD_ENTER_RHD = 0.65
|
||||
self._WHEELPOS_THRESHOLD_ENTER_LHD = 0.35
|
||||
self._WHEELPOS_SAVE_MARGIN = 0.05
|
||||
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
|
||||
@@ -96,24 +85,26 @@ class DistractedType:
|
||||
DISTRACTED_PHONE = 1 << 2
|
||||
|
||||
class DriverPose:
|
||||
def __init__(self, max_trackable):
|
||||
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(max_trackable=max_trackable)
|
||||
self.yaw_offseter = RunningStatFilter(max_trackable=max_trackable)
|
||||
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.
|
||||
|
||||
class DriverProb:
|
||||
def __init__(self, max_trackable):
|
||||
def __init__(self, raw_priors, max_trackable):
|
||||
self.prob = 0.
|
||||
self.prob_offseter = RunningStatFilter(max_trackable=max_trackable)
|
||||
self.prob_offseter = RunningStatFilter(raw_priors=raw_priors, max_trackable=max_trackable)
|
||||
self.prob_calibrated = False
|
||||
|
||||
class DriverBlink:
|
||||
@@ -152,10 +143,11 @@ class DriverMonitoring:
|
||||
self.settings = settings if settings is not None else DRIVER_MONITOR_SETTINGS(device_type=HARDWARE.get_device_type())
|
||||
|
||||
# init driver status
|
||||
self.wheelpos = DriverProb(-1)
|
||||
self.pose = DriverPose(self.settings._POSE_OFFSET_MAX_COUNT)
|
||||
self.phone = DriverProb(self.settings._POSE_OFFSET_MAX_COUNT)
|
||||
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.pose = DriverPose(settings=self.settings)
|
||||
self.blink = DriverBlink()
|
||||
self.phone_prob = 0.
|
||||
|
||||
self.always_on = always_on
|
||||
self.distracted_types = []
|
||||
@@ -256,54 +248,38 @@ class DriverMonitoring:
|
||||
if (self.blink.left + self.blink.right)*0.5 > self.settings._BLINK_THRESHOLD:
|
||||
distracted_types.append(DistractedType.DISTRACTED_BLINK)
|
||||
|
||||
if self.phone.prob_calibrated:
|
||||
using_phone = self.phone.prob > max(min(self.phone.prob_offseter.filtered_stat.M, self.settings._PHONE_MAX_OFFSET), self.settings._PHONE_MIN_OFFSET) \
|
||||
* self.settings._PHONE_THRESH2
|
||||
else:
|
||||
using_phone = self.phone.prob > self.settings._PHONE_THRESH
|
||||
if using_phone:
|
||||
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):
|
||||
rhd_pred = driver_state.wheelOnRightProb
|
||||
left_face_prob = driver_state.leftDriverData.faceProb
|
||||
right_face_prob = driver_state.rightDriverData.faceProb
|
||||
# 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):
|
||||
if car_speed > self.settings._WHEELPOS_CALIB_MIN_SPEED and (left_face_prob > self.settings._FACE_THRESHOLD or
|
||||
right_face_prob > self.settings._FACE_THRESHOLD):
|
||||
self.wheelpos.prob_offseter.push_and_update(rhd_pred)
|
||||
|
||||
self.wheelpos.prob_calibrated = self.wheelpos.prob_offseter.filtered_stat.n > self.settings._WHEELPOS_FILTER_MIN_COUNT
|
||||
startup_override = None
|
||||
if not self.wheelpos.prob_calibrated and not demo_mode and not op_engaged:
|
||||
left_face_detected = driver_state.leftDriverData.faceProb > self.settings._FACE_THRESHOLD
|
||||
right_face_detected = driver_state.rightDriverData.faceProb > self.settings._FACE_THRESHOLD
|
||||
if rhd_pred <= self.settings._WHEELPOS_STARTUP_OVERRIDE_LHD and left_face_detected and not right_face_detected:
|
||||
startup_override = False
|
||||
elif rhd_pred >= self.settings._WHEELPOS_STARTUP_OVERRIDE_RHD and right_face_detected and not left_face_detected:
|
||||
startup_override = True
|
||||
|
||||
if self.wheelpos.prob_calibrated or demo_mode:
|
||||
wheelpos_mean = self.wheelpos.prob_offseter.filtered_stat.M
|
||||
enter_rhd = self.settings._WHEELPOS_THRESHOLD_ENTER_RHD
|
||||
enter_lhd = self.settings._WHEELPOS_THRESHOLD_ENTER_LHD
|
||||
|
||||
# Hysteresis: avoid side flapping near 0.5 and preserve last stable side.
|
||||
if self.wheel_on_right_last is None:
|
||||
if wheelpos_mean >= enter_rhd:
|
||||
self.wheel_on_right = True
|
||||
elif wheelpos_mean <= enter_lhd:
|
||||
self.wheel_on_right = False
|
||||
else:
|
||||
self.wheel_on_right = self.wheel_on_right_default
|
||||
elif self.wheel_on_right_last:
|
||||
self.wheel_on_right = wheelpos_mean > enter_lhd
|
||||
else:
|
||||
self.wheel_on_right = wheelpos_mean >= enter_rhd
|
||||
elif startup_override is not None:
|
||||
self.wheel_on_right = startup_override
|
||||
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
|
||||
|
||||
# On mici/C4, wheel-side inference can hover around 0.5 during startup or after off-car testing.
|
||||
# If one face side is clearly valid and the other is below threshold, prefer the obvious face side
|
||||
# when the wheel-side signal is still weak instead of latching the wrong side and reporting no face.
|
||||
left_face_detected = left_face_prob > self.settings._FACE_THRESHOLD
|
||||
right_face_detected = right_face_prob > self.settings._FACE_THRESHOLD
|
||||
weak_wheelside_signal = abs(rhd_pred - self.settings._WHEELPOS_THRESHOLD) < 0.1
|
||||
if weak_wheelside_signal or not self.wheelpos.prob_calibrated:
|
||||
if left_face_detected and not right_face_detected:
|
||||
self.wheel_on_right = False
|
||||
elif right_face_detected and not left_face_detected:
|
||||
self.wheel_on_right = True
|
||||
|
||||
# make sure no switching when engaged
|
||||
if op_engaged and self.wheel_on_right_last is not None and self.wheel_on_right_last != self.wheel_on_right and not demo_mode:
|
||||
self.wheel_on_right = self.wheel_on_right_last
|
||||
@@ -325,7 +301,7 @@ class DriverMonitoring:
|
||||
* (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
|
||||
self.blink.right = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) \
|
||||
* (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
|
||||
self.phone.prob = driver_data.phoneProb
|
||||
self.phone_prob = driver_data.phoneProb
|
||||
|
||||
self.distracted_types = self._get_distracted_types()
|
||||
self.driver_distracted = (DistractedType.DISTRACTED_PHONE in self.distracted_types
|
||||
@@ -339,11 +315,9 @@ class DriverMonitoring:
|
||||
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.phone.prob_offseter.push_and_update(self.phone.prob)
|
||||
|
||||
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.phone.prob_calibrated = self.phone.prob_offseter.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:
|
||||
@@ -449,8 +423,8 @@ class DriverMonitoring:
|
||||
"posePitchValidCount": self.pose.pitch_offseter.filtered_stat.n,
|
||||
"poseYawOffset": self.pose.yaw_offseter.filtered_stat.mean(),
|
||||
"poseYawValidCount": self.pose.yaw_offseter.filtered_stat.n,
|
||||
"phoneProbOffset": self.phone.prob_offseter.filtered_stat.mean(),
|
||||
"phoneProbValidCount": self.phone.prob_offseter.filtered_stat.n,
|
||||
"phoneProbOffset": 0.,
|
||||
"phoneProbValidCount": 0,
|
||||
"stepChange": self.step_change,
|
||||
"awarenessActive": self.awareness_active,
|
||||
"awarenessPassive": self.awareness_passive,
|
||||
|
||||
@@ -30,23 +30,6 @@ def make_msg(face_detected, distracted=False, model_uncertain=False):
|
||||
return ds
|
||||
|
||||
|
||||
def make_dual_msg(left_face_prob, right_face_prob, wheel_on_right_prob=0., model_uncertain=False):
|
||||
ds = log.DriverStateV2.new_message()
|
||||
ds.wheelOnRightProb = wheel_on_right_prob
|
||||
for side, face_prob in ((ds.leftDriverData, left_face_prob), (ds.rightDriverData, right_face_prob)):
|
||||
side.faceOrientation = [0., 0., 0.]
|
||||
side.facePosition = [0., 0.]
|
||||
side.faceProb = face_prob
|
||||
side.leftEyeProb = 1.
|
||||
side.rightEyeProb = 1.
|
||||
side.leftBlinkProb = 0.
|
||||
side.rightBlinkProb = 0.
|
||||
side.faceOrientationStd = [1.*model_uncertain, 1.*model_uncertain, 1.*model_uncertain]
|
||||
side.facePositionStd = [1.*model_uncertain, 1.*model_uncertain]
|
||||
side.phoneProb = 0.
|
||||
return ds
|
||||
|
||||
|
||||
# driver state from neural net, 10Hz
|
||||
msg_NO_FACE_DETECTED = make_msg(False)
|
||||
msg_ATTENTIVE = make_msg(True)
|
||||
@@ -88,16 +71,6 @@ class TestMonitoring:
|
||||
events, _ = self._run_seq(always_attentive, always_false, always_true, always_false)
|
||||
self._assert_no_events(events)
|
||||
|
||||
def test_saved_rhd_recovers_to_lhd_with_strong_left_face(self):
|
||||
settings = DRIVER_MONITOR_SETTINGS(device_type='mici')
|
||||
DM = DriverMonitoring(rhd_saved=True, settings=settings)
|
||||
msg = make_dual_msg(left_face_prob=0.95, right_face_prob=0.2, wheel_on_right_prob=0.35)
|
||||
|
||||
DM._update_states(msg, [0, 0, 0], 0, False, False)
|
||||
|
||||
assert not DM.wheel_on_right
|
||||
assert DM.face_detected
|
||||
|
||||
# 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)
|
||||
@@ -230,3 +203,4 @@ class TestMonitoring:
|
||||
events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names
|
||||
assert EventName.driverUnresponsive in \
|
||||
events[int((INVISIBLE_SECONDS_TO_RED-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names
|
||||
|
||||
|
||||
Reference in New Issue
Block a user