wip
This commit is contained in:
Your Name
@@ -2,6 +2,5 @@ SConscript(['boardd/SConscript'])
|
|||||||
SConscript(['controls/lib/lateral_mpc_lib/SConscript'])
|
SConscript(['controls/lib/lateral_mpc_lib/SConscript'])
|
||||||
SConscript(['controls/lib/longitudinal_mpc_lib/SConscript'])
|
SConscript(['controls/lib/longitudinal_mpc_lib/SConscript'])
|
||||||
SConscript(['locationd/SConscript'])
|
SConscript(['locationd/SConscript'])
|
||||||
SConscript(['navd/SConscript'])
|
|
||||||
SConscript(['modeld/SConscript'])
|
SConscript(['modeld/SConscript'])
|
||||||
SConscript(['ui/SConscript'])
|
SConscript(['ui/SConscript'])
|
||||||
6
selfdrive/navd/.gitignore
vendored
6
selfdrive/navd/.gitignore
vendored
@@ -1,6 +0,0 @@
|
|||||||
moc_*
|
|
||||||
*.moc
|
|
||||||
|
|
||||||
mapsd
|
|
||||||
map_renderer
|
|
||||||
libmap_renderer.so
|
|
||||||
@@ -1,24 +0,0 @@
|
|||||||
# navigation
|
|
||||||
|
|
||||||
This directory contains two daemons, `navd` and `mapsd`, which support navigation in the openpilot stack.
|
|
||||||
|
|
||||||
### navd
|
|
||||||
|
|
||||||
`navd` takes in a route through the `NavDestination` param and sends out two packets: `navRoute` and `navInstruction`. These packets contain the coordinates of the planned route and turn-by-turn instructions.
|
|
||||||
|
|
||||||
### map renderer
|
|
||||||
|
|
||||||
The map renderer listens for the `navRoute` and publishes a rendered map view over VisionIPC for the navigation model, which lives in `selfdrive/modeld/`. The rendered maps look like this:
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
## development
|
|
||||||
|
|
||||||
Currently, [mapbox](https://www.mapbox.com/) is used for navigation.
|
|
||||||
|
|
||||||
* get an API token: https://docs.mapbox.com/help/glossary/access-token/
|
|
||||||
* set an API token using the `MAPBOX_TOKEN` environment variable
|
|
||||||
* routes/destinations are set through the `NavDestination` param
|
|
||||||
* use `set_destination.py` for debugging
|
|
||||||
* edit the map: https://www.mapbox.com/contribute
|
|
||||||
* mapbox API playground: https://docs.mapbox.com/playground/
|
|
||||||
@@ -1,20 +0,0 @@
|
|||||||
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)
|
|
||||||
@@ -1,189 +0,0 @@
|
|||||||
from __future__ import annotations
|
|
||||||
|
|
||||||
import json
|
|
||||||
import math
|
|
||||||
from typing import Any, cast
|
|
||||||
|
|
||||||
from openpilot.common.conversions import Conversions
|
|
||||||
from openpilot.common.numpy_fast import clip
|
|
||||||
from openpilot.common.params import Params
|
|
||||||
|
|
||||||
DIRECTIONS = ('left', 'right', 'straight')
|
|
||||||
MODIFIABLE_DIRECTIONS = ('left', 'right')
|
|
||||||
|
|
||||||
EARTH_MEAN_RADIUS = 6371007.2
|
|
||||||
SPEED_CONVERSIONS = {
|
|
||||||
'km/h': Conversions.KPH_TO_MS,
|
|
||||||
'mph': Conversions.MPH_TO_MS,
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
class Coordinate:
|
|
||||||
def __init__(self, latitude: float, longitude: float) -> None:
|
|
||||||
self.latitude = latitude
|
|
||||||
self.longitude = longitude
|
|
||||||
self.annotations: dict[str, float] = {}
|
|
||||||
|
|
||||||
@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'Coordinate({self.latitude}, {self.longitude})'
|
|
||||||
|
|
||||||
def __repr__(self) -> str:
|
|
||||||
return self.__str__()
|
|
||||||
|
|
||||||
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: Params = None) -> Coordinate | None:
|
|
||||||
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 DIRECTIONS:
|
|
||||||
if d in direction:
|
|
||||||
if 'slight' in direction and d in MODIFIABLE_DIRECTIONS:
|
|
||||||
return 'slight' + d.capitalize()
|
|
||||||
return d
|
|
||||||
return 'none'
|
|
||||||
|
|
||||||
|
|
||||||
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
|
|
||||||
|
|
||||||
|
|
||||||
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) -> dict[str, Any] | None:
|
|
||||||
if not len(banners):
|
|
||||||
return None
|
|
||||||
|
|
||||||
instruction = {}
|
|
||||||
|
|
||||||
# A segment can contain multiple banners, find one that we need to show now
|
|
||||||
current_banner = banners[0]
|
|
||||||
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 field_valid(p, 'text'):
|
|
||||||
instruction['maneuverPrimaryText'] = p['text']
|
|
||||||
if field_valid(p, 'type'):
|
|
||||||
instruction['maneuverType'] = p['type']
|
|
||||||
if field_valid(p, 'modifier'):
|
|
||||||
instruction['maneuverModifier'] = p['modifier']
|
|
||||||
|
|
||||||
# Secondary
|
|
||||||
if field_valid(current_banner, 'secondary'):
|
|
||||||
instruction['maneuverSecondaryText'] = current_banner['secondary']['text']
|
|
||||||
|
|
||||||
# Lane lines
|
|
||||||
if field_valid(current_banner, 'sub'):
|
|
||||||
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 field_valid(component, 'active_direction'):
|
|
||||||
lane['activeDirection'] = string_to_direction(component['active_direction'])
|
|
||||||
|
|
||||||
lanes.append(lane)
|
|
||||||
instruction['lanes'] = lanes
|
|
||||||
|
|
||||||
return instruction
|
|
||||||
@@ -1,29 +0,0 @@
|
|||||||
#include <csignal>
|
|
||||||
#include <sys/resource.h>
|
|
||||||
|
|
||||||
#include <QApplication>
|
|
||||||
#include <QDebug>
|
|
||||||
|
|
||||||
#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();
|
|
||||||
}
|
|
||||||
@@ -1,334 +0,0 @@
|
|||||||
#include "selfdrive/navd/map_renderer.h"
|
|
||||||
|
|
||||||
#include <cmath>
|
|
||||||
#include <string>
|
|
||||||
#include <QApplication>
|
|
||||||
#include <QBuffer>
|
|
||||||
|
|
||||||
#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<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));
|
|
||||||
|
|
||||||
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<QGeoCoordinate> 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<capnp::byte> &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<capnp::byte> buffer_kj = kj::heapArray<capnp::byte>((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<capnp::byte> buffer_kj = kj::heapArray<capnp::byte>((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<QGeoCoordinate> 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<QMapLibre::Feature>(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;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,59 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
|
|
||||||
#include <memory>
|
|
||||||
|
|
||||||
#include <QOpenGLContext>
|
|
||||||
#include <QMapLibre/Map>
|
|
||||||
#include <QMapLibre/Settings>
|
|
||||||
#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 QMapLibre::Settings &, bool online=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;
|
|
||||||
std::unique_ptr<SubMaster> sm;
|
|
||||||
void publish(const double render_time, const bool loaded);
|
|
||||||
void sendThumbnail(const uint64_t ts, const kj::Array<capnp::byte> &buf);
|
|
||||||
|
|
||||||
QMapLibre::Settings m_settings;
|
|
||||||
QScopedPointer<QMapLibre::Map> 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<QGeoCoordinate> coordinates);
|
|
||||||
void msgUpdate();
|
|
||||||
};
|
|
||||||
@@ -1,96 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
# You might need to uninstall the PyQt5 pip package to avoid conflicts
|
|
||||||
|
|
||||||
import os
|
|
||||||
import time
|
|
||||||
import numpy as np
|
|
||||||
import polyline
|
|
||||||
from cffi import FFI
|
|
||||||
|
|
||||||
from openpilot.common.ffi_wrapper import suffix
|
|
||||||
from openpilot.common.basedir import BASEDIR
|
|
||||||
|
|
||||||
HEIGHT = WIDTH = SIZE = 256
|
|
||||||
METERS_PER_PIXEL = 2
|
|
||||||
|
|
||||||
|
|
||||||
def get_ffi():
|
|
||||||
lib = os.path.join(BASEDIR, "selfdrive", "navd", "libmaprender" + suffix())
|
|
||||||
|
|
||||||
ffi = FFI()
|
|
||||||
ffi.cdef("""
|
|
||||||
void* map_renderer_init(char *maps_host, char *token);
|
|
||||||
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:WIDTH * HEIGHT])
|
|
||||||
lib.map_renderer_free_image(renderer, buf)
|
|
||||||
|
|
||||||
# Convert to numpy
|
|
||||||
r = np.asarray(r)
|
|
||||||
return r.reshape((WIDTH, HEIGHT))
|
|
||||||
|
|
||||||
|
|
||||||
def navRoute_to_polyline(nr):
|
|
||||||
coords = [(m.latitude, m.longitude) for m in nr.navRoute.coordinates]
|
|
||||||
return coords_to_polyline(coords)
|
|
||||||
|
|
||||||
|
|
||||||
def coords_to_polyline(coords):
|
|
||||||
# TODO: where does this factor of 10 come from?
|
|
||||||
return polyline.encode([(lat * 10., lon * 10.) for lat, lon in coords])
|
|
||||||
|
|
||||||
|
|
||||||
def polyline_to_coords(p):
|
|
||||||
coords = polyline.decode(p)
|
|
||||||
return [(lat / 10., lon / 10.) for lat, lon in coords]
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
import matplotlib.pyplot as plt
|
|
||||||
|
|
||||||
ffi, lib = get_ffi()
|
|
||||||
renderer = lib.map_renderer_init(ffi.NULL, ffi.NULL)
|
|
||||||
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" # noqa: E501
|
|
||||||
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), cmap='gray')
|
|
||||||
|
|
||||||
plt.show()
|
|
||||||
@@ -1,476 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
import json
|
|
||||||
import math
|
|
||||||
import os
|
|
||||||
import threading
|
|
||||||
|
|
||||||
import requests
|
|
||||||
|
|
||||||
import cereal.messaging as messaging
|
|
||||||
from cereal import log
|
|
||||||
from openpilot.common.api import Api
|
|
||||||
from openpilot.common.numpy_fast import interp
|
|
||||||
from openpilot.common.params import Params
|
|
||||||
from openpilot.common.realtime import Ratekeeper
|
|
||||||
from openpilot.selfdrive.navd.helpers import (Coordinate, coordinate_from_param,
|
|
||||||
distance_along_geometry, maxspeed_to_ms,
|
|
||||||
minimum_distance,
|
|
||||||
parse_banner_instructions)
|
|
||||||
from openpilot.common.swaglog import cloudlog
|
|
||||||
|
|
||||||
REROUTE_DISTANCE = 25
|
|
||||||
MANEUVER_TRANSITION_THRESHOLD = 10
|
|
||||||
REROUTE_COUNTER_MIN = 3
|
|
||||||
|
|
||||||
|
|
||||||
class RouteEngine:
|
|
||||||
def __init__(self, sm, pm):
|
|
||||||
self.sm = sm
|
|
||||||
self.pm = pm
|
|
||||||
|
|
||||||
self.params = Params()
|
|
||||||
self.params_memory = Params("/dev/shm/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.localizer_valid = 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
|
|
||||||
|
|
||||||
self.reroute_counter = 0
|
|
||||||
|
|
||||||
if "MAPBOX_TOKEN" in os.environ:
|
|
||||||
self.mapbox_token = os.environ["MAPBOX_TOKEN"]
|
|
||||||
self.mapbox_host = "https://api.mapbox.com"
|
|
||||||
elif self.params.get_int("PrimeType") == 0:
|
|
||||||
self.mapbox_token = self.params.get("MapboxPublicKey", encoding='utf8')
|
|
||||||
self.mapbox_host = "https://api.mapbox.com"
|
|
||||||
else:
|
|
||||||
try:
|
|
||||||
self.mapbox_token = Api(self.params.get("DongleId", encoding='utf8')).get_token(expiry_hours=4 * 7 * 24)
|
|
||||||
except FileNotFoundError:
|
|
||||||
cloudlog.exception("Failed to generate mapbox token due to missing private key. Ensure device is registered.")
|
|
||||||
self.mapbox_token = ""
|
|
||||||
self.mapbox_host = "https://maps.comma.ai"
|
|
||||||
|
|
||||||
# FrogPilot variables
|
|
||||||
self.stop_coord = []
|
|
||||||
self.stop_signal = []
|
|
||||||
|
|
||||||
self.approaching_intersection = False
|
|
||||||
self.approaching_turn = False
|
|
||||||
|
|
||||||
self.update_frogpilot_params()
|
|
||||||
|
|
||||||
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()
|
|
||||||
try:
|
|
||||||
self.recompute_route()
|
|
||||||
self.send_instruction()
|
|
||||||
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
|
|
||||||
|
|
||||||
self.localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
|
|
||||||
|
|
||||||
if self.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()
|
|
||||||
self.reset_recompute_limits()
|
|
||||||
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)
|
|
||||||
self.reroute_counter = 0
|
|
||||||
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
|
|
||||||
|
|
||||||
lang = self.params.get('LanguageSetting', encoding='utf8')
|
|
||||||
if lang is not None:
|
|
||||||
lang = lang.replace('main_', '')
|
|
||||||
|
|
||||||
params = {
|
|
||||||
'access_token': self.mapbox_token,
|
|
||||||
'annotations': 'maxspeed',
|
|
||||||
'geometries': 'geojson',
|
|
||||||
'overview': 'full',
|
|
||||||
'steps': 'true',
|
|
||||||
'banner_instructions': 'true',
|
|
||||||
'alternatives': 'false',
|
|
||||||
'language': lang,
|
|
||||||
}
|
|
||||||
|
|
||||||
# TODO: move waypoints into NavDestination param?
|
|
||||||
waypoints = self.params.get('NavDestinationWaypoints', encoding='utf8')
|
|
||||||
waypoint_coords = []
|
|
||||||
if waypoints is not None and len(waypoints) > 0:
|
|
||||||
waypoint_coords = json.loads(waypoints)
|
|
||||||
|
|
||||||
coords = [
|
|
||||||
(self.last_position.longitude, self.last_position.latitude),
|
|
||||||
*waypoint_coords,
|
|
||||||
(destination.longitude, destination.latitude)
|
|
||||||
]
|
|
||||||
params['waypoints'] = f'0;{len(coords)-1}'
|
|
||||||
if self.last_bearing is not None:
|
|
||||||
params['bearings'] = f"{(self.last_bearing + 360) % 360:.0f},90" + (';'*(len(coords)-1))
|
|
||||||
|
|
||||||
coords_str = ';'.join([f'{lon},{lat}' for lon, lat in coords])
|
|
||||||
url = self.mapbox_host + '/directions/v5/mapbox/driving-traffic/' + coords_str
|
|
||||||
try:
|
|
||||||
resp = requests.get(url, params=params, timeout=10)
|
|
||||||
if resp.status_code != 200:
|
|
||||||
cloudlog.event("API request failed", status_code=resp.status_code, text=resp.text, error=True)
|
|
||||||
resp.raise_for_status()
|
|
||||||
|
|
||||||
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):
|
|
||||||
return [remove_keys(item, keys_to_remove) for item in obj]
|
|
||||||
elif isinstance(obj, dict):
|
|
||||||
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')
|
|
||||||
first_route['Destination'] = place_name
|
|
||||||
first_route['Metric'] = self.params.get_bool("IsMetric")
|
|
||||||
self.r3['CurrentStep'] = 0
|
|
||||||
self.r3['uuid'] = self.r2['uuid']
|
|
||||||
except json.JSONDecodeError as e:
|
|
||||||
print(f"Error decoding JSON: {e}")
|
|
||||||
|
|
||||||
# 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:
|
|
||||||
json.dump(self.r3, json_file, indent=4)
|
|
||||||
|
|
||||||
if len(r['routes']):
|
|
||||||
self.route = r['routes'][0]['legs'][0]['steps']
|
|
||||||
self.route_geometry = []
|
|
||||||
|
|
||||||
# Iterate through the steps in self.route to find "stop_sign" and "traffic_light"
|
|
||||||
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:
|
|
||||||
self.stop_signal.append(intersection["geometry_index"])
|
|
||||||
self.stop_coord.append(Coordinate.from_mapbox_tuple(intersection["location"]))
|
|
||||||
|
|
||||||
maxspeed_idx = 0
|
|
||||||
maxspeeds = r['routes'][0]['legs'][0]['annotation']['maxspeed']
|
|
||||||
|
|
||||||
# Convert coordinates
|
|
||||||
for step in self.route:
|
|
||||||
coords = []
|
|
||||||
|
|
||||||
for c in step['geometry']['coordinates']:
|
|
||||||
coord = Coordinate.from_mapbox_tuple(c)
|
|
||||||
|
|
||||||
# Last step does not have maxspeed
|
|
||||||
if (maxspeed_idx < len(maxspeeds)):
|
|
||||||
maxspeed = maxspeeds[maxspeed_idx]
|
|
||||||
if ('unknown' not in maxspeed) and ('none' not in maxspeed):
|
|
||||||
coord.annotations['maxspeed'] = maxspeed_to_ms(maxspeed)
|
|
||||||
|
|
||||||
coords.append(coord)
|
|
||||||
maxspeed_idx += 1
|
|
||||||
|
|
||||||
self.route_geometry.append(coords)
|
|
||||||
maxspeed_idx -= 1 # Every segment ends with the same coordinate as the start of the next
|
|
||||||
|
|
||||||
self.step_idx = 0
|
|
||||||
else:
|
|
||||||
cloudlog.warning("Got empty route response")
|
|
||||||
self.clear_route()
|
|
||||||
|
|
||||||
# clear waypoints to avoid a re-route including past waypoints
|
|
||||||
# TODO: only clear once we're past a waypoint
|
|
||||||
self.params.remove('NavDestinationWaypoints')
|
|
||||||
|
|
||||||
except requests.exceptions.RequestException:
|
|
||||||
cloudlog.exception("failed to get route")
|
|
||||||
self.clear_route()
|
|
||||||
|
|
||||||
self.send_route()
|
|
||||||
|
|
||||||
def send_instruction(self):
|
|
||||||
msg = messaging.new_message('navInstruction', valid=True)
|
|
||||||
|
|
||||||
if self.step_idx is None:
|
|
||||||
msg.valid = False
|
|
||||||
self.params_memory.put_float("NavSpeedLimit", 0)
|
|
||||||
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
|
|
||||||
|
|
||||||
# Banner instructions are for the following maneuver step, don't use empty last step
|
|
||||||
banner_step = step
|
|
||||||
if not len(banner_step['bannerInstructions']) and self.step_idx == len(self.route) - 1:
|
|
||||||
banner_step = self.route[max(self.step_idx - 1, 0)]
|
|
||||||
|
|
||||||
# Current instruction
|
|
||||||
msg.navInstruction.maneuverDistance = distance_to_maneuver_along_geometry
|
|
||||||
instruction = parse_banner_instructions(banner_step['bannerInstructions'], distance_to_maneuver_along_geometry)
|
|
||||||
if instruction is not None:
|
|
||||||
for k,v in instruction.items():
|
|
||||||
setattr(msg.navInstruction, k, v)
|
|
||||||
|
|
||||||
# All instructions
|
|
||||||
maneuvers = []
|
|
||||||
for i, step_i in enumerate(self.route):
|
|
||||||
if i < self.step_idx:
|
|
||||||
distance_to_maneuver = -sum(self.route[j]['distance'] for j in range(i+1, self.step_idx)) - along_geometry
|
|
||||||
elif i == self.step_idx:
|
|
||||||
distance_to_maneuver = distance_to_maneuver_along_geometry
|
|
||||||
else:
|
|
||||||
distance_to_maneuver = distance_to_maneuver_along_geometry + sum(self.route[j]['distance'] for j in range(self.step_idx+1, i+1))
|
|
||||||
|
|
||||||
instruction = parse_banner_instructions(step_i['bannerInstructions'], distance_to_maneuver)
|
|
||||||
if instruction is None:
|
|
||||||
continue
|
|
||||||
maneuver = {'distance': distance_to_maneuver}
|
|
||||||
if 'maneuverType' in instruction:
|
|
||||||
maneuver['type'] = instruction['maneuverType']
|
|
||||||
if 'maneuverModifier' in instruction:
|
|
||||||
maneuver['modifier'] = instruction['maneuverModifier']
|
|
||||||
maneuvers.append(maneuver)
|
|
||||||
|
|
||||||
msg.navInstruction.allManeuvers = maneuvers
|
|
||||||
|
|
||||||
# Compute total remaining time and distance
|
|
||||||
remaining = 1.0 - along_geometry / max(step['distance'], 1)
|
|
||||||
total_distance = step['distance'] * remaining
|
|
||||||
total_time = step['duration'] * remaining
|
|
||||||
|
|
||||||
if step['duration_typical'] is None:
|
|
||||||
total_time_typical = total_time
|
|
||||||
else:
|
|
||||||
total_time_typical = step['duration_typical'] * remaining
|
|
||||||
|
|
||||||
# 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']
|
|
||||||
if self.route[i]['duration_typical'] is None:
|
|
||||||
total_time_typical += self.route[i]['duration']
|
|
||||||
else:
|
|
||||||
total_time_typical += self.route[i]['duration_typical']
|
|
||||||
|
|
||||||
msg.navInstruction.distanceRemaining = total_distance
|
|
||||||
msg.navInstruction.timeRemaining = total_time
|
|
||||||
msg.navInstruction.timeRemainingTypical = total_time_typical
|
|
||||||
|
|
||||||
# Speed limit
|
|
||||||
closest_idx, closest = min(enumerate(geometry), key=lambda p: p[1].distance_to(self.last_position))
|
|
||||||
if closest_idx > 0:
|
|
||||||
# If we are not past the closest point, show previous
|
|
||||||
if along_geometry < distance_along_geometry(geometry, geometry[closest_idx]):
|
|
||||||
closest = geometry[closest_idx - 1]
|
|
||||||
|
|
||||||
if ('maxspeed' in closest.annotations) and self.localizer_valid:
|
|
||||||
msg.navInstruction.speedLimit = closest.annotations['maxspeed']
|
|
||||||
self.params_memory.put_float("NavSpeedLimit", closest.annotations['maxspeed'])
|
|
||||||
if not self.localizer_valid or ('maxspeed' not in closest.annotations):
|
|
||||||
self.params_memory.put_float("NavSpeedLimit", 0)
|
|
||||||
|
|
||||||
# Speed limit sign type
|
|
||||||
if 'speedLimitSign' in step:
|
|
||||||
if step['speedLimitSign'] == 'mutcd':
|
|
||||||
msg.navInstruction.speedLimitSign = log.NavInstruction.SpeedLimitSign.mutcd
|
|
||||||
elif step['speedLimitSign'] == 'vienna':
|
|
||||||
msg.navInstruction.speedLimitSign = log.NavInstruction.SpeedLimitSign.vienna
|
|
||||||
|
|
||||||
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.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
|
|
||||||
# Write the modified JSON data back to the file
|
|
||||||
with open('CurrentStep.json', 'w') as json_file:
|
|
||||||
json.dump(self.r3, json_file, indent=4)
|
|
||||||
else:
|
|
||||||
cloudlog.warning("Destination reached")
|
|
||||||
|
|
||||||
# Clear route if driving away from destination
|
|
||||||
dist = self.nav_destination.distance_to(self.last_position)
|
|
||||||
if dist > REROUTE_DISTANCE:
|
|
||||||
self.params.remove("NavDestination")
|
|
||||||
self.clear_route()
|
|
||||||
|
|
||||||
# 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:
|
|
||||||
closest_condition_index = min(closest_condition_indices, key=lambda idx: abs(closest_idx - idx))
|
|
||||||
index = self.stop_signal.index(closest_condition_index)
|
|
||||||
|
|
||||||
# Calculate the distance to the stopSign or trafficLight
|
|
||||||
distance_to_condition = self.last_position.distance_to(self.stop_coord[index])
|
|
||||||
self.approaching_intersection = self.conditional_navigation_intersections and distance_to_condition < max((seconds_to_stop * v_ego), 25)
|
|
||||||
else:
|
|
||||||
self.approaching_intersection = False # No more stopSign or trafficLight in array
|
|
||||||
|
|
||||||
# Determine if NoO distance to maneuver is upcoming
|
|
||||||
self.approaching_turn = self.conditional_navigation_turns and distance_to_maneuver_along_geometry < max((seconds_to_stop * v_ego), 25)
|
|
||||||
else:
|
|
||||||
self.approaching_intersection = False
|
|
||||||
self.approaching_turn = False
|
|
||||||
|
|
||||||
frogpilot_plan_send = messaging.new_message('frogpilotNavigation')
|
|
||||||
frogpilotNavigation = frogpilot_plan_send.frogpilotNavigation
|
|
||||||
|
|
||||||
frogpilotNavigation.approachingIntersection = self.approaching_intersection
|
|
||||||
frogpilotNavigation.approachingTurn = self.approaching_turn
|
|
||||||
|
|
||||||
self.pm.send('frogpilotNavigation', frogpilot_plan_send)
|
|
||||||
|
|
||||||
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', valid=True)
|
|
||||||
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 reset_recompute_limits(self):
|
|
||||||
self.recompute_backoff = 0
|
|
||||||
self.recompute_countdown = 0
|
|
||||||
|
|
||||||
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))
|
|
||||||
|
|
||||||
if min_d > REROUTE_DISTANCE:
|
|
||||||
self.reroute_counter += 1
|
|
||||||
else:
|
|
||||||
self.reroute_counter = 0
|
|
||||||
return self.reroute_counter > REROUTE_COUNTER_MIN
|
|
||||||
# TODO: Check for going wrong way in segment
|
|
||||||
|
|
||||||
def update_frogpilot_params(self):
|
|
||||||
self.conditional_navigation = self.params.get_bool("CENavigation")
|
|
||||||
self.conditional_navigation_intersections = self.conditional_navigation and self.params.get_bool("CENavigationIntersections")
|
|
||||||
self.conditional_navigation_turns = self.conditional_navigation and self.params.get_bool("CENavigationTurns")
|
|
||||||
|
|
||||||
def main():
|
|
||||||
pm = messaging.PubMaster(['navInstruction', 'navRoute', 'frogpilotNavigation'])
|
|
||||||
sm = messaging.SubMaster(['carState', 'liveLocationKalman', 'managerState'])
|
|
||||||
|
|
||||||
rk = Ratekeeper(1.0)
|
|
||||||
route_engine = RouteEngine(sm, pm)
|
|
||||||
while True:
|
|
||||||
route_engine.update()
|
|
||||||
rk.keep_time()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
main()
|
|
||||||
@@ -1,33 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
import json
|
|
||||||
import sys
|
|
||||||
|
|
||||||
from openpilot.common.params import Params
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
params = Params()
|
|
||||||
|
|
||||||
# set from google maps url
|
|
||||||
if len(sys.argv) > 1:
|
|
||||||
coords = sys.argv[1].split("/@")[-1].split("/")[0].split(",")
|
|
||||||
dest = {
|
|
||||||
"latitude": float(coords[0]),
|
|
||||||
"longitude": float(coords[1])
|
|
||||||
}
|
|
||||||
params.put("NavDestination", json.dumps(dest))
|
|
||||||
params.remove("NavDestinationWaypoints")
|
|
||||||
else:
|
|
||||||
print("Setting to Taco Bell")
|
|
||||||
dest = {
|
|
||||||
"latitude": 32.71160109904473,
|
|
||||||
"longitude": -117.12556569985693,
|
|
||||||
}
|
|
||||||
params.put("NavDestination", json.dumps(dest))
|
|
||||||
|
|
||||||
waypoints = [
|
|
||||||
(-117.16020713111648, 32.71997612490662),
|
|
||||||
]
|
|
||||||
params.put("NavDestinationWaypoints", json.dumps(waypoints))
|
|
||||||
|
|
||||||
print(dest)
|
|
||||||
print(waypoints)
|
|
||||||
File diff suppressed because one or more lines are too long
@@ -1,216 +0,0 @@
|
|||||||
#!/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()
|
|
||||||
@@ -1,63 +0,0 @@
|
|||||||
#!/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()
|
|
||||||
Reference in New Issue
Block a user