From 5069852573128f40dd0174eb1d0f3f983367863d Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Fri, 26 Nov 2021 14:38:02 +0100 Subject: [PATCH] navd: render map into VisionIPC (#22800) * navd: render simple map * render route * offscreen rendering * cleanup * more cleanup * render into visionIPC * rename class * split position update from route update * stop broadcast if not active * gate vipc server behind flag * add python library * faster * no vipc from python * put behind extras * only send when loaded * add glFlush just to be sure * cleanup settings into helper function * function ordering * broadcast thumbnails * put behind param * adjust zoom level * add route to python bindings * revert that freq change * add logging if map rendering is enabled * use rlogs if available * bump cereal --- cereal | 2 +- .../test/get_thumbnails_for_segment.py | 6 +- selfdrive/common/params.cc | 1 + selfdrive/ui/SConscript | 5 +- selfdrive/ui/navd/main.cc | 15 +- selfdrive/ui/navd/map_renderer.cc | 200 ++++++++++++++++++ selfdrive/ui/navd/map_renderer.h | 49 +++++ selfdrive/ui/navd/map_renderer.py | 78 +++++++ selfdrive/ui/navd/route_engine.cc | 36 +++- selfdrive/ui/navd/route_engine.h | 11 +- selfdrive/ui/qt/maps/map.h | 3 - selfdrive/ui/qt/maps/map_helpers.cc | 60 ++++++ selfdrive/ui/qt/maps/map_helpers.h | 8 +- selfdrive/ui/qt/offroad/driverview.cc | 2 +- selfdrive/ui/qt/onroad.cc | 16 +- selfdrive/ui/qt/onroad.h | 2 +- selfdrive/ui/qt/widgets/cameraview.cc | 6 +- selfdrive/ui/qt/widgets/cameraview.h | 3 +- selfdrive/ui/watch3.cc | 18 +- 19 files changed, 476 insertions(+), 45 deletions(-) create mode 100644 selfdrive/ui/navd/map_renderer.cc create mode 100644 selfdrive/ui/navd/map_renderer.h create mode 100755 selfdrive/ui/navd/map_renderer.py diff --git a/cereal b/cereal index 238e0c579..9b245b602 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 238e0c5793daed7dce6e1e072c4ae510f06ea515 +Subproject commit 9b245b60238b54867057c591b9ac2948b81a1794 diff --git a/selfdrive/camerad/test/get_thumbnails_for_segment.py b/selfdrive/camerad/test/get_thumbnails_for_segment.py index 2ffb4bf65..ab9a7b308 100755 --- a/selfdrive/camerad/test/get_thumbnails_for_segment.py +++ b/selfdrive/camerad/test/get_thumbnails_for_segment.py @@ -20,9 +20,13 @@ if __name__ == "__main__": mkdirs_exists_ok(out_path) r = Route(args.route) - lr = list(LogReader(r.qlog_paths()[args.segment])) + path = r.log_paths()[args.segment] or r.qlog_paths()[args.segment] + lr = list(LogReader(path)) for msg in tqdm(lr): if msg.which() == 'thumbnail': with open(os.path.join(out_path, f"{msg.thumbnail.frameId}.jpg"), 'wb') as f: f.write(msg.thumbnail.thumbnail) + elif msg.which() == 'navThumbnail': + with open(os.path.join(out_path, f"nav_{msg.navThumbnail.frameId}.jpg"), 'wb') as f: + f.write(msg.navThumbnail.thumbnail) diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc index 01bf9a8f4..ebb37344e 100644 --- a/selfdrive/common/params.cc +++ b/selfdrive/common/params.cc @@ -135,6 +135,7 @@ std::unordered_map keys = { {"LiveParameters", PERSISTENT}, {"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, {"NavSettingTime24h", PERSISTENT}, + {"NavdRender", PERSISTENT}, {"OpenpilotEnabledToggle", PERSISTENT}, {"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, {"Passive", PERSISTENT}, diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index bc21022dd..cb1b92994 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -126,5 +126,8 @@ if arch in ['x86_64', 'Darwin'] or GetOption('extras'): # navd if maps: - navd_src = ["navd/main.cc", "navd/route_engine.cc"] + navd_src = ["navd/main.cc", "navd/route_engine.cc", "navd/map_renderer.cc"] qt_env.Program("navd/_navd", navd_src, LIBS=qt_libs + ['common', 'json11']) + + if GetOption('extras'): + qt_env.SharedLibrary("navd/map_renderer", ["navd/map_renderer.cc"], LIBS=qt_libs + ['common', 'messaging']) diff --git a/selfdrive/ui/navd/main.cc b/selfdrive/ui/navd/main.cc index cc8d9c339..7ae0393a4 100644 --- a/selfdrive/ui/navd/main.cc +++ b/selfdrive/ui/navd/main.cc @@ -5,9 +5,11 @@ #include #include "selfdrive/ui/qt/util.h" +#include "selfdrive/ui/qt/maps/map_helpers.h" #include "selfdrive/ui/navd/route_engine.h" - -RouteEngine* route_engine = nullptr; +#include "selfdrive/ui/navd/map_renderer.h" +#include "selfdrive/hardware/hw.h" +#include "selfdrive/common/params.h" void sigHandler(int s) { qInfo() << "Shutting down"; @@ -30,7 +32,14 @@ int main(int argc, char *argv[]) { parser.process(app); const QStringList args = parser.positionalArguments(); - route_engine = new RouteEngine(); + + RouteEngine* route_engine = new RouteEngine(); + + if (Params().getBool("NavdRender")) { + MapRenderer * m = new MapRenderer(get_mapbox_settings()); + QObject::connect(route_engine, &RouteEngine::positionUpdated, m, &MapRenderer::updatePosition); + QObject::connect(route_engine, &RouteEngine::routeUpdated, m, &MapRenderer::updateRoute); + } return app.exec(); } diff --git a/selfdrive/ui/navd/map_renderer.cc b/selfdrive/ui/navd/map_renderer.cc new file mode 100644 index 000000000..1d78d68df --- /dev/null +++ b/selfdrive/ui/navd/map_renderer.cc @@ -0,0 +1,200 @@ +#include "selfdrive/ui/navd/map_renderer.h" + +#include +#include +#include + +#include "selfdrive/ui/qt/maps/map_helpers.h" +#include "selfdrive/common/timing.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 enable_vipc) : 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 (enable_vipc) { + qWarning() << "Enabling navd map rendering"; + vipc_server.reset(new VisionIpcServer("navd")); + vipc_server->create_buffers(VisionStreamType::VISION_STREAM_RGB_MAP, NUM_VIPC_BUFFERS, true, WIDTH, HEIGHT); + vipc_server->start_listener(); + + pm.reset(new PubMaster({"navThumbnail"})); + } +} + +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_RGB_MAP); + VisionIpcBufExtra extra = { + .frame_id = frame_id, + .timestamp_sof = ts, + .timestamp_eof = ts, + }; + + assert(cap.sizeInBytes() == buf->len); + memcpy(buf->addr, cap.bits(), buf->len); + 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("blue")); + 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/ui/navd/map_renderer.h b/selfdrive/ui/navd/map_renderer.h new file mode 100644 index 000000000..1746e7669 --- /dev/null +++ b/selfdrive/ui/navd/map_renderer.h @@ -0,0 +1,49 @@ +#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 enable_vipc=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; + void sendVipc(); + + QMapboxGLSettings m_settings; + QScopedPointer m_map; + + void initLayers(); + + uint32_t frame_id = 0; + +public slots: + void updatePosition(QMapbox::Coordinate position, float bearing); + void updateRoute(QList coordinates); +}; diff --git a/selfdrive/ui/navd/map_renderer.py b/selfdrive/ui/navd/map_renderer.py new file mode 100755 index 000000000..28d006841 --- /dev/null +++ b/selfdrive/ui/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", "ui", "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/navd/route_engine.cc b/selfdrive/ui/navd/route_engine.cc index 30d070dc7..f3cb7df3d 100644 --- a/selfdrive/ui/navd/route_engine.cc +++ b/selfdrive/ui/navd/route_engine.cc @@ -91,7 +91,6 @@ static void parse_banner(cereal::NavInstruction::Builder &instruction, const QMa } } } - } RouteEngine::RouteEngine() { @@ -99,14 +98,17 @@ RouteEngine::RouteEngine() { pm = new PubMaster({"navInstruction", "navRoute"}); // Timers - timer = new QTimer(this); - QObject::connect(timer, SIGNAL(timeout()), this, SLOT(timerUpdate())); - timer->start(1000); + route_timer = new QTimer(this); + QObject::connect(route_timer, SIGNAL(timeout()), this, SLOT(routeUpdate())); + route_timer->start(1000); + + msg_timer = new QTimer(this); + QObject::connect(msg_timer, SIGNAL(timeout()), this, SLOT(msgUpdate())); + msg_timer->start(50); // Build routing engine QVariantMap parameters; - QString token = MAPBOX_TOKEN.isEmpty() ? CommaApi::create_jwt({}, 4 * 7 * 24 * 3600) : MAPBOX_TOKEN; - parameters["mapbox.access_token"] = token; + parameters["mapbox.access_token"] = get_mapbox_token(); parameters["mapbox.directions_api_url"] = MAPS_HOST + "/directions/v5/mapbox/"; geoservice_provider = new QGeoServiceProvider("mapbox", parameters); @@ -124,12 +126,14 @@ RouteEngine::RouteEngine() { } } -void RouteEngine::timerUpdate() { - sm->update(0); +void RouteEngine::msgUpdate() { + sm->update(1000); if (!sm->updated("liveLocationKalman")) { + active = false; return; } + if (sm->updated("managerState")) { for (auto const &p : (*sm)["managerState"].getManagerState().getProcesses()) { if (p.getName() == "ui" && p.getRunning()) { @@ -153,6 +157,15 @@ void RouteEngine::timerUpdate() { if (localizer_valid) { last_bearing = RAD2DEG(orientation.getValue()[2]); last_position = QMapbox::Coordinate(pos.getValue()[0], pos.getValue()[1]); + emit positionUpdated(*last_position, *last_bearing); + } + + active = true; +} + +void RouteEngine::routeUpdate() { + if (!active) { + return; } recomputeRoute(); @@ -261,6 +274,10 @@ bool RouteEngine::shouldRecompute() { } void RouteEngine::recomputeRoute() { + if (!last_position) { + return; + } + auto new_destination = coordinate_from_param("NavDestination"); if (!new_destination) { clearRoute(); @@ -308,6 +325,9 @@ void RouteEngine::routeCalculated(QGeoRouteReply *reply) { route = reply->routes().at(0); segment = route.firstRouteSegment(); + + auto path = route.path(); + emit routeUpdated(path); } else { qWarning() << "Got empty route response"; } diff --git a/selfdrive/ui/navd/route_engine.h b/selfdrive/ui/navd/route_engine.h index 9a6c99041..33cbc7910 100644 --- a/selfdrive/ui/navd/route_engine.h +++ b/selfdrive/ui/navd/route_engine.h @@ -23,7 +23,8 @@ public: SubMaster *sm; PubMaster *pm; - QTimer* timer; + QTimer* msg_timer; + QTimer* route_timer; std::optional ui_pid; @@ -41,6 +42,7 @@ public: bool localizer_valid = false; // Route recompute + bool active = false; int recompute_backoff = 0; int recompute_countdown = 0; void calculateRoute(QMapbox::Coordinate destination); @@ -48,8 +50,13 @@ public: bool shouldRecompute(); private slots: - void timerUpdate(); + void routeUpdate(); + void msgUpdate(); void routeCalculated(QGeoRouteReply *reply); void recomputeRoute(); void sendRoute(); + +signals: + void positionUpdated(QMapbox::Coordinate position, float bearing); + void routeUpdated(QList coordinates); }; diff --git a/selfdrive/ui/qt/maps/map.h b/selfdrive/ui/qt/maps/map.h index b74f5bf29..c62089cb2 100644 --- a/selfdrive/ui/qt/maps/map.h +++ b/selfdrive/ui/qt/maps/map.h @@ -22,9 +22,6 @@ #include "selfdrive/common/util.h" #include "cereal/messaging/messaging.h" -const QString MAPBOX_TOKEN = util::getenv("MAPBOX_TOKEN").c_str(); -const QString MAPS_HOST = util::getenv("MAPS_HOST", MAPBOX_TOKEN.isEmpty() ? "https://maps.comma.ai" : "https://api.mapbox.com").c_str(); - class MapInstructions : public QWidget { Q_OBJECT diff --git a/selfdrive/ui/qt/maps/map_helpers.cc b/selfdrive/ui/qt/maps/map_helpers.cc index 422ae99b6..f87e80403 100644 --- a/selfdrive/ui/qt/maps/map_helpers.cc +++ b/selfdrive/ui/qt/maps/map_helpers.cc @@ -4,7 +4,25 @@ #include #include "selfdrive/common/params.h" +#include "selfdrive/hardware/hw.h" +#include "selfdrive/ui/qt/api.h" +QString get_mapbox_token() { + // Valid for 4 weeks since we can't swap tokens on the fly + return MAPBOX_TOKEN.isEmpty() ? CommaApi::create_jwt({}, 4 * 7 * 24 * 3600) : MAPBOX_TOKEN; +} + +QMapboxGLSettings get_mapbox_settings() { + QMapboxGLSettings settings; + + if (!Hardware::PC()) { + settings.setCacheDatabasePath(MAPS_CACHE_PATH); + } + settings.setApiBaseUrl(MAPS_HOST); + settings.setAccessToken(get_mapbox_token()); + + return settings; +} QGeoCoordinate to_QGeoCoordinate(const QMapbox::Coordinate &in) { return QGeoCoordinate(in.first, in.second); @@ -83,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; +} + static QGeoCoordinate sub(QGeoCoordinate v, QGeoCoordinate w) { return QGeoCoordinate(v.latitude() - w.latitude(), v.longitude() - w.longitude()); } diff --git a/selfdrive/ui/qt/maps/map_helpers.h b/selfdrive/ui/qt/maps/map_helpers.h index 53a44f42d..1c8cacbeb 100644 --- a/selfdrive/ui/qt/maps/map_helpers.h +++ b/selfdrive/ui/qt/maps/map_helpers.h @@ -5,12 +5,17 @@ #include #include +#include "selfdrive/common/util.h" #include "common/transformations/coordinates.hpp" #include "common/transformations/orientation.hpp" #include "cereal/messaging/messaging.h" -#define RAD2DEG(x) ((x) * 180.0 / M_PI) +const QString MAPBOX_TOKEN = util::getenv("MAPBOX_TOKEN").c_str(); +const QString MAPS_HOST = util::getenv("MAPS_HOST", MAPBOX_TOKEN.isEmpty() ? "https://maps.comma.ai" : "https://api.mapbox.com").c_str(); +const QString MAPS_CACHE_PATH = "/data/mbgl-cache-navd.db"; +QString get_mapbox_token(); +QMapboxGLSettings get_mapbox_settings(); QGeoCoordinate to_QGeoCoordinate(const QMapbox::Coordinate &in); QMapbox::CoordinatesCollections model_to_collection( const cereal::LiveLocationKalman::Measurement::Reader &calibratedOrientationECEF, @@ -19,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); float minimum_distance(QGeoCoordinate a, QGeoCoordinate b, QGeoCoordinate p); std::optional coordinate_from_param(std::string param); diff --git a/selfdrive/ui/qt/offroad/driverview.cc b/selfdrive/ui/qt/offroad/driverview.cc index f988d193e..6f03e2ed4 100644 --- a/selfdrive/ui/qt/offroad/driverview.cc +++ b/selfdrive/ui/qt/offroad/driverview.cc @@ -12,7 +12,7 @@ DriverViewWindow::DriverViewWindow(QWidget* parent) : QWidget(parent) { layout = new QStackedLayout(this); layout->setStackingMode(QStackedLayout::StackAll); - cameraView = new CameraViewWidget(VISION_STREAM_RGB_FRONT, true, this); + cameraView = new CameraViewWidget("camerad", VISION_STREAM_RGB_FRONT, true, this); layout->addWidget(cameraView); scene = new DriverViewScene(this); diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index e7a233000..5abcf6104 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -5,9 +5,9 @@ #include "selfdrive/common/timing.h" #include "selfdrive/ui/paint.h" #include "selfdrive/ui/qt/util.h" -#include "selfdrive/ui/qt/api.h" #ifdef ENABLE_MAPS #include "selfdrive/ui/qt/maps/map.h" +#include "selfdrive/ui/qt/maps/map_helpers.h" #endif OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) { @@ -68,19 +68,7 @@ void OnroadWindow::offroadTransition(bool offroad) { #ifdef ENABLE_MAPS if (!offroad) { if (map == nullptr && (QUIState::ui_state.has_prime || !MAPBOX_TOKEN.isEmpty())) { - QMapboxGLSettings settings; - - // Valid for 4 weeks since we can't swap tokens on the fly - QString token = MAPBOX_TOKEN.isEmpty() ? CommaApi::create_jwt({}, 4 * 7 * 24 * 3600) : MAPBOX_TOKEN; - - if (!Hardware::PC()) { - settings.setCacheDatabasePath("/data/mbgl-cache.db"); - } - settings.setApiBaseUrl(MAPS_HOST); - settings.setCacheDatabaseMaximumSize(20 * 1024 * 1024); - settings.setAccessToken(token.trimmed()); - - MapWindow * m = new MapWindow(settings); + MapWindow * m = new MapWindow(get_mapbox_settings()); m->setFixedWidth(topWidget(this)->width() / 2); QObject::connect(this, &OnroadWindow::offroadTransitionSignal, m, &MapWindow::offroadTransition); split->addWidget(m, 0, Qt::AlignRight); diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index ee44973cf..1076ed01e 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -29,7 +29,7 @@ class NvgWindow : public CameraViewWidget { Q_OBJECT public: - explicit NvgWindow(VisionStreamType type, QWidget* parent = 0) : CameraViewWidget(type, true, parent) {} + explicit NvgWindow(VisionStreamType type, QWidget* parent = 0) : CameraViewWidget("camerad", type, true, parent) {} protected: void paintGL() override; diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index 6cc26a0ca..240e4c837 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -94,8 +94,8 @@ mat4 get_fit_view_transform(float widget_aspect_ratio, float frame_aspect_ratio) } // namespace -CameraViewWidget::CameraViewWidget(VisionStreamType type, bool zoom, QWidget* parent) : - stream_type(type), zoomed_view(zoom), QOpenGLWidget(parent) { +CameraViewWidget::CameraViewWidget(std::string stream_name, VisionStreamType type, bool zoom, QWidget* parent) : + stream_name(stream_name), stream_type(type), zoomed_view(zoom), QOpenGLWidget(parent) { setAttribute(Qt::WA_OpaquePaintEvent); connect(this, &CameraViewWidget::vipcThreadConnected, this, &CameraViewWidget::vipcConnected, Qt::BlockingQueuedConnection); connect(this, &CameraViewWidget::vipcThreadFrameReceived, this, &CameraViewWidget::vipcFrameReceived); @@ -276,7 +276,7 @@ void CameraViewWidget::vipcThread() { while (!QThread::currentThread()->isInterruptionRequested()) { if (!vipc_client || cur_stream_type != stream_type) { cur_stream_type = stream_type; - vipc_client.reset(new VisionIpcClient("camerad", cur_stream_type, true)); + vipc_client.reset(new VisionIpcClient(stream_name, cur_stream_type, true)); } if (!vipc_client->connected) { diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h index 90622c6b7..5e0fc1300 100644 --- a/selfdrive/ui/qt/widgets/cameraview.h +++ b/selfdrive/ui/qt/widgets/cameraview.h @@ -15,7 +15,7 @@ class CameraViewWidget : public QOpenGLWidget, protected QOpenGLFunctions { public: using QOpenGLWidget::QOpenGLWidget; - explicit CameraViewWidget(VisionStreamType stream_type, bool zoom, QWidget* parent = nullptr); + explicit CameraViewWidget(std::string stream_name, VisionStreamType stream_type, bool zoom, QWidget* parent = nullptr); ~CameraViewWidget(); void setStreamType(VisionStreamType type) { stream_type = type; } void setBackgroundColor(const QColor &color) { bg = color; } @@ -43,6 +43,7 @@ protected: QOpenGLShaderProgram *program; QColor bg = QColor("#000000"); + std::string stream_name; int stream_width = 0; int stream_height = 0; std::atomic stream_type; diff --git a/selfdrive/ui/watch3.cc b/selfdrive/ui/watch3.cc index e5e2864cc..73c8e1131 100644 --- a/selfdrive/ui/watch3.cc +++ b/selfdrive/ui/watch3.cc @@ -17,12 +17,20 @@ int main(int argc, char *argv[]) { QVBoxLayout *layout = new QVBoxLayout(&w); layout->setMargin(0); layout->setSpacing(0); - layout->addWidget(new CameraViewWidget(VISION_STREAM_RGB_BACK, false)); - QHBoxLayout *hlayout = new QHBoxLayout(); - layout->addLayout(hlayout); - hlayout->addWidget(new CameraViewWidget(VISION_STREAM_RGB_FRONT, false)); - hlayout->addWidget(new CameraViewWidget(VISION_STREAM_RGB_WIDE, false)); + { + QHBoxLayout *hlayout = new QHBoxLayout(); + layout->addLayout(hlayout); + hlayout->addWidget(new CameraViewWidget("navd", VISION_STREAM_RGB_MAP, false)); + hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_RGB_BACK, false)); + } + + { + QHBoxLayout *hlayout = new QHBoxLayout(); + layout->addLayout(hlayout); + hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_RGB_FRONT, false)); + hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_RGB_WIDE, false)); + } return a.exec(); }