diff --git a/cereal/log.capnp b/cereal/log.capnp index 39e9f50..590c92a 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -70,12 +70,12 @@ struct OnroadEvent @0xc4fa6047f024e718 { longitudinalManeuver @30; steerTempUnavailableSilent @31; resumeRequired @32; - driverDistracted1 @33; - driverDistracted2 @34; - driverDistracted3 @35; - driverUnresponsive1 @36; - driverUnresponsive2 @37; - driverUnresponsive3 @38; + preDriverDistracted @33; + promptDriverDistracted @34; + driverDistracted @35; + preDriverUnresponsive @36; + promptDriverUnresponsive @37; + driverUnresponsive @38; belowSteerSpeed @39; lowBattery @40; accFaulted @41; @@ -2183,7 +2183,6 @@ struct DriverStateV2 { rightBlinkProb @8 :Float32; sunglassesProb @9 :Float32; phoneProb @13 :Float32; - sleepProb @14 :Float32; notReadyProbDEPRECATED @12 :List(Float32); occludedProbDEPRECATED @10 :Float32; readyProbDEPRECATED @11 :List(Float32); @@ -2225,7 +2224,7 @@ struct DriverStateDEPRECATED @0xb83c6cc593ed0a00 { stdDEPRECATED @2 :Float32; } -struct DriverMonitoringStateDEPRECATED @0xb83cda094a1da284 { +struct DriverMonitoringState @0xb83cda094a1da284 { events @18 :List(OnroadEvent); faceDetected @1 :Bool; isDistracted @2 :Bool; @@ -2251,81 +2250,6 @@ struct DriverMonitoringStateDEPRECATED @0xb83cda094a1da284 { eventsDEPRECATED @0 :List(Car.OnroadEventDEPRECATED); } -struct DriverMonitoringState { - lockout @0 :Bool; - lockoutCount @15 :Int8; - lockoutMinutesRemaining @11 :Int8; - alert3Count @12 :Int8; - noResponseCount @13 :Int8; - noResponseForceDecel @14 :Bool; - - alwaysOn @3 :Bool; - alwaysOnLockout @4 :Bool; - - alertLevel @5 :AlertLevel; - activePolicy @6 :MonitoringPolicy; - isRHD @7 :Bool; - rhdCalibration @8 :CalibrationState; - - visionPolicyState @9 :VisionPolicyState; - wheeltouchPolicyState @10 :WheeltouchPolicyState; - - enum AlertLevel { - none @0; - one @1; - two @2; - three @3; - } - - enum MonitoringPolicy { - wheeltouch @0; - vision @1; - } - - struct VisionPolicyState { - awarenessPercent @0 :Int8; - awarenessStep @1 :Float32; - isDistracted @2 :Bool; - distractedTypes @3 :DistractedTypes; - - faceDetected @4 :Bool; - pose @5 :Pose; - wheeltouchFallbackPercent @6 :Int8; - uncertainOffroadAlertPercent @7 :Int8; - - struct DistractedTypes { - pose @0: Bool; - eye @1: Bool; - phone @2: Bool; - } - - struct Pose { - pitch @0 :Float32; - yaw @1 :Float32; - pitchCalib @2 :CalibrationState; - yawCalib @3 :CalibrationState; - calibrated @4 :Bool; - uncertainty @5 :Float32; - } - } - - struct WheeltouchPolicyState { - awarenessPercent @0 :Int8; - awarenessStep @1 :Float32; - driverInteracting @2 :Bool; - } - - struct CalibrationState { - calibratedPercent @0 :Int8; - offset @1 :Float32; - } - - deprecated :group { - alertCountLockoutPercent @1 :Int8; - alertTimeLockoutPercent @2 :Int8; - } -} - struct Boot { wallTimeNanos @0 :UInt64; pstore @4 :Map(Text, Data); @@ -2646,7 +2570,7 @@ struct Event { thumbnail @66: Thumbnail; onroadEvents @134: List(OnroadEvent); carParams @69: Car.CarParams; - driverMonitoringState @165: DriverMonitoringState; + driverMonitoringState @71: DriverMonitoringState; livePose @129 :LivePose; modelV2 @75 :ModelDataV2; drivingModelData @128 :DrivingModelData; @@ -2769,7 +2693,6 @@ struct Event { wifiScanDEPRECATED @29 :List(Legacy.WifiScan); uiNavigationEventDEPRECATED @50 :Legacy.UiNavigationEvent; liveMapDataDEPRECATED @62 :LiveMapDataDEPRECATED; - driverMonitoringStateDEPRECATED @71 :DriverMonitoringStateDEPRECATED; gpsPlannerPointsDEPRECATED @40 :Legacy.GPSPlannerPoints; gpsPlannerPlanDEPRECATED @41 :Legacy.GPSPlannerPlan; applanixRawDEPRECATED @42 :Data; diff --git a/common/params_keys.h b/common/params_keys.h index 7b773df..dd6aed1 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -41,7 +41,6 @@ inline static std::unordered_map keys = { {"DoShutdown", {CLEAR_ON_MANAGER_START, BOOL}}, {"DoUninstall", {CLEAR_ON_MANAGER_START, BOOL}}, {"DriverTooDistracted", {CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON, BOOL}}, - {"DriverLockoutCount", {CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON, INT, "0"}}, {"AlphaLongitudinalEnabled", {PERSISTENT, BOOL}}, {"ExperimentalMode", {PERSISTENT, BOOL}}, {"ExperimentalModeConfirmed", {PERSISTENT, BOOL}}, diff --git a/iqdbc_repo/iqdbc/car/volkswagen/carcontroller.py b/iqdbc_repo/iqdbc/car/volkswagen/carcontroller.py index ff9c42a..595ab1b 100644 --- a/iqdbc_repo/iqdbc/car/volkswagen/carcontroller.py +++ b/iqdbc_repo/iqdbc/car/volkswagen/carcontroller.py @@ -474,7 +474,7 @@ class CarController(CarControllerBase): if hud_control.leadDistanceBars != self.lead_distance_bars_last: self.distance_bar_frame = self.frame - if self.frame % self.CCP.ACC_HUD_STEP == 0 and self.CP.openpilotLongitudinalControl: + if self.frame % self.CCP.ACC_HUD_STEP == 0 and self.CP.openpilotLongitudinalControl and not CS.out.radarDisableFailed: if self.CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO): fcw_alert = hud_control.visualAlert == VisualAlert.fcw show_distance_bars = self.frame - self.distance_bar_frame < 400 diff --git a/iqdbc_repo/iqdbc/car/volkswagen/carstate.py b/iqdbc_repo/iqdbc/car/volkswagen/carstate.py index 198c829..2a2b130 100644 --- a/iqdbc_repo/iqdbc/car/volkswagen/carstate.py +++ b/iqdbc_repo/iqdbc/car/volkswagen/carstate.py @@ -340,11 +340,19 @@ class CarState(CarStateBase): self.ldw_stock_values = cam_cp.vl["LDW_02"] - awv_values = ext_cp.vl.get("AWV_03", ext_cp.vl.get("ACC_10", {})) - ret.stockFcw = bool(awv_values.get("FCW_Active", 0)) or bool(awv_values.get("AWV2_Freigabe", 0)) + if not (self.CP.flags & VolkswagenFlags.DISABLE_RADAR): + awv_values = ext_cp.vl.get("AWV_03", ext_cp.vl.get("ACC_10", {})) + ret.stockFcw = bool(awv_values.get("FCW_Active", 0)) or bool(awv_values.get("AWV2_Freigabe", 0)) + else: + ret.stockFcw = False ret.stockAeb = False - self.acc_type = ext_cp.vl["ACC_18"]["ACC_Typ"] + # Camera harness (DISABLE_RADAR): ext_cp == pt_cp, so ACC_18 reads our own sent value. + # Hardcode acc_type=2 (stop-and-go capable) to avoid self-referential read on first frame. + if self.CP.flags & VolkswagenFlags.DISABLE_RADAR: + self.acc_type = 2 + else: + self.acc_type = ext_cp.vl["ACC_18"]["ACC_Typ"] self.travel_assist_available = bool(pt_cp.vl.get("TA_01", {}).get("Travel_Assist_Available", 0)) ret.cruiseState.available = pt_cp.vl["Motor_51"]["TSK_Status"] in (2, 3, 4, 5) @@ -764,11 +772,13 @@ class CarState(CarStateBase): # math.nan → ignore_alive=True so it never contributes to can_valid. ("TA_01", math.nan), ] - if CP.networkLocation == NetworkLocation.fwdCamera: + # AWV_03 (stock radar FCW/AEB) — don't subscribe when DISABLE_RADAR, the radar is silenced + # and our carcontroller sends the replacement. Subscribing causes CAN parser timeout errors. + if CP.networkLocation == NetworkLocation.fwdCamera and not (CP.flags & VolkswagenFlags.DISABLE_RADAR): pt_messages.append(("AWV_03", 1)) cam_messages = [] - if CP.networkLocation == NetworkLocation.gateway: + if CP.networkLocation == NetworkLocation.gateway and not (CP.flags & VolkswagenFlags.DISABLE_RADAR): cam_messages.append(("AWV_03", 1)) return { diff --git a/iqdbc_repo/iqdbc/car/volkswagen/mebcan.py b/iqdbc_repo/iqdbc/car/volkswagen/mebcan.py index 2740b17..3c4fc44 100644 --- a/iqdbc_repo/iqdbc/car/volkswagen/mebcan.py +++ b/iqdbc_repo/iqdbc/car/volkswagen/mebcan.py @@ -28,7 +28,7 @@ def create_steering_control(packer, bus, apply_curvature, lkas_enabled, power): values = { "Curvature": abs(apply_curvature), # in rad/m "Curvature_VZ": 1 if apply_curvature > 0 and lkas_enabled else 0, - "Power": 100 if lkas_enabled else 0, # TEST: hard 100%, no ramp + "Power": power if lkas_enabled else 0, "RequestStatus": 4 if lkas_enabled else 2, "HighSendRate": lkas_enabled, } diff --git a/iqdbc_repo/iqdbc/car/volkswagen/values.py b/iqdbc_repo/iqdbc/car/volkswagen/values.py index 9885559..3de31b4 100644 --- a/iqdbc_repo/iqdbc/car/volkswagen/values.py +++ b/iqdbc_repo/iqdbc/car/volkswagen/values.py @@ -141,6 +141,8 @@ class CarControllerParams: } elif CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO): + self.AEB_CONTROL_STEP = 100 # AWV_03 radar-replacement at 1Hz (class default 2 = 50Hz is for MQB ACC_10) + self.AEB_HUD_STEP = 20 # MEB_AWV_01 AEB HUD at 5Hz self.LDW_STEP = 10 self.ACC_HUD_STEP = 6 self.STEER_DRIVER_ALLOWANCE = 60 diff --git a/iqdbc_repo/iqdbc/safety/modes/volkswagen_meb.h b/iqdbc_repo/iqdbc/safety/modes/volkswagen_meb.h index 2777e91..a88d6d3 100644 --- a/iqdbc_repo/iqdbc/safety/modes/volkswagen_meb.h +++ b/iqdbc_repo/iqdbc/safety/modes/volkswagen_meb.h @@ -244,11 +244,11 @@ static bool volkswagen_meb_tx_hook(const CANPacket_t *msg) { }; const CurvatureSteeringLimits VOLKSWAGEN_MEB_STEERING_LIMITS = { - .max_curvature = 32767, // TEST: 15-bit max, no curvature ceiling + .max_curvature = 29105, .curvature_to_can = 149253.7313f, .send_rate = 0.02f, .inactive_curvature_is_zero = true, - .max_power = 65535, // TEST: no power ceiling + .max_power = 225, // 90% (raw byte, 0.4 %/bit; matches Python STEERING_POWER_MAX = 90) }; bool tx = true; diff --git a/iqpilot/common/atlas_alerts.py b/iqpilot/common/atlas_alerts.py index 8060fda..5339155 100644 --- a/iqpilot/common/atlas_alerts.py +++ b/iqpilot/common/atlas_alerts.py @@ -214,10 +214,9 @@ class NoEntryCard(AlertCard): def __init__(self, alert_text_2: str, alert_text_1: str = "openpilot Unavailable", - visual_alert: car.CarControl.HUDControl.VisualAlert = VisualAlert.none, - priority: Tier = Tier.LOW): + visual_alert: car.CarControl.HUDControl.VisualAlert = VisualAlert.none): primary, secondary, size = _mici_reframe(alert_text_1, alert_text_2) - super().__init__(primary, secondary, AlertStatus.normal, size, priority, visual_alert, AudibleAlert.refuse, 3.0) + super().__init__(primary, secondary, AlertStatus.normal, size, Tier.LOW, visual_alert, AudibleAlert.refuse, 3.0) class GentleDisableCard(AlertCard): diff --git a/panda/board/drivers/can_common.h b/panda/board/drivers/can_common.h index 08f21e0..af1f510 100644 --- a/panda/board/drivers/can_common.h +++ b/panda/board/drivers/can_common.h @@ -237,16 +237,17 @@ void ignition_can_hook(CANPacket_t *msg) { } } - - } - - // Volkswagen MEB / MQBevo exception (Klemmen_Status_01) - // On gateway harness cars this message is on bus 1 (powertrain CAN), not bus 0. - if ((msg->bus == 0U) || (msg->bus == 1U)) { - int len = GET_LEN(msg); + // Volkswagen MEB exception if ((msg->addr == 0x3C0U) && (len == 4)) { - ignition_can = ((msg->data[2] >> 1U) & 1U) != 0U; - ignition_can_cnt = 0U; + int counter = msg->data[1] & 0xFU; + + static int prev_counter_vw_meb = -1; + if ((counter == ((prev_counter_vw_meb + 1) % 16)) && (prev_counter_vw_meb != -1)) { + // Klemmen_Status_01->ZAS_Kl_15 + ignition_can = ((msg->data[2] >> 1) & 1U) != 0U; + ignition_can_cnt = 0U; + } + prev_counter_vw_meb = counter; } } } diff --git a/selfdrive/assets/sounds/pre_alert.wav b/selfdrive/assets/sounds/pre_alert.wav deleted file mode 100644 index f83b710..0000000 Binary files a/selfdrive/assets/sounds/pre_alert.wav and /dev/null differ diff --git a/selfdrive/assets/sounds/prompt_distracted.wav b/selfdrive/assets/sounds/prompt_distracted.wav index 6cad556..c3d4475 100644 Binary files a/selfdrive/assets/sounds/prompt_distracted.wav and b/selfdrive/assets/sounds/prompt_distracted.wav differ diff --git a/selfdrive/assets/sounds/warning_immediate.wav b/selfdrive/assets/sounds/warning_immediate.wav index 99b4b03..b1815a9 100644 Binary files a/selfdrive/assets/sounds/warning_immediate.wav and b/selfdrive/assets/sounds/warning_immediate.wav differ diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 3def978..1bd7b31 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -260,7 +260,7 @@ class Controls(IQControlsLayer): hudControl.leadFollowTime = 1.45 hudControl.visualAlert = self.sm['selfdriveState'].alertHudVisual hudControl.audibleAlert = self.sm['selfdriveState'].alertSound - hudControl.driverUnresponsive = self.sm['driverMonitoringState'].noResponseForceDecel + hudControl.driverUnresponsive = self.sm['selfdriveState'].alertType.split('/', 1)[0] == 'driverUnresponsive' hudControl.rightLaneVisible = True hudControl.leftLaneVisible = True @@ -292,7 +292,7 @@ class Controls(IQControlsLayer): cs.upAccelCmd = float(self.LoC.pid.p) cs.uiAccelCmd = float(self.LoC.pid.i) cs.ufAccelCmd = float(self.LoC.pid.f) - cs.forceDecel = bool(self.sm['driverMonitoringState'].noResponseForceDecel or + cs.forceDecel = bool((self.sm['driverMonitoringState'].awarenessStatus < 0.) or (self.sm['selfdriveState'].state == State.softDisabling)) lat_tuning = self.CP.lateralTuning.which() diff --git a/selfdrive/debug/cycle_alerts.py b/selfdrive/debug/cycle_alerts.py index c1f30bc..5b25d8c 100755 --- a/selfdrive/debug/cycle_alerts.py +++ b/selfdrive/debug/cycle_alerts.py @@ -30,9 +30,9 @@ def cycle_alerts(duration=200, is_metric=False): (EventName.accFaulted, ET.IMMEDIATE_DISABLE), # DM sequence - (EventName.driverDistracted1, ET.WARNING), - (EventName.driverDistracted2, ET.WARNING), - (EventName.driverDistracted3, ET.WARNING), + (EventName.preDriverDistracted, ET.WARNING), + (EventName.promptDriverDistracted, ET.WARNING), + (EventName.driverDistracted, ET.WARNING), ] # debug alerts diff --git a/selfdrive/modeld/dmonitoringmodeld.py b/selfdrive/modeld/dmonitoringmodeld.py index 73a4af2..0e50932 100755 --- a/selfdrive/modeld/dmonitoringmodeld.py +++ b/selfdrive/modeld/dmonitoringmodeld.py @@ -75,7 +75,7 @@ def parse_model_output(model_output): face_descs = model_output[f'face_descs_{ds_suffix}'] parsed[f'face_descs_{ds_suffix}'] = face_descs[:, :-6] parsed[f'face_descs_{ds_suffix}_std'] = safe_exp(face_descs[:, -6:]) - for key in ['face_prob', 'left_eye_prob', 'right_eye_prob','left_blink_prob', 'right_blink_prob', 'sunglasses_prob', 'using_phone_prob', 'sleep_prob']: + for key in ['face_prob', 'left_eye_prob', 'right_eye_prob','left_blink_prob', 'right_blink_prob', 'sunglasses_prob', 'using_phone_prob']: parsed[f'{key}_{ds_suffix}'] = sigmoid(model_output[f'{key}_{ds_suffix}']) return parsed @@ -91,7 +91,6 @@ def fill_driver_data(msg, model_output, ds_suffix): msg.rightBlinkProb = model_output[f'right_blink_prob_{ds_suffix}'][0, 0].item() msg.sunglassesProb = model_output[f'sunglasses_prob_{ds_suffix}'][0, 0].item() msg.phoneProb = model_output[f'using_phone_prob_{ds_suffix}'][0, 0].item() - msg.sleepProb = model_output[f'sleep_prob_{ds_suffix}'][0, 0].item() def get_driverstate_packet(model_output, frame_id: int, location_ts: int, exec_time: float, gpu_exec_time: float): msg = messaging.new_message('driverStateV2', valid=True) diff --git a/selfdrive/modeld/models/dmonitoring_model.onnx b/selfdrive/modeld/models/dmonitoring_model.onnx index 873c74b..51d1849 100644 Binary files a/selfdrive/modeld/models/dmonitoring_model.onnx and b/selfdrive/modeld/models/dmonitoring_model.onnx differ diff --git a/selfdrive/modeld/models/dmonitoring_model_metadata.pkl b/selfdrive/modeld/models/dmonitoring_model_metadata.pkl index 9aad8d4..7869f9d 100644 Binary files a/selfdrive/modeld/models/dmonitoring_model_metadata.pkl and b/selfdrive/modeld/models/dmonitoring_model_metadata.pkl differ diff --git a/selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl b/selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl index 8f33bbb..a89f4f1 100644 Binary files a/selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl and b/selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl differ diff --git a/selfdrive/modeld/models/prebuilt_check.json b/selfdrive/modeld/models/prebuilt_check.json index a40a808..6d0839d 100644 --- a/selfdrive/modeld/models/prebuilt_check.json +++ b/selfdrive/modeld/models/prebuilt_check.json @@ -1,9 +1,9 @@ { "dmonitoring_model": { "outputs": { - "dmonitoring_model_metadata.pkl": "5999c262b1c25c62e485fb4ced5806d20a8ca59e3ae94e0ff499c0fe3497fedc", - "dmonitoring_model_tinygrad.pkl": "5aca89a35b42376d56f67ccd28a1080806706d546dd8c63f6e1b4c681f8e2c01" + "dmonitoring_model_metadata.pkl": "31a86ab7a92dc0af088b15787a440dd3b210aa662e445a15145900e559a1b5c3", + "dmonitoring_model_tinygrad.pkl": "806c0ea75df6bf6dfeb81b832314c68e31df5865a52d0359e6eeb76d93ad2b52" }, - "signature": "2364ebd4bb95c4b4e539c9b1ba68324b713cbb56617396b73262accf9cdcfbfc" + "signature": "e1eeb5ce45774a816c8da2394e6ee35ebf700b71dff345e0141dbab8ff592349" } } diff --git a/selfdrive/modeld/test_dmonitoringmodeld.py b/selfdrive/modeld/test_dmonitoringmodeld.py deleted file mode 100644 index af23f46..0000000 --- a/selfdrive/modeld/test_dmonitoringmodeld.py +++ /dev/null @@ -1,19 +0,0 @@ -import pickle - -import numpy as np - -from openpilot.selfdrive.modeld.dmonitoringmodeld import get_driverstate_packet, parse_model_output, slice_outputs -from openpilot.selfdrive.modeld.dmonitoringmodeld import METADATA_PATH - - -def test_sleep_probability_output(): - with open(METADATA_PATH, 'rb') as f: - metadata = pickle.load(f) - - output = np.zeros(metadata['output_shapes']['outputs'][1], dtype=np.float32) - parsed = parse_model_output(slice_outputs(output, metadata['output_slices'])) - parsed['raw_pred'] = b'' - msg = get_driverstate_packet(parsed, 1, 0, 0., 0.) - - assert msg.driverStateV2.leftDriverData.sleepProb == 0.5 - assert msg.driverStateV2.rightDriverData.sleepProb == 0.5 diff --git a/selfdrive/monitoring/README.md b/selfdrive/monitoring/README.md new file mode 100644 index 0000000..2a29ea0 --- /dev/null +++ b/selfdrive/monitoring/README.md @@ -0,0 +1,15 @@ +# driver monitoring (DM) + +Uploading driver-facing camera footage is opt-in, but it is encouraged to opt-in to improve the DM model. You can always change your preference using the "Record and Upload Driver Camera" toggle. + +## Troubleshooting + +Before creating a bug report, go through these troubleshooting steps. + +* Ensure the driver-facing camera has a good view of the driver in normal driving positions. + * This can be checked in Settings -> Device -> Preview Driver Camera (when car is off). +* If the camera can't see the driver, the device should be re-mounted. + +## Bug report + +In order for us to look into DM bug reports, we'll need the driver-facing camera footage. If you don't normally have this enabled, simply enable the toggle for a single drive. Also ensure the "Upload Raw Logs" toggle is enabled before going for a drive. diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index ffefe32..022415a 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -1,20 +1,8 @@ #!/usr/bin/env python3 -from types import SimpleNamespace - 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 - - -def get_dm_inputs(sm): - return { - 'driverStateV2': sm['driverStateV2'], - 'liveCalibration': sm['liveCalibration'], - 'carState': sm['carState'], - 'selfdriveState': SimpleNamespace(enabled=sm['selfdriveState'].enabled or sm['carControl'].latActive), - 'modelV2': sm['modelV2'], - } +from openpilot.selfdrive.monitoring.helpers import DriverMonitoring def dmonitoringd_thread(): @@ -37,9 +25,9 @@ def dmonitoringd_thread(): valid = sm.all_checks() if demo_mode and sm.valid['driverStateV2']: - DM.run_step(get_dm_inputs(sm), demo=True) + DM.run_step(sm, demo=demo_mode) elif valid: - DM.run_step(get_dm_inputs(sm), demo=demo_mode) + DM.run_step(sm, demo=demo_mode) # publish dat = DM.get_state_packet(valid=valid) @@ -52,9 +40,9 @@ 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)): - params.put_bool("IsRhdDetected", DM.wheel_on_right) + DM.wheelpos.prob_offseter.filtered_stat.n > DM.settings._WHEELPOS_FILTER_MIN_COUNT and + DM.wheel_on_right == (DM.wheelpos.prob_offseter.filtered_stat.M > DM.settings._WHEELPOS_THRESHOLD)): + params.put_bool_nonblocking("IsRhdDetected", DM.wheel_on_right) def main(): dmonitoringd_thread() diff --git a/selfdrive/monitoring/helpers.py b/selfdrive/monitoring/helpers.py new file mode 100644 index 0000000..6f8752d --- /dev/null +++ b/selfdrive/monitoring/helpers.py @@ -0,0 +1,474 @@ +from math import atan2 +import numpy as np + +from cereal import car, log +import cereal.messaging as messaging +from openpilot.selfdrive.selfdrived.events import Events +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.selfdrive.locationd.calibration_helpers import get_calibrated_rpy +from openpilot.system.hardware import HARDWARE + +EventName = log.OnroadEvent.EventName + +# ****************************************************************************************** +# NOTE: To fork maintainers. +# Disabling or nerfing safety features will get you and your users banned from our servers. +# We recommend that you do not change these numbers from the defaults. +# ****************************************************************************************** + +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. + + self._FACE_THRESHOLD = 0.7 + self._EYE_THRESHOLD = 0.65 + self._SG_THRESHOLD = 0.9 + self._BLINK_THRESHOLD = 0.865 + + self._PHONE_THRESH = 0.75 if device_type == 'mici' else 0.4 + self._PHONE_THRESH2 = 15.0 + self._PHONE_MAX_OFFSET = 0.06 + self._PHONE_MIN_OFFSET = 0.025 + self._PHONE_DATA_AVG = 0.05 + self._PHONE_DATA_VAR = 3*0.005 + self._PHONE_MAX_COUNT = int(360 / self._DT_DMON) + + self._POSE_PITCH_THRESHOLD = 0.3133 + self._POSE_PITCH_THRESHOLD_SLACK = 0.3237 + self._POSE_PITCH_THRESHOLD_STRICT = self._POSE_PITCH_THRESHOLD + self._POSE_YAW_THRESHOLD = 0.4020 + self._POSE_YAW_THRESHOLD_SLACK = 0.5042 + self._POSE_YAW_THRESHOLD_STRICT = self._POSE_YAW_THRESHOLD + self._PITCH_NATURAL_OFFSET = 0.011 # initial value before offset is learned + self._PITCH_NATURAL_THRESHOLD = 0.449 + self._YAW_NATURAL_OFFSET = 0.075 # initial value before offset is learned + self._PITCH_NATURAL_VAR = 3*0.01 + self._YAW_NATURAL_VAR = 3*0.05 + self._PITCH_MAX_OFFSET = 0.124 + self._PITCH_MIN_OFFSET = -0.0881 + self._YAW_MAX_OFFSET = 0.289 + self._YAW_MIN_OFFSET = -0.0246 + + self._DCAM_UNCERTAIN_ALERT_THRESHOLD = 0.1 + 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._ALWAYS_ON_ALERT_MIN_SPEED = 11 + + 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._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_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.calibrated = False + self.low_std = True + self.cfactor_pitch = 1. + self.cfactor_yaw = 1. + +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 + +class DriverBlink: + def __init__(self): + self.left = 0. + self.right = 0. + + +# 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 + +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 + + pitch_net, yaw_net, roll_net = angles_desc + + 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 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(device_type=HARDWARE.get_device_type()) + + # init driver status + wheelpos_filter_raw_priors = (self.settings._WHEELPOS_DATA_AVG, self.settings._WHEELPOS_DATA_VAR, 2) + phone_filter_raw_priors = (self.settings._PHONE_DATA_AVG, self.settings._PHONE_DATA_VAR, 2) + self.wheelpos = DriverProb(raw_priors=wheelpos_filter_raw_priors, max_trackable=self.settings._WHEELPOS_MAX_COUNT) + self.phone = DriverProb(raw_priors=phone_filter_raw_priors, max_trackable=self.settings._PHONE_MAX_COUNT) + self.pose = DriverPose(settings=self.settings) + self.blink = DriverBlink() + + self.always_on = always_on + self.distracted_types = [] + self.driver_distracted = False + 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 + self.face_detected = False + self.terminal_alert_cnt = 0 + self.terminal_time = 0 + self.step_change = 0. + self.active_monitoring_mode = True + 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.dcam_uncertain_cnt = 0 + self.dcam_reset_cnt = 0 + + self.params = Params() + self.too_distracted = self.params.get_bool("DriverTooDistracted") + + self._reset_awareness() + self._set_timers(active_monitoring=True) + self._reset_events() + + def _reset_awareness(self): + self.awareness = 1. + self.awareness_active = 1. + self.awareness_passive = 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 + else: + self.step_change = 0. + return # no exploit after orange alert + elif self.awareness <= 0.: + return + + if active_monitoring: + # 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 + + 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_monitoring_mode: + self.awareness_active = self.awareness + self.awareness = self.awareness_passive + + 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_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) + self.pose.cfactor_pitch = np.interp(bp_normal, [0, 0.5], + [self.settings._POSE_PITCH_THRESHOLD_SLACK, + self.settings._POSE_PITCH_THRESHOLD_STRICT]) / self.settings._POSE_PITCH_THRESHOLD + self.pose.cfactor_yaw = np.interp(bp_normal, [0, 0.5], + [self.settings._POSE_YAW_THRESHOLD_SLACK, + self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD + + def _get_distracted_types(self): + 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_offseter.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(), + 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 + yaw_error = abs(yaw_error) + + 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.left + self.blink.right)*0.5 > self.settings._BLINK_THRESHOLD: + distracted_types.append(DistractedType.DISTRACTED_BLINK) + + if self.phone.prob_calibrated: + using_phone = self.phone.prob > max(min(self.phone.prob_offseter.filtered_stat.M, self.settings._PHONE_MAX_OFFSET), self.settings._PHONE_MIN_OFFSET) \ + * self.settings._PHONE_THRESH2 + else: + using_phone = self.phone.prob > self.settings._PHONE_THRESH + if using_phone: + distracted_types.append(DistractedType.DISTRACTED_PHONE) + + return distracted_types + + def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged, standstill, demo_mode=False): + rhd_pred = driver_state.wheelOnRightProb + # 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.prob_calibrated = self.wheelpos.prob_offseter.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 + else: + self.wheel_on_right = self.wheel_on_right_default # use default/saved if calibration is unfinished + # make sure no switching when engaged + if op_engaged and self.wheel_on_right_last is not None and self.wheel_on_right_last != self.wheel_on_right and not demo_mode: + self.wheel_on_right = self.wheel_on_right_last + driver_data = driver_state.rightDriverData if self.wheel_on_right else driver_state.leftDriverData + if not all(len(x) > 0 for x in (driver_data.faceOrientation, driver_data.facePosition, + driver_data.faceOrientationStd, driver_data.facePositionStd)): + 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) + if self.wheel_on_right: + self.pose.yaw *= -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.blink.left = driver_data.leftBlinkProb * (driver_data.leftEyeProb > self.settings._EYE_THRESHOLD) \ + * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD) + self.blink.right = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) \ + * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD) + self.phone.prob = driver_data.phoneProb + + self.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) + + # 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_offseter.push_and_update(self.pose.pitch) + self.pose.yaw_offseter.push_and_update(self.pose.yaw) + self.phone.prob_offseter.push_and_update(self.phone.prob) + + self.pose.calibrated = self.pose.pitch_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT and \ + self.pose.yaw_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT + self.phone.prob_calibrated = self.phone.prob_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT + + if self.face_detected and not self.driver_distracted: + if model_std_max > self.settings._DCAM_UNCERTAIN_ALERT_THRESHOLD: + 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_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, 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 (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 + self._reset_awareness() + return + + driver_attentive = self.driver_distraction_filter.x < 0.37 + awareness_prev = self.awareness + + if (driver_attentive and self.face_detected and self.pose.low_std and self.awareness > 0): + 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._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.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_prompt: + return + + _reaching_audible = self.awareness - self.step_change <= self.threshold_prompt + _reaching_terminal = self.awareness - self.step_change <= 0 + standstill_orange_exemption = standstill and _reaching_audible + always_on_red_exemption = always_on_valid and not op_engaged and _reaching_terminal + always_on_lowspeed_exemption = always_on_valid and not op_engaged and car_speed < self.settings._ALWAYS_ON_ALERT_MIN_SPEED + + 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 + + if certainly_distracted or maybe_distracted: + # should always be counting if distracted unless at standstill (lowspeed for always-on) and reaching orange + # also will not be reaching 0 if DM is active when not engaged + if not (standstill_orange_exemption or always_on_red_exemption or (always_on_lowspeed_exemption and _reaching_audible)): + self.awareness = max(self.awareness - self.step_change, -0.1) + + alert = None + if self.awareness <= 0.: + # terminal red alert: disengagement required + alert = EventName.driverDistracted if self.active_monitoring_mode else EventName.driverUnresponsive + self.terminal_time += 1 + if awareness_prev > 0.: + self.terminal_alert_cnt += 1 + elif self.awareness <= self.threshold_prompt: + # prompt orange alert + alert = EventName.promptDriverDistracted if self.active_monitoring_mode else EventName.promptDriverUnresponsive + elif self.awareness <= self.threshold_pre and not always_on_lowspeed_exemption: + # pre green alert + alert = EventName.preDriverDistracted if self.active_monitoring_mode else EventName.preDriverUnresponsive + + if alert is not None: + self.current_events.add(alert) + + 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, + "phoneProbOffset": self.phone.prob_offseter.filtered_stat.mean(), + "phoneProbValidCount": self.phone.prob_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: + highway_speed = 30 + enabled = True + wrong_gear = False + standstill = False + driver_engaged = False + brake_disengage_prob = 1.0 + rpyCalib = [0., 0., 0.] + else: + 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 + calib_rpy = get_calibrated_rpy(sm['liveCalibration']) + rpyCalib = calib_rpy.tolist() if calib_rpy is not None else [0., 0., 0.] + self._set_policy( + brake_disengage_prob=brake_disengage_prob, + car_speed=highway_speed, + ) + + # Parse data from dmonitoringmodeld + self._update_states( + driver_state=sm['driverStateV2'], + cal_rpy=rpyCalib, + car_speed=highway_speed, + op_engaged=enabled, + standstill=standstill, + demo_mode=demo, + ) + + # Update distraction events + self._update_events( + driver_engaged=driver_engaged, + op_engaged=enabled, + standstill=standstill, + wrong_gear=wrong_gear, + car_speed=highway_speed + ) diff --git a/selfdrive/monitoring/policy.py b/selfdrive/monitoring/policy.py deleted file mode 100644 index 1c55b4d..0000000 --- a/selfdrive/monitoring/policy.py +++ /dev/null @@ -1,467 +0,0 @@ -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.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 - -AlertLevel = log.DriverMonitoringState.AlertLevel -MonitoringPolicy = log.DriverMonitoringState.MonitoringPolicy - -def to_percent(v): - return int(min(max(v * 100., 0.), 100.)) - -# ****************************************************************************************** -# NOTE: To fork maintainers. -# Disabling or nerfing safety features will get you and your users banned from our servers. -# We recommend that you do not change these numbers from the defaults. -# ****************************************************************************************** - -class DRIVER_MONITOR_SETTINGS: - def __init__(self): - # https://eur-lex.europa.eu/legal-content/EN/TXT/PDF/?uri=OJ:L_202501899 - self._ALERT_MIN_SPEED = 2.8 # 10 km/h - - self._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT = 5. - self._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT = 15. - self._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT = 25. - self._VISION_POLICY_ALERT_1_TIMEOUT = 5. - self._VISION_POLICY_ALERT_2_TIMEOUT = 8. - self._VISION_POLICY_ALERT_3_TIMEOUT = 13. - - # no response = alert_3 sustained for certain amount of time - self._NO_RESPONSE_TIMEOUT = 5. - - # lockout specs - self._MAX_ALERT_3 = 2 - self._MAX_NO_RESPONSE = 1 - self._LOCKOUT_TIMES = [int(60 * n_min / DT_DMON) for n_min in [1, 5, 15, 30]] - - self._TIMEOUT_RECOVERY_FACTOR_MAX = 5. - self._TIMEOUT_RECOVERY_FACTOR_MIN = 1.25 - - self._FACE_THRESHOLD = 0.7 - self._EYE_THRESHOLD = 0.65 - self._SG_THRESHOLD = 0.9 - self._BLINK_THRESHOLD = 0.865 - 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 - self._POSE_YAW_THRESHOLD = 0.4020 - self._POSE_YAW_THRESHOLD_SLACK = 0.5042 - self._POSE_YAW_THRESHOLD_STRICT = self._POSE_YAW_THRESHOLD - self._POSE_YAW_MIN_STEER_DEG = 30 - self._POSE_YAW_STEER_FACTOR = 0.15 - self._POSE_YAW_STEER_MAX_OFFSET = 0.3927 - self._PITCH_NATURAL_OFFSET = 0.011 # initial value before offset is learned - self._PITCH_NATURAL_THRESHOLD = 0.449 - self._YAW_NATURAL_OFFSET = 0.075 # initial value before offset is learned - self._PITCH_NATURAL_VAR = 3*0.01 - self._YAW_NATURAL_VAR = 3*0.05 - self._PITCH_MAX_OFFSET = 0.124 - self._PITCH_MIN_OFFSET = -0.0881 - self._YAW_MAX_OFFSET = 0.289 - 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(2 / 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 / 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 / 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 - -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.calibrated = False - self.low_std = True - self.cfactor_pitch = 1. - self.cfactor_yaw = 1. - self.steer_yaw_offset = 0. - -class DriverBlink: - def __init__(self): - self.left = 0. - self.right = 0. - -# 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) - -def face_orientation_from_model(orient_model, pos_model, rpy_calib): - pitch_model = orient_model[0] - yaw_model = orient_model[1] - - 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 = pitch_model + pitch_focal_angle - yaw = -yaw_model + yaw_focal_angle - - pitch -= rpy_calib[1] - yaw -= rpy_calib[2] - 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() - - # 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.pose = DriverPose(settings=self.settings) - self.blink = DriverBlink() - self.phone_prob = 0. - - self.alert_level = AlertLevel.none - self.always_on = always_on - self.distracted_types = defaultdict(bool) - self.driver_distracted = False - 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 - self.face_detected = False - self.alert_3_cnt = 0 - self.cnt_since_alert_3 = 0 - self.no_response_timeout = int(self.settings._NO_RESPONSE_TIMEOUT / DT_DMON) - self.no_response_cnt = 0 - self.lockout_active = Params().get_bool("DriverTooDistracted") - self.lockout_count = Params().get("DriverLockoutCount") or 0 - self.lockout_duration = self.settings._LOCKOUT_TIMES[min(max(self.lockout_count - 1, 0), len(self.settings._LOCKOUT_TIMES) - 1)] - self.lockout_time_elapsed = 0 - self.step_change = 0. - self.active_policy = MonitoringPolicy.vision - self.driver_interacting = False - 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.dcam_uncertain_cnt = 0 - self.dcam_reset_cnt = 0 - - self._reset_awareness() - self._set_policy(MonitoringPolicy.vision) - - def _reset_awareness(self): - self.awareness = 1. - self.last_vision_awareness = 1. - self.last_wheeltouch_awareness = 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 - else: - self.step_change = 0. - return # no exploit after orange alert - elif self.awareness <= 0.: - return - - if target_policy == MonitoringPolicy.vision: - # 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 - - 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_policy == MonitoringPolicy.vision: - self.last_vision_awareness = self.awareness - self.awareness = self.last_wheeltouch_awareness - - 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_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) - self.pose.cfactor_pitch = np.interp(bp_normal, [0, 0.5], - [self.settings._POSE_PITCH_THRESHOLD_SLACK, - self.settings._POSE_PITCH_THRESHOLD_STRICT]) / self.settings._POSE_PITCH_THRESHOLD - self.pose.cfactor_yaw = np.interp(bp_normal, [0, 0.5], - [self.settings._POSE_YAW_THRESHOLD_SLACK, - self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD - - def _get_distracted_types(self): - 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_offsetter.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(), - 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 - - if yaw_error * self.pose.steer_yaw_offset > 0: # unidirectional - yaw_error = max(abs(yaw_error) - min(abs(self.pose.steer_yaw_offset), self.settings._POSE_YAW_STEER_MAX_OFFSET), 0.) - else: - yaw_error = abs(yaw_error) - - 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.left + self.blink.right)*0.5 > 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, lowspeed, 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) - - wheelpos_calibrated = self.wheelpos_offsetter.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 - else: - self.wheel_on_right = self.wheel_on_right_default # use default/saved if calibration is unfinished - # make sure no switching when engaged - if op_engaged and self.wheel_on_right_last is not None and self.wheel_on_right_last != self.wheel_on_right and not demo_mode: - self.wheel_on_right = self.wheel_on_right_last - driver_data = driver_state.rightDriverData if self.wheel_on_right else driver_state.leftDriverData - if not all(len(x) > 0 for x in (driver_data.faceOrientation, driver_data.facePosition, - driver_data.faceOrientationStd, driver_data.facePositionStd)): - 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) - 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.blink.left = driver_data.leftBlinkProb * (driver_data.leftEyeProb > self.settings._EYE_THRESHOLD) \ - * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD) - self.blink.right = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) \ - * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD) - self.phone_prob = driver_data.phoneProb - - self._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) - - # 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_offsetter.push_and_update(self.pose.pitch) - self.pose.yaw_offsetter.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 - - 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 lowspeed: - 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) - 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, lowspeed, wrong_gear): - self.alert_level = AlertLevel.none - self.driver_interacting = driver_engaged - - if self.alert_3_cnt >= self.settings._MAX_ALERT_3 or self.no_response_cnt >= self.settings._MAX_NO_RESPONSE: - if not self.lockout_active: - self.lockout_count += 1 - self.lockout_duration = self.settings._LOCKOUT_TIMES[min(self.lockout_count - 1, len(self.settings._LOCKOUT_TIMES) - 1)] - Params().put("DriverLockoutCount", self.lockout_count) - self.lockout_active = True - - if self.lockout_active: - self.lockout_time_elapsed += 1 - if self.lockout_time_elapsed > self.lockout_duration: - self.lockout_active = False - self.alert_3_cnt = 0 - self.cnt_since_alert_3 = 0 - self.no_response_cnt = 0 - self.lockout_time_elapsed = 0 - - 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 \ - (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 - self._reset_awareness() - 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 - lowspeed_exemption = lowspeed 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 lowspeed_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._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.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_alert_2: - 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 - - if certainly_distracted or maybe_distracted: - # should always be counting if distracted unless at low speed and reaching green - # also will not be reaching 0 if DM is active when not engaged - if not (lowspeed_exemption or always_on_exemption): - self.awareness = max(self.awareness - self.step_change, -0.1) - - if self.awareness <= 0.: - # terminal alert: disengagement required - self.alert_level = AlertLevel.three - if awareness_prev > 0.: - self.alert_3_cnt += 1 - self.cnt_since_alert_3 = 0 - else: - self.cnt_since_alert_3 += 1 - if self.cnt_since_alert_3 == self.no_response_timeout: - self.no_response_cnt += 1 - else: - if 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) - dm = dat.driverMonitoringState - - dm.lockout = self.lockout_active - dm.lockoutCount = self.lockout_count - if self.lockout_active: - dm.lockoutMinutesRemaining = max(1, round((self.lockout_duration - self.lockout_time_elapsed) * DT_DMON / 60.)) - dm.alert3Count = self.alert_3_cnt - dm.noResponseCount = self.no_response_cnt - dm.noResponseForceDecel = self.alert_level == AlertLevel.three and self.cnt_since_alert_3 >= self.no_response_timeout - 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: - car_speed = 30 - enabled = True - wrong_gear = False - lowspeed = False - driver_engaged = False - brake_disengage_prob = 1.0 - steering_angle_deg = 0.0 - rpyCalib = [0., 0., 0.] - else: - car_speed = sm['carState'].vEgo - enabled = sm['selfdriveState'].enabled - wrong_gear = sm['carState'].gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low) - lowspeed = car_speed < self.settings._ALERT_MIN_SPEED - 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( - brake_disengage_prob=brake_disengage_prob, - car_speed=car_speed, - ) - - # Parse data from dmonitoringmodeld - self._update_states( - driver_state=sm['driverStateV2'], - cal_rpy=rpyCalib, - car_speed=car_speed, - op_engaged=enabled, - lowspeed=lowspeed, - demo_mode=demo, - steering_angle_deg=steering_angle_deg, - ) - - # Update distraction events - self._update_events( - driver_engaged=driver_engaged, - op_engaged=enabled, - lowspeed=lowspeed, - wrong_gear=wrong_gear, - ) diff --git a/selfdrive/monitoring/test_dmonitoringd.py b/selfdrive/monitoring/test_dmonitoringd.py deleted file mode 100644 index 8aae92e..0000000 --- a/selfdrive/monitoring/test_dmonitoringd.py +++ /dev/null @@ -1,24 +0,0 @@ -from types import SimpleNamespace - -import pytest - -from openpilot.selfdrive.monitoring.dmonitoringd import get_dm_inputs - - -@pytest.mark.parametrize("enabled, lat_active, expected", [ - (False, False, False), - (True, False, True), - (False, True, True), - (True, True, True), -]) -def test_iq_enabled_adapter(enabled, lat_active, expected): - sm = { - 'driverStateV2': object(), - 'liveCalibration': object(), - 'carState': object(), - 'selfdriveState': SimpleNamespace(enabled=enabled), - 'modelV2': object(), - 'carControl': SimpleNamespace(latActive=lat_active), - } - - assert get_dm_inputs(sm)['selfdriveState'].enabled == expected diff --git a/selfdrive/monitoring/test_monitoring.py b/selfdrive/monitoring/test_monitoring.py index 37606fe..fef6a7e 100644 --- a/selfdrive/monitoring/test_monitoring.py +++ b/selfdrive/monitoring/test_monitoring.py @@ -1,15 +1,19 @@ -from cereal import log +import numpy as np +import pytest + +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 +37,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 @@ -47,66 +51,51 @@ always_true = [True] * int(TEST_TIMESPAN / DT_DMON) always_false = [False] * int(TEST_TIMESPAN / DT_DMON) class TestMonitoring: - def _run_seq(self, msgs, interaction, engaged, lowspeed): + 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], lowspeed[idx]) + 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], lowspeed[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.preDriverDistracted + 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.promptDriverDistracted + assert events[int((d_status.settings._DISTRACTED_TIME + \ + ((TEST_TIMESPAN-10-d_status.settings._DISTRACTED_TIME)/2))/DT_DMON)].names[0] == EventName.driverDistracted assert isinstance(d_status.awareness, float) - # engaged, distracted past red and beyond the no-response window -> unavailability response + lockout - def test_distracted_lockout(self): - alert_lvls, d_status = self._run_seq(always_distracted, always_false, always_true, always_false) - assert alert_lvls[int(DISTRACTED_SECONDS_TO_RED / DT_DMON)] == 3 - assert d_status.lockout_active - assert d_status.lockout_time_elapsed > 0 - assert d_status.lockout_count >= 1 - - # no face -> wheeltouch red, sustained past the no-response timeout -> unavailability response + lockout - def test_invisible_lockout(self): - _, d_status = self._run_seq(always_no_face, always_false, always_true, always_false) - assert d_status.active_policy == log.DriverMonitoringState.MonitoringPolicy.wheeltouch - assert d_status.lockout_active - assert d_status.lockout_count >= 1 - # 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.preDriverUnresponsive + 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.promptDriverUnresponsive + assert events[int((d_status.settings._AWARENESS_TIME + \ + ((TEST_TIMESPAN-10-d_status.settings._AWARENESS_TIME)/2))/DT_DMON)].names[0] == EventName.driverUnresponsive # 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 @@ -117,13 +106,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.promptDriverDistracted + 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.promptDriverDistracted + assert events[int((DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)].names[0] == EventName.promptDriverDistracted + 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 @@ -141,31 +130,31 @@ 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.promptDriverDistracted + assert events[int((DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)].names[0] == EventName.driverDistracted + assert events[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)].names[0] == EventName.driverDistracted + 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 def test_sometimes_transparent_commuter(self): - for _visible_time in (0.5, 10): - ds_vector = always_no_face[:]*2 - interaction_vector = always_false[:]*2 - 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(dm_settings._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT/2/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 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)] == 2 - 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 + _visible_time = np.random.choice([0.5, 10]) + ds_vector = always_no_face[:]*2 + interaction_vector = always_false[:]*2 + 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.promptDriverUnresponsive + assert len(events[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.promptDriverUnresponsive + assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)].names[0] == EventName.preDriverUnresponsive + elif _visible_time == 10: + assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0] == EventName.promptDriverUnresponsive + 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 @@ -177,51 +166,105 @@ 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(dm_settings._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT/2/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.promptDriverUnresponsive + assert events[int((INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)].names[0] == EventName.driverUnresponsive + assert events[int((INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)].names[0] == EventName.driverUnresponsive + assert events[int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)].names[0] == EventName.driverUnresponsive + 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 def test_long_traffic_light_victim(self): _redlight_time = 60 # seconds - lowspeed_vector = always_true[:] - lowspeed_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, lowspeed_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 low speed - def test_distracted_then_stops(self): - _stop_time = DISTRACTED_SECONDS_TO_ORANGE + 1 # stop 1 second after reaching orange - lowspeed_vector = always_false[:] - lowspeed_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, lowspeed_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 + 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 events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL+1)/DT_DMON)].names[0] == \ + EventName.preDriverDistracted + assert events[int((_redlight_time-0.1)/DT_DMON)].names[0] == EventName.preDriverDistracted + assert events[int((_redlight_time+0.5)/DT_DMON)].names[0] == EventName.promptDriverDistracted # 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.preDriverUnresponsive in \ + events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME-0.1)/DT_DMON)].names + assert EventName.promptDriverUnresponsive in \ + events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names + assert EventName.driverUnresponsive in \ + events[int((INVISIBLE_SECONDS_TO_RED-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names + + +@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. AOL) + (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}" + diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index a76a620..1085e11 100755 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -79,13 +79,6 @@ def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 0.4) -def too_distracted_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - if sm['driverMonitoringState'].lockout: - mins_left = sm['driverMonitoringState'].lockoutMinutesRemaining - return NoEntryAlert("Too Distracted", f"{mins_left} minute{'s' if mins_left != 1 else ''} Left", priority=Priority.HIGH) - return NoEntryAlert("Pay Attention to Engage", priority=Priority.HIGH) - - def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: first_word = 'Recalibrating' if sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.recalibrating else 'Calibrating' return Alert( @@ -367,15 +360,15 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { Priority.LOW, VisualAlert.steerRequired, AudibleAlert.prompt, 1.8), }, - EventName.driverDistracted1: { + EventName.preDriverDistracted: { ET.PERMANENT: Alert( "Pay Attention", "", AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.preAlert, .1), + Priority.LOW, VisualAlert.none, AudibleAlert.none, .1), }, - EventName.driverDistracted2: { + EventName.promptDriverDistracted: { ET.PERMANENT: Alert( "Pay Attention", "Driver Distracted", @@ -383,7 +376,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1), }, - EventName.driverDistracted3: { + EventName.driverDistracted: { ET.PERMANENT: Alert( "DISENGAGE IMMEDIATELY", "Driver Distracted", @@ -391,7 +384,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.warningImmediate, .1), }, - EventName.driverUnresponsive1: { + EventName.preDriverUnresponsive: { ET.PERMANENT: Alert( "Touch Steering Wheel: No Face Detected", "", @@ -399,7 +392,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .1), }, - EventName.driverUnresponsive2: { + EventName.promptDriverUnresponsive: { ET.PERMANENT: Alert( "Touch Steering Wheel", "Driver Unresponsive", @@ -407,7 +400,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1), }, - EventName.driverUnresponsive3: { + EventName.driverUnresponsive: { ET.PERMANENT: Alert( "DISENGAGE IMMEDIATELY", "Driver Unresponsive", @@ -639,7 +632,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { }, EventName.tooDistracted: { - ET.NO_ENTRY: too_distracted_alert, + ET.NO_ENTRY: NoEntryAlert("Distraction Level Too High"), }, EventName.excessiveActuation: { @@ -885,14 +878,14 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { if HARDWARE.get_device_type() == 'mici': EVENTS.update({ - EventName.driverDistracted1: { + EventName.preDriverDistracted: { ET.PERMANENT: Alert( "Pay Attention", "", AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.preAlert, 2), + Priority.LOW, VisualAlert.none, AudibleAlert.none, 2), }, - EventName.driverDistracted2: { + EventName.promptDriverDistracted: { ET.PERMANENT: Alert( "Pay Attention", "Driver Distracted", diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 15da7cc..285d46f 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -54,8 +54,6 @@ EventName = log.OnroadEvent.EventName ButtonType = car.CarState.ButtonEvent.Type SafetyModel = car.CarParams.SafetyModel TurnDirection = custom.IQTurnSignalDirection -AlertLevel = log.DriverMonitoringState.AlertLevel -MonitoringPolicy = log.DriverMonitoringState.MonitoringPolicy IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) @@ -149,7 +147,6 @@ class SelfdriveD(GapButtonActions): self.events_prev = [] self.logged_comm_issue = None self.not_running_prev = None - self.dm_lockout_set = False self.experimental_mode = False self.personality = get_sanitize_int_param( "LongitudinalPersonality", @@ -186,6 +183,7 @@ class SelfdriveD(GapButtonActions): self.events_iq = IQEvents() self.events_iq_prev = [] + self._cached_dm_event_names: tuple[int, ...] = () self._cached_plan_event_names: tuple[int, ...] = () self._cached_model_event_names: tuple[int, ...] = () self._cached_nav_event_names: tuple[int, ...] = () @@ -207,6 +205,10 @@ class SelfdriveD(GapButtonActions): if self.sm.updated['iqPlan']: self._cached_plan_event_names = tuple(event.name.raw for event in self._get_longitudinal_plan_ext().events) + def _refresh_cached_dm_events(self) -> None: + if self.sm.updated['driverMonitoringState']: + self._cached_dm_event_names = tuple(event.name.raw for event in self.sm['driverMonitoringState'].events) + def _refresh_cached_model_events(self) -> None: if not self.sm.updated['iqDriveModelData']: return @@ -295,24 +297,8 @@ class SelfdriveD(GapButtonActions): self.events.add(EventName.resumeBlocked) if not self.CP.notCar: - if self.sm['driverMonitoringState'].lockout and not self.dm_lockout_set: - self.params.put_bool("DriverTooDistracted", True) - self.dm_lockout_set = True - elif not self.sm['driverMonitoringState'].lockout and self.dm_lockout_set: - self.params.remove("DriverTooDistracted") - self.dm_lockout_set = False - - if self.sm['driverMonitoringState'].lockout or self.sm['driverMonitoringState'].alwaysOnLockout: - self.events.add(EventName.tooDistracted) - - vision_dm = self.sm['driverMonitoringState'].activePolicy == MonitoringPolicy.vision - if self.sm['driverMonitoringState'].alertLevel == AlertLevel.one: - self.events.add(EventName.driverDistracted1 if vision_dm else EventName.driverUnresponsive1) - elif self.sm['driverMonitoringState'].alertLevel == AlertLevel.two: - self.events.add(EventName.driverDistracted2 if vision_dm else EventName.driverUnresponsive2) - elif self.sm['driverMonitoringState'].alertLevel == AlertLevel.three: - self.events.add(EventName.driverDistracted3 if vision_dm else EventName.driverUnresponsive3) - + self._refresh_cached_dm_events() + self._add_event_names(self._cached_dm_event_names) self._refresh_cached_plan_events() self._add_iq_event_names(self._cached_plan_event_names) diff --git a/selfdrive/selfdrived/tests/test_driver_monitoring.py b/selfdrive/selfdrived/tests/test_driver_monitoring.py deleted file mode 100644 index 3d47a9b..0000000 --- a/selfdrive/selfdrived/tests/test_driver_monitoring.py +++ /dev/null @@ -1,32 +0,0 @@ -from types import SimpleNamespace - -from cereal import car, log - -from openpilot.selfdrive.selfdrived.events import EVENTS, ET - - -EventName = log.OnroadEvent.EventName -AudibleAlert = car.CarControl.HUDControl.AudibleAlert - - -def test_driver_monitoring_alert_stages(): - expected_sounds = { - EventName.driverDistracted1: AudibleAlert.preAlert, - EventName.driverDistracted2: AudibleAlert.promptDistracted, - EventName.driverDistracted3: AudibleAlert.warningImmediate, - EventName.driverUnresponsive1: AudibleAlert.none, - EventName.driverUnresponsive2: AudibleAlert.promptDistracted, - EventName.driverUnresponsive3: AudibleAlert.warningImmediate, - } - - for event_name, audible_alert in expected_sounds.items(): - assert EVENTS[event_name][ET.PERMANENT].audible_alert == audible_alert - - -def test_driver_monitoring_lockout_alert(): - callback = EVENTS[EventName.tooDistracted][ET.NO_ENTRY] - sm = {'driverMonitoringState': SimpleNamespace(lockout=True, lockoutMinutesRemaining=5)} - alert = callback(None, None, sm, False, 0, None) - - assert alert.alert_text_1 == "5 minutes Left" - assert alert.alert_text_2 == "Too Distracted" diff --git a/selfdrive/test/process_replay/migration.py b/selfdrive/test/process_replay/migration.py index b135a68..010ce3f 100644 --- a/selfdrive/test/process_replay/migration.py +++ b/selfdrive/test/process_replay/migration.py @@ -455,46 +455,21 @@ def migrate_onroadEvents(msgs): return ops, [], [] -@migration(inputs=["driverMonitoringStateDEPRECATED"]) +@migration(inputs=["driverMonitoringState"]) def migrate_driverMonitoringState(msgs): ops = [] for index, msg in msgs: - old = msg.driverMonitoringStateDEPRECATED - new_msg = messaging.new_message('driverMonitoringState', valid=msg.valid, logMonoTime=msg.logMonoTime) - dm = new_msg.driverMonitoringState - dm.isRHD = old.isRHD - dm.activePolicy = log.DriverMonitoringState.MonitoringPolicy.vision if old.isActiveMode else \ - log.DriverMonitoringState.MonitoringPolicy.wheeltouch + msg = msg.as_builder() + events = [] + for event in msg.driverMonitoringState.eventsDEPRECATED: + try: + if not str(event.name).endswith('DEPRECATED'): + # dict converts name enum into string representation + events.append(log.OnroadEvent(**event.to_dict())) + except RuntimeError: # Member was null + traceback.print_exc() - AlertLevel = log.DriverMonitoringState.AlertLevel - event_to_alert_level = { - 'driverDistracted1': AlertLevel.one, 'driverUnresponsive1': AlertLevel.one, - 'driverDistracted2': AlertLevel.two, 'driverUnresponsive2': AlertLevel.two, - 'driverDistracted3': AlertLevel.three, 'driverUnresponsive3': AlertLevel.three, - } - for event in old.events: - level = event_to_alert_level.get(str(event.name)) - if level is not None: - dm.alertLevel = level - break - dm.lockout = any(str(event.name) == 'tooDistracted' for event in old.events) - - dm.visionPolicyState.awarenessPercent = int(max(0, min(100, (old.awarenessStatus if old.isActiveMode else old.awarenessActive) * 100))) - dm.visionPolicyState.awarenessStep = old.stepChange if old.isActiveMode else 0. - dm.visionPolicyState.isDistracted = old.isDistracted - dm.visionPolicyState.distractedTypes.pose = bool(old.distractedType & 1) - dm.visionPolicyState.distractedTypes.eye = bool(old.distractedType & 2) - dm.visionPolicyState.distractedTypes.phone = bool(old.distractedType & 4) - dm.visionPolicyState.faceDetected = old.faceDetected - dm.visionPolicyState.pose.pitchCalib.offset = old.posePitchOffset - dm.visionPolicyState.pose.pitchCalib.calibratedPercent = int(min(100, old.posePitchValidCount / 1200 * 100)) - dm.visionPolicyState.pose.yawCalib.offset = old.poseYawOffset - dm.visionPolicyState.pose.yawCalib.calibratedPercent = int(min(100, old.poseYawValidCount / 1200 * 100)) - dm.visionPolicyState.pose.calibrated = old.posePitchValidCount >= 1200 and old.poseYawValidCount >= 1200 - dm.visionPolicyState.wheeltouchFallbackPercent = int(min(100, old.hiStdCount / 200 * 100)) - dm.visionPolicyState.uncertainOffroadAlertPercent = int(min(100, old.uncertainCount / 1200 * 100)) - dm.wheeltouchPolicyState.awarenessPercent = int(max(0, min(100, (old.awarenessPassive if old.isActiveMode else old.awarenessStatus) * 100))) - dm.wheeltouchPolicyState.awarenessStep = 0. if old.isActiveMode else old.stepChange - ops.append((index, new_msg.as_reader())) + msg.driverMonitoringState.events = events + ops.append((index, msg.as_reader())) return ops, [], [] diff --git a/selfdrive/ui/mici/layouts/onboarding.py b/selfdrive/ui/mici/layouts/onboarding.py index 8efcdd9..11a8154 100644 --- a/selfdrive/ui/mici/layouts/onboarding.py +++ b/selfdrive/ui/mici/layouts/onboarding.py @@ -200,7 +200,7 @@ class TrainingGuideDMTutorial(Widget): looking_center = False # stay at 100% once reached - if (dm_state.visionPolicyState.faceDetected and looking_center) or self._progress.x > 0.99: + if (dm_state.faceDetected and looking_center) or self._progress.x > 0.99: slow = self._progress.x < 0.25 duration = self.PROGRESS_DURATION * 2 if slow else self.PROGRESS_DURATION self._progress.x += 1.0 / (duration * gui_app.target_fps) diff --git a/selfdrive/ui/mici/onroad/driver_camera_dialog.py b/selfdrive/ui/mici/onroad/driver_camera_dialog.py index 1ad90a2..68ff785 100644 --- a/selfdrive/ui/mici/onroad/driver_camera_dialog.py +++ b/selfdrive/ui/mici/onroad/driver_camera_dialog.py @@ -1,14 +1,20 @@ import pyray as rl -from cereal import car, log, messaging +from cereal import log, messaging from msgq.visionipc import VisionStreamType from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView from openpilot.selfdrive.ui.mici.onroad.driver_state import DriverStateRenderer from openpilot.selfdrive.ui.ui_state import ui_state, device +from openpilot.selfdrive.selfdrived.events import EVENTS, ET from openpilot.system.ui.lib.application import gui_app, FontWeight from openpilot.system.ui.lib.multilang import tr from openpilot.system.ui.widgets.nav_widget import NavWidget from openpilot.system.ui.widgets.label import gui_label +EventName = log.OnroadEvent.EventName + +EVENT_TO_INT = EventName.schema.enumerants + + class DriverCameraView(CameraView): def _calc_frame_matrix(self, rect: rl.Rectangle): base = super()._calc_frame_matrix(rect) @@ -110,14 +116,10 @@ class DriverCameraDialog(NavWidget): return msg = messaging.new_message('selfdriveState') - if dm_state is not None: - AudibleAlert = car.CarControl.HUDControl.AudibleAlert - alert_sounds = { - 'one': AudibleAlert.preAlert, - 'two': AudibleAlert.promptDistracted, - 'three': AudibleAlert.warningImmediate, - } - msg.selfdriveState.alertSound = alert_sounds.get(str(dm_state.alertLevel), AudibleAlert.none) + if dm_state is not None and len(dm_state.events): + event_name = EVENT_TO_INT[dm_state.events[0].name] + if event_name is not None and event_name in EVENTS and ET.PERMANENT in EVENTS[event_name]: + msg.selfdriveState.alertSound = EVENTS[event_name][ET.PERMANENT].audible_alert self._pm.send('selfdriveState', msg) def _render_dm_alerts(self, rect: rl.Rectangle): @@ -125,30 +127,29 @@ class DriverCameraDialog(NavWidget): dm_state = ui_state.sm["driverMonitoringState"] self._publish_alert_sound(dm_state) - is_vision = dm_state.activePolicy == log.DriverMonitoringState.MonitoringPolicy.vision - awareness_pct = dm_state.visionPolicyState.awarenessPercent if is_vision else dm_state.wheeltouchPolicyState.awarenessPercent gui_label(rl.Rectangle(rect.x + 2, rect.y + 2, rect.width, rect.height), - f"Awareness: {awareness_pct:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM, + f"Awareness: {dm_state.awarenessStatus * 100:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM, alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT, alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP, color=rl.Color(0, 0, 0, 180)) - gui_label(rect, f"Awareness: {awareness_pct:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM, + gui_label(rect, f"Awareness: {dm_state.awarenessStatus * 100:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM, alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT, alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP, color=rl.Color(255, 255, 255, int(255 * 0.9))) - if dm_state.alertLevel == log.DriverMonitoringState.AlertLevel.none: + if not dm_state.events: return - alert_level_str = f"{'Pay Attention' if is_vision else 'Touch Wheel'} - level {dm_state.alertLevel}" + # Show first event (only one should be active at a time) + event_name_str = str(dm_state.events[0].name).split('.')[-1] alignment = rl.GuiTextAlignment.TEXT_ALIGN_RIGHT if self.driver_state_renderer.is_rhd else rl.GuiTextAlignment.TEXT_ALIGN_LEFT shadow_rect = rl.Rectangle(rect.x + 2, rect.y + 2, rect.width, rect.height) - gui_label(shadow_rect, alert_level_str, font_size=40, font_weight=FontWeight.BOLD, + gui_label(shadow_rect, event_name_str, font_size=40, font_weight=FontWeight.BOLD, alignment=alignment, alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM, color=rl.Color(0, 0, 0, 180)) - gui_label(rect, alert_level_str, font_size=40, font_weight=FontWeight.BOLD, + gui_label(rect, event_name_str, font_size=40, font_weight=FontWeight.BOLD, alignment=alignment, alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM, color=rl.Color(255, 255, 255, int(255 * 0.9))) @@ -165,7 +166,7 @@ class DriverCameraDialog(NavWidget): def _draw_face_detection(self, rect: rl.Rectangle): dm_state = ui_state.sm["driverMonitoringState"] driver_data = self.driver_state_renderer.get_driver_data() - if not dm_state.visionPolicyState.faceDetected: + if not dm_state.faceDetected: return # Get face position and orientation diff --git a/selfdrive/ui/mici/onroad/driver_state.py b/selfdrive/ui/mici/onroad/driver_state.py index bd2e694..8a20137 100644 --- a/selfdrive/ui/mici/onroad/driver_state.py +++ b/selfdrive/ui/mici/onroad/driver_state.py @@ -6,12 +6,12 @@ from openpilot.common.filter_simple import FirstOrderFilter from openpilot.system.ui.lib.application import gui_app from openpilot.system.ui.widgets import Widget from openpilot.selfdrive.ui.ui_state import ui_state +from openpilot.selfdrive.monitoring.helpers import face_orientation_from_net AlertSize = log.SelfdriveState.AlertSize DEBUG = False ACTIVE_ACCENT = rl.Color(0x0C, 0x94, 0x96, 0xFF) -CONE_COLOR_ORANGE = (255, 115, 0) LOOKING_CENTER_THRESHOLD_UPPER = math.radians(6) LOOKING_CENTER_THRESHOLD_LOWER = math.radians(3) @@ -21,7 +21,6 @@ class DriverStateRenderer(Widget): BASE_SIZE = 60 LINES_ANGLE_INCREMENT = 5 LINES_STALE_ANGLES = 3.0 # seconds - AWARENESS_UNFULL_PERCENT = 95 def __init__(self, lines: bool = False, inset: bool = False): super().__init__() @@ -36,15 +35,11 @@ class DriverStateRenderer(Widget): self._is_active = False self._is_rhd = False self._face_detected = False - self._face_pitch = 0. - self._face_yaw = 0. self._should_draw = False self._force_active = False self._looking_center = False - self._awareness_unfull = False self._fade_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps) - self._color_fade_filter = FirstOrderFilter(1.0, 0.05, 1 / gui_app.target_fps) self._pitch_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps, initialized=False) self._yaw_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps, initialized=False) self._rotation_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps, initialized=False) @@ -100,7 +95,6 @@ class DriverStateRenderer(Widget): rl.Color(255, 255, 255, int(255 * 0.9 * self._fade_filter.x))) if self.effective_active: - active_amount = self._color_fade_filter.update(0.0 if self._awareness_unfull else 1.0) source_rect = rl.Rectangle(0, 0, self._dm_cone.width, self._dm_cone.height) dest_rect = rl.Rectangle( self._rect.x + self._rect.width / 2, @@ -110,16 +104,13 @@ class DriverStateRenderer(Widget): ) if not self._lines: - r = int(round(ACTIVE_ACCENT.r * active_amount + CONE_COLOR_ORANGE[0] * (1 - active_amount))) - g = int(round(ACTIVE_ACCENT.g * active_amount + CONE_COLOR_ORANGE[1] * (1 - active_amount))) - b = int(round(ACTIVE_ACCENT.b * active_amount + CONE_COLOR_ORANGE[2] * (1 - active_amount))) rl.draw_texture_pro( self._dm_cone, source_rect, dest_rect, rl.Vector2(dest_rect.width / 2, dest_rect.height / 2), self._rotation_filter.x - 90, - rl.Color(r, g, b, int(255 * self._fade_filter.x)), + rl.Color(ACTIVE_ACCENT.r, ACTIVE_ACCENT.g, ACTIVE_ACCENT.b, int(255 * self._fade_filter.x)), ) else: @@ -159,12 +150,9 @@ class DriverStateRenderer(Widget): sm = ui_state.sm dm_state = sm["driverMonitoringState"] - self._is_active = dm_state.activePolicy == log.DriverMonitoringState.MonitoringPolicy.vision + self._is_active = dm_state.isActiveMode self._is_rhd = dm_state.isRHD - self._face_detected = dm_state.visionPolicyState.faceDetected - self._awareness_unfull = self.effective_active and dm_state.visionPolicyState.awarenessPercent < self.AWARENESS_UNFULL_PERCENT - self._face_pitch = dm_state.visionPolicyState.pose.pitch + math.radians(6) - self._face_yaw = -dm_state.visionPolicyState.pose.yaw + self._face_detected = dm_state.faceDetected driverstate = sm["driverStateV2"] driver_data = driverstate.rightDriverData if self._is_rhd else driverstate.leftDriverData @@ -172,9 +160,24 @@ class DriverStateRenderer(Widget): def _update_state(self): # Get monitoring state - _ = self.get_driver_data() - pitch = self._pitch_filter.update(self._face_pitch) - yaw = self._yaw_filter.update(self._face_yaw) + driver_data = self.get_driver_data() + driver_orient = driver_data.faceOrientation + + if len(driver_orient) != 3: + return + + # Calibrate orientation so looking straight ahead at the road (instead of at the device) reads + # (0, 0), using live calibration. Makes the cone point in the correct direction. (stock PR #37149) + sm = ui_state.sm + if sm.valid['liveCalibration'] and len(sm['liveCalibration'].rpyCalib) == 3: + cal_rpy = sm['liveCalibration'].rpyCalib + else: + cal_rpy = [0.0, 0.0, 0.0] + _, pitch, yaw = face_orientation_from_net(driver_orient, driver_data.facePosition, cal_rpy) + yaw = -yaw # undo sign flip in face_orientation_from_net to match UI convention + + pitch = self._pitch_filter.update(pitch) + yaw = self._yaw_filter.update(yaw) # hysteresis on looking center if abs(pitch) < LOOKING_CENTER_THRESHOLD_LOWER and abs(yaw) < LOOKING_CENTER_THRESHOLD_LOWER: @@ -195,8 +198,9 @@ class DriverStateRenderer(Widget): rl.draw_circle(int(pitch_x), 100, 5, rl.GREEN) rl.draw_circle(int(yaw_x), 120, 5, rl.GREEN) - # filter head rotation, handling wrap-around - rotation = math.degrees(math.atan2(pitch * 2, yaw)) + # filter head rotation, handling wrap-around (bias pitch up since calib/DM pose isn't exact, + # and halve yaw sensitivity) + rotation = math.degrees(math.atan2((pitch + math.radians(6)) * 2, yaw)) angle_diff = rotation - self._rotation_filter.x angle_diff = ((angle_diff + 180) % 360) - 180 self._rotation_filter.update(self._rotation_filter.x + angle_diff) diff --git a/selfdrive/ui/onroad/driver_state.py b/selfdrive/ui/onroad/driver_state.py index e34b2c1..39aa6d1 100644 --- a/selfdrive/ui/onroad/driver_state.py +++ b/selfdrive/ui/onroad/driver_state.py @@ -114,7 +114,7 @@ class DriverStateRenderer(Widget): # Get monitoring state dm_state = sm["driverMonitoringState"] - self.is_active = dm_state.activePolicy == log.DriverMonitoringState.MonitoringPolicy.vision + self.is_active = dm_state.isActiveMode self.is_rhd = dm_state.isRHD # Update fade state (smoother transition between active/inactive) diff --git a/selfdrive/ui/soundd.py b/selfdrive/ui/soundd.py index 9d92e80..e956e73 100644 --- a/selfdrive/ui/soundd.py +++ b/selfdrive/ui/soundd.py @@ -45,28 +45,26 @@ sound_list_iq: dict[int, tuple[str, int | None, float]] = { AudibleAlertIQ.promptSingleHigh: ("prompt_single_high.wav", 1, MAX_VOLUME), } -def get_sound_list(device_type: str) -> dict[int, tuple[str, int | None, float]]: - sounds = { +sound_list: dict[int, tuple[str, int | None, float]] = { + # AudibleAlert, file name, play count (none for infinite) + AudibleAlert.engage: ("engage.wav", 1, MAX_VOLUME), + AudibleAlert.disengage: ("disengage.wav", 1, MAX_VOLUME), + AudibleAlert.refuse: ("refuse.wav", 1, MAX_VOLUME), + + AudibleAlert.prompt: ("prompt.wav", 1, MAX_VOLUME), + AudibleAlert.promptRepeat: ("prompt.wav", None, MAX_VOLUME), + AudibleAlert.promptDistracted: ("prompt_distracted.wav", None, MAX_VOLUME), + + AudibleAlert.warningSoft: ("warning_soft.wav", None, MAX_VOLUME), + AudibleAlert.warningImmediate: ("warning_immediate.wav", None, MAX_VOLUME), + + **sound_list_iq, +} +if HARDWARE.get_device_type() in ("tizi", "tici"): + sound_list.update({ AudibleAlert.engage: ("engage.wav", 1, MAX_VOLUME), AudibleAlert.disengage: ("disengage.wav", 1, MAX_VOLUME), - AudibleAlert.refuse: ("refuse.wav", 1, MAX_VOLUME), - AudibleAlert.prompt: ("prompt.wav", 1, MAX_VOLUME), - AudibleAlert.promptRepeat: ("prompt.wav", None, MAX_VOLUME), - AudibleAlert.promptDistracted: ("prompt_distracted.wav", None, MAX_VOLUME), - AudibleAlert.preAlert: ("pre_alert.wav", 1, MAX_VOLUME), - AudibleAlert.warningSoft: ("warning_soft.wav", None, MAX_VOLUME), - AudibleAlert.warningImmediate: ("warning_immediate.wav", None, MAX_VOLUME), - **sound_list_iq, - } - if device_type in ("tizi", "tici"): - sounds.update({ - AudibleAlert.engage: ("engage.wav", 1, MAX_VOLUME), - AudibleAlert.disengage: ("disengage.wav", 1, MAX_VOLUME), - }) - return sounds - - -sound_list = get_sound_list(HARDWARE.get_device_type()) + }) def check_selfdrive_timeout_alert(sm): ss_missing = time.monotonic() - sm.recv_time['selfdriveState'] diff --git a/selfdrive/ui/tests/test_soundd.py b/selfdrive/ui/tests/test_soundd.py index a1e18e8..a9da845 100644 --- a/selfdrive/ui/tests/test_soundd.py +++ b/selfdrive/ui/tests/test_soundd.py @@ -2,19 +2,13 @@ from cereal import car from cereal import messaging from cereal.messaging import SubMaster, PubMaster from openpilot.selfdrive.ui.soundd import SELFDRIVE_STATE_TIMEOUT, check_selfdrive_timeout_alert -from openpilot.selfdrive.ui.soundd import get_sound_list -import pytest import time AudibleAlert = car.CarControl.HUDControl.AudibleAlert class TestSoundd: - @pytest.mark.parametrize("device_type", ["mici", "tici", "tizi"]) - def test_prompt_distracted_sound(self, device_type): - assert get_sound_list(device_type)[AudibleAlert.promptDistracted][0] == "prompt_distracted.wav" - def test_check_selfdrive_timeout_alert(self): sm = SubMaster(['selfdriveState']) pm = PubMaster(['selfdriveState']) @@ -38,3 +32,4 @@ class TestSoundd: assert check_selfdrive_timeout_alert(sm) # TODO: add test with micd for checking that soundd actually outputs sounds + diff --git a/system/loggerd/loggerd.h b/system/loggerd/loggerd.h index 74cd043..e8c8c04 100644 --- a/system/loggerd/loggerd.h +++ b/system/loggerd/loggerd.h @@ -28,6 +28,13 @@ const int SEGMENT_LENGTH = LOGGERD_TEST ? atoi(getenv("LOGGERD_SEGMENT_LENGTH")) constexpr char PRESERVE_ATTR_NAME[] = "user.preserve"; constexpr char PRESERVE_ATTR_VALUE = '1'; +// 2.5x the stock 526x330 qcamera, rounded up to even. The msm_vidc encoder rejects +// VIDIOC_S_FMT with ENOTSUPP (524) on an odd width or height, which throws out of +// encoder_thread and SIGABRTs all of encoderd -- taking fcamera/dcamera/ecamera with it. +constexpr int QCAM_WIDTH = 1316; +constexpr int QCAM_HEIGHT = 826; +static_assert(QCAM_WIDTH % 2 == 0 && QCAM_HEIGHT % 2 == 0, "qcamera dimensions must be even"); + struct EncoderSettings { cereal::EncodeIndex::Type encode_type; int bitrate; @@ -140,8 +147,8 @@ const EncoderInfo qcam_encoder_info = { .filename = "qcamera.ts", .cbr = true, // enforce the bitrate so upload size stays predictable (no VBR overshoot) .get_settings = [](int){return EncoderSettings::QcamEncoderSettings();}, - .frame_width = 1315, // 2.5x the stock 526x330, same road-cam aspect ratio - .frame_height = 825, + .frame_width = QCAM_WIDTH, + .frame_height = QCAM_HEIGHT, .include_audio = Params().getBool("RecordAudio"), INIT_ENCODE_FUNCTIONS(QRoadEncode), }; diff --git a/tools/sim/lib/simulated_sensors.py b/tools/sim/lib/simulated_sensors.py index 6ac7c4f..a8374a0 100644 --- a/tools/sim/lib/simulated_sensors.py +++ b/tools/sim/lib/simulated_sensors.py @@ -92,12 +92,11 @@ class SimulatedSensors: # dmonitoringd output dat = messaging.new_message('driverMonitoringState', valid=True) - dm = dat.driverMonitoringState - dm.alertLevel = log.DriverMonitoringState.AlertLevel.none - dm.activePolicy = log.DriverMonitoringState.MonitoringPolicy.vision - dm.visionPolicyState.faceDetected = True - dm.visionPolicyState.isDistracted = False - dm.visionPolicyState.awarenessPercent = 100 + dat.driverMonitoringState = { + "faceDetected": True, + "isDistracted": False, + "awarenessStatus": 1., + } self.pm.send('driverMonitoringState', dat) def send_camera_images(self, world: 'World'):