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

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
+

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)),

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'])

selfdrive/ui/navd/.gitignore

@@ -0,0 +1 @@
+_navd

selfdrive/ui/navd/main.cc

@@ -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();
+}

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

selfdrive/ui/navd/route_engine.cc

@@ -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();
+}

selfdrive/ui/navd/route_engine.h

@@ -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();
+};

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<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
+  if (sm->updated("navInstruction")) {
-  auto point = coordinate_to_collection(*last_position);
+    if (sm->valid("navInstruction")) {
-  QMapbox::Feature feature1(QMapbox::Feature::PointType, point, {}, {});
+      auto i = (*sm)["navInstruction"].getNavInstruction();
-  QVariantMap carPosSource;
+      emit ETAChanged(i.getTimeRemaining(), i.getTimeRemainingTypical(), i.getDistanceRemaining());
-  carPosSource["type"] = "geojson";
-  carPosSource["data"] = QVariant::fromValue<QMapbox::Feature>(feature1);
-  m_map->updateSource("carPosSource", carPosSource);
-
-  // 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();
-      }
 
-      // Transition to next route segment
+      if (localizer_valid) {
-      if (!shouldRecompute() && (distance_to_maneuver_along_geometry < -MANEUVER_TRANSITION_THRESHOLD)) {
+        m_map->setPitch(MAX_PITCH); // TODO: smooth pitching based on maneuver distance
-        auto next_segment = segment.nextRouteSegment();
+        emit distanceChanged(i.getManeuverDistance()); // TODO: combine with instructionsChanged
-        if (next_segment.isValid()) {
+        emit instructionsChanged(i);
-          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();
-          }
-        }
       }
+    } else {
+      m_map->setPitch(MIN_PITCH);
+      clearRoute();
+    }
+  }
+
+  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");
+
+    // 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;
-    QVBoxLayout *layout = new QVBoxLayout(w);
 
     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;
+    lane_widget = new QWidget;
-    layout->addLayout(lane_layout);
+    lane_widget->setFixedHeight(125);
+
+    lane_layout = new QHBoxLayout(lane_widget);
+    layout->addWidget(lane_widget);
 
-    main_layout->addWidget(w);
+    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 = {};
+  this->error = true;
-  error = true;
+  lane_widget->setVisible(false);
 
   setVisible(true);
-  adjustSize();
 }
 
-void MapInstructions::updateInstructions(QMap<QString, QVariant> banner, bool full) {
+void MapInstructions::noError() {
-  // Need multiple calls to adjustSize for it to properly resize
+  error = false;
-  // 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::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();
+  // Show instruction text
-  primary_str += p["text"].toString();
+  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 type = QString::fromStdString(instruction.getManeuverType());
-    QString fn = "../assets/navigation/direction_" + p["type"].toString();
+  QString modifier = QString::fromStdString(instruction.getManeuverModifier());
-    if (p.contains("modifier")) {
+  if (!type.isEmpty()) {
-      fn += "_" + p["modifier"].toString();
+    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) {
+  // Show lanes
-    auto s = banner["secondary"].toMap();
-    secondary_str += s["text"].toString();
-  }
-
-  clearLayout(lane_layout);
   bool has_lanes = false;
-
+  clearLayout(lane_layout);
-  if (banner.contains("sub") && full) {
+  for (auto const &lane: instruction.getLanes()) {
-    auto s = banner["sub"].toMap();
+    has_lanes = true;
-    auto components = s["components"].toList();
+    bool active = lane.getActive();
-    for (auto &c : components) {
+
-      auto cc = c.toMap();
+    // TODO: only use active direction if active
-      if (cc["type"].toString() == "lane") {
+    bool left = false, straight = false, right = false;
-        has_lanes = true;
+    for (auto const &direction: lane.getDirections()) {
-
+      left |= direction == cereal::NavInstruction::Direction::LEFT;
-        bool left = false;
+      right |= direction == cereal::NavInstruction::Direction::RIGHT;
-        bool straight = false;
+      straight |= direction == cereal::NavInstruction::Direction::STRAIGHT;
-        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);
-      }
     }
-  }
 
-  primary->setText(primary_str);
+    // TODO: Make more images based on active direction and combined directions
-  secondary->setVisible(secondary_str.length() > 0);
+    QString fn = "../assets/navigation/direction_";
-  secondary->setText(secondary_str);
+    if (left) {
+      fn += "turn_left";
+    } else if (right) {
+      fn += "turn_right";
+    } else if (straight) {
+      fn += "turn_straight";
+    }
 
-  last_banner = banner;
+    QPixmap pix(fn + ".png");
-  error = false;
+    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);
+  }
+  lane_widget->setVisible(has_lanes);
 
   show();
-  adjustSize();
+  resize(sizeHint());
 }
 
+
 void MapInstructions::hideIfNoError() {
   if (!error) {
     hide();

selfdrive/ui/qt/maps/map.h

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

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

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

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;

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;
+}

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