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

openpilot v0.9.6 release

Browse Source 
date: 2024-01-12T10:13:37
master commit: ba792d576a49a0899b88a753fa1c52956bedf9e6
This commit is contained in:
FrogAi
2024-01-12 22:39:28 -07:00
commit 08e9fb1edc
1881 changed files with 653708 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

393
selfdrive/ui/qt/maps/map.cc Normal file
View File

@@ -0,0 +1,393 @@
#include "selfdrive/ui/qt/maps/map.h"
#include <algorithm>
#include <eigen3/Eigen/Dense>
#include <QDebug>
#include "selfdrive/ui/qt/maps/map_helpers.h"
#include "selfdrive/ui/qt/util.h"
#include "selfdrive/ui/ui.h"
const int INTERACTION_TIMEOUT = 100;
const float MAX_ZOOM = 17;
const float MIN_ZOOM = 14;
const float MAX_PITCH = 50;
const float MIN_PITCH = 0;
const float MAP_SCALE = 2;
MapWindow::MapWindow(const QMapboxGLSettings &settings) : m_settings(settings), velocity_filter(0, 10, 0.05, false) {
QObject::connect(uiState(), &UIState::uiUpdate, this, &MapWindow::updateState);
map_overlay = new QWidget (this);
map_overlay->setAttribute(Qt::WA_TranslucentBackground, true);
QVBoxLayout *overlay_layout = new QVBoxLayout(map_overlay);
overlay_layout->setContentsMargins(0, 0, 0, 0);
// Instructions
map_instructions = new MapInstructions(this);
map_instructions->setVisible(false);
map_eta = new MapETA(this);
map_eta->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
map_eta->setFixedHeight(120);
error = new QLabel(this);
error->setStyleSheet(R"(color:white;padding:50px 11px;font-size: 90px; background-color:rgba(0, 0, 0, 150);)");
error->setAlignment(Qt::AlignCenter);
overlay_layout->addWidget(error);
overlay_layout->addWidget(map_instructions);
overlay_layout->addStretch(1);
overlay_layout->addWidget(map_eta);
last_position = coordinate_from_param("LastGPSPosition");
grabGesture(Qt::GestureType::PinchGesture);
qDebug() << "MapWindow initialized";
}
MapWindow::~MapWindow() {
makeCurrent();
}
void MapWindow::initLayers() {
// This doesn't work from initializeGL
if (!m_map->layerExists("modelPathLayer")) {
qDebug() << "Initializing modelPathLayer";
QVariantMap modelPath;
modelPath["id"] = "modelPathLayer";
modelPath["type"] = "line";
modelPath["source"] = "modelPathSource";
m_map->addLayer(modelPath);
m_map->setPaintProperty("modelPathLayer", "line-color", QColor("red"));
m_map->setPaintProperty("modelPathLayer", "line-width", 5.0);
m_map->setLayoutProperty("modelPathLayer", "line-cap", "round");
}
if (!m_map->layerExists("navLayer")) {
qDebug() << "Initializing navLayer";
QVariantMap nav;
nav["id"] = "navLayer";
nav["type"] = "line";
nav["source"] = "navSource";
m_map->addLayer(nav, "road-intersection");
QVariantMap transition;
transition["duration"] = 400; // ms
m_map->setPaintProperty("navLayer", "line-color", getNavPathColor(uiState()->scene.navigate_on_openpilot));
m_map->setPaintProperty("navLayer", "line-color-transition", transition);
m_map->setPaintProperty("navLayer", "line-width", 7.5);
m_map->setLayoutProperty("navLayer", "line-cap", "round");
}
if (!m_map->layerExists("pinLayer")) {
qDebug() << "Initializing pinLayer";
m_map->addImage("default_marker", QImage("../assets/navigation/default_marker.svg"));
QVariantMap pin;
pin["id"] = "pinLayer";
pin["type"] = "symbol";
pin["source"] = "pinSource";
m_map->addLayer(pin);
m_map->setLayoutProperty("pinLayer", "icon-pitch-alignment", "viewport");
m_map->setLayoutProperty("pinLayer", "icon-image", "default_marker");
m_map->setLayoutProperty("pinLayer", "icon-ignore-placement", true);
m_map->setLayoutProperty("pinLayer", "icon-allow-overlap", true);
m_map->setLayoutProperty("pinLayer", "symbol-sort-key", 0);
m_map->setLayoutProperty("pinLayer", "icon-anchor", "bottom");
}
if (!m_map->layerExists("carPosLayer")) {
qDebug() << "Initializing carPosLayer";
m_map->addImage("label-arrow", QImage("../assets/images/triangle.svg"));
QVariantMap carPos;
carPos["id"] = "carPosLayer";
carPos["type"] = "symbol";
carPos["source"] = "carPosSource";
m_map->addLayer(carPos);
m_map->setLayoutProperty("carPosLayer", "icon-pitch-alignment", "map");
m_map->setLayoutProperty("carPosLayer", "icon-image", "label-arrow");
m_map->setLayoutProperty("carPosLayer", "icon-size", 0.5);
m_map->setLayoutProperty("carPosLayer", "icon-ignore-placement", true);
m_map->setLayoutProperty("carPosLayer", "icon-allow-overlap", true);
// TODO: remove, symbol-sort-key does not seem to matter outside of each layer
m_map->setLayoutProperty("carPosLayer", "symbol-sort-key", 0);
}
}
void MapWindow::updateState(const UIState &s) {
if (!uiState()->scene.started) {
return;
}
const SubMaster &sm = *(s.sm);
update();
if (sm.updated("modelV2")) {
// set path color on change, and show map on rising edge of navigate on openpilot
bool nav_enabled = sm["modelV2"].getModelV2().getNavEnabled() &&
sm["controlsState"].getControlsState().getEnabled();
if (nav_enabled != uiState()->scene.navigate_on_openpilot) {
if (loaded_once) {
m_map->setPaintProperty("navLayer", "line-color", getNavPathColor(nav_enabled));
}
if (nav_enabled) {
emit requestVisible(true);
}
}
uiState()->scene.navigate_on_openpilot = nav_enabled;
}
if (sm.updated("liveLocationKalman")) {
auto locationd_location = sm["liveLocationKalman"].getLiveLocationKalman();
auto locationd_pos = locationd_location.getPositionGeodetic();
auto locationd_orientation = locationd_location.getCalibratedOrientationNED();
auto locationd_velocity = locationd_location.getVelocityCalibrated();
// Check std norm
auto pos_ecef_std = locationd_location.getPositionECEF().getStd();
bool pos_accurate_enough = sqrt(pow(pos_ecef_std[0], 2) + pow(pos_ecef_std[1], 2) + pow(pos_ecef_std[2], 2)) < 100;
locationd_valid = (locationd_pos.getValid() && locationd_orientation.getValid() && locationd_velocity.getValid() && pos_accurate_enough);
if (locationd_valid) {
last_position = QMapbox::Coordinate(locationd_pos.getValue()[0], locationd_pos.getValue()[1]);
last_bearing = RAD2DEG(locationd_orientation.getValue()[2]);
velocity_filter.update(std::max(10.0, locationd_velocity.getValue()[0]));
}
}
if (sm.updated("navRoute") && sm["navRoute"].getNavRoute().getCoordinates().size()) {
auto nav_dest = coordinate_from_param("NavDestination");
bool allow_open = std::exchange(last_valid_nav_dest, nav_dest) != nav_dest &&
nav_dest && !isVisible();
qWarning() << "Got new navRoute from navd. Opening map:" << allow_open;
// Show map on destination set/change
if (allow_open) {
emit requestSettings(false);
emit requestVisible(true);
}
}
loaded_once = loaded_once || (m_map && m_map->isFullyLoaded());
if (!loaded_once) {
setError(tr("Map Loading"));
return;
}
initLayers();
if (!locationd_valid) {
setError(tr("Waiting for GPS"));
} else if (routing_problem) {
setError(tr("Waiting for route"));
} else {
setError("");
}
if (locationd_valid) {
// Update current location marker
auto point = coordinate_to_collection(*last_position);
QMapbox::Feature feature1(QMapbox::Feature::PointType, point, {}, {});
QVariantMap carPosSource;
carPosSource["type"] = "geojson";
carPosSource["data"] = QVariant::fromValue<QMapbox::Feature>(feature1);
m_map->updateSource("carPosSource", carPosSource);
// Map bearing isn't updated when interacting, keep location marker up to date
if (last_bearing) {
m_map->setLayoutProperty("carPosLayer", "icon-rotate", *last_bearing - m_map->bearing());
}
}
if (interaction_counter == 0) {
if (last_position) m_map->setCoordinate(*last_position);
if (last_bearing) m_map->setBearing(*last_bearing);
m_map->setZoom(util::map_val<float>(velocity_filter.x(), 0, 30, MAX_ZOOM, MIN_ZOOM));
} else {
interaction_counter--;
}
if (sm.updated("navInstruction")) {
// an invalid navInstruction packet with a nav destination is only possible if:
// - API exception/no internet
// - route response is empty
// - any time navd is waiting for recompute_countdown
routing_problem = !sm.valid("navInstruction") && coordinate_from_param("NavDestination").has_value();
if (sm.valid("navInstruction")) {
auto i = sm["navInstruction"].getNavInstruction();
map_eta->updateETA(i.getTimeRemaining(), i.getTimeRemainingTypical(), i.getDistanceRemaining());
if (locationd_valid) {
m_map->setPitch(MAX_PITCH); // TODO: smooth pitching based on maneuver distance
map_instructions->updateInstructions(i);
}
} else {
clearRoute();
}
}
if (sm.rcv_frame("navRoute") != route_rcv_frame) {
qWarning() << "Updating navLayer with new route";
auto route = sm["navRoute"].getNavRoute();
auto route_points = capnp_coordinate_list_to_collection(route.getCoordinates());
QMapbox::Feature feature(QMapbox::Feature::LineStringType, route_points, {}, {});
QVariantMap navSource;
navSource["type"] = "geojson";
navSource["data"] = QVariant::fromValue<QMapbox::Feature>(feature);
m_map->updateSource("navSource", navSource);
m_map->setLayoutProperty("navLayer", "visibility", "visible");
route_rcv_frame = sm.rcv_frame("navRoute");
updateDestinationMarker();
}
}
void MapWindow::setError(const QString &err_str) {
if (err_str != error->text()) {
error->setText(err_str);
error->setVisible(!err_str.isEmpty());
if (!err_str.isEmpty()) map_instructions->setVisible(false);
}
}
void MapWindow::resizeGL(int w, int h) {
m_map->resize(size() / MAP_SCALE);
map_overlay->setFixedSize(width(), height());
}
void MapWindow::initializeGL() {
m_map.reset(new QMapboxGL(this, m_settings, size(), 1));
if (last_position) {
m_map->setCoordinateZoom(*last_position, MAX_ZOOM);
} else {
m_map->setCoordinateZoom(QMapbox::Coordinate(64.31990695292795, -149.79038934046247), MIN_ZOOM);
}
m_map->setMargins({0, 350, 0, 50});
m_map->setPitch(MIN_PITCH);
m_map->setStyleUrl("mapbox://styles/commaai/clkqztk0f00ou01qyhsa5bzpj");
QObject::connect(m_map.data(), &QMapboxGL::mapChanged, [=](QMapboxGL::MapChange change) {
// set global animation duration to 0 ms so visibility changes are instant
if (change == QMapboxGL::MapChange::MapChangeDidFinishLoadingStyle) {
m_map->setTransitionOptions(0, 0);
}
if (change == QMapboxGL::MapChange::MapChangeDidFinishLoadingMap) {
loaded_once = true;
}
});
}
void MapWindow::paintGL() {
if (!isVisible() || m_map.isNull()) return;
m_map->render();
}
void MapWindow::clearRoute() {
if (!m_map.isNull()) {
m_map->setLayoutProperty("navLayer", "visibility", "none");
m_map->setPitch(MIN_PITCH);
updateDestinationMarker();
}
map_instructions->setVisible(false);
map_eta->setVisible(false);
last_valid_nav_dest = std::nullopt;
}
void MapWindow::mousePressEvent(QMouseEvent *ev) {
m_lastPos = ev->localPos();
ev->accept();
}
void MapWindow::mouseDoubleClickEvent(QMouseEvent *ev) {
if (last_position) m_map->setCoordinate(*last_position);
if (last_bearing) m_map->setBearing(*last_bearing);
m_map->setZoom(util::map_val<float>(velocity_filter.x(), 0, 30, MAX_ZOOM, MIN_ZOOM));
update();
interaction_counter = 0;
}
void MapWindow::mouseMoveEvent(QMouseEvent *ev) {
QPointF delta = ev->localPos() - m_lastPos;
if (!delta.isNull()) {
interaction_counter = INTERACTION_TIMEOUT;
m_map->moveBy(delta / MAP_SCALE);
update();
}
m_lastPos = ev->localPos();
ev->accept();
}
void MapWindow::wheelEvent(QWheelEvent *ev) {
if (ev->orientation() == Qt::Horizontal) {
return;
}
float factor = ev->delta() / 1200.;
if (ev->delta() < 0) {
factor = factor > -1 ? factor : 1 / factor;
}
m_map->scaleBy(1 + factor, ev->pos() / MAP_SCALE);
update();
interaction_counter = INTERACTION_TIMEOUT;
ev->accept();
}
bool MapWindow::event(QEvent *event) {
if (event->type() == QEvent::Gesture) {
return gestureEvent(static_cast<QGestureEvent*>(event));
}
return QWidget::event(event);
}
bool MapWindow::gestureEvent(QGestureEvent *event) {
if (QGesture *pinch = event->gesture(Qt::PinchGesture)) {
pinchTriggered(static_cast<QPinchGesture *>(pinch));
}
return true;
}
void MapWindow::pinchTriggered(QPinchGesture *gesture) {
QPinchGesture::ChangeFlags changeFlags = gesture->changeFlags();
if (changeFlags & QPinchGesture::ScaleFactorChanged) {
// TODO: figure out why gesture centerPoint doesn't work
m_map->scaleBy(gesture->scaleFactor(), {width() / 2.0 / MAP_SCALE, height() / 2.0 / MAP_SCALE});
update();
interaction_counter = INTERACTION_TIMEOUT;
}
}
void MapWindow::offroadTransition(bool offroad) {
if (offroad) {
clearRoute();
uiState()->scene.navigate_on_openpilot = false;
routing_problem = false;
} else {
auto dest = coordinate_from_param("NavDestination");
emit requestVisible(dest.has_value());
}
last_bearing = {};
}
void MapWindow::updateDestinationMarker() {
auto nav_dest = coordinate_from_param("NavDestination");
if (nav_dest.has_value()) {
auto point = coordinate_to_collection(*nav_dest);
QMapbox::Feature feature(QMapbox::Feature::PointType, point, {}, {});
QVariantMap pinSource;
pinSource["type"] = "geojson";
pinSource["data"] = QVariant::fromValue<QMapbox::Feature>(feature);
m_map->updateSource("pinSource", pinSource);
m_map->setPaintProperty("pinLayer", "visibility", "visible");
} else {
m_map->setPaintProperty("pinLayer", "visibility", "none");
}
}

89
selfdrive/ui/qt/maps/map.h Normal file
View File

@@ -0,0 +1,89 @@
#pragma once
#include <optional>
#include <QGeoCoordinate>
#include <QGestureEvent>
#include <QLabel>
#include <QMap>
#include <QMapboxGL>
#include <QMouseEvent>
#include <QOpenGLWidget>
#include <QPixmap>
#include <QPushButton>
#include <QScopedPointer>
#include <QString>
#include <QVBoxLayout>
#include <QWheelEvent>
#include "cereal/messaging/messaging.h"
#include "common/params.h"
#include "common/util.h"
#include "selfdrive/ui/ui.h"
#include "selfdrive/ui/qt/maps/map_eta.h"
#include "selfdrive/ui/qt/maps/map_instructions.h"
class MapWindow : public QOpenGLWidget {
Q_OBJECT
public:
MapWindow(const QMapboxGLSettings &);
~MapWindow();
private:
void initializeGL() final;
void paintGL() final;
void resizeGL(int w, int h) override;
QMapboxGLSettings m_settings;
QScopedPointer<QMapboxGL> m_map;
void initLayers();
void mousePressEvent(QMouseEvent *ev) final;
void mouseDoubleClickEvent(QMouseEvent *ev) final;
void mouseMoveEvent(QMouseEvent *ev) final;
void wheelEvent(QWheelEvent *ev) final;
bool event(QEvent *event) final;
bool gestureEvent(QGestureEvent *event);
void pinchTriggered(QPinchGesture *gesture);
void setError(const QString &err_str);
bool loaded_once = false;
// Panning
QPointF m_lastPos;
int interaction_counter = 0;
// Position
std::optional<QMapbox::Coordinate> last_valid_nav_dest;
std::optional<QMapbox::Coordinate> last_position;
std::optional<float> last_bearing;
FirstOrderFilter velocity_filter;
bool locationd_valid = false;
bool routing_problem = false;
QWidget *map_overlay;
QLabel *error;
MapInstructions* map_instructions;
MapETA* map_eta;
// Blue with normal nav, green when nav is input into the model
QColor getNavPathColor(bool nav_enabled) {
return nav_enabled ? QColor("#31ee73") : QColor("#31a1ee");
}
void clearRoute();
void updateDestinationMarker();
uint64_t route_rcv_frame = 0;
private slots:
void updateState(const UIState &s);
public slots:
void offroadTransition(bool offroad);
signals:
void requestVisible(bool visible);
void requestSettings(bool settings);
};

56
selfdrive/ui/qt/maps/map_eta.cc Normal file
View File

@@ -0,0 +1,56 @@
#include "selfdrive/ui/qt/maps/map_eta.h"
#include <QDateTime>
#include <QPainter>
#include "selfdrive/ui/qt/maps/map_helpers.h"
#include "selfdrive/ui/ui.h"
const float MANEUVER_TRANSITION_THRESHOLD = 10;
MapETA::MapETA(QWidget *parent) : QWidget(parent) {
setVisible(false);
setAttribute(Qt::WA_TranslucentBackground);
eta_doc.setUndoRedoEnabled(false);
eta_doc.setDefaultStyleSheet("body {font-family:Inter;font-size:70px;color:white;} b{font-weight:600;} td{padding:0 3px;}");
}
void MapETA::paintEvent(QPaintEvent *event) {
if (!eta_doc.isEmpty()) {
QPainter p(this);
p.setRenderHint(QPainter::Antialiasing);
p.setPen(Qt::NoPen);
p.setBrush(QColor(0, 0, 0, 255));
QSizeF txt_size = eta_doc.size();
p.drawRoundedRect((width() - txt_size.width()) / 2 - UI_BORDER_SIZE, 0, txt_size.width() + UI_BORDER_SIZE * 2, height() + 25, 25, 25);
p.translate((width() - txt_size.width()) / 2, (height() - txt_size.height()) / 2);
eta_doc.drawContents(&p);
}
}
void MapETA::updateETA(float s, float s_typical, float d) {
// ETA
auto eta_t = QDateTime::currentDateTime().addSecs(s).time();
auto eta = format_24h ? std::pair{eta_t.toString("HH:mm"), tr("eta")}
: std::pair{eta_t.toString("h:mm a").split(' ')[0], eta_t.toString("a")};
// Remaining time
auto remaining = s < 3600 ? std::pair{QString::number(int(s / 60)), tr("min")}
: std::pair{QString("%1:%2").arg((int)s / 3600).arg(((int)s % 3600) / 60, 2, 10, QLatin1Char('0')), tr("hr")};
QString color = "#25DA6E";
if (s / s_typical > 1.5)
color = "#DA3025";
else if (s / s_typical > 1.2)
color = "#DAA725";
// Distance
auto distance = map_format_distance(d, uiState()->scene.is_metric);
eta_doc.setHtml(QString(R"(<body><table><tr style="vertical-align:bottom;"><td><b>%1</b></td><td>%2</td>
<td style="padding-left:40px;color:%3;"><b>%4</b></td><td style="padding-right:40px;color:%3;">%5</td>
<td><b>%6</b></td><td>%7</td></tr></body>)")
.arg(eta.first, eta.second, color, remaining.first, remaining.second, distance.first, distance.second));
setVisible(d >= MANEUVER_TRANSITION_THRESHOLD);
update();
}

23
selfdrive/ui/qt/maps/map_eta.h Normal file
View File

@@ -0,0 +1,23 @@
#pragma once
#include <QPaintEvent>
#include <QTextDocument>
#include <QWidget>
#include "common/params.h"
class MapETA : public QWidget {
Q_OBJECT
public:
MapETA(QWidget * parent=nullptr);
void updateETA(float seconds, float seconds_typical, float distance);
private:
void paintEvent(QPaintEvent *event) override;
void showEvent(QShowEvent *event) override { format_24h = param.getBool("NavSettingTime24h"); }
bool format_24h = false;
QTextDocument eta_doc;
Params param;
};

152
selfdrive/ui/qt/maps/map_helpers.cc Normal file
View File

@@ -0,0 +1,152 @@
#include "selfdrive/ui/qt/maps/map_helpers.h"
#include <algorithm>
#include <string>
#include <utility>
#include <QJsonDocument>
#include <QJsonObject>
#include "common/params.h"
#include "system/hardware/hw.h"
#include "selfdrive/ui/qt/api.h"
QString get_mapbox_token() {
// Valid for 4 weeks since we can't swap tokens on the fly
return MAPBOX_TOKEN.isEmpty() ? CommaApi::create_jwt({}, 4 * 7 * 24 * 3600) : MAPBOX_TOKEN;
}
QMapboxGLSettings get_mapbox_settings() {
QMapboxGLSettings settings;
if (!Hardware::PC()) {
settings.setCacheDatabasePath(MAPS_CACHE_PATH);
settings.setCacheDatabaseMaximumSize(100 * 1024 * 1024);
}
settings.setApiBaseUrl(MAPS_HOST);
settings.setAccessToken(get_mapbox_token());
return settings;
}
QGeoCoordinate to_QGeoCoordinate(const QMapbox::Coordinate &in) {
return QGeoCoordinate(in.first, in.second);
}
QMapbox::CoordinatesCollections model_to_collection(
const cereal::LiveLocationKalman::Measurement::Reader &calibratedOrientationECEF,
const cereal::LiveLocationKalman::Measurement::Reader &positionECEF,
const cereal::XYZTData::Reader &line){
Eigen::Vector3d ecef(positionECEF.getValue()[0], positionECEF.getValue()[1], positionECEF.getValue()[2]);
Eigen::Vector3d orient(calibratedOrientationECEF.getValue()[0], calibratedOrientationECEF.getValue()[1], calibratedOrientationECEF.getValue()[2]);
Eigen::Matrix3d ecef_from_local = euler2rot(orient);
QMapbox::Coordinates coordinates;
auto x = line.getX();
auto y = line.getY();
auto z = line.getZ();
for (int i = 0; i < x.size(); i++) {
Eigen::Vector3d point_ecef = ecef_from_local * Eigen::Vector3d(x[i], y[i], z[i]) + ecef;
Geodetic point_geodetic = ecef2geodetic((ECEF){.x = point_ecef[0], .y = point_ecef[1], .z = point_ecef[2]});
coordinates.push_back({point_geodetic.lat, point_geodetic.lon});
}
return {QMapbox::CoordinatesCollection{coordinates}};
}
QMapbox::CoordinatesCollections coordinate_to_collection(const QMapbox::Coordinate &c) {
QMapbox::Coordinates coordinates{c};
return {QMapbox::CoordinatesCollection{coordinates}};
}
QMapbox::CoordinatesCollections capnp_coordinate_list_to_collection(const capnp::List<cereal::NavRoute::Coordinate>::Reader& coordinate_list) {
QMapbox::Coordinates coordinates;
for (auto const &c : coordinate_list) {
coordinates.push_back({c.getLatitude(), c.getLongitude()});
}
return {QMapbox::CoordinatesCollection{coordinates}};
}
QMapbox::CoordinatesCollections coordinate_list_to_collection(const QList<QGeoCoordinate> &coordinate_list) {
QMapbox::Coordinates coordinates;
for (auto &c : coordinate_list) {
coordinates.push_back({c.latitude(), c.longitude()});
}
return {QMapbox::CoordinatesCollection{coordinates}};
}
QList<QGeoCoordinate> polyline_to_coordinate_list(const QString &polylineString) {
QList<QGeoCoordinate> path;
if (polylineString.isEmpty())
return path;
QByteArray data = polylineString.toLatin1();
bool parsingLatitude = true;
int shift = 0;
int value = 0;
QGeoCoordinate coord(0, 0);
for (int i = 0; i < data.length(); ++i) {
unsigned char c = data.at(i) - 63;
value |= (c & 0x1f) << shift;
shift += 5;
// another chunk
if (c & 0x20)
continue;
int diff = (value & 1) ? ~(value >> 1) : (value >> 1);
if (parsingLatitude) {
coord.setLatitude(coord.latitude() + (double)diff/1e6);
} else {
coord.setLongitude(coord.longitude() + (double)diff/1e6);
path.append(coord);
}
parsingLatitude = !parsingLatitude;
value = 0;
shift = 0;
}
return path;
}
std::optional<QMapbox::Coordinate> coordinate_from_param(const std::string &param) {
QString json_str = QString::fromStdString(Params().get(param));
if (json_str.isEmpty()) return {};
QJsonDocument doc = QJsonDocument::fromJson(json_str.toUtf8());
if (doc.isNull()) return {};
QJsonObject json = doc.object();
if (json["latitude"].isDouble() && json["longitude"].isDouble()) {
QMapbox::Coordinate coord(json["latitude"].toDouble(), json["longitude"].toDouble());
return coord;
} else {
return {};
}
}
// return {distance, unit}
std::pair<QString, QString> map_format_distance(float d, bool is_metric) {
auto round_distance = [](float d) -> float {
return (d > 10) ? std::nearbyint(d) : std::nearbyint(d * 10) / 10.0;
};
d = std::max(d, 0.0f);
if (is_metric) {
return (d > 500) ? std::pair{QString::number(round_distance(d / 1000)), QObject::tr("km")}
: std::pair{QString::number(50 * std::nearbyint(d / 50)), QObject::tr("m")};
} else {
float feet = d * METER_TO_FOOT;
return (feet > 500) ? std::pair{QString::number(round_distance(d * METER_TO_MILE)), QObject::tr("mi")}
: std::pair{QString::number(50 * std::nearbyint(d / 50)), QObject::tr("ft")};
}
}

31
selfdrive/ui/qt/maps/map_helpers.h Normal file
View File

@@ -0,0 +1,31 @@
#pragma once
#include <optional>
#include <string>
#include <utility>
#include <eigen3/Eigen/Dense>
#include <QMapboxGL>
#include <QGeoCoordinate>
#include "common/util.h"
#include "common/transformations/coordinates.hpp"
#include "common/transformations/orientation.hpp"
#include "cereal/messaging/messaging.h"
const QString MAPBOX_TOKEN = util::getenv("MAPBOX_TOKEN").c_str();
const QString MAPS_HOST = util::getenv("MAPS_HOST", MAPBOX_TOKEN.isEmpty() ? "https://maps.comma.ai" : "https://api.mapbox.com").c_str();
const QString MAPS_CACHE_PATH = "/data/mbgl-cache-navd.db";
QString get_mapbox_token();
QMapboxGLSettings get_mapbox_settings();
QGeoCoordinate to_QGeoCoordinate(const QMapbox::Coordinate &in);
QMapbox::CoordinatesCollections model_to_collection(
const cereal::LiveLocationKalman::Measurement::Reader &calibratedOrientationECEF,
const cereal::LiveLocationKalman::Measurement::Reader &positionECEF,
const cereal::XYZTData::Reader &line);
QMapbox::CoordinatesCollections coordinate_to_collection(const QMapbox::Coordinate &c);
QMapbox::CoordinatesCollections capnp_coordinate_list_to_collection(const capnp::List<cereal::NavRoute::Coordinate>::Reader &coordinate_list);
QMapbox::CoordinatesCollections coordinate_list_to_collection(const QList<QGeoCoordinate> &coordinate_list);
QList<QGeoCoordinate> polyline_to_coordinate_list(const QString &polylineString);
std::optional<QMapbox::Coordinate> coordinate_from_param(const std::string &param);
std::pair<QString, QString> map_format_distance(float d, bool is_metric);

144
selfdrive/ui/qt/maps/map_instructions.cc Normal file
View File

@@ -0,0 +1,144 @@
#include "selfdrive/ui/qt/maps/map_instructions.h"
#include <QDir>
#include <QVBoxLayout>
#include "selfdrive/ui/qt/maps/map_helpers.h"
#include "selfdrive/ui/ui.h"
const QString ICON_SUFFIX = ".png";
MapInstructions::MapInstructions(QWidget *parent) : QWidget(parent) {
is_rhd = Params().getBool("IsRhdDetected");
QVBoxLayout *main_layout = new QVBoxLayout(this);
main_layout->setContentsMargins(11, UI_BORDER_SIZE, 11, 20);
QHBoxLayout *top_layout = new QHBoxLayout;
top_layout->addWidget(icon_01 = new QLabel, 0, Qt::AlignTop);
QVBoxLayout *right_layout = new QVBoxLayout;
right_layout->setContentsMargins(9, 9, 9, 0);
right_layout->addWidget(distance = new QLabel);
distance->setStyleSheet(R"(font-size: 90px;)");
right_layout->addWidget(primary = new QLabel);
primary->setStyleSheet(R"(font-size: 60px;)");
primary->setWordWrap(true);
right_layout->addWidget(secondary = new QLabel);
secondary->setStyleSheet(R"(font-size: 50px;)");
secondary->setWordWrap(true);
top_layout->addLayout(right_layout);
main_layout->addLayout(top_layout);
main_layout->addLayout(lane_layout = new QHBoxLayout);
lane_layout->setAlignment(Qt::AlignHCenter);
lane_layout->setSpacing(10);
setStyleSheet("color:white");
QPalette pal = palette();
pal.setColor(QPalette::Background, QColor(0, 0, 0, 150));
setAutoFillBackground(true);
setPalette(pal);
buildPixmapCache();
}
void MapInstructions::buildPixmapCache() {
QDir dir("../assets/navigation");
for (QString fn : dir.entryList({"*" + ICON_SUFFIX}, QDir::Files)) {
QPixmap pm(dir.filePath(fn));
QString key = fn.left(fn.size() - ICON_SUFFIX.length());
pm = pm.scaledToWidth(200, Qt::SmoothTransformation);
// Maneuver icons
pixmap_cache[key] = pm;
// lane direction icons
if (key.contains("turn_")) {
pixmap_cache["lane_" + key] = pm.scaled({125, 125}, Qt::IgnoreAspectRatio, Qt::SmoothTransformation);
}
// for rhd, reflect direction and then flip
if (key.contains("_left")) {
pixmap_cache["rhd_" + key.replace("_left", "_right")] = pm.transformed(QTransform().scale(-1, 1));
} else if (key.contains("_right")) {
pixmap_cache["rhd_" + key.replace("_right", "_left")] = pm.transformed(QTransform().scale(-1, 1));
}
}
}
void MapInstructions::updateInstructions(cereal::NavInstruction::Reader instruction) {
setUpdatesEnabled(false);
// Show instruction text
QString primary_str = QString::fromStdString(instruction.getManeuverPrimaryText());
QString secondary_str = QString::fromStdString(instruction.getManeuverSecondaryText());
primary->setText(primary_str);
secondary->setVisible(secondary_str.length() > 0);
secondary->setText(secondary_str);
auto distance_str_pair = map_format_distance(instruction.getManeuverDistance(), uiState()->scene.is_metric);
distance->setText(QString("%1 %2").arg(distance_str_pair.first, distance_str_pair.second));
// Show arrow with direction
QString type = QString::fromStdString(instruction.getManeuverType());
QString modifier = QString::fromStdString(instruction.getManeuverModifier());
if (!type.isEmpty()) {
QString fn = "direction_" + type;
if (!modifier.isEmpty()) {
fn += "_" + modifier;
}
fn = fn.replace(' ', '_');
bool rhd = is_rhd && (fn.contains("_left") || fn.contains("_right"));
icon_01->setPixmap(pixmap_cache[!rhd ? fn : "rhd_" + fn]);
icon_01->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
icon_01->setVisible(true);
} else {
icon_01->setVisible(false);
}
// Hide distance after arrival
distance->setVisible(type != "arrive" || instruction.getManeuverDistance() > 0);
// Show lanes
auto lanes = instruction.getLanes();
for (int i = 0; i < lanes.size(); ++i) {
bool active = lanes[i].getActive();
const auto active_direction = lanes[i].getActiveDirection();
// TODO: Make more images based on active direction and combined directions
QString fn = "lane_direction_";
// active direction has precedence
if (active && active_direction != cereal::NavInstruction::Direction::NONE) {
fn += "turn_" + DIRECTIONS[active_direction];
} else {
for (auto const &direction : lanes[i].getDirections()) {
if (direction != cereal::NavInstruction::Direction::NONE) {
fn += "turn_" + DIRECTIONS[direction];
break;
}
}
}
if (!active) {
fn += "_inactive";
}
QLabel *label = (i < lane_labels.size()) ? lane_labels[i] : lane_labels.emplace_back(new QLabel);
if (!label->parentWidget()) {
lane_layout->addWidget(label);
}
label->setPixmap(pixmap_cache[fn]);
label->setVisible(true);
}
for (int i = lanes.size(); i < lane_labels.size(); ++i) {
lane_labels[i]->setVisible(false);
}
setUpdatesEnabled(true);
setVisible(true);
}

38
selfdrive/ui/qt/maps/map_instructions.h Normal file
View File

@@ -0,0 +1,38 @@
#pragma once
#include <map>
#include <vector>
#include <QHash>
#include <QHBoxLayout>
#include <QLabel>
#include "cereal/gen/cpp/log.capnp.h"
static std::map<cereal::NavInstruction::Direction, QString> DIRECTIONS = {
{cereal::NavInstruction::Direction::NONE, "none"},
{cereal::NavInstruction::Direction::LEFT, "left"},
{cereal::NavInstruction::Direction::RIGHT, "right"},
{cereal::NavInstruction::Direction::STRAIGHT, "straight"},
{cereal::NavInstruction::Direction::SLIGHT_LEFT, "slight_left"},
{cereal::NavInstruction::Direction::SLIGHT_RIGHT, "slight_right"},
};
class MapInstructions : public QWidget {
Q_OBJECT
private:
QLabel *distance;
QLabel *primary;
QLabel *secondary;
QLabel *icon_01;
QHBoxLayout *lane_layout;
bool is_rhd = false;
std::vector<QLabel *> lane_labels;
QHash<QString, QPixmap> pixmap_cache;
public:
MapInstructions(QWidget * parent=nullptr);
void buildPixmapCache();
void updateInstructions(cereal::NavInstruction::Reader instruction);
};

43
selfdrive/ui/qt/maps/map_panel.cc Normal file
View File

@@ -0,0 +1,43 @@
#include "selfdrive/ui/qt/maps/map_panel.h"
#include <QHBoxLayout>
#include <QWidget>
#include "selfdrive/ui/qt/maps/map.h"
#include "selfdrive/ui/qt/maps/map_settings.h"
#include "selfdrive/ui/qt/util.h"
#include "selfdrive/ui/ui.h"
MapPanel::MapPanel(const QMapboxGLSettings &mapboxSettings, QWidget *parent) : QFrame(parent) {
content_stack = new QStackedLayout(this);
content_stack->setContentsMargins(0, 0, 0, 0);
auto map = new MapWindow(mapboxSettings);
QObject::connect(uiState(), &UIState::offroadTransition, map, &MapWindow::offroadTransition);
QObject::connect(device(), &Device::interactiveTimeout, this, [=]() {
content_stack->setCurrentIndex(0);
});
QObject::connect(map, &MapWindow::requestVisible, this, [=](bool visible) {
// when we show the map for a new route, signal HomeWindow to hide the sidebar
if (visible) { emit mapPanelRequested(); }
setVisible(visible);
});
QObject::connect(map, &MapWindow::requestSettings, this, [=](bool settings) {
content_stack->setCurrentIndex(settings ? 1 : 0);
});
content_stack->addWidget(map);
auto settings = new MapSettings(true, parent);
QObject::connect(settings, &MapSettings::closeSettings, this, [=]() {
content_stack->setCurrentIndex(0);
});
content_stack->addWidget(settings);
}
void MapPanel::toggleMapSettings() {
// show settings if not visible, then toggle between map and settings
int new_index = isVisible() ? (1 - content_stack->currentIndex()) : 1;
content_stack->setCurrentIndex(new_index);
emit mapPanelRequested();
show();
}

21
selfdrive/ui/qt/maps/map_panel.h Normal file
View File

@@ -0,0 +1,21 @@
#pragma once
#include <QFrame>
#include <QMapboxGL>
#include <QStackedLayout>
class MapPanel : public QFrame {
Q_OBJECT
public:
explicit MapPanel(const QMapboxGLSettings &settings, QWidget *parent = nullptr);
signals:
void mapPanelRequested();
public slots:
void toggleMapSettings();
private:
QStackedLayout *content_stack;
};

385
selfdrive/ui/qt/maps/map_settings.cc Normal file
View File

@@ -0,0 +1,385 @@
#include "selfdrive/ui/qt/maps/map_settings.h"
#include <utility>
#include <QApplication>
#include <QDebug>
#include "common/util.h"
#include "selfdrive/ui/qt/request_repeater.h"
#include "selfdrive/ui/qt/widgets/scrollview.h"
static void swap(QJsonValueRef v1, QJsonValueRef v2) { std::swap(v1, v2); }
static bool locationEqual(const QJsonValue &v1, const QJsonValue &v2) {
return v1["latitude"] == v2["latitude"] && v1["longitude"] == v2["longitude"];
}
static qint64 convertTimestampToEpoch(const QString &timestamp) {
QDateTime dt = QDateTime::fromString(timestamp, Qt::ISODate);
return dt.isValid() ? dt.toSecsSinceEpoch() : 0;
}
MapSettings::MapSettings(bool closeable, QWidget *parent) : QFrame(parent) {
setContentsMargins(0, 0, 0, 0);
setAttribute(Qt::WA_NoMousePropagation);
auto *frame = new QVBoxLayout(this);
frame->setContentsMargins(40, 40, 40, 0);
frame->setSpacing(0);
auto *heading_frame = new QHBoxLayout;
heading_frame->setContentsMargins(0, 0, 0, 0);
heading_frame->setSpacing(32);
{
if (closeable) {
auto *close_btn = new QPushButton("←");
close_btn->setStyleSheet(R"(
QPushButton {
color: #FFFFFF;
font-size: 100px;
padding-bottom: 8px;
border 1px grey solid;
border-radius: 70px;
background-color: #292929;
font-weight: 500;
}
QPushButton:pressed {
background-color: #3B3B3B;
}
)");
close_btn->setFixedSize(140, 140);
QObject::connect(close_btn, &QPushButton::clicked, [=]() { emit closeSettings(); });
// TODO: read map_on_left from ui state
heading_frame->addWidget(close_btn);
}
auto *heading = new QVBoxLayout;
heading->setContentsMargins(0, 0, 0, 0);
heading->setSpacing(16);
{
auto *title = new QLabel(tr("NAVIGATION"), this);
title->setStyleSheet("color: #FFFFFF; font-size: 54px; font-weight: 600;");
heading->addWidget(title);
auto *subtitle = new QLabel(tr("Manage at connect.comma.ai"), this);
subtitle->setStyleSheet("color: #A0A0A0; font-size: 40px; font-weight: 300;");
heading->addWidget(subtitle);
}
heading_frame->addLayout(heading, 1);
}
frame->addLayout(heading_frame);
frame->addSpacing(32);
current_widget = new DestinationWidget(this);
QObject::connect(current_widget, &DestinationWidget::actionClicked,
[]() { NavManager::instance()->setCurrentDestination({}); });
frame->addWidget(current_widget);
frame->addSpacing(32);
QWidget *destinations_container = new QWidget(this);
destinations_layout = new QVBoxLayout(destinations_container);
destinations_layout->setContentsMargins(0, 32, 0, 32);
destinations_layout->setSpacing(20);
destinations_layout->addWidget(home_widget = new DestinationWidget(this));
destinations_layout->addWidget(work_widget = new DestinationWidget(this));
QObject::connect(home_widget, &DestinationWidget::navigateTo, this, &MapSettings::navigateTo);
QObject::connect(work_widget, &DestinationWidget::navigateTo, this, &MapSettings::navigateTo);
destinations_layout->addStretch();
ScrollView *destinations_scroller = new ScrollView(destinations_container, this);
destinations_scroller->setFrameShape(QFrame::NoFrame);
frame->addWidget(destinations_scroller);
setStyleSheet("MapSettings { background-color: #333333; }");
QObject::connect(NavManager::instance(), &NavManager::updated, this, &MapSettings::refresh);
}
void MapSettings::showEvent(QShowEvent *event) {
refresh();
}
void MapSettings::refresh() {
if (!isVisible()) return;
setUpdatesEnabled(false);
auto get_w = [this](int i) {
auto w = i < widgets.size() ? widgets[i] : widgets.emplace_back(new DestinationWidget);
if (!w->parentWidget()) {
destinations_layout->insertWidget(destinations_layout->count() - 1, w);
QObject::connect(w, &DestinationWidget::navigateTo, this, &MapSettings::navigateTo);
}
return w;
};
const auto current_dest = NavManager::instance()->currentDestination();
if (!current_dest.isEmpty()) {
current_widget->set(current_dest, true);
} else {
current_widget->unset("", true);
}
home_widget->unset(NAV_FAVORITE_LABEL_HOME);
work_widget->unset(NAV_FAVORITE_LABEL_WORK);
int n = 0;
for (auto location : NavManager::instance()->currentLocations()) {
DestinationWidget *w = nullptr;
auto dest = location.toObject();
if (dest["save_type"].toString() == NAV_TYPE_FAVORITE) {
auto label = dest["label"].toString();
if (label == NAV_FAVORITE_LABEL_HOME) w = home_widget;
if (label == NAV_FAVORITE_LABEL_WORK) w = work_widget;
}
w = w ? w : get_w(n++);
w->set(dest, false);
w->setVisible(!locationEqual(dest, current_dest));
}
for (; n < widgets.size(); ++n) widgets[n]->setVisible(false);
setUpdatesEnabled(true);
}
void MapSettings::navigateTo(const QJsonObject &place) {
NavManager::instance()->setCurrentDestination(place);
emit closeSettings();
}
DestinationWidget::DestinationWidget(QWidget *parent) : QPushButton(parent) {
setContentsMargins(0, 0, 0, 0);
auto *frame = new QHBoxLayout(this);
frame->setContentsMargins(32, 24, 32, 24);
frame->setSpacing(32);
icon = new QLabel(this);
icon->setAlignment(Qt::AlignCenter);
icon->setFixedSize(96, 96);
icon->setObjectName("icon");
frame->addWidget(icon);
auto *inner_frame = new QVBoxLayout;
inner_frame->setContentsMargins(0, 0, 0, 0);
inner_frame->setSpacing(0);
{
title = new ElidedLabel(this);
title->setAttribute(Qt::WA_TransparentForMouseEvents);
inner_frame->addWidget(title);
subtitle = new ElidedLabel(this);
subtitle->setAttribute(Qt::WA_TransparentForMouseEvents);
subtitle->setObjectName("subtitle");
inner_frame->addWidget(subtitle);
}
frame->addLayout(inner_frame, 1);
action = new QPushButton(this);
action->setFixedSize(96, 96);
action->setObjectName("action");
action->setStyleSheet("font-size: 65px; font-weight: 600;");
QObject::connect(action, &QPushButton::clicked, this, &QPushButton::clicked);
QObject::connect(action, &QPushButton::clicked, this, &DestinationWidget::actionClicked);
frame->addWidget(action);
setFixedHeight(164);
setStyleSheet(R"(
DestinationWidget { background-color: #202123; border-radius: 10px; }
QLabel { color: #FFFFFF; font-size: 48px; font-weight: 400; }
#icon { background-color: #3B4356; border-radius: 48px; }
#subtitle { color: #9BA0A5; }
#action { border: none; border-radius: 48px; color: #FFFFFF; padding-bottom: 4px; }
/* current destination */
[current="true"] { background-color: #E8E8E8; }
[current="true"] QLabel { color: #000000; }
[current="true"] #icon { background-color: #42906B; }
[current="true"] #subtitle { color: #333333; }
[current="true"] #action { color: #202123; }
/* no saved destination */
[set="false"] QLabel { color: #9BA0A5; }
[current="true"][set="false"] QLabel { color: #A0000000; }
/* pressed */
[current="false"]:pressed { background-color: #18191B; }
[current="true"] #action:pressed { background-color: #D6D6D6; }
)");
QObject::connect(this, &QPushButton::clicked, [this]() { if (!dest.isEmpty()) emit navigateTo(dest); });
}
void DestinationWidget::set(const QJsonObject &destination, bool current) {
if (dest == destination) return;
dest = destination;
setProperty("current", current);
setProperty("set", true);
auto icon_pixmap = current ? icons().directions : icons().recent;
auto title_text = destination["place_name"].toString();
auto subtitle_text = destination["place_details"].toString();
if (destination["save_type"] == NAV_TYPE_FAVORITE) {
if (destination["label"] == NAV_FAVORITE_LABEL_HOME) {
icon_pixmap = icons().home;
subtitle_text = title_text + ", " + subtitle_text;
title_text = tr("Home");
} else if (destination["label"] == NAV_FAVORITE_LABEL_WORK) {
icon_pixmap = icons().work;
subtitle_text = title_text + ", " + subtitle_text;
title_text = tr("Work");
} else {
icon_pixmap = icons().favorite;
}
}
icon->setPixmap(icon_pixmap);
title->setText(title_text);
subtitle->setText(subtitle_text);
subtitle->setVisible(true);
// TODO: use pixmap
action->setAttribute(Qt::WA_TransparentForMouseEvents, !current);
action->setText(current ? "×" : "→");
action->setVisible(true);
setStyleSheet(styleSheet());
}
void DestinationWidget::unset(const QString &label, bool current) {
dest = {};
setProperty("current", current);
setProperty("set", false);
if (label.isEmpty()) {
icon->setPixmap(icons().directions);
title->setText(tr("No destination set"));
} else {
QString title_text = label == NAV_FAVORITE_LABEL_HOME ? tr("home") : tr("work");
icon->setPixmap(label == NAV_FAVORITE_LABEL_HOME ? icons().home : icons().work);
title->setText(tr("No %1 location set").arg(title_text));
}
subtitle->setVisible(false);
action->setVisible(false);
setStyleSheet(styleSheet());
setVisible(true);
}
// singleton NavManager
NavManager *NavManager::instance() {
static NavManager *request = new NavManager(qApp);
return request;
}
NavManager::NavManager(QObject *parent) : QObject(parent) {
locations = QJsonDocument::fromJson(params.get("NavPastDestinations").c_str()).array();
current_dest = QJsonDocument::fromJson(params.get("NavDestination").c_str()).object();
if (auto dongle_id = getDongleId()) {
{
// Fetch favorite and recent locations
QString url = CommaApi::BASE_URL + "/v1/navigation/" + *dongle_id + "/locations";
RequestRepeater *repeater = new RequestRepeater(this, url, "ApiCache_NavDestinations", 30, true);
QObject::connect(repeater, &RequestRepeater::requestDone, this, &NavManager::parseLocationsResponse);
}
{
auto param_watcher = new ParamWatcher(this);
QObject::connect(param_watcher, &ParamWatcher::paramChanged, this, &NavManager::updated);
// Destination set while offline
QString url = CommaApi::BASE_URL + "/v1/navigation/" + *dongle_id + "/next";
HttpRequest *deleter = new HttpRequest(this);
RequestRepeater *repeater = new RequestRepeater(this, url, "", 10, true);
QObject::connect(repeater, &RequestRepeater::requestDone, [=](const QString &resp, bool success) {
if (success && resp != "null") {
if (params.get("NavDestination").empty()) {
qWarning() << "Setting NavDestination from /next" << resp;
params.put("NavDestination", resp.toStdString());
} else {
qWarning() << "Got location from /next, but NavDestination already set";
}
// Send DELETE to clear destination server side
deleter->sendRequest(url, HttpRequest::Method::DELETE);
}
// athena can set destination at any time
param_watcher->addParam("NavDestination");
current_dest = QJsonDocument::fromJson(params.get("NavDestination").c_str()).object();
emit updated();
});
}
}
}
void NavManager::parseLocationsResponse(const QString &response, bool success) {
if (!success || response == prev_response) return;
prev_response = response;
QJsonDocument doc = QJsonDocument::fromJson(response.trimmed().toUtf8());
if (doc.isNull()) {
qWarning() << "JSON Parse failed on navigation locations" << response;
return;
}
// set last activity time.
auto remote_locations = doc.array();
for (QJsonValueRef loc : remote_locations) {
auto obj = loc.toObject();
auto serverTime = convertTimestampToEpoch(obj["modified"].toString());
obj.insert("time", qMax(serverTime, getLastActivity(obj)));
loc = obj;
}
locations = remote_locations;
sortLocations();
emit updated();
}
void NavManager::sortLocations() {
// Sort: alphabetical FAVORITES, and then most recent.
// We don't need to care about the ordering of HOME and WORK. DestinationWidget always displays them at the top.
std::stable_sort(locations.begin(), locations.end(), [](const QJsonValue &a, const QJsonValue &b) {
if (a["save_type"] == NAV_TYPE_FAVORITE || b["save_type"] == NAV_TYPE_FAVORITE) {
return (std::tuple(a["save_type"].toString(), a["place_name"].toString()) <
std::tuple(b["save_type"].toString(), b["place_name"].toString()));
} else {
return a["time"].toVariant().toLongLong() > b["time"].toVariant().toLongLong();
}
});
write_param_future = std::async(std::launch::async, [destinations = QJsonArray(locations)]() {
Params().put("NavPastDestinations", QJsonDocument(destinations).toJson().toStdString());
});
}
qint64 NavManager::getLastActivity(const QJsonObject &loc) const {
qint64 last_activity = 0;
auto it = std::find_if(locations.begin(), locations.end(),
[&loc](const QJsonValue &l) { return locationEqual(loc, l); });
if (it != locations.end()) {
auto tm = it->toObject().value("time");
if (!tm.isUndefined() && !tm.isNull()) {
last_activity = tm.toVariant().toLongLong();
}
}
return last_activity;
}
void NavManager::setCurrentDestination(const QJsonObject &loc) {
current_dest = loc;
if (!current_dest.isEmpty()) {
current_dest["time"] = QDateTime::currentSecsSinceEpoch();
auto it = std::find_if(locations.begin(), locations.end(),
[&loc](const QJsonValue &l) { return locationEqual(loc, l); });
if (it != locations.end()) {
*it = current_dest;
sortLocations();
}
params.put("NavDestination", QJsonDocument(current_dest).toJson().toStdString());
} else {
params.remove("NavDestination");
}
emit updated();
}

102
selfdrive/ui/qt/maps/map_settings.h Normal file
View File

@@ -0,0 +1,102 @@
#pragma once
#include <future>
#include <vector>
#include <QFrame>
#include <QJsonArray>
#include <QJsonDocument>
#include <QJsonObject>
#include <QLabel>
#include <QPushButton>
#include <QVBoxLayout>
#include "common/params.h"
#include "selfdrive/ui/qt/util.h"
#include "selfdrive/ui/qt/widgets/controls.h"
const QString NAV_TYPE_FAVORITE = "favorite";
const QString NAV_TYPE_RECENT = "recent";
const QString NAV_FAVORITE_LABEL_HOME = "home";
const QString NAV_FAVORITE_LABEL_WORK = "work";
class DestinationWidget;
class NavManager : public QObject {
Q_OBJECT
public:
static NavManager *instance();
QJsonArray currentLocations() const { return locations; }
QJsonObject currentDestination() const { return current_dest; }
void setCurrentDestination(const QJsonObject &loc);
qint64 getLastActivity(const QJsonObject &loc) const;
signals:
void updated();
private:
NavManager(QObject *parent);
void parseLocationsResponse(const QString &response, bool success);
void sortLocations();
Params params;
QString prev_response;
QJsonArray locations;
QJsonObject current_dest;
std::future<void> write_param_future;
};
class MapSettings : public QFrame {
Q_OBJECT
public:
explicit MapSettings(bool closeable = false, QWidget *parent = nullptr);
void navigateTo(const QJsonObject &place);
private:
void showEvent(QShowEvent *event) override;
void refresh();
QVBoxLayout *destinations_layout;
DestinationWidget *current_widget;
DestinationWidget *home_widget;
DestinationWidget *work_widget;
std::vector<DestinationWidget *> widgets;
signals:
void closeSettings();
};
class DestinationWidget : public QPushButton {
Q_OBJECT
public:
explicit DestinationWidget(QWidget *parent = nullptr);
void set(const QJsonObject &location, bool current = false);
void unset(const QString &label, bool current = false);
signals:
void actionClicked();
void navigateTo(const QJsonObject &destination);
private:
struct NavIcons {
QPixmap home, work, favorite, recent, directions;
};
static NavIcons icons() {
static NavIcons nav_icons {
loadPixmap("../assets/navigation/icon_home.svg", {48, 48}),
loadPixmap("../assets/navigation/icon_work.svg", {48, 48}),
loadPixmap("../assets/navigation/icon_favorite.svg", {48, 48}),
loadPixmap("../assets/navigation/icon_recent.svg", {48, 48}),
loadPixmap("../assets/navigation/icon_directions.svg", {48, 48}),
};
return nav_icons;
}
private:
QLabel *icon, *title, *subtitle;
QPushButton *action;
QJsonObject dest;
};