mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-06-27 17:42:04 +08:00
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 exception old-commit-hash: e72d6b5689dd95f5fd7a6558c72c2e02b37f8b12
This commit is contained in:
Willem Melching
@@ -22,13 +22,13 @@ class Api():
|
||||
def request(self, method, endpoint, timeout=None, access_token=None, **params):
|
||||
return api_get(endpoint, method=method, timeout=timeout, access_token=access_token, **params)
|
||||
|
||||
def get_token(self):
|
||||
def get_token(self, expiry_hours=1):
|
||||
now = datetime.utcnow()
|
||||
payload = {
|
||||
'identity': self.dongle_id,
|
||||
'nbf': now,
|
||||
'iat': now,
|
||||
'exp': now + timedelta(hours=1)
|
||||
'exp': now + timedelta(hours=expiry_hours)
|
||||
}
|
||||
token = jwt.encode(payload, self.private_key, algorithm='RS256')
|
||||
if isinstance(token, bytes):
|
||||
|
||||
@@ -50,10 +50,6 @@ soundd
|
||||
.. autodoxygenindex::
|
||||
:project: selfdrive_ui_soundd
|
||||
|
||||
navd
|
||||
""""
|
||||
.. autodoxygenindex::
|
||||
:project: selfdrive_ui_navd
|
||||
|
||||
replay
|
||||
""""""
|
||||
|
||||
@@ -293,6 +293,9 @@ selfdrive/ui/qt/widgets/*.h
|
||||
selfdrive/ui/replay/*.cc
|
||||
selfdrive/ui/replay/*.h
|
||||
|
||||
selfdrive/ui/qt/maps/*.cc
|
||||
selfdrive/ui/qt/maps/*.h
|
||||
|
||||
selfdrive/camerad/SConscript
|
||||
selfdrive/camerad/main.cc
|
||||
|
||||
@@ -362,6 +365,8 @@ selfdrive/modeld/runners/run.h
|
||||
selfdrive/monitoring/dmonitoringd.py
|
||||
selfdrive/monitoring/driver_monitor.py
|
||||
|
||||
selfdrive/navd/*.py
|
||||
|
||||
selfdrive/assets/.gitignore
|
||||
selfdrive/assets/assets.qrc
|
||||
selfdrive/assets/*.png
|
||||
|
||||
@@ -2,8 +2,6 @@ selfdrive/modeld/runners/onnx*
|
||||
|
||||
third_party/mapbox-gl-native-qt/x86_64/*.so
|
||||
|
||||
third_party/qt-plugins/x86_64/geoservices/*.so
|
||||
|
||||
third_party/libyuv/x64/**
|
||||
third_party/snpe/x86_64/**
|
||||
third_party/snpe/x86_64-linux-clang/**
|
||||
|
||||
@@ -13,11 +13,3 @@ selfdrive/camerad/cameras/real_debayer.cl
|
||||
|
||||
selfdrive/ui/qt/spinner_larch64
|
||||
selfdrive/ui/qt/text_larch64
|
||||
selfdrive/ui/qt/maps/*.cc
|
||||
selfdrive/ui/qt/maps/*.h
|
||||
|
||||
selfdrive/ui/navd/*.cc
|
||||
selfdrive/ui/navd/*.h
|
||||
selfdrive/ui/navd/navd
|
||||
selfdrive/ui/navd/.gitignore
|
||||
|
||||
|
||||
@@ -27,7 +27,6 @@ procs = [
|
||||
NativeProcess("encoderd", "selfdrive/loggerd", ["./encoderd"]),
|
||||
NativeProcess("loggerd", "selfdrive/loggerd", ["./loggerd"], onroad=False, callback=logging),
|
||||
NativeProcess("modeld", "selfdrive/modeld", ["./modeld"]),
|
||||
NativeProcess("navd", "selfdrive/ui/navd", ["./navd"], offroad=True),
|
||||
NativeProcess("proclogd", "selfdrive/proclogd", ["./proclogd"]),
|
||||
NativeProcess("sensord", "selfdrive/sensord", ["./sensord"], enabled=not PC),
|
||||
NativeProcess("ubloxd", "selfdrive/locationd", ["./ubloxd"], enabled=(not PC or WEBCAM)),
|
||||
@@ -40,6 +39,7 @@ procs = [
|
||||
PythonProcess("deleter", "selfdrive.loggerd.deleter", offroad=True),
|
||||
PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", enabled=(not PC or WEBCAM), callback=driverview),
|
||||
PythonProcess("logmessaged", "selfdrive.logmessaged", offroad=True),
|
||||
PythonProcess("navd", "selfdrive.navd.navd"),
|
||||
PythonProcess("pandad", "selfdrive.boardd.pandad", offroad=True),
|
||||
PythonProcess("paramsd", "selfdrive.locationd.paramsd"),
|
||||
PythonProcess("plannerd", "selfdrive.controls.plannerd"),
|
||||
|
||||
@@ -50,7 +50,7 @@ class TestManager(unittest.TestCase):
|
||||
self.assertTrue(state.running, f"{p} not running")
|
||||
|
||||
exit_code = managed_processes[p].stop(retry=False)
|
||||
if (TICI and p in ['ui', 'navd']):
|
||||
if (TICI and p in ['ui',]):
|
||||
# TODO: make Qt UI exit gracefully
|
||||
continue
|
||||
|
||||
|
||||
@@ -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
|
||||
Executable
+250
@@ -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()
|
||||
@@ -118,11 +118,3 @@ if arch in ['x86_64', 'Darwin'] or GetOption('extras'):
|
||||
|
||||
if GetOption('test'):
|
||||
qt_env.Program('replay/tests/test_replay', ['replay/tests/test_runner.cc', 'replay/tests/test_replay.cc'], LIBS=[replay_libs])
|
||||
|
||||
# navd
|
||||
if maps:
|
||||
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'])
|
||||
|
||||
@@ -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);
|
||||
};
|
||||
@@ -101,101 +101,6 @@ 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());
|
||||
}
|
||||
|
||||
static QGeoCoordinate add(QGeoCoordinate v, QGeoCoordinate w) {
|
||||
return QGeoCoordinate(v.latitude() + w.latitude(), v.longitude() + w.longitude());
|
||||
}
|
||||
|
||||
static QGeoCoordinate mul(QGeoCoordinate v, float c) {
|
||||
return QGeoCoordinate(c * v.latitude(), c * v.longitude());
|
||||
}
|
||||
|
||||
static float dot(QGeoCoordinate v, QGeoCoordinate w) {
|
||||
return v.latitude() * w.latitude() + v.longitude() * w.longitude();
|
||||
}
|
||||
|
||||
float minimum_distance(QGeoCoordinate a, QGeoCoordinate b, QGeoCoordinate p) {
|
||||
// If a and b are the same coordinate the computation below doesn't work
|
||||
if (a.distanceTo(b) < 0.01) {
|
||||
return a.distanceTo(p);
|
||||
}
|
||||
|
||||
const QGeoCoordinate ap = sub(p, a);
|
||||
const QGeoCoordinate ab = sub(b, a);
|
||||
const float t = std::clamp(dot(ap, ab) / dot(ab, ab), 0.0f, 1.0f);
|
||||
const QGeoCoordinate projection = add(a, mul(ab, t));
|
||||
return projection.distanceTo(p);
|
||||
}
|
||||
|
||||
float distance_along_geometry(QList<QGeoCoordinate> geometry, QGeoCoordinate pos) {
|
||||
if (geometry.size() <= 2) {
|
||||
return geometry[0].distanceTo(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
|
||||
double total_distance = 0;
|
||||
double total_distance_closest = 0;
|
||||
double closest_distance = std::numeric_limits<double>::max();
|
||||
|
||||
for (int i = 0; i < geometry.size() - 1; i++) {
|
||||
double d = minimum_distance(geometry[i], geometry[i+1], pos);
|
||||
if (d < closest_distance) {
|
||||
closest_distance = d;
|
||||
total_distance_closest = total_distance + geometry[i].distanceTo(pos);
|
||||
}
|
||||
total_distance += geometry[i].distanceTo(geometry[i+1]);
|
||||
}
|
||||
|
||||
return total_distance_closest;
|
||||
}
|
||||
|
||||
std::optional<QMapbox::Coordinate> coordinate_from_param(std::string param) {
|
||||
QString json_str = QString::fromStdString(Params().get(param));
|
||||
if (json_str.isEmpty()) return {};
|
||||
|
||||
@@ -24,8 +24,5 @@ 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);
|
||||
float distance_along_geometry(QList<QGeoCoordinate> geometry, QGeoCoordinate pos);
|
||||
|
||||
@@ -1,6 +1,5 @@
|
||||
#!/bin/sh
|
||||
cd "$(dirname "$0")"
|
||||
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
|
||||
exec ./_ui
|
||||
|
||||
@@ -19,8 +19,6 @@ int main(int argc, char *argv[]) {
|
||||
{
|
||||
QHBoxLayout *hlayout = new QHBoxLayout();
|
||||
layout->addLayout(hlayout);
|
||||
// TODO: make mapd output YUV
|
||||
// hlayout->addWidget(new CameraViewWidget("navd", VISION_STREAM_MAP, false));
|
||||
hlayout->addWidget(new CameraViewWidget("camerad", VISION_STREAM_ROAD, false));
|
||||
}
|
||||
|
||||
|
||||
-8
@@ -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
|
||||
Reference in New Issue
Block a user