From a8c4347a5f60159c0a84dfd8732befbfe59b6d10 Mon Sep 17 00:00:00 2001
From: Adeeb Shihadeh <adeebshihadeh@gmail.com>
Date: Fri, 16 Dec 2022 21:46:07 -0800
Subject: [PATCH] nav: add polyline helpers (#26816)

* nav: add polyline helpers

* Update selfdrive/navd/map_renderer.py
old-commit-hash: d3a3d74830efb33eff6a9f9057a8d552e355bfd7
---
 poetry.lock                    |  4 ++--
 pyproject.toml                 |  1 +
 selfdrive/navd/map_renderer.py | 17 +++++++++++++++++
 3 files changed, 20 insertions(+), 2 deletions(-)

diff --git a/poetry.lock b/poetry.lock
index 6f1ec37d8e..9d44f6fa3b 100644
--- a/poetry.lock
+++ b/poetry.lock
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:f001cc546693d59937130ed9cd49da4bd092d5ec7d1bf96b0419d76ab0271cf7
-size 562441
+oid sha256:f987ed9e1ad0fce8607e7884e389143189ef741be636a4d976437dafbd606cc3
+size 563115
diff --git a/pyproject.toml b/pyproject.toml
index a20868d877..e99b278341 100644
--- a/pyproject.toml
+++ b/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]
diff --git a/selfdrive/navd/map_renderer.py b/selfdrive/navd/map_renderer.py
index 868307bb63..3239470b23 100755
--- a/selfdrive/navd/map_renderer.py
+++ b/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