diff --git a/release/files_tici b/release/files_tici index b5a2e5b7a..29c8353b8 100644 --- a/release/files_tici +++ b/release/files_tici @@ -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 + diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 128450540..8dc010e55 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -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)), diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index 17441b6ba..822a0db0d 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -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']) diff --git a/selfdrive/ui/navd/.gitignore b/selfdrive/ui/navd/.gitignore new file mode 100644 index 000000000..0fa7b173e --- /dev/null +++ b/selfdrive/ui/navd/.gitignore @@ -0,0 +1 @@ +_navd diff --git a/selfdrive/ui/navd/main.cc b/selfdrive/ui/navd/main.cc new file mode 100644 index 000000000..cc8d9c339 --- /dev/null +++ b/selfdrive/ui/navd/main.cc @@ -0,0 +1,36 @@ +#include +#include +#include +#include +#include + +#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(); +} diff --git a/selfdrive/ui/navd/navd b/selfdrive/ui/navd/navd new file mode 100755 index 000000000..ff3103bd9 --- /dev/null +++ b/selfdrive/ui/navd/navd @@ -0,0 +1,4 @@ +#!/bin/sh +cd "$(dirname "$0")" +export QT_PLUGIN_PATH="../../../third_party/qt-plugins/$(uname -m)" +exec ./_navd diff --git a/selfdrive/ui/navd/route_engine.cc b/selfdrive/ui/navd/route_engine.cc new file mode 100644 index 000000000..c4ed872e1 --- /dev/null +++ b/selfdrive/ui/navd/route_engine.cc @@ -0,0 +1,323 @@ +#include "selfdrive/ui/navd/route_engine.h" + +#include + +#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 &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(); +} diff --git a/selfdrive/ui/navd/route_engine.h b/selfdrive/ui/navd/route_engine.h new file mode 100644 index 000000000..af69b5c76 --- /dev/null +++ b/selfdrive/ui/navd/route_engine.h @@ -0,0 +1,50 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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 last_position; + std::optional 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(); +}; diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc index 3468f25e9..e88879082 100644 --- a/selfdrive/ui/qt/maps/map.cc +++ b/selfdrive/ui/qt/maps/map.cc @@ -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(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(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(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(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(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 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 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(); diff --git a/selfdrive/ui/qt/maps/map.h b/selfdrive/ui/qt/maps/map.h index 63f25a625..519750ce5 100644 --- a/selfdrive/ui/qt/maps/map.h +++ b/selfdrive/ui/qt/maps/map.h @@ -3,11 +3,6 @@ #include #include -#include -#include -#include -#include -#include #include #include #include @@ -38,18 +33,19 @@ private: QLabel *primary; QLabel *secondary; QLabel *icon_01; + QWidget *lane_widget; QHBoxLayout *lane_layout; - QMap 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 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 banner, bool full); + void instructionsChanged(cereal::NavInstruction::Reader instruction); void ETAChanged(float seconds, float seconds_typical, float distance); }; diff --git a/selfdrive/ui/qt/maps/map_helpers.cc b/selfdrive/ui/qt/maps/map_helpers.cc index cdfa4b3ce..422ae99b6 100644 --- a/selfdrive/ui/qt/maps/map_helpers.cc +++ b/selfdrive/ui/qt/maps/map_helpers.cc @@ -50,6 +50,23 @@ QMapbox::CoordinatesCollections coordinate_to_collection(QMapbox::Coordinate c) return collections; } +QMapbox::CoordinatesCollections capnp_coordinate_list_to_collection(const capnp::List::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 coordinate_list) { QMapbox::Coordinates coordinates; diff --git a/selfdrive/ui/qt/maps/map_helpers.h b/selfdrive/ui/qt/maps/map_helpers.h index a85352ff8..db9b885b7 100644 --- a/selfdrive/ui/qt/maps/map_helpers.h +++ b/selfdrive/ui/qt/maps/map_helpers.h @@ -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::Reader &coordinate_list); QMapbox::CoordinatesCollections coordinate_list_to_collection(QList coordinate_list); float minimum_distance(QGeoCoordinate a, QGeoCoordinate b, QGeoCoordinate p); diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 8279035fa..7c3a610ee 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -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; diff --git a/selfdrive/ui/qt/util.cc b/selfdrive/ui/qt/util.cc index 3f98283bf..93bb2b0e7 100644 --- a/selfdrive/ui/qt/util.cc +++ b/selfdrive/ui/qt/util.cc @@ -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; +} diff --git a/selfdrive/ui/qt/util.h b/selfdrive/ui/qt/util.h index 1c4e4b5f4..ad28b5116 100644 --- a/selfdrive/ui/qt/util.h +++ b/selfdrive/ui/qt/util.h @@ -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