diff --git a/SConstruct b/SConstruct index 1209894ba4..940b2367fa 100644 --- a/SConstruct +++ b/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']) diff --git a/release/files_common b/release/files_common index 38f86d247a..411e4ff6dc 100644 --- a/release/files_common +++ b/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 diff --git a/selfdrive/navd/.gitignore b/selfdrive/navd/.gitignore new file mode 100644 index 0000000000..a070fe32bb --- /dev/null +++ b/selfdrive/navd/.gitignore @@ -0,0 +1,5 @@ +moc_* +*.moc + +map_renderer +libmap_renderer.so diff --git a/selfdrive/navd/SConscript b/selfdrive/navd/SConscript new file mode 100644 index 0000000000..4fbe41e80b --- /dev/null +++ b/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']) diff --git a/selfdrive/navd/main.cc b/selfdrive/navd/main.cc new file mode 100644 index 0000000000..3a2fedb7d2 --- /dev/null +++ b/selfdrive/navd/main.cc @@ -0,0 +1,31 @@ +#include +#include +#include + +#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(); +} diff --git a/selfdrive/navd/map_renderer.cc b/selfdrive/navd/map_renderer.cc new file mode 100644 index 0000000000..cb24f2087e --- /dev/null +++ b/selfdrive/navd/map_renderer.cc @@ -0,0 +1,236 @@ +#include "selfdrive/navd/map_renderer.h" + +#include +#include +#include + +#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(); + ctx->setFormat(fmt); + ctx->create(); + assert(ctx->isValid()); + + surface = std::make_unique(); + 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 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 buffer_kj = kj::heapArray((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 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(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; + } +} diff --git a/selfdrive/navd/map_renderer.h b/selfdrive/navd/map_renderer.h new file mode 100644 index 0000000000..855dc91894 --- /dev/null +++ b/selfdrive/navd/map_renderer.h @@ -0,0 +1,53 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#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 ctx; + std::unique_ptr surface; + std::unique_ptr gl_functions; + std::unique_ptr fbo; + + std::unique_ptr vipc_server; + std::unique_ptr pm; + std::unique_ptr sm; + void sendVipc(); + + QMapboxGLSettings m_settings; + QScopedPointer m_map; + + void initLayers(); + + uint32_t frame_id = 0; + + QTimer* timer; + +public slots: + void updatePosition(QMapbox::Coordinate position, float bearing); + void updateRoute(QList coordinates); + void msgUpdate(); +}; diff --git a/selfdrive/navd/map_renderer.py b/selfdrive/navd/map_renderer.py new file mode 100755 index 0000000000..dc39f335c7 --- /dev/null +++ b/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() diff --git a/selfdrive/ui/qt/maps/map_helpers.cc b/selfdrive/ui/qt/maps/map_helpers.cc index 66acb7a25d..f97137a7f9 100644 --- a/selfdrive/ui/qt/maps/map_helpers.cc +++ b/selfdrive/ui/qt/maps/map_helpers.cc @@ -101,6 +101,48 @@ QMapbox::CoordinatesCollections coordinate_list_to_collection(QList polyline_to_coordinate_list(const QString &polylineString) { + QList 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 coordinate_from_param(std::string param) { QString json_str = QString::fromStdString(Params().get(param)); if (json_str.isEmpty()) return {}; diff --git a/selfdrive/ui/qt/maps/map_helpers.h b/selfdrive/ui/qt/maps/map_helpers.h index 1c08c541c3..2e1402ccea 100644 --- a/selfdrive/ui/qt/maps/map_helpers.h +++ b/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::Reader &coordinate_list); QMapbox::CoordinatesCollections coordinate_list_to_collection(QList coordinate_list); +QList polyline_to_coordinate_list(const QString &polylineString); std::optional coordinate_from_param(std::string param); double angle_difference(double angle1, double angle2); diff --git a/selfdrive/ui/watch3.cc b/selfdrive/ui/watch3.cc index 00d23ea976..d6b5cc67a7 100644 --- a/selfdrive/ui/watch3.cc +++ b/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)); }