Release 260111

selfdrive/navd/helpers.py

@@ -0,0 +1,190 @@
+from __future__ import annotations
+
+import json
+import math
+from typing import Any, Dict, List, Optional, Tuple, Union, cast
+
+from openpilot.common.conversions import Conversions
+import numpy as np
+from openpilot.common.params import Params
+
+DIRECTIONS = ('left', 'right', 'straight')
+MODIFIABLE_DIRECTIONS = ('left', 'right')
+
+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:
+    return cls(t[1], t[0])
+
+  def as_dict(self) -> Dict[str, float]:
+    return {'latitude': self.latitude, 'longitude': self.longitude}
+
+  def __str__(self) -> str:
+    return f'Coordinate({self.latitude}, {self.longitude})'
+
+  def __repr__(self) -> str:
+    return self.__str__()
+
+  def __eq__(self, other) -> bool:
+    if not isinstance(other, Coordinate):
+      return False
+    return (self.latitude == other.latitude) and (self.longitude == other.longitude)
+
+  def __sub__(self, other: Coordinate) -> Coordinate:
+    return Coordinate(self.latitude - other.latitude, self.longitude - other.longitude)
+
+  def __add__(self, other: Coordinate) -> Coordinate:
+    return Coordinate(self.latitude + other.latitude, self.longitude + other.longitude)
+
+  def __mul__(self, c: float) -> Coordinate:
+    return Coordinate(self.latitude * c, self.longitude * c)
+
+  def dot(self, other: Coordinate) -> float:
+    return self.latitude * other.latitude + self.longitude * other.longitude
+
+  def distance_to(self, other: Coordinate) -> float:
+    # Haversine formula
+    dlat = math.radians(other.latitude - self.latitude)
+    dlon = math.radians(other.longitude - self.longitude)
+
+    haversine_dlat = math.sin(dlat / 2.0)
+    haversine_dlat *= haversine_dlat
+    haversine_dlon = math.sin(dlon / 2.0)
+    haversine_dlon *= haversine_dlon
+
+    y = haversine_dlat \
+             + math.cos(math.radians(self.latitude)) \
+             * math.cos(math.radians(other.latitude)) \
+             * haversine_dlon
+    x = 2 * math.asin(math.sqrt(y))
+    return x * EARTH_MEAN_RADIUS
+
+
+def minimum_distance(a: Coordinate, b: Coordinate, p: Coordinate):
+  if a.distance_to(b) < 0.01:
+    return a.distance_to(p)
+
+  ap = p - a
+  ab = b - a
+  t = np.clip(ap.dot(ab) / ab.dot(ab), 0.0, 1.0)
+  projection = a + ab * t
+  return projection.distance_to(p)
+
+
+def distance_along_geometry(geometry: List[Coordinate], pos: Coordinate) -> float:
+  if len(geometry) <= 2:
+    return geometry[0].distance_to(pos)
+
+  # 1. Find segment that is closest to current position
+  # 2. Total distance is sum of distance to start of closest segment
+  #    + all previous segments
+  total_distance = 0.0
+  total_distance_closest = 0.0
+  closest_distance = 1e9
+
+  for i in range(len(geometry) - 1):
+    d = minimum_distance(geometry[i], geometry[i + 1], pos)
+
+    if d < closest_distance:
+      closest_distance = d
+      total_distance_closest = total_distance + geometry[i].distance_to(pos)
+
+    total_distance += geometry[i].distance_to(geometry[i + 1])
+
+  return total_distance_closest
+
+
+def coordinate_from_param(param: str, params: Optional[Params] = None) -> Tuple[Optional[Coordinate], Optional[str]]:
+  if params is None:
+    params = Params()
+
+  json_str = params.get(param)
+  if json_str is None:
+    return None, None
+
+  pos = json.loads(json_str)
+  if 'latitude' not in pos or 'longitude' not in pos:
+    return None, None
+  place_name = pos.get('place_name', None)
+  return Coordinate(pos['latitude'], pos['longitude']), place_name
+
+
+def string_to_direction(direction: str) -> str:
+  for d in DIRECTIONS:
+    if d in direction:
+      if 'slight' in direction and d in MODIFIABLE_DIRECTIONS:
+        return 'slight' + d.capitalize()
+      return d
+  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 field_valid(dat: dict, field: str) -> bool:
+  return field in dat and dat[field] is not None
+
+
+def parse_banner_instructions(banners: Any, distance_to_maneuver: float = 0.0) -> Optional[Dict[str, Any]]:
+  if not len(banners):
+    return None
+
+  instruction = {}
+
+  # A segment can contain multiple banners, find one that we need to show now
+  current_banner = banners[0]
+  for banner in banners:
+    if distance_to_maneuver < banner['distanceAlongGeometry']:
+      current_banner = banner
+
+  # Only show banner when close enough to maneuver
+  instruction['showFull'] = distance_to_maneuver < current_banner['distanceAlongGeometry']
+
+  # Primary
+  #print(f"current_banner: {current_banner}")
+  p = current_banner['primary']
+  if field_valid(p, 'text'):
+    instruction['maneuverPrimaryText'] = p['text']
+  if field_valid(p, 'type'):
+    instruction['maneuverType'] = p['type']
+  if field_valid(p, 'modifier'):
+    instruction['maneuverModifier'] = p['modifier']
+
+  # Secondary
+  if field_valid(current_banner, 'secondary'):
+    instruction['maneuverSecondaryText'] = current_banner['secondary']['text']
+
+  # Lane lines
+  if field_valid(current_banner, 'sub'):
+    lanes = []
+    for component in current_banner['sub']['components']:
+      if component['type'] != 'lane':
+        continue
+
+      lane = {
+        'active': component['active'],
+        'directions': [string_to_direction(d) for d in component['directions']],
+      }
+
+      if field_valid(component, 'active_direction'):
+        lane['activeDirection'] = string_to_direction(component['active_direction'])
+
+      lanes.append(lane)
+    instruction['lanes'] = lanes
+
+  return instruction

selfdrive/navd/map_renderer.h

@@ -0,0 +1,59 @@
+#pragma once
+
+#include <memory>
+
+#include <QOpenGLContext>
+#include <QMapLibre/Map>
+#include <QMapLibre/Settings>
+#include <QTimer>
+#include <QGeoCoordinate>
+#include <QOpenGLBuffer>
+#include <QOffscreenSurface>
+#include <QOpenGLFunctions>
+#include <QOpenGLFramebufferObject>
+
+#include "msgq/visionipc/visionipc_server.h"
+#include "cereal/messaging/messaging.h"
+
+
+class MapRenderer : public QObject {
+  Q_OBJECT
+
+public:
+  MapRenderer(const QMapLibre::Settings &, bool online=true);
+  uint8_t* getImage();
+  void update();
+  bool loaded();
+  ~MapRenderer();
+
+private:
+  std::unique_ptr<QOpenGLContext> ctx;
+  std::unique_ptr<QOffscreenSurface> surface;
+  std::unique_ptr<QOpenGLFunctions> gl_functions;
+  std::unique_ptr<QOpenGLFramebufferObject> fbo;
+
+  std::unique_ptr<VisionIpcServer> vipc_server;
+  std::unique_ptr<PubMaster> pm;
+  std::unique_ptr<SubMaster> sm;
+  void publish(const double render_time, const bool loaded);
+  void sendThumbnail(const uint64_t ts, const kj::Array<capnp::byte> &buf);
+
+  QMapLibre::Settings m_settings;
+  QScopedPointer<QMapLibre::Map> m_map;
+
+  void initLayers();
+
+  uint32_t frame_id = 0;
+  uint64_t last_llk_rendered = 0;
+  bool rendered() {
+    return last_llk_rendered == (*sm)["carrotMan"].getLogMonoTime();
+  }
+
+  QTimer* timer;
+  bool ever_loaded = false;
+
+public slots:
+  void updatePosition(QMapLibre::Coordinate position, float bearing);
+  void updateRoute(QList<QGeoCoordinate> coordinates);
+  void msgUpdate();
+};

selfdrive/navd/map_renderer.py

@@ -0,0 +1,96 @@
+#!/usr/bin/env python3
+# You might need to uninstall the PyQt5 pip package to avoid conflicts
+
+import os
+import time
+import numpy as np
+import polyline
+from cffi import FFI
+
+from openpilot.common.ffi_wrapper import suffix
+from openpilot.common.basedir import BASEDIR
+
+HEIGHT = WIDTH = SIZE = 256
+METERS_PER_PIXEL = 2
+
+
+def get_ffi():
+  lib = os.path.join(BASEDIR, "selfdrive", "navd", "libmaprender" + suffix())
+
+  ffi = FFI()
+  ffi.cdef("""
+void* map_renderer_init(char *maps_host, char *token);
+void map_renderer_update_position(void *inst, float lat, float lon, float bearing);
+void map_renderer_update_route(void *inst, char *polyline);
+void map_renderer_update(void *inst);
+void map_renderer_process(void *inst);
+bool map_renderer_loaded(void *inst);
+uint8_t* map_renderer_get_image(void *inst);
+void map_renderer_free_image(void *inst, uint8_t *buf);
+""")
+  return ffi, ffi.dlopen(lib)
+
+
+def wait_ready(lib, renderer):
+  while not lib.map_renderer_loaded(renderer):
+    lib.map_renderer_update(renderer)
+
+    # The main qt app is not execed, so we need to periodically process events for e.g. network requests
+    lib.map_renderer_process(renderer)
+
+    time.sleep(0.01)
+
+
+def get_image(lib, renderer):
+  buf = lib.map_renderer_get_image(renderer)
+  r = list(buf[0:WIDTH * HEIGHT])
+  lib.map_renderer_free_image(renderer, buf)
+
+  # Convert to numpy
+  r = np.asarray(r)
+  return r.reshape((WIDTH, HEIGHT))
+
+
+def navRoute_to_polyline(nr):
+  coords = [(m.latitude, m.longitude) for m in nr.navRoute.coordinates]
+  return coords_to_polyline(coords)
+
+
+def coords_to_polyline(coords):
+  # TODO: where does this factor of 10 come from?
+  return polyline.encode([(lat * 10., lon * 10.) for lat, lon in coords])
+
+
+def polyline_to_coords(p):
+  coords = polyline.decode(p)
+  return [(lat / 10., lon / 10.) for lat, lon in coords]
+
+
+
+if __name__ == "__main__":
+  import matplotlib.pyplot as plt
+
+  ffi, lib = get_ffi()
+  renderer = lib.map_renderer_init(ffi.NULL, ffi.NULL)
+  wait_ready(lib, renderer)
+
+  geometry = r"{yxk}@|obn~Eg@@eCFqc@J{RFw@?kA@gA?q|@Riu@NuJBgi@ZqVNcRBaPBkG@iSD{I@_H@cH?gG@mG@gG?aD@{LDgDDkVVyQLiGDgX@q_@@qI@qKhS{R~[}NtYaDbGoIvLwNfP_b@|f@oFnF_JxHel@bf@{JlIuxAlpAkNnLmZrWqFhFoh@jd@kX|TkJxH_RnPy^|[uKtHoZ~Um`DlkCorC``CuShQogCtwB_ThQcr@fk@sVrWgRhVmSb\\oj@jxA{Qvg@u]tbAyHzSos@xjBeKbWszAbgEc~@~jCuTrl@cYfo@mRn\\_m@v}@ij@jp@om@lk@y|A`pAiXbVmWzUod@xj@wNlTw}@|uAwSn\\kRfYqOdS_IdJuK`KmKvJoOhLuLbHaMzGwO~GoOzFiSrEsOhD}PhCqw@vJmnAxSczA`Vyb@bHk[fFgl@pJeoDdl@}}@zIyr@hG}X`BmUdBcM^aRR}Oe@iZc@mR_@{FScHxAn_@vz@zCzH~GjPxAhDlB~DhEdJlIbMhFfG|F~GlHrGjNjItLnGvQ~EhLnBfOn@p`@AzAAvn@CfC?fc@`@lUrArStCfSxEtSzGxM|ElFlBrOzJlEbDnC~BfDtCnHjHlLvMdTnZzHpObOf^pKla@~G|a@dErg@rCbj@zArYlj@ttJ~AfZh@r]LzYg@`TkDbj@gIdv@oE|i@kKzhA{CdNsEfOiGlPsEvMiDpLgBpHyB`MkB|MmArPg@|N?|P^rUvFz~AWpOCdAkB|PuB`KeFfHkCfGy@tAqC~AsBPkDs@uAiAcJwMe@s@eKkPMoXQux@EuuCoH?eI?Kas@}Dy@wAUkMOgDL" # noqa: E501
+  lib.map_renderer_update_route(renderer, geometry.encode())
+
+  POSITIONS = [
+    (32.71569271952601, -117.16384270868463, 0), (32.71569271952601, -117.16384270868463, 45),  # San Diego
+    (52.378641991483136, 4.902623379456488, 0), (52.378641991483136, 4.902623379456488, 45),  # Amsterdam
+  ]
+  plt.figure()
+
+  for i, pos in enumerate(POSITIONS):
+    t = time.time()
+    lib.map_renderer_update_position(renderer, *pos)
+    wait_ready(lib, renderer)
+
+    print(f"{pos} took {time.time() - t:.2f} s")
+
+    plt.subplot(2, 2, i + 1)
+    plt.imshow(get_image(lib, renderer), cmap='gray')
+
+  plt.show()

selfdrive/navd/navd.py

@@ -0,0 +1,391 @@
+#!/usr/bin/env python3
+import json
+import math
+import os
+import threading
+import importlib
+
+import requests
+import numpy as np
+
+import cereal.messaging as messaging
+from cereal import log
+from openpilot.common.api import Api
+
+from openpilot.common.params import Params
+from openpilot.common.realtime import Ratekeeper
+from openpilot.selfdrive.navd.helpers import (Coordinate, coordinate_from_param,
+                                    distance_along_geometry, maxspeed_to_ms,
+                                    minimum_distance,
+                                    parse_banner_instructions)
+from openpilot.common.swaglog import cloudlog
+from selfdrive.carrot.carrot_man import CarrotMan
+
+
+REROUTE_DISTANCE = 25
+MANEUVER_TRANSITION_THRESHOLD = 10
+REROUTE_COUNTER_MIN = 3
+
+
+class RouteEngine:
+  def __init__(self, sm, pm):
+    self.name = "RouteEngine Instance"
+    self.sm = sm
+    self.pm = pm
+
+    self.params = Params()
+
+    # Get last gps position from params
+    self.last_position,_ = coordinate_from_param("LastGPSPosition", self.params)
+    self.last_bearing = None
+
+    self.gps_ok = False
+    self.localizer_valid = False
+
+    self.nav_destination = None
+    self.step_idx = None
+    self.route = None
+    self.route_geometry = None
+
+    self.recompute_backoff = 0
+    self.recompute_countdown = 0
+
+    self.ui_pid = None
+
+    self.reroute_counter = 0
+
+    self.carrot_route_active = False
+
+    if "MAPBOX_TOKEN" in os.environ:
+      self.mapbox_token = os.environ["MAPBOX_TOKEN"]
+      self.mapbox_host = "https://api.mapbox.com"
+    elif self.params.get_int("PrimeType") == 0:
+      self.mapbox_token = self.params.get("MapboxPublicKey", encoding='utf8')
+      self.mapbox_host = "https://api.mapbox.com"
+    else:
+      try:
+        self.mapbox_token = Api(self.params.get("DongleId", encoding='utf8')).get_token(expiry_hours=4 * 7 * 24)
+      except FileNotFoundError:
+        cloudlog.exception("Failed to generate mapbox token due to missing private key. Ensure device is registered.")
+        self.mapbox_token = ""
+      self.mapbox_host = "https://maps.comma.ai"
+
+  def update(self):
+    self.sm.update(0)
+
+    if self.sm.updated["managerState"]:
+      ui_pid = [p.pid for p in self.sm["managerState"].processes if p.name == "ui" and p.running]
+      if ui_pid:
+        if self.ui_pid and self.ui_pid != ui_pid[0]:
+          cloudlog.warning("UI restarting, sending route")
+          print("########## UI restarting, sending route")
+          threading.Timer(5.0, self.send_route).start()
+        self.ui_pid = ui_pid[0]
+
+    self.update_location()
+
+    if self.carrot_route_active:
+      return
+
+    try:
+      self.recompute_route()
+      self.send_instruction()
+    except Exception:
+      cloudlog.exception("navd.failed_to_compute")
+
+  def update_location(self):
+
+    carrot_man = self.sm['carrotMan']
+    if carrot_man.xPosLat != 0.0 and carrot_man.xPosLon != 0.0:
+      self.last_bearing = carrot_man.xPosAngle;
+      self.last_position = Coordinate(carrot_man.xPosLat, carrot_man.xPosLon)
+      self.localizer_valid = self.gps_ok = True
+    else:
+      self.localizer_valid = self.gps_ok = False
+      self.carrot_route_active = False
+
+  def recompute_route(self):
+    if self.last_position is None:
+      return
+
+    new_destination, place_name = coordinate_from_param("NavDestination", self.params)
+    if new_destination is None:
+      self.clear_route()
+      self.reset_recompute_limits()
+      return
+    #print(f"new_destination: {new_destination} nav_destination: {self.nav_destination}")
+    should_recompute = self.should_recompute() and place_name != "External Navi"
+    if new_destination != self.nav_destination and place_name != "External Navi":
+      cloudlog.warning(f"Got new destination from NavDestination param {new_destination}")
+      print(f"Got new destination from NavDestination param {new_destination} {self.nav_destination} {place_name}")
+      should_recompute = True
+
+    # Don't recompute when GPS drifts in tunnels
+    if not self.gps_ok and self.step_idx is not None:
+      return
+
+    if self.recompute_countdown == 0 and should_recompute:
+      self.recompute_countdown = 2**self.recompute_backoff
+      self.recompute_backoff = min(6, self.recompute_backoff + 1)
+      self.calculate_route(new_destination)
+      self.reroute_counter = 0
+    else:
+      self.recompute_countdown = max(0, self.recompute_countdown - 1)
+
+  def calculate_route(self, destination):
+    cloudlog.warning(f"Calculating route {self.last_position} -> {destination}")
+    print(f"############## Calculating route {self.last_position} -> {destination}")
+    self.nav_destination = destination
+
+    lang = self.params.get('LanguageSetting', encoding='utf8')
+    if lang is not None:
+      lang = lang.replace('main_', '')
+
+    params = {
+      'access_token': self.mapbox_token,
+      'annotations': 'maxspeed',
+      'geometries': 'geojson',
+      'overview': 'full',
+      'steps': 'true',
+      'banner_instructions': 'true',
+      'alternatives': 'false',
+      'language': lang,
+    }
+
+    # TODO: move waypoints into NavDestination param?
+    waypoints = self.params.get('NavDestinationWaypoints', encoding='utf8')
+    waypoint_coords = []
+    if waypoints is not None and len(waypoints) > 0:
+      waypoint_coords = json.loads(waypoints)
+
+    coords = [
+      (self.last_position.longitude, self.last_position.latitude),
+      *waypoint_coords,
+      (destination.longitude, destination.latitude)
+    ]
+    params['waypoints'] = f'0;{len(coords)-1}'
+    if self.last_bearing is not None:
+      params['bearings'] = f"{(self.last_bearing + 360) % 360:.0f},90" + (';'*(len(coords)-1))
+
+    coords_str = ';'.join([f'{lon},{lat}' for lon, lat in coords])
+    url = self.mapbox_host + '/directions/v5/mapbox/driving-traffic/' + coords_str
+    try:
+      resp = requests.get(url, params=params, timeout=10)
+      if resp.status_code != 200:
+        cloudlog.event("API request failed", status_code=resp.status_code, text=resp.text, error=True)
+      resp.raise_for_status()
+
+      r = resp.json()
+      if len(r['routes']):
+        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:
+          coords = []
+
+          for c in step['geometry']['coordinates']:
+            coord = Coordinate.from_mapbox_tuple(c)
+
+            # Last step does not have maxspeed
+            if (maxspeed_idx < len(maxspeeds)):
+              maxspeed = maxspeeds[maxspeed_idx]
+              if ('unknown' not in maxspeed) and ('none' not in maxspeed):
+                coord.annotations['maxspeed'] = maxspeed_to_ms(maxspeed)
+
+            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:
+        cloudlog.warning("Got empty route response")
+        print("Got empty route response")
+        self.clear_route()
+
+      # clear waypoints to avoid a re-route including past waypoints
+      # TODO: only clear once we're past a waypoint
+      self.params.remove('NavDestinationWaypoints')
+
+    except requests.exceptions.RequestException:
+      cloudlog.exception("failed to get route")
+      print("failed to get route")
+      self.clear_route()
+
+    self.send_route()
+
+  def send_instruction(self):
+    msg = messaging.new_message('navInstruction', valid=True)
+    #print("navd_navInstruction")
+    if self.step_idx is None:
+      msg.valid = False
+      self.pm.send('navInstruction', msg)
+      return
+    #print("navd_navInstruction")
+    step = self.route[self.step_idx]
+    geometry = self.route_geometry[self.step_idx]
+    along_geometry = distance_along_geometry(geometry, self.last_position)
+    distance_to_maneuver_along_geometry = step['distance'] - along_geometry
+
+    # Banner instructions are for the following maneuver step, don't use empty last step
+    banner_step = step
+    if not len(banner_step['bannerInstructions']) and self.step_idx == len(self.route) - 1:
+      banner_step = self.route[max(self.step_idx - 1, 0)]
+
+    # Current instruction
+    msg.navInstruction.maneuverDistance = distance_to_maneuver_along_geometry
+    instruction = parse_banner_instructions(banner_step['bannerInstructions'], distance_to_maneuver_along_geometry)
+    if instruction is not None:
+      for k,v in instruction.items():
+        setattr(msg.navInstruction, k, v)
+
+    # All instructions
+    maneuvers = []
+    for i, step_i in enumerate(self.route):
+      if i < self.step_idx:
+        distance_to_maneuver = -sum(self.route[j]['distance'] for j in range(i+1, self.step_idx)) - along_geometry
+      elif i == self.step_idx:
+        distance_to_maneuver = distance_to_maneuver_along_geometry
+      else:
+        distance_to_maneuver = distance_to_maneuver_along_geometry + sum(self.route[j]['distance'] for j in range(self.step_idx+1, i+1))
+
+      instruction = parse_banner_instructions(step_i['bannerInstructions'], distance_to_maneuver)
+      if instruction is None:
+        continue
+      maneuver = {'distance': distance_to_maneuver}
+      if 'maneuverType' in instruction:
+        maneuver['type'] = instruction['maneuverType']
+      if 'maneuverModifier' in instruction:
+        maneuver['modifier'] = instruction['maneuverModifier']
+      maneuvers.append(maneuver)
+
+    msg.navInstruction.allManeuvers = maneuvers
+
+    # Compute total remaining time and distance
+    remaining = 1.0 - along_geometry / max(step['distance'], 1)
+    total_distance = step['distance'] * remaining
+    total_time = step['duration'] * remaining
+
+    if step['duration_typical'] is None:
+      total_time_typical = total_time
+    else:
+      total_time_typical = step['duration_typical'] * remaining
+
+    # Add up totals for future steps
+    for i in range(self.step_idx + 1, len(self.route)):
+      total_distance += self.route[i]['distance']
+      total_time += self.route[i]['duration']
+      if self.route[i]['duration_typical'] is None:
+        total_time_typical += self.route[i]['duration']
+      else:
+        total_time_typical += self.route[i]['duration_typical']
+
+    msg.navInstruction.distanceRemaining = total_distance
+    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) and self.localizer_valid:
+      msg.navInstruction.speedLimit = closest.annotations['maxspeed']
+      print(closest.annotations)
+
+
+    # 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
+    if distance_to_maneuver_along_geometry < -MANEUVER_TRANSITION_THRESHOLD:
+      if self.step_idx + 1 < len(self.route):
+        self.step_idx += 1
+        self.reset_recompute_limits()
+      else:
+        cloudlog.warning("Destination reached")
+        print("Destination reached")
+
+        # Clear route if driving away from destination
+        dist = self.nav_destination.distance_to(self.last_position)
+        if dist > REROUTE_DISTANCE:
+          self.params.remove("NavDestination")
+          self.clear_route()
+
+  def send_route(self):
+    coords = []
+
+    if self.route is not None:
+      for path in self.route_geometry:
+        coords += [c.as_dict() for c in path]
+
+    if len(coords) == 0:
+      return
+    #print("$$$$$$$$$$$$$$$$$$coords=",coords)
+    msg = messaging.new_message('navRouteNavd', valid=True)
+    msg.navRouteNavd.coordinates = coords
+    self.pm.send('navRouteNavd', msg)
+
+  def clear_route(self):
+    self.route = None
+    self.route_geometry = None
+    self.step_idx = None
+    self.nav_destination = None
+
+  def reset_recompute_limits(self):
+    self.recompute_backoff = 0
+    self.recompute_countdown = 0
+
+  def should_recompute(self):
+    if self.step_idx is None or self.route is None:
+      return True
+
+    # Don't recompute in last segment, assume destination is reached
+    if self.step_idx == len(self.route) - 1:
+      return False
+
+    # Compute closest distance to all line segments in the current path
+    min_d = REROUTE_DISTANCE + 1
+    path = self.route_geometry[self.step_idx]
+    for i in range(len(path) - 1):
+      a = path[i]
+      b = path[i + 1]
+
+      if a.distance_to(b) < 1.0:
+        continue
+
+      min_d = min(min_d, minimum_distance(a, b, self.last_position))
+
+    if min_d > REROUTE_DISTANCE:
+      self.reroute_counter += 1
+    else:
+      self.reroute_counter = 0
+    return self.reroute_counter > REROUTE_COUNTER_MIN
+    # TODO: Check for going wrong way in segment
+
+def main():
+  pm = messaging.PubMaster(['navInstruction', 'navRouteNavd'])
+  sm = messaging.SubMaster(['managerState', 'carrotMan'])
+
+  rk = Ratekeeper(1.0)
+  route_engine = RouteEngine(sm, pm)
+  while True:
+    route_engine.update()
+    rk.keep_time()
+
+
+if __name__ == "__main__":
+  main()

selfdrive/navd/set_destination.py

@@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+import json
+import sys
+
+from openpilot.common.params import Params
+
+if __name__ == "__main__":
+  params = Params()
+
+  # set from google maps url
+  if len(sys.argv) > 1:
+    coords = sys.argv[1].split("/@")[-1].split("/")[0].split(",")
+    dest = {
+      "latitude": float(coords[0]),
+      "longitude": float(coords[1])
+    }
+    params.put("NavDestination", json.dumps(dest))
+    params.remove("NavDestinationWaypoints")
+  else:
+    print("Setting to Taco Bell")
+    dest = {
+      "latitude": 32.71160109904473,
+      "longitude": -117.12556569985693,
+    }
+    params.put("NavDestination", json.dumps(dest))
+
+    waypoints = [
+      (-117.16020713111648, 32.71997612490662),
+    ]
+    params.put("NavDestinationWaypoints", json.dumps(waypoints))
+
+    print(dest)
+    print(waypoints)