From e72d6b5689dd95f5fd7a6558c72c2e02b37f8b12 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 30 May 2022 15:15:51 +0200 Subject: [PATCH] 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 --- common/api/__init__.py | 4 +- docs/c_docs.rst | 4 - release/files_common | 5 + release/files_pc | 2 - release/files_tici | 8 - selfdrive/manager/process_config.py | 2 +- selfdrive/manager/test/test_manager.py | 2 +- selfdrive/navd/__init__.py | 0 selfdrive/navd/helpers.py | 162 ++++++++ selfdrive/navd/navd.py | 250 ++++++++++++ selfdrive/ui/SConscript | 8 - selfdrive/ui/navd/.gitignore | 1 - selfdrive/ui/navd/main.cc | 45 --- selfdrive/ui/navd/map_renderer.cc | 200 ---------- selfdrive/ui/navd/map_renderer.h | 49 --- selfdrive/ui/navd/map_renderer.py | 78 ---- selfdrive/ui/navd/navd | 4 - selfdrive/ui/navd/route_engine.cc | 359 ------------------ selfdrive/ui/navd/route_engine.h | 62 --- selfdrive/ui/qt/maps/map_helpers.cc | 95 ----- selfdrive/ui/qt/maps/map_helpers.h | 3 - selfdrive/ui/ui | 1 - selfdrive/ui/watch3.cc | 2 - third_party/qt-plugins/build_qtlocation.sh | 8 - .../x86_64/geoservices/.gitattributes | 1 - .../geoservices/libqtgeoservices_mapbox.so | 3 - 26 files changed, 421 insertions(+), 937 deletions(-) create mode 100644 selfdrive/navd/__init__.py create mode 100644 selfdrive/navd/helpers.py create mode 100755 selfdrive/navd/navd.py delete mode 100644 selfdrive/ui/navd/.gitignore delete mode 100644 selfdrive/ui/navd/main.cc delete mode 100644 selfdrive/ui/navd/map_renderer.cc delete mode 100644 selfdrive/ui/navd/map_renderer.h delete mode 100755 selfdrive/ui/navd/map_renderer.py delete mode 100755 selfdrive/ui/navd/navd delete mode 100644 selfdrive/ui/navd/route_engine.cc delete mode 100644 selfdrive/ui/navd/route_engine.h delete mode 100755 third_party/qt-plugins/build_qtlocation.sh delete mode 100644 third_party/qt-plugins/x86_64/geoservices/.gitattributes delete mode 100755 third_party/qt-plugins/x86_64/geoservices/libqtgeoservices_mapbox.so diff --git a/common/api/__init__.py b/common/api/__init__.py index 8b83dfc641..fd2e70910e 100644 --- a/common/api/__init__.py +++ b/common/api/__init__.py @@ -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): diff --git a/docs/c_docs.rst b/docs/c_docs.rst index db7100ab27..1e080a826d 100644 --- a/docs/c_docs.rst +++ b/docs/c_docs.rst @@ -50,10 +50,6 @@ soundd .. autodoxygenindex:: :project: selfdrive_ui_soundd -navd -"""" -.. autodoxygenindex:: - :project: selfdrive_ui_navd replay """""" diff --git a/release/files_common b/release/files_common index e254abec66..982194d774 100644 --- a/release/files_common +++ b/release/files_common @@ -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 diff --git a/release/files_pc b/release/files_pc index e401badb80..01ecae4327 100644 --- a/release/files_pc +++ b/release/files_pc @@ -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/** diff --git a/release/files_tici b/release/files_tici index 75abc13abc..ee1ac63c34 100644 --- a/release/files_tici +++ b/release/files_tici @@ -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 - diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index ad3cabbbd9..7f31d837b5 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -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"), diff --git a/selfdrive/manager/test/test_manager.py b/selfdrive/manager/test/test_manager.py index 1750c81b2e..c1a6d4816b 100755 --- a/selfdrive/manager/test/test_manager.py +++ b/selfdrive/manager/test/test_manager.py @@ -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 diff --git a/selfdrive/navd/__init__.py b/selfdrive/navd/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/navd/helpers.py b/selfdrive/navd/helpers.py new file mode 100644 index 0000000000..89fd5e7e4e --- /dev/null +++ b/selfdrive/navd/helpers.py @@ -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 diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py new file mode 100755 index 0000000000..e063773d52 --- /dev/null +++ b/selfdrive/navd/navd.py @@ -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() diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index 1ea13d5564..6b9db34b4d 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -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']) diff --git a/selfdrive/ui/navd/.gitignore b/selfdrive/ui/navd/.gitignore deleted file mode 100644 index 0fa7b173e4..0000000000 --- a/selfdrive/ui/navd/.gitignore +++ /dev/null @@ -1 +0,0 @@ -_navd diff --git a/selfdrive/ui/navd/main.cc b/selfdrive/ui/navd/main.cc deleted file mode 100644 index fe354b7b7d..0000000000 --- a/selfdrive/ui/navd/main.cc +++ /dev/null @@ -1,45 +0,0 @@ -#include -#include -#include -#include -#include - -#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(); -} diff --git a/selfdrive/ui/navd/map_renderer.cc b/selfdrive/ui/navd/map_renderer.cc deleted file mode 100644 index 8d2a8810c9..0000000000 --- a/selfdrive/ui/navd/map_renderer.cc +++ /dev/null @@ -1,200 +0,0 @@ -#include "selfdrive/ui/navd/map_renderer.h" - -#include -#include -#include - -#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(); - ctx->setFormat(fmt); - ctx->create(); - assert(ctx->isValid()); - - surface = std::make_unique(); - surface->setFormat(ctx->format()); - surface->create(); - - ctx->makeCurrent(surface.get()); - assert(QOpenGLContext::currentContext() == ctx.get()); - - gl_functions.reset(ctx->functions()); - gl_functions->initializeOpenGLFunctions(); - - QOpenGLFramebufferObjectFormat fbo_format; - fbo.reset(new QOpenGLFramebufferObject(WIDTH, HEIGHT, fbo_format)); - - m_map.reset(new QMapboxGL(nullptr, m_settings, fbo->size(), 1)); - m_map->setCoordinateZoom(QMapbox::Coordinate(0, 0), ZOOM); - m_map->setStyleUrl("mapbox://styles/commaai/ckvmksrpd4n0a14pfdo5heqzr"); - m_map->createRenderer(); - - m_map->resize(fbo->size()); - m_map->setFramebufferObject(fbo->handle(), fbo->size()); - gl_functions->glViewport(0, 0, WIDTH, HEIGHT); - - if (enable_vipc) { - qWarning() << "Enabling navd map rendering"; - vipc_server.reset(new VisionIpcServer("navd")); - vipc_server->create_buffers(VisionStreamType::VISION_STREAM_RGB_MAP, NUM_VIPC_BUFFERS, true, WIDTH, HEIGHT); - vipc_server->start_listener(); - - pm.reset(new PubMaster({"navThumbnail"})); - } -} - -void MapRenderer::updatePosition(QMapbox::Coordinate position, float bearing) { - if (m_map.isNull()) { - return; - } - - m_map->setCoordinate(position); - m_map->setBearing(bearing); - update(); -} - -bool MapRenderer::loaded() { - return m_map->isFullyLoaded(); -} - -void MapRenderer::update() { - gl_functions->glClear(GL_COLOR_BUFFER_BIT); - m_map->render(); - gl_functions->glFlush(); - - sendVipc(); -} - -void MapRenderer::sendVipc() { - if (!vipc_server || !loaded()) { - return; - } - - QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor); - uint64_t ts = nanos_since_boot(); - VisionBuf* buf = vipc_server->get_buffer(VisionStreamType::VISION_STREAM_RGB_MAP); - VisionIpcBufExtra extra = { - .frame_id = frame_id, - .timestamp_sof = ts, - .timestamp_eof = ts, - }; - - assert(cap.sizeInBytes() == buf->len); - memcpy(buf->addr, cap.bits(), buf->len); - vipc_server->send(buf, &extra); - - if (frame_id % 100 == 0) { - // Write jpeg into buffer - QByteArray buffer_bytes; - QBuffer buffer(&buffer_bytes); - buffer.open(QIODevice::WriteOnly); - cap.save(&buffer, "JPG", 50); - - kj::Array buffer_kj = kj::heapArray((const capnp::byte*)buffer_bytes.constData(), buffer_bytes.size()); - - // Send thumbnail - MessageBuilder msg; - auto thumbnaild = msg.initEvent().initNavThumbnail(); - thumbnaild.setFrameId(frame_id); - thumbnaild.setTimestampEof(ts); - thumbnaild.setThumbnail(buffer_kj); - pm->send("navThumbnail", msg); - } - - frame_id++; -} - -uint8_t* MapRenderer::getImage() { - QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor); - uint8_t* buf = new uint8_t[cap.sizeInBytes()]; - memcpy(buf, cap.bits(), cap.sizeInBytes()); - - return buf; -} - -void MapRenderer::updateRoute(QList coordinates) { - if (m_map.isNull()) return; - initLayers(); - - auto route_points = coordinate_list_to_collection(coordinates); - QMapbox::Feature feature(QMapbox::Feature::LineStringType, route_points, {}, {}); - QVariantMap navSource; - navSource["type"] = "geojson"; - navSource["data"] = QVariant::fromValue(feature); - m_map->updateSource("navSource", navSource); - m_map->setLayoutProperty("navLayer", "visibility", "visible"); -} - -void MapRenderer::initLayers() { - if (!m_map->layerExists("navLayer")) { - QVariantMap nav; - nav["id"] = "navLayer"; - nav["type"] = "line"; - nav["source"] = "navSource"; - m_map->addLayer(nav, "road-intersection"); - m_map->setPaintProperty("navLayer", "line-color", QColor("blue")); - m_map->setPaintProperty("navLayer", "line-width", 3); - m_map->setLayoutProperty("navLayer", "line-cap", "round"); - } -} - -MapRenderer::~MapRenderer() { -} - -extern "C" { - MapRenderer* map_renderer_init() { - char *argv[] = { - (char*)"navd", - nullptr - }; - int argc = 0; - QApplication *app = new QApplication(argc, argv); - assert(app); - - QMapboxGLSettings settings; - settings.setApiBaseUrl(MAPS_HOST); - settings.setAccessToken(get_mapbox_token()); - - return new MapRenderer(settings, false); - } - - void map_renderer_update_position(MapRenderer *inst, float lat, float lon, float bearing) { - inst->updatePosition({lat, lon}, bearing); - QApplication::processEvents(); - } - - void map_renderer_update_route(MapRenderer *inst, char* polyline) { - inst->updateRoute(polyline_to_coordinate_list(QString::fromUtf8(polyline))); - } - - void map_renderer_update(MapRenderer *inst) { - inst->update(); - } - - void map_renderer_process(MapRenderer *inst) { - QApplication::processEvents(); - } - - bool map_renderer_loaded(MapRenderer *inst) { - return inst->loaded(); - } - - uint8_t * map_renderer_get_image(MapRenderer *inst) { - return inst->getImage(); - } - - void map_renderer_free_image(MapRenderer *inst, uint8_t * buf) { - delete[] buf; - } -} diff --git a/selfdrive/ui/navd/map_renderer.h b/selfdrive/ui/navd/map_renderer.h deleted file mode 100644 index 1746e76695..0000000000 --- a/selfdrive/ui/navd/map_renderer.h +++ /dev/null @@ -1,49 +0,0 @@ -#pragma once - -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "cereal/visionipc/visionipc_server.h" -#include "cereal/messaging/messaging.h" - - -class MapRenderer : public QObject { - Q_OBJECT - -public: - MapRenderer(const QMapboxGLSettings &, bool enable_vipc=true); - uint8_t* getImage(); - void update(); - bool loaded(); - ~MapRenderer(); - - -private: - std::unique_ptr ctx; - std::unique_ptr surface; - std::unique_ptr gl_functions; - std::unique_ptr fbo; - - std::unique_ptr vipc_server; - std::unique_ptr pm; - void sendVipc(); - - QMapboxGLSettings m_settings; - QScopedPointer m_map; - - void initLayers(); - - uint32_t frame_id = 0; - -public slots: - void updatePosition(QMapbox::Coordinate position, float bearing); - void updateRoute(QList coordinates); -}; diff --git a/selfdrive/ui/navd/map_renderer.py b/selfdrive/ui/navd/map_renderer.py deleted file mode 100755 index 28d006841b..0000000000 --- a/selfdrive/ui/navd/map_renderer.py +++ /dev/null @@ -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() diff --git a/selfdrive/ui/navd/navd b/selfdrive/ui/navd/navd deleted file mode 100755 index ff3103bd97..0000000000 --- a/selfdrive/ui/navd/navd +++ /dev/null @@ -1,4 +0,0 @@ -#!/bin/sh -cd "$(dirname "$0")" -export QT_PLUGIN_PATH="../../../third_party/qt-plugins/$(uname -m)" -exec ./_navd diff --git a/selfdrive/ui/navd/route_engine.cc b/selfdrive/ui/navd/route_engine.cc deleted file mode 100644 index 577f267c9b..0000000000 --- a/selfdrive/ui/navd/route_engine.cc +++ /dev/null @@ -1,359 +0,0 @@ -#include "selfdrive/ui/navd/route_engine.h" - -#include - -#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 &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); -} diff --git a/selfdrive/ui/navd/route_engine.h b/selfdrive/ui/navd/route_engine.h deleted file mode 100644 index 33cbc79107..0000000000 --- a/selfdrive/ui/navd/route_engine.h +++ /dev/null @@ -1,62 +0,0 @@ -#pragma once - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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 ui_pid; - - // Route - bool gps_ok = false; - QGeoServiceProvider *geoservice_provider; - QGeoRoutingManager *routing_manager; - QGeoRoute route; - QGeoRouteSegment segment; - QMapbox::Coordinate nav_destination; - - // Position - std::optional last_position; - std::optional 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 coordinates); -}; diff --git a/selfdrive/ui/qt/maps/map_helpers.cc b/selfdrive/ui/qt/maps/map_helpers.cc index be1098994b..83576eb630 100644 --- a/selfdrive/ui/qt/maps/map_helpers.cc +++ b/selfdrive/ui/qt/maps/map_helpers.cc @@ -101,101 +101,6 @@ QMapbox::CoordinatesCollections coordinate_list_to_collection(QList polyline_to_coordinate_list(const QString &polylineString) { - QList path; - if (polylineString.isEmpty()) - return path; - - QByteArray data = polylineString.toLatin1(); - - bool parsingLatitude = true; - - int shift = 0; - int value = 0; - - QGeoCoordinate coord(0, 0); - - for (int i = 0; i < data.length(); ++i) { - unsigned char c = data.at(i) - 63; - - value |= (c & 0x1f) << shift; - shift += 5; - - // another chunk - if (c & 0x20) - continue; - - int diff = (value & 1) ? ~(value >> 1) : (value >> 1); - - if (parsingLatitude) { - coord.setLatitude(coord.latitude() + (double)diff/1e6); - } else { - coord.setLongitude(coord.longitude() + (double)diff/1e6); - path.append(coord); - } - - parsingLatitude = !parsingLatitude; - - value = 0; - shift = 0; - } - - return path; -} - -static QGeoCoordinate sub(QGeoCoordinate v, QGeoCoordinate w) { - return QGeoCoordinate(v.latitude() - w.latitude(), v.longitude() - w.longitude()); -} - -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 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::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 coordinate_from_param(std::string param) { QString json_str = QString::fromStdString(Params().get(param)); if (json_str.isEmpty()) return {}; diff --git a/selfdrive/ui/qt/maps/map_helpers.h b/selfdrive/ui/qt/maps/map_helpers.h index cda4cd1cfb..344246bb05 100644 --- a/selfdrive/ui/qt/maps/map_helpers.h +++ b/selfdrive/ui/qt/maps/map_helpers.h @@ -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::Reader &coordinate_list); QMapbox::CoordinatesCollections coordinate_list_to_collection(QList coordinate_list); -QList polyline_to_coordinate_list(const QString &polylineString); -float minimum_distance(QGeoCoordinate a, QGeoCoordinate b, QGeoCoordinate p); std::optional coordinate_from_param(std::string param); -float distance_along_geometry(QList geometry, QGeoCoordinate pos); diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 16ddbab050..c9f81c0539 100755 --- a/selfdrive/ui/ui +++ b/selfdrive/ui/ui @@ -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 diff --git a/selfdrive/ui/watch3.cc b/selfdrive/ui/watch3.cc index c1d47d040d..00d23ea976 100644 --- a/selfdrive/ui/watch3.cc +++ b/selfdrive/ui/watch3.cc @@ -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)); } diff --git a/third_party/qt-plugins/build_qtlocation.sh b/third_party/qt-plugins/build_qtlocation.sh deleted file mode 100755 index 09e6182d1f..0000000000 --- a/third_party/qt-plugins/build_qtlocation.sh +++ /dev/null @@ -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) diff --git a/third_party/qt-plugins/x86_64/geoservices/.gitattributes b/third_party/qt-plugins/x86_64/geoservices/.gitattributes deleted file mode 100644 index 51b1d333fe..0000000000 --- a/third_party/qt-plugins/x86_64/geoservices/.gitattributes +++ /dev/null @@ -1 +0,0 @@ -libqtgeoservices_mapbox.so filter=lfs diff=lfs merge=lfs -text diff --git a/third_party/qt-plugins/x86_64/geoservices/libqtgeoservices_mapbox.so b/third_party/qt-plugins/x86_64/geoservices/libqtgeoservices_mapbox.so deleted file mode 100755 index 375fc0e012..0000000000 --- a/third_party/qt-plugins/x86_64/geoservices/libqtgeoservices_mapbox.so +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:5c8bda321dc524e753f67823cb5353358e756cfc1c5328e22e32920e80dd9e9b -size 12166248