navd: add back VisionIPC map renderer (#25212)

* builds standalone

* process live msg

* render into nv12a

* VISION_STREAM_RGB_MAP -> VISION_STREAM_MAP

* cleanup sconscript

* main include cleanup
old-commit-hash: 86c1e8164a

SConstruct

@@ -415,6 +415,7 @@ SConscript(['selfdrive/loggerd/SConscript'])
 SConscript(['selfdrive/locationd/SConscript'])
 SConscript(['selfdrive/sensord/SConscript'])
 SConscript(['selfdrive/ui/SConscript'])
+SConscript(['selfdrive/navd/SConscript'])
 
 SConscript(['tools/replay/SConscript'])
 

release/files_common

@@ -380,7 +380,9 @@ selfdrive/modeld/runners/run.h
 selfdrive/monitoring/dmonitoringd.py
 selfdrive/monitoring/driver_monitor.py
 
-selfdrive/navd/*.py
+selfdrive/navd/__init__.py
+selfdrive/navd/navd.py
+selfdrive/navd/helpers.py
 
 selfdrive/assets/.gitignore
 selfdrive/assets/assets.qrc

selfdrive/navd/.gitignore

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

selfdrive/navd/SConscript

@@ -0,0 +1,20 @@
+Import('qt_env', 'arch', 'common', 'messaging', 'visionipc', 'cereal', 'transformations')
+
+base_libs = [common, messaging, cereal, visionipc, transformations, 'zmq',
+             'capnp', 'kj', 'm', 'OpenCL', 'ssl', 'crypto', 'pthread'] + qt_env["LIBS"]
+
+if arch == 'larch64':
+  base_libs.append('EGL')
+
+if arch in ['larch64', 'x86_64']:
+  if arch == 'x86_64':
+    rpath = [Dir(f"#third_party/mapbox-gl-native-qt/{arch}").srcnode().abspath]
+    qt_env["RPATH"] += rpath
+
+  qt_libs = ["qt_widgets", "qt_util", "qmapboxgl"] + base_libs
+
+  nav_src = ["main.cc", "map_renderer.cc"]
+  qt_env.Program("map_renderer", nav_src, LIBS=qt_libs + ['common', 'json11'])
+
+  if GetOption('extras'):
+    qt_env.SharedLibrary("map_renderer", ["map_renderer.cc"], LIBS=qt_libs + ['common', 'messaging'])

selfdrive/navd/main.cc

@@ -0,0 +1,31 @@
+#include <QApplication>
+#include <QDebug>
+#include <csignal>
+
+#include "selfdrive/ui/qt/util.h"
+#include "selfdrive/ui/qt/maps/map_helpers.h"
+#include "selfdrive/navd/map_renderer.h"
+#include "selfdrive/hardware/hw.h"
+
+
+
+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);
+
+  MapRenderer * m = new MapRenderer(get_mapbox_settings());
+  assert(m);
+
+  return app.exec();
+}

selfdrive/navd/map_renderer.cc

@@ -0,0 +1,236 @@
+#include "selfdrive/navd/map_renderer.h"
+
+#include <QApplication>
+#include <QBuffer>
+#include <QDebug>
+
+#include "common/timing.h"
+#include "selfdrive/ui/qt/maps/map_helpers.h"
+
+const float ZOOM = 13.5; // Don't go below 13 or features will start to disappear
+const int WIDTH = 256;
+const int HEIGHT = WIDTH;
+
+const int NUM_VIPC_BUFFERS = 4;
+
+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));
+
+  m_map.reset(new QMapboxGL(nullptr, m_settings, fbo->size(), 1));
+  m_map->setCoordinateZoom(QMapbox::Coordinate(0, 0), ZOOM);
+  m_map->setStyleUrl("mapbox://styles/commaai/ckvmksrpd4n0a14pfdo5heqzr");
+  m_map->createRenderer();
+
+  m_map->resize(fbo->size());
+  m_map->setFramebufferObject(fbo->handle(), fbo->size());
+  gl_functions->glViewport(0, 0, WIDTH, HEIGHT);
+
+  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"}));
+    sm.reset(new SubMaster({"liveLocationKalman", "navRoute"}));
+
+    timer = new QTimer(this);
+    QObject::connect(timer, SIGNAL(timeout()), this, SLOT(msgUpdate()));
+    timer->start(50);
+  }
+}
+
+void MapRenderer::msgUpdate() {
+  sm->update(0);
+
+  if (sm->updated("liveLocationKalman")) {
+    auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman();
+    auto pos = location.getPositionGeodetic();
+    auto orientation = location.getCalibratedOrientationNED();
+
+    bool localizer_valid = (location.getStatus() == cereal::LiveLocationKalman::Status::VALID) && pos.getValid();
+    if (localizer_valid) {
+      updatePosition(QMapbox::Coordinate(pos.getValue()[0], pos.getValue()[1]), RAD2DEG(orientation.getValue()[2]));
+    }
+  }
+
+  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);
+  }
+}
+
+void MapRenderer::updatePosition(QMapbox::Coordinate position, float bearing) {
+  if (m_map.isNull()) {
+    return;
+  }
+
+  m_map->setCoordinate(position);
+  m_map->setBearing(bearing);
+  update();
+}
+
+bool MapRenderer::loaded() {
+  return m_map->isFullyLoaded();
+}
+
+void MapRenderer::update() {
+  gl_functions->glClear(GL_COLOR_BUFFER_BIT);
+  m_map->render();
+  gl_functions->glFlush();
+
+  sendVipc();
+}
+
+void MapRenderer::sendVipc() {
+  if (!vipc_server || !loaded()) {
+    return;
+  }
+
+  QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor);
+  uint64_t ts = nanos_since_boot();
+  VisionBuf* buf = vipc_server->get_buffer(VisionStreamType::VISION_STREAM_MAP);
+  VisionIpcBufExtra extra = {
+    .frame_id = frame_id,
+    .timestamp_sof = ts,
+    .timestamp_eof = ts,
+  };
+
+  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);
+
+  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());
+
+    // Send thumbnail
+    MessageBuilder msg;
+    auto thumbnaild = msg.initEvent().initNavThumbnail();
+    thumbnaild.setFrameId(frame_id);
+    thumbnaild.setTimestampEof(ts);
+    thumbnaild.setThumbnail(buffer_kj);
+    pm->send("navThumbnail", msg);
+  }
+
+  frame_id++;
+}
+
+uint8_t* MapRenderer::getImage() {
+  QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor);
+  uint8_t* buf = new uint8_t[cap.sizeInBytes()];
+  memcpy(buf, cap.bits(), cap.sizeInBytes());
+
+  return buf;
+}
+
+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")) {
+    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", 3);
+    m_map->setLayoutProperty("navLayer", "line-cap", "round");
+  }
+}
+
+MapRenderer::~MapRenderer() {
+}
+
+extern "C" {
+  MapRenderer* map_renderer_init() {
+    char *argv[] = {
+      (char*)"navd",
+      nullptr
+    };
+    int argc = 0;
+    QApplication *app = new QApplication(argc, argv);
+    assert(app);
+
+    QMapboxGLSettings settings;
+    settings.setApiBaseUrl(MAPS_HOST);
+    settings.setAccessToken(get_mapbox_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,53 @@
+#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 sendVipc();
+
+  QMapboxGLSettings m_settings;
+  QScopedPointer<QMapboxGL> m_map;
+
+  void initLayers();
+
+  uint32_t frame_id = 0;
+
+  QTimer* timer;
+
+public slots:
+  void updatePosition(QMapbox::Coordinate position, float bearing);
+  void updateRoute(QList<QGeoCoordinate> coordinates);
+  void msgUpdate();
+};

selfdrive/navd/map_renderer.py

@@ -0,0 +1,78 @@
+#!/usr/bin/env python3
+# You might need to uninstall the PyQt5 pip package to avoid conflicts
+
+import os
+import time
+from cffi import FFI
+
+from common.ffi_wrapper import suffix
+from common.basedir import BASEDIR
+
+HEIGHT = WIDTH = 256
+
+
+def get_ffi():
+  lib = os.path.join(BASEDIR, "selfdrive", "navd", "libmap_renderer" + suffix())
+
+  ffi = FFI()
+  ffi.cdef("""
+void* map_renderer_init();
+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:3 * WIDTH * HEIGHT])
+  lib.map_renderer_free_image(renderer, buf)
+
+  # Convert to numpy
+  r = np.asarray(r)
+  return r.reshape((WIDTH, HEIGHT, 3))
+
+
+if __name__ == "__main__":
+  import matplotlib.pyplot as plt
+  import numpy as np
+
+  ffi, lib = get_ffi()
+  renderer = lib.map_renderer_init()
+  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"
+  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))
+
+  plt.show()

selfdrive/ui/qt/maps/map_helpers.cc

@@ -101,6 +101,48 @@ QMapbox::CoordinatesCollections coordinate_list_to_collection(QList<QGeoCoordina
   return collections;
 }
 
+QList<QGeoCoordinate> polyline_to_coordinate_list(const QString &polylineString) {
+  QList<QGeoCoordinate> path;
+  if (polylineString.isEmpty())
+      return path;
+
+  QByteArray data = polylineString.toLatin1();
+
+  bool parsingLatitude = true;
+
+  int shift = 0;
+  int value = 0;
+
+  QGeoCoordinate coord(0, 0);
+
+  for (int i = 0; i < data.length(); ++i) {
+      unsigned char c = data.at(i) - 63;
+
+      value |= (c & 0x1f) << shift;
+      shift += 5;
+
+      // another chunk
+      if (c & 0x20)
+          continue;
+
+      int diff = (value & 1) ? ~(value >> 1) : (value >> 1);
+
+      if (parsingLatitude) {
+          coord.setLatitude(coord.latitude() + (double)diff/1e6);
+      } else {
+          coord.setLongitude(coord.longitude() + (double)diff/1e6);
+          path.append(coord);
+      }
+
+      parsingLatitude = !parsingLatitude;
+
+      value = 0;
+      shift = 0;
+  }
+
+  return path;
+}
+
 std::optional<QMapbox::Coordinate> coordinate_from_param(std::string param) {
   QString json_str = QString::fromStdString(Params().get(param));
   if (json_str.isEmpty()) return {};

selfdrive/ui/qt/maps/map_helpers.h

@@ -24,6 +24,7 @@ QMapbox::CoordinatesCollections model_to_collection(
 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);
+QList<QGeoCoordinate> polyline_to_coordinate_list(const QString &polylineString);
 
 std::optional<QMapbox::Coordinate> coordinate_from_param(std::string param);
 double angle_difference(double angle1, double angle2);

selfdrive/ui/watch3.cc

@@ -19,6 +19,7 @@ int main(int argc, char *argv[]) {
   {
     QHBoxLayout *hlayout = new QHBoxLayout();
     layout->addLayout(hlayout);
+    hlayout->addWidget(new CameraViewWidget("navd", VISION_STREAM_MAP, false));
     hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_ROAD, false));
   }