diff --git a/selfdrive/navd/helpers.py b/selfdrive/navd/helpers.py index 89fd5e7e4e..eda813154a 100644 --- a/selfdrive/navd/helpers.py +++ b/selfdrive/navd/helpers.py @@ -2,18 +2,24 @@ from __future__ import annotations import json import math -from typing import Any, Dict, List, Optional, Tuple +from typing import Any, Dict, List, Optional, Tuple, Union, cast +from common.conversions import Conversions from common.numpy_fast import clip from common.params import Params EARTH_MEAN_RADIUS = 6371007.2 +SPEED_CONVERSIONS = { + 'km/h': Conversions.KPH_TO_MS, + 'mph': Conversions.MPH_TO_MS, + } class Coordinate: def __init__(self, latitude: float, longitude: float) -> None: self.latitude = latitude self.longitude = longitude + self.annotations: Dict[str, float] = {} @classmethod def from_mapbox_tuple(cls, t: Tuple[float, float]) -> Coordinate: @@ -116,6 +122,12 @@ def string_to_direction(direction: str) -> str: return 'none' +def maxspeed_to_ms(maxspeed: Dict[str, Union[str, float]]) -> float: + unit = cast(str, maxspeed['unit']) + speed = cast(float, maxspeed['speed']) + return SPEED_CONVERSIONS[unit] * speed + + def parse_banner_instructions(instruction: Any, banners: Any, distance_to_maneuver: float = 0.0) -> None: if not len(banners): return diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index 0a8eab7e06..52db9b1d08 100755 --- a/selfdrive/navd/navd.py +++ b/selfdrive/navd/navd.py @@ -10,11 +10,11 @@ from cereal import log from common.api import Api from common.params import Params from common.realtime import Ratekeeper -from selfdrive.swaglog import cloudlog from selfdrive.navd.helpers import (Coordinate, coordinate_from_param, - distance_along_geometry, + distance_along_geometry, maxspeed_to_ms, minimum_distance, parse_banner_instructions) +from selfdrive.swaglog import cloudlog REROUTE_DISTANCE = 25 MANEUVER_TRANSITION_THRESHOLD = 10 @@ -110,7 +110,7 @@ class RouteEngine: params = { 'access_token': self.mapbox_token, - # 'annotations': 'maxspeed', + 'annotations': 'maxspeed', 'geometries': 'geojson', 'overview': 'full', 'steps': 'true', @@ -131,9 +131,25 @@ class RouteEngine: self.route = r['routes'][0]['legs'][0]['steps'] self.route_geometry = [] + maxspeed_idx = 0 + maxspeeds = r['routes'][0]['legs'][0]['annotation']['maxspeed'] + # Convert coordinates for step in self.route: - self.route_geometry.append([Coordinate.from_mapbox_tuple(c) for c in step['geometry']['coordinates']]) + coords = [] + + for c in step['geometry']['coordinates']: + coord = Coordinate.from_mapbox_tuple(c) + + # Last step does not have maxspeed + if (maxspeed_idx < len(maxspeeds)) and ('unknown' not in maxspeeds[maxspeed_idx]): + coord.annotations['maxspeed'] = maxspeed_to_ms(maxspeeds[maxspeed_idx]) + + coords.append(coord) + maxspeed_idx += 1 + + self.route_geometry.append(coords) + maxspeed_idx -= 1 # Every segment ends with the same coordinate as the start of the next self.step_idx = 0 else: @@ -179,6 +195,23 @@ class RouteEngine: msg.navInstruction.timeRemaining = total_time msg.navInstruction.timeRemainingTypical = total_time_typical + # Speed limit + closest_idx, closest = min(enumerate(geometry), key=lambda p: p[1].distance_to(self.last_position)) + if closest_idx > 0: + # If we are not past the closest point, show previous + if along_geometry < distance_along_geometry(geometry, geometry[closest_idx]): + closest = geometry[closest_idx - 1] + + if 'maxspeed' in closest.annotations: + msg.navInstruction.speedLimit = closest.annotations['maxspeed'] + + # Speed limit sign type + if 'speedLimitSign' in step: + if step['speedLimitSign'] == 'mutcd': + msg.navInstruction.speedLimitSign = log.NavInstruction.SpeedLimitSign.mutcd + elif step['speedLimitSign'] == 'vienna': + msg.navInstruction.speedLimitSign = log.NavInstruction.SpeedLimitSign.vienna + self.pm.send('navInstruction', msg) # Transition to next route segment