sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

dmonitoringd: simplify main loop (#32517)

Browse Source 
* one call does all

* no need

* update etst

* filename

* dbf5b05ff480145a79b5941e360d0698b70979cd
pull/32527/head
ZwX1616 11 months ago committed by GitHub
parent e0d20d2cf3
commit 204219695f
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
5 changed files with 133 additions and 123 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      release/files_common
  2. 66
      selfdrive/monitoring/dmonitoringd.py
  3. 171
      selfdrive/monitoring/helpers.py
  4. 15
      selfdrive/monitoring/test_monitoring.py
  5. 2
      selfdrive/test/process_replay/ref_commit

2
release/files_common
Unescape View File

@ -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

66
selfdrive/monitoring/dmonitoringd.py
Unescape View File

@ -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()

171
selfdrive/monitoring/driver_monitor.py → selfdrive/monitoring/helpers.py
Unescape View File

@ -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
)

15
selfdrive/monitoring/test_monitoring.py
Unescape View File

@ -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)

2
selfdrive/test/process_replay/ref_commit
Unescape View File

@ -1 +1 @@
da1b19b2d1d54412ac72f2e9645046d262f9c7ab
dbf5b05ff480145a79b5941e360d0698b70979cd
Write Preview
Loading…
Cancel
Save