diff --git a/selfdrive/navd/SConscript b/selfdrive/navd/SConscript new file mode 100644 index 0000000..5a173c0 --- /dev/null +++ b/selfdrive/navd/SConscript @@ -0,0 +1,20 @@ +Import('qt_env', 'arch', 'common', 'messaging', 'visionipc', 'cereal', 'transformations') + +map_env = qt_env.Clone() +libs = ['qt_widgets', 'qt_util', 'QMapLibre', common, messaging, cereal, visionipc, transformations, + 'zmq', 'capnp', 'kj', 'm', 'OpenCL', 'ssl', 'crypto', 'pthread', 'json11'] + map_env["LIBS"] +if arch == 'larch64': + libs.append(':libEGL_mesa.so.0') + +if arch in ['larch64', 'aarch64', 'x86_64']: + if arch == 'x86_64': + rpath = Dir(f"#third_party/maplibre-native-qt/{arch}/lib").srcnode().abspath + map_env["RPATH"] += [rpath, ] + + style_path = File("style.json").abspath + map_env['CXXFLAGS'].append(f'-DSTYLE_PATH=\\"{style_path}\\"') + + map_env["RPATH"].append(Dir('.').abspath) + map_env["LIBPATH"].append(Dir('.').abspath) + maplib = map_env.SharedLibrary("maprender", ["map_renderer.cc"], LIBS=libs) + map_env.Program("mapsd", ["main.cc", ], LIBS=[maplib[0].get_path(), ] + libs) diff --git a/selfdrive/navd/helpers.py b/selfdrive/navd/helpers.py index 55c3f88..0f0410c 100644 --- a/selfdrive/navd/helpers.py +++ b/selfdrive/navd/helpers.py @@ -2,7 +2,7 @@ from __future__ import annotations import json import math -from typing import Any, Dict, List, Optional, Tuple, Union, cast +from typing import Any, cast from openpilot.common.conversions import Conversions from openpilot.common.numpy_fast import clip @@ -22,13 +22,13 @@ class Coordinate: def __init__(self, latitude: float, longitude: float) -> None: self.latitude = latitude self.longitude = longitude - self.annotations: Dict[str, float] = {} + self.annotations: dict[str, float] = {} @classmethod - def from_mapbox_tuple(cls, t: Tuple[float, float]) -> Coordinate: + def from_mapbox_tuple(cls, t: tuple[float, float]) -> Coordinate: return cls(t[1], t[0]) - def as_dict(self) -> Dict[str, float]: + def as_dict(self) -> dict[str, float]: return {'latitude': self.latitude, 'longitude': self.longitude} def __str__(self) -> str: @@ -83,7 +83,7 @@ def minimum_distance(a: Coordinate, b: Coordinate, p: Coordinate): return projection.distance_to(p) -def distance_along_geometry(geometry: List[Coordinate], pos: Coordinate) -> float: +def distance_along_geometry(geometry: list[Coordinate], pos: Coordinate) -> float: if len(geometry) <= 2: return geometry[0].distance_to(pos) @@ -106,7 +106,7 @@ def distance_along_geometry(geometry: List[Coordinate], pos: Coordinate) -> floa return total_distance_closest -def coordinate_from_param(param: str, params: Optional[Params] = None) -> Optional[Coordinate]: +def coordinate_from_param(param: str, params: Params = None) -> Coordinate | None: if params is None: params = Params() @@ -130,7 +130,7 @@ def string_to_direction(direction: str) -> str: return 'none' -def maxspeed_to_ms(maxspeed: Dict[str, Union[str, float]]) -> float: +def maxspeed_to_ms(maxspeed: dict[str, str | float]) -> float: unit = cast(str, maxspeed['unit']) speed = cast(float, maxspeed['speed']) return SPEED_CONVERSIONS[unit] * speed @@ -140,7 +140,7 @@ def field_valid(dat: dict, field: str) -> bool: return field in dat and dat[field] is not None -def parse_banner_instructions(banners: Any, distance_to_maneuver: float = 0.0) -> Optional[Dict[str, Any]]: +def parse_banner_instructions(banners: Any, distance_to_maneuver: float = 0.0) -> dict[str, Any] | None: if not len(banners): return None diff --git a/selfdrive/navd/libmaprender.so b/selfdrive/navd/libmaprender.so deleted file mode 100644 index f155280..0000000 Binary files a/selfdrive/navd/libmaprender.so and /dev/null differ diff --git a/selfdrive/navd/main.cc b/selfdrive/navd/main.cc new file mode 100644 index 0000000..2e7b4d3 --- /dev/null +++ b/selfdrive/navd/main.cc @@ -0,0 +1,29 @@ +#include +#include + +#include +#include + +#include "common/util.h" +#include "selfdrive/ui/qt/util.h" +#include "selfdrive/ui/qt/maps/map_helpers.h" +#include "selfdrive/navd/map_renderer.h" +#include "system/hardware/hw.h" + +int main(int argc, char *argv[]) { + Hardware::config_cpu_rendering(true); + + qInstallMessageHandler(swagLogMessageHandler); + setpriority(PRIO_PROCESS, 0, -20); + int ret = util::set_core_affinity({0, 1, 2, 3}); + assert(ret == 0); + + QApplication app(argc, argv); + std::signal(SIGINT, sigTermHandler); + std::signal(SIGTERM, sigTermHandler); + + MapRenderer * m = new MapRenderer(get_mapbox_settings()); + assert(m); + + return app.exec(); +} diff --git a/selfdrive/navd/map_renderer.cc b/selfdrive/navd/map_renderer.cc new file mode 100644 index 0000000..d52ee16 --- /dev/null +++ b/selfdrive/navd/map_renderer.cc @@ -0,0 +1,334 @@ +#include "selfdrive/navd/map_renderer.h" + +#include +#include +#include +#include + +#include "common/util.h" +#include "common/timing.h" +#include "common/swaglog.h" +#include "selfdrive/ui/qt/maps/map_helpers.h" + +const float DEFAULT_ZOOM = 13.5; // Don't go below 13 or features will start to disappear +const int HEIGHT = 256, WIDTH = 256; +const int NUM_VIPC_BUFFERS = 4; + +const int EARTH_CIRCUMFERENCE_METERS = 40075000; +const int EARTH_RADIUS_METERS = 6378137; +const int PIXELS_PER_TILE = 256; +const int MAP_OFFSET = 128; + +const bool TEST_MODE = getenv("MAP_RENDER_TEST_MODE"); +const int LLK_DECIMATION = TEST_MODE ? 1 : 10; + +float get_zoom_level_for_scale(float lat, float meters_per_pixel) { + float meters_per_tile = meters_per_pixel * PIXELS_PER_TILE; + float num_tiles = cos(DEG2RAD(lat)) * EARTH_CIRCUMFERENCE_METERS / meters_per_tile; + return log2(num_tiles) - 1; +} + +QMapLibre::Coordinate get_point_along_line(float lat, float lon, float bearing, float dist) { + float ang_dist = dist / EARTH_RADIUS_METERS; + float lat1 = DEG2RAD(lat), lon1 = DEG2RAD(lon), bearing1 = DEG2RAD(bearing); + float lat2 = asin(sin(lat1)*cos(ang_dist) + cos(lat1)*sin(ang_dist)*cos(bearing1)); + float lon2 = lon1 + atan2(sin(bearing1)*sin(ang_dist)*cos(lat1), cos(ang_dist)-sin(lat1)*sin(lat2)); + return QMapLibre::Coordinate(RAD2DEG(lat2), RAD2DEG(lon2)); +} + + +MapRenderer::MapRenderer(const QMapLibre::Settings &settings, bool online) : 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)); + + std::string style = util::read_file(STYLE_PATH); + m_map.reset(new QMapLibre::Map(nullptr, m_settings, fbo->size(), 1)); + m_map->setCoordinateZoom(QMapLibre::Coordinate(0, 0), DEFAULT_ZOOM); + m_map->setStyleJson(style.c_str()); + m_map->createRenderer(); + ever_loaded = false; + + m_map->resize(fbo->size()); + m_map->setFramebufferObject(fbo->handle(), fbo->size()); + gl_functions->glViewport(0, 0, WIDTH, HEIGHT); + + QObject::connect(m_map.data(), &QMapLibre::Map::mapChanged, [=](QMapLibre::Map::MapChange change) { + // Ignore expected signals + // https://github.com/mapbox/mapbox-gl-native/blob/cf734a2fec960025350d8de0d01ad38aeae155a0/platform/qt/include/qmapboxgl.hpp#L116 + if (ever_loaded) { + if (change != QMapLibre::Map::MapChange::MapChangeRegionWillChange && + change != QMapLibre::Map::MapChange::MapChangeRegionDidChange && + change != QMapLibre::Map::MapChange::MapChangeWillStartRenderingFrame && + change != QMapLibre::Map::MapChange::MapChangeDidFinishRenderingFrameFullyRendered) { + LOGD("New map state: %d", change); + } + } + }); + + QObject::connect(m_map.data(), &QMapLibre::Map::mapLoadingFailed, [=](QMapLibre::Map::MapLoadingFailure err_code, const QString &reason) { + LOGE("Map loading failed with %d: '%s'\n", err_code, reason.toStdString().c_str()); + }); + + if (online) { + vipc_server.reset(new VisionIpcServer("navd")); + vipc_server->create_buffers(VisionStreamType::VISION_STREAM_MAP, NUM_VIPC_BUFFERS, false, WIDTH, HEIGHT); + vipc_server->start_listener(); + + pm.reset(new PubMaster({"navThumbnail", "mapRenderState"})); + sm.reset(new SubMaster({"liveLocationKalman", "navRoute"}, {"liveLocationKalman"})); + + timer = new QTimer(this); + timer->setSingleShot(true); + QObject::connect(timer, SIGNAL(timeout()), this, SLOT(msgUpdate())); + timer->start(0); + } +} + +void MapRenderer::msgUpdate() { + sm->update(1000); + + if (sm->updated("liveLocationKalman")) { + auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman(); + auto pos = location.getPositionGeodetic(); + auto orientation = location.getCalibratedOrientationNED(); + + if ((sm->rcv_frame("liveLocationKalman") % LLK_DECIMATION) == 0) { + float bearing = RAD2DEG(orientation.getValue()[2]); + updatePosition(get_point_along_line(pos.getValue()[0], pos.getValue()[1], bearing, MAP_OFFSET), bearing); + + // TODO: use the static rendering mode instead + // retry render a few times + for (int i = 0; i < 5 && !rendered(); i++) { + QApplication::processEvents(QEventLoop::AllEvents, 100); + update(); + if (rendered()) { + LOGW("rendered after %d retries", i+1); + break; + } + } + + // fallback to sending a blank frame + if (!rendered()) { + publish(0, false); + } + } + } + + if (sm->updated("navRoute")) { + QList route; + auto coords = (*sm)["navRoute"].getNavRoute().getCoordinates(); + for (auto const &c : coords) { + route.push_back(QGeoCoordinate(c.getLatitude(), c.getLongitude())); + } + updateRoute(route); + } + + // schedule next update + timer->start(0); +} + +void MapRenderer::updatePosition(QMapLibre::Coordinate position, float bearing) { + if (m_map.isNull()) { + return; + } + + // Choose a scale that ensures above 13 zoom level up to and above 75deg of lat + float meters_per_pixel = 2; + float zoom = get_zoom_level_for_scale(position.first, meters_per_pixel); + + m_map->setCoordinate(position); + m_map->setBearing(bearing); + m_map->setZoom(zoom); + update(); +} + +bool MapRenderer::loaded() { + return m_map->isFullyLoaded(); +} + +void MapRenderer::update() { + double start_t = millis_since_boot(); + gl_functions->glClear(GL_COLOR_BUFFER_BIT); + m_map->render(); + gl_functions->glFlush(); + double end_t = millis_since_boot(); + + if ((vipc_server != nullptr) && loaded()) { + publish((end_t - start_t) / 1000.0, true); + last_llk_rendered = (*sm)["liveLocationKalman"].getLogMonoTime(); + } +} + +void MapRenderer::sendThumbnail(const uint64_t ts, const kj::Array &buf) { + MessageBuilder msg; + auto thumbnaild = msg.initEvent().initNavThumbnail(); + thumbnaild.setFrameId(frame_id); + thumbnaild.setTimestampEof(ts); + thumbnaild.setThumbnail(buf); + pm->send("navThumbnail", msg); +} + +void MapRenderer::publish(const double render_time, const bool loaded) { + QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor); + + auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman(); + bool valid = loaded && (location.getStatus() == cereal::LiveLocationKalman::Status::VALID) && location.getPositionGeodetic().getValid(); + ever_loaded = ever_loaded || loaded; + uint64_t ts = nanos_since_boot(); + VisionBuf* buf = vipc_server->get_buffer(VisionStreamType::VISION_STREAM_MAP); + VisionIpcBufExtra extra = { + .frame_id = frame_id, + .timestamp_sof = (*sm)["liveLocationKalman"].getLogMonoTime(), + .timestamp_eof = ts, + .valid = valid, + }; + + assert(cap.sizeInBytes() >= buf->len); + uint8_t* dst = (uint8_t*)buf->addr; + uint8_t* src = cap.bits(); + + // RGB to greyscale + memset(dst, 128, buf->len); + for (int i = 0; i < WIDTH * HEIGHT; i++) { + dst[i] = src[i * 3]; + } + + vipc_server->send(buf, &extra); + + // Send thumbnail + if (TEST_MODE) { + // Full image in thumbnails in test mode + kj::Array buffer_kj = kj::heapArray((const capnp::byte*)cap.bits(), cap.sizeInBytes()); + sendThumbnail(ts, buffer_kj); + } else 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()); + sendThumbnail(ts, buffer_kj); + } + + // Send state msg + MessageBuilder msg; + auto evt = msg.initEvent(); + auto state = evt.initMapRenderState(); + evt.setValid(valid); + state.setLocationMonoTime((*sm)["liveLocationKalman"].getLogMonoTime()); + state.setRenderTime(render_time); + state.setFrameId(frame_id); + pm->send("mapRenderState", msg); + + frame_id++; +} + +uint8_t* MapRenderer::getImage() { + QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor); + + uint8_t* src = cap.bits(); + uint8_t* dst = new uint8_t[WIDTH * HEIGHT]; + + // RGB to greyscale + for (int i = 0; i < WIDTH * HEIGHT; i++) { + dst[i] = src[i * 3]; + } + + return dst; +} + +void MapRenderer::updateRoute(QList coordinates) { + if (m_map.isNull()) return; + initLayers(); + + auto route_points = coordinate_list_to_collection(coordinates); + QMapLibre::Feature feature(QMapLibre::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")) { + LOGD("Initializing navLayer"); + QVariantMap nav; + nav["type"] = "line"; + nav["source"] = "navSource"; + m_map->addLayer("navLayer", nav, "road-intersection"); + m_map->setPaintProperty("navLayer", "line-color", QColor("grey")); + m_map->setPaintProperty("navLayer", "line-width", 5); + m_map->setLayoutProperty("navLayer", "line-cap", "round"); + } +} + +MapRenderer::~MapRenderer() { +} + +extern "C" { + MapRenderer* map_renderer_init(char *maps_host = nullptr, char *token = nullptr) { + char *argv[] = { + (char*)"navd", + nullptr + }; + int argc = 0; + QApplication *app = new QApplication(argc, argv); + assert(app); + + QMapLibre::Settings settings; + settings.setProviderTemplate(QMapLibre::Settings::ProviderTemplate::MapboxProvider); + settings.setApiBaseUrl(maps_host == nullptr ? MAPS_HOST : maps_host); + settings.setApiKey(token == nullptr ? get_mapbox_token() : 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/navd/map_renderer.h b/selfdrive/navd/map_renderer.h new file mode 100644 index 0000000..fd5922b --- /dev/null +++ b/selfdrive/navd/map_renderer.h @@ -0,0 +1,59 @@ +#pragma once + +#include + +#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 QMapLibre::Settings &, bool online=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; + std::unique_ptr sm; + void publish(const double render_time, const bool loaded); + void sendThumbnail(const uint64_t ts, const kj::Array &buf); + + QMapLibre::Settings m_settings; + QScopedPointer m_map; + + void initLayers(); + + uint32_t frame_id = 0; + uint64_t last_llk_rendered = 0; + bool rendered() { + return last_llk_rendered == (*sm)["liveLocationKalman"].getLogMonoTime(); + } + + QTimer* timer; + bool ever_loaded = false; + +public slots: + void updatePosition(QMapLibre::Coordinate position, float bearing); + void updateRoute(QList coordinates); + void msgUpdate(); +}; diff --git a/selfdrive/navd/mapsd b/selfdrive/navd/mapsd deleted file mode 100755 index 854e92b..0000000 Binary files a/selfdrive/navd/mapsd and /dev/null differ diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index d9cdcaa..e22b3ac 100755 --- a/selfdrive/navd/navd.py +++ b/selfdrive/navd/navd.py @@ -18,8 +18,6 @@ from openpilot.selfdrive.navd.helpers import (Coordinate, coordinate_from_param, parse_banner_instructions) from openpilot.common.swaglog import cloudlog -from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController - REROUTE_DISTANCE = 25 MANEUVER_TRANSITION_THRESHOLD = 10 REROUTE_COUNTER_MIN = 3 @@ -55,7 +53,6 @@ class RouteEngine: if "MAPBOX_TOKEN" in os.environ: self.mapbox_token = os.environ["MAPBOX_TOKEN"] self.mapbox_host = "https://api.mapbox.com" - self.params.put("MapboxPublicKey", self.mapbox_token) elif self.params.get_int("PrimeType") == 0: self.mapbox_token = self.params.get("MapboxPublicKey", encoding='utf8') self.mapbox_host = "https://api.mapbox.com" @@ -77,10 +74,6 @@ class RouteEngine: self.update_frogpilot_params() def update(self): - # Update FrogPilot parameters - if self.params_memory.get_bool("FrogPilotTogglesUpdated"): - self.update_frogpilot_params() - self.sm.update(0) if self.sm.updated["managerState"]: @@ -98,6 +91,10 @@ class RouteEngine: except Exception: cloudlog.exception("navd.failed_to_compute") + # Update FrogPilot parameters + if self.params_memory.get_bool("FrogPilotTogglesUpdated"): + self.update_frogpilot_params() + def update_location(self): location = self.sm['liveLocationKalman'] self.gps_ok = location.gpsOK @@ -179,6 +176,7 @@ class RouteEngine: r = resp.json() r1 = resp.json() + # Function to remove specified keys recursively unnessary for display def remove_keys(obj, keys_to_remove): if isinstance(obj, list): @@ -187,13 +185,16 @@ class RouteEngine: return {key: remove_keys(value, keys_to_remove) for key, value in obj.items() if key not in keys_to_remove} else: return obj + keys_to_remove = ['geometry', 'annotation', 'incidents', 'intersections', 'components', 'sub', 'waypoints'] self.r2 = remove_keys(r1, keys_to_remove) self.r3 = {} + # Add items for display under "routes" if 'routes' in self.r2 and len(self.r2['routes']) > 0: first_route = self.r2['routes'][0] nav_destination_json = self.params.get('NavDestination') + try: nav_destination_data = json.loads(nav_destination_json) place_name = nav_destination_data.get('place_name', 'Default Place Name') @@ -203,7 +204,8 @@ class RouteEngine: self.r3['uuid'] = self.r2['uuid'] except json.JSONDecodeError as e: print(f"Error decoding JSON: {e}") - # Save slim json as file + + # Save slim json as file with open('navdirections.json', 'w') as json_file: json.dump(self.r2, json_file, indent=4) with open('CurrentStep.json', 'w') as json_file: @@ -217,6 +219,7 @@ class RouteEngine: if self.conditional_navigation_intersections: self.stop_signal = [] self.stop_coord = [] + for step in self.route: for intersection in step["intersections"]: if "stop_sign" in intersection or "traffic_signal" in intersection: @@ -265,11 +268,7 @@ class RouteEngine: if self.step_idx is None: msg.valid = False - SpeedLimitController.nav_speed_limit = 0 - SpeedLimitController.write_nav_state() - - if SpeedLimitController.desired_speed_limit != 0: - msg.navInstruction.speedLimit = SpeedLimitController.desired_speed_limit + self.params_memory.put_float("NavSpeedLimit", 0) self.pm.send('navInstruction', msg) return @@ -344,14 +343,9 @@ class RouteEngine: if ('maxspeed' in closest.annotations) and self.localizer_valid: msg.navInstruction.speedLimit = closest.annotations['maxspeed'] - SpeedLimitController.nav_speed_limit = closest.annotations['maxspeed'] - SpeedLimitController.write_nav_state() + self.params_memory.put_float("NavSpeedLimit", closest.annotations['maxspeed']) if not self.localizer_valid or ('maxspeed' not in closest.annotations): - SpeedLimitController.nav_speed_limit = 0 - SpeedLimitController.write_nav_state() - - if SpeedLimitController.desired_speed_limit != 0: - msg.navInstruction.speedLimit = SpeedLimitController.desired_speed_limit + self.params_memory.put_float("NavSpeedLimit", 0) # Speed limit sign type if 'speedLimitSign' in step: @@ -367,6 +361,7 @@ class RouteEngine: if self.step_idx + 1 < len(self.route): self.step_idx += 1 self.reset_recompute_limits() + # Update the 'CurrentStep' value in the JSON if 'routes' in self.r2 and len(self.r2['routes']) > 0: self.r3['CurrentStep'] = self.step_idx @@ -382,10 +377,11 @@ class RouteEngine: self.params.remove("NavDestination") self.clear_route() - # 5-10 Seconds to stop condition based on v_ego or minimum of 25 meters + # 5-10 Seconds to stop condition based on the current speed or minimum of 25 meters if self.conditional_navigation: v_ego = self.sm['carState'].vEgo seconds_to_stop = interp(v_ego, [0, 22.3, 44.7], [5, 10, 10]) + # Determine the location of the closest upcoming stopSign or trafficLight closest_condition_indices = [idx for idx in self.stop_signal if idx >= closest_idx] if closest_condition_indices: diff --git a/selfdrive/navd/tests/test_map_renderer.py b/selfdrive/navd/tests/test_map_renderer.py new file mode 100755 index 0000000..b5f186d --- /dev/null +++ b/selfdrive/navd/tests/test_map_renderer.py @@ -0,0 +1,216 @@ +#!/usr/bin/env python3 +import time +import numpy as np +import os +import pytest +import unittest +import requests +import threading +import http.server +import cereal.messaging as messaging + +from typing import Any +from cereal.visionipc import VisionIpcClient, VisionStreamType +from openpilot.common.mock.generators import LLK_DECIMATION, LOCATION1, LOCATION2, generate_liveLocationKalman +from openpilot.selfdrive.test.helpers import with_processes + +CACHE_PATH = "/data/mbgl-cache-navd.db" + +RENDER_FRAMES = 15 +DEFAULT_ITERATIONS = RENDER_FRAMES * LLK_DECIMATION +LOCATION1_REPEATED = [LOCATION1] * DEFAULT_ITERATIONS +LOCATION2_REPEATED = [LOCATION2] * DEFAULT_ITERATIONS + + +class MapBoxInternetDisabledRequestHandler(http.server.BaseHTTPRequestHandler): + INTERNET_ACTIVE = True + + def do_GET(self): + if not self.INTERNET_ACTIVE: + self.send_response(500) + self.end_headers() + return + + url = f'https://api.mapbox.com{self.path}' + + headers = dict(self.headers) + headers["Host"] = "api.mapbox.com" + + r = requests.get(url, headers=headers, timeout=5) + + self.send_response(r.status_code) + self.end_headers() + self.wfile.write(r.content) + + def log_message(self, *args: Any) -> None: + return + + def log_error(self, *args: Any) -> None: + return + + +class MapBoxInternetDisabledServer(threading.Thread): + def run(self): + self.server = http.server.HTTPServer(("127.0.0.1", 0), MapBoxInternetDisabledRequestHandler) + self.port = self.server.server_port + self.server.serve_forever() + + def stop(self): + self.server.shutdown() + + def disable_internet(self): + MapBoxInternetDisabledRequestHandler.INTERNET_ACTIVE = False + + def enable_internet(self): + MapBoxInternetDisabledRequestHandler.INTERNET_ACTIVE = True + + +class TestMapRenderer(unittest.TestCase): + server: MapBoxInternetDisabledServer + + @classmethod + def setUpClass(cls): + assert "MAPBOX_TOKEN" in os.environ + cls.original_token = os.environ["MAPBOX_TOKEN"] + cls.server = MapBoxInternetDisabledServer() + cls.server.start() + time.sleep(0.5) # wait for server to startup + + @classmethod + def tearDownClass(cls) -> None: + cls.server.stop() + + def setUp(self): + self.server.enable_internet() + os.environ['MAPS_HOST'] = f'http://localhost:{self.server.port}' + + self.sm = messaging.SubMaster(['mapRenderState']) + self.pm = messaging.PubMaster(['liveLocationKalman']) + self.vipc = VisionIpcClient("navd", VisionStreamType.VISION_STREAM_MAP, True) + + if os.path.exists(CACHE_PATH): + os.remove(CACHE_PATH) + + def _setup_test(self): + assert self.pm.wait_for_readers_to_update("liveLocationKalman", 10) + + time.sleep(0.5) + + assert VisionIpcClient.available_streams("navd", False) == {VisionStreamType.VISION_STREAM_MAP, } + assert self.vipc.connect(False) + self.vipc.recv() + + def _run_test(self, expect_valid, locations=LOCATION1_REPEATED): + starting_frame_id = None + + render_times = [] + + # run test + prev_frame_id = -1 + for i, location in enumerate(locations): + frame_expected = (i+1) % LLK_DECIMATION == 0 + + if self.sm.logMonoTime['mapRenderState'] == 0: + prev_valid = False + prev_frame_id = -1 + else: + prev_valid = self.sm.valid['mapRenderState'] + prev_frame_id = self.sm['mapRenderState'].frameId + + if starting_frame_id is None: + starting_frame_id = prev_frame_id + + llk = generate_liveLocationKalman(location) + self.pm.send("liveLocationKalman", llk) + self.pm.wait_for_readers_to_update("liveLocationKalman", 10) + self.sm.update(1000 if frame_expected else 0) + assert self.sm.updated['mapRenderState'] == frame_expected, "renderer running at wrong frequency" + + if not frame_expected: + continue + + frames_since_test_start = self.sm['mapRenderState'].frameId - starting_frame_id + + # give a few frames to switch from valid to invalid, or vice versa + invalid_and_not_previously_valid = (expect_valid and not self.sm.valid['mapRenderState'] and not prev_valid) + valid_and_not_previously_invalid = (not expect_valid and self.sm.valid['mapRenderState'] and prev_valid) + + if (invalid_and_not_previously_valid or valid_and_not_previously_invalid) and frames_since_test_start < 5: + continue + + # check output + assert self.sm.valid['mapRenderState'] == expect_valid + assert self.sm['mapRenderState'].frameId == (prev_frame_id + 1) + assert self.sm['mapRenderState'].locationMonoTime == llk.logMonoTime + if not expect_valid: + assert self.sm['mapRenderState'].renderTime == 0. + else: + assert 0. < self.sm['mapRenderState'].renderTime < 0.1 + render_times.append(self.sm['mapRenderState'].renderTime) + + # check vision ipc output + assert self.vipc.recv() is not None + assert self.vipc.valid == expect_valid + assert self.vipc.timestamp_sof == llk.logMonoTime + assert self.vipc.frame_id == self.sm['mapRenderState'].frameId + + assert frames_since_test_start >= RENDER_FRAMES + + return render_times + + @with_processes(["mapsd"]) + def test_with_internet(self): + self._setup_test() + self._run_test(True) + + @with_processes(["mapsd"]) + def test_with_no_internet(self): + self.server.disable_internet() + self._setup_test() + self._run_test(False) + + @with_processes(["mapsd"]) + @pytest.mark.skip(reason="slow, flaky, and unlikely to break") + def test_recover_from_no_internet(self): + self._setup_test() + self._run_test(True) + + self.server.disable_internet() + + # change locations to force mapsd to refetch + self._run_test(False, LOCATION2_REPEATED) + + self.server.enable_internet() + self._run_test(True, LOCATION2_REPEATED) + + @with_processes(["mapsd"]) + @pytest.mark.tici + def test_render_time_distribution(self): + self._setup_test() + # from location1 -> location2 and back + locations = np.array([*np.linspace(LOCATION1, LOCATION2, 2000), *np.linspace(LOCATION2, LOCATION1, 2000)]).tolist() + + render_times = self._run_test(True, locations) + + _min = np.min(render_times) + _max = np.max(render_times) + _mean = np.mean(render_times) + _median = np.median(render_times) + _stddev = np.std(render_times) + + print(f"Stats: min: {_min}, max: {_max}, mean: {_mean}, median: {_median}, stddev: {_stddev}, count: {len(render_times)}") + + def assert_stat(stat, nominal, tol=0.3): + tol = (nominal / (1+tol)), (nominal * (1+tol)) + self.assertTrue(tol[0] < stat < tol[1], f"{stat} not in tolerance {tol}") + + assert_stat(_mean, 0.030) + assert_stat(_median, 0.027) + assert_stat(_stddev, 0.0078) + + self.assertLess(_max, 0.065) + self.assertGreater(_min, 0.015) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/navd/tests/test_navd.py b/selfdrive/navd/tests/test_navd.py new file mode 100755 index 0000000..61be6cc --- /dev/null +++ b/selfdrive/navd/tests/test_navd.py @@ -0,0 +1,63 @@ +#!/usr/bin/env python3 +import json +import random +import unittest +import numpy as np + +from parameterized import parameterized + +import cereal.messaging as messaging +from openpilot.common.params import Params +from openpilot.selfdrive.manager.process_config import managed_processes + + +class TestNavd(unittest.TestCase): + def setUp(self): + self.params = Params() + self.sm = messaging.SubMaster(['navRoute', 'navInstruction']) + + def tearDown(self): + managed_processes['navd'].stop() + + def _check_route(self, start, end, check_coords=True): + self.params.put("NavDestination", json.dumps(end)) + self.params.put("LastGPSPosition", json.dumps(start)) + + managed_processes['navd'].start() + for _ in range(30): + self.sm.update(1000) + if all(f > 0 for f in self.sm.recv_frame.values()): + break + else: + raise Exception("didn't get a route") + + assert managed_processes['navd'].proc.is_alive() + managed_processes['navd'].stop() + + # ensure start and end match up + if check_coords: + coords = self.sm['navRoute'].coordinates + assert np.allclose([start['latitude'], start['longitude'], end['latitude'], end['longitude']], + [coords[0].latitude, coords[0].longitude, coords[-1].latitude, coords[-1].longitude], + rtol=1e-3) + + def test_simple(self): + start = { + "latitude": 32.7427228, + "longitude": -117.2321177, + } + end = { + "latitude": 32.7557004, + "longitude": -117.268002, + } + self._check_route(start, end) + + @parameterized.expand([(i,) for i in range(10)]) + def test_random(self, index): + start = {"latitude": random.uniform(-90, 90), "longitude": random.uniform(-180, 180)} + end = {"latitude": random.uniform(-90, 90), "longitude": random.uniform(-180, 180)} + self._check_route(start, end, check_coords=False) + + +if __name__ == "__main__": + unittest.main()