brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compile FrogPilot

Browse Source
This commit is contained in:
FrogAi
2024-03-10 20:59:55 -07:00
parent 69e7feaf01
commit f1acd339d7
1485 changed files with 426154 additions and 398339 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
selfdrive/navd/SConscript
View File

@@ -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)

BIN
selfdrive/navd/libmaprender.so Normal file
View File

Binary file not shown.

29
selfdrive/navd/main.cc
View File

@@ -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();
}

334
selfdrive/navd/map_renderer.cc
View File

@@ -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;
}
}

59
selfdrive/navd/map_renderer.h
View File

@@ -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();
};

BIN
selfdrive/navd/mapsd Executable file
View File

Binary file not shown.

216
selfdrive/navd/tests/test_map_renderer.py
View File

@@ -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()

63
selfdrive/navd/tests/test_navd.py
View File

@@ -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()