nav: add polyline helpers (#26816)

* nav: add polyline helpers

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

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