diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index cb97c9e..01c4c5c 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -151,10 +151,16 @@ class FrogPilotPlanner: frogpilot_toggles.standard_follow, frogpilot_toggles.relaxed_follow, controlsState.personality) if self.lead_one.status: + self.safe_obstacle_distance_stock = int(get_safe_obstacle_distance(v_ego, self.t_follow)) self.update_follow_values(lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles) + self.safe_obstacle_distance = int(get_safe_obstacle_distance(v_ego, self.t_follow)) + self.stopped_equivalence_factor = int(get_stopped_equivalence_factor(v_lead)) else: self.acceleration_jerk = self.base_acceleration_jerk self.speed_jerk = self.base_speed_jerk + self.safe_obstacle_distance = 0 + self.safe_obstacle_distance_stock = 0 + self.stopped_equivalence_factor = 0 if self.frame % 10 == 0: self.lead_departing = lead_distance - self.previous_lead_distance > 0.5 and self.previous_lead_distance != 0 and carState.standstill @@ -277,6 +283,11 @@ class FrogPilotPlanner: frogpilotPlan.conditionalExperimental = self.cem.experimental_mode frogpilotPlan.redLight = self.cem.red_light_detected + 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.laneWidthLeft = self.lane_width_left frogpilotPlan.laneWidthRight = self.lane_width_right diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 7928362..c521a67 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -353,6 +353,23 @@ void OnroadWindow::paintEvent(QPaintEvent *event) { } QString logicsDisplayString = QString(); + if (scene.show_jerk) { + logicsDisplayString += QString("Acceleration Jerk: %1") + .arg(scene.acceleration_jerk, 0, 'f', 3); + if (scene.acceleration_jerk_difference != 0) { + logicsDisplayString += QString(" (%1%2)") + .arg(scene.acceleration_jerk_difference > 0 ? "-" : "", 0) + .arg(scene.acceleration_jerk_difference, 0, 'f', 3); + } + logicsDisplayString += QString(" | Speed Jerk: %1") + .arg(scene.speed_jerk, 0, 'f', 3); + if (scene.speed_jerk_difference != 0) { + logicsDisplayString += QString(" (%1%2)") + .arg(scene.speed_jerk_difference > 0 ? "-" : "", 0) + .arg(scene.speed_jerk_difference, 0, 'f', 3); + } + logicsDisplayString += " | "; + } if (scene.show_tuning) { if (!scene.live_valid) { logicsDisplayString += "Friction: Calculating... | Lateral Acceleration: Calculating..."; @@ -375,7 +392,32 @@ void OnroadWindow::paintEvent(QPaintEvent *event) { int logicsX = (rect.width() - logicsWidth) / 2; int logicsY = rect.top() + 27; - p.drawText(logicsX, logicsY, logicsDisplayString); + QStringList parts = logicsDisplayString.split(" | "); + int currentX = logicsX; + + for (const QString &part : parts) { + QStringList subParts = part.split(" "); + for (int i = 0; i < subParts.size(); ++i) { + QString text = subParts[i]; + + if (text.endsWith(")") && (subParts[i - 1].contains("Acceleration") || subParts[i - 1].contains("Speed"))) { + p.drawText(currentX, logicsY, subParts[i - 1] + " ("); + currentX += p.fontMetrics().horizontalAdvance(subParts[i - 1] + " ("); + text.chop(1); + p.setPen(text.contains("-") ? redColor() : Qt::white); + } else if (text.startsWith("(") && i > 0) { + p.drawText(currentX, logicsY, " ("); + currentX += p.fontMetrics().horizontalAdvance(" ("); + text = text.mid(1); + p.setPen(text.contains("-") ? redColor() : Qt::white); + } else { + p.setPen(Qt::white); + } + + p.drawText(currentX, logicsY, text); + currentX += p.fontMetrics().horizontalAdvance(text + " "); + } + } update(); } } @@ -451,7 +493,7 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) { // ExperimentalButton ExperimentalButton::ExperimentalButton(QWidget *parent) : experimental_mode(false), engageable(false), QPushButton(parent), scene(uiState()->scene) { - 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}); @@ -486,7 +528,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() || scene.always_on_lateral_active; if ((cs.getExperimentalMode() != experimental_mode) || (eng != engageable)) { @@ -502,6 +544,8 @@ void ExperimentalButton::updateState(const UIState &s) { wheelIcon = scene.wheel_icon; wheelIconGif = 0; + y_offset = leadInfo ? 10 : 0; + if (randomEvent == 0 && gifLabel) { delete gifLabel; gifLabel = nullptr; @@ -519,7 +563,7 @@ void ExperimentalButton::updateState(const UIState &s) { if (movie) { gifLabel->setMovie(movie); gifLabel->setFixedSize(img_size, img_size); - 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(); } } @@ -555,9 +599,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); } } @@ -650,7 +694,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(); @@ -1075,6 +1119,24 @@ 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") + .arg(qRound(d_rel * distanceConversion)) + .arg(leadDistanceUnit) + .arg(qRound(lead_speed * speedConversion)) + .arg(leadSpeedUnit); + + QFontMetrics metrics(painter.font()); + int middle_x = (chevron[2].x() + chevron[0].x()) / 2; + int textWidth = metrics.horizontalAdvance(text); + painter.drawText(middle_x - textWidth / 2, chevron[0].y() + metrics.height() + 5, text); + } + painter.restore(); } @@ -1295,6 +1357,10 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets() { laneWidthLeft = scene.lane_width_left; laneWidthRight = scene.lane_width_right; + leadInfo = scene.lead_info; + obstacleDistance = scene.obstacle_distance; + obstacleDistanceStock = scene.obstacle_distance_stock; + mapOpen = scene.map_open; onroadDistanceButton = scene.onroad_distance_button; @@ -1364,6 +1430,10 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p) { animationTimer->stop(); } + if (leadInfo) { + drawLeadInfo(p); + } + if (scene.speed_limit_changed) { drawSLCConfirmation(p); } @@ -1571,6 +1641,97 @@ void DistanceButton::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(scene.acceleration * 100) / 100; + int randomEvent = scene.current_random_event; + + if (acceleration > maxAcceleration && (status == STATUS_ENGAGED || status == STATUS_TRAFFIC_MODE_ACTIVE)) { + maxAcceleration = acceleration; + isFiveSecondsPassed = false; + timer.start(); + } else if (randomEvent == 2 && maxAcceleration < 3.0) { + maxAcceleration = 3.0; + isFiveSecondsPassed = false; + timer.start(); + } else if (randomEvent == 3 && maxAcceleration < 3.5) { + maxAcceleration = 3.5; + isFiveSecondsPassed = false; + timer.start(); + } else if (randomEvent == 4 && maxAcceleration < 4.0) { + maxAcceleration = 4.0; + isFiveSecondsPassed = false; + timer.start(); + } else { + isFiveSecondsPassed = timer.hasExpired(maxAccelDuration); + } + + auto createText = [&](const QString &title, const 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 = 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: "), scene.stopped_equivalence); + QString followText = " = " + createText(mapOpen ? tr("Follow: ") : tr("Follow Distance: "), scene.desired_follow); + + auto createDiffText = [&](const double data, const 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(); + + int totalTextWidth = p.fontMetrics().horizontalAdvance(accelText) + + p.fontMetrics().horizontalAdvance(maxAccSuffix) + + p.fontMetrics().horizontalAdvance(obstacleText) + + p.fontMetrics().horizontalAdvance(createDiffText(obstacleDistance, obstacleDistanceStock)) + + p.fontMetrics().horizontalAdvance(stopText) + + p.fontMetrics().horizontalAdvance(followText); + + int textStartPos = adjustedRect.x() + (adjustedRect.width() - totalTextWidth) / 2; + + auto drawText = [&](const QString &text, const QColor &color) { + p.setPen(color); + p.drawText(textStartPos, textBaseLine, text); + textStartPos += p.fontMetrics().horizontalAdvance(text); + }; + + drawText(accelText, Qt::white); + if (!maxAccSuffix.isEmpty()) { + drawText(maxAccSuffix, isFiveSecondsPassed ? Qt::white : redColor()); + } + drawText(obstacleText, Qt::white); + drawText(createDiffText(obstacleDistance, obstacleDistanceStock), (obstacleDistance - obstacleDistanceStock) > 0 ? Qt::green : Qt::red); + drawText(stopText, Qt::white); + drawText(followText, Qt::white); + + p.restore(); +} + PedalIcons::PedalIcons(QWidget *parent) : QWidget(parent), scene(uiState()->scene) { setFixedSize(btn_size, btn_size); diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 1207019..556f9aa 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -92,7 +92,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; @@ -123,6 +123,7 @@ private: int steeringAngleDeg; int wheelIcon; int wheelIconGif; + int y_offset; }; @@ -200,6 +201,7 @@ private: void paintFrogPilotWidgets(QPainter &p); void updateFrogPilotWidgets(); + void drawLeadInfo(QPainter &p); void drawSLCConfirmation(QPainter &p); void drawStatusBar(QPainter &p); void drawTurnSignals(QPainter &p); @@ -219,6 +221,7 @@ private: bool blindSpotRight; bool compass; bool experimentalMode; + bool leadInfo; bool mapOpen; bool onroadDistanceButton; bool roadNameUI; @@ -247,6 +250,8 @@ private: int currentHolidayTheme; int customColors; int customSignals; + int obstacleDistance; + int obstacleDistanceStock; int totalFrames = 8; QString accelerationUnit; @@ -312,6 +317,8 @@ private: QPoint timeoutPoint = QPoint(420, 69); QTimer clickTimer; + inline QColor redColor(int alpha = 255) { return QColor(201, 34, 49, alpha); } + private slots: void offroadTransition(bool offroad); void primeChanged(bool prime); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index d1d3bc7..c274f8d 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; @@ -251,13 +252,21 @@ static void update_state(UIState *s) { } if (sm.updated("frogpilotPlan")) { auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan(); + scene.acceleration_jerk = frogpilotPlan.getAccelerationJerk(); + scene.acceleration_jerk_difference = frogpilotPlan.getAccelerationJerkStock() - scene.acceleration_jerk; 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.speed_jerk = frogpilotPlan.getSpeedJerk(); + scene.speed_jerk_difference = frogpilotPlan.getSpeedJerkStock() - scene.speed_jerk; 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(); } @@ -333,6 +342,8 @@ void ui_update_frogpilot_params(UIState *s) { scene.show_signal = border_metrics && params.getBool("SignalMetrics"); scene.show_steering = border_metrics && params.getBool("ShowSteering"); scene.fps_counter = developer_ui && params.getBool("FPSCounter"); + scene.lead_info = scene.longitudinal_control && developer_ui && params.getBool("LongitudinalMetrics"); + scene.show_jerk = scene.longitudinal_control && developer_ui && params.getBool("LongitudinalMetrics"); scene.show_tuning = developer_ui && scene.has_auto_tune && params.getBool("LateralMetrics"); scene.disable_smoothing_mtsc = params.getBool("MTSCEnabled") && params.getBool("DisableMTSCSmoothing"); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index c520966..0ee12c6 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -205,6 +205,7 @@ typedef struct UIScene { bool fps_counter; bool has_auto_tune; bool holiday_themes; + bool lead_info; bool live_valid; bool map_open; bool online; @@ -220,6 +221,7 @@ typedef struct UIScene { bool show_aol_status_bar; bool show_blind_spot; bool show_cem_status_bar; + bool show_jerk; bool show_signal; bool show_slc_offset; bool show_slc_offset_ui; @@ -239,6 +241,9 @@ typedef struct UIScene { bool use_vienna_slc_sign; bool vtsc_controlling_curve; + float acceleration; + float acceleration_jerk; + float acceleration_jerk_difference; float adjusted_cruise; float friction; float lane_detection_width; @@ -246,6 +251,8 @@ typedef struct UIScene { float lane_width_right; float lat_accel; float lead_detection_threshold; + float speed_jerk; + float speed_jerk_difference; float speed_limit; float speed_limit_offset; float speed_limit_overridden_speed; @@ -262,7 +269,11 @@ typedef struct UIScene { int custom_colors; int custom_icons; int custom_signals; + int desired_follow; + int obstacle_distance; + int obstacle_distance_stock; int steering_angle_deg; + int stopped_equivalence; int wheel_icon; QPolygonF track_adjacent_vertices[6];