wip

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', 'QMapLibre', 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/maplibre-native-qt/{arch}/lib").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

@@ -2,7 +2,7 @@ from __future__ import annotations
 
 import json
 import math
-from typing import Any, Dict, List, Optional, Tuple, Union, cast
+from typing import Any, cast
 
 from openpilot.common.conversions import Conversions
 from openpilot.common.numpy_fast import clip
@@ -22,13 +22,13 @@ class Coordinate:
   def __init__(self, latitude: float, longitude: float) -> None:
     self.latitude = latitude
     self.longitude = longitude
-    self.annotations: Dict[str, float] = {}
+    self.annotations: dict[str, float] = {}
 
   @classmethod
-  def from_mapbox_tuple(cls, t: Tuple[float, float]) -> Coordinate:
+  def from_mapbox_tuple(cls, t: tuple[float, float]) -> Coordinate:
     return cls(t[1], t[0])
 
-  def as_dict(self) -> Dict[str, float]:
+  def as_dict(self) -> dict[str, float]:
     return {'latitude': self.latitude, 'longitude': self.longitude}
 
   def __str__(self) -> str:
@@ -83,7 +83,7 @@ def minimum_distance(a: Coordinate, b: Coordinate, p: Coordinate):
   return projection.distance_to(p)
 
 
-def distance_along_geometry(geometry: List[Coordinate], pos: Coordinate) -> float:
+def distance_along_geometry(geometry: list[Coordinate], pos: Coordinate) -> float:
   if len(geometry) <= 2:
     return geometry[0].distance_to(pos)
 
@@ -106,7 +106,7 @@ def distance_along_geometry(geometry: List[Coordinate], pos: Coordinate) -> floa
   return total_distance_closest
 
 
-def coordinate_from_param(param: str, params: Optional[Params] = None) -> Optional[Coordinate]:
+def coordinate_from_param(param: str, params: Params = None) -> Coordinate | None:
   if params is None:
     params = Params()
 
@@ -130,7 +130,7 @@ def string_to_direction(direction: str) -> str:
   return 'none'
 
 
-def maxspeed_to_ms(maxspeed: Dict[str, Union[str, float]]) -> float:
+def maxspeed_to_ms(maxspeed: dict[str, str | float]) -> float:
   unit = cast(str, maxspeed['unit'])
   speed = cast(float, maxspeed['speed'])
   return SPEED_CONVERSIONS[unit] * speed
@@ -140,7 +140,7 @@ 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]]:
+def parse_banner_instructions(banners: Any, distance_to_maneuver: float = 0.0) -> dict[str, Any] | None:
   if not len(banners):
     return None
 

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;
+}
+
+QMapLibre::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 QMapLibre::Coordinate(RAD2DEG(lat2), RAD2DEG(lon2));
+}
+
+
+MapRenderer::MapRenderer(const QMapLibre::Settings &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 QMapLibre::Map(nullptr, m_settings, fbo->size(), 1));
+  m_map->setCoordinateZoom(QMapLibre::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(), &QMapLibre::Map::mapChanged, [=](QMapLibre::Map::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 != QMapLibre::Map::MapChange::MapChangeRegionWillChange &&
+          change != QMapLibre::Map::MapChange::MapChangeRegionDidChange &&
+          change != QMapLibre::Map::MapChange::MapChangeWillStartRenderingFrame &&
+          change != QMapLibre::Map::MapChange::MapChangeDidFinishRenderingFrameFullyRendered) {
+        LOGD("New map state: %d", change);
+      }
+    }
+  });
+
+  QObject::connect(m_map.data(), &QMapLibre::Map::mapLoadingFailed, [=](QMapLibre::Map::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(QMapLibre::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);
+  QMapLibre::Feature feature(QMapLibre::Feature::LineStringType, route_points, {}, {});
+  QVariantMap navSource;
+  navSource["type"] = "geojson";
+  navSource["data"] = QVariant::fromValue<QMapLibre::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["type"] = "line";
+    nav["source"] = "navSource";
+    m_map->addLayer("navLayer", 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);
+
+    QMapLibre::Settings settings;
+    settings.setProviderTemplate(QMapLibre::Settings::ProviderTemplate::MapboxProvider);
+    settings.setApiBaseUrl(maps_host == nullptr ? MAPS_HOST : maps_host);
+    settings.setApiKey(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,59 @@
+#pragma once
+
+#include <memory>
+
+#include <QOpenGLContext>
+#include <QMapLibre/Map>
+#include <QMapLibre/Settings>
+#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 QMapLibre::Settings &, 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);
+
+  QMapLibre::Settings m_settings;
+  QScopedPointer<QMapLibre::Map> 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(QMapLibre::Coordinate position, float bearing);
+  void updateRoute(QList<QGeoCoordinate> coordinates);
+  void msgUpdate();
+};

selfdrive/navd/navd.py

@@ -18,8 +18,6 @@ from openpilot.selfdrive.navd.helpers import (Coordinate, coordinate_from_param,
                                     parse_banner_instructions)
 from openpilot.common.swaglog import cloudlog
 
-from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
-
 REROUTE_DISTANCE = 25
 MANEUVER_TRANSITION_THRESHOLD = 10
 REROUTE_COUNTER_MIN = 3
@@ -55,7 +53,6 @@ class RouteEngine:
     if "MAPBOX_TOKEN" in os.environ:
       self.mapbox_token = os.environ["MAPBOX_TOKEN"]
       self.mapbox_host = "https://api.mapbox.com"
-      self.params.put("MapboxPublicKey", self.mapbox_token)
     elif self.params.get_int("PrimeType") == 0:
       self.mapbox_token = self.params.get("MapboxPublicKey", encoding='utf8')
       self.mapbox_host = "https://api.mapbox.com"
@@ -77,10 +74,6 @@ class RouteEngine:
     self.update_frogpilot_params()
 
   def update(self):
-    # Update FrogPilot parameters
-    if self.params_memory.get_bool("FrogPilotTogglesUpdated"):
-      self.update_frogpilot_params()
-
     self.sm.update(0)
 
     if self.sm.updated["managerState"]:
@@ -98,6 +91,10 @@ class RouteEngine:
     except Exception:
       cloudlog.exception("navd.failed_to_compute")
 
+    # Update FrogPilot parameters
+    if self.params_memory.get_bool("FrogPilotTogglesUpdated"):
+      self.update_frogpilot_params()
+
   def update_location(self):
     location = self.sm['liveLocationKalman']
     self.gps_ok = location.gpsOK
@@ -179,6 +176,7 @@ class RouteEngine:
 
       r = resp.json()
       r1 = resp.json()
+
       # Function to remove specified keys recursively unnessary for display
       def remove_keys(obj, keys_to_remove):
         if isinstance(obj, list):
@@ -187,13 +185,16 @@ class RouteEngine:
           return {key: remove_keys(value, keys_to_remove) for key, value in obj.items() if key not in keys_to_remove}
         else:
           return obj
+
       keys_to_remove = ['geometry', 'annotation', 'incidents', 'intersections', 'components', 'sub', 'waypoints']
       self.r2 = remove_keys(r1, keys_to_remove)
       self.r3 = {}
+
       # Add items for display under "routes"
       if 'routes' in self.r2 and len(self.r2['routes']) > 0:
         first_route = self.r2['routes'][0]
         nav_destination_json = self.params.get('NavDestination')
+
         try:
           nav_destination_data = json.loads(nav_destination_json)
           place_name = nav_destination_data.get('place_name', 'Default Place Name')
@@ -203,7 +204,8 @@ class RouteEngine:
           self.r3['uuid'] = self.r2['uuid']
         except json.JSONDecodeError as e:
           print(f"Error decoding JSON: {e}")
-	  # Save slim json as file
+
+      # Save slim json as file
       with open('navdirections.json', 'w') as json_file:
         json.dump(self.r2, json_file, indent=4)
       with open('CurrentStep.json', 'w') as json_file:
@@ -217,6 +219,7 @@ class RouteEngine:
         if self.conditional_navigation_intersections:
           self.stop_signal = []
           self.stop_coord = []
+
           for step in self.route:
             for intersection in step["intersections"]:
               if "stop_sign" in intersection or "traffic_signal" in intersection:
@@ -265,11 +268,7 @@ class RouteEngine:
 
     if self.step_idx is None:
       msg.valid = False
-      SpeedLimitController.nav_speed_limit = 0
-      SpeedLimitController.write_nav_state()
-
-      if SpeedLimitController.desired_speed_limit != 0:
-        msg.navInstruction.speedLimit = SpeedLimitController.desired_speed_limit
+      self.params_memory.put_float("NavSpeedLimit", 0)
       self.pm.send('navInstruction', msg)
       return
 
@@ -344,14 +343,9 @@ class RouteEngine:
 
     if ('maxspeed' in closest.annotations) and self.localizer_valid:
       msg.navInstruction.speedLimit = closest.annotations['maxspeed']
-      SpeedLimitController.nav_speed_limit = closest.annotations['maxspeed']
-      SpeedLimitController.write_nav_state()
+      self.params_memory.put_float("NavSpeedLimit", closest.annotations['maxspeed'])
     if not self.localizer_valid or ('maxspeed' not in closest.annotations):
-      SpeedLimitController.nav_speed_limit = 0
-      SpeedLimitController.write_nav_state()
-
-    if SpeedLimitController.desired_speed_limit != 0:
-      msg.navInstruction.speedLimit = SpeedLimitController.desired_speed_limit
+      self.params_memory.put_float("NavSpeedLimit", 0)
 
     # Speed limit sign type
     if 'speedLimitSign' in step:
@@ -367,6 +361,7 @@ class RouteEngine:
       if self.step_idx + 1 < len(self.route):
         self.step_idx += 1
         self.reset_recompute_limits()
+
         # Update the 'CurrentStep' value in the JSON
         if 'routes' in self.r2 and len(self.r2['routes']) > 0:
           self.r3['CurrentStep'] = self.step_idx
@@ -382,10 +377,11 @@ class RouteEngine:
           self.params.remove("NavDestination")
           self.clear_route()
 
-    # 5-10 Seconds to stop condition based on v_ego or minimum of 25 meters
+    # 5-10 Seconds to stop condition based on the current speed or minimum of 25 meters
     if self.conditional_navigation:
       v_ego = self.sm['carState'].vEgo
       seconds_to_stop = interp(v_ego, [0, 22.3, 44.7], [5, 10, 10])
+
       # Determine the location of the closest upcoming stopSign or trafficLight
       closest_condition_indices = [idx for idx in self.stop_signal if idx >= closest_idx]
       if closest_condition_indices:

selfdrive/navd/tests/test_map_renderer.py

@@ -0,0 +1,216 @@
+#!/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.common.mock.generators import LLK_DECIMATION, LOCATION1, LOCATION2, generate_liveLocationKalman
+from openpilot.selfdrive.test.helpers import with_processes
+
+CACHE_PATH = "/data/mbgl-cache-navd.db"
+
+RENDER_FRAMES = 15
+DEFAULT_ITERATIONS = RENDER_FRAMES * LLK_DECIMATION
+LOCATION1_REPEATED = [LOCATION1] * DEFAULT_ITERATIONS
+LOCATION2_REPEATED = [LOCATION2] * DEFAULT_ITERATIONS
+
+
+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 = generate_liveLocationKalman(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"])
+  @pytest.mark.skip(reason="slow, flaky, and unlikely to break")
+  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,63 @@
+#!/usr/bin/env python3
+import json
+import random
+import unittest
+import numpy as np
+
+from parameterized import parameterized
+
+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.recv_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)
+
+  @parameterized.expand([(i,) for i in range(10)])
+  def test_random(self, index):
+    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()