diff --git a/selfdrive/assets/img_driver_face.png b/selfdrive/assets/img_driver_face.png index 271c4e4dd0..35d1b8fdc9 100644 --- a/selfdrive/assets/img_driver_face.png +++ b/selfdrive/assets/img_driver_face.png @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:9a9b85f959e3881c65d018486ae2a6c41b25b4586cc61b0caad27d211fb517d9 -size 24112 +oid sha256:eea9c1f018fb5176d38b74b996595f4aa2ded3f286f1989c9227f3ff0e826cc9 +size 19875 diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 7a4b4240c3..33b1ea8e27 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -231,7 +231,7 @@ AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget* par experimental_btn = new ExperimentalButton(this); main_layout->addWidget(experimental_btn, 0, Qt::AlignTop | Qt::AlignRight); - dm_img = loadPixmap("../assets/img_driver_face.png", {img_size, img_size}); + dm_img = loadPixmap("../assets/img_driver_face.png", {img_size + 5, img_size + 5}); } void AnnotatedCameraWidget::updateState(const UIState &s) { @@ -275,7 +275,7 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { setProperty("speed", cur_speed); setProperty("setSpeed", set_speed); setProperty("speedUnit", s.scene.is_metric ? tr("km/h") : tr("mph")); - setProperty("hideDM", cs.getAlertSize() != cereal::ControlsState::AlertSize::NONE); + setProperty("hideDM", (cs.getAlertSize() != cereal::ControlsState::AlertSize::NONE)); setProperty("status", s.status); // update engageability/experimental mode button @@ -286,6 +286,9 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { setProperty("dmActive", sm["driverMonitoringState"].getDriverMonitoringState().getIsActiveMode()); setProperty("rightHandDM", sm["driverMonitoringState"].getDriverMonitoringState().getIsRHD()); } + + // DM icon transition + dm_fade_state = fmax(0.0, fmin(1.0, dm_fade_state+0.2*(0.5-(float)(dmActive)))); } void AnnotatedCameraWidget::drawHud(QPainter &p) { @@ -435,12 +438,6 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { configFont(p, "Inter", 66, "Regular"); drawText(p, rect().center().x(), 290, speedUnit, 200); - // dm icon - if (!hideDM) { - int dm_icon_x = rightHandDM ? rect().right() - btn_size / 2 - (bdr_s * 2) : btn_size / 2 + (bdr_s * 2); - drawIcon(p, dm_icon_x, rect().bottom() - footer_h / 2, - dm_img, blackColor(70), dmActive ? 1.0 : 0.2); - } p.restore(); } @@ -544,6 +541,49 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { painter.restore(); } +void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s) { + const UIScene &scene = s->scene; + + painter.save(); + + // base icon + int x = rightHandDM ? rect().right() - (btn_size - 24) / 2 - (bdr_s * 2) : (btn_size - 24) / 2 + (bdr_s * 2); + int y = rect().bottom() - footer_h / 2; + float opacity = dmActive ? 0.65 : 0.2; + drawIcon(painter, x, y, dm_img, blackColor(0), opacity); + + // circle background + painter.setOpacity(1.0); + painter.setPen(Qt::NoPen); + painter.setBrush(blackColor(70)); + painter.drawEllipse(x - btn_size / 2, y - btn_size / 2, btn_size, btn_size); + + // face + QPointF face_kpts_draw[std::size(default_face_kpts_3d)]; + float kp; + for (int i = 0; i < std::size(default_face_kpts_3d); ++i) { + kp = (scene.face_kpts_draw[i].v[2] - 8) / 120 + 1.0; + face_kpts_draw[i] = QPointF(scene.face_kpts_draw[i].v[0] * kp + x, scene.face_kpts_draw[i].v[1] * kp + y); + } + + painter.setPen(QPen(QColor::fromRgbF(1.0, 1.0, 1.0, opacity), 5.2, Qt::SolidLine, Qt::RoundCap)); + painter.drawPolyline(face_kpts_draw, std::size(default_face_kpts_3d)); + + // tracking arcs + const int arc_l = 133; + const float arc_t_default = 6.7; + const float arc_t_extend = 12.0; + QColor arc_color = QColor::fromRgbF(0.09, 0.945, 0.26, 0.4*(1.0-dm_fade_state)*(s->engaged())); + float delta_x = -scene.driver_pose_sins[1] * arc_l / 2; + float delta_y = -scene.driver_pose_sins[0] * arc_l / 2; + painter.setPen(QPen(arc_color, arc_t_default+arc_t_extend*fmin(1.0, scene.driver_pose_diff[1] * 5.0), Qt::SolidLine, Qt::RoundCap)); + painter.drawArc(QRectF(std::fmin(x + delta_x, x), y - arc_l / 2, fabs(delta_x), arc_l), (scene.driver_pose_sins[1]>0 ? 90 : -90) * 16, 180 * 16); + painter.setPen(QPen(arc_color, arc_t_default+arc_t_extend*fmin(1.0, scene.driver_pose_diff[0] * 5.0), Qt::SolidLine, Qt::RoundCap)); + painter.drawArc(QRectF(x - arc_l / 2, std::fmin(y + delta_y, y), arc_l, fabs(delta_y)), (scene.driver_pose_sins[0]>0 ? 0 : 180) * 16, 180 * 16); + + painter.restore(); +} + void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd) { painter.save(); @@ -654,6 +694,12 @@ void AnnotatedCameraWidget::paintGL() { } } + // DMoji + if (!hideDM && (sm.rcv_frame("driverStateV2") > s->scene.started_frame)) { + update_dmonitoring(s, sm["driverStateV2"].getDriverStateV2(), dm_fade_state, rightHandDM); + drawDriverState(painter, s); + } + drawHud(painter); double cur_draw_t = millis_since_boot(); diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 73c2e03795..73c2a37892 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -80,6 +80,7 @@ private: bool dmActive = false; bool hideDM = false; bool rightHandDM = false; + float dm_fade_state = 1.0; bool has_us_speed_limit = false; bool has_eu_speed_limit = false; bool v_ego_cluster_seen = false; @@ -97,6 +98,7 @@ protected: void drawLaneLines(QPainter &painter, const UIState *s); void drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd); void drawHud(QPainter &p); + void drawDriverState(QPainter &painter, const UIState *s); inline QColor redColor(int alpha = 255) { return QColor(201, 34, 49, alpha); } inline QColor whiteColor(int alpha = 255) { return QColor(255, 255, 255, alpha); } inline QColor blackColor(int alpha = 255) { return QColor(0, 0, 0, alpha); } diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 4df963167a..6c850b8ca4 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -112,6 +112,39 @@ void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) { update_line_data(s, model_position, 0.9, 1.22, &scene.track_vertices, max_idx, false); } +void update_dmonitoring(UIState *s, const cereal::DriverStateV2::Reader &driverstate, float dm_fade_state, bool is_rhd) { + UIScene &scene = s->scene; + const auto driver_orient = is_rhd ? driverstate.getRightDriverData().getFaceOrientation() : driverstate.getLeftDriverData().getFaceOrientation(); + for (int i = 0; i < std::size(scene.driver_pose_vals); i++) { + float v_this = (i == 0 ? (driver_orient[i] < 0 ? 0.7 : 0.9) : 0.4) * driver_orient[i]; + scene.driver_pose_diff[i] = fabs(scene.driver_pose_vals[i] - v_this); + scene.driver_pose_vals[i] = 0.8 * v_this + (1 - 0.8) * scene.driver_pose_vals[i]; + scene.driver_pose_sins[i] = sinf(scene.driver_pose_vals[i]*(1.0-dm_fade_state)); + scene.driver_pose_coss[i] = cosf(scene.driver_pose_vals[i]*(1.0-dm_fade_state)); + } + + const mat3 r_xyz = (mat3){{ + scene.driver_pose_coss[1]*scene.driver_pose_coss[2], + scene.driver_pose_coss[1]*scene.driver_pose_sins[2], + -scene.driver_pose_sins[1], + + -scene.driver_pose_sins[0]*scene.driver_pose_sins[1]*scene.driver_pose_coss[2] - scene.driver_pose_coss[0]*scene.driver_pose_sins[2], + -scene.driver_pose_sins[0]*scene.driver_pose_sins[1]*scene.driver_pose_sins[2] + scene.driver_pose_coss[0]*scene.driver_pose_coss[2], + -scene.driver_pose_sins[0]*scene.driver_pose_coss[1], + + scene.driver_pose_coss[0]*scene.driver_pose_sins[1]*scene.driver_pose_coss[2] - scene.driver_pose_sins[0]*scene.driver_pose_sins[2], + scene.driver_pose_coss[0]*scene.driver_pose_sins[1]*scene.driver_pose_sins[2] + scene.driver_pose_sins[0]*scene.driver_pose_coss[2], + scene.driver_pose_coss[0]*scene.driver_pose_coss[1], + }}; + + // transform vertices + for (int kpi = 0; kpi < std::size(default_face_kpts_3d); kpi++) { + vec3 kpt_this = default_face_kpts_3d[kpi]; + kpt_this = matvecmul3(r_xyz, kpt_this); + scene.face_kpts_draw[kpi] = (vec3){{(float)kpt_this.v[0], (float)kpt_this.v[1], (float)(kpt_this.v[2] * (1.0-dm_fade_state) + 8 * dm_fade_state)}}; + } +} + static void update_sockets(UIState *s) { s->sm->update(0); } @@ -213,7 +246,7 @@ void UIState::updateStatus() { UIState::UIState(QObject *parent) : QObject(parent) { sm = std::make_unique>({ "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "roadCameraState", - "pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", + "pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "driverStateV2", "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "gnssMeasurements", }); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 9e1c54948b..e3eb97a762 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -25,6 +25,16 @@ typedef cereal::CarControl::HUDControl::AudibleAlert AudibleAlert; const mat3 DEFAULT_CALIBRATION = {{ 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0 }}; +const vec3 default_face_kpts_3d[] = { + {-5.98, -51.20, 8.00}, {-17.64, -49.14, 8.00}, {-23.81, -46.40, 8.00}, {-29.98, -40.91, 8.00}, {-32.04, -37.49, 8.00}, + {-34.10, -32.00, 8.00}, {-36.16, -21.03, 8.00}, {-36.16, 6.40, 8.00}, {-35.47, 10.51, 8.00}, {-32.73, 19.43, 8.00}, + {-29.30, 26.29, 8.00}, {-24.50, 33.83, 8.00}, {-19.01, 41.37, 8.00}, {-14.21, 46.17, 8.00}, {-12.16, 47.54, 8.00}, + {-4.61, 49.60, 8.00}, {4.99, 49.60, 8.00}, {12.53, 47.54, 8.00}, {14.59, 46.17, 8.00}, {19.39, 41.37, 8.00}, + {24.87, 33.83, 8.00}, {29.67, 26.29, 8.00}, {33.10, 19.43, 8.00}, {35.84, 10.51, 8.00}, {36.53, 6.40, 8.00}, + {36.53, -21.03, 8.00}, {34.47, -32.00, 8.00}, {32.42, -37.49, 8.00}, {30.36, -40.91, 8.00}, {24.19, -46.40, 8.00}, + {18.02, -49.14, 8.00}, {6.36, -51.20, 8.00}, {-5.98, -51.20, 8.00}, +}; + struct Alert { QString text1; QString text2; @@ -103,6 +113,13 @@ typedef struct UIScene { // lead QPointF lead_vertices[2]; + // DMoji state + float driver_pose_vals[3]; + float driver_pose_diff[3]; + float driver_pose_sins[3]; + float driver_pose_coss[3]; + vec3 face_kpts_draw[std::size(default_face_kpts_3d)]; + float light_sensor; bool started, ignition, is_metric, map_on_left, longitudinal_control; uint64_t started_frame; @@ -183,6 +200,7 @@ public slots: void ui_update_params(UIState *s); int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line, const float path_height); void update_model(UIState *s, const cereal::ModelDataV2::Reader &model); +void update_dmonitoring(UIState *s, const cereal::DriverStateV2::Reader &driverstate, float dm_fade_state, bool is_rhd); void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, const cereal::ModelDataV2::XYZTData::Reader &line); void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line, float y_off, float z_off, QPolygonF *pvd, int max_idx, bool allow_invert);