sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

nav: use laikad position if locationd is not yet available (#25033)

Browse Source 
* 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: a9401319df
taco
Willem Melching 3 years ago committed by GitHub
parent 1c8af02e54
commit c20b96764c
6 changed files with 79 additions and 22 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 19
      selfdrive/navd/navd.py
  2. 71
      selfdrive/ui/qt/maps/map.cc
  3. 3
      selfdrive/ui/qt/maps/map.h
  4. 5
      selfdrive/ui/qt/maps/map_helpers.cc
  5. 1
      selfdrive/ui/qt/maps/map_helpers.h
  6. 2
      selfdrive/ui/ui.cc

19
selfdrive/navd/navd.py
Unescape View File

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

71
selfdrive/ui/qt/maps/map.cc
Unescape View File

@ -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();
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<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);

3
selfdrive/ui/qt/maps/map.h
Unescape View File

@ -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;

5
selfdrive/ui/qt/maps/map_helpers.cc
Unescape View File

@ -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;
}

1
selfdrive/ui/qt/maps/map_helpers.h
Unescape View File

@ -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);

2
selfdrive/ui/ui.cc
Unescape View File

@ -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;

Write Preview
Loading…
Cancel
Save