mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-06-20 13:32:04 +08:00
dmonitoringd: simplify main loop (#32517)
* one call does all * no need * update etst * filename * dbf5b05ff480145a79b5941e360d0698b70979cd
This commit is contained in:
ZwX1616
@@ -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
|
||||
|
||||
@@ -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
|
||||
DM.run_step(sm)
|
||||
|
||||
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)
|
||||
|
||||
# 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()
|
||||
|
||||
@@ -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,41 +201,7 @@ class DriverStatus:
|
||||
self.step_change = self.settings._DT_DMON / self.settings._AWARENESS_TIME
|
||||
self.active_monitoring_mode = False
|
||||
|
||||
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)
|
||||
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:
|
||||
distracted_types.append(DistractedType.DISTRACTED_BLINK)
|
||||
|
||||
if self.ee1_calibrated:
|
||||
ee1_dist = self.eev1 > max(min(self.ee1_offseter.filtered_stat.M, self.settings._EE_MAX_OFFSET1), self.settings._EE_MIN_OFFSET1) \
|
||||
* 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):
|
||||
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)
|
||||
@@ -242,7 +212,37 @@ class DriverStatus:
|
||||
[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 _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)
|
||||
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 + self.blink.right)*0.5 > self.settings._BLINK_THRESHOLD:
|
||||
distracted_types.append(DistractedType.DISTRACTED_BLINK)
|
||||
|
||||
if self.ee1_calibrated:
|
||||
ee1_dist = self.eev1 > max(min(self.ee1_offseter.filtered_stat.M, self.settings._EE_MAX_OFFSET1), self.settings._EE_MIN_OFFSET1) \
|
||||
* self.settings._EE_THRESH12
|
||||
else:
|
||||
ee1_dist = self.eev1 > self.settings._EE_THRESH11
|
||||
if ee1_dist:
|
||||
distracted_types.append(DistractedType.DISTRACTED_E2E)
|
||||
|
||||
return distracted_types
|
||||
|
||||
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
|
||||
)
|
||||
@@ -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)
|
||||
|
||||
@@ -1 +1 @@
|
||||
da1b19b2d1d54412ac72f2e9645046d262f9c7ab
|
||||
dbf5b05ff480145a79b5941e360d0698b70979cd
|
||||
Reference in New Issue
Block a user