From e4567c3cf85c536d57d8e9a51f09b5369e691c1c Mon Sep 17 00:00:00 2001 From: Comma Device Date: Tue, 7 Jun 2022 13:58:08 -0700 Subject: [PATCH] v2 packets --- selfdrive/camerad/cameras/camera_qcom2.cc | 2 +- selfdrive/modeld/dmonitoringmodeld.cc | 2 +- selfdrive/modeld/models/dmonitoring.cc | 18 +++++++++--------- selfdrive/modeld/models/dmonitoring.h | 4 ++-- selfdrive/monitoring/dmonitoringd.py | 6 +++--- selfdrive/monitoring/driver_monitor.py | 4 ++-- selfdrive/test/process_replay/model_replay.py | 8 ++++---- .../test/process_replay/process_replay.py | 2 +- selfdrive/test/test_onroad.py | 4 ++-- selfdrive/ui/qt/offroad/driverview.cc | 10 +++++----- 10 files changed, 30 insertions(+), 30 deletions(-) diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc index d43beb0921..b949dfb80f 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.cc +++ b/selfdrive/camerad/cameras/camera_qcom2.cc @@ -837,7 +837,7 @@ void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_i s->road_cam.camera_init(s, v, CAMERA_ID_AR0231, 1, 20, device_id, ctx, VISION_STREAM_RGB_ROAD, VISION_STREAM_ROAD, !env_disable_road); s->wide_road_cam.camera_init(s, v, CAMERA_ID_AR0231, 0, 20, device_id, ctx, VISION_STREAM_RGB_WIDE_ROAD, VISION_STREAM_WIDE_ROAD, !env_disable_wide_road); - s->sm = new SubMaster({"driverState"}); + s->sm = new SubMaster({"driverStateV2"}); s->pm = new PubMaster({"roadCameraState", "driverCameraState", "wideRoadCameraState", "thumbnail"}); } diff --git a/selfdrive/modeld/dmonitoringmodeld.cc b/selfdrive/modeld/dmonitoringmodeld.cc index d0c33baf4e..cde13a9bee 100644 --- a/selfdrive/modeld/dmonitoringmodeld.cc +++ b/selfdrive/modeld/dmonitoringmodeld.cc @@ -12,7 +12,7 @@ ExitHandler do_exit; void run_model(DMonitoringModelState &model, VisionIpcClient &vipc_client) { - PubMaster pm({"driverState"}); + PubMaster pm({"driverStateV2"}); SubMaster sm({"liveCalibration"}); float calib[CALIB_LEN] = {0}; double last = 0; diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc index e213c0ceb0..140197178c 100644 --- a/selfdrive/modeld/models/dmonitoring.cc +++ b/selfdrive/modeld/models/dmonitoring.cc @@ -55,7 +55,7 @@ void parse_driver_data(DriverStateResult &ds_res, const DMonitoringModelState* s ds_res.occluded_prob = sigmoid(s->output[out_idx_offset+34]); } -void fill_driver_data(cereal::DriverState::DriverData::Builder ddata, const DriverStateResult &ds_res) { +void fill_driver_data(cereal::DriverStateV2::DriverData::Builder ddata, const DriverStateResult &ds_res) { ddata.setFaceOrientation(ds_res.face_orientation); ddata.setFaceOrientationStd(ds_res.face_orientation_std); ddata.setFacePosition(ds_res.face_position); @@ -110,8 +110,8 @@ DMonitoringModelResult dmonitoring_eval_frame(DMonitoringModelState* s, void* st DMonitoringModelResult model_res = {0}; parse_driver_data(model_res.driver_state_lhd, s, 0); parse_driver_data(model_res.driver_state_rhd, s, 41); - model_res.poor_vision = sigmoid(s->output[82]); - model_res.wheel_on_right = sigmoid(s->output[83]); + model_res.poor_vision_prob = sigmoid(s->output[82]); + model_res.wheel_on_right_prob = sigmoid(s->output[83]); model_res.dsp_execution_time = (t2 - t1) / 1000.; return model_res; @@ -120,21 +120,21 @@ DMonitoringModelResult dmonitoring_eval_frame(DMonitoringModelState* s, void* st void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringModelResult &model_res, float execution_time, kj::ArrayPtr raw_pred) { // make msg MessageBuilder msg; - auto framed = msg.initEvent().initDriverState(); + auto framed = msg.initEvent().initDriverStateV2(); framed.setFrameId(frame_id); framed.setModelExecutionTime(execution_time); framed.setDspExecutionTime(model_res.dsp_execution_time); - framed.setPoorVision(model_res.poor_vision); - framed.setWheelOnRight(model_res.wheel_on_right); - fill_driver_data(framed.initDriverDataLH(), model_res.driver_state_lhd); - fill_driver_data(framed.initDriverDataRH(), model_res.driver_state_rhd); + framed.setPoorVisionProb(model_res.poor_vision_prob); + framed.setWheelOnRightProb(model_res.wheel_on_right_prob); + fill_driver_data(framed.initLeftDriverData(), model_res.driver_state_lhd); + fill_driver_data(framed.initRightDriverData(), model_res.driver_state_rhd); if (send_raw_pred) { framed.setRawPredictions(raw_pred.asBytes()); } - pm.send("driverState", msg); + pm.send("driverStateV2", msg); } void dmonitoring_free(DMonitoringModelState* s) { diff --git a/selfdrive/modeld/models/dmonitoring.h b/selfdrive/modeld/models/dmonitoring.h index 1a6e825e9b..b6a2eee9c4 100644 --- a/selfdrive/modeld/models/dmonitoring.h +++ b/selfdrive/modeld/models/dmonitoring.h @@ -31,8 +31,8 @@ typedef struct DriverStateResult { typedef struct DMonitoringModelResult { DriverStateResult driver_state_lhd; DriverStateResult driver_state_rhd; - float poor_vision; - float wheel_on_right; + float poor_vision_prob; + float wheel_on_right_prob; float dsp_execution_time; } DMonitoringModelResult; diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index 6c41daeefb..c31e9da43b 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -18,7 +18,7 @@ def dmonitoringd_thread(sm=None, pm=None): pm = messaging.PubMaster(['driverMonitoringState']) if sm is None: - sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll=['driverState']) + sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll=['driverStateV2']) driver_status = DriverStatus(rhd=Params().get_bool("IsRHD")) @@ -34,7 +34,7 @@ def dmonitoringd_thread(sm=None, pm=None): while True: sm.update() - if not sm.updated['driverState']: + if not sm.updated['driverStateV2']: continue # Get interaction @@ -51,7 +51,7 @@ def dmonitoringd_thread(sm=None, pm=None): # Get data from dmonitoringmodeld events = Events() - driver_status.update_states(sm['driverState'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled) + driver_status.update_states(sm['driverStateV2'], 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 \ diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py index 492ecd838e..309996ad3f 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/selfdrive/monitoring/driver_monitor.py @@ -225,14 +225,14 @@ 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 + # rhd_pred = driver_state.wheelOnRightProb # 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 + driver_data = driver_state.rightDriverData if self.wheel_on_right else driver_state.leftDriverData 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)): diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py index 34f35e0ed7..630752d81e 100755 --- a/selfdrive/test/process_replay/model_replay.py +++ b/selfdrive/test/process_replay/model_replay.py @@ -50,7 +50,7 @@ def model_replay(lr, frs): vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, *(tici_f_frame_size)) vipc_server.start_listener() - sm = messaging.SubMaster(['modelV2', 'driverState']) + sm = messaging.SubMaster(['modelV2', 'driverStateV2']) pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'driverCameraState', 'liveCalibration', 'lateralPlan']) try: @@ -110,7 +110,7 @@ def model_replay(lr, frs): if not TICI or min(frame_idxs['roadCameraState'], frame_idxs['wideRoadCameraState']) > recv_cnt['modelV2']: recv = "modelV2" elif msg.which() == 'driverCameraState': - recv = "driverState" + recv = "driverStateV2" # wait for a response with Timeout(15, f"timed out waiting for {recv}"): @@ -164,8 +164,8 @@ if __name__ == "__main__": 'logMonoTime', 'modelV2.frameDropPerc', 'modelV2.modelExecutionTime', - 'driverState.modelExecutionTime', - 'driverState.dspExecutionTime' + 'driverStateV2.modelExecutionTime', + 'driverStateV2.dspExecutionTime' ] tolerance = None if not PC else 1e-3 results: Any = {TEST_ROUTE: {}} diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 1eda9bc7f5..89885ef397 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -293,7 +293,7 @@ CONFIGS = [ ProcessConfig( proc_name="dmonitoringd", pub_sub={ - "driverState": ["driverMonitoringState"], + "driverStateV2": ["driverMonitoringState"], "liveCalibration": [], "carState": [], "modelV2": [], "controlsState": [], }, ignore=["logMonoTime", "valid"], diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 6cf53a046e..0cf55e3318 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -60,7 +60,7 @@ TIMINGS = { "roadCameraState": [2.5, 0.35], "driverCameraState": [2.5, 0.35], "modelV2": [2.5, 0.35], - "driverState": [2.5, 0.40], + "driverStateV2": [2.5, 0.40], "liveLocationKalman": [2.5, 0.35], "wideRoadCameraState": [1.5, 0.35], } @@ -221,7 +221,7 @@ class TestOnroad(unittest.TestCase): # TODO: this went up when plannerd cpu usage increased, why? cfgs = [ ("modelV2", 0.050, 0.036), - ("driverState", 0.050, 0.026), + ("driverStateV2", 0.050, 0.026), ] for (s, instant_max, avg_max) in cfgs: ts = [getattr(getattr(m, s), "modelExecutionTime") for m in self.lr if m.which() == s] diff --git a/selfdrive/ui/qt/offroad/driverview.cc b/selfdrive/ui/qt/offroad/driverview.cc index 6921c34d9d..5d518d93df 100644 --- a/selfdrive/ui/qt/offroad/driverview.cc +++ b/selfdrive/ui/qt/offroad/driverview.cc @@ -25,7 +25,7 @@ void DriverViewWindow::mouseReleaseEvent(QMouseEvent* e) { emit done(); } -DriverViewScene::DriverViewScene(QWidget* parent) : sm({"driverState"}), QWidget(parent) { +DriverViewScene::DriverViewScene(QWidget* parent) : sm({"driverStateV2"}), QWidget(parent) { face_img = loadPixmap("../assets/img_driver_face.png", {FACE_IMG_SIZE, FACE_IMG_SIZE}); } @@ -56,11 +56,11 @@ void DriverViewScene::paintEvent(QPaintEvent* event) { return; } - cereal::DriverState::Reader driver_state = sm["driverState"].getDriverState(); - cereal::DriverState::DriverData::Reader driver_data; + cereal::DriverStateV2::Reader driver_state = sm["driverStateV2"].getDriverStateV2(); + cereal::DriverStateV2::DriverData::Reader driver_data; - is_rhd = driver_state.getWheelOnRight() > 0.5; - driver_data = is_rhd ? driver_state.getDriverDataRH() : driver_state.getDriverDataLH(); + is_rhd = driver_state.getWheelOnRightProb() > 0.5; + driver_data = is_rhd ? driver_state.getRightDriverData() : driver_state.getLeftDriverData(); bool face_detected = driver_data.getFaceProb() > 0.5; if (face_detected) {