Move navigation into separate daemon (#22767)
* Add navd folder * prints route instructions to console * broadcast NavInstuction without lanes * show basic instructions and eta * parse out lane info * use swaglog * clip distance on ui side * draw lane directions * show route * add to process config * add to release files * small cleanup * show route without gps * pop open map on initial route * fix error messages around no gps * done * make persistent process * handle end of route * clear route on offroad * only one timer * fix layout hacks * explicit rendering at 20Hzpull/22793/head
Willem Melching
4 years ago
committed by
GitHub
parent
b7394c6171
commit
bef686f275
15 changed files with 567 additions and 334 deletions
@ -0,0 +1 @@ |
||||
_navd |
||||
@ -0,0 +1,36 @@ |
||||
#include <QApplication> |
||||
#include <QCommandLineParser> |
||||
#include <QDebug> |
||||
#include <QThread> |
||||
#include <csignal> |
||||
|
||||
#include "selfdrive/ui/qt/util.h" |
||||
#include "selfdrive/ui/navd/route_engine.h" |
||||
|
||||
RouteEngine* route_engine = nullptr; |
||||
|
||||
void sigHandler(int s) { |
||||
qInfo() << "Shutting down"; |
||||
std::signal(s, SIG_DFL); |
||||
|
||||
qApp->quit(); |
||||
} |
||||
|
||||
|
||||
int main(int argc, char *argv[]) { |
||||
qInstallMessageHandler(swagLogMessageHandler); |
||||
|
||||
QApplication app(argc, argv); |
||||
std::signal(SIGINT, sigHandler); |
||||
std::signal(SIGTERM, sigHandler); |
||||
|
||||
QCommandLineParser parser; |
||||
parser.setApplicationDescription("Navigation server. Runs stand-alone, or using pre-computer route"); |
||||
parser.addHelpOption(); |
||||
parser.process(app); |
||||
const QStringList args = parser.positionalArguments(); |
||||
|
||||
route_engine = new RouteEngine(); |
||||
|
||||
return app.exec(); |
||||
} |
||||
@ -0,0 +1,4 @@ |
||||
#!/bin/sh |
||||
cd "$(dirname "$0")" |
||||
export QT_PLUGIN_PATH="../../../third_party/qt-plugins/$(uname -m)" |
||||
exec ./_navd |
||||
@ -0,0 +1,323 @@ |
||||
#include "selfdrive/ui/navd/route_engine.h" |
||||
|
||||
#include <QDebug> |
||||
|
||||
#include "selfdrive/ui/qt/maps/map.h" |
||||
#include "selfdrive/ui/qt/maps/map_helpers.h" |
||||
#include "selfdrive/ui/qt/api.h" |
||||
|
||||
#include "selfdrive/common/params.h" |
||||
|
||||
const qreal REROUTE_DISTANCE = 25; |
||||
const float MANEUVER_TRANSITION_THRESHOLD = 10; |
||||
|
||||
static float get_time_typical(const QGeoRouteSegment &segment) { |
||||
auto maneuver = segment.maneuver(); |
||||
auto attrs = maneuver.extendedAttributes(); |
||||
return attrs.contains("mapbox.duration_typical") ? attrs["mapbox.duration_typical"].toDouble() : segment.travelTime(); |
||||
} |
||||
|
||||
static cereal::NavInstruction::Direction string_to_direction(QString d) { |
||||
if (d.contains("left")) { |
||||
return cereal::NavInstruction::Direction::LEFT; |
||||
} else if (d.contains("right")) { |
||||
return cereal::NavInstruction::Direction::RIGHT; |
||||
} else if (d.contains("straight")) { |
||||
return cereal::NavInstruction::Direction::STRAIGHT; |
||||
} |
||||
|
||||
return cereal::NavInstruction::Direction::NONE; |
||||
} |
||||
|
||||
static void parse_banner(cereal::NavInstruction::Builder &instruction, const QMap<QString, QVariant> &banner, bool full) { |
||||
QString primary_str, secondary_str; |
||||
|
||||
auto p = banner["primary"].toMap(); |
||||
primary_str += p["text"].toString(); |
||||
|
||||
instruction.setShowFull(full); |
||||
|
||||
if (p.contains("type")) { |
||||
instruction.setManeuverType(p["type"].toString().toStdString()); |
||||
} |
||||
|
||||
if (p.contains("modifier")) { |
||||
instruction.setManeuverModifier(p["modifier"].toString().toStdString()); |
||||
} |
||||
|
||||
if (banner.contains("secondary")) { |
||||
auto s = banner["secondary"].toMap(); |
||||
secondary_str += s["text"].toString(); |
||||
} |
||||
|
||||
instruction.setManeuverPrimaryText(primary_str.toStdString()); |
||||
instruction.setManeuverSecondaryText(secondary_str.toStdString()); |
||||
|
||||
if (banner.contains("sub")) { |
||||
auto s = banner["sub"].toMap(); |
||||
auto components = s["components"].toList(); |
||||
|
||||
size_t num_lanes = 0; |
||||
for (auto &c : components) { |
||||
auto cc = c.toMap(); |
||||
if (cc["type"].toString() == "lane") { |
||||
num_lanes += 1; |
||||
} |
||||
} |
||||
|
||||
auto lanes = instruction.initLanes(num_lanes); |
||||
|
||||
size_t i = 0; |
||||
for (auto &c : components) { |
||||
auto cc = c.toMap(); |
||||
if (cc["type"].toString() == "lane") { |
||||
auto lane = lanes[i]; |
||||
lane.setActive(cc["active"].toBool()); |
||||
|
||||
if (cc.contains("active_direction")) { |
||||
lane.setActiveDirection(string_to_direction(cc["active_direction"].toString())); |
||||
} |
||||
|
||||
auto directions = lane.initDirections(cc["directions"].toList().size()); |
||||
|
||||
size_t j = 0; |
||||
for (auto &dir : cc["directions"].toList()) { |
||||
directions.set(j, string_to_direction(dir.toString())); |
||||
j++; |
||||
} |
||||
|
||||
|
||||
i++; |
||||
} |
||||
} |
||||
} |
||||
|
||||
} |
||||
|
||||
RouteEngine::RouteEngine() { |
||||
sm = new SubMaster({"liveLocationKalman"}); |
||||
pm = new PubMaster({"navInstruction", "navRoute"}); |
||||
|
||||
// Timers
|
||||
timer = new QTimer(this); |
||||
QObject::connect(timer, SIGNAL(timeout()), this, SLOT(timerUpdate())); |
||||
timer->start(1000); |
||||
|
||||
// Build routing engine
|
||||
QVariantMap parameters; |
||||
QString token = MAPBOX_TOKEN.isEmpty() ? CommaApi::create_jwt({}, 4 * 7 * 24 * 3600) : MAPBOX_TOKEN; |
||||
parameters["mapbox.access_token"] = token; |
||||
parameters["mapbox.directions_api_url"] = MAPS_HOST + "/directions/v5/mapbox/"; |
||||
|
||||
geoservice_provider = new QGeoServiceProvider("mapbox", parameters); |
||||
routing_manager = geoservice_provider->routingManager(); |
||||
if (routing_manager == nullptr) { |
||||
qWarning() << geoservice_provider->errorString(); |
||||
assert(routing_manager); |
||||
} |
||||
QObject::connect(routing_manager, &QGeoRoutingManager::finished, this, &RouteEngine::routeCalculated); |
||||
|
||||
// Get last gps position from params
|
||||
auto last_gps_position = coordinate_from_param("LastGPSPosition"); |
||||
if (last_gps_position) { |
||||
last_position = *last_gps_position; |
||||
} |
||||
} |
||||
|
||||
void RouteEngine::timerUpdate() { |
||||
sm->update(0); |
||||
if (!sm->updated("liveLocationKalman")) { |
||||
return; |
||||
} |
||||
|
||||
auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman(); |
||||
gps_ok = location.getGpsOK(); |
||||
|
||||
localizer_valid = location.getStatus() == cereal::LiveLocationKalman::Status::VALID; |
||||
|
||||
if (localizer_valid) { |
||||
auto pos = location.getPositionGeodetic(); |
||||
auto orientation = location.getCalibratedOrientationNED(); |
||||
|
||||
last_bearing = RAD2DEG(orientation.getValue()[2]); |
||||
last_position = QMapbox::Coordinate(pos.getValue()[0], pos.getValue()[1]); |
||||
} |
||||
|
||||
recomputeRoute(); |
||||
|
||||
MessageBuilder msg; |
||||
cereal::Event::Builder evt = msg.initEvent(segment.isValid()); |
||||
cereal::NavInstruction::Builder instruction = evt.initNavInstruction(); |
||||
|
||||
// Show route instructions
|
||||
if (segment.isValid()) { |
||||
auto cur_maneuver = segment.maneuver(); |
||||
auto attrs = cur_maneuver.extendedAttributes(); |
||||
if (cur_maneuver.isValid() && attrs.contains("mapbox.banner_instructions")) { |
||||
float along_geometry = distance_along_geometry(segment.path(), to_QGeoCoordinate(*last_position)); |
||||
float distance_to_maneuver_along_geometry = segment.distance() - along_geometry; |
||||
|
||||
auto banners = attrs["mapbox.banner_instructions"].toList(); |
||||
if (banners.size()) { |
||||
auto banner = banners[0].toMap(); |
||||
|
||||
for (auto &b : banners) { |
||||
auto bb = b.toMap(); |
||||
if (distance_to_maneuver_along_geometry < bb["distance_along_geometry"].toDouble()) { |
||||
banner = bb; |
||||
} |
||||
} |
||||
|
||||
instruction.setManeuverDistance(distance_to_maneuver_along_geometry); |
||||
parse_banner(instruction, banner, distance_to_maneuver_along_geometry < banner["distance_along_geometry"].toDouble()); |
||||
|
||||
// ETA
|
||||
float progress = distance_along_geometry(segment.path(), to_QGeoCoordinate(*last_position)) / segment.distance(); |
||||
float total_distance = segment.distance() * (1.0 - progress); |
||||
float total_time = segment.travelTime() * (1.0 - progress); |
||||
float total_time_typical = get_time_typical(segment) * (1.0 - progress); |
||||
|
||||
auto s = segment.nextRouteSegment(); |
||||
while (s.isValid()) { |
||||
total_distance += s.distance(); |
||||
total_time += s.travelTime(); |
||||
total_time_typical += get_time_typical(s); |
||||
|
||||
s = s.nextRouteSegment(); |
||||
} |
||||
instruction.setTimeRemaining(total_time); |
||||
instruction.setTimeRemainingTypical(total_time_typical); |
||||
instruction.setDistanceRemaining(total_distance); |
||||
} |
||||
|
||||
// Transition to next route segment
|
||||
if (distance_to_maneuver_along_geometry < -MANEUVER_TRANSITION_THRESHOLD) { |
||||
auto next_segment = segment.nextRouteSegment(); |
||||
if (next_segment.isValid()) { |
||||
segment = next_segment; |
||||
|
||||
recompute_backoff = std::max(0, recompute_backoff - 1); |
||||
recompute_countdown = 0; |
||||
} else { |
||||
qWarning() << "Destination reached"; |
||||
Params().remove("NavDestination"); |
||||
|
||||
// Clear route if driving away from destination
|
||||
float d = segment.maneuver().position().distanceTo(to_QGeoCoordinate(*last_position)); |
||||
if (d > REROUTE_DISTANCE) { |
||||
clearRoute(); |
||||
} |
||||
} |
||||
} |
||||
} |
||||
} |
||||
|
||||
pm->send("navInstruction", msg); |
||||
} |
||||
|
||||
void RouteEngine::clearRoute() { |
||||
segment = QGeoRouteSegment(); |
||||
nav_destination = QMapbox::Coordinate(); |
||||
} |
||||
|
||||
bool RouteEngine::shouldRecompute() { |
||||
if (!segment.isValid()) { |
||||
return true; |
||||
} |
||||
|
||||
// Don't recompute in last segment, assume destination is reached
|
||||
if (!segment.nextRouteSegment().isValid()) { |
||||
return false; |
||||
} |
||||
|
||||
// Compute closest distance to all line segments in the current path
|
||||
float min_d = REROUTE_DISTANCE + 1; |
||||
auto path = segment.path(); |
||||
auto cur = to_QGeoCoordinate(*last_position); |
||||
for (size_t i = 0; i < path.size() - 1; i++) { |
||||
auto a = path[i]; |
||||
auto b = path[i+1]; |
||||
if (a.distanceTo(b) < 1.0) { |
||||
continue; |
||||
} |
||||
min_d = std::min(min_d, minimum_distance(a, b, cur)); |
||||
} |
||||
return min_d > REROUTE_DISTANCE; |
||||
|
||||
// TODO: Check for going wrong way in segment
|
||||
} |
||||
|
||||
void RouteEngine::recomputeRoute() { |
||||
auto new_destination = coordinate_from_param("NavDestination"); |
||||
if (!new_destination) { |
||||
clearRoute(); |
||||
return; |
||||
} |
||||
|
||||
bool should_recompute = shouldRecompute(); |
||||
if (*new_destination != nav_destination) { |
||||
qWarning() << "Got new destination from NavDestination param" << *new_destination; |
||||
should_recompute = true; |
||||
} |
||||
|
||||
if (!gps_ok && segment.isValid()) return; // Don't recompute when gps drifts in tunnels
|
||||
|
||||
if (recompute_countdown == 0 && should_recompute) { |
||||
recompute_countdown = std::pow(2, recompute_backoff); |
||||
recompute_backoff = std::min(7, recompute_backoff + 1); |
||||
calculateRoute(*new_destination); |
||||
} else { |
||||
recompute_countdown = std::max(0, recompute_countdown - 1); |
||||
} |
||||
} |
||||
|
||||
void RouteEngine::calculateRoute(QMapbox::Coordinate destination) { |
||||
qWarning() << "Calculating route" << *last_position << "->" << destination; |
||||
|
||||
nav_destination = destination; |
||||
QGeoRouteRequest request(to_QGeoCoordinate(*last_position), to_QGeoCoordinate(destination)); |
||||
request.setFeatureWeight(QGeoRouteRequest::TrafficFeature, QGeoRouteRequest::AvoidFeatureWeight); |
||||
|
||||
if (last_bearing) { |
||||
QVariantMap params; |
||||
int bearing = ((int)(*last_bearing) + 360) % 360; |
||||
params["bearing"] = bearing; |
||||
request.setWaypointsMetadata({params}); |
||||
} |
||||
|
||||
routing_manager->calculateRoute(request); |
||||
} |
||||
|
||||
void RouteEngine::routeCalculated(QGeoRouteReply *reply) { |
||||
MessageBuilder msg; |
||||
cereal::Event::Builder evt = msg.initEvent(); |
||||
cereal::NavRoute::Builder nav_route = evt.initNavRoute(); |
||||
|
||||
if (reply->error() == QGeoRouteReply::NoError) { |
||||
if (reply->routes().size() != 0) { |
||||
qWarning() << "Got route response"; |
||||
|
||||
route = reply->routes().at(0); |
||||
segment = route.firstRouteSegment(); |
||||
|
||||
auto path = route.path(); |
||||
auto coordinates = nav_route.initCoordinates(path.size()); |
||||
|
||||
size_t i = 0; |
||||
for (auto const &c : route.path()) { |
||||
coordinates[i].setLatitude(c.latitude()); |
||||
coordinates[i].setLongitude(c.longitude()); |
||||
i++; |
||||
} |
||||
|
||||
} else { |
||||
qWarning() << "Got empty route response"; |
||||
} |
||||
} else { |
||||
qWarning() << "Got error in route reply" << reply->errorString(); |
||||
} |
||||
|
||||
pm->send("navRoute", msg); |
||||
|
||||
reply->deleteLater(); |
||||
} |
||||
@ -0,0 +1,50 @@ |
||||
#pragma once |
||||
|
||||
#include <QThread> |
||||
#include <QGeoCoordinate> |
||||
#include <QGeoManeuver> |
||||
#include <QGeoRouteRequest> |
||||
#include <QGeoRouteSegment> |
||||
#include <QGeoRoutingManager> |
||||
#include <QGeoServiceProvider> |
||||
#include <QTimer> |
||||
#include <QMapboxGL> |
||||
|
||||
#include "cereal/messaging/messaging.h" |
||||
|
||||
class RouteEngine : public QObject { |
||||
Q_OBJECT |
||||
|
||||
public: |
||||
RouteEngine(); |
||||
|
||||
SubMaster *sm; |
||||
PubMaster *pm; |
||||
|
||||
QTimer* timer; |
||||
|
||||
// Route
|
||||
bool gps_ok = false; |
||||
QGeoServiceProvider *geoservice_provider; |
||||
QGeoRoutingManager *routing_manager; |
||||
QGeoRoute route; |
||||
QGeoRouteSegment segment; |
||||
QMapbox::Coordinate nav_destination; |
||||
|
||||
// Position
|
||||
std::optional<QMapbox::Coordinate> last_position; |
||||
std::optional<float> last_bearing; |
||||
bool localizer_valid = false; |
||||
|
||||
// Route recompute
|
||||
int recompute_backoff = 0; |
||||
int recompute_countdown = 0; |
||||
void calculateRoute(QMapbox::Coordinate destination); |
||||
void clearRoute(); |
||||
bool shouldRecompute(); |
||||
|
||||
private slots: |
||||
void timerUpdate(); |
||||
void routeCalculated(QGeoRouteReply *reply); |
||||
void recomputeRoute(); |
||||
}; |
||||
Loading…
Reference in new issue