nav: add setting for extra coordinates to specify route (#26803)

common/params.cc

@@ -149,6 +149,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"LiveTorqueCarParams", PERSISTENT},
     {"LiveTorqueParameters", PERSISTENT | DONT_LOG},
     {"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
+    {"NavDestinationWaypoints", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
     {"NavSettingTime24h", PERSISTENT},
     {"NavSettingLeftSide", PERSISTENT},
     {"NavdRender", PERSISTENT},

selfdrive/navd/navd.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python3
+import json
 import math
 import os
 import threading
@@ -135,12 +136,27 @@ class RouteEngine:
       '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;"
+      params['bearings'] = f"{(self.last_bearing + 360) % 360:.0f},90" + (';'*(len(coords)-1))
 
-    url = self.mapbox_host + f'/directions/v5/mapbox/driving-traffic/{self.last_position.longitude},{self.last_position.latitude};{destination.longitude},{destination.latitude}'
+    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()

selfdrive/navd/set_destination.py

@@ -5,6 +5,29 @@ import sys
 from common.params import Params
 
 if __name__ == "__main__":
-  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 = 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)