diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index cfdaf87b7d1485..319ca4547c670f 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -31,7 +31,7 @@ qt_src = ["main.cc", "qt/sidebar.cc", "qt/body.cc", "qt/offroad/software_settings.cc", "qt/offroad/onboarding.cc", "qt/offroad/driverview.cc", "qt/offroad/experimental_mode.cc", "qt/onroad/onroad_home.cc", "qt/onroad/annotated_camera.cc", - "qt/onroad/buttons.cc", "qt/onroad/alerts.cc"] + "qt/onroad/buttons.cc", "qt/onroad/alerts.cc", "qt/onroad/driver_monitoring.cc"] # build translation files with open(File("translations/languages.json").abspath) as f: diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index 3eb74c9bfcc0df..4a950068386c40 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -19,8 +19,6 @@ 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 + 5, img_size + 5}); } void AnnotatedCameraWidget::updateState(const UIState &s) { @@ -48,18 +46,13 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { speed *= is_metric ? MS_TO_KPH : MS_TO_MPH; speedUnit = is_metric ? tr("km/h") : tr("mph"); - hideBottomIcons = (sm["selfdriveState"].getSelfdriveState().getAlertSize() != cereal::SelfdriveState::AlertSize::NONE); status = s.status; // update engageability/experimental mode button experimental_btn->updateState(s); // update DM icon - auto dm_state = sm["driverMonitoringState"].getDriverMonitoringState(); - dmActive = dm_state.getIsActiveMode(); - rightHandDM = dm_state.getIsRHD(); - // DM icon transition - dm_fade_state = std::clamp(dm_fade_state+0.2*(0.5-dmActive), 0.0, 1.0); + dmon.updateState(s); } void AnnotatedCameraWidget::drawHud(QPainter &p) { @@ -209,47 +202,6 @@ 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 offset = UI_BORDER_SIZE + btn_size / 2; - int x = rightHandDM ? width() - offset : offset; - int y = height() - offset; - float opacity = dmActive ? 0.65 : 0.2; - drawIcon(painter, QPoint(x, y), dm_img, blackColor(70), opacity); - - // 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.545 - 0.445 * s->engaged(), - 0.545 + 0.4 * s->engaged(), - 0.545 - 0.285 * s->engaged(), - 0.4 * (1.0 - dm_fade_state)); - 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(); @@ -357,11 +309,7 @@ void AnnotatedCameraWidget::paintGL() { } } - // DMoji - if (!hideBottomIcons && (sm.rcv_frame("driverStateV2") > s->scene.started_frame)) { - update_dmonitoring(s, sm["driverStateV2"].getDriverStateV2(), dm_fade_state, rightHandDM); - drawDriverState(painter, s); - } + dmon.draw(painter, rect()); drawHud(painter); diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index 465ba23e049636..4a4196b22ce028 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -4,6 +4,7 @@ #include #include "selfdrive/ui/qt/onroad/buttons.h" +#include "selfdrive/ui/qt/onroad/driver_monitoring.h" #include "selfdrive/ui/qt/widgets/cameraview.h" class AnnotatedCameraWidget : public CameraWidget { @@ -18,16 +19,12 @@ class AnnotatedCameraWidget : public CameraWidget { QVBoxLayout *main_layout; ExperimentalButton *experimental_btn; - QPixmap dm_img; + DriverMonitorRenderer dmon; float speed; QString speedUnit; float setSpeed; bool is_cruise_set = false; bool is_metric = false; - bool dmActive = false; - bool hideBottomIcons = false; - bool rightHandDM = false; - float dm_fade_state = 1.0; bool v_ego_cluster_seen = false; int status = STATUS_DISENGAGED; std::unique_ptr pm; @@ -43,7 +40,6 @@ class AnnotatedCameraWidget : public CameraWidget { 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/qt/onroad/driver_monitoring.cc b/selfdrive/ui/qt/onroad/driver_monitoring.cc new file mode 100644 index 00000000000000..afd003cf8f412a --- /dev/null +++ b/selfdrive/ui/qt/onroad/driver_monitoring.cc @@ -0,0 +1,107 @@ +#include "selfdrive/ui/qt/onroad/driver_monitoring.h" +#include +#include + +#include "selfdrive/ui/qt/onroad/buttons.h" +#include "selfdrive/ui/qt/util.h" + +// Default 3D coordinates for face keypoints +static constexpr 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}, +}; + +// Colors used for drawing based on monitoring state +static const QColor DMON_ENGAGED_COLOR = QColor::fromRgbF(0.1, 0.945, 0.26); +static const QColor DMON_DISENGAGED_COLOR = QColor::fromRgbF(0.545, 0.545, 0.545); + +DriverMonitorRenderer::DriverMonitorRenderer() : face_kpts_draw(std::size(DEFAULT_FACE_KPTS_3D)) { + dm_img = loadPixmap("../assets/img_driver_face.png", {img_size + 5, img_size + 5}); +} + +void DriverMonitorRenderer::updateState(const UIState &s) { + auto &sm = *(s.sm); + is_visible = sm["selfdriveState"].getSelfdriveState().getAlertSize() == cereal::SelfdriveState::AlertSize::NONE && + sm.rcv_frame("driverStateV2") > s.scene.started_frame; + if (!is_visible) return; + + auto dm_state = sm["driverMonitoringState"].getDriverMonitoringState(); + is_active = dm_state.getIsActiveMode(); + is_rhd = dm_state.getIsRHD(); + dm_fade_state = std::clamp(dm_fade_state + 0.2f * (0.5f - is_active), 0.0f, 1.0f); + + const auto &driverstate = sm["driverStateV2"].getDriverStateV2(); + const auto driver_orient = is_rhd ? driverstate.getRightDriverData().getFaceOrientation() : driverstate.getLeftDriverData().getFaceOrientation(); + + for (int i = 0; i < 3; ++i) { + float v_this = (i == 0 ? (driver_orient[i] < 0 ? 0.7 : 0.9) : 0.4) * driver_orient[i]; + driver_pose_diff[i] = std::abs(driver_pose_vals[i] - v_this); + driver_pose_vals[i] = 0.8f * v_this + (1 - 0.8) * driver_pose_vals[i]; + driver_pose_sins[i] = std::sin(driver_pose_vals[i] * (1.0f - dm_fade_state)); + driver_pose_coss[i] = std::cos(driver_pose_vals[i] * (1.0f - dm_fade_state)); + } + + auto [sin_y, sin_x, sin_z] = driver_pose_sins; + auto [cos_y, cos_x, cos_z] = driver_pose_coss; + + // Rotation matrix for transforming face keypoints based on driver's head orientation + const mat3 r_xyz = {{ + cos_x * cos_z, cos_x * sin_z, -sin_x, + -sin_y * sin_x * cos_z - cos_y * sin_z, -sin_y * sin_x * sin_z + cos_y * cos_z, -sin_y * cos_x, + cos_y * sin_x * cos_z - sin_y * sin_z, cos_y * sin_x * sin_z + sin_y * cos_z, cos_y * cos_x, + }}; + + // Transform vertices + for (int i = 0; i < face_kpts_draw.size(); ++i) { + vec3 kpt = matvecmul3(r_xyz, DEFAULT_FACE_KPTS_3D[i]); + face_kpts_draw[i] = {{kpt.v[0], kpt.v[1], kpt.v[2] * (1.0f - dm_fade_state) + 8 * dm_fade_state}}; + } +} + +void DriverMonitorRenderer::draw(QPainter &painter, const QRect &surface_rect) { + if (!is_visible) return; + + painter.save(); + + int offset = UI_BORDER_SIZE + btn_size / 2; + float x = is_rhd ? surface_rect.width() - offset : offset; + float y = surface_rect.height() - offset; + float opacity = is_active ? 0.65f : 0.2f; + + drawIcon(painter, QPoint(x, y), dm_img, QColor(0, 0, 0, 70), opacity); + + QPointF keypoints[std::size(DEFAULT_FACE_KPTS_3D)]; + for (int i = 0; i < std::size(keypoints); ++i) { + const auto &v = face_kpts_draw[i].v; + float kp = (v[2] - 8) / 120.0f + 1.0f; + keypoints[i] = QPointF(v[0] * kp + x, v[1] * kp + y); + } + + painter.setPen(QPen(QColor::fromRgbF(1.0, 1.0, 1.0, opacity), 5.2, Qt::SolidLine, Qt::RoundCap)); + painter.drawPolyline(keypoints, std::size(keypoints)); + + // tracking arcs + const int arc_l = 133; + const float arc_t_default = 6.7f; + const float arc_t_extend = 12.0f; + QColor arc_color = uiState()->engaged() ? DMON_ENGAGED_COLOR : DMON_DISENGAGED_COLOR; + arc_color.setAlphaF(0.4 * (1.0f - dm_fade_state)); + + float delta_x = -driver_pose_sins[1] * arc_l / 2.0f; + float delta_y = -driver_pose_sins[0] * arc_l / 2.0f; + + // Draw horizontal tracking arc + painter.setPen(QPen(arc_color, arc_t_default + arc_t_extend * std::min(1.0, driver_pose_diff[1] * 5.0), Qt::SolidLine, Qt::RoundCap)); + painter.drawArc(QRectF(std::min(x + delta_x, x), y - arc_l / 2, std::abs(delta_x), arc_l), (driver_pose_sins[1] > 0 ? 90 : -90) * 16, 180 * 16); + + // Draw vertical tracking arc + painter.setPen(QPen(arc_color, arc_t_default + arc_t_extend * std::min(1.0, driver_pose_diff[0] * 5.0), Qt::SolidLine, Qt::RoundCap)); + painter.drawArc(QRectF(x - arc_l / 2, std::min(y + delta_y, y), arc_l, std::abs(delta_y)), (driver_pose_sins[0] > 0 ? 0 : 180) * 16, 180 * 16); + + painter.restore(); +} diff --git a/selfdrive/ui/qt/onroad/driver_monitoring.h b/selfdrive/ui/qt/onroad/driver_monitoring.h new file mode 100644 index 00000000000000..47db151c81b136 --- /dev/null +++ b/selfdrive/ui/qt/onroad/driver_monitoring.h @@ -0,0 +1,24 @@ +#pragma once + +#include +#include +#include "selfdrive/ui/ui.h" + +class DriverMonitorRenderer { +public: + DriverMonitorRenderer(); + void updateState(const UIState &s); + void draw(QPainter &painter, const QRect &surface_rect); + +private: + float driver_pose_vals[3] = {}; + float driver_pose_diff[3] = {}; + float driver_pose_sins[3] = {}; + float driver_pose_coss[3] = {}; + bool is_visible = false; + bool is_active = false; + bool is_rhd = false; + float dm_fade_state = 1.0; + QPixmap dm_img; + std::vector face_kpts_draw; +}; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 8613eb2f2ff2fe..1a3198c936a0f5 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -110,41 +110,6 @@ void update_model(UIState *s, 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)); - } - - auto [sin_y, sin_x, sin_z] = scene.driver_pose_sins; - auto [cos_y, cos_x, cos_z] = scene.driver_pose_coss; - - const mat3 r_xyz = (mat3){{ - cos_x * cos_z, - cos_x * sin_z, - -sin_x, - - -sin_y * sin_x * cos_z - cos_y * sin_z, - -sin_y * sin_x * sin_z + cos_y * cos_z, - -sin_y * cos_x, - - cos_y * sin_x * cos_z - sin_y * sin_z, - cos_y * sin_x * sin_z + sin_y * cos_z, - cos_y * cos_x, - }}; - - // transform vertices - for (int kpi = 0; kpi < std::size(default_face_kpts_3d); kpi++) { - vec3 kpt_this = matvecmul3(r_xyz, default_face_kpts_3d[kpi]); - scene.face_kpts_draw[kpi] = (vec3){{kpt_this.v[0], 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); } diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 2e3bd3927f8460..73b8c5ddeb6012 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -34,18 +34,6 @@ constexpr mat3 ECAM_INTRINSIC_MATRIX = (mat3){{567.0, 0.0, 1928.0 / 2, 0.0, 567.0, 1208.0 / 2, 0.0, 0.0, 1.0}}; - -constexpr 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}, -}; - - typedef enum UIStatus { STATUS_DISENGAGED, STATUS_OVERRIDE, @@ -88,13 +76,6 @@ 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)]; - cereal::LongitudinalPersonality personality; float light_sensor = -1;