From 41dea5d48d70b7fd47ab9a413b43aae9df9d80ce Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Sun, 26 Apr 2026 13:02:31 -0700 Subject: [PATCH] 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}"