zhudongjer/carrot
1
1
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

Visuals - Developer UI - Longitudinal Metrics

Browse Source 
Display various metrics related to the longitudinal performance of openpilot.
This commit is contained in:
FrogAi 2024-05-22 21:03:08 -07:00
parent 119e63952b
commit 72aa33447e
5 changed files with 209 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
selfdrive/frogpilot/controls/frogpilot_planner.py
View File

@ -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

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

@ -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);

9
selfdrive/ui/qt/onroad.h
View File

@ -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);

11
selfdrive/ui/ui.cc
View File

@ -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");

11
selfdrive/ui/ui.h
View File

@ -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];