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

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'])
 

selfdrive/ui/qt/maps/map.cc

@@ -1,5 +1,6 @@
 #include "selfdrive/ui/qt/maps/map.h"
 
+#include <eigen3/Eigen/Dense>
 #include <cmath>
 
 #include <QDebug>
@@ -7,6 +8,7 @@
 #include <QPainterPath>
 
 #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();
+    auto locationd_location = sm["liveLocationKalman"].getLiveLocationKalman();
+    auto locationd_pos = locationd_location.getPositionGeodetic();
+    auto locationd_orientation = locationd_location.getCalibratedOrientationNED();
+    auto locationd_velocity = locationd_location.getVelocityCalibrated();
 
-    localizer_valid = (location.getStatus() == cereal::LiveLocationKalman::Status::VALID) &&
-     pos.getValid() && orientation.getValid() && velocity.getValid();
+    locationd_valid = (locationd_location.getStatus() == cereal::LiveLocationKalman::Status::VALID) &&
+      locationd_pos.getValid() && locationd_orientation.getValid() && locationd_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]);
+    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<QMapbox::Feature>(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);

selfdrive/ui/qt/maps/map.h

@@ -104,7 +104,8 @@ private:
   std::optional<QMapbox::Coordinate> last_position;
   std::optional<float> last_bearing;
   FirstOrderFilter velocity_filter;
-  bool localizer_valid = false;
+  bool laikad_valid = false;
+  bool locationd_valid = false;
 
   MapInstructions* map_instructions;
   MapETA* map_eta;

selfdrive/ui/qt/maps/map_helpers.cc

@@ -116,3 +116,8 @@ std::optional<QMapbox::Coordinate> 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;
+}

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<QGeoCoordinate> coordinate_list);
 
 std::optional<QMapbox::Coordinate> coordinate_from_param(std::string param);
+double angle_difference(double angle1, double angle2);

selfdrive/ui/ui.cc

@@ -227,7 +227,7 @@ UIState::UIState(QObject *parent) : QObject(parent) {
   sm = std::make_unique<SubMaster, const std::initializer_list<const char *>>({
     "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "roadCameraState",
     "pandaStates", "carParams", "driverMonitoringState", "sensorEvents", "carState", "liveLocationKalman",
-    "wideRoadCameraState", "managerState", "navInstruction", "navRoute",
+    "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "gnssMeasurements",
   });
 
   Params params;