diff --git a/cereal/custom.capnp b/cereal/custom.capnp index b7bfb20..2f615f2 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -24,8 +24,12 @@ struct FrogPilotNavigation @0xf35cc4560bbf6ec2 { struct FrogPilotPlan @0xda96579883444c35 { conditionalExperimental @1 :Bool; + desiredFollowDistance @2 :Int16; laneWidthLeft @3 :Float32; laneWidthRight @4 :Float32; + safeObstacleDistance @6 :Int16; + safeObstacleDistanceStock @7 :Int16; + stoppedEquivalenceFactor @12 :Int16; } struct CustomReserved4 @0x80ae746ee2596b11 { diff --git a/common/params.cc b/common/params.cc index 355cd96..ddaaf1c 100644 --- a/common/params.cc +++ b/common/params.cc @@ -256,6 +256,7 @@ std::unordered_map keys = { {"GoatScream", PERSISTENT}, {"LaneLinesWidth", PERSISTENT}, {"LateralTune", PERSISTENT}, + {"LeadInfo", PERSISTENT}, {"LongitudinalTune", PERSISTENT}, {"ManualUpdateInitiated", CLEAR_ON_MANAGER_START}, {"ModelUI", PERSISTENT}, @@ -281,6 +282,7 @@ std::unordered_map keys = { {"UnlimitedLength", PERSISTENT}, {"UpdateSchedule", PERSISTENT}, {"UpdateTime", PERSISTENT}, + {"UseSI", PERSISTENT}, {"WarningSoftVolume", PERSISTENT}, {"WarningImmediateVolume", PERSISTENT}, }; diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 425849f..857f46d 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -373,6 +373,16 @@ class LongitudinalMpc: else: self.acceleration_offset = 1 + # LongitudinalPlan variables for onroad driving insights + if self.status: + self.safe_obstacle_distance = int(np.mean(get_safe_obstacle_distance(v_ego, t_follow))) + self.safe_obstacle_distance_stock = int(np.mean(get_safe_obstacle_distance(v_ego, self.t_follow))) + self.stopped_equivalence_factor = int(np.mean(get_stopped_equivalence_factor(lead_xv_0[:,1]))) + else: + self.safe_obstacle_distance = 0 + self.safe_obstacle_distance_stock = 0 + self.stopped_equivalence_factor = 0 + # To estimate a safe distance from a moving lead, we calculate how much stopping # distance that lead needs as a minimum. We can add that to the current distance # and then treat that as a stopped car/obstacle at this new distance. diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index a7d7d5a..71bdf20 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -94,6 +94,11 @@ class FrogPilotPlanner: frogpilotPlan.conditionalExperimental = self.cem.experimental_mode + frogpilotPlan.desiredFollowDistance = mpc.safe_obstacle_distance - mpc.stopped_equivalence_factor + frogpilotPlan.safeObstacleDistance = mpc.safe_obstacle_distance + frogpilotPlan.safeObstacleDistanceStock = mpc.safe_obstacle_distance_stock + frogpilotPlan.stoppedEquivalenceFactor = mpc.stopped_equivalence_factor + frogpilotPlan.laneWidthLeft = self.lane_width_left frogpilotPlan.laneWidthRight = self.lane_width_right diff --git a/selfdrive/frogpilot/ui/visual_settings.cc b/selfdrive/frogpilot/ui/visual_settings.cc index 036b386..1af72fd 100644 --- a/selfdrive/frogpilot/ui/visual_settings.cc +++ b/selfdrive/frogpilot/ui/visual_settings.cc @@ -24,6 +24,7 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot {"CustomUI", "Custom Onroad UI", "Customize the Onroad UI with some additional visual functions.", "../assets/offroad/icon_road.png"}, {"AccelerationPath", "Acceleration Path", "Visualize the car's intended acceleration or deceleration with a color-coded path.", ""}, {"BlindSpotPath", "Blind Spot Path", "Visualize your blind spots with a red path when another vehicle is detected nearby.", ""}, + {"LeadInfo", "Lead Info and Logics", "Get detailed information about the vehicle ahead, including speed and distance, and the logic behind your following distance.", ""}, {"ModelUI", "Model UI", "Personalize how the model's visualizations appear on your screen.", "../assets/offroad/icon_calibration.png"}, {"DynamicPathWidth", "Dynamic Path Width", "Have the path width dynamically adjust based on the current engagement state of openpilot.", ""}, @@ -106,6 +107,10 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot } }); toggle = customUIToggle; + } else if (param == "LeadInfo") { + std::vector leadInfoToggles{"UseSI"}; + std::vector leadInfoToggleNames{tr("Use SI Values")}; + toggle = new FrogPilotParamToggleControl(param, title, desc, icon, leadInfoToggles, leadInfoToggleNames); } else if (param == "ModelUI") { FrogPilotParamManageControl *modelUIToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); diff --git a/selfdrive/frogpilot/ui/visual_settings.h b/selfdrive/frogpilot/ui/visual_settings.h index e1541fc..01b94ac 100644 --- a/selfdrive/frogpilot/ui/visual_settings.h +++ b/selfdrive/frogpilot/ui/visual_settings.h @@ -30,7 +30,7 @@ private: std::set alertVolumeControlKeys = {"EngageVolume", "DisengageVolume", "RefuseVolume", "PromptVolume", "PromptDistractedVolume", "WarningSoftVolume", "WarningImmediateVolume"}; std::set customAlertsKeys = {}; - std::set customOnroadUIKeys = {"AccelerationPath", "BlindSpotPath"}; + std::set customOnroadUIKeys = {"AccelerationPath", "BlindSpotPath", "LeadInfo"}; std::set customThemeKeys = {"CustomColors", "CustomIcons", "CustomSignals", "CustomSounds"}; std::set modelUIKeys = {"DynamicPathWidth", "LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth", "UnlimitedLength"}; std::set qolKeys = {"DriveStats"}; diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index ba1748c..d8c5812 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -223,7 +223,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}); @@ -240,7 +240,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)) { @@ -250,12 +250,13 @@ void ExperimentalButton::updateState(const UIState &s) { } // FrogPilot variables + y_offset = leadInfo ? 10 : 0; } void ExperimentalButton::paintEvent(QPaintEvent *event) { QPainter p(this); QPixmap img = experimental_mode ? experimental_img : engage_img; - drawIcon(p, QPoint(btn_size / 2, btn_size / 2), img, QColor(0, 0, 0, 166), (isDown() || !(engageable || scene.always_on_lateral_active)) ? 0.6 : 1.0); + drawIcon(p, QPoint(btn_size / 2, btn_size / 2 + y_offset), img, QColor(0, 0, 0, 166), (isDown() || !(engageable || scene.always_on_lateral_active)) ? 0.6 : 1.0); } @@ -331,7 +332,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(); @@ -700,6 +701,28 @@ void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState } painter.drawPolygon(chevron, std::size(chevron)); + // Add lead info + if (leadInfo) { + // Declare the variables + float lead_speed = std::max(lead_data.getVLead(), 0.0f); // Ensure lead speed doesn't go under 0 m/s cause that's dumb + + // Form the text and center it below the chevron + 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); + + // Calculate the text starting position + 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(); } @@ -859,11 +882,19 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) { experimentalMode = scene.experimental_mode; + leadInfo = scene.lead_info; + obstacleDistance = scene.obstacle_distance; + obstacleDistanceStock = scene.obstacle_distance_stock; + mapOpen = scene.map_open; turnSignalLeft = scene.turn_signal_left; turnSignalRight = scene.turn_signal_right; + if (leadInfo) { + drawLeadInfo(p); + } + if (alwaysOnLateral || conditionalExperimental) { drawStatusBar(p); } @@ -912,6 +943,113 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) { } } +void AnnotatedCameraWidget::drawLeadInfo(QPainter &p) { + // Declare the variables + static QElapsedTimer timer; + static bool isFiveSecondsPassed = false; + constexpr int maxAccelDuration = 5000; + + // Constants for units and conversions + QString accelerationUnit = " m/s²"; + leadDistanceUnit = mapOpen ? "m" : "meters"; + leadSpeedUnit = "m/2"; + + float accelerationConversion = 1.0f; + distanceConversion = 1.0f; + speedConversion = 1.0f; + + if (!scene.use_si) { + if (is_metric) { + // Metric conversion + leadSpeedUnit = "kph"; + speedConversion = MS_TO_KPH; + } else { + // US imperial conversion + accelerationUnit = " ft/s²"; + leadDistanceUnit = mapOpen ? "ft" : "feet"; + leadSpeedUnit = "mph"; + + accelerationConversion = METER_TO_FOOT; + distanceConversion = METER_TO_FOOT; + speedConversion = MS_TO_MPH; + } + } + + // Update acceleration + double currentAcceleration = std::round(scene.acceleration * 100) / 100; + static double maxAcceleration = 0.0; + + if (currentAcceleration > maxAcceleration && status == STATUS_ENGAGED) { + maxAcceleration = currentAcceleration; + isFiveSecondsPassed = false; + timer.start(); + } else { + isFiveSecondsPassed = timer.hasExpired(maxAccelDuration); + } + + // Construct text segments + auto createText = [&](const QString &title, const double data) { + return title + QString::number(std::round(data * distanceConversion)) + " " + leadDistanceUnit; + }; + + // Create segments for insights + QString accelText = QString("Accel: %1%2") + .arg(currentAcceleration * accelerationConversion, 0, 'f', 2) + .arg(accelerationUnit); + + QString maxAccSuffix = QString(mapOpen ? "" : " - Max: %1%2") + .arg(maxAcceleration * accelerationConversion, 0, 'f', 2) + .arg(accelerationUnit); + + QString obstacleText = createText(mapOpen ? " | Obstacle: " : " | Obstacle Factor: ", obstacleDistance); + QString stopText = createText(mapOpen ? " - Stop: " : " - Stop Factor: ", scene.stopped_equivalence); + QString followText = " = " + createText(mapOpen ? "Follow: " : "Follow Distance: ", scene.desired_follow); + + // Check if the longitudinal toggles have an impact on the driving logics + 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(); + }; + + // Prepare rectangle for insights + 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(30, QFont::DemiBold)); + p.setRenderHint(QPainter::TextAntialiasing); + + // Calculate positioning for text drawing + QRect adjustedRect = insightsRect.adjusted(0, 27, 0, 27); + int textBaseLine = adjustedRect.y() + (adjustedRect.height() + p.fontMetrics().height()) / 2 - p.fontMetrics().descent(); + + // Calculate the entire text width to ensure perfect centering + 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; + + // Draw the text + 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); + drawText(maxAccSuffix, isFiveSecondsPassed ? Qt::white : Qt::red); + 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(); +} + void AnnotatedCameraWidget::drawStatusBar(QPainter &p) { p.save(); diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 664d219..9e2ca12 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -40,7 +40,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; @@ -54,6 +54,8 @@ private: // FrogPilot variables UIScene &scene; + + int y_offset; }; @@ -109,6 +111,7 @@ private: void initializeFrogPilotWidgets(); void updateFrogPilotWidgets(QPainter &p); + void drawLeadInfo(QPainter &p); void drawStatusBar(QPainter &p); void drawTurnSignals(QPainter &p); @@ -125,18 +128,27 @@ private: bool blindSpotRight; bool conditionalExperimental; bool experimentalMode; + bool leadInfo; bool mapOpen; bool turnSignalLeft; bool turnSignalRight; + float distanceConversion; + float speedConversion; + int cameraView; int conditionalSpeed; int conditionalSpeedLead; int conditionalStatus; int customColors; int customSignals; + int obstacleDistance; + int obstacleDistanceStock; int totalFrames = 8; + QString leadDistanceUnit; + QString leadSpeedUnit; + size_t animationFrameIndex; std::unordered_map>> themeConfiguration; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 120750b..7583ba4 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -246,6 +246,12 @@ static void update_state(UIState *s) { scene.lane_width_left = frogpilotPlan.getLaneWidthLeft(); scene.lane_width_right = frogpilotPlan.getLaneWidthRight(); } + if (scene.lead_info) { + scene.desired_follow = frogpilotPlan.getDesiredFollowDistance(); + scene.obstacle_distance = frogpilotPlan.getSafeObstacleDistance(); + scene.obstacle_distance_stock = frogpilotPlan.getSafeObstacleDistanceStock(); + scene.stopped_equivalence = frogpilotPlan.getStoppedEquivalenceFactor(); + } } if (sm.updated("liveLocationKalman")) { auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman(); @@ -286,6 +292,8 @@ void ui_update_frogpilot_params(UIState *s) { bool custom_onroad_ui = params.getBool("CustomUI"); scene.acceleration_path = custom_onroad_ui && params.getBool("AccelerationPath"); scene.blind_spot_path = custom_onroad_ui && params.getBool("BlindSpotPath"); + scene.lead_info = custom_onroad_ui && params.getBool("LeadInfo"); + scene.use_si = scene.lead_info && params.getBool("UseSI"); bool custom_theme = params.getBool("CustomTheme"); scene.custom_colors = custom_theme ? params.getInt("CustomColors") : 0; diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index e8ce438..7b59ef4 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -179,11 +179,13 @@ typedef struct UIScene { bool dynamic_path_width; bool enabled; bool experimental_mode; + bool lead_info; bool map_open; bool model_ui; bool turn_signal_left; bool turn_signal_right; bool unlimited_road_ui_length; + bool use_si; float lane_line_width; float lane_width_left; @@ -199,6 +201,10 @@ typedef struct UIScene { int custom_colors; int custom_icons; int custom_signals; + int desired_follow; + int obstacle_distance; + int obstacle_distance_stock; + int stopped_equivalence; QPolygonF track_adjacent_vertices[6]; QPolygonF track_edge_vertices;