From f464a02782da3a76d5776f82bcc92b98164588ec Mon Sep 17 00:00:00 2001 From: ZwX1616 Date: Tue, 14 Dec 2021 12:13:59 -0800 Subject: [PATCH] DM: more adaptive pose policy (#23184) * rename and add dep * proto thresholds * tweak vk * update natural offset and clip offsets * 95th looks good * no punish for being relaxed * yaw laplace only * some pay more attention * update ref Co-authored-by: Willem Melching old-commit-hash: af5a418fa65347fb9215ce28c86c85cd3c4d9b0a --- selfdrive/monitoring/dmonitoringd.py | 2 +- selfdrive/monitoring/driver_monitor.py | 51 +++++++++++++++--------- selfdrive/test/process_replay/ref_commit | 2 +- 3 files changed, 35 insertions(+), 20 deletions(-) diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index f11d72266c..1728cc9e79 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -43,7 +43,7 @@ def dmonitoringd_thread(sm=None, pm=None): v_cruise_last = v_cruise if sm.updated['modelV2']: - driver_status.set_policy(sm['modelV2']) + driver_status.set_policy(sm['modelV2'], sm['carState'].vEgo) # Get data from dmonitoringmodeld events = Events() diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py index e4018d66f6..c470612dc0 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/selfdrive/monitoring/driver_monitor.py @@ -32,15 +32,21 @@ class DRIVER_MONITOR_SETTINGS(): self._BLINK_THRESHOLD = 0.82 if TICI else 0.588 self._BLINK_THRESHOLD_SLACK = 0.9 if TICI else 0.77 self._BLINK_THRESHOLD_STRICT = self._BLINK_THRESHOLD - self._PITCH_WEIGHT = 1.35 # pitch matters a lot more - self._POSESTD_THRESHOLD = 0.38 if TICI else 0.3 - self._METRIC_THRESHOLD = 0.48 - self._METRIC_THRESHOLD_SLACK = 0.66 - self._METRIC_THRESHOLD_STRICT = self._METRIC_THRESHOLD - self._PITCH_NATURAL_OFFSET = 0.02 # people don't seem to look straight when they drive relaxed, rather a bit up - self._YAW_NATURAL_OFFSET = 0.08 # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car) + self._POSE_PITCH_THRESHOLD = 0.3237 + self._POSE_PITCH_THRESHOLD_SLACK = 0.3657 + self._POSE_PITCH_THRESHOLD_STRICT = self._POSE_PITCH_THRESHOLD + self._POSE_YAW_THRESHOLD = 0.3109 + self._POSE_YAW_THRESHOLD_SLACK = 0.4294 + self._POSE_YAW_THRESHOLD_STRICT = self._POSE_YAW_THRESHOLD + self._PITCH_NATURAL_OFFSET = 0.057 # initial value before offset is learned + self._YAW_NATURAL_OFFSET = 0.11 # initial value before offset is learned + 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._POSESTD_THRESHOLD = 0.38 if TICI else 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 @@ -93,7 +99,8 @@ class DriverPose(): self.pitch_offseter = RunningStatFilter(max_trackable=max_trackable) self.yaw_offseter = RunningStatFilter(max_trackable=max_trackable) self.low_std = True - self.cfactor = 1. + self.cfactor_pitch = 1. + self.cfactor_yaw = 1. class DriverBlink(): def __init__(self): @@ -164,30 +171,38 @@ class DriverStatus(): pitch_error = pose.pitch - self.settings._PITCH_NATURAL_OFFSET yaw_error = pose.yaw - self.settings._YAW_NATURAL_OFFSET else: - pitch_error = pose.pitch - self.pose.pitch_offseter.filtered_stat.mean() - yaw_error = pose.yaw - self.pose.yaw_offseter.filtered_stat.mean() + pitch_error = pose.pitch - min(max(self.pose.pitch_offseter.filtered_stat.mean(), + self.settings._PITCH_MIN_OFFSET), self.settings._PITCH_MAX_OFFSET) + yaw_error = 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._PITCH_WEIGHT > self.settings._METRIC_THRESHOLD*pose.cfactor or \ - yaw_error > self.settings._METRIC_THRESHOLD*pose.cfactor: + if pitch_error > self.settings._POSE_PITCH_THRESHOLD*pose.cfactor_pitch or \ + yaw_error > self.settings._POSE_YAW_THRESHOLD*pose.cfactor_yaw: return DistractedType.BAD_POSE elif (blink.left_blink + blink.right_blink)*0.5 > self.settings._BLINK_THRESHOLD*blink.cfactor: return DistractedType.BAD_BLINK else: return DistractedType.NOT_DISTRACTED - def set_policy(self, model_data): - ep = min(model_data.meta.engagedProb, 0.8) / 0.8 - self.pose.cfactor = interp(ep, [0, 0.5, 1], - [self.settings._METRIC_THRESHOLD_STRICT, - self.settings. _METRIC_THRESHOLD, - self.settings._METRIC_THRESHOLD_SLACK]) / self.settings._METRIC_THRESHOLD + def set_policy(self, model_data, car_speed): + ep = min(model_data.meta.engagedProb, 0.8) / 0.8 # engaged prob + bp = model_data.meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s + # TODO: retune adaptive blink self.blink.cfactor = interp(ep, [0, 0.5, 1], [self.settings._BLINK_THRESHOLD_STRICT, self.settings._BLINK_THRESHOLD, self.settings._BLINK_THRESHOLD_SLACK]) / self.settings._BLINK_THRESHOLD + 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_pose(self, driver_state, cal_rpy, car_speed, op_engaged): if not all(len(x) > 0 for x in [driver_state.faceOrientation, driver_state.facePosition, diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 863b5a20fa..2cb7782462 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -7f618b16bfa47352143d6edac30fba05f9fdfc28 \ No newline at end of file +179e288c649e6fd7b51773e5942da2013bfbd211 \ No newline at end of file