mawei
/
openpilot1
mirror of https://github.com/huifan0114/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Visuals - Developer UI - Longitudinal Metrics - Lead Info

This commit is contained in:
FrogAi 2024-07-07 05:46:10 -07:00
parent 0fbb4ddf8a
commit be5fc3d4ff
7 changed files with 159 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

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

120
selfdrive/ui/qt/onroad/annotated_camera.cc
View File

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

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

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

11
selfdrive/ui/qt/onroad/buttons.cc
View File

@ -33,7 +33,7 @@ void drawIconGif(QPainter &p, const QPoint &center, 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);
}
}

3
selfdrive/ui/qt/onroad/buttons.h
View File

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

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

6
selfdrive/ui/ui.h
View File

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