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 cerealpull/23053/head
Willem Melching
GitHub
parent
21ff97b8c9
commit
5069852573
2
cereal
2
cereal
|
|
@ -1 +1 @@
|
|||
Subproject commit 238e0c5793daed7dce6e1e072c4ae510f06ea515
|
||||
Subproject commit 9b245b60238b54867057c591b9ac2948b81a1794
|
||||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -135,6 +135,7 @@ std::unordered_map<std::string, uint32_t> 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},
|
||||
|
|
|
|||
|
|
@ -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'])
|
||||
|
|
|
|||
|
|
@ -5,9 +5,11 @@
|
|||
#include <csignal>
|
||||
|
||||
#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();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,200 @@
|
|||
#include "selfdrive/ui/navd/map_renderer.h"
|
||||
|
||||
#include <QApplication>
|
||||
#include <QBuffer>
|
||||
#include <QDebug>
|
||||
|
||||
#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<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 (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<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("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;
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,49 @@
|
|||
#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 enable_vipc=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;
|
||||
void sendVipc();
|
||||
|
||||
QMapboxGLSettings m_settings;
|
||||
QScopedPointer<QMapboxGL> m_map;
|
||||
|
||||
void initLayers();
|
||||
|
||||
uint32_t frame_id = 0;
|
||||
|
||||
public slots:
|
||||
void updatePosition(QMapbox::Coordinate position, float bearing);
|
||||
void updateRoute(QList<QGeoCoordinate> coordinates);
|
||||
};
|
||||
|
|
@ -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()
|
||||
|
|
@ -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";
|
||||
}
|
||||
|
|
|
|||
|
|
@ -23,7 +23,8 @@ public:
|
|||
SubMaster *sm;
|
||||
PubMaster *pm;
|
||||
|
||||
QTimer* timer;
|
||||
QTimer* msg_timer;
|
||||
QTimer* route_timer;
|
||||
|
||||
std::optional<int> 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<QGeoCoordinate> coordinates);
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -4,7 +4,25 @@
|
|||
#include <QJsonObject>
|
||||
|
||||
#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<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;
|
||||
}
|
||||
|
||||
static QGeoCoordinate sub(QGeoCoordinate v, QGeoCoordinate w) {
|
||||
return QGeoCoordinate(v.latitude() - w.latitude(), v.longitude() - w.longitude());
|
||||
}
|
||||
|
|
|
|||
|
|
@ -5,12 +5,17 @@
|
|||
#include <QMapboxGL>
|
||||
#include <QGeoCoordinate>
|
||||
|
||||
#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<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);
|
||||
|
||||
float minimum_distance(QGeoCoordinate a, QGeoCoordinate b, QGeoCoordinate p);
|
||||
std::optional<QMapbox::Coordinate> coordinate_from_param(std::string param);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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<VisionStreamType> stream_type;
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue