openpilot v0.9.6 release

date: 2024-01-12T10:13:37
master commit: ba792d576a49a0899b88a753fa1c52956bedf9e6

selfdrive/navd/.gitignore

@@ -0,0 +1,6 @@
+moc_*
+*.moc
+
+mapsd
+map_renderer
+libmap_renderer.so

selfdrive/navd/README.md

@@ -0,0 +1,24 @@
+# navigation
+
+This directory contains two daemons, `navd` and `mapsd`, which support navigation in the openpilot stack.
+
+### navd
+
+`navd` takes in a route through the `NavDestination` param and sends out two packets: `navRoute` and `navInstruction`. These packets contain the coordinates of the planned route and turn-by-turn instructions.
+
+### map renderer
+
+The map renderer listens for the `navRoute` and publishes a rendered map view over VisionIPC for the navigation model, which lives in `selfdrive/modeld/`. The rendered maps look like this:
+
+![](https://i.imgur.com/oZLfmwq.png)
+
+## development
+
+Currently, [mapbox](https://www.mapbox.com/) is used for navigation.
+
+* get an API token: https://docs.mapbox.com/help/glossary/access-token/
+* set an API token using the `MAPBOX_TOKEN` environment variable
+* routes/destinations are set through the `NavDestination` param
+  * use `set_destination.py` for debugging
+* edit the map: https://www.mapbox.com/contribute
+* mapbox API playground: https://docs.mapbox.com/playground/

selfdrive/navd/SConscript

@@ -0,0 +1,20 @@
+Import('qt_env', 'arch', 'common', 'messaging', 'visionipc', 'cereal', 'transformations')
+
+map_env = qt_env.Clone()
+libs = ['qt_widgets', 'qt_util', 'qmapboxgl', common, messaging, cereal, visionipc, transformations,
+        'zmq', 'capnp', 'kj', 'm', 'OpenCL', 'ssl', 'crypto', 'pthread', 'json11'] + map_env["LIBS"]
+if arch == 'larch64':
+  libs.append(':libEGL_mesa.so.0')
+
+if arch in ['larch64', 'aarch64', 'x86_64']:
+  if arch == 'x86_64':
+    rpath = Dir(f"#third_party/mapbox-gl-native-qt/{arch}").srcnode().abspath
+    map_env["RPATH"] += [rpath, ]
+
+  style_path = File("style.json").abspath
+  map_env['CXXFLAGS'].append(f'-DSTYLE_PATH=\\"{style_path}\\"')
+
+  map_env["RPATH"].append(Dir('.').abspath)
+  map_env["LIBPATH"].append(Dir('.').abspath)
+  maplib = map_env.SharedLibrary("maprender", ["map_renderer.cc"], LIBS=libs)
+  map_env.Program("mapsd", ["main.cc", ], LIBS=[maplib[0].get_path(), ] + libs)

selfdrive/navd/helpers.py

@@ -0,0 +1,189 @@
+from __future__ import annotations
+
+import json
+import math
+from typing import Any, Dict, List, Optional, Tuple, Union, cast
+
+from openpilot.common.conversions import Conversions
+from openpilot.common.numpy_fast import clip
+from openpilot.common.params import Params
+
+DIRECTIONS = ('left', 'right', 'straight')
+MODIFIABLE_DIRECTIONS = ('left', 'right')
+
+EARTH_MEAN_RADIUS = 6371007.2
+SPEED_CONVERSIONS = {
+    'km/h': Conversions.KPH_TO_MS,
+    'mph': Conversions.MPH_TO_MS,
+  }
+
+
+class Coordinate:
+  def __init__(self, latitude: float, longitude: float) -> None:
+    self.latitude = latitude
+    self.longitude = longitude
+    self.annotations: Dict[str, float] = {}
+
+  @classmethod
+  def from_mapbox_tuple(cls, t: Tuple[float, float]) -> Coordinate:
+    return cls(t[1], t[0])
+
+  def as_dict(self) -> Dict[str, float]:
+    return {'latitude': self.latitude, 'longitude': self.longitude}
+
+  def __str__(self) -> str:
+    return f'Coordinate({self.latitude}, {self.longitude})'
+
+  def __repr__(self) -> str:
+    return self.__str__()
+
+  def __eq__(self, other) -> bool:
+    if not isinstance(other, Coordinate):
+      return False
+    return (self.latitude == other.latitude) and (self.longitude == other.longitude)
+
+  def __sub__(self, other: Coordinate) -> Coordinate:
+    return Coordinate(self.latitude - other.latitude, self.longitude - other.longitude)
+
+  def __add__(self, other: Coordinate) -> Coordinate:
+    return Coordinate(self.latitude + other.latitude, self.longitude + other.longitude)
+
+  def __mul__(self, c: float) -> Coordinate:
+    return Coordinate(self.latitude * c, self.longitude * c)
+
+  def dot(self, other: Coordinate) -> float:
+    return self.latitude * other.latitude + self.longitude * other.longitude
+
+  def distance_to(self, other: Coordinate) -> float:
+    # Haversine formula
+    dlat = math.radians(other.latitude - self.latitude)
+    dlon = math.radians(other.longitude - self.longitude)
+
+    haversine_dlat = math.sin(dlat / 2.0)
+    haversine_dlat *= haversine_dlat
+    haversine_dlon = math.sin(dlon / 2.0)
+    haversine_dlon *= haversine_dlon
+
+    y = haversine_dlat \
+             + math.cos(math.radians(self.latitude)) \
+             * math.cos(math.radians(other.latitude)) \
+             * haversine_dlon
+    x = 2 * math.asin(math.sqrt(y))
+    return x * EARTH_MEAN_RADIUS
+
+
+def minimum_distance(a: Coordinate, b: Coordinate, p: Coordinate):
+  if a.distance_to(b) < 0.01:
+    return a.distance_to(p)
+
+  ap = p - a
+  ab = b - a
+  t = clip(ap.dot(ab) / ab.dot(ab), 0.0, 1.0)
+  projection = a + ab * t
+  return projection.distance_to(p)
+
+
+def distance_along_geometry(geometry: List[Coordinate], pos: Coordinate) -> float:
+  if len(geometry) <= 2:
+    return geometry[0].distance_to(pos)
+
+  # 1. Find segment that is closest to current position
+  # 2. Total distance is sum of distance to start of closest segment
+  #    + all previous segments
+  total_distance = 0.0
+  total_distance_closest = 0.0
+  closest_distance = 1e9
+
+  for i in range(len(geometry) - 1):
+    d = minimum_distance(geometry[i], geometry[i + 1], pos)
+
+    if d < closest_distance:
+      closest_distance = d
+      total_distance_closest = total_distance + geometry[i].distance_to(pos)
+
+    total_distance += geometry[i].distance_to(geometry[i + 1])
+
+  return total_distance_closest
+
+
+def coordinate_from_param(param: str, params: Optional[Params] = None) -> Optional[Coordinate]:
+  if params is None:
+    params = Params()
+
+  json_str = params.get(param)
+  if json_str is None:
+    return None
+
+  pos = json.loads(json_str)
+  if 'latitude' not in pos or 'longitude' not in pos:
+    return None
+
+  return Coordinate(pos['latitude'], pos['longitude'])
+
+
+def string_to_direction(direction: str) -> str:
+  for d in DIRECTIONS:
+    if d in direction:
+      if 'slight' in direction and d in MODIFIABLE_DIRECTIONS:
+        return 'slight' + d.capitalize()
+      return d
+  return 'none'
+
+
+def maxspeed_to_ms(maxspeed: Dict[str, Union[str, float]]) -> float:
+  unit = cast(str, maxspeed['unit'])
+  speed = cast(float, maxspeed['speed'])
+  return SPEED_CONVERSIONS[unit] * speed
+
+
+def field_valid(dat: dict, field: str) -> bool:
+  return field in dat and dat[field] is not None
+
+
+def parse_banner_instructions(banners: Any, distance_to_maneuver: float = 0.0) -> Optional[Dict[str, Any]]:
+  if not len(banners):
+    return None
+
+  instruction = {}
+
+  # A segment can contain multiple banners, find one that we need to show now
+  current_banner = banners[0]
+  for banner in banners:
+    if distance_to_maneuver < banner['distanceAlongGeometry']:
+      current_banner = banner
+
+  # Only show banner when close enough to maneuver
+  instruction['showFull'] = distance_to_maneuver < current_banner['distanceAlongGeometry']
+
+  # Primary
+  p = current_banner['primary']
+  if field_valid(p, 'text'):
+    instruction['maneuverPrimaryText'] = p['text']
+  if field_valid(p, 'type'):
+    instruction['maneuverType'] = p['type']
+  if field_valid(p, 'modifier'):
+    instruction['maneuverModifier'] = p['modifier']
+
+  # Secondary
+  if field_valid(current_banner, 'secondary'):
+    instruction['maneuverSecondaryText'] = current_banner['secondary']['text']
+
+  # Lane lines
+  if field_valid(current_banner, 'sub'):
+    lanes = []
+    for component in current_banner['sub']['components']:
+      if component['type'] != 'lane':
+        continue
+
+      lane = {
+        'active': component['active'],
+        'directions': [string_to_direction(d) for d in component['directions']],
+      }
+
+      if field_valid(component, 'active_direction'):
+        lane['activeDirection'] = string_to_direction(component['active_direction'])
+
+      lanes.append(lane)
+    instruction['lanes'] = lanes
+
+  return instruction

selfdrive/navd/main.cc

@@ -0,0 +1,29 @@
+#include <csignal>
+#include <sys/resource.h>
+
+#include <QApplication>
+#include <QDebug>
+
+#include "common/util.h"
+#include "selfdrive/ui/qt/util.h"
+#include "selfdrive/ui/qt/maps/map_helpers.h"
+#include "selfdrive/navd/map_renderer.h"
+#include "system/hardware/hw.h"
+
+int main(int argc, char *argv[]) {
+  Hardware::config_cpu_rendering(true);
+
+  qInstallMessageHandler(swagLogMessageHandler);
+  setpriority(PRIO_PROCESS, 0, -20);
+  int ret = util::set_core_affinity({0, 1, 2, 3});
+  assert(ret == 0);
+
+  QApplication app(argc, argv);
+  std::signal(SIGINT, sigTermHandler);
+  std::signal(SIGTERM, sigTermHandler);
+
+  MapRenderer * m = new MapRenderer(get_mapbox_settings());
+  assert(m);
+
+  return app.exec();
+}

selfdrive/navd/map_renderer.cc

@@ -0,0 +1,334 @@
+#include "selfdrive/navd/map_renderer.h"
+
+#include <cmath>
+#include <string>
+#include <QApplication>
+#include <QBuffer>
+
+#include "common/util.h"
+#include "common/timing.h"
+#include "common/swaglog.h"
+#include "selfdrive/ui/qt/maps/map_helpers.h"
+
+const float DEFAULT_ZOOM = 13.5; // Don't go below 13 or features will start to disappear
+const int HEIGHT = 256, WIDTH = 256;
+const int NUM_VIPC_BUFFERS = 4;
+
+const int EARTH_CIRCUMFERENCE_METERS = 40075000;
+const int EARTH_RADIUS_METERS = 6378137;
+const int PIXELS_PER_TILE = 256;
+const int MAP_OFFSET = 128;
+
+const bool TEST_MODE = getenv("MAP_RENDER_TEST_MODE");
+const int LLK_DECIMATION = TEST_MODE ? 1 : 10;
+
+float get_zoom_level_for_scale(float lat, float meters_per_pixel) {
+  float meters_per_tile = meters_per_pixel * PIXELS_PER_TILE;
+  float num_tiles = cos(DEG2RAD(lat)) * EARTH_CIRCUMFERENCE_METERS / meters_per_tile;
+  return log2(num_tiles) - 1;
+}
+
+QMapbox::Coordinate get_point_along_line(float lat, float lon, float bearing, float dist) {
+  float ang_dist = dist / EARTH_RADIUS_METERS;
+  float lat1 = DEG2RAD(lat), lon1 = DEG2RAD(lon), bearing1 = DEG2RAD(bearing);
+  float lat2 = asin(sin(lat1)*cos(ang_dist) + cos(lat1)*sin(ang_dist)*cos(bearing1));
+  float lon2 = lon1 + atan2(sin(bearing1)*sin(ang_dist)*cos(lat1), cos(ang_dist)-sin(lat1)*sin(lat2));
+  return QMapbox::Coordinate(RAD2DEG(lat2), RAD2DEG(lon2));
+}
+
+
+MapRenderer::MapRenderer(const QMapboxGLSettings &settings, bool online) : m_settings(settings) {
+  QSurfaceFormat fmt;
+  fmt.setRenderableType(QSurfaceFormat::OpenGLES);
+
+  ctx = std::make_unique<QOpenGLContext>();
+  ctx->setFormat(fmt);
+  ctx->create();
+  assert(ctx->isValid());
+
+  surface = std::make_unique<QOffscreenSurface>();
+  surface->setFormat(ctx->format());
+  surface->create();
+
+  ctx->makeCurrent(surface.get());
+  assert(QOpenGLContext::currentContext() == ctx.get());
+
+  gl_functions.reset(ctx->functions());
+  gl_functions->initializeOpenGLFunctions();
+
+  QOpenGLFramebufferObjectFormat fbo_format;
+  fbo.reset(new QOpenGLFramebufferObject(WIDTH, HEIGHT, fbo_format));
+
+  std::string style = util::read_file(STYLE_PATH);
+  m_map.reset(new QMapboxGL(nullptr, m_settings, fbo->size(), 1));
+  m_map->setCoordinateZoom(QMapbox::Coordinate(0, 0), DEFAULT_ZOOM);
+  m_map->setStyleJson(style.c_str());
+  m_map->createRenderer();
+  ever_loaded = false;
+
+  m_map->resize(fbo->size());
+  m_map->setFramebufferObject(fbo->handle(), fbo->size());
+  gl_functions->glViewport(0, 0, WIDTH, HEIGHT);
+
+  QObject::connect(m_map.data(), &QMapboxGL::mapChanged, [=](QMapboxGL::MapChange change) {
+    // Ignore expected signals
+    // https://github.com/mapbox/mapbox-gl-native/blob/cf734a2fec960025350d8de0d01ad38aeae155a0/platform/qt/include/qmapboxgl.hpp#L116
+    if (ever_loaded) {
+      if (change != QMapboxGL::MapChange::MapChangeRegionWillChange &&
+          change != QMapboxGL::MapChange::MapChangeRegionDidChange &&
+          change != QMapboxGL::MapChange::MapChangeWillStartRenderingFrame &&
+          change != QMapboxGL::MapChange::MapChangeDidFinishRenderingFrameFullyRendered) {
+        LOGD("New map state: %d", change);
+      }
+    }
+  });
+
+  QObject::connect(m_map.data(), &QMapboxGL::mapLoadingFailed, [=](QMapboxGL::MapLoadingFailure err_code, const QString &reason) {
+    LOGE("Map loading failed with %d: '%s'\n", err_code, reason.toStdString().c_str());
+  });
+
+  if (online) {
+    vipc_server.reset(new VisionIpcServer("navd"));
+    vipc_server->create_buffers(VisionStreamType::VISION_STREAM_MAP, NUM_VIPC_BUFFERS, false, WIDTH, HEIGHT);
+    vipc_server->start_listener();
+
+    pm.reset(new PubMaster({"navThumbnail", "mapRenderState"}));
+    sm.reset(new SubMaster({"liveLocationKalman", "navRoute"}, {"liveLocationKalman"}));
+
+    timer = new QTimer(this);
+    timer->setSingleShot(true);
+    QObject::connect(timer, SIGNAL(timeout()), this, SLOT(msgUpdate()));
+    timer->start(0);
+  }
+}
+
+void MapRenderer::msgUpdate() {
+  sm->update(1000);
+
+  if (sm->updated("liveLocationKalman")) {
+    auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman();
+    auto pos = location.getPositionGeodetic();
+    auto orientation = location.getCalibratedOrientationNED();
+
+    if ((sm->rcv_frame("liveLocationKalman") % LLK_DECIMATION) == 0) {
+      float bearing = RAD2DEG(orientation.getValue()[2]);
+      updatePosition(get_point_along_line(pos.getValue()[0], pos.getValue()[1], bearing, MAP_OFFSET), bearing);
+
+      // TODO: use the static rendering mode instead
+      // retry render a few times
+      for (int i = 0; i < 5 && !rendered(); i++) {
+        QApplication::processEvents(QEventLoop::AllEvents, 100);
+        update();
+        if (rendered()) {
+          LOGW("rendered after %d retries", i+1);
+          break;
+        }
+      }
+
+      // fallback to sending a blank frame
+      if (!rendered()) {
+        publish(0, false);
+      }
+    }
+  }
+
+  if (sm->updated("navRoute")) {
+    QList<QGeoCoordinate> route;
+    auto coords = (*sm)["navRoute"].getNavRoute().getCoordinates();
+    for (auto const &c : coords) {
+      route.push_back(QGeoCoordinate(c.getLatitude(), c.getLongitude()));
+    }
+    updateRoute(route);
+  }
+
+  // schedule next update
+  timer->start(0);
+}
+
+void MapRenderer::updatePosition(QMapbox::Coordinate position, float bearing) {
+  if (m_map.isNull()) {
+    return;
+  }
+
+  // Choose a scale that ensures above 13 zoom level up to and above 75deg of lat
+  float meters_per_pixel = 2;
+  float zoom = get_zoom_level_for_scale(position.first, meters_per_pixel);
+
+  m_map->setCoordinate(position);
+  m_map->setBearing(bearing);
+  m_map->setZoom(zoom);
+  update();
+}
+
+bool MapRenderer::loaded() {
+  return m_map->isFullyLoaded();
+}
+
+void MapRenderer::update() {
+  double start_t = millis_since_boot();
+  gl_functions->glClear(GL_COLOR_BUFFER_BIT);
+  m_map->render();
+  gl_functions->glFlush();
+  double end_t = millis_since_boot();
+
+  if ((vipc_server != nullptr) && loaded()) {
+    publish((end_t - start_t) / 1000.0, true);
+    last_llk_rendered = (*sm)["liveLocationKalman"].getLogMonoTime();
+  }
+}
+
+void MapRenderer::sendThumbnail(const uint64_t ts, const kj::Array<capnp::byte> &buf) {
+  MessageBuilder msg;
+  auto thumbnaild = msg.initEvent().initNavThumbnail();
+  thumbnaild.setFrameId(frame_id);
+  thumbnaild.setTimestampEof(ts);
+  thumbnaild.setThumbnail(buf);
+  pm->send("navThumbnail", msg);
+}
+
+void MapRenderer::publish(const double render_time, const bool loaded) {
+  QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor);
+
+  auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman();
+  bool valid = loaded && (location.getStatus() == cereal::LiveLocationKalman::Status::VALID) && location.getPositionGeodetic().getValid();
+  ever_loaded = ever_loaded || loaded;
+  uint64_t ts = nanos_since_boot();
+  VisionBuf* buf = vipc_server->get_buffer(VisionStreamType::VISION_STREAM_MAP);
+  VisionIpcBufExtra extra = {
+    .frame_id = frame_id,
+    .timestamp_sof = (*sm)["liveLocationKalman"].getLogMonoTime(),
+    .timestamp_eof = ts,
+    .valid = valid,
+  };
+
+  assert(cap.sizeInBytes() >= buf->len);
+  uint8_t* dst = (uint8_t*)buf->addr;
+  uint8_t* src = cap.bits();
+
+  // RGB to greyscale
+  memset(dst, 128, buf->len);
+  for (int i = 0; i < WIDTH * HEIGHT; i++) {
+    dst[i] = src[i * 3];
+  }
+
+  vipc_server->send(buf, &extra);
+
+  // Send thumbnail
+  if (TEST_MODE) {
+    // Full image in thumbnails in test mode
+    kj::Array<capnp::byte> buffer_kj = kj::heapArray<capnp::byte>((const capnp::byte*)cap.bits(), cap.sizeInBytes());
+    sendThumbnail(ts, buffer_kj);
+  } else if (frame_id % 100 == 0) {
+    // Write jpeg into buffer
+    QByteArray buffer_bytes;
+    QBuffer buffer(&buffer_bytes);
+    buffer.open(QIODevice::WriteOnly);
+    cap.save(&buffer, "JPG", 50);
+
+    kj::Array<capnp::byte> buffer_kj = kj::heapArray<capnp::byte>((const capnp::byte*)buffer_bytes.constData(), buffer_bytes.size());
+    sendThumbnail(ts, buffer_kj);
+  }
+
+  // Send state msg
+  MessageBuilder msg;
+  auto evt = msg.initEvent();
+  auto state = evt.initMapRenderState();
+  evt.setValid(valid);
+  state.setLocationMonoTime((*sm)["liveLocationKalman"].getLogMonoTime());
+  state.setRenderTime(render_time);
+  state.setFrameId(frame_id);
+  pm->send("mapRenderState", msg);
+
+  frame_id++;
+}
+
+uint8_t* MapRenderer::getImage() {
+  QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor);
+
+  uint8_t* src = cap.bits();
+  uint8_t* dst = new uint8_t[WIDTH * HEIGHT];
+
+  // RGB to greyscale
+  for (int i = 0; i < WIDTH * HEIGHT; i++) {
+    dst[i] = src[i * 3];
+  }
+
+  return dst;
+}
+
+void MapRenderer::updateRoute(QList<QGeoCoordinate> coordinates) {
+  if (m_map.isNull()) return;
+  initLayers();
+
+  auto route_points = coordinate_list_to_collection(coordinates);
+  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");
+}
+
+void MapRenderer::initLayers() {
+  if (!m_map->layerExists("navLayer")) {
+    LOGD("Initializing navLayer");
+    QVariantMap nav;
+    nav["id"] = "navLayer";
+    nav["type"] = "line";
+    nav["source"] = "navSource";
+    m_map->addLayer(nav, "road-intersection");
+    m_map->setPaintProperty("navLayer", "line-color", QColor("grey"));
+    m_map->setPaintProperty("navLayer", "line-width", 5);
+    m_map->setLayoutProperty("navLayer", "line-cap", "round");
+  }
+}
+
+MapRenderer::~MapRenderer() {
+}
+
+extern "C" {
+  MapRenderer* map_renderer_init(char *maps_host = nullptr, char *token = nullptr) {
+    char *argv[] = {
+      (char*)"navd",
+      nullptr
+    };
+    int argc = 0;
+    QApplication *app = new QApplication(argc, argv);
+    assert(app);
+
+    QMapboxGLSettings settings;
+    settings.setApiBaseUrl(maps_host == nullptr ? MAPS_HOST : maps_host);
+    settings.setAccessToken(token == nullptr ? get_mapbox_token() : token);
+
+    return new MapRenderer(settings, false);
+  }
+
+  void map_renderer_update_position(MapRenderer *inst, float lat, float lon, float bearing) {
+    inst->updatePosition({lat, lon}, bearing);
+    QApplication::processEvents();
+  }
+
+  void map_renderer_update_route(MapRenderer *inst, char* polyline) {
+    inst->updateRoute(polyline_to_coordinate_list(QString::fromUtf8(polyline)));
+  }
+
+  void map_renderer_update(MapRenderer *inst) {
+    inst->update();
+  }
+
+  void map_renderer_process(MapRenderer *inst) {
+    QApplication::processEvents();
+  }
+
+  bool map_renderer_loaded(MapRenderer *inst) {
+    return inst->loaded();
+  }
+
+  uint8_t * map_renderer_get_image(MapRenderer *inst) {
+    return inst->getImage();
+  }
+
+  void map_renderer_free_image(MapRenderer *inst, uint8_t * buf) {
+    delete[] buf;
+  }
+}

selfdrive/navd/map_renderer.h

@@ -0,0 +1,58 @@
+#pragma once
+
+#include <memory>
+
+#include <QOpenGLContext>
+#include <QMapboxGL>
+#include <QTimer>
+#include <QGeoCoordinate>
+#include <QOpenGLBuffer>
+#include <QOffscreenSurface>
+#include <QOpenGLFunctions>
+#include <QOpenGLFramebufferObject>
+
+#include "cereal/visionipc/visionipc_server.h"
+#include "cereal/messaging/messaging.h"
+
+
+class MapRenderer : public QObject {
+  Q_OBJECT
+
+public:
+  MapRenderer(const QMapboxGLSettings &, bool online=true);
+  uint8_t* getImage();
+  void update();
+  bool loaded();
+  ~MapRenderer();
+
+private:
+  std::unique_ptr<QOpenGLContext> ctx;
+  std::unique_ptr<QOffscreenSurface> surface;
+  std::unique_ptr<QOpenGLFunctions> gl_functions;
+  std::unique_ptr<QOpenGLFramebufferObject> fbo;
+
+  std::unique_ptr<VisionIpcServer> vipc_server;
+  std::unique_ptr<PubMaster> pm;
+  std::unique_ptr<SubMaster> sm;
+  void publish(const double render_time, const bool loaded);
+  void sendThumbnail(const uint64_t ts, const kj::Array<capnp::byte> &buf);
+
+  QMapboxGLSettings m_settings;
+  QScopedPointer<QMapboxGL> m_map;
+
+  void initLayers();
+
+  uint32_t frame_id = 0;
+  uint64_t last_llk_rendered = 0;
+  bool rendered() {
+    return last_llk_rendered == (*sm)["liveLocationKalman"].getLogMonoTime();
+  }
+
+  QTimer* timer;
+  bool ever_loaded = false;
+
+public slots:
+  void updatePosition(QMapbox::Coordinate position, float bearing);
+  void updateRoute(QList<QGeoCoordinate> coordinates);
+  void msgUpdate();
+};

selfdrive/navd/map_renderer.py

@@ -0,0 +1,96 @@
+#!/usr/bin/env python3
+# You might need to uninstall the PyQt5 pip package to avoid conflicts
+
+import os
+import time
+import numpy as np
+import polyline
+from cffi import FFI
+
+from openpilot.common.ffi_wrapper import suffix
+from openpilot.common.basedir import BASEDIR
+
+HEIGHT = WIDTH = SIZE = 256
+METERS_PER_PIXEL = 2
+
+
+def get_ffi():
+  lib = os.path.join(BASEDIR, "selfdrive", "navd", "libmaprender" + suffix())
+
+  ffi = FFI()
+  ffi.cdef("""
+void* map_renderer_init(char *maps_host, char *token);
+void map_renderer_update_position(void *inst, float lat, float lon, float bearing);
+void map_renderer_update_route(void *inst, char *polyline);
+void map_renderer_update(void *inst);
+void map_renderer_process(void *inst);
+bool map_renderer_loaded(void *inst);
+uint8_t* map_renderer_get_image(void *inst);
+void map_renderer_free_image(void *inst, uint8_t *buf);
+""")
+  return ffi, ffi.dlopen(lib)
+
+
+def wait_ready(lib, renderer):
+  while not lib.map_renderer_loaded(renderer):
+    lib.map_renderer_update(renderer)
+
+    # The main qt app is not execed, so we need to periodically process events for e.g. network requests
+    lib.map_renderer_process(renderer)
+
+    time.sleep(0.01)
+
+
+def get_image(lib, renderer):
+  buf = lib.map_renderer_get_image(renderer)
+  r = list(buf[0:WIDTH * HEIGHT])
+  lib.map_renderer_free_image(renderer, buf)
+
+  # Convert to numpy
+  r = np.asarray(r)
+  return r.reshape((WIDTH, HEIGHT))
+
+
+def navRoute_to_polyline(nr):
+  coords = [(m.latitude, m.longitude) for m in nr.navRoute.coordinates]
+  return coords_to_polyline(coords)
+
+
+def coords_to_polyline(coords):
+  # TODO: where does this factor of 10 come from?
+  return polyline.encode([(lat * 10., lon * 10.) for lat, lon in coords])
+
+
+def polyline_to_coords(p):
+  coords = polyline.decode(p)
+  return [(lat / 10., lon / 10.) for lat, lon in coords]
+
+
+
+if __name__ == "__main__":
+  import matplotlib.pyplot as plt
+
+  ffi, lib = get_ffi()
+  renderer = lib.map_renderer_init(ffi.NULL, ffi.NULL)
+  wait_ready(lib, renderer)
+
+  geometry = r"{yxk}@|obn~Eg@@eCFqc@J{RFw@?kA@gA?q|@Riu@NuJBgi@ZqVNcRBaPBkG@iSD{I@_H@cH?gG@mG@gG?aD@{LDgDDkVVyQLiGDgX@q_@@qI@qKhS{R~[}NtYaDbGoIvLwNfP_b@|f@oFnF_JxHel@bf@{JlIuxAlpAkNnLmZrWqFhFoh@jd@kX|TkJxH_RnPy^|[uKtHoZ~Um`DlkCorC``CuShQogCtwB_ThQcr@fk@sVrWgRhVmSb\\oj@jxA{Qvg@u]tbAyHzSos@xjBeKbWszAbgEc~@~jCuTrl@cYfo@mRn\\_m@v}@ij@jp@om@lk@y|A`pAiXbVmWzUod@xj@wNlTw}@|uAwSn\\kRfYqOdS_IdJuK`KmKvJoOhLuLbHaMzGwO~GoOzFiSrEsOhD}PhCqw@vJmnAxSczA`Vyb@bHk[fFgl@pJeoDdl@}}@zIyr@hG}X`BmUdBcM^aRR}Oe@iZc@mR_@{FScHxAn_@vz@zCzH~GjPxAhDlB~DhEdJlIbMhFfG|F~GlHrGjNjItLnGvQ~EhLnBfOn@p`@AzAAvn@CfC?fc@`@lUrArStCfSxEtSzGxM|ElFlBrOzJlEbDnC~BfDtCnHjHlLvMdTnZzHpObOf^pKla@~G|a@dErg@rCbj@zArYlj@ttJ~AfZh@r]LzYg@`TkDbj@gIdv@oE|i@kKzhA{CdNsEfOiGlPsEvMiDpLgBpHyB`MkB|MmArPg@|N?|P^rUvFz~AWpOCdAkB|PuB`KeFfHkCfGy@tAqC~AsBPkDs@uAiAcJwMe@s@eKkPMoXQux@EuuCoH?eI?Kas@}Dy@wAUkMOgDL" # noqa: E501
+  lib.map_renderer_update_route(renderer, geometry.encode())
+
+  POSITIONS = [
+    (32.71569271952601, -117.16384270868463, 0), (32.71569271952601, -117.16384270868463, 45),  # San Diego
+    (52.378641991483136, 4.902623379456488, 0), (52.378641991483136, 4.902623379456488, 45),  # Amsterdam
+  ]
+  plt.figure()
+
+  for i, pos in enumerate(POSITIONS):
+    t = time.time()
+    lib.map_renderer_update_position(renderer, *pos)
+    wait_ready(lib, renderer)
+
+    print(f"{pos} took {time.time() - t:.2f} s")
+
+    plt.subplot(2, 2, i + 1)
+    plt.imshow(get_image(lib, renderer), cmap='gray')
+
+  plt.show()

selfdrive/navd/navd.py

@@ -0,0 +1,365 @@
+#!/usr/bin/env python3
+import json
+import math
+import os
+import threading
+
+import requests
+
+import cereal.messaging as messaging
+from cereal import log
+from openpilot.common.api import Api
+from openpilot.common.params import Params
+from openpilot.common.realtime import Ratekeeper
+from openpilot.selfdrive.navd.helpers import (Coordinate, coordinate_from_param,
+                                    distance_along_geometry, maxspeed_to_ms,
+                                    minimum_distance,
+                                    parse_banner_instructions)
+from openpilot.common.swaglog import cloudlog
+
+REROUTE_DISTANCE = 25
+MANEUVER_TRANSITION_THRESHOLD = 10
+REROUTE_COUNTER_MIN = 3
+
+
+class RouteEngine:
+  def __init__(self, sm, pm):
+    self.sm = sm
+    self.pm = pm
+
+    self.params = Params()
+
+    # Get last gps position from params
+    self.last_position = coordinate_from_param("LastGPSPosition", self.params)
+    self.last_bearing = None
+
+    self.gps_ok = False
+    self.localizer_valid = False
+
+    self.nav_destination = None
+    self.step_idx = None
+    self.route = None
+    self.route_geometry = None
+
+    self.recompute_backoff = 0
+    self.recompute_countdown = 0
+
+    self.ui_pid = None
+
+    self.reroute_counter = 0
+
+    if "MAPBOX_TOKEN" in os.environ:
+      self.mapbox_token = os.environ["MAPBOX_TOKEN"]
+      self.mapbox_host = "https://api.mapbox.com"
+    else:
+      try:
+        self.mapbox_token = Api(self.params.get("DongleId", encoding='utf8')).get_token(expiry_hours=4 * 7 * 24)
+      except FileNotFoundError:
+        cloudlog.exception("Failed to generate mapbox token due to missing private key. Ensure device is registered.")
+        self.mapbox_token = ""
+      self.mapbox_host = "https://maps.comma.ai"
+
+  def update(self):
+    self.sm.update(0)
+
+    if self.sm.updated["managerState"]:
+      ui_pid = [p.pid for p in self.sm["managerState"].processes if p.name == "ui" and p.running]
+      if ui_pid:
+        if self.ui_pid and self.ui_pid != ui_pid[0]:
+          cloudlog.warning("UI restarting, sending route")
+          threading.Timer(5.0, self.send_route).start()
+        self.ui_pid = ui_pid[0]
+
+    self.update_location()
+    try:
+      self.recompute_route()
+      self.send_instruction()
+    except Exception:
+      cloudlog.exception("navd.failed_to_compute")
+
+  def update_location(self):
+    location = self.sm['liveLocationKalman']
+    self.gps_ok = location.gpsOK
+
+    self.localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
+
+    if self.localizer_valid:
+      self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2])
+      self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1])
+
+  def recompute_route(self):
+    if self.last_position is None:
+      return
+
+    new_destination = coordinate_from_param("NavDestination", self.params)
+    if new_destination is None:
+      self.clear_route()
+      self.reset_recompute_limits()
+      return
+
+    should_recompute = self.should_recompute()
+    if new_destination != self.nav_destination:
+      cloudlog.warning(f"Got new destination from NavDestination param {new_destination}")
+      should_recompute = True
+
+    # Don't recompute when GPS drifts in tunnels
+    if not self.gps_ok and self.step_idx is not None:
+      return
+
+    if self.recompute_countdown == 0 and should_recompute:
+      self.recompute_countdown = 2**self.recompute_backoff
+      self.recompute_backoff = min(6, self.recompute_backoff + 1)
+      self.calculate_route(new_destination)
+      self.reroute_counter = 0
+    else:
+      self.recompute_countdown = max(0, self.recompute_countdown - 1)
+
+  def calculate_route(self, destination):
+    cloudlog.warning(f"Calculating route {self.last_position} -> {destination}")
+    self.nav_destination = destination
+
+    lang = self.params.get('LanguageSetting', encoding='utf8')
+    if lang is not None:
+      lang = lang.replace('main_', '')
+
+    params = {
+      'access_token': self.mapbox_token,
+      'annotations': 'maxspeed',
+      'geometries': 'geojson',
+      'overview': 'full',
+      'steps': 'true',
+      'banner_instructions': 'true',
+      'alternatives': 'false',
+      'language': lang,
+    }
+
+    # TODO: move waypoints into NavDestination param?
+    waypoints = self.params.get('NavDestinationWaypoints', encoding='utf8')
+    waypoint_coords = []
+    if waypoints is not None and len(waypoints) > 0:
+      waypoint_coords = json.loads(waypoints)
+
+    coords = [
+      (self.last_position.longitude, self.last_position.latitude),
+      *waypoint_coords,
+      (destination.longitude, destination.latitude)
+    ]
+    params['waypoints'] = f'0;{len(coords)-1}'
+    if self.last_bearing is not None:
+      params['bearings'] = f"{(self.last_bearing + 360) % 360:.0f},90" + (';'*(len(coords)-1))
+
+    coords_str = ';'.join([f'{lon},{lat}' for lon, lat in coords])
+    url = self.mapbox_host + '/directions/v5/mapbox/driving-traffic/' + coords_str
+    try:
+      resp = requests.get(url, params=params, timeout=10)
+      if resp.status_code != 200:
+        cloudlog.event("API request failed", status_code=resp.status_code, text=resp.text, error=True)
+      resp.raise_for_status()
+
+      r = resp.json()
+      if len(r['routes']):
+        self.route = r['routes'][0]['legs'][0]['steps']
+        self.route_geometry = []
+
+        maxspeed_idx = 0
+        maxspeeds = r['routes'][0]['legs'][0]['annotation']['maxspeed']
+
+        # Convert coordinates
+        for step in self.route:
+          coords = []
+
+          for c in step['geometry']['coordinates']:
+            coord = Coordinate.from_mapbox_tuple(c)
+
+            # Last step does not have maxspeed
+            if (maxspeed_idx < len(maxspeeds)):
+              maxspeed = maxspeeds[maxspeed_idx]
+              if ('unknown' not in maxspeed) and ('none' not in maxspeed):
+                coord.annotations['maxspeed'] = maxspeed_to_ms(maxspeed)
+
+            coords.append(coord)
+            maxspeed_idx += 1
+
+          self.route_geometry.append(coords)
+          maxspeed_idx -= 1  # Every segment ends with the same coordinate as the start of the next
+
+        self.step_idx = 0
+      else:
+        cloudlog.warning("Got empty route response")
+        self.clear_route()
+
+      # clear waypoints to avoid a re-route including past waypoints
+      # TODO: only clear once we're past a waypoint
+      self.params.remove('NavDestinationWaypoints')
+
+    except requests.exceptions.RequestException:
+      cloudlog.exception("failed to get route")
+      self.clear_route()
+
+    self.send_route()
+
+  def send_instruction(self):
+    msg = messaging.new_message('navInstruction', valid=True)
+
+    if self.step_idx is None:
+      msg.valid = False
+      self.pm.send('navInstruction', msg)
+      return
+
+    step = self.route[self.step_idx]
+    geometry = self.route_geometry[self.step_idx]
+    along_geometry = distance_along_geometry(geometry, self.last_position)
+    distance_to_maneuver_along_geometry = step['distance'] - along_geometry
+
+    # Banner instructions are for the following maneuver step, don't use empty last step
+    banner_step = step
+    if not len(banner_step['bannerInstructions']) and self.step_idx == len(self.route) - 1:
+      banner_step = self.route[max(self.step_idx - 1, 0)]
+
+    # Current instruction
+    msg.navInstruction.maneuverDistance = distance_to_maneuver_along_geometry
+    instruction = parse_banner_instructions(banner_step['bannerInstructions'], distance_to_maneuver_along_geometry)
+    if instruction is not None:
+      for k,v in instruction.items():
+        setattr(msg.navInstruction, k, v)
+
+    # All instructions
+    maneuvers = []
+    for i, step_i in enumerate(self.route):
+      if i < self.step_idx:
+        distance_to_maneuver = -sum(self.route[j]['distance'] for j in range(i+1, self.step_idx)) - along_geometry
+      elif i == self.step_idx:
+        distance_to_maneuver = distance_to_maneuver_along_geometry
+      else:
+        distance_to_maneuver = distance_to_maneuver_along_geometry + sum(self.route[j]['distance'] for j in range(self.step_idx+1, i+1))
+
+      instruction = parse_banner_instructions(step_i['bannerInstructions'], distance_to_maneuver)
+      if instruction is None:
+        continue
+      maneuver = {'distance': distance_to_maneuver}
+      if 'maneuverType' in instruction:
+        maneuver['type'] = instruction['maneuverType']
+      if 'maneuverModifier' in instruction:
+        maneuver['modifier'] = instruction['maneuverModifier']
+      maneuvers.append(maneuver)
+
+    msg.navInstruction.allManeuvers = maneuvers
+
+    # Compute total remaining time and distance
+    remaining = 1.0 - along_geometry / max(step['distance'], 1)
+    total_distance = step['distance'] * remaining
+    total_time = step['duration'] * remaining
+
+    if step['duration_typical'] is None:
+      total_time_typical = total_time
+    else:
+      total_time_typical = step['duration_typical'] * remaining
+
+    # Add up totals for future steps
+    for i in range(self.step_idx + 1, len(self.route)):
+      total_distance += self.route[i]['distance']
+      total_time += self.route[i]['duration']
+      if self.route[i]['duration_typical'] is None:
+        total_time_typical += self.route[i]['duration']
+      else:
+        total_time_typical += self.route[i]['duration_typical']
+
+    msg.navInstruction.distanceRemaining = total_distance
+    msg.navInstruction.timeRemaining = total_time
+    msg.navInstruction.timeRemainingTypical = total_time_typical
+
+    # Speed limit
+    closest_idx, closest = min(enumerate(geometry), key=lambda p: p[1].distance_to(self.last_position))
+    if closest_idx > 0:
+      # If we are not past the closest point, show previous
+      if along_geometry < distance_along_geometry(geometry, geometry[closest_idx]):
+        closest = geometry[closest_idx - 1]
+
+    if ('maxspeed' in closest.annotations) and self.localizer_valid:
+      msg.navInstruction.speedLimit = closest.annotations['maxspeed']
+
+    # Speed limit sign type
+    if 'speedLimitSign' in step:
+      if step['speedLimitSign'] == 'mutcd':
+        msg.navInstruction.speedLimitSign = log.NavInstruction.SpeedLimitSign.mutcd
+      elif step['speedLimitSign'] == 'vienna':
+        msg.navInstruction.speedLimitSign = log.NavInstruction.SpeedLimitSign.vienna
+
+    self.pm.send('navInstruction', msg)
+
+    # Transition to next route segment
+    if distance_to_maneuver_along_geometry < -MANEUVER_TRANSITION_THRESHOLD:
+      if self.step_idx + 1 < len(self.route):
+        self.step_idx += 1
+        self.reset_recompute_limits()
+      else:
+        cloudlog.warning("Destination reached")
+
+        # Clear route if driving away from destination
+        dist = self.nav_destination.distance_to(self.last_position)
+        if dist > REROUTE_DISTANCE:
+          self.params.remove("NavDestination")
+          self.clear_route()
+
+  def send_route(self):
+    coords = []
+
+    if self.route is not None:
+      for path in self.route_geometry:
+        coords += [c.as_dict() for c in path]
+
+    msg = messaging.new_message('navRoute', valid=True)
+    msg.navRoute.coordinates = coords
+    self.pm.send('navRoute', msg)
+
+  def clear_route(self):
+    self.route = None
+    self.route_geometry = None
+    self.step_idx = None
+    self.nav_destination = None
+
+  def reset_recompute_limits(self):
+    self.recompute_backoff = 0
+    self.recompute_countdown = 0
+
+  def should_recompute(self):
+    if self.step_idx is None or self.route is None:
+      return True
+
+    # Don't recompute in last segment, assume destination is reached
+    if self.step_idx == len(self.route) - 1:
+      return False
+
+    # Compute closest distance to all line segments in the current path
+    min_d = REROUTE_DISTANCE + 1
+    path = self.route_geometry[self.step_idx]
+    for i in range(len(path) - 1):
+      a = path[i]
+      b = path[i + 1]
+
+      if a.distance_to(b) < 1.0:
+        continue
+
+      min_d = min(min_d, minimum_distance(a, b, self.last_position))
+
+    if min_d > REROUTE_DISTANCE:
+      self.reroute_counter += 1
+    else:
+      self.reroute_counter = 0
+    return self.reroute_counter > REROUTE_COUNTER_MIN
+    # TODO: Check for going wrong way in segment
+
+
+def main():
+  pm = messaging.PubMaster(['navInstruction', 'navRoute'])
+  sm = messaging.SubMaster(['liveLocationKalman', 'managerState'])
+
+  rk = Ratekeeper(1.0)
+  route_engine = RouteEngine(sm, pm)
+  while True:
+    route_engine.update()
+    rk.keep_time()
+
+
+if __name__ == "__main__":
+  main()

selfdrive/navd/set_destination.py

@@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+import json
+import sys
+
+from openpilot.common.params import Params
+
+if __name__ == "__main__":
+  params = Params()
+
+  # set from google maps url
+  if len(sys.argv) > 1:
+    coords = sys.argv[1].split("/@")[-1].split("/")[0].split(",")
+    dest = {
+      "latitude": float(coords[0]),
+      "longitude": float(coords[1])
+    }
+    params.put("NavDestination", json.dumps(dest))
+    params.remove("NavDestinationWaypoints")
+  else:
+    print("Setting to Taco Bell")
+    dest = {
+      "latitude": 32.71160109904473,
+      "longitude": -117.12556569985693,
+    }
+    params.put("NavDestination", json.dumps(dest))
+
+    waypoints = [
+      (-117.16020713111648, 32.71997612490662),
+    ]
+    params.put("NavDestinationWaypoints", json.dumps(waypoints))
+
+    print(dest)
+    print(waypoints)

selfdrive/navd/tests/test_map_renderer.py

@@ -0,0 +1,226 @@
+#!/usr/bin/env python3
+import time
+import numpy as np
+import os
+import pytest
+import unittest
+import requests
+import threading
+import http.server
+import cereal.messaging as messaging
+
+from typing import Any
+from cereal.visionipc import VisionIpcClient, VisionStreamType
+from openpilot.selfdrive.test.helpers import with_processes
+
+LLK_DECIMATION = 10
+CACHE_PATH = "/data/mbgl-cache-navd.db"
+
+LOCATION1 = (32.7174, -117.16277)
+LOCATION2 = (32.7558, -117.2037)
+
+RENDER_FRAMES = 15
+DEFAULT_ITERATIONS = RENDER_FRAMES * LLK_DECIMATION
+
+LOCATION1_REPEATED = [LOCATION1] * DEFAULT_ITERATIONS
+LOCATION2_REPEATED = [LOCATION2] * DEFAULT_ITERATIONS
+
+def gen_llk(location=LOCATION1):
+  msg = messaging.new_message('liveLocationKalman')
+  msg.liveLocationKalman.positionGeodetic = {'value': [*location, 0], 'std': [0., 0., 0.], 'valid': True}
+  msg.liveLocationKalman.calibratedOrientationNED = {'value': [0., 0., 0.], 'std': [0., 0., 0.], 'valid': True}
+  msg.liveLocationKalman.status = 'valid'
+  return msg
+
+
+class MapBoxInternetDisabledRequestHandler(http.server.BaseHTTPRequestHandler):
+  INTERNET_ACTIVE = True
+
+  def do_GET(self):
+    if not self.INTERNET_ACTIVE:
+      self.send_response(500)
+      self.end_headers()
+      return
+
+    url = f'https://api.mapbox.com{self.path}'
+
+    headers = dict(self.headers)
+    headers["Host"] = "api.mapbox.com"
+
+    r = requests.get(url, headers=headers, timeout=5)
+
+    self.send_response(r.status_code)
+    self.end_headers()
+    self.wfile.write(r.content)
+
+  def log_message(self, *args: Any) -> None:
+    return
+
+  def log_error(self, *args: Any) -> None:
+    return
+
+
+class MapBoxInternetDisabledServer(threading.Thread):
+  def run(self):
+    self.server = http.server.HTTPServer(("127.0.0.1", 0), MapBoxInternetDisabledRequestHandler)
+    self.port = self.server.server_port
+    self.server.serve_forever()
+
+  def stop(self):
+    self.server.shutdown()
+
+  def disable_internet(self):
+    MapBoxInternetDisabledRequestHandler.INTERNET_ACTIVE = False
+
+  def enable_internet(self):
+    MapBoxInternetDisabledRequestHandler.INTERNET_ACTIVE = True
+
+
+class TestMapRenderer(unittest.TestCase):
+  server: MapBoxInternetDisabledServer
+
+  @classmethod
+  def setUpClass(cls):
+    assert "MAPBOX_TOKEN" in os.environ
+    cls.original_token = os.environ["MAPBOX_TOKEN"]
+    cls.server = MapBoxInternetDisabledServer()
+    cls.server.start()
+    time.sleep(0.5) # wait for server to startup
+
+  @classmethod
+  def tearDownClass(cls) -> None:
+    cls.server.stop()
+
+  def setUp(self):
+    self.server.enable_internet()
+    os.environ['MAPS_HOST'] = f'http://localhost:{self.server.port}'
+
+    self.sm = messaging.SubMaster(['mapRenderState'])
+    self.pm = messaging.PubMaster(['liveLocationKalman'])
+    self.vipc = VisionIpcClient("navd", VisionStreamType.VISION_STREAM_MAP, True)
+
+    if os.path.exists(CACHE_PATH):
+      os.remove(CACHE_PATH)
+
+  def _setup_test(self):
+    assert self.pm.wait_for_readers_to_update("liveLocationKalman", 10)
+
+    time.sleep(0.5)
+
+    assert VisionIpcClient.available_streams("navd", False) == {VisionStreamType.VISION_STREAM_MAP, }
+    assert self.vipc.connect(False)
+    self.vipc.recv()
+
+  def _run_test(self, expect_valid, locations=LOCATION1_REPEATED):
+    starting_frame_id = None
+
+    render_times = []
+
+    # run test
+    prev_frame_id = -1
+    for i, location in enumerate(locations):
+      frame_expected = (i+1) % LLK_DECIMATION == 0
+
+      if self.sm.logMonoTime['mapRenderState'] == 0:
+        prev_valid = False
+        prev_frame_id = -1
+      else:
+        prev_valid = self.sm.valid['mapRenderState']
+        prev_frame_id = self.sm['mapRenderState'].frameId
+
+      if starting_frame_id is None:
+        starting_frame_id = prev_frame_id
+
+      llk = gen_llk(location)
+      self.pm.send("liveLocationKalman", llk)
+      self.pm.wait_for_readers_to_update("liveLocationKalman", 10)
+      self.sm.update(1000 if frame_expected else 0)
+      assert self.sm.updated['mapRenderState'] == frame_expected, "renderer running at wrong frequency"
+
+      if not frame_expected:
+        continue
+
+      frames_since_test_start = self.sm['mapRenderState'].frameId - starting_frame_id
+
+      # give a few frames to switch from valid to invalid, or vice versa
+      invalid_and_not_previously_valid = (expect_valid and not self.sm.valid['mapRenderState'] and not prev_valid)
+      valid_and_not_previously_invalid = (not expect_valid and self.sm.valid['mapRenderState'] and prev_valid)
+
+      if (invalid_and_not_previously_valid or valid_and_not_previously_invalid) and frames_since_test_start < 5:
+        continue
+
+      # check output
+      assert self.sm.valid['mapRenderState'] == expect_valid
+      assert self.sm['mapRenderState'].frameId == (prev_frame_id + 1)
+      assert self.sm['mapRenderState'].locationMonoTime == llk.logMonoTime
+      if not expect_valid:
+        assert self.sm['mapRenderState'].renderTime == 0.
+      else:
+        assert 0. < self.sm['mapRenderState'].renderTime < 0.1
+        render_times.append(self.sm['mapRenderState'].renderTime)
+
+      # check vision ipc output
+      assert self.vipc.recv() is not None
+      assert self.vipc.valid == expect_valid
+      assert self.vipc.timestamp_sof == llk.logMonoTime
+      assert self.vipc.frame_id == self.sm['mapRenderState'].frameId
+
+    assert frames_since_test_start >= RENDER_FRAMES
+
+    return render_times
+
+  @with_processes(["mapsd"])
+  def test_with_internet(self):
+    self._setup_test()
+    self._run_test(True)
+
+  @with_processes(["mapsd"])
+  def test_with_no_internet(self):
+    self.server.disable_internet()
+    self._setup_test()
+    self._run_test(False)
+
+  @with_processes(["mapsd"])
+  def test_recover_from_no_internet(self):
+    self._setup_test()
+    self._run_test(True)
+
+    self.server.disable_internet()
+
+    # change locations to force mapsd to refetch
+    self._run_test(False, LOCATION2_REPEATED)
+
+    self.server.enable_internet()
+    self._run_test(True, LOCATION2_REPEATED)
+
+  @with_processes(["mapsd"])
+  @pytest.mark.tici
+  def test_render_time_distribution(self):
+    self._setup_test()
+    # from location1 -> location2 and back
+    locations = np.array([*np.linspace(LOCATION1, LOCATION2, 2000), *np.linspace(LOCATION2, LOCATION1, 2000)]).tolist()
+
+    render_times = self._run_test(True, locations)
+
+    _min = np.min(render_times)
+    _max = np.max(render_times)
+    _mean = np.mean(render_times)
+    _median = np.median(render_times)
+    _stddev = np.std(render_times)
+
+    print(f"Stats: min: {_min}, max: {_max}, mean: {_mean}, median: {_median}, stddev: {_stddev}, count: {len(render_times)}")
+
+    def assert_stat(stat, nominal, tol=0.3):
+      tol = (nominal / (1+tol)), (nominal * (1+tol))
+      self.assertTrue(tol[0] < stat < tol[1], f"{stat} not in tolerance {tol}")
+
+    assert_stat(_mean,   0.030)
+    assert_stat(_median, 0.027)
+    assert_stat(_stddev, 0.0078)
+
+    self.assertLess(_max, 0.065)
+    self.assertGreater(_min, 0.015)
+
+
+if __name__ == "__main__":
+  unittest.main()

selfdrive/navd/tests/test_navd.py

@@ -0,0 +1,61 @@
+#!/usr/bin/env python3
+import json
+import random
+import unittest
+import numpy as np
+
+import cereal.messaging as messaging
+from openpilot.common.params import Params
+from openpilot.selfdrive.manager.process_config import managed_processes
+
+
+class TestNavd(unittest.TestCase):
+  def setUp(self):
+    self.params = Params()
+    self.sm = messaging.SubMaster(['navRoute', 'navInstruction'])
+
+  def tearDown(self):
+    managed_processes['navd'].stop()
+
+  def _check_route(self, start, end, check_coords=True):
+    self.params.put("NavDestination", json.dumps(end))
+    self.params.put("LastGPSPosition", json.dumps(start))
+
+    managed_processes['navd'].start()
+    for _ in range(30):
+      self.sm.update(1000)
+      if all(f > 0 for f in self.sm.rcv_frame.values()):
+        break
+    else:
+      raise Exception("didn't get a route")
+
+    assert managed_processes['navd'].proc.is_alive()
+    managed_processes['navd'].stop()
+
+    # ensure start and end match up
+    if check_coords:
+      coords = self.sm['navRoute'].coordinates
+      assert np.allclose([start['latitude'], start['longitude'], end['latitude'], end['longitude']],
+                         [coords[0].latitude, coords[0].longitude, coords[-1].latitude, coords[-1].longitude],
+                         rtol=1e-3)
+
+  def test_simple(self):
+    start = {
+      "latitude": 32.7427228,
+      "longitude": -117.2321177,
+    }
+    end = {
+      "latitude": 32.7557004,
+      "longitude": -117.268002,
+    }
+    self._check_route(start, end)
+
+  def test_random(self):
+    for _ in range(10):
+      start = {"latitude": random.uniform(-90, 90), "longitude": random.uniform(-180, 180)}
+      end = {"latitude": random.uniform(-90, 90), "longitude": random.uniform(-180, 180)}
+      self._check_route(start, end, check_coords=False)
+
+
+if __name__ == "__main__":
+  unittest.main()