diff --git a/cereal b/cereal index e20113a55..0dca98677 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit e20113a5551810da1afdc5eed5dda3bd37dacfdd +Subproject commit 0dca986771da39f1a810e58cfb65850be71f1ffb diff --git a/models/dmonitoring_model.current b/models/dmonitoring_model.current index b9e0c2475..21ebe144d 100644 --- a/models/dmonitoring_model.current +++ b/models/dmonitoring_model.current @@ -1,2 +1,2 @@ -4e19be90-bd5b-485d-b79a-2462f7f1b49e -08f7ec37b78228cd1cb750b6ddb9c6ba1769e911 \ No newline at end of file +0ff292a8-3a9c-47e7-9134-2c3b0f69a1fe +d7c2883ee58d7b757e588bdf13ec45891caf397c \ No newline at end of file diff --git a/models/dmonitoring_model.onnx b/models/dmonitoring_model.onnx index 9fe743334..40a65d761 100644 --- a/models/dmonitoring_model.onnx +++ b/models/dmonitoring_model.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:895ee32e2a1c77496e015270db475eef65034b25331f2859bac0ccf702f64298 -size 3294407 +oid sha256:36cdea3c4b03f91cb243e4fa51f52e350e4906afcc5014345190e0c1d2bfaf25 +size 3792414 diff --git a/models/dmonitoring_model_q.dlc b/models/dmonitoring_model_q.dlc index 1e34e0de3..310683142 100644 --- a/models/dmonitoring_model_q.dlc +++ b/models/dmonitoring_model_q.dlc @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:6e4ac870984d11cd8e86cda4a63e3321fde837bacf4a055a27b7c8ba34facfe2 -size 916079 +oid sha256:41901abff0e16e6c404627234cca68001fbd21f6961709a729a84a8d5e3cc56d +size 1146001 diff --git a/selfdrive/modeld/dmonitoringmodeld.cc b/selfdrive/modeld/dmonitoringmodeld.cc index c85c05c9d..8016180f4 100644 --- a/selfdrive/modeld/dmonitoringmodeld.cc +++ b/selfdrive/modeld/dmonitoringmodeld.cc @@ -13,6 +13,8 @@ ExitHandler do_exit; void run_model(DMonitoringModelState &model, VisionIpcClient &vipc_client) { PubMaster pm({"driverState"}); + SubMaster sm({"liveCalibration"}); + float calib[CALIB_LEN] = {0}; double last = 0; while (!do_exit) { @@ -20,8 +22,16 @@ void run_model(DMonitoringModelState &model, VisionIpcClient &vipc_client) { VisionBuf *buf = vipc_client.recv(&extra); if (buf == nullptr) continue; + sm.update(0); + if (sm.updated("liveCalibration")) { + auto calib_msg = sm["liveCalibration"].getLiveCalibration().getRpyCalib(); + for (int i = 0; i < CALIB_LEN; i++) { + calib[i] = calib_msg[i]; + } + } + double t1 = millis_since_boot(); - DMonitoringResult res = dmonitoring_eval_frame(&model, buf->addr, buf->width, buf->height); + DMonitoringResult res = dmonitoring_eval_frame(&model, buf->addr, buf->width, buf->height, calib); double t2 = millis_since_boot(); // send dm packet diff --git a/selfdrive/modeld/models/commonmodel.cc b/selfdrive/modeld/models/commonmodel.cc index 1a1f6eaea..a075c08de 100644 --- a/selfdrive/modeld/models/commonmodel.cc +++ b/selfdrive/modeld/models/commonmodel.cc @@ -70,7 +70,3 @@ void softmax(const float* input, float* output, size_t len) { float sigmoid(float input) { return 1 / (1 + expf(-input)); } - -float softplus(float input) { - return log1p(expf(input)); -} diff --git a/selfdrive/modeld/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h index 19af75eef..182e8978b 100644 --- a/selfdrive/modeld/models/commonmodel.h +++ b/selfdrive/modeld/models/commonmodel.h @@ -19,7 +19,6 @@ const bool send_raw_pred = getenv("SEND_RAW_PRED") != NULL; void softmax(const float* input, float* output, size_t len); -float softplus(float input); float sigmoid(float input); class ModelFrame { diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc index 005d0bcc5..40f4220c0 100644 --- a/selfdrive/modeld/models/dmonitoring.cc +++ b/selfdrive/modeld/models/dmonitoring.cc @@ -43,6 +43,8 @@ void dmonitoring_init(DMonitoringModelState* s) { #else s->m = new SNPEModel("../../models/dmonitoring_model_q.dlc", &s->output[0], OUTPUT_SIZE, USE_DSP_RUNTIME); #endif + + s->m->addCalib(s->calib, CALIB_LEN); } static inline auto get_yuv_buf(std::vector &buf, const int width, int height) { @@ -65,7 +67,7 @@ void crop_yuv(uint8_t *raw, int width, int height, uint8_t *y, uint8_t *u, uint8 } } -DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height) { +DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, float *calib) { Rect crop_rect; if (width == TICI_CAM_WIDTH) { const int cropped_height = tici_dm_crop::width / 1.33; @@ -167,29 +169,38 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_ double t1 = millis_since_boot(); s->m->addImage(net_input_buf, yuv_buf_len); + for (int i = 0; i < CALIB_LEN; i++) { + s->calib[i] = calib[i]; + } s->m->execute(); double t2 = millis_since_boot(); DMonitoringResult ret = {0}; for (int i = 0; i < 3; ++i) { - ret.face_orientation[i] = s->output[i]; - ret.face_orientation_meta[i] = softplus(s->output[6 + i]); + ret.face_orientation[i] = s->output[i] * REG_SCALE; + ret.face_orientation_meta[i] = exp(s->output[6 + i]); } for (int i = 0; i < 2; ++i) { - ret.face_position[i] = s->output[3 + i]; - ret.face_position_meta[i] = softplus(s->output[9 + i]); + ret.face_position[i] = s->output[3 + i] * REG_SCALE; + ret.face_position_meta[i] = exp(s->output[9 + i]); } - ret.face_prob = s->output[12]; - ret.left_eye_prob = s->output[21]; - ret.right_eye_prob = s->output[30]; - ret.left_blink_prob = s->output[31]; - ret.right_blink_prob = s->output[32]; - ret.sg_prob = s->output[33]; - ret.poor_vision = s->output[34]; - ret.partial_face = s->output[35]; - ret.distracted_pose = s->output[36]; - ret.distracted_eyes = s->output[37]; - ret.occluded_prob = s->output[38]; + for (int i = 0; i < 4; ++i) { + ret.ready_prob[i] = sigmoid(s->output[39 + i]); + } + for (int i = 0; i < 2; ++i) { + ret.not_ready_prob[i] = sigmoid(s->output[43 + i]); + } + ret.face_prob = sigmoid(s->output[12]); + ret.left_eye_prob = sigmoid(s->output[21]); + ret.right_eye_prob = sigmoid(s->output[30]); + ret.left_blink_prob = sigmoid(s->output[31]); + ret.right_blink_prob = sigmoid(s->output[32]); + ret.sg_prob = sigmoid(s->output[33]); + ret.poor_vision = sigmoid(s->output[34]); + ret.partial_face = sigmoid(s->output[35]); + ret.distracted_pose = sigmoid(s->output[36]); + ret.distracted_eyes = sigmoid(s->output[37]); + ret.occluded_prob = sigmoid(s->output[38]); ret.dsp_execution_time = (t2 - t1) / 1000.; return ret; } @@ -217,6 +228,8 @@ void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResu framed.setDistractedPose(res.distracted_pose); framed.setDistractedEyes(res.distracted_eyes); framed.setOccludedProb(res.occluded_prob); + framed.setReadyProb(res.ready_prob); + framed.setNotReadyProb(res.not_ready_prob); if (send_raw_pred) { framed.setRawPredictions(raw_pred.asBytes()); } diff --git a/selfdrive/modeld/models/dmonitoring.h b/selfdrive/modeld/models/dmonitoring.h index 9b3ee631f..b23305686 100644 --- a/selfdrive/modeld/models/dmonitoring.h +++ b/selfdrive/modeld/models/dmonitoring.h @@ -7,7 +7,10 @@ #include "selfdrive/modeld/models/commonmodel.h" #include "selfdrive/modeld/runners/run.h" -#define OUTPUT_SIZE 39 +#define CALIB_LEN 3 + +#define OUTPUT_SIZE 45 +#define REG_SCALE 0.25f typedef struct DMonitoringResult { float face_orientation[3]; @@ -25,6 +28,8 @@ typedef struct DMonitoringResult { float distracted_pose; float distracted_eyes; float occluded_prob; + float ready_prob[4]; + float not_ready_prob[2]; float dsp_execution_time; } DMonitoringResult; @@ -36,11 +41,12 @@ typedef struct DMonitoringModelState { std::vector cropped_buf; std::vector premirror_cropped_buf; std::vector net_input_buf; + float calib[CALIB_LEN]; float tensor[UINT8_MAX + 1]; } DMonitoringModelState; void dmonitoring_init(DMonitoringModelState* s); -DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height); +DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, float *calib); void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, float execution_time, kj::ArrayPtr raw_pred); void dmonitoring_free(DMonitoringModelState* s); diff --git a/selfdrive/modeld/runners/onnxmodel.cc b/selfdrive/modeld/runners/onnxmodel.cc index b2de9e9a1..9ba08570e 100644 --- a/selfdrive/modeld/runners/onnxmodel.cc +++ b/selfdrive/modeld/runners/onnxmodel.cc @@ -100,6 +100,11 @@ void ONNXModel::addTrafficConvention(float *state, int state_size) { traffic_convention_size = state_size; } +void ONNXModel::addCalib(float *state, int state_size) { + calib_input_buf = state; + calib_size = state_size; +} + void ONNXModel::addImage(float *image_buf, int buf_size) { image_input_buf = image_buf; image_buf_size = buf_size; @@ -124,6 +129,9 @@ void ONNXModel::execute() { if (traffic_convention_input_buf != NULL) { pwrite(traffic_convention_input_buf, traffic_convention_size); } + if (calib_input_buf != NULL) { + pwrite(calib_input_buf, calib_size); + } if (rnn_input_buf != NULL) { pwrite(rnn_input_buf, rnn_state_size); } diff --git a/selfdrive/modeld/runners/onnxmodel.h b/selfdrive/modeld/runners/onnxmodel.h index a3a6a1862..567d81d29 100644 --- a/selfdrive/modeld/runners/onnxmodel.h +++ b/selfdrive/modeld/runners/onnxmodel.h @@ -11,6 +11,7 @@ public: void addRecurrent(float *state, int state_size); void addDesire(float *state, int state_size); void addTrafficConvention(float *state, int state_size); + void addCalib(float *state, int state_size); void addImage(float *image_buf, int buf_size); void addExtra(float *image_buf, int buf_size); void execute(); @@ -26,6 +27,8 @@ private: int desire_state_size; float *traffic_convention_input_buf = NULL; int traffic_convention_size; + float *calib_input_buf = NULL; + int calib_size; float *image_input_buf = NULL; int image_buf_size; float *extra_input_buf = NULL; diff --git a/selfdrive/modeld/runners/runmodel.h b/selfdrive/modeld/runners/runmodel.h index 2d61351eb..02b8c4b20 100644 --- a/selfdrive/modeld/runners/runmodel.h +++ b/selfdrive/modeld/runners/runmodel.h @@ -5,6 +5,7 @@ public: virtual void addRecurrent(float *state, int state_size) {} virtual void addDesire(float *state, int state_size) {} virtual void addTrafficConvention(float *state, int state_size) {} + virtual void addCalib(float *state, int state_size) {} virtual void addImage(float *image_buf, int buf_size) {} virtual void addExtra(float *image_buf, int buf_size) {} virtual void execute() {} diff --git a/selfdrive/modeld/runners/snpemodel.cc b/selfdrive/modeld/runners/snpemodel.cc index df66a9db9..0d394ca51 100644 --- a/selfdrive/modeld/runners/snpemodel.cc +++ b/selfdrive/modeld/runners/snpemodel.cc @@ -141,6 +141,11 @@ void SNPEModel::addDesire(float *state, int state_size) { desireBuffer = this->addExtra(state, state_size, 1); } +void SNPEModel::addCalib(float *state, int state_size) { + calib = state; + calibBuffer = this->addExtra(state, state_size, 1); +} + void SNPEModel::addImage(float *image_buf, int buf_size) { input = image_buf; input_size = buf_size; diff --git a/selfdrive/modeld/runners/snpemodel.h b/selfdrive/modeld/runners/snpemodel.h index 0c927a2f4..584c63680 100644 --- a/selfdrive/modeld/runners/snpemodel.h +++ b/selfdrive/modeld/runners/snpemodel.h @@ -25,6 +25,7 @@ public: SNPEModel(const char *path, float *loutput, size_t loutput_size, int runtime, bool luse_extra = false); void addRecurrent(float *state, int state_size); void addTrafficConvention(float *state, int state_size); + void addCalib(float *state, int state_size); void addDesire(float *state, int state_size); void addImage(float *image_buf, int buf_size); void addExtra(float *image_buf, int buf_size); @@ -71,4 +72,6 @@ private: std::unique_ptr trafficConventionBuffer; float *desire; std::unique_ptr desireBuffer; + float *calib; + std::unique_ptr calibBuffer; }; diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index f1b0c42bd..9a3196030 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -51,7 +51,7 @@ def dmonitoringd_thread(sm=None, pm=None): # Get data from dmonitoringmodeld events = Events() - driver_status.get_pose(sm['driverState'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled) + driver_status.update_states(sm['driverState'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled) # Block engaging after max number of distrations if driver_status.terminal_alert_cnt >= driver_status.settings._MAX_TERMINAL_ALERTS or \ @@ -59,7 +59,7 @@ def dmonitoringd_thread(sm=None, pm=None): events.add(car.CarEvent.EventName.tooDistracted) # Update events from driver state - driver_status.update(events, driver_engaged, sm['controlsState'].enabled, sm['carState'].standstill) + driver_status.update_events(events, driver_engaged, sm['controlsState'].enabled, sm['carState'].standstill) # build driverMonitoringState packet dat = messaging.new_message('driverMonitoringState') @@ -67,6 +67,7 @@ def dmonitoringd_thread(sm=None, pm=None): "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, diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py index 2e317aefb..5ca02f793 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/selfdrive/monitoring/driver_monitor.py @@ -27,13 +27,18 @@ class DRIVER_MONITOR_SETTINGS(): self._DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6. self._FACE_THRESHOLD = 0.5 - self._PARTIAL_FACE_THRESHOLD = 0.765 if TICI else 0.43 - self._EYE_THRESHOLD = 0.61 if TICI else 0.55 - self._SG_THRESHOLD = 0.89 if TICI else 0.86 - self._BLINK_THRESHOLD = 0.82 if TICI else 0.588 - self._BLINK_THRESHOLD_SLACK = 0.9 if TICI else 0.77 + self._PARTIAL_FACE_THRESHOLD = 0.8 if TICI else 0.45 + self._EYE_THRESHOLD = 0.55 + self._SG_THRESHOLD = 0.88 if TICI else 0.86 + self._BLINK_THRESHOLD = 0.61 if TICI else 0.59 + self._BLINK_THRESHOLD_SLACK = 0.8 if TICI else 0.75 self._BLINK_THRESHOLD_STRICT = self._BLINK_THRESHOLD + self._EE_THRESH11 = 0.75 if TICI else 0.4 + self._EE_THRESH12 = 3.25 if TICI else 2.45 + self._EE_THRESH21 = 0.01 + self._EE_THRESH22 = 0.35 + self._POSE_PITCH_THRESHOLD = 0.3237 self._POSE_PITCH_THRESHOLD_SLACK = 0.3657 self._POSE_PITCH_THRESHOLD_STRICT = self._POSE_PITCH_THRESHOLD @@ -47,7 +52,7 @@ class DRIVER_MONITOR_SETTINGS(): self._YAW_MAX_OFFSET = 0.289 self._YAW_MIN_OFFSET = -0.0246 - self._POSESTD_THRESHOLD = 0.38 if TICI else 0.3 + self._POSESTD_THRESHOLD = 0.315 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 @@ -68,8 +73,9 @@ H, W, FULL_W = 320, 160, 426 class DistractedType: NOT_DISTRACTED = 0 - BAD_POSE = 1 - BAD_BLINK = 2 + DISTRACTED_POSE = 1 + DISTRACTED_BLINK = 2 + DISTRACTED_E2E = 4 def face_orientation_from_net(angles_desc, pos_desc, rpy_calib, is_rhd): # the output of these angles are in device frame @@ -119,9 +125,17 @@ class DriverStatus(): self.pose = DriverPose(self.settings._POSE_OFFSET_MAX_COUNT) self.pose_calibrated = False self.blink = DriverBlink() + self.eev1 = 0. + self.eev2 = 1. + self.ee1_offseter = RunningStatFilter(max_trackable=self.settings._POSE_OFFSET_MAX_COUNT) + self.ee2_offseter = RunningStatFilter(max_trackable=self.settings._POSE_OFFSET_MAX_COUNT) + self.ee1_calibrated = False + self.ee2_calibrated = False + 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) self.face_detected = False @@ -167,26 +181,38 @@ class DriverStatus(): self.step_change = self.settings._DT_DMON / self.settings._AWARENESS_TIME self.active_monitoring_mode = False - def _is_driver_distracted(self, pose, blink): - if not self.pose_calibrated: - pitch_error = pose.pitch - self.settings._PITCH_NATURAL_OFFSET - yaw_error = pose.yaw - self.settings._YAW_NATURAL_OFFSET - else: - 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) + 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 or \ + yaw_error > self.settings._POSE_YAW_THRESHOLD*self.pose.cfactor_yaw: + distracted_types.append(DistractedType.DISTRACTED_POSE) - 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 + if (self.blink.left_blink + self.blink.right_blink)*0.5 > self.settings._BLINK_THRESHOLD*self.blink.cfactor: + distracted_types.append(DistractedType.DISTRACTED_BLINK) + + if self.ee1_calibrated: + ee1_dist = self.eev1 > self.ee1_offseter.filtered_stat.M * self.settings._EE_THRESH12 else: - return DistractedType.NOT_DISTRACTED + 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 or ee2_dist: + distracted_types.append(DistractedType.DISTRACTED_E2E) + + return distracted_types def set_policy(self, model_data, car_speed): ep = min(model_data.meta.engagedProb, 0.8) / 0.8 # engaged prob @@ -205,9 +231,10 @@ class DriverStatus(): [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): + def update_states(self, driver_state, cal_rpy, car_speed, op_engaged): if not all(len(x) > 0 for x in (driver_state.faceOrientation, driver_state.facePosition, - driver_state.faceOrientationStd, driver_state.facePositionStd)): + driver_state.faceOrientationStd, driver_state.facePositionStd, + driver_state.readyProb, driver_state.notReadyProb)): return self.face_partial = driver_state.partialFace > self.settings._PARTIAL_FACE_THRESHOLD @@ -220,9 +247,13 @@ class DriverStatus(): self.pose.low_std = model_std_max < self.settings._POSESTD_THRESHOLD and not self.face_partial self.blink.left_blink = driver_state.leftBlinkProb * (driver_state.leftEyeProb > self.settings._EYE_THRESHOLD) * (driver_state.sunglassesProb < self.settings._SG_THRESHOLD) self.blink.right_blink = driver_state.rightBlinkProb * (driver_state.rightEyeProb > self.settings._EYE_THRESHOLD) * (driver_state.sunglassesProb < self.settings._SG_THRESHOLD) + self.eev1 = driver_state.notReadyProb[1] + self.eev2 = driver_state.readyProb[0] - self.driver_distracted = self._is_driver_distracted(self.pose, self.blink) > 0 and \ - driver_state.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std + self.distracted_types = self._get_distracted_types() + self.driver_distracted = (DistractedType.DISTRACTED_POSE in self.distracted_types or + DistractedType.DISTRACTED_BLINK in self.distracted_types) and \ + driver_state.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std self.driver_distraction_filter.update(self.driver_distracted) # update offseter @@ -230,9 +261,13 @@ class DriverStatus(): if self.face_detected and car_speed > self.settings._POSE_CALIB_MIN_SPEED and self.pose.low_std and (not op_engaged or not self.driver_distracted): self.pose.pitch_offseter.push_and_update(self.pose.pitch) self.pose.yaw_offseter.push_and_update(self.pose.yaw) + 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.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 self.is_model_uncertain = self.hi_stds > self.settings._HI_STD_FALLBACK_TIME self._set_timers(self.face_detected and not self.is_model_uncertain) @@ -241,7 +276,7 @@ class DriverStatus(): elif self.face_detected and self.pose.low_std: self.hi_stds = 0 - def update(self, events, driver_engaged, ctrl_active, standstill): + def update_events(self, events, driver_engaged, ctrl_active, standstill): if (driver_engaged and self.awareness > 0) or not ctrl_active: # reset only when on disengagement if red reached self.awareness = 1. diff --git a/selfdrive/monitoring/test_monitoring.py b/selfdrive/monitoring/test_monitoring.py index 4079e3413..66b345194 100755 --- a/selfdrive/monitoring/test_monitoring.py +++ b/selfdrive/monitoring/test_monitoring.py @@ -27,6 +27,9 @@ def make_msg(face_detected, distracted=False, model_uncertain=False): ds.rightBlinkProb = 1. * distracted ds.faceOrientationStd = [1.*model_uncertain, 1.*model_uncertain, 1.*model_uncertain] ds.facePositionStd = [1.*model_uncertain, 1.*model_uncertain] + # TODO: test both separately when e2e is used + ds.readyProb = [0., 0., 0., 0.] + ds.notReadyProb = [0., 0.] return ds @@ -57,11 +60,11 @@ class TestMonitoring(unittest.TestCase): events = [] for idx in range(len(msgs)): e = Events() - DS.get_pose(msgs[idx], [0, 0, 0], 0, engaged[idx]) + DS.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(e, interaction[idx], engaged[idx], standstill[idx]) + DS.update_events(e, interaction[idx], engaged[idx], standstill[idx]) events.append(e) assert len(events) == len(msgs), f"got {len(events)} for {len(msgs)} driverState input msgs" return events, DS diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index a875d709c..8ef8fcb57 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -710313545a2b5e0899c6ee0b99ecd9d322cfecb7 +012f7c1b4aca0e0f38a1a2ddea41e738debe0003 \ No newline at end of file diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 7135a57ce..419d1adab 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -e4a770725fd7240388e1cadf6baf9ee456f65aaf \ No newline at end of file +6b5f707e5c58b0162044b2d5a07a35ec00296504 \ No newline at end of file