From dfc12926144df063fb6bd20a97bb4390a4f5a4ef Mon Sep 17 00:00:00 2001 From: Vivek Aithal Date: Tue, 15 Mar 2022 14:40:29 -0700 Subject: [PATCH 1/5] locationd: Commissue fix (#23951) * check allAliveAndValid only at filter init * revert cereal and bugfix test * rename filterValid to filterInitialized * bump cereal * correct err * update refs --- cereal | 2 +- selfdrive/locationd/liblocationd.cc | 4 ++-- selfdrive/locationd/locationd.cc | 15 +++++++++------ selfdrive/locationd/locationd.h | 2 +- selfdrive/locationd/test/test_locationd.py | 6 +++--- selfdrive/test/process_replay/ref_commit | 2 +- 6 files changed, 17 insertions(+), 14 deletions(-) diff --git a/cereal b/cereal index ad2fe885d..e20113a55 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit ad2fe885dab99896908b88e765a5f720bfd79b3b +Subproject commit e20113a5551810da1afdc5eed5dda3bd37dacfdd diff --git a/selfdrive/locationd/liblocationd.cc b/selfdrive/locationd/liblocationd.cc index 250d90b38..07216365b 100755 --- a/selfdrive/locationd/liblocationd.cc +++ b/selfdrive/locationd/liblocationd.cc @@ -8,10 +8,10 @@ extern "C" { } void localizer_get_message_bytes(Localizer *localizer, uint64_t logMonoTime, - bool inputsOK, bool sensorsOK, bool gpsOK, char *buff, size_t buff_size) + bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, char *buff, size_t buff_size) { MessageBuilder msg_builder; - kj::ArrayPtr arr = localizer->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK).asChars(); + kj::ArrayPtr arr = localizer->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK, msgValid).asChars(); assert(buff_size >= arr.size()); memcpy(buff, arr.begin(), arr.size()); } diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 5cec492d6..6864ada0a 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -457,15 +457,16 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) { } kj::ArrayPtr Localizer::get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime, - bool inputsOK, bool sensorsOK, bool gpsOK) + bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid) { cereal::Event::Builder evt = msg_builder.initEvent(); evt.setLogMonoTime(logMonoTime); - evt.setValid(inputsOK); + evt.setValid(msgValid); cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman(); this->build_live_location(liveLoc); liveLoc.setSensorsOK(sensorsOK); liveLoc.setGpsOK(gpsOK); + liveLoc.setInputsOK(inputsOK); return msg_builder.toBytes(); } @@ -497,19 +498,21 @@ int Localizer::locationd_thread() { SubMaster sm(service_list, nullptr, { "gpsLocationExternal" }); uint64_t cnt = 0; + bool filterInitialized = false; while (!do_exit) { sm.update(); - if (sm.allAliveAndValid()){ + if (filterInitialized){ for (const char* service : service_list) { - if (sm.updated(service)){ + if (sm.updated(service) && sm.valid(service)){ const cereal::Event::Reader log = sm[service]; this->handle_msg(log); } } + } else { + filterInitialized = sm.allAliveAndValid(); } - if (sm.updated("cameraOdometry")) { uint64_t logMonoTime = sm["cameraOdometry"].getLogMonoTime(); bool inputsOK = sm.allAliveAndValid(); @@ -517,7 +520,7 @@ int Localizer::locationd_thread() { bool gpsOK = this->isGpsOK(); MessageBuilder msg_builder; - kj::ArrayPtr bytes = this->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK); + kj::ArrayPtr bytes = this->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK, filterInitialized); pm.send("liveLocationKalman", bytes.begin(), bytes.size()); if (cnt % 1200 == 0 && gpsOK) { // once a minute diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index 9bc864bf6..f75e33442 100755 --- a/selfdrive/locationd/locationd.h +++ b/selfdrive/locationd/locationd.h @@ -36,7 +36,7 @@ public: void determine_gps_mode(double current_time); kj::ArrayPtr get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime, - bool inputsOK, bool sensorsOK, bool gpsOK); + bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid); void build_live_location(cereal::LiveLocationKalman::Builder& fix); Eigen::VectorXd get_position_geodetic(); diff --git a/selfdrive/locationd/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py index 515bd5943..e804917a0 100755 --- a/selfdrive/locationd/test/test_locationd.py +++ b/selfdrive/locationd/test/test_locationd.py @@ -24,7 +24,7 @@ class TestLocationdLib(unittest.TestCase): def setUp(self): header = '''typedef ...* Localizer_t; Localizer_t localizer_init(); -void localizer_get_message_bytes(Localizer_t localizer, uint64_t logMonoTime, bool inputsOK, bool sensorsOK, bool gpsOK, char *buff, size_t buff_size); +void localizer_get_message_bytes(Localizer_t localizer, uint64_t logMonoTime, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, char *buff, size_t buff_size); void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t size);''' self.ffi = FFI() @@ -40,8 +40,8 @@ void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t bytstr = msg_builder.to_bytes() self.lib.localizer_handle_msg_bytes(self.localizer, self.ffi.from_buffer(bytstr), len(bytstr)) - def localizer_get_msg(self, t=0, inputsOK=True, sensorsOK=True, gpsOK=True): - self.lib.localizer_get_message_bytes(self.localizer, t, inputsOK, sensorsOK, gpsOK, self.ffi.addressof(self.msg_buff, 0), self.buff_size) + def localizer_get_msg(self, t=0, inputsOK=True, sensorsOK=True, gpsOK=True, msgValid=True): + self.lib.localizer_get_message_bytes(self.localizer, t, inputsOK, sensorsOK, gpsOK, msgValid, self.ffi.addressof(self.msg_buff, 0), self.buff_size) return log.Event.from_bytes(self.ffi.buffer(self.msg_buff), nesting_limit=self.buff_size // 8) def test_liblocalizer(self): diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 3226b8614..7135a57ce 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -37aa2e3afedc8364d2bbecc5f1b7ed4efec52e30 \ No newline at end of file +e4a770725fd7240388e1cadf6baf9ee456f65aaf \ No newline at end of file From 7deba690e253dbe1191f3227e8240e6842dd9580 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Tue, 15 Mar 2022 17:55:15 -0700 Subject: [PATCH 2/5] Honda: fix possible controls mismatch on Nidecs (#23973) * Honda: fix possible controls mismatch on Nidecs * bump panda --- panda | 2 +- selfdrive/car/tests/test_models.py | 23 +++++++++++++++++++++-- 2 files changed, 22 insertions(+), 3 deletions(-) diff --git a/panda b/panda index 5a7af82f0..e6722c7f9 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 5a7af82f06bf5f8039df8f58f9b1114f7d3436ee +Subproject commit e6722c7f99b8f7b9f81bb0216edb88cc2e5ad7c0 diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index 09d17462e..0b0a83e98 100755 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -50,6 +50,9 @@ class TestCarModel(unittest.TestCase): @classmethod def setUpClass(cls): + #if cls.car_model != "HONDA RIDGELINE 2017": + # raise unittest.SkipTest + if cls.route is None: if cls.car_model in non_tested_cars: print(f"Skipping tests for {cls.car_model}: missing route") @@ -59,7 +62,12 @@ class TestCarModel(unittest.TestCase): params = Params() params.clear_all() - for seg in [2, 1, 0]: + for seg in (2, 1, 0): + #from tools.lib.route import Route + #from tools.lib.logreader import MultiLogIterator + #r = Route("97bbe58ea225ad1d|2022-03-08--12-54-42") + #lr = MultiLogIterator(r.log_paths()[3:4]) + try: lr = LogReader(get_url(cls.route, seg)) except Exception: @@ -189,6 +197,7 @@ class TestCarModel(unittest.TestCase): self.safety.safety_rx_hook(to_send) self.CI.update(CC, (can_list_to_can_capnp([msg, ]), )) + CS_prev = car.CarState.new_message() checks = defaultdict(lambda: 0) for can in self.can_msgs: CS = self.CI.update(CC, (can.as_builder().to_bytes(), )) @@ -217,11 +226,21 @@ class TestCarModel(unittest.TestCase): checks['brakePressed'] += brake_pressed != self.safety.get_brake_pressed_prev() if self.CP.pcmCruise: - checks['controlsAllowed'] += not CS.cruiseState.enabled and self.safety.get_controls_allowed() + # On most pcmCruise cars, openpilot's state is always tied to the PCM's cruise state. + # On Honda Nidec, we always engage on the rising edge of the PCM cruise state, but + # openpilot brakes to zero even if the min ACC speed is non-zero (i.e. the PCM disengages). + if self.CP.carName == "honda" and self.CP.carFingerprint not in HONDA_BOSCH: + # only the rising edges are expected to match + if CS.cruiseState.enabled and not CS_prev.cruiseState.enabled: + checks['controlsAllowed'] += not self.safety.get_controls_allowed() + else: + checks['controlsAllowed'] += not CS.cruiseState.enabled and self.safety.get_controls_allowed() if self.CP.carName == "honda": checks['mainOn'] += CS.cruiseState.available != self.safety.get_acc_main_on() + CS_prev = CS + # TODO: add flag to toyota safety if self.CP.carFingerprint == TOYOTA.SIENNA and checks['brakePressed'] < 25: checks['brakePressed'] = 0 From de4031c98eac2d7151bb99b787e5d68193eee1b3 Mon Sep 17 00:00:00 2001 From: ZwX1616 Date: Tue, 15 Mar 2022 19:02:21 -0700 Subject: [PATCH 3/5] DM: more precision running on DSP + e2e outputs (#23900) * update cereal * run but not use * log distraction type * regression scaling * clean up naming * add calib buf * add to header * fake model * no calib model * adjust threshs * 018a305f * fix bn * tweak1 * tweak2 * 0ff2/666 * tweak3 * t4 * t5 * fix out of bound * skip when replaying old segments * update ref * fix onnxmodel * get calib * update model replay refs * up ref --- cereal | 2 +- models/dmonitoring_model.current | 4 +- models/dmonitoring_model.onnx | 4 +- models/dmonitoring_model_q.dlc | 4 +- selfdrive/modeld/dmonitoringmodeld.cc | 12 ++- selfdrive/modeld/models/commonmodel.cc | 4 - selfdrive/modeld/models/commonmodel.h | 1 - selfdrive/modeld/models/dmonitoring.cc | 45 +++++---- selfdrive/modeld/models/dmonitoring.h | 10 +- selfdrive/modeld/runners/onnxmodel.cc | 8 ++ selfdrive/modeld/runners/onnxmodel.h | 3 + selfdrive/modeld/runners/runmodel.h | 1 + selfdrive/modeld/runners/snpemodel.cc | 5 + selfdrive/modeld/runners/snpemodel.h | 3 + selfdrive/monitoring/dmonitoringd.py | 5 +- selfdrive/monitoring/driver_monitor.py | 91 +++++++++++++------ selfdrive/monitoring/test_monitoring.py | 7 +- .../process_replay/model_replay_ref_commit | 2 +- selfdrive/test/process_replay/ref_commit | 2 +- 19 files changed, 148 insertions(+), 65 deletions(-) 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 From 7482a6fc4bd59435b4e4003f05ed5928459cb699 Mon Sep 17 00:00:00 2001 From: ZwX1616 Date: Tue, 15 Mar 2022 19:09:17 -0700 Subject: [PATCH 4/5] checkout cereal master --- cereal | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cereal b/cereal index 0dca98677..64b601419 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 0dca986771da39f1a810e58cfb65850be71f1ffb +Subproject commit 64b6014193a69fe2e2b3b7a17f9e228162081ddf From 5fe00fb7738358c19b7db907047353d8785b5f20 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 15 Mar 2022 20:51:07 -0700 Subject: [PATCH 5/5] Move gas/brake pedal event logic to controlsd (#23850) * move some state machine logic to controlsd move some state machine logic to controlsd * same order same order * move unsafeMode up * disable at standstill * save last CS save last CS * do we need to copy at all? * don't copy * rename * Update selfdrive/controls/controlsd.py Co-authored-by: Adeeb Shihadeh * Revert "Update selfdrive/controls/controlsd.py" This reverts commit 10bb51a0f0ee0d8e2a27c746a0638280a5eaea8a. * Update refs Co-authored-by: Adeeb Shihadeh --- selfdrive/car/interfaces.py | 7 ------- selfdrive/controls/controlsd.py | 8 ++++++++ selfdrive/test/process_replay/ref_commit | 2 +- 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index ab8a179bf..04098413a 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -72,7 +72,6 @@ class CarInterfaceBase(ABC): def get_std_params(candidate, fingerprint): ret = car.CarParams.new_message() ret.carFingerprint = candidate - ret.unsafeMode = 0 # see panda/board/safety_declarations.h for allowed values # standard ALC params ret.steerControlType = car.CarParams.SteerControlType.torque @@ -142,7 +141,6 @@ class CarInterfaceBase(ABC): if cs_out.parkingBrake: events.add(EventName.parkBrake) - # Handle permanent and temporary steering faults self.steering_unpressed = 0 if cs_out.steeringPressed else self.steering_unpressed + 1 if cs_out.steerFaultTemporary: @@ -157,11 +155,6 @@ class CarInterfaceBase(ABC): if cs_out.steerFaultPermanent: events.add(EventName.steerUnavailable) - # Disable on rising edge of gas or brake. Also disable on brake when speed > 0. - if (cs_out.gasPressed and not self.CS.out.gasPressed) or \ - (cs_out.brakePressed and (not self.CS.out.brakePressed or not cs_out.standstill)): - events.add(EventName.pedalPressed) - # we engage when pcm is active (rising edge) if pcm_enable: if cs_out.cruiseState.enabled and not self.CS.out.cruiseState.enabled: diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index fcb1a4974..1a911ef98 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -94,6 +94,7 @@ class Controls: get_one_can(self.can_sock) self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan']) + self.CP.unsafeMode = 0 # see panda/board/safety_declarations.h for allowed values # read params self.is_metric = params.get_bool("IsMetric") @@ -119,6 +120,7 @@ class Controls: put_nonblocking("CarParamsCache", cp_bytes) self.CC = car.CarControl.new_message() + self.CS_prev = car.CarState.new_message() self.AM = AlertManager() self.events = Events() @@ -192,6 +194,11 @@ class Controls: self.events.add(EventName.controlsInitializing) return + # Disable on rising edge of gas or brake. Also disable on brake when speed > 0 + if (CS.gasPressed and not self.CS_prev.gasPressed) or \ + (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)): + self.events.add(EventName.pedalPressed) + self.events.add_from_msg(CS.events) self.events.add_from_msg(self.sm['driverMonitoringState'].events) @@ -733,6 +740,7 @@ class Controls: self.prof.checkpoint("Sent") self.update_button_timers(CS.buttonEvents) + self.CS_prev = CS def controlsd_thread(self): while True: diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 419d1adab..f9edf2f88 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -6b5f707e5c58b0162044b2d5a07a35ec00296504 \ No newline at end of file +927918307b45657df0a4ac0255c3e8e3dc62d7cd \ No newline at end of file