ZwX1616
3 years ago
commit
c5293d322b
115 changed files with 969 additions and 1530 deletions
@ -1 +1 @@ |
||||
Subproject commit f13d16c78a69fdef26b7448520c4398515758861 |
||||
Subproject commit 5fab88302ec375ba8c1c3fca3d85afee96113bcf |
||||
@ -1,14 +0,0 @@ |
||||
#include "common/visionimg.h" |
||||
|
||||
#include <cassert> |
||||
|
||||
EGLImageTexture::EGLImageTexture(const VisionBuf *buf) { |
||||
glGenTextures(1, &frame_tex); |
||||
glBindTexture(GL_TEXTURE_2D, frame_tex); |
||||
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, buf->width, buf->height, 0, GL_RGB, GL_UNSIGNED_BYTE, nullptr); |
||||
glGenerateMipmap(GL_TEXTURE_2D); |
||||
} |
||||
|
||||
EGLImageTexture::~EGLImageTexture() { |
||||
glDeleteTextures(1, &frame_tex); |
||||
} |
||||
@ -1,16 +0,0 @@ |
||||
#pragma once |
||||
|
||||
#include "cereal/visionipc/visionbuf.h" |
||||
|
||||
#ifdef __APPLE__ |
||||
#include <OpenGL/gl3.h> |
||||
#else |
||||
#include <GLES3/gl3.h> |
||||
#endif |
||||
|
||||
class EGLImageTexture { |
||||
public: |
||||
EGLImageTexture(const VisionBuf *buf); |
||||
~EGLImageTexture(); |
||||
GLuint frame_tex = 0; |
||||
}; |
||||
@ -0,0 +1,162 @@ |
||||
from __future__ import annotations |
||||
|
||||
import json |
||||
import math |
||||
from typing import Any, Dict, List, Optional, Tuple |
||||
|
||||
from common.numpy_fast import clip |
||||
from common.params import Params |
||||
|
||||
EARTH_MEAN_RADIUS = 6371007.2 |
||||
|
||||
|
||||
class Coordinate: |
||||
def __init__(self, latitude: float, longitude: float) -> None: |
||||
self.latitude = latitude |
||||
self.longitude = longitude |
||||
|
||||
@classmethod |
||||
def from_mapbox_tuple(cls, t: Tuple[float, float]) -> Coordinate: |
||||
return cls(t[1], t[0]) |
||||
|
||||
def as_dict(self) -> Dict[str, float]: |
||||
return {'latitude': self.latitude, 'longitude': self.longitude} |
||||
|
||||
def __str__(self) -> str: |
||||
return f"({self.latitude}, {self.longitude})" |
||||
|
||||
def __eq__(self, other) -> bool: |
||||
if not isinstance(other, Coordinate): |
||||
return False |
||||
return (self.latitude == other.latitude) and (self.longitude == other.longitude) |
||||
|
||||
def __sub__(self, other: Coordinate) -> Coordinate: |
||||
return Coordinate(self.latitude - other.latitude, self.longitude - other.longitude) |
||||
|
||||
def __add__(self, other: Coordinate) -> Coordinate: |
||||
return Coordinate(self.latitude + other.latitude, self.longitude + other.longitude) |
||||
|
||||
def __mul__(self, c: float) -> Coordinate: |
||||
return Coordinate(self.latitude * c, self.longitude * c) |
||||
|
||||
def dot(self, other: Coordinate) -> float: |
||||
return self.latitude * other.latitude + self.longitude * other.longitude |
||||
|
||||
def distance_to(self, other: Coordinate) -> float: |
||||
# Haversine formula |
||||
dlat = math.radians(other.latitude - self.latitude) |
||||
dlon = math.radians(other.longitude - self.longitude) |
||||
|
||||
haversine_dlat = math.sin(dlat / 2.0) |
||||
haversine_dlat *= haversine_dlat |
||||
haversine_dlon = math.sin(dlon / 2.0) |
||||
haversine_dlon *= haversine_dlon |
||||
|
||||
y = haversine_dlat \ |
||||
+ math.cos(math.radians(self.latitude)) \ |
||||
* math.cos(math.radians(other.latitude)) \ |
||||
* haversine_dlon |
||||
x = 2 * math.asin(math.sqrt(y)) |
||||
return x * EARTH_MEAN_RADIUS |
||||
|
||||
|
||||
def minimum_distance(a: Coordinate, b: Coordinate, p: Coordinate): |
||||
if a.distance_to(b) < 0.01: |
||||
return a.distance_to(p) |
||||
|
||||
ap = p - a |
||||
ab = b - a |
||||
t = clip(ap.dot(ab) / ab.dot(ab), 0.0, 1.0) |
||||
projection = a + ab * t |
||||
return projection.distance_to(p) |
||||
|
||||
|
||||
def distance_along_geometry(geometry: List[Coordinate], pos: Coordinate) -> float: |
||||
if len(geometry) <= 2: |
||||
return geometry[0].distance_to(pos) |
||||
|
||||
# 1. Find segment that is closest to current position |
||||
# 2. Total distance is sum of distance to start of closest segment |
||||
# + all previous segments |
||||
total_distance = 0.0 |
||||
total_distance_closest = 0.0 |
||||
closest_distance = 1e9 |
||||
|
||||
for i in range(len(geometry) - 1): |
||||
d = minimum_distance(geometry[i], geometry[i + 1], pos) |
||||
|
||||
if d < closest_distance: |
||||
closest_distance = d |
||||
total_distance_closest = total_distance + geometry[i].distance_to(pos) |
||||
|
||||
total_distance += geometry[i].distance_to(geometry[i + 1]) |
||||
|
||||
return total_distance_closest |
||||
|
||||
|
||||
def coordinate_from_param(param: str, params: Optional[Params] = None) -> Optional[Coordinate]: |
||||
if params is None: |
||||
params = Params() |
||||
|
||||
json_str = params.get(param) |
||||
if json_str is None: |
||||
return None |
||||
|
||||
pos = json.loads(json_str) |
||||
if 'latitude' not in pos or 'longitude' not in pos: |
||||
return None |
||||
|
||||
return Coordinate(pos['latitude'], pos['longitude']) |
||||
|
||||
|
||||
def string_to_direction(direction: str) -> str: |
||||
for d in ['left', 'right', 'straight']: |
||||
if d in direction: |
||||
return d |
||||
return 'none' |
||||
|
||||
|
||||
def parse_banner_instructions(instruction: Any, banners: Any, distance_to_maneuver: float = 0.0) -> None: |
||||
if not len(banners): |
||||
return |
||||
|
||||
current_banner = banners[0] |
||||
|
||||
# A segment can contain multiple banners, find one that we need to show now |
||||
for banner in banners: |
||||
if distance_to_maneuver < banner['distanceAlongGeometry']: |
||||
current_banner = banner |
||||
|
||||
# Only show banner when close enough to maneuver |
||||
instruction.showFull = distance_to_maneuver < current_banner['distanceAlongGeometry'] |
||||
|
||||
# Primary |
||||
p = current_banner['primary'] |
||||
if 'text' in p: |
||||
instruction.maneuverPrimaryText = p['text'] |
||||
if 'type' in p: |
||||
instruction.maneuverType = p['type'] |
||||
if 'modifier' in p: |
||||
instruction.maneuverModifier = p['modifier'] |
||||
|
||||
# Secondary |
||||
if 'secondary' in current_banner: |
||||
instruction.maneuverSecondaryText = current_banner['secondary']['text'] |
||||
|
||||
# Lane lines |
||||
if 'sub' in current_banner: |
||||
lanes = [] |
||||
for component in current_banner['sub']['components']: |
||||
if component['type'] != 'lane': |
||||
continue |
||||
|
||||
lane = { |
||||
'active': component['active'], |
||||
'directions': [string_to_direction(d) for d in component['directions']], |
||||
} |
||||
|
||||
if 'active_direction' in component: |
||||
lane['activeDirection'] = string_to_direction(component['active_direction']) |
||||
|
||||
lanes.append(lane) |
||||
instruction.lanes = lanes |
||||
@ -0,0 +1,255 @@ |
||||
#!/usr/bin/env python3 |
||||
import math |
||||
import os |
||||
import threading |
||||
|
||||
import requests |
||||
|
||||
import cereal.messaging as messaging |
||||
from cereal import log |
||||
from common.api import Api |
||||
from common.params import Params |
||||
from common.realtime import Ratekeeper |
||||
from selfdrive.swaglog import cloudlog |
||||
from selfdrive.navd.helpers import (Coordinate, coordinate_from_param, |
||||
distance_along_geometry, |
||||
minimum_distance, |
||||
parse_banner_instructions) |
||||
|
||||
REROUTE_DISTANCE = 25 |
||||
MANEUVER_TRANSITION_THRESHOLD = 10 |
||||
|
||||
|
||||
class RouteEngine: |
||||
def __init__(self, sm, pm): |
||||
self.sm = sm |
||||
self.pm = pm |
||||
|
||||
self.params = Params() |
||||
|
||||
# Get last gps position from params |
||||
self.last_position = coordinate_from_param("LastGPSPosition", self.params) |
||||
self.last_bearing = None |
||||
|
||||
self.gps_ok = False |
||||
|
||||
self.nav_destination = None |
||||
self.step_idx = None |
||||
self.route = None |
||||
self.route_geometry = None |
||||
|
||||
self.recompute_backoff = 0 |
||||
self.recompute_countdown = 0 |
||||
|
||||
self.ui_pid = None |
||||
|
||||
if "MAPBOX_TOKEN" in os.environ: |
||||
self.mapbox_token = os.environ["MAPBOX_TOKEN"] |
||||
self.mapbox_host = "https://api.mapbox.com" |
||||
else: |
||||
try: |
||||
self.mapbox_token = Api(self.params.get("DongleId", encoding='utf8')).get_token(expiry_hours=4 * 7 * 24) |
||||
except FileNotFoundError: |
||||
cloudlog.exception("Failed to generate mapbox token due to missing private key. Ensure device is registered.") |
||||
self.mapbox_token = "" |
||||
self.mapbox_host = "https://maps.comma.ai" |
||||
|
||||
def update(self): |
||||
self.sm.update(0) |
||||
|
||||
if self.sm.updated["managerState"]: |
||||
ui_pid = [p.pid for p in self.sm["managerState"].processes if p.name == "ui" and p.running] |
||||
if ui_pid: |
||||
if self.ui_pid and self.ui_pid != ui_pid[0]: |
||||
cloudlog.warning("UI restarting, sending route") |
||||
threading.Timer(5.0, self.send_route).start() |
||||
self.ui_pid = ui_pid[0] |
||||
|
||||
self.update_location() |
||||
self.recompute_route() |
||||
self.send_instruction() |
||||
|
||||
def update_location(self): |
||||
location = self.sm['liveLocationKalman'] |
||||
self.gps_ok = location.gpsOK |
||||
|
||||
localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid |
||||
|
||||
if localizer_valid: |
||||
self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2]) |
||||
self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1]) |
||||
|
||||
def recompute_route(self): |
||||
if self.last_position is None: |
||||
return |
||||
|
||||
new_destination = coordinate_from_param("NavDestination", self.params) |
||||
if new_destination is None: |
||||
self.clear_route() |
||||
return |
||||
|
||||
should_recompute = self.should_recompute() |
||||
if new_destination != self.nav_destination: |
||||
cloudlog.warning(f"Got new destination from NavDestination param {new_destination}") |
||||
should_recompute = True |
||||
|
||||
# Don't recompute when GPS drifts in tunnels |
||||
if not self.gps_ok and self.step_idx is not None: |
||||
return |
||||
|
||||
if self.recompute_countdown == 0 and should_recompute: |
||||
self.recompute_countdown = 2**self.recompute_backoff |
||||
self.recompute_backoff = min(6, self.recompute_backoff + 1) |
||||
self.calculate_route(new_destination) |
||||
else: |
||||
self.recompute_countdown = max(0, self.recompute_countdown - 1) |
||||
|
||||
def calculate_route(self, destination): |
||||
cloudlog.warning(f"Calculating route {self.last_position} -> {destination}") |
||||
self.nav_destination = destination |
||||
|
||||
params = { |
||||
'access_token': self.mapbox_token, |
||||
# 'annotations': 'maxspeed', |
||||
'geometries': 'geojson', |
||||
'overview': 'full', |
||||
'steps': 'true', |
||||
'banner_instructions': 'true', |
||||
'alternatives': 'false', |
||||
} |
||||
|
||||
if self.last_bearing is not None: |
||||
params['bearings'] = f"{(self.last_bearing + 360) % 360:.0f},90;" |
||||
|
||||
url = self.mapbox_host + f'/directions/v5/mapbox/driving-traffic/{self.last_position.longitude},{self.last_position.latitude};{destination.longitude},{destination.latitude}' |
||||
try: |
||||
resp = requests.get(url, params=params) |
||||
resp.raise_for_status() |
||||
|
||||
r = resp.json() |
||||
if len(r['routes']): |
||||
self.route = r['routes'][0]['legs'][0]['steps'] |
||||
self.route_geometry = [] |
||||
|
||||
# Convert coordinates |
||||
for step in self.route: |
||||
self.route_geometry.append([Coordinate.from_mapbox_tuple(c) for c in step['geometry']['coordinates']]) |
||||
|
||||
self.step_idx = 0 |
||||
else: |
||||
cloudlog.warning("Got empty route response") |
||||
self.clear_route() |
||||
|
||||
except requests.exceptions.RequestException: |
||||
cloudlog.exception("failed to get route") |
||||
self.clear_route() |
||||
|
||||
self.send_route() |
||||
|
||||
def send_instruction(self): |
||||
msg = messaging.new_message('navInstruction') |
||||
|
||||
if self.step_idx is None: |
||||
msg.valid = False |
||||
self.pm.send('navInstruction', msg) |
||||
return |
||||
|
||||
step = self.route[self.step_idx] |
||||
geometry = self.route_geometry[self.step_idx] |
||||
along_geometry = distance_along_geometry(geometry, self.last_position) |
||||
distance_to_maneuver_along_geometry = step['distance'] - along_geometry |
||||
|
||||
# Current instruction |
||||
msg.navInstruction.maneuverDistance = distance_to_maneuver_along_geometry |
||||
parse_banner_instructions(msg.navInstruction, step['bannerInstructions'], distance_to_maneuver_along_geometry) |
||||
|
||||
# Compute total remaining time and distance |
||||
remaning = 1.0 - along_geometry / max(step['distance'], 1) |
||||
total_distance = step['distance'] * remaning |
||||
total_time = step['duration'] * remaning |
||||
total_time_typical = step['duration_typical'] * remaning |
||||
|
||||
# Add up totals for future steps |
||||
for i in range(self.step_idx + 1, len(self.route)): |
||||
total_distance += self.route[i]['distance'] |
||||
total_time += self.route[i]['duration'] |
||||
total_time_typical += self.route[i]['duration_typical'] |
||||
|
||||
msg.navInstruction.distanceRemaining = total_distance |
||||
msg.navInstruction.timeRemaining = total_time |
||||
msg.navInstruction.timeRemainingTypical = total_time_typical |
||||
|
||||
self.pm.send('navInstruction', msg) |
||||
|
||||
# Transition to next route segment |
||||
if distance_to_maneuver_along_geometry < -MANEUVER_TRANSITION_THRESHOLD: |
||||
if self.step_idx + 1 < len(self.route): |
||||
self.step_idx += 1 |
||||
self.recompute_backoff = 0 |
||||
self.recompute_countdown = 0 |
||||
else: |
||||
cloudlog.warning("Destination reached") |
||||
Params().delete("NavDestination") |
||||
|
||||
# Clear route if driving away from destination |
||||
dist = self.nav_destination.distance_to(self.last_position) |
||||
if dist > REROUTE_DISTANCE: |
||||
self.clear_route() |
||||
|
||||
def send_route(self): |
||||
coords = [] |
||||
|
||||
if self.route is not None: |
||||
for path in self.route_geometry: |
||||
coords += [c.as_dict() for c in path] |
||||
|
||||
msg = messaging.new_message('navRoute') |
||||
msg.navRoute.coordinates = coords |
||||
self.pm.send('navRoute', msg) |
||||
|
||||
def clear_route(self): |
||||
self.route = None |
||||
self.route_geometry = None |
||||
self.step_idx = None |
||||
self.nav_destination = None |
||||
|
||||
def should_recompute(self): |
||||
if self.step_idx is None or self.route is None: |
||||
return True |
||||
|
||||
# Don't recompute in last segment, assume destination is reached |
||||
if self.step_idx == len(self.route) - 1: |
||||
return False |
||||
|
||||
# Compute closest distance to all line segments in the current path |
||||
min_d = REROUTE_DISTANCE + 1 |
||||
path = self.route_geometry[self.step_idx] |
||||
for i in range(len(path) - 1): |
||||
a = path[i] |
||||
b = path[i + 1] |
||||
|
||||
if a.distance_to(b) < 1.0: |
||||
continue |
||||
|
||||
min_d = min(min_d, minimum_distance(a, b, self.last_position)) |
||||
|
||||
return min_d > REROUTE_DISTANCE |
||||
|
||||
# TODO: Check for going wrong way in segment |
||||
|
||||
|
||||
def main(sm=None, pm=None): |
||||
if sm is None: |
||||
sm = messaging.SubMaster(['liveLocationKalman', 'managerState']) |
||||
if pm is None: |
||||
pm = messaging.PubMaster(['navInstruction', 'navRoute']) |
||||
|
||||
rk = Ratekeeper(1.0) |
||||
route_engine = RouteEngine(sm, pm) |
||||
while True: |
||||
route_engine.update() |
||||
rk.keep_time() |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
main() |
||||
@ -1 +1 @@ |
||||
0956446adfa91506f0a3d88f893e041bfb2890c1 |
||||
49e231ccf06ef35994a3ec907af959c28e3c0870 |
||||
@ -1 +0,0 @@ |
||||
_navd |
||||
@ -1,45 +0,0 @@ |
||||
#include <QApplication> |
||||
#include <QCommandLineParser> |
||||
#include <QDebug> |
||||
#include <QThread> |
||||
#include <csignal> |
||||
|
||||
#include "selfdrive/ui/qt/util.h" |
||||
#include "selfdrive/ui/qt/maps/map_helpers.h" |
||||
#include "selfdrive/ui/navd/route_engine.h" |
||||
#include "selfdrive/ui/navd/map_renderer.h" |
||||
#include "selfdrive/hardware/hw.h" |
||||
#include "common/params.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); |
||||
|
||||
QCommandLineParser parser; |
||||
parser.setApplicationDescription("Navigation server. Runs stand-alone, or using pre-computer route"); |
||||
parser.addHelpOption(); |
||||
parser.process(app); |
||||
const QStringList args = parser.positionalArguments(); |
||||
|
||||
|
||||
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(); |
||||
} |
||||
@ -1,200 +0,0 @@ |
||||
#include "selfdrive/ui/navd/map_renderer.h" |
||||
|
||||
#include <QApplication> |
||||
#include <QBuffer> |
||||
#include <QDebug> |
||||
|
||||
#include "selfdrive/ui/qt/maps/map_helpers.h" |
||||
#include "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; |
||||
} |
||||
} |
||||
@ -1,49 +0,0 @@ |
||||
#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); |
||||
}; |
||||
@ -1,78 +0,0 @@ |
||||
#!/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() |
||||
@ -1,4 +0,0 @@ |
||||
#!/bin/sh |
||||
cd "$(dirname "$0")" |
||||
export QT_PLUGIN_PATH="../../../third_party/qt-plugins/$(uname -m)" |
||||
exec ./_navd |
||||
@ -1,359 +0,0 @@ |
||||
#include "selfdrive/ui/navd/route_engine.h" |
||||
|
||||
#include <QDebug> |
||||
|
||||
#include "selfdrive/ui/qt/maps/map.h" |
||||
#include "selfdrive/ui/qt/maps/map_helpers.h" |
||||
#include "selfdrive/ui/qt/api.h" |
||||
|
||||
#include "common/params.h" |
||||
|
||||
const qreal REROUTE_DISTANCE = 25; |
||||
const float MANEUVER_TRANSITION_THRESHOLD = 10; |
||||
|
||||
static float get_time_typical(const QGeoRouteSegment &segment) { |
||||
auto maneuver = segment.maneuver(); |
||||
auto attrs = maneuver.extendedAttributes(); |
||||
return attrs.contains("mapbox.duration_typical") ? attrs["mapbox.duration_typical"].toDouble() : segment.travelTime(); |
||||
} |
||||
|
||||
static cereal::NavInstruction::Direction string_to_direction(QString d) { |
||||
if (d.contains("left")) { |
||||
return cereal::NavInstruction::Direction::LEFT; |
||||
} else if (d.contains("right")) { |
||||
return cereal::NavInstruction::Direction::RIGHT; |
||||
} else if (d.contains("straight")) { |
||||
return cereal::NavInstruction::Direction::STRAIGHT; |
||||
} |
||||
|
||||
return cereal::NavInstruction::Direction::NONE; |
||||
} |
||||
|
||||
static void parse_banner(cereal::NavInstruction::Builder &instruction, const QMap<QString, QVariant> &banner, bool full) { |
||||
QString primary_str, secondary_str; |
||||
|
||||
auto p = banner["primary"].toMap(); |
||||
primary_str += p["text"].toString(); |
||||
|
||||
instruction.setShowFull(full); |
||||
|
||||
if (p.contains("type")) { |
||||
instruction.setManeuverType(p["type"].toString().toStdString()); |
||||
} |
||||
|
||||
if (p.contains("modifier")) { |
||||
instruction.setManeuverModifier(p["modifier"].toString().toStdString()); |
||||
} |
||||
|
||||
if (banner.contains("secondary")) { |
||||
auto s = banner["secondary"].toMap(); |
||||
secondary_str += s["text"].toString(); |
||||
} |
||||
|
||||
instruction.setManeuverPrimaryText(primary_str.toStdString()); |
||||
instruction.setManeuverSecondaryText(secondary_str.toStdString()); |
||||
|
||||
if (banner.contains("sub")) { |
||||
auto s = banner["sub"].toMap(); |
||||
auto components = s["components"].toList(); |
||||
|
||||
size_t num_lanes = 0; |
||||
for (auto &c : components) { |
||||
auto cc = c.toMap(); |
||||
if (cc["type"].toString() == "lane") { |
||||
num_lanes += 1; |
||||
} |
||||
} |
||||
|
||||
auto lanes = instruction.initLanes(num_lanes); |
||||
|
||||
size_t i = 0; |
||||
for (auto &c : components) { |
||||
auto cc = c.toMap(); |
||||
if (cc["type"].toString() == "lane") { |
||||
auto lane = lanes[i]; |
||||
lane.setActive(cc["active"].toBool()); |
||||
|
||||
if (cc.contains("active_direction")) { |
||||
lane.setActiveDirection(string_to_direction(cc["active_direction"].toString())); |
||||
} |
||||
|
||||
auto directions = lane.initDirections(cc["directions"].toList().size()); |
||||
|
||||
size_t j = 0; |
||||
for (auto &dir : cc["directions"].toList()) { |
||||
directions.set(j, string_to_direction(dir.toString())); |
||||
j++; |
||||
} |
||||
|
||||
|
||||
i++; |
||||
} |
||||
} |
||||
} |
||||
} |
||||
|
||||
RouteEngine::RouteEngine() { |
||||
sm = new SubMaster({"liveLocationKalman", "managerState"}); |
||||
pm = new PubMaster({"navInstruction", "navRoute"}); |
||||
|
||||
// Timers
|
||||
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; |
||||
parameters["mapbox.access_token"] = get_mapbox_token(); |
||||
parameters["mapbox.directions_api_url"] = MAPS_HOST + "/directions/v5/mapbox/"; |
||||
|
||||
geoservice_provider = new QGeoServiceProvider("mapbox", parameters); |
||||
routing_manager = geoservice_provider->routingManager(); |
||||
if (routing_manager == nullptr) { |
||||
qWarning() << geoservice_provider->errorString(); |
||||
assert(routing_manager); |
||||
} |
||||
QObject::connect(routing_manager, &QGeoRoutingManager::finished, this, &RouteEngine::routeCalculated); |
||||
|
||||
// Get last gps position from params
|
||||
auto last_gps_position = coordinate_from_param("LastGPSPosition"); |
||||
if (last_gps_position) { |
||||
last_position = *last_gps_position; |
||||
} |
||||
} |
||||
|
||||
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()) { |
||||
if (ui_pid && *ui_pid != p.getPid()){ |
||||
qWarning() << "UI restarting, sending route"; |
||||
QTimer::singleShot(5000, this, &RouteEngine::sendRoute); |
||||
} |
||||
ui_pid = p.getPid(); |
||||
} |
||||
} |
||||
} |
||||
|
||||
auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman(); |
||||
auto pos = location.getPositionGeodetic(); |
||||
auto orientation = location.getCalibratedOrientationNED(); |
||||
|
||||
gps_ok = location.getGpsOK(); |
||||
|
||||
localizer_valid = (location.getStatus() == cereal::LiveLocationKalman::Status::VALID) && pos.getValid(); |
||||
|
||||
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(); |
||||
|
||||
MessageBuilder msg; |
||||
cereal::Event::Builder evt = msg.initEvent(segment.isValid()); |
||||
cereal::NavInstruction::Builder instruction = evt.initNavInstruction(); |
||||
|
||||
// Show route instructions
|
||||
if (segment.isValid()) { |
||||
auto cur_maneuver = segment.maneuver(); |
||||
auto attrs = cur_maneuver.extendedAttributes(); |
||||
if (cur_maneuver.isValid() && attrs.contains("mapbox.banner_instructions")) { |
||||
float along_geometry = distance_along_geometry(segment.path(), to_QGeoCoordinate(*last_position)); |
||||
float distance_to_maneuver_along_geometry = segment.distance() - along_geometry; |
||||
|
||||
auto banners = attrs["mapbox.banner_instructions"].toList(); |
||||
if (banners.size()) { |
||||
auto banner = banners[0].toMap(); |
||||
|
||||
for (auto &b : banners) { |
||||
auto bb = b.toMap(); |
||||
if (distance_to_maneuver_along_geometry < bb["distance_along_geometry"].toDouble()) { |
||||
banner = bb; |
||||
} |
||||
} |
||||
|
||||
instruction.setManeuverDistance(distance_to_maneuver_along_geometry); |
||||
parse_banner(instruction, banner, distance_to_maneuver_along_geometry < banner["distance_along_geometry"].toDouble()); |
||||
|
||||
// ETA
|
||||
float progress = distance_along_geometry(segment.path(), to_QGeoCoordinate(*last_position)) / segment.distance(); |
||||
float total_distance = segment.distance() * (1.0 - progress); |
||||
float total_time = segment.travelTime() * (1.0 - progress); |
||||
float total_time_typical = get_time_typical(segment) * (1.0 - progress); |
||||
|
||||
auto s = segment.nextRouteSegment(); |
||||
while (s.isValid()) { |
||||
total_distance += s.distance(); |
||||
total_time += s.travelTime(); |
||||
total_time_typical += get_time_typical(s); |
||||
|
||||
s = s.nextRouteSegment(); |
||||
} |
||||
instruction.setTimeRemaining(total_time); |
||||
instruction.setTimeRemainingTypical(total_time_typical); |
||||
instruction.setDistanceRemaining(total_distance); |
||||
} |
||||
|
||||
// Transition to next route segment
|
||||
if (distance_to_maneuver_along_geometry < -MANEUVER_TRANSITION_THRESHOLD) { |
||||
auto next_segment = segment.nextRouteSegment(); |
||||
if (next_segment.isValid()) { |
||||
segment = next_segment; |
||||
|
||||
recompute_backoff = 0; |
||||
recompute_countdown = 0; |
||||
} else { |
||||
qWarning() << "Destination reached"; |
||||
Params().remove("NavDestination"); |
||||
|
||||
// Clear route if driving away from destination
|
||||
float d = segment.maneuver().position().distanceTo(to_QGeoCoordinate(*last_position)); |
||||
if (d > REROUTE_DISTANCE) { |
||||
clearRoute(); |
||||
} |
||||
} |
||||
} |
||||
} |
||||
} |
||||
|
||||
pm->send("navInstruction", msg); |
||||
} |
||||
|
||||
void RouteEngine::clearRoute() { |
||||
route = QGeoRoute(); |
||||
segment = QGeoRouteSegment(); |
||||
nav_destination = QMapbox::Coordinate(); |
||||
} |
||||
|
||||
bool RouteEngine::shouldRecompute() { |
||||
if (!segment.isValid()) { |
||||
return true; |
||||
} |
||||
|
||||
// Don't recompute in last segment, assume destination is reached
|
||||
if (!segment.nextRouteSegment().isValid()) { |
||||
return false; |
||||
} |
||||
|
||||
// Compute closest distance to all line segments in the current path
|
||||
float min_d = REROUTE_DISTANCE + 1; |
||||
auto path = segment.path(); |
||||
auto cur = to_QGeoCoordinate(*last_position); |
||||
for (size_t i = 0; i < path.size() - 1; i++) { |
||||
auto a = path[i]; |
||||
auto b = path[i+1]; |
||||
if (a.distanceTo(b) < 1.0) { |
||||
continue; |
||||
} |
||||
min_d = std::min(min_d, minimum_distance(a, b, cur)); |
||||
} |
||||
return min_d > REROUTE_DISTANCE; |
||||
|
||||
// TODO: Check for going wrong way in segment
|
||||
} |
||||
|
||||
void RouteEngine::recomputeRoute() { |
||||
if (!last_position) { |
||||
return; |
||||
} |
||||
|
||||
auto new_destination = coordinate_from_param("NavDestination"); |
||||
if (!new_destination) { |
||||
clearRoute(); |
||||
return; |
||||
} |
||||
|
||||
bool should_recompute = shouldRecompute(); |
||||
if (*new_destination != nav_destination) { |
||||
qWarning() << "Got new destination from NavDestination param" << *new_destination; |
||||
should_recompute = true; |
||||
} |
||||
|
||||
if (!gps_ok && segment.isValid()) return; // Don't recompute when gps drifts in tunnels
|
||||
|
||||
if (recompute_countdown == 0 && should_recompute) { |
||||
recompute_countdown = std::pow(2, recompute_backoff); |
||||
recompute_backoff = std::min(6, recompute_backoff + 1); |
||||
calculateRoute(*new_destination); |
||||
} else { |
||||
recompute_countdown = std::max(0, recompute_countdown - 1); |
||||
} |
||||
} |
||||
|
||||
void RouteEngine::calculateRoute(QMapbox::Coordinate destination) { |
||||
qWarning() << "Calculating route" << *last_position << "->" << destination; |
||||
|
||||
nav_destination = destination; |
||||
QGeoRouteRequest request(to_QGeoCoordinate(*last_position), to_QGeoCoordinate(destination)); |
||||
request.setFeatureWeight(QGeoRouteRequest::TrafficFeature, QGeoRouteRequest::AvoidFeatureWeight); |
||||
|
||||
if (last_bearing) { |
||||
QVariantMap params; |
||||
int bearing = ((int)(*last_bearing) + 360) % 360; |
||||
params["bearing"] = bearing; |
||||
request.setWaypointsMetadata({params}); |
||||
} |
||||
|
||||
routing_manager->calculateRoute(request); |
||||
} |
||||
|
||||
void RouteEngine::routeCalculated(QGeoRouteReply *reply) { |
||||
if (reply->error() == QGeoRouteReply::NoError) { |
||||
if (reply->routes().size() != 0) { |
||||
qWarning() << "Got route response"; |
||||
|
||||
route = reply->routes().at(0); |
||||
segment = route.firstRouteSegment(); |
||||
|
||||
auto path = route.path(); |
||||
emit routeUpdated(path); |
||||
} else { |
||||
qWarning() << "Got empty route response"; |
||||
} |
||||
} else { |
||||
qWarning() << "Got error in route reply" << reply->errorString(); |
||||
} |
||||
|
||||
sendRoute(); |
||||
|
||||
reply->deleteLater(); |
||||
} |
||||
|
||||
void RouteEngine::sendRoute() { |
||||
MessageBuilder msg; |
||||
cereal::Event::Builder evt = msg.initEvent(); |
||||
cereal::NavRoute::Builder nav_route = evt.initNavRoute(); |
||||
|
||||
auto path = route.path(); |
||||
auto coordinates = nav_route.initCoordinates(path.size()); |
||||
|
||||
size_t i = 0; |
||||
for (auto const &c : route.path()) { |
||||
coordinates[i].setLatitude(c.latitude()); |
||||
coordinates[i].setLongitude(c.longitude()); |
||||
i++; |
||||
} |
||||
|
||||
pm->send("navRoute", msg); |
||||
} |
||||
@ -1,62 +0,0 @@ |
||||
#pragma once |
||||
|
||||
#include <optional> |
||||
|
||||
#include <QThread> |
||||
#include <QGeoCoordinate> |
||||
#include <QGeoManeuver> |
||||
#include <QGeoRouteRequest> |
||||
#include <QGeoRouteSegment> |
||||
#include <QGeoRoutingManager> |
||||
#include <QGeoServiceProvider> |
||||
#include <QTimer> |
||||
#include <QMapboxGL> |
||||
|
||||
#include "cereal/messaging/messaging.h" |
||||
|
||||
class RouteEngine : public QObject { |
||||
Q_OBJECT |
||||
|
||||
public: |
||||
RouteEngine(); |
||||
|
||||
SubMaster *sm; |
||||
PubMaster *pm; |
||||
|
||||
QTimer* msg_timer; |
||||
QTimer* route_timer; |
||||
|
||||
std::optional<int> ui_pid; |
||||
|
||||
// Route
|
||||
bool gps_ok = false; |
||||
QGeoServiceProvider *geoservice_provider; |
||||
QGeoRoutingManager *routing_manager; |
||||
QGeoRoute route; |
||||
QGeoRouteSegment segment; |
||||
QMapbox::Coordinate nav_destination; |
||||
|
||||
// Position
|
||||
std::optional<QMapbox::Coordinate> last_position; |
||||
std::optional<float> last_bearing; |
||||
bool localizer_valid = false; |
||||
|
||||
// Route recompute
|
||||
bool active = false; |
||||
int recompute_backoff = 0; |
||||
int recompute_countdown = 0; |
||||
void calculateRoute(QMapbox::Coordinate destination); |
||||
void clearRoute(); |
||||
bool shouldRecompute(); |
||||
|
||||
private slots: |
||||
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); |
||||
}; |
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in new issue