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/longitudinal_mpc_lib/SConscript'])
|
||||
SConscript(['locationd/SConscript'])
|
||||
SConscript(['navd/SConscript'])
|
||||
SConscript(['modeld/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