nav: add polyline helpers (#26816)

* nav: add polyline helpers

* Update selfdrive/navd/map_renderer.py
old-commit-hash: d3a3d74830

poetry.lock

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:f001cc546693d59937130ed9cd49da4bd092d5ec7d1bf96b0419d76ab0271cf7
+oid sha256:f987ed9e1ad0fce8607e7884e389143189ef741be636a4d976437dafbd606cc3
-size 562441
+size 563115

pyproject.toml

@@ -57,6 +57,7 @@ tqdm = "^4.64.0"
 urllib3 = "^1.26.10"
 utm = "^0.7.0"
 websocket_client = "^1.3.3"
+polyline = "^1.4.0"
 
 
 [tool.poetry.group.dev.dependencies]

selfdrive/navd/map_renderer.py

@@ -4,6 +4,7 @@
 import os
 import time
 import numpy as np
+import polyline
 from cffi import FFI
 
 from common.ffi_wrapper import suffix
@@ -50,6 +51,22 @@ def get_image(lib, renderer):
   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