Move navigation into separate daemon (#22767)

* Add navd folder

* prints route instructions to console

* broadcast NavInstuction without lanes

* show basic instructions and eta

* parse out lane info

* use swaglog

* clip distance on ui side

* draw lane directions

* show route

* add to process config

* add to release files

* small cleanup

* show route without gps

* pop open map on initial route

* fix error messages around no gps

* done

* make persistent process

* handle end of route

* clear route on offroad

* only one timer

* fix layout hacks

* explicit rendering at 20Hz
pull/22793/head
Willem Melching 2021-11-04 14:32:32 +01:00 committed by GitHub
parent b7394c6171
commit bef686f275
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
15 changed files with 565 additions and 332 deletions

View File

@ -23,3 +23,9 @@ selfdrive/ui/qt/spinner_larch64
selfdrive/ui/qt/text_larch64
selfdrive/ui/qt/maps/*.cc
selfdrive/ui/qt/maps/*.h
selfdrive/ui/navd/*.cc
selfdrive/ui/navd/*.h
selfdrive/ui/navd/navd
selfdrive/ui/navd/.gitignore

View File

@ -14,6 +14,7 @@ procs = [
NativeProcess("logcatd", "selfdrive/logcatd", ["./logcatd"]),
NativeProcess("loggerd", "selfdrive/loggerd", ["./loggerd"]),
NativeProcess("modeld", "selfdrive/modeld", ["./modeld"]),
NativeProcess("navd", "selfdrive/ui/navd", ["./navd"], enabled=(PC or TICI), persistent=True),
NativeProcess("proclogd", "selfdrive/proclogd", ["./proclogd"]),
NativeProcess("sensord", "selfdrive/sensord", ["./sensord"], enabled=not PC, persistent=EON, sigkill=EON),
NativeProcess("ubloxd", "selfdrive/locationd", ["./ubloxd"], enabled=(not PC or WEBCAM)),

View File

@ -122,3 +122,8 @@ if arch in ['x86_64', 'Darwin'] or GetOption('extras'):
if GetOption('test'):
qt_env.Program('replay/tests/test_replay', ['replay/tests/test_runner.cc', 'replay/tests/test_replay.cc'], LIBS=[replay_libs])
# navd
if maps:
navd_src = ["navd/main.cc", "navd/route_engine.cc"]
qt_env.Program("navd/_navd", navd_src, LIBS=qt_libs + ['common', 'json11'])

1
selfdrive/ui/navd/.gitignore vendored 100644
View File

@ -0,0 +1 @@
_navd

View File

@ -0,0 +1,36 @@
#include <QApplication>
#include <QCommandLineParser>
#include <QDebug>
#include <QThread>
#include <csignal>
#include "selfdrive/ui/qt/util.h"
#include "selfdrive/ui/navd/route_engine.h"
RouteEngine* route_engine = nullptr;
void sigHandler(int s) {
qInfo() << "Shutting down";
std::signal(s, SIG_DFL);
qApp->quit();
}
int main(int argc, char *argv[]) {
qInstallMessageHandler(swagLogMessageHandler);
QApplication app(argc, argv);
std::signal(SIGINT, sigHandler);
std::signal(SIGTERM, sigHandler);
QCommandLineParser parser;
parser.setApplicationDescription("Navigation server. Runs stand-alone, or using pre-computer route");
parser.addHelpOption();
parser.process(app);
const QStringList args = parser.positionalArguments();
route_engine = new RouteEngine();
return app.exec();
}

View File

@ -0,0 +1,4 @@
#!/bin/sh
cd "$(dirname "$0")"
export QT_PLUGIN_PATH="../../../third_party/qt-plugins/$(uname -m)"
exec ./_navd

View File

@ -0,0 +1,323 @@
#include "selfdrive/ui/navd/route_engine.h"
#include <QDebug>
#include "selfdrive/ui/qt/maps/map.h"
#include "selfdrive/ui/qt/maps/map_helpers.h"
#include "selfdrive/ui/qt/api.h"
#include "selfdrive/common/params.h"
const qreal REROUTE_DISTANCE = 25;
const float MANEUVER_TRANSITION_THRESHOLD = 10;
static float get_time_typical(const QGeoRouteSegment &segment) {
auto maneuver = segment.maneuver();
auto attrs = maneuver.extendedAttributes();
return attrs.contains("mapbox.duration_typical") ? attrs["mapbox.duration_typical"].toDouble() : segment.travelTime();
}
static cereal::NavInstruction::Direction string_to_direction(QString d) {
if (d.contains("left")) {
return cereal::NavInstruction::Direction::LEFT;
} else if (d.contains("right")) {
return cereal::NavInstruction::Direction::RIGHT;
} else if (d.contains("straight")) {
return cereal::NavInstruction::Direction::STRAIGHT;
}
return cereal::NavInstruction::Direction::NONE;
}
static void parse_banner(cereal::NavInstruction::Builder &instruction, const QMap<QString, QVariant> &banner, bool full) {
QString primary_str, secondary_str;
auto p = banner["primary"].toMap();
primary_str += p["text"].toString();
instruction.setShowFull(full);
if (p.contains("type")) {
instruction.setManeuverType(p["type"].toString().toStdString());
}
if (p.contains("modifier")) {
instruction.setManeuverModifier(p["modifier"].toString().toStdString());
}
if (banner.contains("secondary")) {
auto s = banner["secondary"].toMap();
secondary_str += s["text"].toString();
}
instruction.setManeuverPrimaryText(primary_str.toStdString());
instruction.setManeuverSecondaryText(secondary_str.toStdString());
if (banner.contains("sub")) {
auto s = banner["sub"].toMap();
auto components = s["components"].toList();
size_t num_lanes = 0;
for (auto &c : components) {
auto cc = c.toMap();
if (cc["type"].toString() == "lane") {
num_lanes += 1;
}
}
auto lanes = instruction.initLanes(num_lanes);
size_t i = 0;
for (auto &c : components) {
auto cc = c.toMap();
if (cc["type"].toString() == "lane") {
auto lane = lanes[i];
lane.setActive(cc["active"].toBool());
if (cc.contains("active_direction")) {
lane.setActiveDirection(string_to_direction(cc["active_direction"].toString()));
}
auto directions = lane.initDirections(cc["directions"].toList().size());
size_t j = 0;
for (auto &dir : cc["directions"].toList()) {
directions.set(j, string_to_direction(dir.toString()));
j++;
}
i++;
}
}
}
}
RouteEngine::RouteEngine() {
sm = new SubMaster({"liveLocationKalman"});
pm = new PubMaster({"navInstruction", "navRoute"});
// Timers
timer = new QTimer(this);
QObject::connect(timer, SIGNAL(timeout()), this, SLOT(timerUpdate()));
timer->start(1000);
// Build routing engine
QVariantMap parameters;
QString token = MAPBOX_TOKEN.isEmpty() ? CommaApi::create_jwt({}, 4 * 7 * 24 * 3600) : MAPBOX_TOKEN;
parameters["mapbox.access_token"] = token;
parameters["mapbox.directions_api_url"] = MAPS_HOST + "/directions/v5/mapbox/";
geoservice_provider = new QGeoServiceProvider("mapbox", parameters);
routing_manager = geoservice_provider->routingManager();
if (routing_manager == nullptr) {
qWarning() << geoservice_provider->errorString();
assert(routing_manager);
}
QObject::connect(routing_manager, &QGeoRoutingManager::finished, this, &RouteEngine::routeCalculated);
// Get last gps position from params
auto last_gps_position = coordinate_from_param("LastGPSPosition");
if (last_gps_position) {
last_position = *last_gps_position;
}
}
void RouteEngine::timerUpdate() {
sm->update(0);
if (!sm->updated("liveLocationKalman")) {
return;
}
auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman();
gps_ok = location.getGpsOK();
localizer_valid = location.getStatus() == cereal::LiveLocationKalman::Status::VALID;
if (localizer_valid) {
auto pos = location.getPositionGeodetic();
auto orientation = location.getCalibratedOrientationNED();
last_bearing = RAD2DEG(orientation.getValue()[2]);
last_position = QMapbox::Coordinate(pos.getValue()[0], pos.getValue()[1]);
}
recomputeRoute();
MessageBuilder msg;
cereal::Event::Builder evt = msg.initEvent(segment.isValid());
cereal::NavInstruction::Builder instruction = evt.initNavInstruction();
// Show route instructions
if (segment.isValid()) {
auto cur_maneuver = segment.maneuver();
auto attrs = cur_maneuver.extendedAttributes();
if (cur_maneuver.isValid() && attrs.contains("mapbox.banner_instructions")) {
float along_geometry = distance_along_geometry(segment.path(), to_QGeoCoordinate(*last_position));
float distance_to_maneuver_along_geometry = segment.distance() - along_geometry;
auto banners = attrs["mapbox.banner_instructions"].toList();
if (banners.size()) {
auto banner = banners[0].toMap();
for (auto &b : banners) {
auto bb = b.toMap();
if (distance_to_maneuver_along_geometry < bb["distance_along_geometry"].toDouble()) {
banner = bb;
}
}
instruction.setManeuverDistance(distance_to_maneuver_along_geometry);
parse_banner(instruction, banner, distance_to_maneuver_along_geometry < banner["distance_along_geometry"].toDouble());
// ETA
float progress = distance_along_geometry(segment.path(), to_QGeoCoordinate(*last_position)) / segment.distance();
float total_distance = segment.distance() * (1.0 - progress);
float total_time = segment.travelTime() * (1.0 - progress);
float total_time_typical = get_time_typical(segment) * (1.0 - progress);
auto s = segment.nextRouteSegment();
while (s.isValid()) {
total_distance += s.distance();
total_time += s.travelTime();
total_time_typical += get_time_typical(s);
s = s.nextRouteSegment();
}
instruction.setTimeRemaining(total_time);
instruction.setTimeRemainingTypical(total_time_typical);
instruction.setDistanceRemaining(total_distance);
}
// Transition to next route segment
if (distance_to_maneuver_along_geometry < -MANEUVER_TRANSITION_THRESHOLD) {
auto next_segment = segment.nextRouteSegment();
if (next_segment.isValid()) {
segment = next_segment;
recompute_backoff = std::max(0, recompute_backoff - 1);
recompute_countdown = 0;
} else {
qWarning() << "Destination reached";
Params().remove("NavDestination");
// Clear route if driving away from destination
float d = segment.maneuver().position().distanceTo(to_QGeoCoordinate(*last_position));
if (d > REROUTE_DISTANCE) {
clearRoute();
}
}
}
}
}
pm->send("navInstruction", msg);
}
void RouteEngine::clearRoute() {
segment = QGeoRouteSegment();
nav_destination = QMapbox::Coordinate();
}
bool RouteEngine::shouldRecompute() {
if (!segment.isValid()) {
return true;
}
// Don't recompute in last segment, assume destination is reached
if (!segment.nextRouteSegment().isValid()) {
return false;
}
// Compute closest distance to all line segments in the current path
float min_d = REROUTE_DISTANCE + 1;
auto path = segment.path();
auto cur = to_QGeoCoordinate(*last_position);
for (size_t i = 0; i < path.size() - 1; i++) {
auto a = path[i];
auto b = path[i+1];
if (a.distanceTo(b) < 1.0) {
continue;
}
min_d = std::min(min_d, minimum_distance(a, b, cur));
}
return min_d > REROUTE_DISTANCE;
// TODO: Check for going wrong way in segment
}
void RouteEngine::recomputeRoute() {
auto new_destination = coordinate_from_param("NavDestination");
if (!new_destination) {
clearRoute();
return;
}
bool should_recompute = shouldRecompute();
if (*new_destination != nav_destination) {
qWarning() << "Got new destination from NavDestination param" << *new_destination;
should_recompute = true;
}
if (!gps_ok && segment.isValid()) return; // Don't recompute when gps drifts in tunnels
if (recompute_countdown == 0 && should_recompute) {
recompute_countdown = std::pow(2, recompute_backoff);
recompute_backoff = std::min(7, recompute_backoff + 1);
calculateRoute(*new_destination);
} else {
recompute_countdown = std::max(0, recompute_countdown - 1);
}
}
void RouteEngine::calculateRoute(QMapbox::Coordinate destination) {
qWarning() << "Calculating route" << *last_position << "->" << destination;
nav_destination = destination;
QGeoRouteRequest request(to_QGeoCoordinate(*last_position), to_QGeoCoordinate(destination));
request.setFeatureWeight(QGeoRouteRequest::TrafficFeature, QGeoRouteRequest::AvoidFeatureWeight);
if (last_bearing) {
QVariantMap params;
int bearing = ((int)(*last_bearing) + 360) % 360;
params["bearing"] = bearing;
request.setWaypointsMetadata({params});
}
routing_manager->calculateRoute(request);
}
void RouteEngine::routeCalculated(QGeoRouteReply *reply) {
MessageBuilder msg;
cereal::Event::Builder evt = msg.initEvent();
cereal::NavRoute::Builder nav_route = evt.initNavRoute();
if (reply->error() == QGeoRouteReply::NoError) {
if (reply->routes().size() != 0) {
qWarning() << "Got route response";
route = reply->routes().at(0);
segment = route.firstRouteSegment();
auto path = route.path();
auto coordinates = nav_route.initCoordinates(path.size());
size_t i = 0;
for (auto const &c : route.path()) {
coordinates[i].setLatitude(c.latitude());
coordinates[i].setLongitude(c.longitude());
i++;
}
} else {
qWarning() << "Got empty route response";
}
} else {
qWarning() << "Got error in route reply" << reply->errorString();
}
pm->send("navRoute", msg);
reply->deleteLater();
}

View File

@ -0,0 +1,50 @@
#pragma once
#include <QThread>
#include <QGeoCoordinate>
#include <QGeoManeuver>
#include <QGeoRouteRequest>
#include <QGeoRouteSegment>
#include <QGeoRoutingManager>
#include <QGeoServiceProvider>
#include <QTimer>
#include <QMapboxGL>
#include "cereal/messaging/messaging.h"
class RouteEngine : public QObject {
Q_OBJECT
public:
RouteEngine();
SubMaster *sm;
PubMaster *pm;
QTimer* timer;
// Route
bool gps_ok = false;
QGeoServiceProvider *geoservice_provider;
QGeoRoutingManager *routing_manager;
QGeoRoute route;
QGeoRouteSegment segment;
QMapbox::Coordinate nav_destination;
// Position
std::optional<QMapbox::Coordinate> last_position;
std::optional<float> last_bearing;
bool localizer_valid = false;
// Route recompute
int recompute_backoff = 0;
int recompute_countdown = 0;
void calculateRoute(QMapbox::Coordinate destination);
void clearRoute();
bool shouldRecompute();
private slots:
void timerUpdate();
void routeCalculated(QGeoRouteReply *reply);
void recomputeRoute();
};

View File

@ -13,7 +13,6 @@
const int PAN_TIMEOUT = 100;
const qreal REROUTE_DISTANCE = 25;
const float MANEUVER_TRANSITION_THRESHOLD = 10;
const float MAX_ZOOM = 17;
@ -22,19 +21,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.1) {
sm = new SubMaster({"liveLocationKalman"});
sm = new SubMaster({"liveLocationKalman", "navInstruction", "navRoute"});
timer = new QTimer(this);
QObject::connect(timer, SIGNAL(timeout()), this, SLOT(timerUpdate()));
timer->start(100);
recompute_timer = new QTimer(this);
QObject::connect(recompute_timer, SIGNAL(timeout()), this, SLOT(recomputeRoute()));
recompute_timer->start(1000);
// Instructions
map_instructions = new MapInstructions(this);
QObject::connect(this, &MapWindow::instructionsChanged, map_instructions, &MapInstructions::updateInstructions);
@ -50,19 +44,6 @@ MapWindow::MapWindow(const QMapboxGLSettings &settings) :
map_eta->move(25, 1080 - h - bdr_s*2);
map_eta->setVisible(false);
// Routing
QVariantMap parameters;
parameters["mapbox.access_token"] = m_settings.accessToken();
parameters["mapbox.directions_api_url"] = MAPS_HOST + "/directions/v5/mapbox/";
geoservice_provider = new QGeoServiceProvider("mapbox", parameters);
routing_manager = geoservice_provider->routingManager();
if (routing_manager == nullptr) {
qDebug() << geoservice_provider->errorString();
assert(routing_manager);
}
QObject::connect(routing_manager, &QGeoRoutingManager::finished, this, &MapWindow::routeCalculated);
auto last_gps_position = coordinate_from_param("LastGPSPosition");
if (last_gps_position) {
last_position = *last_gps_position;
@ -122,14 +103,15 @@ void MapWindow::timerUpdate() {
return;
}
if (isVisible()) {
update();
update();
if (m_map.isNull()) {
return;
}
sm->update(0);
if (sm->updated("liveLocationKalman")) {
auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman();
gps_ok = location.getGpsOK();
localizer_valid = location.getStatus() == cereal::LiveLocationKalman::Status::VALID;
@ -146,11 +128,6 @@ void MapWindow::timerUpdate() {
velocity_filter.update(velocity);
}
}
if (m_map.isNull()) {
return;
}
loaded_once = loaded_once || m_map->isFullyLoaded();
if (!loaded_once) {
map_instructions->showError("Map Loading");
@ -161,7 +138,16 @@ void MapWindow::timerUpdate() {
if (!localizer_valid) {
map_instructions->showError("Waiting for GPS");
return;
} else {
map_instructions->noError();
// 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);
}
if (pan_counter == 0) {
@ -177,61 +163,36 @@ void MapWindow::timerUpdate() {
zoom_counter--;
}
// 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);
if (sm->updated("navInstruction")) {
if (sm->valid("navInstruction")) {
auto i = (*sm)["navInstruction"].getNavInstruction();
emit ETAChanged(i.getTimeRemaining(), i.getTimeRemainingTypical(), i.getDistanceRemaining());
// Show route instructions
if (segment.isValid()) {
auto cur_maneuver = segment.maneuver();
auto attrs = cur_maneuver.extendedAttributes();
if (cur_maneuver.isValid() && attrs.contains("mapbox.banner_instructions")) {
float along_geometry = distance_along_geometry(segment.path(), to_QGeoCoordinate(*last_position));
float distance_to_maneuver_along_geometry = segment.distance() - along_geometry;
emit distanceChanged(std::max(0.0f, distance_to_maneuver_along_geometry));
m_map->setPitch(MAX_PITCH); // TODO: smooth pitching based on maneuver distance
auto banners = attrs["mapbox.banner_instructions"].toList();
if (banners.size()) {
auto banner = banners[0].toMap();
for (auto &b : banners) {
auto bb = b.toMap();
if (distance_to_maneuver_along_geometry < bb["distance_along_geometry"].toDouble()) {
banner = bb;
}
}
// Show full banner if ready to show, otherwise give summarized version of first banner in segment
emit instructionsChanged(banner, distance_to_maneuver_along_geometry < banner["distance_along_geometry"].toDouble());
} else {
map_instructions->hideIfNoError();
if (localizer_valid) {
m_map->setPitch(MAX_PITCH); // TODO: smooth pitching based on maneuver distance
emit distanceChanged(i.getManeuverDistance()); // TODO: combine with instructionsChanged
emit instructionsChanged(i);
}
} else {
m_map->setPitch(MIN_PITCH);
clearRoute();
}
}
// Transition to next route segment
if (!shouldRecompute() && (distance_to_maneuver_along_geometry < -MANEUVER_TRANSITION_THRESHOLD)) {
auto next_segment = segment.nextRouteSegment();
if (next_segment.isValid()) {
segment = next_segment;
if (sm->updated("navRoute")) {
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");
recompute_backoff = std::max(0, recompute_backoff - 1);
recompute_countdown = 0;
} else {
qWarning() << "Destination reached";
Params().remove("NavDestination");
// Clear route if driving away from destination
float d = segment.maneuver().position().distanceTo(to_QGeoCoordinate(*last_position));
if (d > REROUTE_DISTANCE) {
clearRoute();
}
}
}
// Only open the map on setting destination the first time
if (allow_open) {
setVisible(true); // Show map on destination set/change
allow_open = false;
}
}
}
@ -254,7 +215,6 @@ void MapWindow::initializeGL() {
m_map->setPitch(MIN_PITCH);
m_map->setStyleUrl("mapbox://styles/commaai/ckr64tlwp0azb17nqvr9fj13s");
connect(m_map.data(), SIGNAL(needsRendering()), this, SLOT(update()));
QObject::connect(m_map.data(), &QMapboxGL::mapChanged, [=](QMapboxGL::MapChange change) {
if (change == QMapboxGL::MapChange::MapChangeDidFinishLoadingMap) {
loaded_once = true;
@ -263,137 +223,11 @@ void MapWindow::initializeGL() {
}
void MapWindow::paintGL() {
if (!isVisible()) return;
if (!isVisible() || m_map.isNull()) return;
m_map->render();
}
static float get_time_typical(const QGeoRouteSegment &segment) {
auto maneuver = segment.maneuver();
auto attrs = maneuver.extendedAttributes();
return attrs.contains("mapbox.duration_typical") ? attrs["mapbox.duration_typical"].toDouble() : segment.travelTime();
}
void MapWindow::recomputeRoute() {
if (!QUIState::ui_state.scene.started) {
return;
}
if (!last_position) {
return;
}
auto new_destination = coordinate_from_param("NavDestination");
if (!new_destination) {
clearRoute();
return;
}
bool should_recompute = shouldRecompute();
if (*new_destination != nav_destination) {
qWarning() << "Got new destination from NavDestination param" << *new_destination;
// Only open the map on setting destination the first time
if (allow_open) {
setVisible(true); // Show map on destination set/change
allow_open = false;
}
// TODO: close sidebar
should_recompute = true;
}
if (!should_recompute) updateETA(); // ETA is updated after recompute
if (!gps_ok && segment.isValid()) return; // Don't recompute when gps drifts in tunnels
// Only do API request when map is fully loaded
if (loaded_once) {
if (recompute_countdown == 0 && should_recompute) {
recompute_countdown = std::pow(2, recompute_backoff);
recompute_backoff = std::min(7, recompute_backoff + 1);
calculateRoute(*new_destination);
} else {
recompute_countdown = std::max(0, recompute_countdown - 1);
}
}
}
void MapWindow::updateETA() {
if (segment.isValid()) {
float progress = distance_along_geometry(segment.path(), to_QGeoCoordinate(*last_position)) / segment.distance();
float total_distance = segment.distance() * (1.0 - progress);
float total_time = segment.travelTime() * (1.0 - progress);
float total_time_typical = get_time_typical(segment) * (1.0 - progress);
auto s = segment.nextRouteSegment();
while (s.isValid()) {
total_distance += s.distance();
total_time += s.travelTime();
total_time_typical += get_time_typical(s);
s = s.nextRouteSegment();
}
emit ETAChanged(total_time, total_time_typical, total_distance);
}
}
void MapWindow::calculateRoute(QMapbox::Coordinate destination) {
qWarning() << "Calculating route" << *last_position << "->" << destination;
nav_destination = destination;
QGeoRouteRequest request(to_QGeoCoordinate(*last_position), to_QGeoCoordinate(destination));
request.setFeatureWeight(QGeoRouteRequest::TrafficFeature, QGeoRouteRequest::AvoidFeatureWeight);
if (last_bearing) {
QVariantMap params;
int bearing = ((int)(*last_bearing) + 360) % 360;
params["bearing"] = bearing;
request.setWaypointsMetadata({params});
}
routing_manager->calculateRoute(request);
}
void MapWindow::routeCalculated(QGeoRouteReply *reply) {
bool got_route = false;
if (reply->error() == QGeoRouteReply::NoError) {
if (reply->routes().size() != 0) {
qWarning() << "Got route response";
route = reply->routes().at(0);
segment = route.firstRouteSegment();
auto route_points = coordinate_list_to_collection(route.path());
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");
got_route = true;
updateETA();
} else {
qWarning() << "Got empty route response";
}
} else {
qWarning() << "Got error in route reply" << reply->errorString();
}
if (!got_route) {
map_instructions->showError("Failed to Route");
}
reply->deleteLater();
}
void MapWindow::clearRoute() {
segment = QGeoRouteSegment();
nav_destination = QMapbox::Coordinate();
if (!m_map.isNull()) {
m_map->setLayoutProperty("navLayer", "visibility", "none");
m_map->setPitch(MIN_PITCH);
@ -404,29 +238,6 @@ void MapWindow::clearRoute() {
allow_open = true;
}
bool MapWindow::shouldRecompute() {
if (!segment.isValid()) {
return true;
}
// Compute closest distance to all line segments in the current path
float min_d = REROUTE_DISTANCE + 1;
auto path = segment.path();
auto cur = to_QGeoCoordinate(*last_position);
for (size_t i = 0; i < path.size() - 1; i++) {
auto a = path[i];
auto b = path[i+1];
if (a.distanceTo(b) < 1.0) {
continue;
}
min_d = std::min(min_d, minimum_distance(a, b, cur));
}
return min_d > REROUTE_DISTANCE;
// TODO: Check for going wrong way in segment
}
void MapWindow::mousePressEvent(QMouseEvent *ev) {
m_lastPos = ev->localPos();
ev->accept();
@ -436,6 +247,7 @@ 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();
pan_counter = 0;
zoom_counter = 0;
@ -447,6 +259,7 @@ void MapWindow::mouseMoveEvent(QMouseEvent *ev) {
if (!delta.isNull()) {
pan_counter = PAN_TIMEOUT;
m_map->moveBy(delta / MAP_SCALE);
update();
}
m_lastPos = ev->localPos();
@ -464,6 +277,8 @@ void MapWindow::wheelEvent(QWheelEvent *ev) {
}
m_map->scaleBy(1 + factor, ev->pos() / MAP_SCALE);
update();
zoom_counter = PAN_TIMEOUT;
ev->accept();
}
@ -488,12 +303,15 @@ void MapWindow::pinchTriggered(QPinchGesture *gesture) {
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();
zoom_counter = PAN_TIMEOUT;
}
}
void MapWindow::offroadTransition(bool offroad) {
if (!offroad) {
if (offroad) {
clearRoute();
} else {
auto dest = coordinate_from_param("NavDestination");
setVisible(dest.has_value());
}
@ -512,8 +330,7 @@ MapInstructions::MapInstructions(QWidget * parent) : QWidget(parent) {
}
{
QWidget *w = new QWidget;
QVBoxLayout *layout = new QVBoxLayout(w);
QVBoxLayout *layout = new QVBoxLayout;
distance = new QLabel;
distance->setStyleSheet(R"(font-size: 90px;)");
@ -529,10 +346,13 @@ MapInstructions::MapInstructions(QWidget * parent) : QWidget(parent) {
secondary->setWordWrap(true);
layout->addWidget(secondary);
lane_layout = new QHBoxLayout;
layout->addLayout(lane_layout);
lane_widget = new QWidget;
lane_widget->setFixedHeight(125);
main_layout->addWidget(w);
lane_layout = new QHBoxLayout(lane_widget);
layout->addWidget(lane_widget);
main_layout->addLayout(layout);
}
setStyleSheet(R"(
@ -549,6 +369,7 @@ MapInstructions::MapInstructions(QWidget * parent) : QWidget(parent) {
}
void MapInstructions::updateDistance(float d) {
d = std::max(d, 0.0f);
QString distance_str;
if (QUIState::ui_state.scene.is_metric) {
@ -584,34 +405,37 @@ void MapInstructions::showError(QString error_text) {
secondary->setVisible(false);
icon_01->setVisible(false);
last_banner = {};
error = true;
this->error = true;
lane_widget->setVisible(false);
setVisible(true);
adjustSize();
}
void MapInstructions::updateInstructions(QMap<QString, QVariant> banner, bool full) {
// Need multiple calls to adjustSize for it to properly resize
// seems like it takes a little bit of time for the images to change and
// the size can only be changed afterwards
adjustSize();
void MapInstructions::noError() {
error = false;
}
void MapInstructions::updateInstructions(cereal::NavInstruction::Reader instruction) {
// Word wrap widgets need fixed width
primary->setFixedWidth(width() - 250);
secondary->setFixedWidth(width() - 250);
if (banner == last_banner) return;
QString primary_str, secondary_str;
auto p = banner["primary"].toMap();
primary_str += p["text"].toString();
// 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);
// Show arrow with direction
if (p.contains("type")) {
QString fn = "../assets/navigation/direction_" + p["type"].toString();
if (p.contains("modifier")) {
fn += "_" + p["modifier"].toString();
QString type = QString::fromStdString(instruction.getManeuverType());
QString modifier = QString::fromStdString(instruction.getManeuverModifier());
if (!type.isEmpty()) {
QString fn = "../assets/navigation/direction_" + type;
if (!modifier.isEmpty()) {
fn += "_" + modifier;
}
fn += + ".png";
fn = fn.replace(' ', '_');
@ -622,64 +446,45 @@ void MapInstructions::updateInstructions(QMap<QString, QVariant> banner, bool fu
icon_01->setVisible(true);
}
if (banner.contains("secondary") && full) {
auto s = banner["secondary"].toMap();
secondary_str += s["text"].toString();
}
clearLayout(lane_layout);
// Show lanes
bool has_lanes = false;
clearLayout(lane_layout);
for (auto const &lane: instruction.getLanes()) {
has_lanes = true;
bool active = lane.getActive();
if (banner.contains("sub") && full) {
auto s = banner["sub"].toMap();
auto components = s["components"].toList();
for (auto &c : components) {
auto cc = c.toMap();
if (cc["type"].toString() == "lane") {
has_lanes = true;
bool left = false;
bool straight = false;
bool right = false;
bool active = cc["active"].toBool();
for (auto &dir : cc["directions"].toList()) {
auto d = dir.toString();
left |= d.contains("left");
straight |= d.contains("straight");
right |= d.contains("right");
}
// TODO: Make more images based on active direction and combined directions
QString fn = "../assets/navigation/direction_";
if (left) {
fn += "turn_left";
} else if (right) {
fn += "turn_right";
} else if (straight) {
fn += "turn_straight";
}
QPixmap pix(fn + ".png");
auto icon = new QLabel;
icon->setPixmap(pix.scaledToWidth(active ? 125 : 75, Qt::SmoothTransformation));
icon->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
lane_layout->addWidget(icon);
}
// TODO: only use active direction if active
bool left = false, straight = false, right = false;
for (auto const &direction: lane.getDirections()) {
left |= direction == cereal::NavInstruction::Direction::LEFT;
right |= direction == cereal::NavInstruction::Direction::RIGHT;
straight |= direction == cereal::NavInstruction::Direction::STRAIGHT;
}
// TODO: Make more images based on active direction and combined directions
QString fn = "../assets/navigation/direction_";
if (left) {
fn += "turn_left";
} else if (right) {
fn += "turn_right";
} else if (straight) {
fn += "turn_straight";
}
QPixmap pix(fn + ".png");
auto icon = new QLabel;
int wh = active ? 125 : 75;
icon->setPixmap(pix.scaled(wh, wh, Qt::IgnoreAspectRatio, Qt::SmoothTransformation));
icon->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed));
lane_layout->addWidget(icon);
}
primary->setText(primary_str);
secondary->setVisible(secondary_str.length() > 0);
secondary->setText(secondary_str);
last_banner = banner;
error = false;
lane_widget->setVisible(has_lanes);
show();
adjustSize();
resize(sizeHint());
}
void MapInstructions::hideIfNoError() {
if (!error) {
hide();

View File

@ -3,11 +3,6 @@
#include <optional>
#include <QGeoCoordinate>
#include <QGeoManeuver>
#include <QGeoRouteRequest>
#include <QGeoRouteSegment>
#include <QGeoRoutingManager>
#include <QGeoServiceProvider>
#include <QGestureEvent>
#include <QHBoxLayout>
#include <QVBoxLayout>
@ -38,18 +33,19 @@ private:
QLabel *primary;
QLabel *secondary;
QLabel *icon_01;
QWidget *lane_widget;
QHBoxLayout *lane_layout;
QMap<QString, QVariant> last_banner;
bool error = false;
public:
MapInstructions(QWidget * parent=nullptr);
void showError(QString error);
void noError();
void hideIfNoError();
public slots:
void updateDistance(float d);
void updateInstructions(QMap<QString, QVariant> banner, bool full);
void updateInstructions(cereal::NavInstruction::Reader instruction);
};
class MapETA : public QWidget {
@ -101,6 +97,7 @@ private:
QTimer* timer;
bool loaded_once = false;
bool allow_open = true;
// Panning
QPointF m_lastPos;
@ -113,39 +110,20 @@ private:
FirstOrderFilter velocity_filter;
bool localizer_valid = false;
// Route
bool allow_open = true;
bool gps_ok = false;
QGeoServiceProvider *geoservice_provider;
QGeoRoutingManager *routing_manager;
QGeoRoute route;
QGeoRouteSegment segment;
MapInstructions* map_instructions;
MapETA* map_eta;
QMapbox::Coordinate nav_destination;
// Route recompute
QTimer* recompute_timer;
int recompute_backoff = 0;
int recompute_countdown = 0;
void calculateRoute(QMapbox::Coordinate destination);
void clearRoute();
bool shouldRecompute();
void updateETA();
private slots:
void timerUpdate();
void routeCalculated(QGeoRouteReply *reply);
void recomputeRoute();
public slots:
void offroadTransition(bool offroad);
signals:
void distanceChanged(float distance);
void instructionsChanged(QMap<QString, QVariant> banner, bool full);
void instructionsChanged(cereal::NavInstruction::Reader instruction);
void ETAChanged(float seconds, float seconds_typical, float distance);
};

View File

@ -50,6 +50,23 @@ QMapbox::CoordinatesCollections coordinate_to_collection(QMapbox::Coordinate c)
return collections;
}
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) {
QMapbox::Coordinate coordinate(c.getLatitude(), c.getLongitude());
coordinates.push_back(coordinate);
}
QMapbox::CoordinatesCollection collection;
collection.push_back(coordinates);
QMapbox::CoordinatesCollections collections;
collections.push_back(collection);
return collections;
}
QMapbox::CoordinatesCollections coordinate_list_to_collection(QList<QGeoCoordinate> coordinate_list) {
QMapbox::Coordinates coordinates;

View File

@ -19,6 +19,7 @@ QMapbox::CoordinatesCollections model_to_collection(
const cereal::LiveLocationKalman::Measurement::Reader &positionECEF,
const cereal::ModelDataV2::XYZTData::Reader &line);
QMapbox::CoordinatesCollections coordinate_to_collection(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(QList<QGeoCoordinate> coordinate_list);
float minimum_distance(QGeoCoordinate a, QGeoCoordinate b, QGeoCoordinate p);

View File

@ -10,7 +10,6 @@
#include "selfdrive/ui/qt/maps/map.h"
#endif
OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) {
QVBoxLayout *main_layout = new QVBoxLayout(this);
main_layout->setMargin(bdr_s);
@ -93,7 +92,7 @@ void OnroadWindow::offroadTransition(bool offroad) {
settings.setAccessToken(token.trimmed());
MapWindow * m = new MapWindow(settings);
m->setFixedWidth(width() / 2 - bdr_s);
m->setFixedWidth(topWidget(this)->width() / 2);
QObject::connect(this, &OnroadWindow::offroadTransitionSignal, m, &MapWindow::offroadTransition);
split->addWidget(m, 0, Qt::AlignRight);
map = m;

View File

@ -120,3 +120,9 @@ void swagLogMessageHandler(QtMsgType type, const QMessageLogContext &context, co
auto bts = msg.toUtf8();
cloudlog_e(levels[type], file.c_str(), context.line, function.c_str(), "%s", bts.constData());
}
QWidget* topWidget (QWidget* widget) {
while (widget->parentWidget() != nullptr) widget=widget->parentWidget();
return widget;
}

View File

@ -18,6 +18,7 @@ void setQtSurfaceFormat();
QString timeAgo(const QDateTime &date);
void swagLogMessageHandler(QtMsgType type, const QMessageLogContext &context, const QString &msg);
void initApp();
QWidget* topWidget (QWidget* widget);
// convenience class for wrapping layouts