LiveMapData: Implementation

launch_chffrplus.sh

@@ -84,7 +84,7 @@ function launch {
 
   # start manager
   cd selfdrive/manager
-  ./build.py && ./manager.py
+  ./custom_dep.py && ./build.py && ./manager.py
 
   # if broken, keep on screen error
   while true; do sleep 1; done

pyproject.toml

@@ -112,6 +112,7 @@ tqdm = "*"
 websocket_client = "*"
 polyline = "*"
 sconscontrib = {git = "https://github.com/SCons/scons-contrib.git"}
+overpy = "==0.6"
 
 
 [tool.poetry.group.dev.dependencies]

selfdrive/controls/plannerd.py

@@ -42,7 +42,7 @@ def plannerd_thread():
   lateral_planner = LateralPlanner(CP, debug=debug_mode)
 
   pm = messaging.PubMaster(['longitudinalPlan', 'lateralPlan', 'uiPlan'])
-  sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2'],
+  sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'liveMapData'],
                            poll=['radarState', 'modelV2'], ignore_avg_freq=['radarState'])
 
   while True:

selfdrive/manager/custom_dep.py

@@ -0,0 +1,85 @@
+#!/usr/bin/env python3
+import os
+import sys
+import errno
+import shutil
+import time
+from urllib.request import urlopen
+from glob import glob
+import subprocess
+import importlib.util
+
+# NOTE: Do NOT import anything here that needs be built (e.g. params)
+from openpilot.common.spinner import Spinner
+from openpilot.common.basedir import BASEDIR
+
+sys.path.append(os.path.join(BASEDIR, "third_party"))
+OPSPLINE_SPEC = importlib.util.find_spec('scipy')
+OVERPY_SPEC = importlib.util.find_spec('overpy')
+MAX_BUILD_PROGRESS = 100
+TMP_DIR = '/data/tmp'
+THIRD_PARTY_DIR = '/data/openpilot/third_party'
+
+
+def wait_for_internet_connection(return_on_failure=False):
+  retries = 0
+  while True:
+    try:
+      _ = urlopen('https://www.google.com/', timeout=10)
+      return True
+    except Exception as e:
+      print(f'Wait for internet failed: {e}')
+      if return_on_failure and retries == 15:
+        return False
+      retries += 1
+      time.sleep(2)  # Wait for 2 seconds before retrying
+
+
+def install_dep(spinner):
+  wait_for_internet_connection()
+
+  TOTAL_PIP_STEPS = 2986
+
+  try:
+    os.makedirs(TMP_DIR)
+  except OSError as e:
+    if e.errno != errno.EEXIST:
+      raise
+  my_env = os.environ.copy()
+  my_env['TMPDIR'] = TMP_DIR
+
+  pip_target = [f'--target={THIRD_PARTY_DIR}']
+  packages = []
+  if OPSPLINE_SPEC is None:
+    packages.append('scipy==1.7.1')
+  if OVERPY_SPEC is None:
+    packages.append('overpy==0.6')
+
+  pip = subprocess.Popen([sys.executable, "-m", "pip", "install", "-v"] + pip_target + packages,
+                          stdout=subprocess.PIPE, env=my_env)
+
+  # Read progress from pip and update spinner
+  steps = 0
+  while True:
+    output = pip.stdout.readline()
+    if pip.poll() is not None:
+      break
+    if output:
+      steps += 1
+      spinner.update_progress(MAX_BUILD_PROGRESS * min(1., steps / TOTAL_PIP_STEPS), 100.)
+      print(output.decode('utf8', 'replace'))
+
+  shutil.rmtree(TMP_DIR)
+  os.unsetenv('TMPDIR')
+
+  # remove numpy installed to THIRD_PARTY_DIR since numpy is already present in the AGNOS image
+  if OPSPLINE_SPEC is None:
+    for directory in glob(f'{THIRD_PARTY_DIR}/numpy*'):
+      shutil.rmtree(directory)
+    shutil.rmtree(f'{THIRD_PARTY_DIR}/bin')
+
+
+if __name__ == "__main__" and (OPSPLINE_SPEC is None or OVERPY_SPEC is None):
+  spinner = Spinner()
+  spinner.update_progress(0, 100)
+  install_dep(spinner)

selfdrive/manager/manager.py

@@ -25,6 +25,8 @@
                            is_tested_branch, is_release_branch
 
 
+sys.path.append(os.path.join(BASEDIR, "third_party"))
+
 
 def manager_init() -> None:
   # update system time from panda

selfdrive/manager/process_config.py

@@ -81,6 +81,7 @@ def only_offroad(started, params, CP: car.CarParams) -> bool:
   PythonProcess("updated", "selfdrive.updated", only_offroad, enabled=not PC),
   PythonProcess("uploader", "system.loggerd.uploader", always_run),
   PythonProcess("statsd", "selfdrive.statsd", always_run),
+  PythonProcess("mapd", "selfdrive.mapd.mapd", only_onroad),
 
   # debug procs
   NativeProcess("bridge", "cereal/messaging", ["./bridge"], notcar),

selfdrive/mapd/config.py

@@ -0,0 +1,7 @@
+# Map query config
+
+QUERY_RADIUS = 3000  # mts. Radius to use on OSM data queries.
+MIN_DISTANCE_FOR_NEW_QUERY = 1000  # mts. Minimum distance to query area edge before issuing a new query.
+FULL_STOP_MAX_SPEED = 1.39  # m/s Max speed for considering car is stopped.
+LOOK_AHEAD_HORIZON_TIME = 15.  # s. Time horizon for look ahead of turn speed sections to provide on liveMapData msg.
+LANE_WIDTH = 3.7  # Lane width estimate. Used for detecting departures from way.