navd: rewrite in python (#24621)
* navd: start python rewrite * use enum * send empty instruction packet * parse banner * cleanup * switch to coordinate class * add segment transition logic * add recompute logic * cleanup old navd code * split out helpers * cleanup release files * fix typo * fix docs * add typing to helpers * small fixes * move outside of ui * get last pos from param * add ui restart detection * check running * send route * set navInstruction to invalid with no active route * whitespace * do not overwrite response and use ratekeeper * reuse params object * remove navd exceptionpull/24682/head
Willem Melching
3 years ago
committed by
GitHub
parent
85b07bf3e8
commit
e72d6b5689
26 changed files with 421 additions and 937 deletions
@ -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,250 @@ |
|||||||
|
#!/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: |
||||||
|
self.mapbox_token = Api(self.params.get("DongleId", encoding='utf8')).get_token(expiry_hours=4 * 7 * 24) |
||||||
|
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}' |
||||||
|
resp = requests.get(url, params=params) |
||||||
|
|
||||||
|
if resp.status_code == 200: |
||||||
|
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() |
||||||
|
|
||||||
|
else: |
||||||
|
cloudlog.warning(f"Got error in route reply {resp.status_code}") |
||||||
|
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 +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); |
|
||||||
}; |
|
||||||
@ -1,6 +1,5 @@ |
|||||||
#!/bin/sh |
#!/bin/sh |
||||||
cd "$(dirname "$0")" |
cd "$(dirname "$0")" |
||||||
export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH" |
export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH" |
||||||
export QT_PLUGIN_PATH="../../third_party/qt-plugins/$(uname -m)" |
|
||||||
export QT_DBL_CLICK_DIST=150 |
export QT_DBL_CLICK_DIST=150 |
||||||
exec ./_ui |
exec ./_ui |
||||||
|
|||||||
@ -1,8 +0,0 @@ |
|||||||
#!/usr/bin/env sh |
|
||||||
|
|
||||||
# Qtlocation plugin with extra fields parsed from api response |
|
||||||
cd /tmp |
|
||||||
git clone https://github.com/commaai/qtlocation.git |
|
||||||
cd qtlocation |
|
||||||
qmake |
|
||||||
make -j$(nproc) |
|
||||||
@ -1 +0,0 @@ |
|||||||
libqtgeoservices_mapbox.so filter=lfs diff=lfs merge=lfs -text |
|
||||||
@ -1,3 +0,0 @@ |
|||||||
version https://git-lfs.github.com/spec/v1 |
|
||||||
oid sha256:5c8bda321dc524e753f67823cb5353358e756cfc1c5328e22e32920e80dd9e9b |
|
||||||
size 12166248 |
|
||||||
Loading…
Reference in new issue