From c55f40e77d90f2128b8d2fcf14e08c2728339021 Mon Sep 17 00:00:00 2001 From: Kumar <36933347+rav4kumar@users.noreply.github.com> Date: Fri, 19 Sep 2025 22:01:08 -0700 Subject: [PATCH] ui: Speed Limit Information (#995) * less for now * always on and fix conv * update api * missed * fix convert * only draw when decreasing, seems like a bug from mapd * use frame check, otherwise too jumpy * only update when mapd updates * count up and down * introduce toggle * more ui * slightly wider * desc * rename * slight cleanup * fix offset position * more vienna fix * fix ahead alignment * road name in another PR * cleanup * single place * adjust ahead distance display * cleanup * Near * bump it * cleanup logic --------- Co-authored-by: Jason Wen --- common/params_keys.h | 1 + common/util.h | 1 + .../longitudinal/speed_limit/helpers.h | 10 + .../speed_limit/speed_limit_settings.cc | 37 ++- .../speed_limit/speed_limit_settings.h | 16 ++ selfdrive/ui/sunnypilot/qt/onroad/hud.cc | 213 +++++++++++++++++- selfdrive/ui/sunnypilot/qt/onroad/hud.h | 11 + selfdrive/ui/sunnypilot/ui.cc | 3 +- selfdrive/ui/sunnypilot/ui_scene.h | 1 + 9 files changed, 281 insertions(+), 12 deletions(-) diff --git a/common/params_keys.h b/common/params_keys.h index a9408037a0..6d60e2cfdd 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -227,6 +227,7 @@ inline static std::unordered_map keys = { {"RoadName", {CLEAR_ON_ONROAD_TRANSITION, STRING}}, // Speed Limit + {"SpeedLimitMode", {PERSISTENT | BACKUP, INT, "1"}}, {"SpeedLimitOffsetType", {PERSISTENT | BACKUP, INT, "0"}}, {"SpeedLimitPolicy", {PERSISTENT | BACKUP, INT, "3"}}, {"SpeedLimitValueOffset", {PERSISTENT | BACKUP, INT, "0"}}, diff --git a/common/util.h b/common/util.h index f46db4d9fa..6d28e3ccbc 100644 --- a/common/util.h +++ b/common/util.h @@ -36,6 +36,7 @@ const double MS_TO_KPH = 3.6; const double MS_TO_MPH = MS_TO_KPH * KM_TO_MILE; const double METER_TO_MILE = KM_TO_MILE / 1000.0; const double METER_TO_FOOT = 3.28084; +const double METER_TO_KM = 1. / 1000.0; #define ALIGNED_SIZE(x, align) (((x) + (align)-1) & ~((align)-1)) diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h index 84979a53af..55764372c4 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h @@ -33,3 +33,13 @@ inline const QString SpeedLimitSourcePolicyTexts[]{ QObject::tr("Map\nFirst"), QObject::tr("Combined\nData") }; + +enum class SpeedLimitMode { + OFF, + INFORMATION, +}; + +inline const QString SpeedLimitModeTexts[]{ + QObject::tr("Off"), + QObject::tr("Information"), +}; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc index 7508ea134c..2bacbace7c 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc @@ -20,16 +20,26 @@ SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent) subPanelLayout->addSpacing(20); - ListWidgetSP *list = new ListWidgetSP(this); - - auto *speedLimitBtnFrame = new QFrame(this); - auto *speedLimitBtnFrameLayout = new QGridLayout(); - speedLimitBtnFrame->setLayout(speedLimitBtnFrameLayout); - speedLimitBtnFrameLayout->setContentsMargins(0, 40, 0, 40); - speedLimitBtnFrameLayout->setSpacing(0); + ListWidgetSP *list = new ListWidgetSP(this, false); speedLimitPolicyScreen = new SpeedLimitPolicy(this); + std::vector speed_limit_mode_texts{ + SpeedLimitModeTexts[static_cast(SpeedLimitMode::OFF)], + SpeedLimitModeTexts[static_cast(SpeedLimitMode::INFORMATION)], + }; + speed_limit_mode_settings = new ButtonParamControlSP( + "SpeedLimitMode", + tr("Speed Limit Mode"), + "", + "", + speed_limit_mode_texts, + 385); + list->addItem(speed_limit_mode_settings); + + list->addItem(horizontal_line()); + list->addItem(vertical_space()); + speedLimitSource = new PushButtonSP(tr("Customize Source")); connect(speedLimitSource, &QPushButton::clicked, [&]() { setCurrentWidget(speedLimitPolicyScreen); @@ -41,8 +51,10 @@ SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent) }); speedLimitSource->setFixedWidth(720); - speedLimitBtnFrameLayout->addWidget(speedLimitSource, 0, 0, Qt::AlignLeft); - list->addItem(speedLimitBtnFrame); + list->addItem(speedLimitSource); + + list->addItem(vertical_space(0)); + list->addItem(horizontal_line()); QFrame *offsetFrame = new QFrame(this); QVBoxLayout *offsetLayout = new QVBoxLayout(offsetFrame); @@ -73,6 +85,7 @@ SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent) list->addItem(offsetFrame); + connect(speed_limit_mode_settings, &ButtonParamControlSP::buttonClicked, this, &SpeedLimitSettings::refresh); connect(speed_limit_offset, &OptionControlSP::updateLabels, this, &SpeedLimitSettings::refresh); connect(speed_limit_offset_settings, &ButtonParamControlSP::showDescriptionEvent, speed_limit_offset, &OptionControlSP::showDescription); connect(speed_limit_offset_settings, &ButtonParamControlSP::buttonClicked, this, &SpeedLimitSettings::refresh); @@ -86,9 +99,11 @@ SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent) void SpeedLimitSettings::refresh() { bool is_metric_param = params.getBool("IsMetric"); + SpeedLimitMode speed_limit_mode_param = static_cast(std::atoi(params.get("SpeedLimitMode").c_str())); SpeedLimitOffsetType offset_type_param = static_cast(std::atoi(params.get("SpeedLimitOffsetType").c_str())); QString offsetLabel = QString::fromStdString(params.get("SpeedLimitValueOffset")); + speed_limit_mode_settings->setDescription(modeDescription(speed_limit_mode_param)); speed_limit_offset->setDescription(offsetDescription(offset_type_param)); if (offset_type_param == SpeedLimitOffsetType::PERCENT) { @@ -104,9 +119,11 @@ void SpeedLimitSettings::refresh() { speed_limit_offset->setLabel(offsetLabel); speed_limit_offset->showDescription(); } + + speed_limit_mode_settings->showDescription(); + speed_limit_offset->showDescription(); } void SpeedLimitSettings::showEvent(QShowEvent *event) { refresh(); - speed_limit_offset->showDescription(); } diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h index 40ffead700..ebffcf65e4 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h @@ -27,6 +27,7 @@ signals: private: Params params; QFrame *subPanelFrame; + ButtonParamControlSP *speed_limit_mode_settings; PushButtonSP *speedLimitSource; SpeedLimitPolicy *speedLimitPolicyScreen; ButtonParamControlSP *speed_limit_offset_settings; @@ -50,4 +51,19 @@ private: .arg(fixed_str) .arg(percent_str); } + + static QString modeDescription(SpeedLimitMode mode = SpeedLimitMode::OFF) { + QString off_str = tr("⦿ Off: Disables the Speed Limit functions."); + QString info_str = tr("⦿ Information: Displays the current road's speed limit."); + + if (mode == SpeedLimitMode::INFORMATION) { + info_str = "" + info_str + ""; + } else { + off_str = "" + off_str + ""; + } + + return QString("%1
%2") + .arg(off_str) + .arg(info_str); + } }; diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc index fb2a69c24b..923eb3bb81 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc @@ -27,6 +27,24 @@ void HudRendererSP::updateState(const UIState &s) { const auto ltp = sm["liveTorqueParameters"].getLiveTorqueParameters(); const auto car_params = sm["carParams"].getCarParams(); const auto lp_sp = sm["longitudinalPlanSP"].getLongitudinalPlanSP(); + const auto lmd = sm["liveMapDataSP"].getLiveMapDataSP(); + + is_metric = s.scene.is_metric; + float speedConv = is_metric ? MS_TO_KPH : MS_TO_MPH; + speedLimit = lp_sp.getSpeedLimit().getResolver().getSpeedLimit() * speedConv; + speedLimitOffset = lp_sp.getSpeedLimit().getResolver().getSpeedLimitOffset() * speedConv; + speedLimitMode = static_cast(s.scene.speed_limit_mode); + if (sm.updated("liveMapDataSP")) { + speedLimitAheadValid = lmd.getSpeedLimitAheadValid(); + speedLimitAhead = lmd.getSpeedLimitAhead() * speedConv; + speedLimitAheadDistance = lmd.getSpeedLimitAheadDistance(); + if (speedLimitAheadDistance < speedLimitAheadDistancePrev && speedLimitAheadValidFrame < 2) { + speedLimitAheadValidFrame++; + } else if (speedLimitAheadDistance > speedLimitAheadDistancePrev && speedLimitAheadValidFrame > 0) { + speedLimitAheadValidFrame--; + } + } + speedLimitAheadDistancePrev = speedLimitAheadDistance; static int reverse_delay = 0; bool reverse_allowed = false; @@ -41,7 +59,6 @@ void HudRendererSP::updateState(const UIState &s) { } reversing = reverse_allowed; - is_metric = s.scene.is_metric; // Handle older routes where vEgoCluster is not set v_ego_cluster_seen = v_ego_cluster_seen || car_state.getVEgoCluster() != 0.0; @@ -123,6 +140,12 @@ void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) { if (standstillTimer) { drawStandstillTimer(p, surface_rect.right() / 12 * 10, surface_rect.bottom() / 12 * 1.53); } + + // Speed Limit + if (speedLimitMode != SpeedLimitMode::OFF) { + drawSpeedLimitSigns(p); + drawUpcomingSpeedLimit(p); + } } } @@ -311,3 +334,191 @@ void HudRendererSP::drawStandstillTimer(QPainter &p, int x, int y) { standstillElapsedTime = 0.0; } } + +void HudRendererSP::drawSpeedLimitSigns(QPainter &p) { + QString speedLimitStr = speedLimit > 0 ? QString::number(std::nearbyint(speedLimit)) : "---"; + + // Offset display text + QString speedLimitSubText = ""; + if (speedLimitOffset != 0) { + speedLimitSubText = (speedLimitOffset > 0 ? "+" : "") + QString::number(std::nearbyint(speedLimitOffset)); + } + + // Position next to MAX speed box + const int sign_width = is_metric ? 200 : 172; + const int sign_x = is_metric ? 280 : 272; + const int sign_y = 45; + const int sign_height = 204; + QRect sign_rect(sign_x, sign_y, sign_width, sign_height); + + int alpha = 255; + + if (is_metric) { + // EU Vienna Convention style circular sign + QRect vienna_rect = sign_rect; + int circle_size = std::min(vienna_rect.width(), vienna_rect.height()); + QRect circle_rect(vienna_rect.x(), vienna_rect.y(), circle_size, circle_size); + + if (vienna_rect.width() > vienna_rect.height()) { + circle_rect.moveLeft(vienna_rect.x() + (vienna_rect.width() - circle_size) / 2); + } else if (vienna_rect.height() > vienna_rect.width()) { + circle_rect.moveTop(vienna_rect.y() + (vienna_rect.height() - circle_size) / 2); + } + + // White background circle + p.setPen(Qt::NoPen); + p.setBrush(QColor(255, 255, 255, alpha)); + p.drawEllipse(circle_rect); + + // Red border ring with color coding + QRect red_ring = circle_rect; + QColor ring_color = QColor(255, 0, 0, alpha); + + p.setBrush(ring_color); + p.drawEllipse(red_ring); + + // Center white circle for text + int ring_size = circle_size * 0.12; + QRect center_circle = red_ring.adjusted(ring_size, ring_size, -ring_size, -ring_size); + p.setBrush(QColor(255, 255, 255, alpha)); + p.drawEllipse(center_circle); + + // Speed value, smaller font for 3+ digits + int font_size = (speedLimitStr.size() >= 3) ? 70 : 85; + p.setFont(InterFont(font_size, QFont::Bold)); + QColor speed_color = QColor(0, 0, 0, alpha); + + p.setPen(speed_color); + p.drawText(center_circle, Qt::AlignCenter, speedLimitStr); + + // Offset value in small circular box + if (!speedLimitSubText.isEmpty()) { + int offset_circle_size = circle_size * 0.4; + int overlap = offset_circle_size * 0.25; + QRect offset_circle_rect( + circle_rect.right() - offset_circle_size/1.25 + overlap, + circle_rect.top() - offset_circle_size/1.75 + overlap, + offset_circle_size, + offset_circle_size + ); + + p.setPen(QPen(QColor(77, 77, 77, 255), 6)); + p.setBrush(QColor(0, 0, 0, alpha)); + p.drawEllipse(offset_circle_rect); + + p.setFont(InterFont(offset_circle_size * 0.45, QFont::Bold)); + p.setPen(QColor(255, 255, 255, alpha)); + p.drawText(offset_circle_rect, Qt::AlignCenter, speedLimitSubText); + } + } else { + // US/Canada MUTCD style sign + p.setPen(Qt::NoPen); + p.setBrush(QColor(255, 255, 255, alpha)); + p.drawRoundedRect(sign_rect, 32, 32); + + // Inner border with violation color coding + QRect inner_rect = sign_rect.adjusted(10, 10, -10, -10); + QColor border_color = QColor(0, 0, 0, alpha); + + p.setPen(QPen(border_color, 4)); + p.setBrush(QColor(255, 255, 255, alpha)); + p.drawRoundedRect(inner_rect, 22, 22); + + // "SPEED LIMIT" text + p.setFont(InterFont(40, QFont::DemiBold)); + p.setPen(QColor(0, 0, 0, alpha)); + p.drawText(inner_rect.adjusted(0, 10, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("SPEED")); + p.drawText(inner_rect.adjusted(0, 50, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT")); + + // Speed value with color coding + p.setFont(InterFont(90, QFont::Bold)); + QColor speed_color = QColor(0, 0, 0, alpha); + + p.setPen(speed_color); + p.drawText(inner_rect.adjusted(0, 80, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr); + + // Offset value in small box + if (!speedLimitSubText.isEmpty()) { + int offset_box_size = sign_rect.width() * 0.4; + int overlap = offset_box_size * 0.25; + QRect offset_box_rect( + sign_rect.right() - offset_box_size/1.5 + overlap, + sign_rect.top() - offset_box_size/1.25 + overlap, + offset_box_size, + offset_box_size + ); + + int corner_radius = offset_box_size * 0.2; + p.setPen(QPen(QColor(77, 77, 77, 255), 6)); + p.setBrush(QColor(0, 0, 0, alpha)); + p.drawRoundedRect(offset_box_rect, corner_radius, corner_radius); + + p.setFont(InterFont(offset_box_size * 0.45, QFont::Bold)); + p.setPen(QColor(255, 255, 255, alpha)); + p.drawText(offset_box_rect, Qt::AlignCenter, speedLimitSubText); + } + } +} + +void HudRendererSP::drawUpcomingSpeedLimit(QPainter &p) { + bool speed_limit_ahead = speedLimitAheadValid && speedLimitAhead > 0 && speedLimitAhead != speedLimit && speedLimitAheadValidFrame > 0; + if (!speed_limit_ahead) { + return; + } + + auto roundToInterval = [&](float distance, int interval, int threshold) { + int base = static_cast(distance / interval) * interval; + return (distance - base >= threshold) ? base + interval : base; + }; + + auto outputDistance = [&] { + if (is_metric) { + if (speedLimitAheadDistance < 50) return tr("Near"); + if (speedLimitAheadDistance >= 1000) return QString::number(speedLimitAheadDistance * METER_TO_KM, 'f', 1) + tr("km"); + + int rounded = (speedLimitAheadDistance < 200) ? std::max(10, roundToInterval(speedLimitAheadDistance, 10, 5)) : roundToInterval(speedLimitAheadDistance, 100, 50); + return QString::number(rounded) + tr("m"); + } else { + float distance_ft = speedLimitAheadDistance * METER_TO_FOOT; + if (distance_ft < 100) return tr("Near"); + if (distance_ft >= 900) return QString::number(speedLimitAheadDistance * METER_TO_MILE, 'f', 1) + tr("mi"); + + int rounded = (distance_ft < 500) ? std::max(50, roundToInterval(distance_ft, 50, 25)) : roundToInterval(distance_ft, 100, 50); + return QString::number(rounded) + tr("ft"); + } + }; + + QString speedStr = QString::number(std::nearbyint(speedLimitAhead)); + QString distanceStr = outputDistance(); + + // Position below current speed limit sign + const int sign_width = is_metric ? 200 : 172; + const int sign_x = is_metric ? 280 : 272; + const int sign_y = 45; + const int sign_height = 204; + + const int ahead_width = 170; + const int ahead_height = 160; + const int ahead_x = sign_x + (sign_width - ahead_width) / 2; + const int ahead_y = sign_y + sign_height + 10; + + QRect ahead_rect(ahead_x, ahead_y, ahead_width, ahead_height); + p.setPen(QPen(QColor(255, 255, 255, 100), 3)); + p.setBrush(QColor(0, 0, 0, 180)); + p.drawRoundedRect(ahead_rect, 16, 16); + + // "AHEAD" label + p.setFont(InterFont(40, QFont::DemiBold)); + p.setPen(QColor(200, 200, 200, 255)); + p.drawText(ahead_rect.adjusted(0, 4, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("AHEAD")); + + // Speed value + p.setFont(InterFont(70, QFont::Bold)); + p.setPen(QColor(255, 255, 255, 255)); + p.drawText(ahead_rect.adjusted(0, 38, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedStr); + + // Distance + p.setFont(InterFont(40, QFont::Normal)); + p.setPen(QColor(180, 180, 180, 255)); + p.drawText(ahead_rect.adjusted(0, 110, 0, 0), Qt::AlignTop | Qt::AlignHCenter, distanceStr); +} diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.h b/selfdrive/ui/sunnypilot/qt/onroad/hud.h index 4c92835957..d76eb06060 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.h +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.h @@ -8,6 +8,7 @@ #pragma once #include "selfdrive/ui/qt/onroad/hud.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h" #include "selfdrive/ui/sunnypilot/qt/onroad/developer_ui/developer_ui.h" class HudRendererSP : public HudRenderer { @@ -28,6 +29,8 @@ private: void drawStandstillTimer(QPainter &p, int x, int y); bool pulseElement(int frame); void drawSmartCruiseControlOnroadIcon(QPainter &p, const QRect &surface_rect, int x_offset, int y_offset, std::string name); + void drawSpeedLimitSigns(QPainter &p); + void drawUpcomingSpeedLimit(QPainter &p); bool lead_status; float lead_d_rel; @@ -63,4 +66,12 @@ private: bool smartCruiseControlVisionEnabled; bool smartCruiseControlVisionActive; int smartCruiseControlVisionFrame; + float speedLimit; + float speedLimitOffset; + bool speedLimitAheadValid; + float speedLimitAhead; + float speedLimitAheadDistance; + float speedLimitAheadDistancePrev; + int speedLimitAheadValidFrame; + SpeedLimitMode speedLimitMode = SpeedLimitMode::OFF; }; diff --git a/selfdrive/ui/sunnypilot/ui.cc b/selfdrive/ui/sunnypilot/ui.cc index 7b10929bc6..7f83e5ea79 100644 --- a/selfdrive/ui/sunnypilot/ui.cc +++ b/selfdrive/ui/sunnypilot/ui.cc @@ -20,7 +20,7 @@ UIStateSP::UIStateSP(QObject *parent) : UIState(parent) { "wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan", "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP", "carControl", "gpsLocationExternal", "gpsLocation", "liveTorqueParameters", - "carStateSP", "liveParameters" + "carStateSP", "liveParameters", "liveMapDataSP" }); // update timer @@ -53,6 +53,7 @@ void ui_update_params_sp(UIStateSP *s) { auto params = Params(); s->scene.dev_ui_info = std::atoi(params.get("DevUIInfo").c_str()); s->scene.standstill_timer = params.getBool("StandstillTimer"); + s->scene.speed_limit_mode = params.getBool("SpeedLimitMode"); } DeviceSP::DeviceSP(QObject *parent) : Device(parent) { diff --git a/selfdrive/ui/sunnypilot/ui_scene.h b/selfdrive/ui/sunnypilot/ui_scene.h index c941be675c..69cebd6d79 100644 --- a/selfdrive/ui/sunnypilot/ui_scene.h +++ b/selfdrive/ui/sunnypilot/ui_scene.h @@ -10,4 +10,5 @@ typedef struct UISceneSP : UIScene { int dev_ui_info = 0; bool standstill_timer = false; + int speed_limit_mode = 0; } UISceneSP;