diff --git a/selfdrive/ui/navd/route_engine.cc b/selfdrive/ui/navd/route_engine.cc index 09ac84d9a5..30d070dc73 100644 --- a/selfdrive/ui/navd/route_engine.cc +++ b/selfdrive/ui/navd/route_engine.cc @@ -95,7 +95,7 @@ static void parse_banner(cereal::NavInstruction::Builder &instruction, const QMa } RouteEngine::RouteEngine() { - sm = new SubMaster({"liveLocationKalman"}); + sm = new SubMaster({"liveLocationKalman", "managerState"}); pm = new PubMaster({"navInstruction", "navRoute"}); // Timers @@ -130,6 +130,18 @@ void RouteEngine::timerUpdate() { return; } + if (sm->updated("managerState")) { + for (auto const &p : (*sm)["managerState"].getManagerState().getProcesses()) { + if (p.getName() == "ui" && p.getRunning()) { + if (ui_pid && *ui_pid != p.getPid()){ + qWarning() << "UI restarting, sending route"; + QTimer::singleShot(5000, this, &RouteEngine::sendRoute); + } + ui_pid = p.getPid(); + } + } + } + auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman(); auto pos = location.getPositionGeodetic(); auto orientation = location.getCalibratedOrientationNED(); @@ -216,6 +228,7 @@ void RouteEngine::timerUpdate() { } void RouteEngine::clearRoute() { + route = QGeoRoute(); segment = QGeoRouteSegment(); nav_destination = QMapbox::Coordinate(); } @@ -289,27 +302,12 @@ void RouteEngine::calculateRoute(QMapbox::Coordinate destination) { } 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"; } @@ -317,7 +315,25 @@ void RouteEngine::routeCalculated(QGeoRouteReply *reply) { qWarning() << "Got error in route reply" << reply->errorString(); } - pm->send("navRoute", msg); + sendRoute(); reply->deleteLater(); } + +void RouteEngine::sendRoute() { + MessageBuilder msg; + cereal::Event::Builder evt = msg.initEvent(); + cereal::NavRoute::Builder nav_route = evt.initNavRoute(); + + 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++; + } + + pm->send("navRoute", msg); +} diff --git a/selfdrive/ui/navd/route_engine.h b/selfdrive/ui/navd/route_engine.h index af69b5c76d..5b8de698a9 100644 --- a/selfdrive/ui/navd/route_engine.h +++ b/selfdrive/ui/navd/route_engine.h @@ -23,6 +23,8 @@ public: QTimer* timer; + std::optional ui_pid; + // Route bool gps_ok = false; QGeoServiceProvider *geoservice_provider; @@ -47,4 +49,5 @@ private slots: void timerUpdate(); void routeCalculated(QGeoRouteReply *reply); void recomputeRoute(); + void sendRoute(); };