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