brianhansonxyz/oscarpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Custom UI

Browse Source 
Added toggles to customize the lane lines, path, road edges, path edges, show the acceleration/deceleration on the path, lead info, driving logics, adjacent lanes, blind spot path, fps tracker, and an "Unlimited Length" mode that extends the road UI out as far as the model can see.
This commit is contained in:
FrogAi
2024-01-12 22:39:30 -07:00
parent fb8103b646
commit d6e17df710
16 changed files with 705 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

321
selfdrive/ui/qt/onroad.cc
View File

@@ -153,6 +153,61 @@ void OnroadWindow::paintEvent(QPaintEvent *event) {
QPainter p(this);
p.fillRect(rect(), QColor(bg.red(), bg.green(), bg.blue(), 255));
// Draw FPS on screen
if (scene.show_fps) {
constexpr double minAllowedFPS = 0.1;
constexpr double maxAllowedFPS = 99.9;
constexpr qint64 oneMinuteInMilliseconds = 60000;
static double avgFPS;
static double maxFPS;
static double minFPS;
static double totalFPS;
static qint64 frameCount;
// Store the last reset time
static qint64 lastResetTime = QDateTime::currentMSecsSinceEpoch();
const qint64 currentMillis = QDateTime::currentMSecsSinceEpoch();
// Reset the counter if it's been 60 seconds
if (currentMillis - lastResetTime >= oneMinuteInMilliseconds) {
avgFPS = 0;
frameCount = 0;
minFPS = maxAllowedFPS;
maxFPS = minAllowedFPS;
totalFPS = 0;
lastResetTime = currentMillis;
}
// Update the FPS variables
fps = qBound(minAllowedFPS, fps, maxAllowedFPS);
minFPS = qMin(minFPS, fps);
maxFPS = qMax(maxFPS, fps);
frameCount++;
totalFPS += fps;
avgFPS = totalFPS / frameCount;
update();
// Text declarations
p.setFont(InterFont(30, QFont::DemiBold));
p.setRenderHint(QPainter::TextAntialiasing);
p.setPen(Qt::white);
// Construct the FPS display string
QString fpsDisplayString = QString("FPS: %1 (%2) | Min: %3 | Max: %4 | Avg: %5")
.arg(fps, 0, 'f', 2)
.arg(paramsMemory.getInt("CameraFPS"))
.arg(minFPS, 0, 'f', 2)
.arg(maxFPS, 0, 'f', 2)
.arg(avgFPS, 0, 'f', 2);
// Calculate text positioning
const QRect currentRect = rect();
const int textWidth = p.fontMetrics().horizontalAdvance(fpsDisplayString);
const int xPos = (currentRect.width() - textWidth) / 2;
const int yPos = currentRect.bottom() - 5;
p.drawText(xPos, yPos, fpsDisplayString);
}
}
// ***** onroad widgets *****
@@ -226,7 +281,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});
@@ -243,7 +298,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();
if ((cs.getExperimentalMode() != experimental_mode) || (eng != engageable)) {
@@ -253,12 +308,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);
}
@@ -334,7 +390,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();
@@ -504,7 +560,7 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
// paint path
QLinearGradient bg(0, height(), 0, 0);
if (sm["controlsState"].getControlsState().getExperimentalMode()) {
if (sm["controlsState"].getControlsState().getExperimentalMode() || accelerationPath) {
// The first half of track_vertices are the points for the right side of the path
// and the indices match the positions of accel from uiPlan
const auto &acceleration = sm["uiPlan"].getUiPlan().getAccel();
@@ -540,6 +596,97 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
painter.setBrush(bg);
painter.drawPolygon(scene.track_vertices);
// create new path with track vertices and track edge vertices
QPainterPath path;
path.addPolygon(scene.track_vertices);
path.addPolygon(scene.track_edge_vertices);
// paint path edges
QLinearGradient pe(0, height(), 0, 0);
if (alwaysOnLateral) {
pe.setColorAt(0.0, QColor::fromHslF(178 / 360., 0.90, 0.38, 1.0));
pe.setColorAt(0.5, QColor::fromHslF(178 / 360., 0.90, 0.38, 0.5));
pe.setColorAt(1.0, QColor::fromHslF(178 / 360., 0.90, 0.38, 0.1));
} else if (conditionalStatus == 1 || conditionalStatus == 3) {
pe.setColorAt(0.0, QColor::fromHslF(58 / 360., 1.00, 0.50, 1.0));
pe.setColorAt(0.5, QColor::fromHslF(58 / 360., 1.00, 0.50, 0.5));
pe.setColorAt(1.0, QColor::fromHslF(58 / 360., 1.00, 0.50, 0.1));
} else if (experimentalMode) {
pe.setColorAt(0.0, QColor::fromHslF(25 / 360., 0.71, 0.50, 1.0));
pe.setColorAt(0.5, QColor::fromHslF(25 / 360., 0.71, 0.50, 0.5));
pe.setColorAt(1.0, QColor::fromHslF(25 / 360., 0.71, 0.50, 0.1));
} else if (scene.navigate_on_openpilot) {
pe.setColorAt(0.0, QColor::fromHslF(205 / 360., 0.85, 0.56, 1.0));
pe.setColorAt(0.5, QColor::fromHslF(205 / 360., 0.85, 0.56, 0.5));
pe.setColorAt(1.0, QColor::fromHslF(205 / 360., 0.85, 0.56, 0.1));
} else {
pe.setColorAt(0.0, QColor::fromHslF(148 / 360., 0.94, 0.51, 1.0));
pe.setColorAt(0.5, QColor::fromHslF(112 / 360., 1.00, 0.68, 0.5));
pe.setColorAt(1.0, QColor::fromHslF(112 / 360., 1.00, 0.68, 0.1));
}
painter.setBrush(pe);
painter.drawPath(path);
// paint blindspot path
QLinearGradient bs(0, height(), 0, 0);
if (blindSpotLeft || blindSpotRight) {
bs.setColorAt(0.0, QColor::fromHslF(0 / 360., 0.75, 0.50, 0.6));
bs.setColorAt(0.5, QColor::fromHslF(0 / 360., 0.75, 0.50, 0.4));
bs.setColorAt(1.0, QColor::fromHslF(0 / 360., 0.75, 0.50, 0.2));
}
painter.setBrush(bs);
if (blindSpotLeft) {
painter.drawPolygon(scene.track_adjacent_vertices[4]);
}
if (blindSpotRight) {
painter.drawPolygon(scene.track_adjacent_vertices[5]);
}
// paint adjacent lane paths
if (adjacentPath && (laneWidthLeft != 0 || laneWidthRight != 0)) {
// Set up the units
double conversionFactor = is_metric ? 1.0 : 3.28084;
QString unit_d = is_metric ? " meters" : " feet";
// Declare the lane width thresholds
constexpr float minLaneWidth = 2.5f;
constexpr float maxLaneWidth = 3.5f;
// Set gradient colors based on laneWidth and blindspot
auto setGradientColors = [](QLinearGradient &gradient, float laneWidth, bool blindspot) {
// Make the path red for smaller paths or if there's a car in the blindspot and green for larger paths
double hue = (laneWidth < minLaneWidth || blindspot) ? 0.0 :
(laneWidth >= maxLaneWidth) ? 120.0 :
120.0 * (laneWidth - minLaneWidth) / (maxLaneWidth - minLaneWidth);
auto hue_ratio = hue / 360.0;
gradient.setColorAt(0.0, QColor::fromHslF(hue_ratio, 0.75, 0.50, 0.6));
gradient.setColorAt(0.5, QColor::fromHslF(hue_ratio, 0.75, 0.50, 0.4));
gradient.setColorAt(1.0, QColor::fromHslF(hue_ratio, 0.75, 0.50, 0.2));
};
// Paint the lanes
auto paintLane = [&](QPainter &painter, const QPolygonF &lane, const float laneWidth, const bool blindspot) {
QLinearGradient gradient(0, height(), 0, 0);
setGradientColors(gradient, laneWidth, blindspot);
painter.setFont(InterFont(30, QFont::DemiBold));
painter.setBrush(gradient);
painter.setPen(Qt::transparent);
painter.drawPolygon(lane);
painter.setPen(Qt::white);
QRectF boundingRect = lane.boundingRect();
painter.drawText(boundingRect.center(), blindspot ? "Vehicle in blind spot" :
QString("%1%2").arg(laneWidth * conversionFactor, 0, 'f', 2).arg(unit_d));
painter.setPen(Qt::NoPen);
};
// Paint lanes
paintLane(painter, scene.track_adjacent_vertices[4], laneWidthLeft, blindSpotLeft);
paintLane(painter, scene.track_adjacent_vertices[5], laneWidthRight, blindSpotRight);
}
painter.restore();
}
@@ -616,6 +763,47 @@ void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState
painter.setBrush(redColor(fillAlpha));
painter.drawPolygon(chevron, std::size(chevron));
// Add lead info
if (leadInfo) {
// Declare the variables
QString unit_d = "meters";
QString unit_s = "km/h";
float distance = d_rel;
float lead_speed = std::max(lead_data.getVLead(), 0.0f); // Ensure speed doesn't go under 0 m/s cause that's dumb
// Conversion factors and units
constexpr float toFeet = 3.28084f;
constexpr float toMph = 2.23694f;
constexpr float toKmph = 3.6f;
// Metric speed conversion
if (is_metric || useSI) {
lead_speed *= toKmph;
} else {
// US imperial conversion
distance *= toFeet;
lead_speed *= toMph;
unit_d = "feet";
unit_s = "mph";
}
// 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(distance, 0, 'f', 2, '0')
.arg(unit_d)
.arg(lead_speed, 0, 'f', 2, '0')
.arg(unit_s);
// 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();
}
@@ -655,6 +843,7 @@ void AnnotatedCameraWidget::paintGL() {
// for replay of old routes, never go to widecam
wide_cam_requested = wide_cam_requested && s->scene.calibration_wide_valid;
}
paramsMemory.putBoolNonBlocking("WideCamera", wide_cam_requested);
CameraWidget::setStreamType(cameraView == 3 ? VISION_STREAM_DRIVER :
wide_cam_requested && cameraView != 1 ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD);
@@ -701,7 +890,7 @@ void AnnotatedCameraWidget::paintGL() {
double cur_draw_t = millis_since_boot();
double dt = cur_draw_t - prev_draw_t;
double fps = fps_filter.update(1. / dt * 1000);
fps = fps_filter.update(1. / dt * 1000);
if (fps < 15) {
LOGW("slow frame rate: %.2f fps", fps);
}
@@ -740,13 +929,29 @@ void AnnotatedCameraWidget::initializeFrogPilotWidgets() {
}
void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) {
accelerationPath = scene.acceleration_path;
adjacentPath = scene.adjacent_path;
alwaysOnLateral = scene.always_on_lateral_active;
blindSpotLeft = scene.blind_spot_left;
blindSpotRight = scene.blind_spot_right;
cameraView = scene.camera_view;
conditionalExperimental = scene.conditional_experimental;
conditionalSpeed = scene.conditional_speed;
conditionalSpeedLead = scene.conditional_speed_lead;
conditionalStatus = scene.conditional_status;
desiredFollow = scene.desired_follow;
experimentalMode = scene.experimental_mode;
laneWidthLeft = scene.lane_width_left;
laneWidthRight = scene.lane_width_right;
leadInfo = scene.lead_info;
obstacleDistance = scene.obstacle_distance;
obstacleDistanceStock = scene.obstacle_distance_stock;
stoppedEquivalence = scene.stopped_equivalence;
useSI = scene.use_si;
if (leadInfo) {
drawLeadInfo(p);
}
if (alwaysOnLateral || conditionalExperimental) {
drawStatusBar(p);
@@ -759,6 +964,110 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) {
}
}
void AnnotatedCameraWidget::drawLeadInfo(QPainter &p) {
SubMaster &sm = *uiState()->sm;
// Declare the variables
static QElapsedTimer timer;
static bool isFiveSecondsPassed = false;
constexpr int maxAccelDuration = 5000;
// Constants for units and conversions
QString unit_a = " m/s²";
QString unit_d = "meters";
QString unit_s = "kmh";
float distanceConversion = 1.0f;
float speedConversion = 1.0f;
// Conversion factors and units
constexpr float toFeet = 3.28084f;
constexpr float toMph = 2.23694f;
// Metric speed conversion
if (!(is_metric || useSI)) {
// US imperial conversion
unit_a = " ft/s²";
unit_d = "feet";
unit_s = "mph";
distanceConversion = toFeet;
speedConversion = toMph;
}
// Update acceleration
double currentAcceleration = std::round(sm["carState"].getCarState().getAEgo() * 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)) + " " + unit_d;
};
// Create segments for insights
QString accelText = QString("Accel: %1%2")
.arg(currentAcceleration * speedConversion, 0, 'f', 2)
.arg(unit_a);
QString maxAccSuffix = QString(" - Max: %1%2")
.arg(maxAcceleration * speedConversion, 0, 'f', 2)
.arg(unit_a);
QString obstacleText = createText(" | Obstacle Factor: ", obstacleDistance);
QString stopText = createText(" - Stop Factor: ", stoppedEquivalence);
QString followText = " = " + createText("Follow Distance: ", desiredFollow);
// 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();