From da100a9afa9fc8ca7d1eb8bd52aae5a7254db16d Mon Sep 17 00:00:00 2001 From: ZwX1616 Date: Tue, 31 May 2022 13:30:11 -0700 Subject: [PATCH] use toggle --- selfdrive/monitoring/dmonitoringd.py | 4 ++-- selfdrive/monitoring/driver_monitor.py | 28 +++++++++++++------------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index 5d13d1897c..6c41daeefb 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -3,7 +3,7 @@ import gc import cereal.messaging as messaging from cereal import car -# from common.params import Params +from common.params import Params from common.realtime import set_realtime_priority from selfdrive.controls.lib.events import Events from selfdrive.locationd.calibrationd import Calibration @@ -20,7 +20,7 @@ def dmonitoringd_thread(sm=None, pm=None): if sm is None: sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll=['driverState']) - driver_status = DriverStatus() + driver_status = DriverStatus(rhd=Params().get_bool("IsRHD")) sm['liveCalibration'].calStatus = Calibration.INVALID sm['liveCalibration'].rpyCalib = [0, 0, 0] diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py index 1311bccbfd..b4c1e020ed 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/selfdrive/monitoring/driver_monitor.py @@ -77,7 +77,7 @@ class DistractedType: DISTRACTED_BLINK = 2 DISTRACTED_E2E = 4 -def face_orientation_from_net(angles_desc, pos_desc, rpy_calib): +def face_orientation_from_net(angles_desc, pos_desc, rpy_calib, is_rhd): # the output of these angles are in device frame # so from driver's perspective, pitch is up and yaw is right @@ -92,7 +92,7 @@ def face_orientation_from_net(angles_desc, pos_desc, rpy_calib): # no calib for roll pitch -= rpy_calib[1] - yaw -= rpy_calib[2] + yaw -= rpy_calib[2] * (1 - 2 * int(is_rhd)) return roll_net, pitch, yaw class DriverPose(): @@ -115,12 +115,12 @@ class DriverBlink(): self.right_blink = 0. class DriverStatus(): - def __init__(self, settings=DRIVER_MONITOR_SETTINGS()): + def __init__(self, rhd=False, settings=DRIVER_MONITOR_SETTINGS()): # init policy settings self.settings = settings # init driver status - self.wheelpos_learner = RunningStatFilter() + # self.wheelpos_learner = RunningStatFilter() self.pose = DriverPose(self.settings._POSE_OFFSET_MAX_COUNT) self.pose_calibrated = False self.blink = DriverBlink() @@ -137,7 +137,7 @@ class DriverStatus(): 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 = rhd self.face_detected = False self.terminal_alert_cnt = 0 self.terminal_time = 0 @@ -225,21 +225,21 @@ class DriverStatus(): self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD def update_states(self, driver_state, cal_rpy, car_speed, op_engaged): - rhd_pred = driver_state.wheelOnRight - if car_speed > 0.01: - self.wheelpos_learner.push_and_update(rhd_pred) - if self.wheelpos_learner.filtered_stat.n > self.settings._WHEELPOS_FILTER_MIN_COUNT: - self.wheel_on_right = self.wheelpos_learner.filtered_stat.M > self.settings._WHEELPOS_THRESHOLD - else: - self.wheel_on_right = rhd_pred > self.settings._WHEELPOS_THRESHOLD - driver_data = driver_state.driverDataRH if self.wheel_on_right else driver_state.driverDataLH + # rhd_pred = driver_state.wheelOnRight + # if car_speed > 0.01: + # self.wheelpos_learner.push_and_update(rhd_pred) + # if self.wheelpos_learner.filtered_stat.n > self.settings._WHEELPOS_FILTER_MIN_COUNT: + # self.wheel_on_right = self.wheelpos_learner.filtered_stat.M > self.settings._WHEELPOS_THRESHOLD + # else: + # self.wheel_on_right = rhd_pred > self.settings._WHEELPOS_THRESHOLD + driver_data = driver_state.driverDataLH if not all(len(x) > 0 for x in (driver_data.faceOrientation, driver_data.facePosition, driver_data.faceOrientationStd, driver_data.facePositionStd, driver_data.readyProb, driver_data.notReadyProb)): return self.face_detected = driver_data.faceProb > self.settings._FACE_THRESHOLD - self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_data.faceOrientation, driver_data.facePosition, cal_rpy) + self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_data.faceOrientation, driver_data.facePosition, cal_rpy, 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)