From be5fc3d4ffb75ef642e764a4f28ca879b5b17598 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Sun, 7 Jul 2024 05:46:10 -0700 Subject: [PATCH] Visuals - Developer UI - Longitudinal Metrics - Lead Info --- .../frogpilot/controls/frogpilot_planner.py | 11 ++ selfdrive/ui/qt/onroad/annotated_camera.cc | 120 +++++++++++++++++- selfdrive/ui/qt/onroad/annotated_camera.h | 9 ++ selfdrive/ui/qt/onroad/buttons.cc | 11 +- selfdrive/ui/qt/onroad/buttons.h | 3 +- selfdrive/ui/ui.cc | 7 + selfdrive/ui/ui.h | 6 + 7 files changed, 159 insertions(+), 8 deletions(-) diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 6c33abb8..b9fce612 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -201,8 +201,14 @@ class FrogPilotPlanner: ) if self.tracking_lead: + self.safe_obstacle_distance = int(get_safe_obstacle_distance(v_ego, self.t_follow)) + self.safe_obstacle_distance_stock = self.safe_obstacle_distance + self.stopped_equivalence_factor = int(get_stopped_equivalence_factor(v_lead)) self.update_follow_values(lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles) else: + self.safe_obstacle_distance = 0 + self.safe_obstacle_distance_stock = 0 + self.stopped_equivalence_factor = 0 self.acceleration_jerk = self.base_acceleration_jerk self.danger_jerk = self.base_danger_jerk self.speed_jerk = self.base_speed_jerk @@ -332,6 +338,11 @@ class FrogPilotPlanner: frogpilotPlan.conditionalExperimentalActive = self.cem.experimental_mode + frogpilotPlan.desiredFollowDistance = self.safe_obstacle_distance - self.stopped_equivalence_factor + frogpilotPlan.safeObstacleDistance = self.safe_obstacle_distance + frogpilotPlan.safeObstacleDistanceStock = self.safe_obstacle_distance_stock + frogpilotPlan.stoppedEquivalenceFactor = self.stopped_equivalence_factor + frogpilotPlan.forcingStop = self.forcing_stop frogpilotPlan.greenLight = not self.model_stopped diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index de019a0b..a752e2a3 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -81,7 +81,7 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { status = s.status; // update engageability/experimental mode button - experimental_btn->updateState(s); + experimental_btn->updateState(s, leadInfo); // update DM icon auto dm_state = sm["driverMonitoringState"].getDriverMonitoringState(); @@ -530,6 +530,29 @@ void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::ModelDataV } painter.drawPolygon(chevron, std::size(chevron)); + if (leadInfo) { + float lead_speed = std::max(v_rel + v_ego, 0.0f); + + painter.setPen(Qt::white); + painter.setFont(InterFont(35, QFont::Bold)); + + QString text = QString("%1 %2 | %3 %4 | %5 %6") + .arg(qRound(d_rel * distanceConversion)) + .arg(leadDistanceUnit) + .arg(qRound(lead_speed * speedConversion)) + .arg(leadSpeedUnit) + .arg(QString::number(d_rel / std::max(v_ego, 1.0f), 'f', 1)) + .arg("s"); + + QFontMetrics metrics(painter.font()); + int middle_x = (chevron[2].x() + chevron[0].x()) / 2; + int textWidth = metrics.horizontalAdvance(text); + int text_x = middle_x - textWidth / 2; + int text_y = chevron[0].y() + metrics.height() + 5; + + painter.drawText(text_x, text_y, text); + } + painter.restore(); } @@ -597,7 +620,7 @@ void AnnotatedCameraWidget::paintGL() { const auto &lead = model.getLeadsV3()[i]; auto lead_drel = lead.getX()[0]; if (s->scene.has_lead && (prev_drel < 0 || std::abs(lead_drel - prev_drel) > 3.0)) { - drawLead(painter, lead, s->scene.lead_vertices[i], v_ego); + drawLead(painter, lead, s->scene.lead_vertices[i], (speed / (is_metric ? MS_TO_KPH : MS_TO_MPH))); } prev_drel = lead_drel; } @@ -759,14 +782,28 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &painter, const UISce cruiseAdjustment = disableSmoothing || !is_cruise_set ? fmax(setSpeed - scene.adjusted_cruise, 0) : fmax(0.25 * (setSpeed - scene.adjusted_cruise) + 0.75 * cruiseAdjustment - 1, 0); vtscControllingCurve = scene.vtsc_controlling_curve; + currentAcceleration = scene.acceleration; + + currentRandomEvent = scene.current_random_event; + customColors = scene.custom_colors; + desiredFollow = scene.desired_follow; + stoppedEquivalence = scene.stopped_equivalence; + experimentalMode = scene.experimental_mode; laneDetectionWidth = scene.lane_detection_width; laneWidthLeft = scene.lane_width_left; laneWidthRight = scene.lane_width_right; + leadInfo = scene.lead_info; + obstacleDistance = scene.obstacle_distance; + obstacleDistanceStock = scene.obstacle_distance_stock; + if (leadInfo) { + drawLeadInfo(painter); + } + mapOpen = scene.map_open; map_settings_btn_bottom->setEnabled(map_settings_btn->isEnabled()); if (map_settings_btn_bottom->isEnabled()) { @@ -953,6 +990,85 @@ void Compass::paintEvent(QPaintEvent *event) { } } +void AnnotatedCameraWidget::drawLeadInfo(QPainter &p) { + static QElapsedTimer timer; + + static bool isFiveSecondsPassed = false; + + static double maxAcceleration = 0.0; + constexpr int maxAccelDuration = 5000; + + double acceleration = std::round(currentAcceleration * 100) / 100; + + auto resetTimer = [&]() { + timer.start(); + isFiveSecondsPassed = false; + }; + + if ((acceleration > maxAcceleration && (status == STATUS_ENGAGED || status == STATUS_TRAFFIC_MODE_ACTIVE)) || + (currentRandomEvent == 2 && maxAcceleration < 3.0) || + (currentRandomEvent == 3 && maxAcceleration < 3.5) || + (currentRandomEvent == 4 && maxAcceleration < 4.0)) { + maxAcceleration = std::max({acceleration, currentRandomEvent == 2 ? 3.0 : maxAcceleration, currentRandomEvent == 3 ? 3.5 : maxAcceleration, currentRandomEvent == 4 ? 4.0 : maxAcceleration}); + resetTimer(); + } else { + isFiveSecondsPassed = timer.hasExpired(maxAccelDuration); + } + + auto createText = [&](const QString &title, double data) { + return title + QString::number(std::round(data * distanceConversion)) + " " + leadDistanceUnit; + }; + + QString accelText = QString(tr("Accel: %1%2")) + .arg(acceleration * accelerationConversion, 0, 'f', 2) + .arg(accelerationUnit); + + QString maxAccSuffix; + if (!mapOpen) { + maxAccSuffix = QString(tr(" - Max: %1%2")) + .arg(maxAcceleration * accelerationConversion, 0, 'f', 2) + .arg(accelerationUnit); + } + + QString obstacleText = createText(mapOpen ? tr(" | Obstacle: ") : tr(" | Obstacle Factor: "), obstacleDistance); + QString stopText = createText(mapOpen ? tr(" - Stop: ") : tr(" - Stop Factor: "), stoppedEquivalence); + QString followText = " = " + createText(mapOpen ? tr("Follow: ") : tr("Follow Distance: "), desiredFollow); + + auto createDiffText = [&](double data, double stockData) { + double difference = std::round((data - stockData) * distanceConversion); + return difference != 0 ? QString(" (%1%2)").arg(difference > 0 ? "+" : "").arg(difference) : QString(); + }; + + p.save(); + + QRect insightsRect(rect().left() - 1, rect().top() - 60, rect().width() + 2, 100); + p.setBrush(QColor(0, 0, 0, 150)); + p.drawRoundedRect(insightsRect, 30, 30); + p.setFont(InterFont(28, QFont::Bold)); + p.setRenderHint(QPainter::TextAntialiasing); + + QRect adjustedRect = insightsRect.adjusted(0, 27, 0, 27); + int textBaseLine = adjustedRect.y() + (adjustedRect.height() + p.fontMetrics().height()) / 2 - p.fontMetrics().descent(); + + QStringList texts = {accelText, maxAccSuffix, obstacleText, createDiffText(obstacleDistance, obstacleDistanceStock), stopText, followText}; + QList colors = {Qt::white, isFiveSecondsPassed ? Qt::white : redColor(), Qt::white, (obstacleDistance - obstacleDistanceStock) > 0 ? Qt::green : Qt::red, Qt::white, Qt::white}; + + int totalTextWidth = 0; + for (const auto &text : texts) { + totalTextWidth += p.fontMetrics().horizontalAdvance(text); + } + + int textStartPos = adjustedRect.x() + (adjustedRect.width() - totalTextWidth) / 2; + + for (int i = 0; i < texts.size(); ++i) { + p.setPen(colors[i]); + p.drawText(textStartPos, textBaseLine, texts[i]); + textStartPos += p.fontMetrics().horizontalAdvance(texts[i]); + } + + p.restore(); +} + PedalIcons::PedalIcons(QWidget *parent) : QWidget(parent) { setFixedSize(btn_size, btn_size); diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index e4708d39..c56eef40 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -91,6 +91,7 @@ private: void initializeFrogPilotWidgets(); void paintFrogPilotWidgets(QPainter &painter, const UIScene &scene); + void drawLeadInfo(QPainter &p); void drawSLCConfirmation(QPainter &p); void drawStatusBar(QPainter &p); void drawTurnSignals(QPainter &p); @@ -109,6 +110,7 @@ private: bool blindSpotRight; bool compass; bool experimentalMode; + bool leadInfo; bool mapOpen; bool onroadDistanceButton; bool reverseCruise; @@ -125,6 +127,8 @@ private: bool useViennaSLCSign; bool vtscControllingCurve; + double currentAcceleration; + float accelerationConversion; float cruiseAdjustment; float distanceConversion; @@ -140,8 +144,13 @@ private: int conditionalSpeedLead; int conditionalStatus; int currentHolidayTheme; + int currentRandomEvent; int customColors; int customSignals; + int desiredFollow; + int obstacleDistance; + int obstacleDistanceStock; + int stoppedEquivalence; int totalFrames; QPixmap stopSignImg; diff --git a/selfdrive/ui/qt/onroad/buttons.cc b/selfdrive/ui/qt/onroad/buttons.cc index 418f53f6..71768330 100644 --- a/selfdrive/ui/qt/onroad/buttons.cc +++ b/selfdrive/ui/qt/onroad/buttons.cc @@ -33,7 +33,7 @@ void drawIconGif(QPainter &p, const QPoint ¢er, const QMovie &img, const QBr // ExperimentalButton ExperimentalButton::ExperimentalButton(QWidget *parent) : experimental_mode(false), engageable(false), QPushButton(parent) { - setFixedSize(btn_size, btn_size); + setFixedSize(btn_size, btn_size + 10); engage_img = loadPixmap("../assets/img_chffr_wheel.png", {img_size, img_size}); experimental_img = loadPixmap("../assets/img_experimental.svg", {img_size, img_size}); @@ -70,7 +70,7 @@ void ExperimentalButton::changeMode() { } } -void ExperimentalButton::updateState(const UIState &s) { +void ExperimentalButton::updateState(const UIState &s, bool leadInfo) { const auto cs = (*s.sm)["controlsState"].getControlsState(); bool eng = cs.getEngageable() || cs.getEnabled() || s.scene.always_on_lateral_active; if ((cs.getExperimentalMode() != experimental_mode) || (eng != engageable)) { @@ -91,6 +91,7 @@ void ExperimentalButton::updateState(const UIState &s) { trafficModeActive = scene.traffic_mode_active; wheelIcon = scene.wheel_icon; wheelIconGif = 0; + y_offset = leadInfo ? 10 : 0; if (randomEvent == 0 && gifLabel) { delete gifLabel; @@ -102,7 +103,7 @@ void ExperimentalButton::updateState(const UIState &s) { if (movie) { movie->setScaledSize(QSize(img_size, img_size)); gifLabel->setMovie(movie); - gifLabel->move((width() - gifLabel->width()) / 2, (height() - gifLabel->height()) / 2); + gifLabel->move((width() - gifLabel->width()) / 2, (height() - gifLabel->height()) / 2 + y_offset); gifLabel->movie()->start(); } @@ -135,9 +136,9 @@ void ExperimentalButton::paintEvent(QPaintEvent *event) { QColor(0, 0, 0, 166); if (wheelIconGif != 0) { - drawIconGif(p, QPoint(btn_size / 2, btn_size / 2), *gif, background_color, 1.0); + drawIconGif(p, QPoint(btn_size / 2, btn_size / 2 + y_offset), *gif, background_color, 1.0); } else { - drawIcon(p, QPoint(btn_size / 2, btn_size / 2), img, background_color, (isDown() || !engageable) ? 0.6 : 1.0, steeringAngleDeg); + drawIcon(p, QPoint(btn_size / 2, btn_size / 2 + y_offset), img, background_color, (isDown() || !engageable) ? 0.6 : 1.0, steeringAngleDeg); } } diff --git a/selfdrive/ui/qt/onroad/buttons.h b/selfdrive/ui/qt/onroad/buttons.h index 6cf03c37..7eb3b777 100644 --- a/selfdrive/ui/qt/onroad/buttons.h +++ b/selfdrive/ui/qt/onroad/buttons.h @@ -14,7 +14,7 @@ class ExperimentalButton : public QPushButton { public: explicit ExperimentalButton(QWidget *parent = 0); - void updateState(const UIState &s); + void updateState(const UIState &s, bool leadInfo); private: void paintEvent(QPaintEvent *event) override; @@ -38,6 +38,7 @@ private: int steeringAngleDeg; int wheelIcon; int wheelIconGif; + int y_offset; QLabel *gifLabel; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 5bd7cb6f..b1202220 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -221,6 +221,7 @@ static void update_state(UIState *s) { } if (sm.updated("carState")) { auto carState = sm["carState"].getCarState(); + scene.acceleration = carState.getAEgo(); scene.blind_spot_left = carState.getLeftBlindspot(); scene.blind_spot_right = carState.getRightBlindspot(); scene.parked = carState.getGearShifter() == cereal::CarState::GearShifter::PARK; @@ -253,13 +254,17 @@ static void update_state(UIState *s) { if (sm.updated("frogpilotPlan")) { auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan(); scene.adjusted_cruise = frogpilotPlan.getAdjustedCruise(); + scene.desired_follow = frogpilotPlan.getDesiredFollowDistance(); scene.lane_width_left = frogpilotPlan.getLaneWidthLeft(); scene.lane_width_right = frogpilotPlan.getLaneWidthRight(); + scene.obstacle_distance = frogpilotPlan.getSafeObstacleDistance(); + scene.obstacle_distance_stock = frogpilotPlan.getSafeObstacleDistanceStock(); scene.red_light = frogpilotPlan.getRedLight(); scene.speed_limit = frogpilotPlan.getSlcSpeedLimit(); scene.speed_limit_offset = frogpilotPlan.getSlcSpeedLimitOffset(); scene.speed_limit_overridden = frogpilotPlan.getSlcOverridden(); scene.speed_limit_overridden_speed = frogpilotPlan.getSlcOverriddenSpeed(); + scene.stopped_equivalence = frogpilotPlan.getStoppedEquivalenceFactor(); scene.unconfirmed_speed_limit = frogpilotPlan.getUnconfirmedSlcSpeedLimit(); scene.vtsc_controlling_curve = frogpilotPlan.getVtscControllingCurve(); } @@ -350,6 +355,8 @@ void ui_update_frogpilot_params(UIState *s, Params ¶ms) { scene.show_steering = border_metrics && params.getBool("ShowSteering"); bool show_lateral = developer_ui && params.getBool("LateralMetrics"); scene.show_tuning = show_lateral && scene.has_auto_tune && params.getBool("TuningInfo"); + bool show_longitudinal = scene.longitudinal_control && developer_ui && params.getBool("LongitudinalMetrics"); + scene.lead_info = show_longitudinal && params.getBool("LeadInfo"); scene.disable_smoothing_mtsc = params.getBool("MTSCEnabled") && params.getBool("DisableMTSCSmoothing"); scene.disable_smoothing_vtsc = params.getBool("VisionTurnControl") && params.getBool("DisableVTSCSmoothing"); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index cb2218e3..44d52617 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -141,6 +141,7 @@ typedef struct UIScene { bool has_auto_tune; bool has_lead; bool holiday_themes; + bool lead_info; bool live_valid; bool map_open; bool model_randomizer; @@ -183,6 +184,7 @@ typedef struct UIScene { double fps; + float acceleration; float adjusted_cruise; float friction; float lane_detection_width; @@ -206,8 +208,12 @@ typedef struct UIScene { int custom_colors; int custom_icons; int custom_signals; + int desired_follow; int model_length; + int obstacle_distance; + int obstacle_distance_stock; int steering_angle_deg; + int stopped_equivalence; int tethering_config; int wheel_icon;