From 204219695f57f6ba76347df4df99e00a28b51e9f Mon Sep 17 00:00:00 2001 From: ZwX1616 Date: Thu, 23 May 2024 20:14:28 -0700 Subject: [PATCH] dmonitoringd: simplify main loop (#32517) * one call does all * no need * update etst * filename * dbf5b05ff480145a79b5941e360d0698b70979cd --- release/files_common | 2 +- selfdrive/monitoring/dmonitoringd.py | 66 ++----- .../{driver_monitor.py => helpers.py} | 171 ++++++++++++------ selfdrive/monitoring/test_monitoring.py | 15 +- selfdrive/test/process_replay/ref_commit | 2 +- 5 files changed, 133 insertions(+), 123 deletions(-) rename selfdrive/monitoring/{driver_monitor.py => helpers.py} (82%) diff --git a/release/files_common b/release/files_common index 8636959da5..f6afd9390e 100644 --- a/release/files_common +++ b/release/files_common @@ -357,7 +357,7 @@ selfdrive/modeld/runners/*.h selfdrive/modeld/runners/*.py selfdrive/monitoring/dmonitoringd.py -selfdrive/monitoring/driver_monitor.py +selfdrive/monitoring/helpers.py selfdrive/navd/.gitignore selfdrive/navd/__init__.py diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index 91a4932584..e572bf5bd9 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -2,11 +2,9 @@ import gc import cereal.messaging as messaging -from cereal import car from openpilot.common.params import Params from openpilot.common.realtime import set_realtime_priority -from openpilot.selfdrive.controls.lib.events import Events -from openpilot.selfdrive.monitoring.driver_monitor import DriverStatus +from openpilot.selfdrive.monitoring.helpers import DriverMonitoring def dmonitoringd_thread(): @@ -17,70 +15,30 @@ def dmonitoringd_thread(): pm = messaging.PubMaster(['driverMonitoringState']) sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll='driverStateV2') - driver_status = DriverStatus(rhd_saved=params.get_bool("IsRhdDetected"), always_on=params.get_bool("AlwaysOnDM")) - - driver_engaged = False + DM = DriverMonitoring(rhd_saved=params.get_bool("IsRhdDetected"), always_on=params.get_bool("AlwaysOnDM")) # 20Hz <- dmonitoringmodeld while True: sm.update() - if not sm.updated['driverStateV2']: + if (not sm.updated['driverStateV2']) or (not sm.all_checks()): + # iterate when model has new output continue - # Get interaction - if sm.updated['carState']: - driver_engaged = sm['carState'].steeringPressed or \ - sm['carState'].gasPressed - - if sm.updated['modelV2']: - driver_status.set_policy(sm['modelV2'], sm['carState'].vEgo) - - # Get data from dmonitoringmodeld - events = Events() - - if sm.all_checks() and len(sm['liveCalibration'].rpyCalib): - driver_status.update_states(sm['driverStateV2'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled) - - # Block engaging after max number of distrations or when alert active - if driver_status.terminal_alert_cnt >= driver_status.settings._MAX_TERMINAL_ALERTS or \ - driver_status.terminal_time >= driver_status.settings._MAX_TERMINAL_DURATION or \ - driver_status.always_on and driver_status.awareness <= driver_status.threshold_prompt: - events.add(car.CarEvent.EventName.tooDistracted) - - # Update events from driver state - driver_status.update_events(events, driver_engaged, sm['controlsState'].enabled, - sm['carState'].standstill, sm['carState'].gearShifter in [car.CarState.GearShifter.reverse, car.CarState.GearShifter.park], sm['carState'].vEgo) + DM.run_step(sm) - # build driverMonitoringState packet - dat = messaging.new_message('driverMonitoringState', valid=sm.all_checks()) - dat.driverMonitoringState = { - "events": events.to_msg(), - "faceDetected": driver_status.face_detected, - "isDistracted": driver_status.driver_distracted, - "distractedType": sum(driver_status.distracted_types), - "awarenessStatus": driver_status.awareness, - "posePitchOffset": driver_status.pose.pitch_offseter.filtered_stat.mean(), - "posePitchValidCount": driver_status.pose.pitch_offseter.filtered_stat.n, - "poseYawOffset": driver_status.pose.yaw_offseter.filtered_stat.mean(), - "poseYawValidCount": driver_status.pose.yaw_offseter.filtered_stat.n, - "stepChange": driver_status.step_change, - "awarenessActive": driver_status.awareness_active, - "awarenessPassive": driver_status.awareness_passive, - "isLowStd": driver_status.pose.low_std, - "hiStdCount": driver_status.hi_stds, - "isActiveMode": driver_status.active_monitoring_mode, - "isRHD": driver_status.wheel_on_right, - } + # publish + dat = DM.get_state_packet() pm.send('driverMonitoringState', dat) + # load live always-on toggle if sm['driverStateV2'].frameId % 40 == 1: - driver_status.always_on = params.get_bool("AlwaysOnDM") + DM.always_on = params.get_bool("AlwaysOnDM") # save rhd virtual toggle every 5 mins if (sm['driverStateV2'].frameId % 6000 == 0 and - driver_status.wheelpos_learner.filtered_stat.n > driver_status.settings._WHEELPOS_FILTER_MIN_COUNT and - driver_status.wheel_on_right == (driver_status.wheelpos_learner.filtered_stat.M > driver_status.settings._WHEELPOS_THRESHOLD)): - params.put_bool_nonblocking("IsRhdDetected", driver_status.wheel_on_right) + DM.wheelpos_learner.filtered_stat.n > DM.settings._WHEELPOS_FILTER_MIN_COUNT and + DM.wheel_on_right == (DM.wheelpos_learner.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/driver_monitor.py b/selfdrive/monitoring/helpers.py similarity index 82% rename from selfdrive/monitoring/driver_monitor.py rename to selfdrive/monitoring/helpers.py index 85cc19eb8b..347cb245d1 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/selfdrive/monitoring/helpers.py @@ -1,6 +1,8 @@ from math import atan2 from cereal import car +import cereal.messaging as messaging +from openpilot.selfdrive.controls.lib.events import Events from openpilot.common.numpy_fast import interp from openpilot.common.realtime import DT_DMON from openpilot.common.filter_simple import FirstOrderFilter @@ -71,19 +73,38 @@ class DRIVER_MONITOR_SETTINGS: 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_E2E = 1 << 2 + +class DriverPose: + def __init__(self, max_trackable): + self.yaw = 0. + self.pitch = 0. + self.roll = 0. + self.yaw_std = 0. + self.pitch_std = 0. + self.roll_std = 0. + self.pitch_offseter = RunningStatFilter(max_trackable=max_trackable) + self.yaw_offseter = RunningStatFilter(max_trackable=max_trackable) + self.calibrated = False + self.low_std = True + self.cfactor_pitch = 1. + self.cfactor_yaw = 1. + +class DriverBlink: + def __init__(self): + self.left = 0. + self.right = 0. + -# TODO: get these live # 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 -class DistractedType: - NOT_DISTRACTED = 0 - DISTRACTED_POSE = 1 - DISTRACTED_BLINK = 2 - DISTRACTED_E2E = 4 - 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 @@ -102,26 +123,8 @@ def face_orientation_from_net(angles_desc, pos_desc, rpy_calib): yaw -= rpy_calib[2] return roll_net, pitch, yaw -class DriverPose: - def __init__(self, max_trackable): - self.yaw = 0. - self.pitch = 0. - self.roll = 0. - self.yaw_std = 0. - self.pitch_std = 0. - self.roll_std = 0. - self.pitch_offseter = RunningStatFilter(max_trackable=max_trackable) - self.yaw_offseter = RunningStatFilter(max_trackable=max_trackable) - self.low_std = True - self.cfactor_pitch = 1. - self.cfactor_yaw = 1. - -class DriverBlink: - def __init__(self): - self.left_blink = 0. - self.right_blink = 0. -class DriverStatus: +class DriverMonitoring: def __init__(self, rhd_saved=False, settings=None, always_on=False): if settings is None: settings = DRIVER_MONITOR_SETTINGS() @@ -131,7 +134,6 @@ class DriverStatus: # init driver status self.wheelpos_learner = RunningStatFilter() self.pose = DriverPose(self.settings._POSE_OFFSET_MAX_COUNT) - self.pose_calibrated = False self.blink = DriverBlink() self.eev1 = 0. self.eev2 = 1. @@ -141,9 +143,6 @@ class DriverStatus: self.ee2_calibrated = False self.always_on = always_on - self.awareness = 1. - self.awareness_active = 1. - self.awareness_passive = 1. self.distracted_types = [] self.driver_distracted = False self.driver_distraction_filter = FirstOrderFilter(0., self.settings._DISTRACTED_FILTER_TS, self.settings._DT_DMON) @@ -160,13 +159,18 @@ class DriverStatus: 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._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: @@ -197,10 +201,21 @@ class DriverStatus: self.step_change = self.settings._DT_DMON / self.settings._AWARENESS_TIME self.active_monitoring_mode = False + def _set_policy(self, model_data, car_speed): + bp = model_data.meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s + 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 = 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 = 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: + 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: @@ -210,11 +225,11 @@ class DriverStatus: 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) - if pitch_error > (self.settings._POSE_PITCH_THRESHOLD*self.pose.cfactor_pitch if self.pose_calibrated else self.settings._PITCH_NATURAL_THRESHOLD) or \ + if pitch_error > (self.settings._POSE_PITCH_THRESHOLD*self.pose.cfactor_pitch if self.pose.calibrated else self.settings._PITCH_NATURAL_THRESHOLD) or \ yaw_error > self.settings._POSE_YAW_THRESHOLD*self.pose.cfactor_yaw: distracted_types.append(DistractedType.DISTRACTED_POSE) - if (self.blink.left_blink + self.blink.right_blink)*0.5 > self.settings._BLINK_THRESHOLD: + if (self.blink.left + self.blink.right)*0.5 > self.settings._BLINK_THRESHOLD: distracted_types.append(DistractedType.DISTRACTED_BLINK) if self.ee1_calibrated: @@ -222,27 +237,12 @@ class DriverStatus: * self.settings._EE_THRESH12 else: ee1_dist = self.eev1 > self.settings._EE_THRESH11 - # if self.ee2_calibrated: - # ee2_dist = self.eev2 < self.ee2_offseter.filtered_stat.M * self.settings._EE_THRESH22 - # else: - # ee2_dist = self.eev2 < self.settings._EE_THRESH21 if ee1_dist: distracted_types.append(DistractedType.DISTRACTED_E2E) return distracted_types - def set_policy(self, model_data, car_speed): - bp = model_data.meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s - 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 = 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 = interp(bp_normal, [0, 0.5], - [self.settings._POSE_YAW_THRESHOLD_SLACK, - self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD - - def update_states(self, driver_state, cal_rpy, car_speed, op_engaged): + def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged): 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 @@ -270,9 +270,9 @@ class DriverStatus: 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_blink = driver_data.leftBlinkProb * (driver_data.leftEyeProb > self.settings._EYE_THRESHOLD) \ + self.blink.left = driver_data.leftBlinkProb * (driver_data.leftEyeProb > self.settings._EYE_THRESHOLD) \ * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD) - self.blink.right_blink = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) \ + self.blink.right = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) \ * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD) self.eev1 = driver_data.notReadyProb[0] self.eev2 = driver_data.readyProb[0] @@ -291,7 +291,7 @@ class DriverStatus: self.ee1_offseter.push_and_update(self.eev1) self.ee2_offseter.push_and_update(self.eev2) - self.pose_calibrated = self.pose.pitch_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT and \ + 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.ee1_calibrated = self.ee1_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT self.ee2_calibrated = self.ee2_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT @@ -303,11 +303,18 @@ class DriverStatus: elif self.face_detected and self.pose.low_std: self.hi_stds = 0 - def update_events(self, events, driver_engaged, ctrl_active, standstill, wrong_gear, car_speed): + def _update_events(self, driver_engaged, op_engaged, standstill, wrong_gear, car_speed): + self._reset_events() + # Block engaging after max number of distrations or when alert active + if self.terminal_alert_cnt >= self.settings._MAX_TERMINAL_ALERTS or \ + self.terminal_time >= self.settings._MAX_TERMINAL_DURATION 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 ctrl_active) or \ - (always_on_valid and not ctrl_active and self.awareness <= 0): + (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 @@ -331,8 +338,8 @@ class DriverStatus: _reaching_audible = self.awareness - self.step_change <= self.threshold_prompt _reaching_terminal = self.awareness - self.step_change <= 0 standstill_exemption = standstill and _reaching_audible - always_on_red_exemption = always_on_valid and not ctrl_active and _reaching_terminal - always_on_lowspeed_exemption = always_on_valid and not ctrl_active and car_speed < self.settings._ALWAYS_ON_ALERT_MIN_SPEED 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 and _reaching_audible 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 @@ -358,4 +365,52 @@ class DriverStatus: alert = EventName.preDriverDistracted if self.active_monitoring_mode else EventName.preDriverUnresponsive if alert is not None: - events.add(alert) + self.current_events.add(alert) + + + def get_state_packet(self): + # build driverMonitoringState packet + dat = messaging.new_message('driverMonitoringState', valid=True) + dat.driverMonitoringState = { + "events": self.current_events.to_msg(), + "faceDetected": self.face_detected, + "isDistracted": self.driver_distracted, + "distractedType": sum(self.distracted_types), + "awarenessStatus": self.awareness, + "posePitchOffset": self.pose.pitch_offseter.filtered_stat.mean(), + "posePitchValidCount": self.pose.pitch_offseter.filtered_stat.n, + "poseYawOffset": self.pose.yaw_offseter.filtered_stat.mean(), + "poseYawValidCount": self.pose.yaw_offseter.filtered_stat.n, + "stepChange": self.step_change, + "awarenessActive": self.awareness_active, + "awarenessPassive": self.awareness_passive, + "isLowStd": self.pose.low_std, + "hiStdCount": self.hi_stds, + "isActiveMode": self.active_monitoring_mode, + "isRHD": self.wheel_on_right, + } + return dat + + def run_step(self, sm): + # Set strictness + self._set_policy( + model_data=sm['modelV2'], + car_speed=sm['carState'].vEgo + ) + + # Parse data from dmonitoringmodeld + self._update_states( + driver_state=sm['driverStateV2'], + cal_rpy=sm['liveCalibration'].rpyCalib, + car_speed=sm['carState'].vEgo, + op_engaged=sm['controlsState'].enabled + ) + + # Update distraction events + self._update_events( + driver_engaged=sm['carState'].steeringPressed or sm['carState'].gasPressed, + op_engaged=sm['controlsState'].enabled, + standstill=sm['carState'].standstill, + wrong_gear=sm['carState'].gearShifter in [car.CarState.GearShifter.reverse, car.CarState.GearShifter.park], + car_speed=sm['carState'].vEgo + ) diff --git a/selfdrive/monitoring/test_monitoring.py b/selfdrive/monitoring/test_monitoring.py index 9395960b65..a960a379e2 100755 --- a/selfdrive/monitoring/test_monitoring.py +++ b/selfdrive/monitoring/test_monitoring.py @@ -3,8 +3,7 @@ import numpy as np from cereal import car, log from openpilot.common.realtime import DT_DMON -from openpilot.selfdrive.controls.lib.events import Events -from openpilot.selfdrive.monitoring.driver_monitor import DriverStatus, DRIVER_MONITOR_SETTINGS +from openpilot.selfdrive.monitoring.helpers import DriverMonitoring, DRIVER_MONITOR_SETTINGS EventName = car.CarEvent.EventName dm_settings = DRIVER_MONITOR_SETTINGS() @@ -51,21 +50,19 @@ always_distracted = [msg_DISTRACTED] * int(TEST_TIMESPAN / DT_DMON) always_true = [True] * int(TEST_TIMESPAN / DT_DMON) always_false = [False] * int(TEST_TIMESPAN / DT_DMON) -# TODO: this only tests DriverStatus class TestMonitoring: def _run_seq(self, msgs, interaction, engaged, standstill): - DS = DriverStatus() + DM = DriverMonitoring() events = [] for idx in range(len(msgs)): - e = Events() - DS.update_states(msgs[idx], [0, 0, 0], 0, engaged[idx]) + DM._update_states(msgs[idx], [0, 0, 0], 0, engaged[idx]) # cal_rpy and car_speed don't matter here # evaluate events at 10Hz for tests - DS.update_events(e, interaction[idx], engaged[idx], standstill[idx], 0, 0) - events.append(e) + 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, DS + return events, DM def _assert_no_events(self, events): assert all(not len(e) for e in events) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index c3ab383967..4fec7453fc 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -da1b19b2d1d54412ac72f2e9645046d262f9c7ab \ No newline at end of file +dbf5b05ff480145a79b5941e360d0698b70979cd \ No newline at end of file