From c20b96764ce71053ce08f11494f5c86780bd9b71 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Fri, 8 Jul 2022 18:20:36 +0200 Subject: [PATCH] nav: use laikad position if locationd is not yet available (#25033) * ui: use laikad position when locationd is not ready * cleanup * same threshold as locationd * use first bearing directly * use in navd too old-commit-hash: a9401319dfa89fea2d2699aaddc758cbc47b6396 --- selfdrive/navd/navd.py | 19 ++++++-- selfdrive/ui/qt/maps/map.cc | 71 ++++++++++++++++++++++------- selfdrive/ui/qt/maps/map.h | 3 +- selfdrive/ui/qt/maps/map_helpers.cc | 5 ++ selfdrive/ui/qt/maps/map_helpers.h | 1 + selfdrive/ui/ui.cc | 2 +- 6 files changed, 79 insertions(+), 22 deletions(-) diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index f02de43c7b..89a1c9bdfb 100755 --- a/selfdrive/navd/navd.py +++ b/selfdrive/navd/navd.py @@ -4,12 +4,14 @@ import os import threading import requests +import numpy as np import cereal.messaging as messaging from cereal import log from common.api import Api from common.params import Params from common.realtime import Ratekeeper +from common.transformations.coordinates import ecef2geodetic from selfdrive.navd.helpers import (Coordinate, coordinate_from_param, distance_along_geometry, maxspeed_to_ms, minimum_distance, @@ -18,6 +20,7 @@ from system.swaglog import cloudlog REROUTE_DISTANCE = 25 MANEUVER_TRANSITION_THRESHOLD = 10 +VALID_POS_STD = 50.0 class RouteEngine: @@ -72,13 +75,21 @@ class RouteEngine: def update_location(self): location = self.sm['liveLocationKalman'] - self.gps_ok = location.gpsOK + laikad = self.sm['gnssMeasurements'] - self.localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid + locationd_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid + laikad_valid = laikad.positionECEF.valid and np.linalg.norm(laikad.positionECEF.std) < VALID_POS_STD - if self.localizer_valid: + self.localizer_valid = locationd_valid or laikad_valid + self.gps_ok = location.gpsOK or laikad_valid + + if locationd_valid: self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2]) self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1]) + elif laikad_valid: + geodetic = ecef2geodetic(laikad.positionECEF.value) + self.last_position = Coordinate(geodetic[0], geodetic[1]) + self.last_bearing = None def recompute_route(self): if self.last_position is None: @@ -276,7 +287,7 @@ class RouteEngine: def main(sm=None, pm=None): if sm is None: - sm = messaging.SubMaster(['liveLocationKalman', 'managerState']) + sm = messaging.SubMaster(['liveLocationKalman', 'gnssMeasurements', 'managerState']) if pm is None: pm = messaging.PubMaster(['navInstruction', 'navRoute']) diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc index 4c6a0a4e65..fd47f4188f 100644 --- a/selfdrive/ui/qt/maps/map.cc +++ b/selfdrive/ui/qt/maps/map.cc @@ -1,5 +1,6 @@ #include "selfdrive/ui/qt/maps/map.h" +#include #include #include @@ -7,6 +8,7 @@ #include #include "common/swaglog.h" +#include "common/transformations/coordinates.hpp" #include "selfdrive/ui/qt/maps/map_helpers.h" #include "selfdrive/ui/qt/request_repeater.h" #include "selfdrive/ui/qt/util.h" @@ -22,6 +24,8 @@ const float MAX_PITCH = 50; const float MIN_PITCH = 0; const float MAP_SCALE = 2; +const float VALID_POS_STD = 50.0; // m + const QString ICON_SUFFIX = ".png"; MapWindow::MapWindow(const QMapboxGLSettings &settings) : m_settings(settings), velocity_filter(0, 10, 0.05) { @@ -105,18 +109,53 @@ void MapWindow::updateState(const UIState &s) { update(); if (sm.updated("liveLocationKalman")) { - auto location = sm["liveLocationKalman"].getLiveLocationKalman(); - auto pos = location.getPositionGeodetic(); - auto orientation = location.getCalibratedOrientationNED(); - auto velocity = location.getVelocityCalibrated(); - - localizer_valid = (location.getStatus() == cereal::LiveLocationKalman::Status::VALID) && - pos.getValid() && orientation.getValid() && velocity.getValid(); - - if (localizer_valid) { - last_position = QMapbox::Coordinate(pos.getValue()[0], pos.getValue()[1]); - last_bearing = RAD2DEG(orientation.getValue()[2]); - velocity_filter.update(velocity.getValue()[0]); + auto locationd_location = sm["liveLocationKalman"].getLiveLocationKalman(); + auto locationd_pos = locationd_location.getPositionGeodetic(); + auto locationd_orientation = locationd_location.getCalibratedOrientationNED(); + auto locationd_velocity = locationd_location.getVelocityCalibrated(); + + locationd_valid = (locationd_location.getStatus() == cereal::LiveLocationKalman::Status::VALID) && + locationd_pos.getValid() && locationd_orientation.getValid() && locationd_velocity.getValid(); + + if (locationd_valid) { + last_position = QMapbox::Coordinate(locationd_pos.getValue()[0], locationd_pos.getValue()[1]); + last_bearing = RAD2DEG(locationd_orientation.getValue()[2]); + velocity_filter.update(locationd_velocity.getValue()[0]); + } + } + + if (sm.updated("gnssMeasurements")) { + auto laikad_location = sm["gnssMeasurements"].getGnssMeasurements(); + auto laikad_pos = laikad_location.getPositionECEF(); + auto laikad_pos_ecef = laikad_pos.getValue(); + auto laikad_pos_std = laikad_pos.getStd(); + auto laikad_velocity_ecef = laikad_location.getVelocityECEF().getValue(); + + laikad_valid = laikad_pos.getValid() && Eigen::Vector3d(laikad_pos_std[0], laikad_pos_std[1], laikad_pos_std[2]).norm() < VALID_POS_STD; + + if (laikad_valid && !locationd_valid) { + ECEF ecef = {.x = laikad_pos_ecef[0], .y = laikad_pos_ecef[1], .z = laikad_pos_ecef[2]}; + Geodetic laikad_pos_geodetic = ecef2geodetic(ecef); + last_position = QMapbox::Coordinate(laikad_pos_geodetic.lat, laikad_pos_geodetic.lon); + + // Compute NED velocity + LocalCoord converter(ecef); + ECEF next_ecef = {.x = ecef.x + laikad_velocity_ecef[0], .y = ecef.y + laikad_velocity_ecef[1], .z = ecef.z + laikad_velocity_ecef[2]}; + Eigen::VectorXd ned_vel = converter.ecef2ned(next_ecef).to_vector() - converter.ecef2ned(ecef).to_vector(); + + float velocity = ned_vel.norm(); + velocity_filter.update(velocity); + + // Convert NED velocity to angle + if (velocity > 1.0) { + float new_bearing = fmod(RAD2DEG(atan2(ned_vel[1], ned_vel[0])) + 360.0, 360.0); + if (last_bearing) { + float delta = 0.1 * angle_difference(*last_bearing, new_bearing); // Smooth heading + last_bearing = fmod(*last_bearing + delta + 360.0, 360.0); + } else { + last_bearing = new_bearing; + } + } } } @@ -142,9 +181,7 @@ void MapWindow::updateState(const UIState &s) { initLayers(); - if (!localizer_valid) { - map_instructions->showError("Waiting for GPS"); - } else { + if (locationd_valid || laikad_valid) { map_instructions->noError(); // Update current location marker @@ -154,6 +191,8 @@ void MapWindow::updateState(const UIState &s) { carPosSource["type"] = "geojson"; carPosSource["data"] = QVariant::fromValue(feature1); m_map->updateSource("carPosSource", carPosSource); + } else { + map_instructions->showError("Waiting for GPS"); } if (pan_counter == 0) { @@ -174,7 +213,7 @@ void MapWindow::updateState(const UIState &s) { auto i = sm["navInstruction"].getNavInstruction(); emit ETAChanged(i.getTimeRemaining(), i.getTimeRemainingTypical(), i.getDistanceRemaining()); - if (localizer_valid) { + if (locationd_valid || laikad_valid) { m_map->setPitch(MAX_PITCH); // TODO: smooth pitching based on maneuver distance emit distanceChanged(i.getManeuverDistance()); // TODO: combine with instructionsChanged emit instructionsChanged(i); diff --git a/selfdrive/ui/qt/maps/map.h b/selfdrive/ui/qt/maps/map.h index 7c39b24c3c..ecba867edb 100644 --- a/selfdrive/ui/qt/maps/map.h +++ b/selfdrive/ui/qt/maps/map.h @@ -104,7 +104,8 @@ private: std::optional last_position; std::optional last_bearing; FirstOrderFilter velocity_filter; - bool localizer_valid = false; + bool laikad_valid = false; + bool locationd_valid = false; MapInstructions* map_instructions; MapETA* map_eta; diff --git a/selfdrive/ui/qt/maps/map_helpers.cc b/selfdrive/ui/qt/maps/map_helpers.cc index 2b2c27418e..66acb7a25d 100644 --- a/selfdrive/ui/qt/maps/map_helpers.cc +++ b/selfdrive/ui/qt/maps/map_helpers.cc @@ -116,3 +116,8 @@ std::optional coordinate_from_param(std::string param) { return {}; } } + +double angle_difference(double angle1, double angle2) { + double diff = fmod(angle2 - angle1 + 180.0, 360.0) - 180.0; + return diff < -180.0 ? diff + 360.0 : diff; +} diff --git a/selfdrive/ui/qt/maps/map_helpers.h b/selfdrive/ui/qt/maps/map_helpers.h index 344246bb05..1c08c541c3 100644 --- a/selfdrive/ui/qt/maps/map_helpers.h +++ b/selfdrive/ui/qt/maps/map_helpers.h @@ -26,3 +26,4 @@ QMapbox::CoordinatesCollections capnp_coordinate_list_to_collection(const capnp: QMapbox::CoordinatesCollections coordinate_list_to_collection(QList coordinate_list); std::optional coordinate_from_param(std::string param); +double angle_difference(double angle1, double angle2); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index f6193f97a6..6fe1d838ed 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -227,7 +227,7 @@ UIState::UIState(QObject *parent) : QObject(parent) { sm = std::make_unique>({ "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "roadCameraState", "pandaStates", "carParams", "driverMonitoringState", "sensorEvents", "carState", "liveLocationKalman", - "wideRoadCameraState", "managerState", "navInstruction", "navRoute", + "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "gnssMeasurements", }); Params params;