diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc index 87767d7e5d..4b7df98979 100644 --- a/selfdrive/ui/qt/maps/map.cc +++ b/selfdrive/ui/qt/maps/map.cc @@ -39,6 +39,7 @@ MapWindow::MapWindow(const QMapboxGLSettings &settings) : m_settings(settings) { map_instructions = new MapInstructions(this); connect(this, &MapWindow::instructionsChanged, map_instructions, &MapInstructions::updateInstructions); connect(this, &MapWindow::distanceChanged, map_instructions, &MapInstructions::updateDistance); + connect(this, &MapWindow::GPSValidChanged, map_instructions, &MapInstructions::updateGPSValid); map_instructions->setFixedWidth(width()); map_eta = new MapETA(this); @@ -121,8 +122,10 @@ void MapWindow::timerUpdate() { auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman(); gps_ok = location.getGpsOK(); - // Update map location, orientation and zoom on valid localizer output - if (location.getStatus() == cereal::LiveLocationKalman::Status::VALID) { + bool localizer_valid = location.getStatus() == cereal::LiveLocationKalman::Status::VALID; + emit GPSValidChanged(localizer_valid); + + if (localizer_valid) { auto pos = location.getPositionGeodetic(); auto orientation = location.getOrientationNED(); @@ -165,61 +168,59 @@ void MapWindow::timerUpdate() { modelPathSource["data"] = QVariant::fromValue(feature2); m_map->updateSource("modelPathSource", modelPathSource); } - } - - // 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 = std::max(0.0f, float(segment.distance()) - along_geometry); - emit distanceChanged(distance); - - m_map->setPitch(MAX_PITCH); // TODO: smooth pitching based on maneuver distance - auto banner = attrs["mapbox.banner_instructions"].toList(); - if (banner.size()) { - map_instructions->setVisible(true); - - auto banner_0 = banner[0].toMap(); - float show_at = banner_0["distance_along_geometry"].toDouble(); - emit instructionsChanged(banner_0, distance < show_at); + if (segment.isValid()) { + // Show route instructions + 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 = std::max(0.0f, float(segment.distance()) - along_geometry); + emit distanceChanged(distance); + + m_map->setPitch(MAX_PITCH); // TODO: smooth pitching based on maneuver distance + + auto banner = attrs["mapbox.banner_instructions"].toList(); + if (banner.size()) { + auto banner_0 = banner[0].toMap(); + float show_at = banner_0["distance_along_geometry"].toDouble(); + emit instructionsChanged(banner_0, distance < show_at); + } } - } - auto next_segment = segment.nextRouteSegment(); - if (next_segment.isValid()) { - auto next_maneuver = next_segment.maneuver(); - if (next_maneuver.isValid()) { - float next_maneuver_distance = next_maneuver.position().distanceTo(to_QGeoCoordinate(last_position)); - // Switch to next route segment - if (next_maneuver_distance < REROUTE_DISTANCE && next_maneuver_distance > last_maneuver_distance) { - segment = next_segment; - - recompute_backoff = std::max(0, recompute_backoff - 1); - recompute_countdown = 0; + // Handle transition to next route segment + auto next_segment = segment.nextRouteSegment(); + if (next_segment.isValid()) { + auto next_maneuver = next_segment.maneuver(); + if (next_maneuver.isValid()) { + float next_maneuver_distance = next_maneuver.position().distanceTo(to_QGeoCoordinate(*last_position)); + // Switch to next route segment + if (next_maneuver_distance < REROUTE_DISTANCE && next_maneuver_distance > last_maneuver_distance) { + segment = next_segment; + + recompute_backoff = std::max(0, recompute_backoff - 1); + recompute_countdown = 0; + } + last_maneuver_distance = next_maneuver_distance; + } + } else { + // 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(); } - last_maneuver_distance = next_maneuver_distance; } } else { - 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(); - } + map_instructions->setVisible(false); } } - } update(); - if (!segment.isValid()) { - map_instructions->setVisible(false); - } } @@ -230,7 +231,12 @@ void MapWindow::resizeGL(int w, int h) { void MapWindow::initializeGL() { m_map.reset(new QMapboxGL(nullptr, m_settings, size(), 1)); - m_map->setCoordinateZoom(last_position, MAX_ZOOM); + if (last_position) { + m_map->setCoordinateZoom(*last_position, MAX_ZOOM); + } else { + m_map->setCoordinateZoom(QMapbox::Coordinate(64.31990695292795, -149.79038934046247), MIN_ZOOM); + } + m_map->setMargins({0, 350, 0, 50}); m_map->setPitch(MIN_PITCH); m_map->setStyleUrl("mapbox://styles/pd0wm/cknuhcgvr0vs817o1akcx6pek"); // Larger fonts @@ -255,6 +261,11 @@ static float get_time_typical(const QGeoRouteSegment &segment) { void MapWindow::recomputeRoute() { + // Last position is valid if read from param or from GPS + if (!last_position) { + return; + } + bool should_recompute = shouldRecompute(); auto new_destination = coordinate_from_param("NavDestination"); @@ -286,7 +297,7 @@ void MapWindow::recomputeRoute() { void MapWindow::updateETA() { if (segment.isValid()) { - float progress = distance_along_geometry(segment.path(), to_QGeoCoordinate(last_position)) / segment.distance(); + 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); @@ -307,7 +318,7 @@ void MapWindow::updateETA() { void MapWindow::calculateRoute(QMapbox::Coordinate destination) { LOGW("calculating route"); nav_destination = destination; - QGeoRouteRequest request(to_QGeoCoordinate(last_position), to_QGeoCoordinate(destination)); + QGeoRouteRequest request(to_QGeoCoordinate(*last_position), to_QGeoCoordinate(destination)); request.setFeatureWeight(QGeoRouteRequest::TrafficFeature, QGeoRouteRequest::AvoidFeatureWeight); if (last_bearing) { @@ -362,7 +373,7 @@ bool MapWindow::shouldRecompute() { // 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); + 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]; @@ -512,9 +523,24 @@ void MapInstructions::updateDistance(float d) { } } + distance->setAlignment(Qt::AlignLeft); distance->setText(distance_str); } +void MapInstructions::updateGPSValid(bool valid) { + if (!valid) { + primary->setText(""); + distance->setText("GPS not available"); + distance->setAlignment(Qt::AlignCenter); + + secondary->setVisible(false); + icon_01->setVisible(false); + + setVisible(true); + adjustSize(); + } +} + void MapInstructions::updateInstructions(QMap banner, bool full) { // Need multiple calls to adjustSize for it to properly resize // seems like it takes a little bit of time for the images to change and @@ -539,6 +565,7 @@ void MapInstructions::updateInstructions(QMap banner, bool fu QPixmap pix(fn); icon_01->setPixmap(pix.scaledToWidth(200, Qt::SmoothTransformation)); icon_01->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed)); + icon_01->setVisible(true); } // Parse components (e.g. lanes, exit number) @@ -601,8 +628,11 @@ void MapInstructions::updateInstructions(QMap banner, bool fu primary->setText(primary_str); secondary->setVisible(secondary_str.length() > 0); secondary->setText(secondary_str); - adjustSize(); + last_banner = banner; + + setVisible(true); + adjustSize(); } MapETA::MapETA(QWidget * parent) : QWidget(parent) { diff --git a/selfdrive/ui/qt/maps/map.h b/selfdrive/ui/qt/maps/map.h index 2d28633a8b..fb7a056feb 100644 --- a/selfdrive/ui/qt/maps/map.h +++ b/selfdrive/ui/qt/maps/map.h @@ -43,6 +43,7 @@ public: public slots: void updateDistance(float d); void updateInstructions(QMap banner, bool full); + void updateGPSValid(bool valid); }; class MapETA : public QWidget { @@ -97,7 +98,7 @@ private: int zoom_counter = 0; // Position - QMapbox::Coordinate last_position = QMapbox::Coordinate(37.7393118509158, -122.46471285025565); + std::optional last_position; std::optional last_bearing; // Route @@ -134,5 +135,6 @@ signals: void distanceChanged(float distance); void instructionsChanged(QMap banner, bool full); void ETAChanged(float seconds, float seconds_typical, float distance); + void GPSValidChanged(bool valid); };