diff --git a/CHANGELOGS.md b/CHANGELOGS.md index 36630a042b..981a98717c 100644 --- a/CHANGELOGS.md +++ b/CHANGELOGS.md @@ -5,6 +5,18 @@ sunnypilot - 0.9.6.1 (2023-xx-xx) * UPDATED: Vision-based Turn Speed Control (V-TSC) implementation * Refactored implementation thanks to pfeiferj! * More accurate and consistent velocity calculation to achieve smoother longitudinal control in curves +* UPDATED: OpenStreetMap (OSM) implementation + * Refactored implementation thanks to pfeiferj! + * Less resource impact + * Significantly smaller sizes with databases + * All regions are available to download + * Weekly map updates thanks to pfeiferj! + * Increased the font size of the road name + * C3X-specific changes + * Altitude (ALT.) display on Developer UI + * Current street name on top of driving screen when "OSM Debug UI" is enabled + * DISABLED: Map-based Turn Speed Control (M-TSC) + * Reimplementation in near future updates * UI updates * RE-ENABLED: Navigation: Full screen support * Display the map view in full screen diff --git a/common/params.cc b/common/params.cc index 1e00a751e0..1ea1f39602 100644 --- a/common/params.cc +++ b/common/params.cc @@ -178,6 +178,7 @@ std::unordered_map keys = { {"Offroad_TemperatureTooHigh", CLEAR_ON_MANAGER_START}, {"Offroad_UnofficialHardware", CLEAR_ON_MANAGER_START}, {"Offroad_UpdateFailed", CLEAR_ON_MANAGER_START}, + {"Offroad_OSMUpdateRequired", CLEAR_ON_MANAGER_START}, {"OpenpilotEnabledToggle", PERSISTENT}, {"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, {"PandaSomResetTriggered", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, @@ -274,9 +275,7 @@ std::unordered_map keys = { {"OnroadScreenOff", PERSISTENT}, {"OnroadScreenOffBrightness", PERSISTENT}, {"OnroadScreenOffEvent", PERSISTENT}, - {"OsmDbUpdatesCheck", PERSISTENT}, {"OsmLocal", PERSISTENT}, - {"OsmLocalDb", PERSISTENT}, {"OsmLocationName", PERSISTENT}, {"OsmLocationTitle", PERSISTENT}, {"OsmLocationUrl", PERSISTENT}, @@ -310,6 +309,21 @@ std::unordered_map keys = { {"VwAccType", PERSISTENT}, {"VwCCOnly", PERSISTENT}, {"Offroad_SupersededUpdate", PERSISTENT}, + + // PFEIFER - MAPD {{ + {"MapdVersion", PERSISTENT}, + {"RoadName", CLEAR_ON_ONROAD_TRANSITION}, + {"MapSpeedLimit", CLEAR_ON_ONROAD_TRANSITION}, + {"MapAdvisorySpeedLimit", CLEAR_ON_ONROAD_TRANSITION}, + {"NextMapSpeedLimit", CLEAR_ON_ONROAD_TRANSITION}, + {"OSMDownloadBounds", PERSISTENT}, + {"OSMDownloadLocations", PERSISTENT}, + {"OsmDownloadedDate", PERSISTENT}, + {"OsmStateTitle", PERSISTENT}, + {"OsmStateName", PERSISTENT}, + {"OSMDownloadProgress", CLEAR_ON_MANAGER_START}, + {"OsmDbUpdatesCheck", CLEAR_ON_MANAGER_START}, // mapd database update happens with device ON, reset on boot + // }} PFEIFER - MAPD }; } // namespace diff --git a/installer/custom/install_osm.sh b/installer/custom/install_osm.sh deleted file mode 100755 index 5eeeeed018..0000000000 --- a/installer/custom/install_osm.sh +++ /dev/null @@ -1,35 +0,0 @@ -# OSM Installation instructions: -# https://wiki.openstreetmap.org/wiki/Overpass_API/Installation - -# Install expat. All other needed libraries are already installed. -# g++ make expat libexpat1-dev zlib1g-dev -#sudo apt-get update -#sudo apt-get install expat - -# Add required path variables to environment -export OSM_ROOT=/data/media/0/osm -export OSM_VERSION=0.7.57 -export GZ_FILE=osm-3s_v${OSM_VERSION}.tar.xz -export OSM_DIR=${OSM_ROOT}/v${OSM_VERSION} -# export DB_DIR=/data/osm/db/ - -# Download and extract overpass library - -if [ ! -d ${OSM_ROOT} ]; then - mkdir -p ${OSM_ROOT} -fi -tar -vxf /data/openpilot/selfdrive/mapd/assets/${GZ_FILE} -C ${OSM_ROOT} - -# Configure and install overpass -#cd $(ls | grep $SOURCE_FILE_ROOT\.[0-9]*) -#cd osm-3s_v0.7.56 -#./configure CXXFLAGS="-O2" --prefix=$EXEC_DIR -#make install - -# Remove source files after installation -#cd .. -if [ -d ${OSM_DIR} ]; then - rm -rf ${OSM_DIR} -fi - -mv ${OSM_ROOT}/osm-3s_v${OSM_VERSION} ${OSM_ROOT}/v${OSM_VERSION} diff --git a/installer/custom/install_osm_db.sh b/installer/custom/install_osm_db.sh deleted file mode 100755 index 759a1ee139..0000000000 --- a/installer/custom/install_osm_db.sh +++ /dev/null @@ -1,35 +0,0 @@ -export OSM_DIR=/data/media/0/osm -export DB_DIR=${OSM_DIR}/db - -export OSM_LOCATION=$(cat /data/params/d/OsmLocationUrl) - -export OSM_LOCATION_TEXT=$(cat /data/params/d/OsmLocationName) - -export XZ_MAP_FILE_NAME=${OSM_LOCATION_TEXT}.tar.xz -export XZ_MAP_FILE=${OSM_DIR}/${XZ_MAP_FILE_NAME} - -# WD -cd $OSM_DIR - -# Remove legacy compressed map file if existing -rm -rf $XZ_MAP_FILE - -# Download map file -wget -O ${XZ_MAP_FILE_NAME} ${OSM_LOCATION} - -if [[ "$?" != 0 ]]; then - echo "Error downloading map file" -else - echo "Successfully downloaded map file" - # Remove current db dir if existing - rm -rf $DB_DIR - if [ -d ${OSM_DIR}/${OSM_LOCATION_TEXT} ]; then - rm -rf ${OSM_DIR}/${OSM_LOCATION_TEXT} - fi - # Decompressing - tar -vxf ${XZ_MAP_FILE_NAME} - mv ${OSM_LOCATION_TEXT} db - - # Remove compressed map files after expanding - rm -rf $XZ_MAP_FILE -fi diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index 45757e1a61..5112334488 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -84,12 +84,7 @@ function launch { # start manager cd selfdrive/manager - if [ ! -f "/data/params/d/OsmLocal" ]; then - ./custom_dep.py && ./build.py && ./manager.py - else - ./custom_dep.py && ./build.py && ./local_osm_install.py && ./manager.py - fi - + ./build.py && ./mapd_installer.py && ./manager.py # if broken, keep on screen error while true; do sleep 1; done } diff --git a/release/files_common b/release/files_common index f36f6c7d66..f02ff7563e 100644 --- a/release/files_common +++ b/release/files_common @@ -65,9 +65,6 @@ common/api/__init__.py release/* -installer/custom/install_osm.sh -installer/custom/install_osm_db.sh - tools/__init__.py tools/lib/* tools/bodyteleop/* @@ -155,11 +152,9 @@ selfdrive/debug/vw_mqb_config.py selfdrive/gpxd/gpx_uploader.py selfdrive/gpxd/gpxd.py -selfdrive/mapd/assets/* -selfdrive/mapd/config.py -selfdrive/mapd/mapd.py -selfdrive/mapd/lib/* -selfdrive/mapd/README.md +selfdrive/mapd_manager.py +selfdrive/sunnypilot/*.py +selfdrive/sunnypilot/live_map_data/*.py common/SConscript common/version.h @@ -378,9 +373,8 @@ system/fleetmanager/* selfdrive/manager/__init__.py selfdrive/manager/build.py -selfdrive/manager/custom_dep.py selfdrive/manager/helpers.py -selfdrive/manager/local_osm_install.py +selfdrive/manager/mapd_installer.py selfdrive/manager/manager.py selfdrive/manager/process_config.py selfdrive/manager/process.py @@ -480,6 +474,8 @@ third_party/acados/acados_template/** third_party/bootstrap/** third_party/qt5/larch64/bin/** +third_party/pfeiferj-mapd/** + scripts/update_now.sh scripts/stop_updater.sh diff --git a/selfdrive/controls/lib/alerts_offroad.json b/selfdrive/controls/lib/alerts_offroad.json index 35446ca22b..e08d67f43b 100644 --- a/selfdrive/controls/lib/alerts_offroad.json +++ b/selfdrive/controls/lib/alerts_offroad.json @@ -52,5 +52,9 @@ "Offroad_Recalibration": { "text": "openpilot detected a change in the device's mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield.", "severity": 0 + }, + "Offroad_OSMUpdateRequired": { + "text": "OpenStreetMap database is out of date. New maps must be downloaded if you wish to continue using OpenStreetMap data for Enhanced Speed Control and road name display.\n\n%1", + "severity": 0 } } diff --git a/selfdrive/controls/lib/sunnypilot/speed_limit_resolver.py b/selfdrive/controls/lib/sunnypilot/speed_limit_resolver.py index b9d1eb56db..c604f024fa 100644 --- a/selfdrive/controls/lib/sunnypilot/speed_limit_resolver.py +++ b/selfdrive/controls/lib/sunnypilot/speed_limit_resolver.py @@ -21,20 +21,20 @@ class SpeedLimitResolver: Policy.combined: [Source.car_state, Source.nav, Source.map_data], Policy.nav_only: [Source.nav] } - self._reset_limit_sources() + for source in Source: + self._reset_limit_sources(source) def change_policy(self, policy: Policy): self._policy = policy - def _reset_limit_sources(self): - self._limit_solutions.clear() - self._distance_solutions.clear() - for source in Source: - self._limit_solutions[source] = 0. - self._distance_solutions[source] = 0. + def _reset_limit_sources(self, source): + self._limit_solutions[source] = 0. + self._distance_solutions[source] = 0. + + def _is_sock_updated(self, sock): + return self._sm.alive[sock] and self._sm.updated[sock] def resolve(self, v_ego, current_speed_limit, sm): - self._reset_limit_sources() self._v_ego = v_ego self._current_speed_limit = current_speed_limit self._sm = sm @@ -49,36 +49,44 @@ class SpeedLimitResolver: self._get_from_map_data() def _get_from_car_state(self): + if not self._is_sock_updated('carState'): + debug('SL: No carState instruction for speed limit') + return + + self._reset_limit_sources(Source.car_state) self._limit_solutions[Source.car_state] = self._sm['carState'].cruiseState.speedLimit self._distance_solutions[Source.car_state] = 0. def _get_from_nav(self): - if not self._sm.alive['navInstruction']: + if not self._is_sock_updated('navInstruction'): debug('SL: No nav instruction for speed limit') return # Load limits from nav instruction + self._reset_limit_sources(Source.nav) self._limit_solutions[Source.nav] = self._sm['navInstruction'].speedLimit self._distance_solutions[Source.nav] = 0. def _get_from_map_data(self): sock = 'liveMapDataSP' - if self._sm.logMonoTime[sock] is None: + + if not self._is_sock_updated(sock): debug('SL: No map data for speed limit') return # Load limits from map_data + self._reset_limit_sources(Source.map_data) self._process_map_data(self._sm[sock]) def _process_map_data(self, map_data): - speed_limit = map_data.speedLimit if map_data.speedLimitValid else 0. - next_speed_limit = map_data.speedLimitAhead if map_data.speedLimitAheadValid else 0. - gps_fix_age = time.time() - map_data.lastGpsTimestamp * 1e-3 if gps_fix_age > LIMIT_MAX_MAP_DATA_AGE: debug(f'SL: Ignoring map data as is too old. Age: {gps_fix_age}') return + speed_limit = map_data.speedLimit if map_data.speedLimitValid else 0. + next_speed_limit = map_data.speedLimitAhead if map_data.speedLimitAheadValid else 0. + self._calculate_map_data_limits(speed_limit, next_speed_limit, map_data) def _calculate_map_data_limits(self, speed_limit, next_speed_limit, map_data): diff --git a/selfdrive/manager/custom_dep.py b/selfdrive/manager/custom_dep.py deleted file mode 100755 index a08fcb9802..0000000000 --- a/selfdrive/manager/custom_dep.py +++ /dev/null @@ -1,162 +0,0 @@ -#!/usr/bin/env python3 -import os -import sys -import errno -import shutil -import tarfile -import time -import traceback -from common.basedir import BASEDIR -from common.text_window import TextWindow -from urllib.request import urlopen -from glob import glob -import subprocess -import importlib.util -from importlib.metadata import version - -# NOTE: Do NOT import anything here that needs be built (e.g. params) -from common.spinner import Spinner - - -sys.path.append(os.path.join(BASEDIR, "third_party/mapd")) -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/mapd' -THIRD_PARTY_DIR_SP = '/data/third_party_community' -PRELOADED_DEP_FILE = os.path.join(BASEDIR, "selfdrive/mapd/assets/mapd_deps.tar.xz") -OPSPLINE_VERSION = "1.11.1" -OVERPY_VERSION = "0.6" -SPECS = { - 'scipy': OPSPLINE_VERSION, - 'overpy': OVERPY_VERSION, -} - - -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(f'scipy=={OPSPLINE_VERSION}') - if OVERPY_SPEC is None: - packages.append(f'overpy=={OVERPY_VERSION}') - - 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) - if os.path.exists(f'{THIRD_PARTY_DIR}/bin'): - shutil.rmtree(f'{THIRD_PARTY_DIR}/bin') - - dup = f'cp -rf {THIRD_PARTY_DIR} {THIRD_PARTY_DIR_SP}' - process_dup = subprocess.Popen(dup, stdout=subprocess.PIPE, shell=True) - - -if __name__ == "__main__": - reload_required = False - for package, req_version in SPECS.items(): - package_spec = importlib.util.find_spec(package) - if package_spec is not None and version(package) != req_version: - print(f"SP_LOG: current {package} is {version(package)}, requires {req_version}. Removing directory {THIRD_PARTY_DIR}...") - reload_required = True - if reload_required: - command = f'rm -rf {THIRD_PARTY_DIR}' - process = subprocess.Popen(command, stdout=subprocess.PIPE, shell=True) - if OPSPLINE_SPEC is None or OVERPY_SPEC is None or reload_required: - spinner = Spinner() - preload_fault = False - try: - if os.path.exists(PRELOADED_DEP_FILE): - spinner.update("Loading preloaded dependencies") - try: - with tarfile.open(PRELOADED_DEP_FILE, "r:xz") as tar: - for member in tar.getmembers(): - split_components = member.name.split('/') - if len(split_components) > 1: - member.name = '/'.join(split_components[1:]) - tar.extract(member, path=THIRD_PARTY_DIR) - print(f"SP_LOG: Preloaded dependencies extracted to {THIRD_PARTY_DIR}") - except Exception as e: - preload_fault = True - command = f'rm -rf {THIRD_PARTY_DIR}' - process = subprocess.Popen(command, stdout=subprocess.PIPE, shell=True) - print(f"SP_LOG: An error occurred while extracting preloaded dependencies: {e}") - print(f"SP_LOG: Cleanup directory {e}") - if not os.path.exists(PRELOADED_DEP_FILE) or preload_fault: - if os.path.exists(THIRD_PARTY_DIR_SP): - try: - spinner.update("Loading cached dependencies") - command = f'rm -rf {THIRD_PARTY_DIR}; cp -rf {THIRD_PARTY_DIR_SP} {THIRD_PARTY_DIR}' - process = subprocess.Popen(command, stdout=subprocess.PIPE, shell=True) - print(f"SP_LOG: Removed directory {THIRD_PARTY_DIR}") - print(f"SP_LOG: Copied {THIRD_PARTY_DIR_SP} to {THIRD_PARTY_DIR}") - except Exception as e: - command = f'rm -rf {THIRD_PARTY_DIR}' - process = subprocess.Popen(command, stdout=subprocess.PIPE, shell=True) - print(f"SP_LOG: An error occurred while loading cached dependencies: {e}") - print(f"SP_LOG: Cleanup directory {e}") - else: - spinner.update("Waiting for internet") - try: - install_dep(spinner) - except Exception as e: - command = f'rm -rf {THIRD_PARTY_DIR}' - process = subprocess.Popen(command, stdout=subprocess.PIPE, shell=True) - print(f"SP_LOG: An error occurred while downloading dependencies: {e}") - print(f"SP_LOG: Cleanup directory {e}") - except Exception: - command = f'rm -rf {THIRD_PARTY_DIR}' - process = subprocess.Popen(command, stdout=subprocess.PIPE, shell=True) - import selfdrive.sentry as sentry - sentry.init(sentry.SentryProject.SELFDRIVE) - traceback.print_exc() - sentry.capture_exception() - - error = traceback.format_exc(-3) - error = "Dependency Manager failed to start\n\n" + error - with TextWindow(error) as t: - t.wait_for_exit() diff --git a/selfdrive/manager/local_osm_install.py b/selfdrive/manager/local_osm_install.py deleted file mode 100755 index b0bb60a1ee..0000000000 --- a/selfdrive/manager/local_osm_install.py +++ /dev/null @@ -1,72 +0,0 @@ -#!/usr/bin/env python3 -import os -import subprocess -import sys -import time -import traceback - -# NOTE: Do NOT import anything here that needs be built (e.g. params) -from common.basedir import BASEDIR -from common.params import Params -from common.spinner import Spinner -from common.text_window import TextWindow -import selfdrive.sentry as sentry -from selfdrive.manager.custom_dep import wait_for_internet_connection - - -def install_local_osm(_spinner): - _install(_spinner, "./install_osm.sh", "Installing OSM Server") - - -def install_osm_db(_spinner): - _install(_spinner, "./install_osm_db.sh", "Installing OSM DB - " + Params().get("OsmLocationName", encoding="utf-8")) - - -def _install(_spinner, script, title): - _spinner.update(title) - process = subprocess.Popen(['sh', script], cwd=os.path.join(BASEDIR, 'installer/custom/'), - stdout=subprocess.PIPE) - # Read progress from install script and update spinner - frame = 0 - while True: - output = process.stdout.readline() - if process.poll() is not None: - break - _spinner.update(title + (".".replace(".", "." * (frame % 5), 1))) - frame += 1 - print(output.decode('utf8', 'replace')) - time.sleep(0.1) - - -if __name__ == "__main__": - try: - from selfdrive.mapd.lib.helpers import is_local_osm_installed, timestamp_local_osm_db, is_osm_db_up_to_date, OSM_LOCAL_PATH - sys.path.append(os.path.join(BASEDIR, "third_party/mapd")) - params = Params() - update_osm_db_check = params.get_bool("OsmDbUpdatesCheck") - if not (os.path.exists(f"{OSM_LOCAL_PATH}/db") or - os.path.exists(f"{OSM_LOCAL_PATH}/v0.7.57")) or update_osm_db_check: - spinner = Spinner() - spinner.update("Waiting for internet connection...") - if wait_for_internet_connection(return_on_failure=True): - is_osm_installed = is_local_osm_installed(params) - is_db_updated = is_osm_db_up_to_date() - print(f'Local OSM Installer:\nOSM currently installed: {is_osm_installed}\nDB up to date: {is_db_updated}') - - if not is_osm_installed: - install_local_osm(spinner) - if not is_db_updated: - install_osm_db(spinner) - timestamp_local_osm_db() - spinner.close() - - params.put_bool("OsmDbUpdatesCheck", False) - except Exception: - sentry.init(sentry.SentryProject.SELFDRIVE) - traceback.print_exc() - sentry.capture_exception() - - error = traceback.format_exc(-3) - error = "OSM Offline Database Manager failed to start\n\n" + error - with TextWindow(error) as t: - t.wait_for_exit() diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 2c9343707e..b5d3fcc4b4 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -16,6 +16,7 @@ from openpilot.common.text_window import TextWindow from openpilot.selfdrive.boardd.set_time import set_time from openpilot.system.hardware import HARDWARE, PC from openpilot.selfdrive.manager.helpers import unblock_stdout, write_onroad_params +from openpilot.selfdrive.manager.mapd_installer import VERSION from openpilot.selfdrive.manager.process import ensure_running from openpilot.selfdrive.manager.process_config import managed_processes from openpilot.selfdrive.athena.registration import register, UNREGISTERED_DONGLE_ID, is_registered_device @@ -100,6 +101,10 @@ def manager_init() -> None: ("TurnVisionControl", "0"), ("VisionCurveLaneless", "0"), ("VwAccType", "0"), + ("OsmDbUpdatesCheck", "0"), + ("OsmDownloadedDate", "0"), + ("OSMDownloadProgress", "{}"), + ("MapdVersion", f"{VERSION}"), ] if not PC: default_params.append(("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8'))) diff --git a/selfdrive/manager/mapd_installer.py b/selfdrive/manager/mapd_installer.py new file mode 100755 index 0000000000..2767115861 --- /dev/null +++ b/selfdrive/manager/mapd_installer.py @@ -0,0 +1,143 @@ +#!/usr/bin/env python3 +import logging +import os +import stat +import time +import traceback +import requests +from pathlib import Path +from urllib.request import urlopen +import openpilot.selfdrive.sentry as sentry +from cereal import messaging +from common.spinner import Spinner +from common.params import Params +from openpilot.selfdrive.mapd_manager import COMMON_DIR, MAPD_PATH, MAPD_BIN_DIR +from openpilot.system.version import is_prebuilt + +VERSION = 'v1.8.0' +URL = f"https://github.com/pfeiferj/openpilot-mapd/releases/download/{VERSION}/mapd" + + +class MapdInstallManager: + def __init__(self, spinner_ref: Spinner): + self._spinner = spinner_ref + + def download(self): + self.ensure_directories_exist() + self._download_file() + self.update_installed_version(VERSION) + + def check_and_download(self): + if self.download_needed(): + self.download() + + @staticmethod + def download_needed(): + return not os.path.exists(MAPD_PATH) or MapdInstallManager.get_installed_version() != VERSION + + @staticmethod + def ensure_directories_exist(): + if not os.path.exists(COMMON_DIR): + os.makedirs(COMMON_DIR) + if not os.path.exists(MAPD_BIN_DIR): + os.makedirs(MAPD_BIN_DIR) + + @staticmethod + def _safe_write_and_set_executable(file_path, content): + with open(file_path, 'wb') as output: + output.write(content) + output.flush() + os.fsync(output.fileno()) + current_permissions = stat.S_IMODE(os.lstat(file_path).st_mode) + os.chmod(file_path, current_permissions | stat.S_IEXEC) + + def _download_file(self, num_retries=5): + temp_file = Path(MAPD_PATH + ".tmp") + download_timeout = 60 + for cnt in range(num_retries): + try: + response = requests.get(URL, stream=True, timeout=download_timeout) + response.raise_for_status() + self._safe_write_and_set_executable(temp_file, response.content) + # No exceptions encountered. Safe to replace original file. + temp_file.replace(MAPD_PATH) + return + except requests.exceptions.ReadTimeout: + self._spinner.update(f"ReadTimeout caught. Timeout is [{download_timeout}]. Retrying download... [{cnt}]") + time.sleep(0.5) + except requests.exceptions.RequestException as e: + self._spinner.update(f"RequestException caught: {e}. Retrying download... [{cnt}]") + time.sleep(0.5) + + # Delete temp file if the process was not successful. + if temp_file.exists(): + temp_file.unlink() + logging.error("Failed to download file after all retries") + + @staticmethod + def update_installed_version(version): + Params().put("MapdVersion", version) + + @staticmethod + def get_installed_version(): + return Params().get("MapdVersion", encoding="utf-8") + + def wait_for_internet_connection(self, return_on_failure=False): + max_retries = 10 + for retries in range(max_retries+1): + self._spinner.update(f"Waiting for internet connection... [{retries}/{max_retries}]") + time.sleep(2) + try: + _ = urlopen('https://sentry.io', timeout=10) + return True + except Exception as e: + print(f'Wait for internet failed: {e}') + if return_on_failure and retries == max_retries: + return False + + def non_prebuilt_install(self): + sm = messaging.SubMaster(['deviceState']) + metered = sm['deviceState'].networkMetered + + if metered: + self._spinner.update(f"Can't proceed with mapd install since network is metered!") + time.sleep(5) + return False + + try: + self.ensure_directories_exist() + if not self.download_needed(): + self._spinner.update("Mapd is good!") + time.sleep(0.1) + return True + + if self.wait_for_internet_connection(return_on_failure=True): + self._spinner.update(f"Downloading pfeiferj's mapd [{install_manager.get_installed_version()}] => [{VERSION}].") + time.sleep(0.1) + self.check_and_download() + self._spinner.close() + + except Exception: + for i in range(6): + self._spinner.update(f"Failed to download OSM maps won't work until properly downloaded!" + f"Try again manually rebooting. " + f"Boot will continue in {5 - i}s...") + time.sleep(1) + + sentry.init(sentry.SentryProject.SELFDRIVE) + traceback.print_exc() + sentry.capture_exception() + + +if __name__ == "__main__": + spinner = Spinner() + install_manager = MapdInstallManager(spinner) + install_manager.ensure_directories_exist() + if is_prebuilt(): + debug_msg = f"[DEBUG] This is prebuilt, no mapd install required. VERSION: [{VERSION}], Param [{install_manager.get_installed_version()}]" + spinner.update(debug_msg) + install_manager.update_installed_version(VERSION) + else: + spinner.update(f"Checking if mapd is installed and valid. Prebuilt [{is_prebuilt()}]") + time.sleep(1) + install_manager.non_prebuilt_install() diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 23cff24a97..cabe87f18b 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -4,6 +4,7 @@ from cereal import car from openpilot.common.params import Params from openpilot.system.hardware import PC, TICI from openpilot.selfdrive.manager.process import PythonProcess, NativeProcess, DaemonProcess +from openpilot.selfdrive.mapd_manager import MAPD_PATH, COMMON_DIR WEBCAM = os.getenv("USE_WEBCAM") is not None @@ -82,7 +83,11 @@ procs = [ PythonProcess("statsd", "selfdrive.statsd", always_run), NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(5 if not PC else None), always_watchdog=True), - PythonProcess("mapd", "selfdrive.mapd.mapd", only_onroad), + # PFEIFER - MAPD {{ + NativeProcess("mapd", COMMON_DIR, [MAPD_PATH], always_run), + PythonProcess("mapd_manager", "selfdrive.mapd_manager", always_run), + # }} PFEIFER - MAPD + PythonProcess("otisserv", "selfdrive.navd.otisserv", always_run), PythonProcess("fleet_manager", "system.fleetmanager.fleet_manager", always_run), diff --git a/selfdrive/mapd/README.md b/selfdrive/mapd/README.md deleted file mode 100644 index bd3a3c9497..0000000000 --- a/selfdrive/mapd/README.md +++ /dev/null @@ -1,8 +0,0 @@ -# MapD -The OpenStreetMap-based speed logical by the [Move Fast team](https://github.com/move-fast), [dragonpilot team](https://github.com/dragonpilot-community/dragonpilot), and additional improvements by [sunnypilot](https://github.com/sunnyhaibin/sunnypilot). - -The comma three uses regular SciPy. To have a better experience with `mapd`, please go to [OpenStreetMap](https://openstreetmap.org) to update and improve your area's data (i.e., Speed Limit, Stop Signs, Traffic Lights). - -To use `mapd`, you consent to `mapd` uploading the traces. You may opt out of uploading traces at any time. - -© OpenStreetMap contributors diff --git a/selfdrive/mapd/assets/mapd_deps.tar.xz b/selfdrive/mapd/assets/mapd_deps.tar.xz deleted file mode 100644 index 05e23804ad..0000000000 Binary files a/selfdrive/mapd/assets/mapd_deps.tar.xz and /dev/null differ diff --git a/selfdrive/mapd/assets/osm-3s_v0.7.57.tar.xz b/selfdrive/mapd/assets/osm-3s_v0.7.57.tar.xz deleted file mode 100644 index 880a400338..0000000000 Binary files a/selfdrive/mapd/assets/osm-3s_v0.7.57.tar.xz and /dev/null differ diff --git a/selfdrive/mapd/config.py b/selfdrive/mapd/config.py deleted file mode 100644 index afa6866601..0000000000 --- a/selfdrive/mapd/config.py +++ /dev/null @@ -1,8 +0,0 @@ -# Map query config - -QUERY_RADIUS = 3000 # mts. Radius to use on OSM data queries. -QUERY_RADIUS_OFFLINE = 2250 # mts. Radius to use on offline 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 liveMapDataSP msg. -LANE_WIDTH = 3.7 # Lane width estimate. Used for detecting departures from way. diff --git a/selfdrive/mapd/lib/NodesData.py b/selfdrive/mapd/lib/NodesData.py deleted file mode 100644 index 8bb02ecb45..0000000000 --- a/selfdrive/mapd/lib/NodesData.py +++ /dev/null @@ -1,452 +0,0 @@ -import numpy as np -from enum import Enum -from selfdrive.mapd.lib.geo import DIRECTION, R, vectors - -from scipy.interpolate import splev, splprep - - -_TURN_CURVATURE_THRESHOLD = 0.002 # 1/mts. A curvature over this value will generate a speed limit section. -_MAX_LAT_ACC = 2. # Maximum lateral acceleration in turns. -_SPLINE_EVAL_STEP = 5 # mts for spline evaluation for curvature calculation -_MIN_SPEED_SECTION_LENGTH = 100. # mts. Sections below this value will not be split in smaller sections. -_MAX_CURV_DEVIATION_FOR_SPLIT = 2. # Split a speed section if the max curvature deviates from mean by this factor. -_MAX_CURV_SPLIT_ARC_ANGLE = 90. # degrees. Arc section to split into new speed section around max curvature. -_MIN_NODE_DISTANCE = 50. # mts. Minimum distance between nodes for spline evaluation. Data is enhanced if not met. -_ADDED_NODES_DIST = 15. # mts. Distance between added nodes when data is enhanced for spline evaluation. -_DIVERTION_SEARCH_RANGE = [-200., 50.] # mt. Range of distance to current location for diversion search. - - -def nodes_raw_data_array_for_wr(wr, drop_last=False, feature_sl=False): - """Provides an array of raw node data (id, lat, lon, speed_limit, advisory_speed_limit) for all nodes in way relation - """ - sl = wr.speed_limit - asl = wr.advisory_speed_limit - data = np.array([(n.id, n.lat, n.lon, sl, asl) for n in wr.way.nodes], dtype=float) - - if feature_sl: - for count, node in enumerate(wr.way.nodes): - if 'highway' in node.tags: - if node.tags['highway'] == 'mini_roundabout': - data[count][3] = 4.1667 - - if 'direction' in node.tags and (node.tags['highway'] == 'stop' or node.tags['highway'] == 'give_way'): - if (wr.direction == DIRECTION.BACKWARD and node.tags['direction'] == 'backward') or (wr.direction == DIRECTION.FORWARD and node.tags['direction'] == 'forward'): - if node.tags['highway'] == 'give_way': - data[count][3] = 2.7777 - if node.tags['highway'] == 'stop': - data[count][3] = 0.1 - if 'traffic_calming' in node.tags: - if node.tags['traffic_calming'] == 'yes': - data[count][3] = 40/3.6 - if node.tags['traffic_calming'] == 'chicane' or node.tags['traffic_calming'] == 'choker': - data[count][3] = 20/3.6 - if node.tags['traffic_calming'] == 'bump': - data[count][3] = 2.24 - if node.tags['traffic_calming'] == 'hump': - data[count][3] = 8.94 - - # reverse the order if way direction is backwards - if wr.direction == DIRECTION.BACKWARD: - data = np.flip(data, axis=0) - - # drop last if requested - return data[:-1] if drop_last else data - - -def node_calculations(points): - """Provides node calculations based on an array of (lat, lon) points in radians. - points is a (N x 1) array where N >= 3 - """ - if len(points) < 3: - raise(IndexError) - - # Get the vector representation of node points in cartesian plane. - # (N-1, 2) array. Not including (0., 0.) - v = vectors(points) * R - - # Calculate the vector magnitudes (or distance) - # (N-1, 1) array. No distance for v[-1] - d = np.linalg.norm(v, axis=1) - - # Calculate the bearing (from true north clockwise) for every node. - # (N-1, 1) array. No bearing for v[-1] - b = np.arctan2(v[:, 0], v[:, 1]) - - # Add origin to vector space. (i.e first node in list) - v = np.concatenate(([[0., 0.]], v)) - - # Provide distance to previous node and distance to next node - dp = np.concatenate(([0.], d)) - dn = np.concatenate((d, [0.])) - - # Provide cumulative distance on route - dr = np.cumsum(dp, axis=0) - - # Bearing of last node should keep bearing from previous. - b = np.concatenate((b, [b[-1]])) - - return v, dp, dn, dr, b - - -def spline_curvature_calculations(vect, dist_prev): - """Provides an array of curvatures and its distances by applying a spline interpolation - to the path described by the nodes data. - """ - # We need to artificially enhance the data before applying spline interpolation to avoid getting - # inexistent curvature values close to irregularities on the road when the resolution of nodes data - # approaching the irregularity is low. - - # - Find indexes where dist_prev is greater than threshold - too_far_idxs = np.nonzero(dist_prev >= _MIN_NODE_DISTANCE)[0] - - # - Traversing in reverse order, enhance data by adding points at the found indexes. - for idx in too_far_idxs[::-1]: - dp = dist_prev[idx] # distance of vector that needs to be replaced by higher resolution vectors. - n = int(np.ceil(dp / _ADDED_NODES_DIST)) # number of vectors that need to be added. - new_v = vect[idx, :] / n # new relative vector to insert. - vect = np.delete(vect, idx, axis=0) # remove the relative vector to be replaced by the insertion of new vectors. - vect = np.insert(vect, [idx] * n, [new_v] * n, axis=0) # insert n new relative vectors - - # Data is now enhanced, we can proceed with curvature evaluation. - # - Create cumulative arrays for distance traveled and vector (x, y) - ds = np.cumsum(dist_prev, axis=0) - vs = np.cumsum(vect, axis=0) - - # - spline interpolation - tck, u = splprep([vs[:, 0], vs[:, 1]]) # pylint: disable=unbalanced-tuple-unpacking - - # - evaluate every _SPLINE_EVAL_STEP mts. - n = max(int(ds[-1] / _SPLINE_EVAL_STEP), len(u)) - unew = np.arange(0, n + 1) / n - - # - get derivatives - d1 = splev(unew, tck, der=1) - d2 = splev(unew, tck, der=2) - - # - calculate curvatures - num = d1[0] * d2[1] - d1[1] * d2[0] - den = (d1[0]**2 + d1[1]**2)**(1.5) - curv = num / den - curv_ds = unew * ds[-1] - - return curv, curv_ds - - -def speed_section(curv_sec): - """Map curvature section data into turn speed sections data. - Returns: [section start distance, section end distance, speed limit based on max curvature, sing of curvature] - """ - max_curv_idx = np.argmax(curv_sec[:, 0]) - start = np.amin(curv_sec[:, 2]) - end = np.amax(curv_sec[:, 2]) - - return np.array([start, end, np.sqrt(_MAX_LAT_ACC / curv_sec[max_curv_idx, 0]), curv_sec[max_curv_idx, 1]]) - - -def split_speed_section_by_sign(curv_sec): - """Will split the given curvature section in subsections if there is a change of sign on the curvature value - in the section. - """ - # Find the indexes where the curvatures change signs (if any). - c_idx = np.nonzero(np.diff(curv_sec[:, 1]))[0] + 1 - - # Split section base on change of sign. - return np.split(curv_sec, c_idx) - - -def split_speed_section_by_curv_degree(curv_sec): - """Will split the given curvature section in subsections as to isolate peaks of turn with substantially - higher curvature values. This will aid on preventing having very long turn sections with low speed limit - that is only really necessary for a small region of the section. - """ - # Only consider splitting a section if long enough. - length = curv_sec[-1, 2] - curv_sec[0, 2] - if length <= _MIN_SPEED_SECTION_LENGTH: - return [curv_sec] - - # Only split if max curvature deviates substantially from mean curvature. - max_curv_idx = np.argmax(curv_sec[:, 0]) - max_curv = curv_sec[max_curv_idx, 0] - mean_curv = np.mean(curv_sec[:, 0]) - if max_curv / mean_curv <= _MAX_CURV_DEVIATION_FOR_SPLIT: - return [curv_sec] - - # Calculate where to split as to isolate a curve section around the max curvature peak. - arc_side = (np.radians(_MAX_CURV_SPLIT_ARC_ANGLE) / max_curv) / 2. - arc_side_idx_lenght = int(np.ceil(arc_side / _SPLINE_EVAL_STEP)) - split_idxs = [max_curv_idx - arc_side_idx_lenght, max_curv_idx + arc_side_idx_lenght] - split_idxs = list(filter(lambda idx: idx > 0 and idx < len(curv_sec) - 1, split_idxs)) - - # If the arc section to split extendes outside the section, then no need to split. - if len(split_idxs) == 0: - return [curv_sec] - - # Create the splits and split the resulting sections recursevly. - splits = [split_speed_section_by_curv_degree(cs) for cs in np.split(curv_sec, split_idxs)] - - # Flatten the results and return the new list of curvature sections. - curv_secs = [cs for split in splits for cs in split] - return curv_secs - - -def speed_limits_for_curvatures_data(curv, dist): - """Provides the calculations for the speed limits from the curvatures array and distances, - by providing distances to curvature sections and corresponding speed limit values as well as - curvature direction/sign. - """ - # Prepare a data array for processing with absolute curvature values, curvature sign and distances. - curv_abs = np.abs(curv) - data = np.column_stack((curv_abs, np.sign(curv), dist)) - - # Find where curvatures overshoot turn curvature threshold and define as section - is_section = curv_abs >= _TURN_CURVATURE_THRESHOLD - - # Find the indexes where the sections start and end. i.e. change indexes. - c_idx = np.nonzero(np.diff(is_section))[0] + 1 - - # Create independent arrays for each split section base on change indexes. - splits = np.array(np.split(data, c_idx), dtype=object) - - # Filter the splits to keep only the curvature section arrays by getting the odd or even split arrays depending - # on whether the first split is a curvature split or not. - curv_sec_idxs = np.arange(0 if is_section[0] else 1, len(splits), 2, dtype=int) - curv_secs = splits[curv_sec_idxs] - - # Further split the curv sections by sign change - sub_secs = [split_speed_section_by_sign(cs) for cs in curv_secs] - curv_secs = [cs for sub_sec in sub_secs for cs in sub_sec] - - # Further split the curv sections by degree of curvature - sub_secs = [split_speed_section_by_curv_degree(cs) for cs in curv_secs] - curv_secs = [cs for sub_sec in sub_secs for cs in sub_sec] - - # Return an array where each row represents a turn speed limit section. - # [start, end, speed_limit, curvature_sign] - return np.array([speed_section(cs) for cs in curv_secs]) - -def is_wr_a_valid_divertion_from_node(wr, node_id, wr_ids): - """ - Evaluates if the way relation `wr` is a valid diversion from node with id `node_id`. - A valid diversion is a way relation with an edge node with the given `node_id` that is not already included - in the list of way relations in the route (`wr_ids`) and that can be travaled in the direction as if starting - from node with id `node_id` - """ - if wr.id in wr_ids: - return False - wr.update_direction_from_starting_node(node_id) - return not wr.is_prohibited - - -class SpeedLimitSection(): - """And object representing a speed limited road section ahead. - provides the start and end distance and the speed limit value - """ - def __init__(self, start, end, value): - self.start = start - self.end = end - self.value = value - - def __repr__(self): - return f'from: {self.start}, to: {self.end}, limit: {self.value}' - - -class TurnSpeedLimitSection(SpeedLimitSection): - def __init__(self, start, end, value, sign): - super().__init__(start, end, value) - self.curv_sign = sign - - def __repr__(self): - return f'{super().__repr__()}, sign: {self.curv_sign}' - - -class NodeDataIdx(Enum): - """Column index for data elements on NodesData underlying data store. - """ - node_id = 0 - lat = 1 - lon = 2 - speed_limit = 3 - advisory_speed_limit = 4 - x = 5 # x value of cartesian vector representing the section between last node and this node. - y = 6 # y value of cartesian vector representing the section between last node and this node. - dist_prev = 7 # distance to previous node. - dist_next = 8 # distance to next node - dist_route = 9 # cumulative distance on route - bearing = 10 # bearing of the vector departing from this node. - - -class NodesData: - """Container for the list of node data from a ordered list of way relations to be used in a Route - """ - def __init__(self, way_relations, wr_index): - self._nodes_data = np.array([]) - self._divertions = [[]] - self._curvature_speed_sections_data = np.array([]) - - way_count = len(way_relations) - if way_count == 0: - return - - # We want all the nodes from the last way section - nodes_data = nodes_raw_data_array_for_wr(way_relations[-1]) - - # For the ways before the last in the route we want all the nodes but the last, as that one is the first on - # the next section. Collect them, append last way node data and concatenate the numpy arrays. - if way_count > 1: - wrs_data = tuple([nodes_raw_data_array_for_wr(wr, drop_last=True) for wr in way_relations[:-1]]) - wrs_data += (nodes_data,) - nodes_data = np.concatenate(wrs_data) - - # Get a subarray with lat, lon to compute the remaining node values. - lat_lon_array = nodes_data[:, [1, 2]] - points = np.radians(lat_lon_array) - # Ensure we have more than 3 points, if not calculations are not possible. - if len(points) <= 3: - return - vect, dist_prev, dist_next, dist_route, bearing = node_calculations(points) - - # append calculations to nodes_data - # nodes_data structure: [id, lat, lon, speed_limit, advisory_speed_limit, x, y, dist_prev, dist_next, dist_route, bearing] - self._nodes_data = np.column_stack((nodes_data, vect, dist_prev, dist_next, dist_route, bearing)) - - # Build route diversion options data from the wr_index. - wr_ids = [wr.id for wr in way_relations] - self._divertions = [[wr for wr in wr_index.way_relations_with_edge_node_id(node_id) - if is_wr_a_valid_divertion_from_node(wr, node_id, wr_ids)] - for node_id in nodes_data[:, 0]] - - # Store calculcations for curvature sections speed limits. We need more than 3 points to be able to process. - # _curvature_speed_sections_data structure: [dist_start, dist_stop, speed_limits, curv_sign] - if len(vect) > 3: - curv, curv_ds = spline_curvature_calculations(vect, dist_prev) - self._curvature_speed_sections_data = speed_limits_for_curvatures_data(curv, curv_ds) - - @property - def count(self): - return len(self._nodes_data) - - def get(self, node_data_idx): - """Returns the array containing all the elements of a specific NodeDataIdx type. - """ - if len(self._nodes_data) == 0 or node_data_idx.value >= self._nodes_data.shape[1]: - return np.array([]) - - return self._nodes_data[:, node_data_idx.value] - - def speed_limits_ahead(self, ahead_idx, distance_to_node_ahead): - """Returns and array of SpeedLimitSection objects for the actual route ahead of current location - """ - if len(self._nodes_data) == 0 or ahead_idx is None: - return [] - - # Find the cumulative distances where speed limit changes. Build Speed limit sections for those. - dist = np.concatenate(([distance_to_node_ahead], self.get(NodeDataIdx.dist_next)[ahead_idx:])) - dist = np.cumsum(dist, axis=0) - sl = self.get(NodeDataIdx.speed_limit)[ahead_idx - 1:] - sl_next = np.concatenate((sl[1:], [0.])) - - # Create a boolean mask where speed limit changes and filter values - sl_change = sl != sl_next - distances = dist[sl_change] - speed_limits = sl[sl_change] - - # Create speed limits sections combining all continuous nodes that have same speed limit value. - start = 0. - limits_ahead = [] - for idx, end in enumerate(distances): - limits_ahead.append(SpeedLimitSection(start, end, speed_limits[idx])) - start = end - - return limits_ahead - - - def advisory_speed_limits_ahead(self, ahead_idx, distance_to_node_ahead): - """Returns and array of SpeedLimitSection objects for the actual route ahead of current location - """ - if len(self._nodes_data) == 0 or ahead_idx is None: - return [] - - # Find the cumulative distances where speed limit changes. Build Speed limit sections for those. - dist = np.concatenate(([distance_to_node_ahead], self.get(NodeDataIdx.dist_next)[ahead_idx:])) - dist = np.cumsum(dist, axis=0) - sl = self.get(NodeDataIdx.advisory_speed_limit)[ahead_idx - 1:] - sl_next = np.concatenate((sl[1:], [0.])) - - # Create a boolean mask where speed limit changes and filter values - sl_change = sl != sl_next - distances = dist[sl_change] - speed_limits = sl[sl_change] - - # Create speed limits sections combining all continuous nodes that have same speed limit value. - start = 0. - limits_ahead = [] - for idx, end in enumerate(distances): - if speed_limits[idx] != None and speed_limits[idx] > 0: - limits_ahead.append(SpeedLimitSection(start, end, speed_limits[idx])) - start = end - - return limits_ahead - - - def distance_to_end(self, ahead_idx, distance_to_node_ahead): - if len(self._nodes_data) == 0 or ahead_idx is None: - return None - - return np.sum(np.concatenate(([distance_to_node_ahead], self.get(NodeDataIdx.dist_next)[ahead_idx:]))) - - def curvatures_speed_limit_sections_ahead(self, ahead_idx, distance_to_node_ahead): - """Returns and array of TurnSpeedLimitSection objects for the actual route ahead of current location for - speed limit sections due to curvatures in the road. - """ - if len(self._curvature_speed_sections_data) == 0 or ahead_idx is None: - return [] - - # Find the current distance traveled so far on the route. - dist_curr = self.get(NodeDataIdx.dist_route)[ahead_idx] - distance_to_node_ahead - - # Filter the sections to get only those where the stop distance is ahead of current. - sec_filter = self._curvature_speed_sections_data[:, 1] > dist_curr - data = self._curvature_speed_sections_data[sec_filter] - - # Offset distances to current distance. - data[:, [0, 1]] -= dist_curr - - # Create speed limits sections - limits_ahead = [TurnSpeedLimitSection(max(0., d[0]), d[1], d[2], d[3]) for d in data] - - advisory_speed_limits_ahead = self.advisory_speed_limits_ahead(ahead_idx, distance_to_node_ahead) - for advisory_limit in advisory_speed_limits_ahead: - for limit in limits_ahead: - if limit.start >= advisory_limit.start and limit.end <= advisory_limit.end: - limit.value = advisory_limit.value - - - return limits_ahead - - def possible_divertions(self, ahead_idx, distance_to_node_ahead): - """ Returns and array with the way relations the route could possible divert to by finding - the alternative way diversions on the nodes in the vicinity of the current location. - """ - if len(self._nodes_data) == 0 or ahead_idx is None: - return [] - - dist_route = self.get(NodeDataIdx.dist_route) - rel_dist = dist_route - dist_route[ahead_idx] + distance_to_node_ahead - valid_idxs = np.nonzero(np.logical_and(rel_dist >= _DIVERTION_SEARCH_RANGE[0], - rel_dist <= _DIVERTION_SEARCH_RANGE[1]))[0] - valid_divertions = [self._divertions[i] for i in valid_idxs] - - return [wr for wrs in valid_divertions for wr in wrs] # flatten. - - def distance_to_node(self, node_id, ahead_idx, distance_to_node_ahead): - """ - Provides the distance to a specific node in the route identified by `node_id` in reference to the node ahead - (`ahead_idx`) and the distance from current location to the node ahead (`distance_to_node_ahead`). - """ - node_ids = self.get(NodeDataIdx.node_id) - node_idxs = np.nonzero(node_ids == node_id)[0] - if len(self._nodes_data) == 0 or ahead_idx is None or len(node_idxs) == 0: - return None - - return self.get(NodeDataIdx.dist_route)[node_idxs[0]] - self.get(NodeDataIdx.dist_route)[ahead_idx] + \ - distance_to_node_ahead diff --git a/selfdrive/mapd/lib/Route.py b/selfdrive/mapd/lib/Route.py deleted file mode 100644 index a292a3257a..0000000000 --- a/selfdrive/mapd/lib/Route.py +++ /dev/null @@ -1,343 +0,0 @@ -from selfdrive.mapd.lib.NodesData import NodesData, NodeDataIdx -from selfdrive.mapd.config import QUERY_RADIUS -from selfdrive.mapd.lib.geo import ref_vectors, R, distance_to_points -from itertools import compress -import numpy as np - - -_ACCEPTABLE_BEARING_DELTA_COSINE = -0.7 # Continuation paths with a bearing of 180 +/- 45 degrees. -_MAX_ALLOWED_BEARING_DELTA_COSINE_AT_EDGE = -0.3420 # bearing delta at route edge must be 180 +/- 70 degrees. -_MAP_DATA_EDGE_DISTANCE = 50 # mts. Consider edge of map data from this distance to edge of query radius. - - -class Route(): - """A set of consecutive way relations forming a default driving route. - """ - def __init__(self, current, wr_index, way_collection_id, query_center): - """Create a Route object from a given `wr_index` (Way relation index) - - Args: - current (WayRelation): The Way Relation that is currently located. It must be active. - wr_index (WayRelationIndex): The indexes of WayRelations by node id. - way_collection_id (UUID): The id of the Way Collection that created this Route. - query_center (Numpy Array): lat, lon] numpy array in radians indicating the center of the data query. - """ - self.way_collection_id = way_collection_id - self._ordered_way_relations = [] - self._nodes_data = None - self._reset() - - # An active current way is needed to be able to build a route - if not current.active: - return - - # Build the route by finding iteratavely the best matching ways continuing after the end of the - # current (last_wr) way. Use the index to find the continuation possibilities on each iteration. - last_wr = current - ordered_way_ids = [] - split_wrs = [] - while True: - try: - # - Append current element to the route list of ordered way relations. - self._ordered_way_relations.append(last_wr) - ordered_way_ids.append(last_wr.id) - - # - Get the id of the node at the end of the way and then fetch the way relations that share the end node id. - last_node_id = last_wr.last_node.id - way_relations = wr_index.way_relations_with_edge_node_id(last_node_id) - - # - Add split way relations when necessary and remove parent way relations. - split_wrs_to_add = [wr for wr in split_wrs if last_node_id in wr.edge_nodes_ids] - way_relations.extend(split_wrs_to_add) - parent_ids = [wr.parent_wr_id for wr in split_wrs_to_add] - way_relations = [wr for wr in way_relations if wr.id not in parent_ids] - - # - If no more way_relations than last_wr, we have to check if we join another wr on an internal node, and - # if we do, we replace such way relation with the split of it and continue. - if len(way_relations) == 1: - way_relations = wr_index.way_relations_with_node_id(last_node_id) - # If no more way_relations than last_wr or its parent, we got to the end. - if len(way_relations) == 1: - break - - # If last_wr is a split, replace its parent with last_wr - way_relations = [last_wr if wr is last_wr.parent else wr for wr in way_relations] - - # If we join a wr on an internal node, then we artificially split the wr in two and pass both wrs as - # candidates to the wr selection code below. - wr_to_split = [wr for wr in way_relations if wr is not last_wr][0] - next_split_way_id = -len(split_wrs) - 1 # Keep split wrs ids unique on Route - new_wrs = wr_to_split.split(last_node_id, [next_split_way_id, next_split_way_id - 1]) - # If it could not be splited, we are done. - if len(new_wrs) != 2: - break - - # Replace the original way relation for the splitted version on way_relations and track splited wrs. - split_wrs.extend(new_wrs) - way_relations.remove(wr_to_split) - way_relations.extend(new_wrs) - - # - Get the coordinates for the edge node and build the array of coordinates for the nodes before the edge node - # on each of the common way relations, then get the vectors in cartesian plane for the end sections of each way. - ref_point = last_wr.last_node_coordinates - points = np.array([wr.node_before_edge_coordinates(last_node_id) for wr in way_relations]) - v = ref_vectors(ref_point, points) * R - - # - Calculate the bearing (from true north clockwise) for every end section of each way. - b = np.arctan2(v[:, 0], v[:, 1]) - - # - Find index of las_wr section and calculate deltas of bearings to the other sections. - last_wr_idx = way_relations.index(last_wr) - b_ref = b[last_wr_idx] - delta = b - b_ref - - # - Update the direction of the possible route continuation ways as starting from last_node_id. - # Make sure to exclude any ways already included in the ordered list as to not modify direction when there - # are looping roads (like roundabouts). A way will never be included twice in a route anyway. - for wr in way_relations: - if wr.id not in ordered_way_ids: - wr.update_direction_from_starting_node(last_node_id) - - # - Filter the possible route continuation way relations: - # - exclude any way already added to the ordered list. - # - exclude all way relations that are prohibited due to traffic direction. - mask = [wr.id not in ordered_way_ids and not wr.is_prohibited for wr in way_relations] - way_relations = list(compress(way_relations, mask)) - delta = delta[mask] - - # if no options left, we got to the end. - if len(way_relations) == 0: - break - - # - The cosine of the bearing delta will aid us in choosing the way that continues. The cosine is - # minimum (-1) for a perfect straight continuation as delta would be pi or -pi. - cos_delta = np.cos(delta) - - def pick_best_idx(cos_delta): - """Selects the best index on `cos_delta` array for a way that continues the route. - In principle we want to choose the way that continues as straight as possible. - Bue we need to make sure that if there are 2 or more ways continuing relatively straight, then we - need to disambiguate, either by matching the `ref` or `name` value of the continuing way with the - last way selected. - This can prevent cases where the chosen route could be for instance an exit ramp of a way due to the fact - that the ramp has a better match on bearing to previous way. We choose to stay on the road with the same `ref` - or `name` value if available. - If there is no ambiguity or there are no `name` or `ref` values to disambiguate, then we pick the one with - the straightest following direction. - """ - # Find the indexes of the cosine of the deltas that are considered straight enough to continue. - idxs = np.nonzero(cos_delta < _ACCEPTABLE_BEARING_DELTA_COSINE)[0] - - # If no amiguity or no way to break it, just return the straightest line. - if len(idxs) <= 1 or (last_wr.ref is None and last_wr.name is None): - # The section with the best continuation is the one with a bearing delta closest to pi. This is equivalent - # to taking the one with the smallest cosine of the bearing delta, as cosine is minimum (-1) on both pi - # and -pi. - return np.argmin(cos_delta) - - wrs = [way_relations[idx] for idx in idxs] - - # If we find a continuation way with the same reference we just choose it. - refs = list(map(lambda wr: wr.ref, wrs)) - if last_wr.ref is not None: - idx = next((idx for idx, ref in enumerate(refs) if ref == last_wr.ref), None) - if idx is not None: - return idxs[idx] - - # If we find a continuation way with the same name we just choose it. - names = list(map(lambda wr: wr.name, wrs)) - if last_wr.name is not None: - idx = next((idx for idx, name in enumerate(names) if name == last_wr.name), None) - if idx is not None: - return idxs[idx] - - # We did not manage to deambiguate, choose straightest path. - return np.argmin(cos_delta) - - # Get the index of the continuation way. - best_idx = pick_best_idx(cos_delta) - - # - Make sure to not select as route continuation a way that turns too much if we are close to the border of - # map data queried. This is to avoid building a route that takes a sharp turn just because we do not have the - # data for the way that actually continues straight. - if cos_delta[best_idx] > _MAX_ALLOWED_BEARING_DELTA_COSINE_AT_EDGE: - dist_to_center = distance_to_points(query_center, np.array([ref_point]))[0] - if dist_to_center > QUERY_RADIUS - _MAP_DATA_EDGE_DISTANCE: - break - - # - Select next way. - last_wr = way_relations[best_idx] - except Exception as e: - print("Exception \"", str(e), "\" caught") - - # Build the node data from the ordered list of way relations - self._nodes_data = NodesData(self._ordered_way_relations, wr_index) - - # Locate where we are in the route node list. - self._locate() - - def __repr__(self): - count = self._nodes_data.count if self._nodes_data is not None else None - return f'Route: {self.way_collection_id}, idx ahead: {self._ahead_idx} of {count}' - - def _reset(self): - self._limits_ahead = None - self._cuvature_limits_ahead = None - self._curvatures_ahead = None - self._ahead_idx = None - self._distance_to_node_ahead = None - - @property - def located(self): - return self._ahead_idx is not None - - def _locate(self): - """Will resolve the index in the nodes_data list for the node ahead of the current location. - It updates as well the distance from the current location to the node ahead. - """ - current = self.current_wr - if current is None: - return - - node_ahead_id = current.node_ahead.id - self._distance_to_node_ahead = current.distance_to_node_ahead - start_idx = self._ahead_idx if self._ahead_idx is not None else 1 - self._ahead_idx = None - - ids = self._nodes_data.get(NodeDataIdx.node_id) - for idx in range(start_idx, len(ids)): - if ids[idx] == node_ahead_id: - self._ahead_idx = idx - break - - @property - def current_wr(self): - return self._ordered_way_relations[0] if len(self._ordered_way_relations) else None - - def update(self, location_rad, bearing_rad, location_stdev): - """Will update the route structure based on the given `location_rad` and `bearing_rad` assuming progress on the - route on the original direction. If direction has changed or active point on the route can not be found, the route - will become invalid. - """ - if len(self._ordered_way_relations) == 0 or location_rad is None or bearing_rad is None: - return - - # Skip if no update on location or bearing. - if np.array_equal(self.current_wr.location_rad, location_rad) and self.current_wr.bearing_rad == bearing_rad: - return - - # Transverse the way relations on the actual order until we find an active one. From there, rebuild the route - # with the way relations remaining ahead. - for idx, wr in enumerate(self._ordered_way_relations): - active_direction = wr.direction - wr.update(location_rad, bearing_rad, location_stdev) - - if not wr.active: - continue - - if wr.direction != active_direction: - # Driving direction on the route has changed. stop. - break - - # We have now the current wr. Repopulate from here till the end and locate - self._ordered_way_relations = self._ordered_way_relations[idx:] - self._reset() - self._locate() - - # If the active way is diverting, check whether there are possibilities to divert from the route in the - # vecinity of the current location. If there are possibilities, then stop here to loose the route as we are - # most likely driving away. If there are no possibilities, then stick to the route as the diversion is probably - # just a matter of GPS accuracy. (It can happen after driving under a bridge) - if wr.diverting and len(self._nodes_data.possible_divertions(self._ahead_idx, self._distance_to_node_ahead)) > 0: - break - - # The current location in route is valid, return. - return - - # if we got here, there is no new active way relation or driving direction has changed. Reset. - self._reset() - - @property - def speed_limits_ahead(self): - """Returns and array of SpeedLimitSection objects for the actual route ahead of current location - """ - if self._limits_ahead is not None: - return self._limits_ahead - - if self._nodes_data is None or self._ahead_idx is None: - return [] - - self._limits_ahead = self._nodes_data.speed_limits_ahead(self._ahead_idx, self._distance_to_node_ahead) - return self._limits_ahead - - @property - def curvature_speed_limits_ahead(self): - """Returns and array of TurnSpeedLimitSection objects for the actual route ahead of current location due - to curvatures - """ - if self._cuvature_limits_ahead is not None: - return self._cuvature_limits_ahead - - if self._nodes_data is None or self._ahead_idx is None: - return [] - - self._cuvature_limits_ahead = self._nodes_data. \ - curvatures_speed_limit_sections_ahead(self._ahead_idx, self._distance_to_node_ahead) - - return self._cuvature_limits_ahead - - @property - def current_speed_limit(self): - if not self.located: - return None - - limits_ahead = self.speed_limits_ahead - if len(limits_ahead) == 0 or limits_ahead[0].start != 0: - return None - - return limits_ahead[0].value - - @property - def current_curvature_speed_limit_section(self): - if not self.located: - return None - - limits_ahead = self.curvature_speed_limits_ahead - if len(limits_ahead) == 0 or limits_ahead[0].start != 0: - return None - - return limits_ahead[0] - - @property - def next_speed_limit_section(self): - if not self.located: - return None - - limits_ahead = self.speed_limits_ahead - if len(limits_ahead) == 0: - return None - - # Find the first section that does not start in 0. i.e. the next section - for section in limits_ahead: - if section.start > 0: - return section - - return None - - def next_curvature_speed_limit_sections(self, horizon_mts): - if not self.located: - return [] - - # Provide the curvature speed sections that start ahead (> 0) and up to horizon - return list(filter(lambda la: la.start > 0 and la.start <= horizon_mts, self.curvature_speed_limits_ahead)) - - @property - def distance_to_end(self): - if not self.located: - return None - - return self._nodes_data.distance_to_end(self._ahead_idx, self._distance_to_node_ahead) - - @property - def current_road_name(self): - return self.current_wr.road_name if self.located else None diff --git a/selfdrive/mapd/lib/WayCollection.py b/selfdrive/mapd/lib/WayCollection.py deleted file mode 100644 index 91e7a161a2..0000000000 --- a/selfdrive/mapd/lib/WayCollection.py +++ /dev/null @@ -1,85 +0,0 @@ -from selfdrive.mapd.lib.WayRelation import WayRelation -from selfdrive.mapd.lib.WayRelationIndex import WayRelationIndex -from selfdrive.mapd.lib.Route import Route -from selfdrive.mapd.config import LANE_WIDTH -import uuid - - -_ACCEPTABLE_BEARING_DELTA_IND = 0.7071067811865475 # sin(pi/4) | 45 degrees acceptable bearing delta - - -class WayCollection(): - """A collection of WayRelations to use for maps data analysis. - """ - def __init__(self, areas, ways, query_center): - """Creates a WayCollection with a set of OSM way objects. - - Args: - ways (Array): Collection of Way objects fetched from OSM in a radius around `query_center` - query_center (Numpy Array): [lat, lon] numpy array in radians indicating the center of the data query. - """ - self.id = uuid.uuid4() - self.way_relations = [WayRelation(areas, way) for way in ways] - self.query_center = query_center - - self.wr_index = WayRelationIndex(self.way_relations) - - def get_route(self, location_rad, bearing_rad, location_stdev): - """Provides the best route found in the way collection based on current location and bearing. - """ - if location_rad is None or bearing_rad is None or location_stdev is None: - return None - - # Update all way relations in collection to the provided location and bearing. - for wr in self.way_relations: - wr.update(location_rad, bearing_rad, location_stdev) - - # Get the way relations where a match was found. i.e. those now marked as active as long as the direction of - # travel is valid. - valid_way_relations = [wr for wr in self.way_relations if wr.active and not wr.is_prohibited] - - # If no active, then we could not find a current way to build a route. - if len(valid_way_relations) == 0: - return None - - # If only one valid, then pick it as current. - if len(valid_way_relations) == 1: - current = valid_way_relations[0] - - # If more than one is valid, filter out any valid way relation where the bearing delta indicator is too high. - else: - wr_acceptable_bearing = list(filter(lambda wr: wr.active_bearing_delta <= _ACCEPTABLE_BEARING_DELTA_IND, - valid_way_relations)) - - # If delta bearing indicator is too high for all, then use as current the one that has the shorter one. - if len(wr_acceptable_bearing) == 0: - valid_way_relations.sort(key=lambda wr: wr.active_bearing_delta) - current = valid_way_relations[0] - - # If only one with acceptable bearing, use it. - elif len(wr_acceptable_bearing) == 1: - current = wr_acceptable_bearing[0] - - else: - # If more than one with acceptable bearing, filter the ones with distance to way lower than 2 standard - # deviation from GPS accuracy (95%) + half the road width estimate. - wr_accurate_distance = [wr for wr in wr_acceptable_bearing - if wr.distance_to_way <= 2. * location_stdev + wr.lanes * LANE_WIDTH / 2.] - - # If none with accurate distance to way, then select the closest to the way - if len(wr_accurate_distance) == 0: - wr_acceptable_bearing.sort(key=lambda wr: wr.distance_to_way) - current = wr_acceptable_bearing[0] - - # If only one with distance under accuracy, select this one. - elif len(wr_accurate_distance) == 1: - current = wr_accurate_distance[0] - - # If more than one with distance under accuracy. Then select the one with lowest highway rank. - # i.e. preferred motorways over other roads and so on. This is to prevent selecting a small parallel - # road to a main road when the accuracy is poor. - else: - wr_accurate_distance.sort(key=lambda wr: wr.highway_rank) - current = wr_accurate_distance[0] - - return Route(current, self.wr_index, self.id, self.query_center) diff --git a/selfdrive/mapd/lib/WayRelation.py b/selfdrive/mapd/lib/WayRelation.py deleted file mode 100644 index ce222a6c19..0000000000 --- a/selfdrive/mapd/lib/WayRelation.py +++ /dev/null @@ -1,487 +0,0 @@ -from selfdrive.mapd.lib.geo import DIRECTION, R, vectors, bearing_to_points, distance_to_points, point_on_line -from selfdrive.mapd.lib.osm import create_way -from common.conversions import Conversions as CV -from selfdrive.mapd.config import LANE_WIDTH -from common.basedir import BASEDIR -from datetime import datetime as dt -import numpy as np -import re -import json - - -_WAY_BBOX_PADING = 80. / R # 80 mts of padding to bounding box. (expressed in radians) - -with open(BASEDIR + "/selfdrive/mapd/lib/default_speeds_by_region.json", "rb") as f: - DEFAULT_SPEEDS_BY_REGION = json.loads(f.read()) - -with open(BASEDIR + "/selfdrive/mapd/lib/default_speeds.json", "rb") as f: - _COUNTRY_LIMITS = json.loads(f.read()) - - -_WD = { - 'Mo': 0, - 'Tu': 1, - 'We': 2, - 'Th': 3, - 'Fr': 4, - 'Sa': 5, - 'Su': 6 -} - -_HIGHWAY_RANK = { - 'motorway': 0, - 'motorway_link': 1, - 'trunk': 10, - 'trunk_link': 11, - 'primary': 20, - 'primary_link': 21, - 'secondary': 30, - 'secondary_link': 31, - 'tertiary': 40, - 'tertiary_link': 41, - 'unclassified': 50, - 'residential': 60, - 'living_street': 61, - 'service': 62 -} - - -def is_osm_time_condition_active(condition_string): - """ - Will indicate if a time condition for a restriction as described - @ https://wiki.openstreetmap.org/wiki/Conditional_restrictions - is active for the current date and time of day. - """ - now = dt.now().astimezone() - today = now.date() - week_days = [] - - # Look for days of week matched and validate if today matches criteria. - dr = re.findall(r'(Mo|Tu|We|Th|Fr|Sa|Su[-,\s]*?)', condition_string) - - if len(dr) == 1: - week_days = [_WD[dr[0]]] - # If two or more matches condider it a range of days between 1st and 2nd element. - elif len(dr) > 1: - week_days = list(range(_WD[dr[0]], _WD[dr[1]] + 1)) - - # If valid week days list is not empty and today day is not in the list, then the time-date range is not active. - if len(week_days) > 0 and now.weekday() not in week_days: - return False - - # Look for time ranges on the day. No time range, means all day - tr = re.findall(r'([0-9]{1,2}:[0-9]{2})\s*?-\s*?([0-9]{1,2}:[0-9]{2})', condition_string) - - # if no time range but there were week days set, consider it active during the whole day - if len(tr) == 0: - return len(dr) > 0 - - # Search among time ranges matched, one where now time belongs too. If found range is active. - for times_tup in tr: - times = list(map(lambda tt: dt. - combine(today, dt.strptime(tt, '%H:%M').time().replace(tzinfo=now.tzinfo)), times_tup)) - if now >= times[0] and now <= times[1]: - return True - - return False - - -def speed_limit_value_for_limit_string(limit_string): - # Look for matches of speed by default in kph, or in mph when explicitly noted. - v = re.match(r'^\s*([0-9]{1,3})\s*?(mph)?\s*$', limit_string) - if v is None: - return None - conv = CV.MPH_TO_MS if v[2] is not None and v[2] == "mph" else CV.KPH_TO_MS - return conv * float(v[1]) - - -def speed_limit_for_osm_tag_limit_string(limit_string): - # https://wiki.openstreetmap.org/wiki/Key:maxspeed - if limit_string is None: - # When limit is set to 0. is considered not existing. - return 0. - - # Attempt to parse limit as simple numeric value considering units. - limit = speed_limit_value_for_limit_string(limit_string) - if limit is not None: - return limit - - # Look for matches of speed with country implicit values. - v = re.match(r'^\s*([A-Z]{2}):([a-z_]+):?([0-9]{1,3})?(\s+)?(mph)?\s*', limit_string) - if v is None: - return 0. - - if v[2] == "zone" and v[3] is not None: - conv = CV.MPH_TO_MS if v[5] is not None and v[5] == "mph" else CV.KPH_TO_MS - limit = conv * float(v[3]) - elif f'{v[1]}:{v[2]}' in _COUNTRY_LIMITS: - limit = speed_limit_value_for_limit_string(_COUNTRY_LIMITS[f'{v[1]}:{v[2]}']) - - return limit if limit is not None else 0. - - -def conditional_speed_limit_for_osm_tag_limit_string(limit_string): - if limit_string is None: - # When limit is set to 0. is considered not existing. - return 0. - - # Look for matches of the ` @ ()` format - v = re.match(r'^(.*)@\s*\((.*)\).*$', limit_string) - if v is None: - return 0. # No valid format match - - value = speed_limit_for_osm_tag_limit_string(v[1]) - if value == 0.: - return 0. # Invalid speed limit value - - # Look for date-time conditions separated by semicolon - v = re.findall(r'(?:;|^)([^;]*)', v[2]) - for datetime_condition in v: - if is_osm_time_condition_active(datetime_condition): - return value - - # If we get here, no current date-time condition is active. - return 0. - -def speed_limit_value_for_highway_type(areas, tags): - max_speed = None - try: - geocode_country = '' - geocode_region = '' - for area in areas: - if area.tags.get('admin_level', '') == "2": - if area.tags.get('ISO3166-1:alpha2', '') != '': - geocode_country = area.tags.get('ISO3166-1:alpha2', '') - elif area.tags.get('admin_level', '') == "4": - geocode_region = area.tags.get('name', '') - country_rules = DEFAULT_SPEEDS_BY_REGION.get(geocode_country, {}) - country_defaults = country_rules.get('Default', []) - for rule in country_defaults: - rule_valid = all( - tag_name in tags - and tags[tag_name] == value - for tag_name, value in rule['tags'].items() - ) - if rule_valid: - max_speed = rule['speed'] - break #stop searching country - - region_rules = country_rules.get(geocode_region, []) - for rule in region_rules: - rule_valid = all( - tag_name in tags - and tags[tag_name] == value - for tag_name, value in rule['tags'].items() - ) - if rule_valid: - max_speed = rule['speed'] - break #stop searching region - except KeyError as e: - print(e) - except TypeError as e: - print(f"TypeError: {e} object is not iterable.") - if max_speed is None: - return 0 - v = re.match(r'^\s*([0-9]{1,3})\s*?(mph)?\s*$', str(max_speed)) - if v is None: - return None - conv = CV.MPH_TO_MS if v[2] is not None and v[2] == "mph" else CV.KPH_TO_MS - return conv * float(v[1]) - - -class WayRelation(): - """A class that represent the relationship of an OSM way and a given `location` and `bearing` of a driving vehicle. - """ - def __init__(self, areas, way, parent=None): - self.way = way - self.areas = areas - self.parent = parent - self.parent_wr_id = parent.id if parent is not None else None # For WRs created as splits of other WRs - self.reset_location_variables() - self.direction = DIRECTION.NONE - self._speed_limit = None - self._advisory_speed_limit = None - self._one_way = way.tags.get("oneway") - self.name = way.tags.get('name') - self.ref = way.tags.get('ref') - self.highway_type = way.tags.get("highway") - self.highway_rank = _HIGHWAY_RANK.get(self.highway_type, 1000) - try: - self.lanes = int(way.tags.get('lanes')) - except Exception: - self.lanes = 2 - - # Create numpy arrays with nodes data to support calculations. - self._nodes_np = np.radians(np.array([[nd.lat, nd.lon] for nd in way.nodes], dtype=float)) - self._nodes_ids = np.array([nd.id for nd in way .nodes], dtype=int) - - # Get the vectors representation of the segments betwheen consecutive nodes. (N-1, 2) - v = vectors(self._nodes_np) * R - - # Calculate the vector magnitudes (or distance) between nodes. (N-1) - self._way_distances = np.linalg.norm(v, axis=1) - - # Calculate the bearing (from true north clockwise) for every section of the way (vectors between nodes). (N-1) - self._way_bearings = np.arctan2(v[:, 0], v[:, 1]) - - # Define bounding box to ease the process of locating a node in a way. - # [[min_lat, min_lon], [max_lat, max_lon]] - self.bbox = np.row_stack((np.amin(self._nodes_np, 0) - _WAY_BBOX_PADING, - np.amax(self._nodes_np, 0) + _WAY_BBOX_PADING)) - - # Get the edge nodes ids. - self.edge_nodes_ids = [way.nodes[0].id, way.nodes[-1].id] - - def __repr__(self): - return f'(id: {self.id}, between {self.behind_idx} and {self.ahead_idx}, {self.direction}, active: {self.active})' - - def __eq__(self, other): - if isinstance(other, WayRelation): - return self.id == other.id - return False - - def reset_location_variables(self): - self.distance_to_node_ahead = 0. - self.location_rad = None - self.bearing_rad = None - self.active = False - self.diverting = False - self.ahead_idx = None - self.behind_idx = None - self._active_bearing_delta = None - self._distance_to_way = None - - @property - def id(self): - return self.way.id - - @property - def road_name(self): - if self.name is not None: - return self.name - return self.ref - - def update(self, location_rad, bearing_rad, location_stdev): - """Will update and validate the associated way with a given `location_rad` and `bearing_rad`. - Specifically it will find the nodes behind and ahead of the current location and bearing. - If no proper fit to the way geometry, the way relation is marked as invalid. - """ - self.reset_location_variables() - - # Ignore if location not in way bounding box - if not self.is_location_in_bbox(location_rad): - return - - # - Get the distance and bearings from location to all nodes. (N) - bearings = bearing_to_points(location_rad, self._nodes_np) - - # - Get absolute bearing delta to current driving bearing. (N) - delta = np.abs(bearing_rad - bearings) - - # - Nodes are ahead if the cosine of the delta is positive (N) - is_ahead = np.cos(delta) >= 0. - - # - Possible locations on the way are those where adjacent nodes change from ahead to behind or vice-versa. - possible_idxs = np.nonzero(np.diff(is_ahead))[0] - - # - when no possible locations found, then the location is not in this way. - if len(possible_idxs) == 0: - return - - projections = point_on_line(self._nodes_np[:-1], self._nodes_np[1:], location_rad) - h = distance_to_points(location_rad, projections) - - # - Calculate the delta between driving bearing and way bearings. (N-1) - bw_delta = self._way_bearings - bearing_rad - - # - The absolute value of the sin of `bw_delta` indicates how close the bearings match independent of direction. - # We will use this value along the distance to the way to aid on way selection. (N-1) - abs_sin_bw_delta = np.abs(np.sin(bw_delta)) - - # - Get the delta to way bearing indicators and the distance to the way for the possible locations. - abs_sin_bw_delta_possible = abs_sin_bw_delta[possible_idxs] - h_possible = h[possible_idxs] - - # - Get the index where the distance to the way is minimum. That is the chosen location. - min_h_possible_idx = np.argmin(h_possible) - min_delta_idx = possible_idxs[min_h_possible_idx] - projection = projections[min_delta_idx] - - # - If the distance to the way is over 4 standard deviations of the gps accuracy + the maximum road width - # estimate, then we are way too far to stick to this way (i.e. we are not on this way anymore) - # In theory the osm path is centered on the road which means half the road width would cover the whole road. - # however, often times the osm path is not perfectly centered so we'll make the possible route more lenient by using - # the full road width. - road_width_estimate = self.lanes * LANE_WIDTH - half_road_width_estimate = road_width_estimate / 2. - if h_possible[min_h_possible_idx] > 4. * location_stdev + road_width_estimate: - return - - # If the distance to the road is greater than 2 standard deviations of the gps accuracy + half the maximum road - # width estimate + 1 lane width then we are most likely diverting from this route. Adding a lane width to give - # leniency to not perfectly centered osm paths - diverting = h_possible[min_h_possible_idx] > 2. * location_stdev + half_road_width_estimate + LANE_WIDTH - - # Populate location variables with result - if is_ahead[min_delta_idx]: - self.direction = DIRECTION.BACKWARD - self.ahead_idx = min_delta_idx - self.behind_idx = min_delta_idx + 1 - else: - self.direction = DIRECTION.FORWARD - self.ahead_idx = min_delta_idx + 1 - self.behind_idx = min_delta_idx - - self._distance_to_way = h[min_delta_idx] - self._active_bearing_delta = abs_sin_bw_delta_possible[min_h_possible_idx] - - # find the distance to the next node by projecting our location onto the line and finding the delta between that - # point and the next point on the route - self.distance_to_node_ahead = distance_to_points(projection, np.array([self._nodes_np[self.ahead_idx]]))[0] - self.active = True - self.diverting = diverting - self.location_rad = location_rad - self.bearing_rad = bearing_rad - self._speed_limit = None - self._advisory_speed_limit = None - - def update_direction_from_starting_node(self, start_node_id): - self._speed_limit = None - self._advisory_speed_limit = None - if self.edge_nodes_ids[0] == start_node_id: - self.direction = DIRECTION.FORWARD - elif self.edge_nodes_ids[-1] == start_node_id: - self.direction = DIRECTION.BACKWARD - else: - self.direction = DIRECTION.NONE - - def is_location_in_bbox(self, location_rad): - """Indicates if a given location is contained in the bounding box surrounding the way. - self.bbox = [[min_lat, min_lon], [max_lat, max_lon]] - """ - is_g = np.greater_equal(location_rad, self.bbox[0, :]) - is_l = np.less_equal(location_rad, self.bbox[1, :]) - - return np.all(np.concatenate((is_g, is_l))) - - @property - def speed_limit(self): - if self._speed_limit is not None: - return self._speed_limit - - # Get string from corresponding tag, consider conditional limits first. - limit_string = self.way.tags.get("maxspeed:conditional") - if limit_string is None: - if self.direction == DIRECTION.FORWARD: - limit_string = self.way.tags.get("maxspeed:forward:conditional") - elif self.direction == DIRECTION.BACKWARD: - limit_string = self.way.tags.get("maxspeed:backward:conditional") - - limit = conditional_speed_limit_for_osm_tag_limit_string(limit_string) - - # When no conditional limit set, attempt to get from regular speed limit tags. - if limit == 0.: - limit_string = self.way.tags.get("maxspeed") - if limit_string is None: - if self.direction == DIRECTION.FORWARD: - limit_string = self.way.tags.get("maxspeed:forward") - elif self.direction == DIRECTION.BACKWARD: - limit_string = self.way.tags.get("maxspeed:backward") - - limit = speed_limit_for_osm_tag_limit_string(limit_string) - - if limit == 0.: - limit = speed_limit_value_for_highway_type(self.areas, self.way.tags) - - self._speed_limit = limit - return self._speed_limit - - - @property - def advisory_speed_limit(self): - if self._advisory_speed_limit is not None: - return self._advisory_speed_limit - - limit_string = self.way.tags.get("maxspeed:advisory") - limit = speed_limit_for_osm_tag_limit_string(limit_string) - - self._advisory_speed_limit = limit - return self._advisory_speed_limit - - - @property - def active_bearing_delta(self): - """Returns the sine of the delta between the current location bearing and the exact - bearing of the portion of way we are currentluy located at. - """ - return self._active_bearing_delta - - @property - def is_one_way(self): - return self._one_way in ['yes'] or self.highway_type in ["motorway"] - - @property - def is_prohibited(self): - # Direction must be defined to asses this property. Default to `True` if not. - if self.direction == DIRECTION.NONE: - return True - return self.is_one_way and self.direction == DIRECTION.BACKWARD - - @property - def distance_to_way(self): - """Returns the perpendicular (i.e. minimum) distance between current location and the way - """ - return self._distance_to_way - - @property - def node_ahead(self): - return self.way.nodes[self.ahead_idx] if self.ahead_idx is not None else None - - @property - def last_node(self): - """Returns the last node on the way considering the traveling direction - """ - if self.direction == DIRECTION.FORWARD: - return self.way.nodes[-1] - if self.direction == DIRECTION.BACKWARD: - return self.way.nodes[0] - return None - - @property - def last_node_coordinates(self): - """Returns the coordinates for the last node on the way considering the traveling direction. (in radians) - """ - if self.direction == DIRECTION.FORWARD: - return self._nodes_np[-1] - if self.direction == DIRECTION.BACKWARD: - return self._nodes_np[0] - return None - - def node_before_edge_coordinates(self, node_id): - """Returns the coordinates of the node before the edge node identifeid with `node_id`. (in radians) - """ - if self.edge_nodes_ids[0] == node_id: - return self._nodes_np[1] - - if self.edge_nodes_ids[-1] == node_id: - return self._nodes_np[-2] - - return np.array([0., 0.]) - - def split(self, node_id, way_ids=None): - """ Returns and array with the way relations resulting from splitting the current way relation at node_id - """ - idxs = np.nonzero(self._nodes_ids == node_id)[0] - if len(idxs) == 0: - return [] - - idx = idxs[0] - if idx == 0 or idx == len(self._nodes_ids) - 1: - return [self] - - if not isinstance(way_ids, list): - way_ids = [-1, -2] # Default id values. - - ways = [create_way(way_ids[0], node_ids=self._nodes_ids[:idx + 1], from_way=self.way), - create_way(way_ids[1], node_ids=self._nodes_ids[idx:], from_way=self.way)] - return [WayRelation(self.areas, way, parent=self) for way in ways] diff --git a/selfdrive/mapd/lib/WayRelationIndex.py b/selfdrive/mapd/lib/WayRelationIndex.py deleted file mode 100644 index e941dcd5b7..0000000000 --- a/selfdrive/mapd/lib/WayRelationIndex.py +++ /dev/null @@ -1,34 +0,0 @@ - - -class WayRelationIndex(): - """ - A class containing an index of WayRelations by node ids of internal nodes and edge nodes. - """ - def __init__(self, way_relations): - self._edge_nodes_index_dict = {} - self._full_nodes_index_dict = {} - - for wr in way_relations: - self.add(wr) - - def add(self, way_relation): - for node in way_relation.way.nodes: - node_id = node.id - self._full_nodes_index_dict[node_id] = self._full_nodes_index_dict.get(node_id, []) + [way_relation] - if node_id in way_relation.edge_nodes_ids: - self._edge_nodes_index_dict[node_id] = self._edge_nodes_index_dict.get(node_id, []) + [way_relation] - - def remove(self, way_relation): - for node in way_relation.way.nodes: - node_id = node.id - self._full_nodes_index_dict[node_id] = [wr for wr in self._full_nodes_index_dict.get(node_id, []) - if wr is not way_relation] - if node_id in way_relation.edge_nodes_ids: - self._edge_nodes_index_dict[node_id] = [wr for wr in self._edge_nodes_index_dict.get(node_id, []) - if wr is not way_relation] - - def way_relations_with_edge_node_id(self, node_id): - return self._edge_nodes_index_dict.get(node_id, []) - - def way_relations_with_node_id(self, node_id): - return self._full_nodes_index_dict.get(node_id, []) diff --git a/selfdrive/mapd/lib/default_speeds.json b/selfdrive/mapd/lib/default_speeds.json deleted file mode 100644 index 0a7d28474a..0000000000 --- a/selfdrive/mapd/lib/default_speeds.json +++ /dev/null @@ -1,112 +0,0 @@ -{ - "_comment": "These speeds are from https://wiki.openstreetmap.org/wiki/Speed_limits Special cases have been stripped", - "AR:urban": "40", - "AR:urban:primary": "60", - "AR:urban:secondary": "60", - "AR:rural": "110", - "AT:urban": "50", - "AT:rural": "100", - "AT:trunk": "100", - "AT:motorway": "130", - "BE:urban": "50", - "BE-VLG:rural": "70", - "BE-WAL:rural": "90", - "BE:trunk": "120", - "BE:motorway": "120", - "CH:urban[1]": "50", - "CH:rural": "80", - "CH:trunk": "100", - "CH:motorway": "120", - "CZ:pedestrian_zone": "20", - "CZ:living_street": "20", - "CZ:urban": "50", - "CZ:urban_trunk": "80", - "CZ:urban_motorway": "80", - "CZ:rural": "90", - "CZ:trunk": "110", - "CZ:motorway": "130", - "DK:urban": "50", - "DK:rural": "80", - "DK:motorway": "130", - "DE:living_street": "10", - "DE:service": "10", - "DE:residential": "30", - "DE:urban": "50", - "DE:rural": "100", - "DE:trunk": "none", - "DE:motorway": "none", - "FI:urban": "50", - "FI:rural": "80", - "FI:trunk": "100", - "FI:motorway": "120", - "FR:urban": "50", - "FR:rural": "80", - "FR:trunk": "110", - "FR:motorway": "130", - "GR:urban": "50", - "GR:rural": "90", - "GR:trunk": "110", - "GR:motorway": "130", - "HU:urban": "50", - "HU:rural": "90", - "HU:trunk": "110", - "HU:motorway": "130", - "IT:urban": "50", - "IT:rural": "90", - "IT:trunk": "110", - "IT:motorway": "130", - "JP:national": "60", - "JP:motorway": "100", - "LT:living_street": "20", - "LT:urban": "50", - "LT:rural": "90", - "LT:trunk": "120", - "LT:motorway": "130", - "PL:living_street": "20", - "PL:urban": "50", - "PL:rural": "90", - "PL:trunk": "100", - "PL:motorway": "140", - "RO:urban": "50", - "RO:rural": "90", - "RO:trunk": "100", - "RO:motorway": "130", - "RU:living_street": "20", - "RU:urban": "60", - "RU:rural": "90", - "RU:motorway": "110", - "SK:urban": "50", - "SK:rural": "90", - "SK:trunk": "90", - "SK:motorway": "90", - "SI:urban": "50", - "SI:rural": "90", - "SI:trunk": "110", - "SI:motorway": "130", - "ES:living_street": "20", - "ES:urban": "50", - "ES:rural": "50", - "ES:trunk": "90", - "ES:motorway": "120", - "SE:urban": "50", - "SE:rural": "70", - "SE:trunk": "90", - "SE:motorway": "110", - "GB:nsl_restricted": "30 mph", - "GB:nsl_single": "60 mph", - "GB:nsl_dual": "70 mph", - "GB:motorway": "70 mph", - "UA:urban": "50", - "UA:rural": "90", - "UA:trunk": "110", - "UA:motorway": "130", - "UZ:living_street": "30", - "UZ:urban": "70", - "UZ:rural": "100", - "UZ:motorway": "110", - "ZA:trunk": "120", - "ZA:residential": "60", - "ZA:rural": "100", - "ZA:urban": "60", - "ZA:motorway": "120" -} diff --git a/selfdrive/mapd/lib/default_speeds_by_region.json b/selfdrive/mapd/lib/default_speeds_by_region.json deleted file mode 100644 index fd3365b89f..0000000000 --- a/selfdrive/mapd/lib/default_speeds_by_region.json +++ /dev/null @@ -1,624 +0,0 @@ -{ - "AU": { - "Default": [ - { - "speed": "100", - "tags": { - "highway": "motorway" - } - }, - { - "speed": "80", - "tags": { - "highway": "trunk" - } - }, - { - "speed": "80", - "tags": { - "highway": "primary" - } - }, - { - "speed": "50", - "tags": { - "highway": "secondary" - } - }, - { - "speed": "50", - "tags": { - "highway": "tertiary" - } - }, - { - "speed": "80", - "tags": { - "highway": "unclassified" - } - }, - { - "speed": "50", - "tags": { - "highway": "residential" - } - }, - { - "speed": "40", - "tags": { - "highway": "service" - } - }, - { - "speed": "90", - "tags": { - "highway": "motorway_link" - } - }, - { - "speed": "80", - "tags": { - "highway": "trunk_link" - } - }, - { - "speed": "80", - "tags": { - "highway": "primary_link" - } - }, - { - "speed": "50", - "tags": { - "highway": "secondary_link" - } - }, - { - "speed": "50", - "tags": { - "highway": "tertiary_link" - } - }, - { - "speed": "30", - "tags": { - "highway": "living_street" - } - } - ] - }, - "EE": { - "Default": [ - { - "speed": "90", - "tags": { - "highway": "motorway" - } - }, - { - "speed": "90", - "tags": { - "highway": "trunk" - } - }, - { - "speed": "90", - "tags": { - "highway": "primary" - } - }, - { - "speed": "50", - "tags": { - "highway": "secondary" - } - }, - { - "speed": "50", - "tags": { - "highway": "tertiary" - } - }, - { - "speed": "90", - "tags": { - "highway": "unclassified" - } - }, - { - "speed": "40", - "tags": { - "highway": "residential" - } - }, - { - "speed": "40", - "tags": { - "highway": "service" - } - }, - { - "speed": "90", - "tags": { - "highway": "motorway_link" - } - }, - { - "speed": "70", - "tags": { - "highway": "trunk_link" - } - }, - { - "speed": "70", - "tags": { - "highway": "primary_link" - } - }, - { - "speed": "50", - "tags": { - "highway": "secondary_link" - } - }, - { - "speed": "50", - "tags": { - "highway": "tertiary_link" - } - }, - { - "speed": "20", - "tags": { - "highway": "living_street" - } - } - ] - }, - "CA": { - "Default": [ - { - "speed": "100", - "tags": { - "highway": "motorway" - } - }, - { - "speed": "80", - "tags": { - "highway": "trunk" - } - }, - { - "speed": "80", - "tags": { - "highway": "primary" - } - }, - { - "speed": "50", - "tags": { - "highway": "secondary" - } - }, - { - "speed": "50", - "tags": { - "highway": "tertiary" - } - }, - { - "speed": "80", - "tags": { - "highway": "unclassified" - } - }, - { - "speed": "40", - "tags": { - "highway": "residential" - } - }, - { - "speed": "40", - "tags": { - "highway": "service" - } - }, - { - "speed": "90", - "tags": { - "highway": "motorway_link" - } - }, - { - "speed": "80", - "tags": { - "highway": "trunk_link" - } - }, - { - "speed": "80", - "tags": { - "highway": "primary_link" - } - }, - { - "speed": "50", - "tags": { - "highway": "secondary_link" - } - }, - { - "speed": "50", - "tags": { - "highway": "tertiary_link" - } - }, - { - "speed": "20", - "tags": { - "highway": "living_street" - } - } - ] - }, - "DE": { - "Default": [ - { - "speed": "none", - "tags": { - "highway": "motorway" - } - }, - { - "speed": "10", - "tags": { - "highway": "living_street" - } - }, - { - "speed": "30", - "tags": { - "highway": "residential" - } - }, - { - "speed": "10", - "tags": { - "highway": "service" - } - }, - { - "speed": "100", - "tags": { - "zone:traffic": "DE:rural" - } - }, - { - "speed": "50", - "tags": { - "zone:traffic": "DE:urban" - } - }, - { - "speed": "30", - "tags": { - "zone:maxspeed": "DE:30" - } - }, - { - "speed": "50", - "tags": { - "zone:maxspeed": "DE:urban" - } - }, - { - "speed": "100", - "tags": { - "zone:maxspeed": "DE:rural" - } - }, - { - "speed": "none", - "tags": { - "zone:maxspeed": "DE:motorway" - } - }, - { - "speed": "30", - "tags": { - "bicycle_road": "yes" - } - } - ] - }, - "ZA": { - "Default": [ - { - "speed": "120", - "tags": { - "highway": "motorway" - } - }, - { - "speed": "120", - "tags": { - "highway": "motorway_link" - } - }, - { - "speed": "100", - "tags": { - "highway": "primary" - } - }, - { - "speed": "100", - "tags": { - "highway": "secondary" - } - }, - { - "speed": "60", - "tags": { - "highway": "residential" - } - } - ] - }, - "US": { - "South Dakota": [ - { - "speed": "80 mph", - "tags": { - "highway": "motorway" - } - }, - { - "speed": "70 mph", - "tags": { - "highway": "trunk" - } - }, - { - "speed": "65 mph", - "tags": { - "highway": "primary" - } - }, - { - "speed": "70 mph", - "tags": { - "highway": "trunk_link" - } - }, - { - "speed": "65 mph", - "tags": { - "highway": "primary_link" - } - } - ], - "Wisconsin": [ - { - "speed": "65 mph", - "tags": { - "highway": "trunk" - } - }, - { - "speed": "45 mph", - "tags": { - "highway": "tertiary" - } - }, - { - "speed": "35 mph", - "tags": { - "highway": "unclassified" - } - }, - { - "speed": "65 mph", - "tags": { - "highway": "trunk_link" - } - }, - { - "speed": "45 mph", - "tags": { - "highway": "tertiary_link" - } - } - ], - "Default": [ - { - "speed": "65 mph", - "tags": { - "highway": "motorway" - } - }, - { - "speed": "55 mph", - "tags": { - "highway": "trunk" - } - }, - { - "speed": "55 mph", - "tags": { - "highway": "primary" - } - }, - { - "speed": "45 mph", - "tags": { - "highway": "secondary" - } - }, - { - "speed": "35 mph", - "tags": { - "highway": "tertiary" - } - }, - { - "speed": "55 mph", - "tags": { - "highway": "unclassified" - } - }, - { - "speed": "25 mph", - "tags": { - "highway": "residential" - } - }, - { - "speed": "25 mph", - "tags": { - "highway": "service" - } - }, - { - "speed": "55 mph", - "tags": { - "highway": "motorway_link" - } - }, - { - "speed": "55 mph", - "tags": { - "highway": "trunk_link" - } - }, - { - "speed": "55 mph", - "tags": { - "highway": "primary_link" - } - }, - { - "speed": "45 mph", - "tags": { - "highway": "secondary_link" - } - }, - { - "speed": "35 mph", - "tags": { - "highway": "tertiary_link" - } - }, - { - "speed": "15 mph", - "tags": { - "highway": "living_street" - } - } - ], - "Michigan": [ - { - "speed": "70 mph", - "tags": { - "highway": "motorway" - } - } - ], - "Oregon": [ - { - "speed": "55 mph", - "tags": { - "highway": "motorway" - } - }, - { - "speed": "35 mph", - "tags": { - "highway": "secondary" - } - }, - { - "speed": "30 mph", - "tags": { - "highway": "tertiary" - } - }, - { - "speed": "15 mph", - "tags": { - "highway": "service" - } - }, - { - "speed": "35 mph", - "tags": { - "highway": "secondary_link" - } - }, - { - "speed": "30 mph", - "tags": { - "highway": "tertiary_link" - } - } - ], - "New York": [ - { - "speed": "65 mph", - "tags": { - "highway": "motorway" - } - }, - { - "speed": "45 mph", - "tags": { - "highway": "primary" - } - }, - { - "speed": "55 mph", - "tags": { - "highway": "secondary" - } - }, - { - "speed": "55 mph", - "tags": { - "highway": "tertiary" - } - }, - { - "speed": "30 mph", - "tags": { - "highway": "residential" - } - }, - { - "speed": "45 mph", - "tags": { - "highway": "primary_link" - } - }, - { - "speed": "55 mph", - "tags": { - "highway": "secondary_link" - } - }, - { - "speed": "55 mph", - "tags": { - "highway": "tertiary_link" - } - }, - { - "speed": "20 mph", - "tags": { - "highway": "living_street" - } - } - ] - } -} diff --git a/selfdrive/mapd/lib/geo.py b/selfdrive/mapd/lib/geo.py deleted file mode 100644 index 51947481ae..0000000000 --- a/selfdrive/mapd/lib/geo.py +++ /dev/null @@ -1,78 +0,0 @@ -from enum import Enum -import numpy as np - - -R = 6373000.0 # approximate radius of earth in mts - - -def vectors(points): - """Provides a array of vectors on cartesian space (x, y). - Each vector represents the path from a point in `points` to the next. - `points` must by a (N, 2) array of [lat, lon] pairs in radians. - """ - latA = points[:-1, 0] - latB = points[1:, 0] - delta = np.diff(points, axis=0) - dlon = delta[:, 1] - - x = np.sin(dlon) * np.cos(latB) - y = np.cos(latA) * np.sin(latB) - (np.sin(latA) * np.cos(latB) * np.cos(dlon)) - - return np.column_stack((x, y)) - - -def ref_vectors(ref, points): - """Provides a array of vectors on cartesian space (x, y). - Each vector represents the path from ref to a point in `points`. - `points` must by a (N, 2) array of [lat, lon] pairs in radians. - """ - latA = ref[0] - latB = points[:, 0] - delta = points - ref - dlon = delta[:, 1] - - x = np.sin(dlon) * np.cos(latB) - y = np.cos(latA) * np.sin(latB) - (np.sin(latA) * np.cos(latB) * np.cos(dlon)) - - return np.column_stack((x, y)) - - -def bearing_to_points(point, points): - """Calculate the bearings (angle from true north clockwise) of the vectors between `point` and each - one of the entries in `points`. Both `point` and `points` elements are 2 element arrays containing a latitud, - longitude pair in radians. - """ - delta = points - point - x = np.sin(delta[:, 1]) * np.cos(points[:, 0]) - y = np.cos(point[0]) * np.sin(points[:, 0]) - (np.sin(point[0]) * np.cos(points[:, 0]) * np.cos(delta[:, 1])) - return np.arctan2(x, y) - -def point_on_line(start_points, end_points, point, extend_line = False): - """project a single point onto each line for an np array of start points and end points - ref: https://stackoverflow.com/a/61342198 - """ - ap = np.subtract(point, start_points) - ab = np.subtract(end_points, start_points) - t = np.array([np.dot(ap[i], ab[i]) / np.dot(ab[i], ab[i]) for i in range(len(ap))]) - # if you need the the closest point belonging to the segment - if not extend_line: - t = np.maximum(0, np.minimum(1, t)) - result = np.add(start_points, np.array([t[i] * ab[i] for i in range(len(t))])) - return result - -def distance_to_points(point, points): - """Calculate the distance of the vectors between `point` and each one of the entries in `points`. - Both `point` and `points` elements are 2 element arrays containing a latitud, longitude pair in radians. - """ - delta = points - point - a = np.sin(delta[:, 0] / 2)**2 + np.cos(point[0]) * np.cos(points[:, 0]) * np.sin(delta[:, 1] / 2)**2 - c = 2 * np.arctan2(np.sqrt(a), np.sqrt(1 - a)) - return c * R - - -class DIRECTION(Enum): - NONE = 0 - AHEAD = 1 - BEHIND = 2 - FORWARD = 3 - BACKWARD = 4 diff --git a/selfdrive/mapd/lib/helpers.py b/selfdrive/mapd/lib/helpers.py deleted file mode 100644 index ca7dfdfead..0000000000 --- a/selfdrive/mapd/lib/helpers.py +++ /dev/null @@ -1,102 +0,0 @@ -import overpy -import subprocess -import email.utils as eut -import time - -from common.params import Params -from system.version import get_version -from urllib.request import Request, urlopen - - -OSM_LOCAL_PATH = "/data/media/0/osm" -OSM_DB_STAMP_FILE = OSM_LOCAL_PATH + "/db_stamp" -OSM_QUERY = [f"{OSM_LOCAL_PATH}/v0.7.57/bin/osm3s_query", f"--db-dir={OSM_LOCAL_PATH}/db"] -OSM_DB_STAMP_REMOTE = "https://sunnypilot-osm.s3.us-east-2.amazonaws.com/osm-db/timestamps" - - -def get_current_s3_osm_db_timestamp(): - try: - local_osm_db_name = Params().get("OsmLocationName", encoding="utf8") - req = Request(url=f"{OSM_DB_STAMP_REMOTE}/{local_osm_db_name}.txt", headers={"User-Agent": f"sunnypilot-{get_version()}"}) - r = urlopen(req) - if r.status != 200: - print(f'Failed to fetch timestamp for S3 OSM db.\n\n{r.status}') - return None - - timestamp_string = r.read().decode("utf-8").strip() - if timestamp_string is None: - print('Timestamp file for S3 OSM db contained no value.') - return None - - parsed_date = eut.parsedate(timestamp_string) - return time.mktime(parsed_date) - except Exception as e: - print(f'Could not parse timestamp for S3 local osm db.\n\n{e}') - return None - - -def persist_s3_osm_db_timestamp(timestamp): - try: - with open(OSM_DB_STAMP_FILE, 'w') as file: - file.write(f'{timestamp}') - except Exception as e: - print(f'Failed to timestamp local OSM db.\n\n{e}') - - -def get_local_osm_timestamp(): - try: - with open(OSM_DB_STAMP_FILE, 'r') as file: - return float(file.readline()) - except Exception as e: - print(f'Failed to read timestamp for local OSM db.\n\n{e}') - return None - - -def is_osm_db_up_to_date(): - current_osm_ts = get_local_osm_timestamp() - if current_osm_ts is None: - return False - - current_s3_osm_ts = get_current_s3_osm_db_timestamp() - if current_s3_osm_ts is None: - return True - - return current_osm_ts == current_s3_osm_ts - - -def timestamp_local_osm_db(): - current_s3_osm_ts = get_current_s3_osm_db_timestamp() - if current_s3_osm_ts is not None: - persist_s3_osm_db_timestamp(current_s3_osm_ts) - - -def is_local_osm_installed(params=Params()): - api = overpy.Overpass() - waypoint = params.get("OsmWayTest", encoding="utf8") - if waypoint is None: - return False - q = f""" - way({waypoint}); - (._;>;); - out; - """ - - try: - completion = subprocess.run(OSM_QUERY + [f"--request={q}"], check=True, capture_output=True) - print(f'OSM local query returned with exit code: {completion.returncode}') - - if completion.returncode != 0: - return False - - print(f'OSM Local query returned:\n\n{completion.stdout}') - - ways = api.parse_xml(completion.stdout).ways - success = len(ways) == 1 - print(f"Test osm script returned {len(ways)} ways") - print(f'OSM local server query {"succeeded" if success else "failed"}') - - return success - - except Exception as e: - print(e) - return False diff --git a/selfdrive/mapd/lib/osm.py b/selfdrive/mapd/lib/osm.py deleted file mode 100644 index dd931ed95e..0000000000 --- a/selfdrive/mapd/lib/osm.py +++ /dev/null @@ -1,71 +0,0 @@ -import overpy -import subprocess -import numpy as np -from cereal import custom -from common.params import Params -from selfdrive.mapd.lib.geo import R -from selfdrive.mapd.lib.helpers import is_local_osm_installed, OSM_QUERY - -DataType = custom.LiveMapDataSP.DataType - - -def create_way(way_id, node_ids, from_way): - """ - Creates and OSM Way with the given `way_id` and list of `node_ids`, copying attributes and tags from `from_way` - """ - return overpy.Way(way_id, node_ids=node_ids, attributes={}, result=from_way._result, - tags=from_way.tags) - - -class OSM: - def __init__(self): - self.api = overpy.Overpass() - self.param_s = Params() - self._osm_local_db_enabled = self.param_s.get_bool("OsmLocalDb") - self._local_osm_installed = is_local_osm_installed(self.param_s) - # self.api = overpy.Overpass(url='http://3.65.170.21/api/interpreter') - - def _online_query(self, q, area_q): - print("Query OSM from remote server") - query = self.api.query(q + area_q) - areas, ways = query.areas, query.ways - data_type = DataType.online - return areas, ways, data_type - - def fetch_road_ways_around_location(self, lat, lon, radius): - # Calculate the bounding box coordinates for the bbox containing the circle around location. - bbox_angle = np.degrees(radius / R) - # fetch all ways and nodes on this ways in bbox - bbox_str = f'{str(lat - bbox_angle)},{str(lon - bbox_angle)},{str(lat + bbox_angle)},{str(lon + bbox_angle)}' - lat_lon = "(%f,%f)" % (lat, lon) - q = """ - way(""" + bbox_str + """) - [highway] - [highway!~"^(footway|path|corridor|bridleway|steps|cycleway|construction|bus_guideway|escape|service|track)$"]; - (._;>;); - out;""" - area_q = """is_in""" + lat_lon + """;area._[admin_level~"[24]"]; - convert area ::id = id(), admin_level = t['admin_level'], - name = t['name'], "ISO3166-1:alpha2" = t['ISO3166-1:alpha2'];out; - """ - try: - if self._osm_local_db_enabled and self._local_osm_installed: - print("Query OSM from local server") - completion = subprocess.run(OSM_QUERY + [f"--request={q}"], check=True, capture_output=True) - ways = self.api.parse_xml(completion.stdout).ways - if completion.returncode == 0 and len(ways) != 0: - try: - areas = self.api.query(area_q).areas - except Exception as e: - print(f'Exception while querying "AREAS" OSM from local server:\n{e}') - areas = None - data_type = DataType.offline - else: - areas, ways, data_type = self._online_query(q, area_q) - else: - areas, ways, data_type = self._online_query(q, area_q) - except Exception as e: - print(f'Exception while querying OSM:\n{e}') - areas, ways, data_type = [], [], DataType.default - - return areas, ways, data_type diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py deleted file mode 100644 index a2f9a5f5af..0000000000 --- a/selfdrive/mapd/mapd.py +++ /dev/null @@ -1,294 +0,0 @@ -#!/usr/bin/env python3 -import json -import threading -from traceback import print_exception -import numpy as np -from time import strftime, gmtime -from cereal import custom -import cereal.messaging as messaging -from common.params import Params -from common.realtime import set_core_affinity, set_thread_affinity, Ratekeeper -from selfdrive.mapd.lib.osm import OSM -from selfdrive.mapd.lib.geo import distance_to_points -from selfdrive.mapd.lib.WayCollection import WayCollection -from selfdrive.mapd.config import QUERY_RADIUS, QUERY_RADIUS_OFFLINE, MIN_DISTANCE_FOR_NEW_QUERY, FULL_STOP_MAX_SPEED, LOOK_AHEAD_HORIZON_TIME -from system.swaglog import cloudlog - -DataType = custom.LiveMapDataSP.DataType - - -_DEBUG = False -_CLOUDLOG_DEBUG = True -ROAD_NAME_TIMEOUT = 30 # secs - - -def _debug(msg, log_to_cloud=True): - if _CLOUDLOG_DEBUG and log_to_cloud: - cloudlog.debug(msg) - if _DEBUG: - print(msg) - - -def excepthook(args): - _debug(f'MapD: Threading exception:\n{args}') - print_exception(args.exc_type, args.exc_value, args.exc_traceback) - - -threading.excepthook = excepthook - - -class MapD(): - def __init__(self): - self.osm = OSM() - self.way_collection = None - self.route = None - self.last_gps_fix_timestamp = 0 - self.last_gps = None - self.location_deg = None # The current location in degrees. - self.location_rad = None # The current location in radians as a Numpy array. - self.bearing_rad = None - self.location_stdev = None # The current location accuracy in mts. 1 standard devitation. - self.gps_speed = 0. - self.last_fetch_location = None - self.last_route_update_fix_timestamp = 0 - self.last_publish_fix_timestamp = 0 - self.data_type = DataType.default - self._op_enabled = False - self._disengaging = False - self._query_thread = None - self._lock = threading.RLock() - self.gps_sock = 'gpsLocationExternal' - - # dp - use LastGPSPosition as init position (if we are in a undercover car park?) - # this way we can prefetch osm data before we get a fix. - last_pos = Params().get("LastGPSPosition") - if last_pos is not None and last_pos != "": - l = json.loads(last_pos) - lat = float(l["latitude"]) - lon = float(l["longitude"]) - self.location_rad = np.radians(np.array([lat, lon], dtype=float)) - self.location_deg = (lat, lon) - self.bearing_rad = np.radians(0, dtype=float) - _debug("Use LastGPSPosition position - lat: %s, lon: %s" % (lat, lon)) - - def udpate_state(self, sm): - sock = 'carControl' - if not sm.updated[sock] or not sm.valid[sock]: - return - - hud_control = sm[sock].hudControl - self._disengaging = not hud_control.speedVisible and self._op_enabled - self._op_enabled = hud_control.speedVisible - - def update_gps(self, sm): - self.gps_sock = 'gpsLocationExternal' if sm.rcv_frame['gpsLocationExternal'] > 1 else 'gpsLocation' - if not sm.updated[self.gps_sock] or not sm.valid[self.gps_sock]: - return - - log = sm[self.gps_sock] - self.last_gps = log - - # ignore the message if the fix is invalid - if log.flags % 2 == 0: - return - - self.last_gps_fix_timestamp = log.unixTimestampMillis # Unix TS. Milliseconds since January 1, 1970. - self.location_rad = np.radians(np.array([log.latitude, log.longitude], dtype=float)) - self.location_deg = (log.latitude, log.longitude) - self.bearing_rad = np.radians(log.bearingDeg, dtype=float) - self.gps_speed = log.speed - self.location_stdev = log.accuracy if self.gps_sock == 'gpsLocationExternal' else 1 # gpsLocation doesn't report accuracy - - _debug('Mapd: ********* Got GPS fix' - + f'Pos: {self.location_deg} +/- {self.location_stdev * 2.} mts.\n' - + f'Bearing: {log.bearingDeg} +/- {log.bearingAccuracyDeg * 2.} deg.\n' - + f'timestamp: {strftime("%d-%m-%y %H:%M:%S", gmtime(self.last_gps_fix_timestamp * 1e-3))}' - + '*******', log_to_cloud=False) - - def _query_osm_not_blocking(self): - def query(osm, location_deg, location_rad, radius): - _debug(f'Mapd: Start query for OSM map data at {location_deg}') - lat, lon = location_deg - areas, ways, self.data_type = osm.fetch_road_ways_around_location(lat, lon, radius) - _debug(f'Mapd: Query to OSM finished with {len(ways)} ways') - - # Only issue an update if we received some ways. Otherwise it is most likely a connectivity issue. - # Will retry on next loop. - if len(ways) > 0: - new_way_collection = WayCollection(areas, ways, location_rad) - - # Use the lock to update the way_collection as it might be being used to update the route. - _debug('Mapd: Locking to write results from osm.', log_to_cloud=False) - with self._lock: - self.way_collection = new_way_collection - self.last_fetch_location = location_rad - _debug(f'Mapd: Updated map data @ {location_deg} - got {len(ways)} ways') - - _debug('Mapd: Releasing Lock to write results from osm', log_to_cloud=False) - - # Ignore if we have a query thread already running. - if self._query_thread is not None and self._query_thread.is_alive(): - return - - self._query_thread = threading.Thread(target=query, args=(self.osm, self.location_deg, self.location_rad, - QUERY_RADIUS_OFFLINE if self.data_type == DataType.offline else QUERY_RADIUS)) - set_thread_affinity(self._query_thread, [0, 1, 2, 3]) - self._query_thread.start() - - def updated_osm_data(self): - if self.route is not None: - distance_to_end = self.route.distance_to_end - if distance_to_end is not None and distance_to_end >= MIN_DISTANCE_FOR_NEW_QUERY: - # do not query as long as we have a route with enough distance ahead. - return - - if self.location_rad is None: - return - - if self.last_fetch_location is not None: - distance_since_last = distance_to_points(self.last_fetch_location, np.array([self.location_rad]))[0] - if distance_since_last < (QUERY_RADIUS_OFFLINE if self.data_type == DataType.offline else QUERY_RADIUS) - MIN_DISTANCE_FOR_NEW_QUERY: - # do not query if are still not close to the border of previous query area - return - - self._query_osm_not_blocking() - - def update_route(self): - def update_proc(): - # Ensure we clear the route on op disengage, this way we can correct possible incorrect map data due - # to wrongly locating or picking up the wrong route. - if self._disengaging: - self.route = None - _debug('Mapd *****: Clearing Route as system is disengaging. ********') - - if self.way_collection is None or self.location_rad is None or self.bearing_rad is None: - _debug('Mapd *****: Can not update route. Missing WayCollection, location or bearing ********') - return - - if self.route is not None and self.last_route_update_fix_timestamp == self.last_gps_fix_timestamp: - _debug('Mapd *****: Skipping route update. No new fix since last update ********') - return - - self.last_route_update_fix_timestamp = self.last_gps_fix_timestamp - - # Create the route if not existent or if it was generated by an older way collection - if self.route is None or self.route.way_collection_id != self.way_collection.id: - self.route = self.way_collection.get_route(self.location_rad, self.bearing_rad, self.location_stdev) - _debug(f'Mapd *****: Route created: \n{self.route}\n********') - return - - # Do not attempt to update the route if the car is going close to a full stop, as the bearing can start - # jumping and creating unnecessary losing of the route. Since the route update timestamp has been updated - # a new liveMapDataSP message will be published with the current values (which is desirable) - if self.gps_speed < FULL_STOP_MAX_SPEED: - _debug('Mapd *****: Route Not updated as car has Stopped ********') - return - - self.route.update(self.location_rad, self.bearing_rad, self.location_stdev) - if self.route.located: - _debug(f'Mapd *****: Route updated: \n{self.route}\n********') - return - - # if an old route did not mange to locate, attempt to regenerate form way collection. - self.route = self.way_collection.get_route(self.location_rad, self.bearing_rad, self.location_stdev) - _debug(f'Mapd *****: Failed to update location in route. Regenerated with route: \n{self.route}\n********') - - # We use the lock when updating the route, as it reads `way_collection` which can ben updated by - # a new query result from the _query_thread. - _debug('Mapd: Locking to update route.', log_to_cloud=False) - with self._lock: - update_proc() - - _debug('Mapd: Releasing Lock to update route', log_to_cloud=False) - - def publish(self, pm, sm): - # Ensure we have a route currently located - if self.route is None or not self.route.located: - _debug('Mapd: Skipping liveMapDataSP message as there is no route or is not located.') - return - - # Ensure we have a route update since last publish - if self.last_publish_fix_timestamp == self.last_route_update_fix_timestamp: - _debug('Mapd: Skipping liveMapDataSP since there is no new gps fix.') - return - - self.last_publish_fix_timestamp = self.last_route_update_fix_timestamp - - speed_limit = self.route.current_speed_limit - next_speed_limit_section = self.route.next_speed_limit_section - turn_speed_limit_section = self.route.current_curvature_speed_limit_section - horizon_mts = self.gps_speed * LOOK_AHEAD_HORIZON_TIME - next_turn_speed_limit_sections = self.route.next_curvature_speed_limit_sections(horizon_mts) - current_road_name = self.route.current_road_name - - map_data_msg = messaging.new_message('liveMapDataSP') - map_data_msg.valid = sm.all_alive(service_list=[self.gps_sock]) and \ - sm.all_valid(service_list=[self.gps_sock]) - - liveMapDataSP = map_data_msg.liveMapDataSP - liveMapDataSP.lastGpsTimestamp = self.last_gps.unixTimestampMillis - liveMapDataSP.lastGpsLatitude = float(self.last_gps.latitude) - liveMapDataSP.lastGpsLongitude = float(self.last_gps.longitude) - liveMapDataSP.lastGpsSpeed = float(self.last_gps.speed) - liveMapDataSP.lastGpsBearingDeg = float(self.last_gps.bearingDeg) - liveMapDataSP.lastGpsAccuracy = float(self.last_gps.accuracy if self.gps_sock == 'gpsLocationExternal' else 1) # gpsLocation doesnt report accuracy - liveMapDataSP.lastGpsBearingAccuracyDeg = float(self.last_gps.bearingAccuracyDeg) - - liveMapDataSP.speedLimitValid = bool(speed_limit is not None) - liveMapDataSP.speedLimit = float(speed_limit if speed_limit is not None else 0.0) - liveMapDataSP.speedLimitAheadValid = bool(next_speed_limit_section is not None) - liveMapDataSP.speedLimitAhead = float(next_speed_limit_section.value - if next_speed_limit_section is not None else 0.0) - liveMapDataSP.speedLimitAheadDistance = float(next_speed_limit_section.start - if next_speed_limit_section is not None else 0.0) - - liveMapDataSP.turnSpeedLimitValid = bool(turn_speed_limit_section is not None) - liveMapDataSP.turnSpeedLimit = float(turn_speed_limit_section.value - if turn_speed_limit_section is not None else 0.0) - liveMapDataSP.turnSpeedLimitSign = int(turn_speed_limit_section.curv_sign - if turn_speed_limit_section is not None else 0) - liveMapDataSP.turnSpeedLimitEndDistance = float(turn_speed_limit_section.end - if turn_speed_limit_section is not None else 0.0) - liveMapDataSP.turnSpeedLimitsAhead = [float(s.value) for s in next_turn_speed_limit_sections] - liveMapDataSP.turnSpeedLimitsAheadDistances = [float(s.start) for s in next_turn_speed_limit_sections] - liveMapDataSP.turnSpeedLimitsAheadSigns = [float(s.curv_sign) for s in next_turn_speed_limit_sections] - - liveMapDataSP.currentRoadName = str(current_road_name if current_road_name is not None else "") - - liveMapDataSP.dataType = self.data_type - - pm.send('liveMapDataSP', map_data_msg) - _debug(f'Mapd *****: Publish: \n{map_data_msg}\n********', log_to_cloud=False) - - -# provides live map data information -def mapd_thread(sm=None, pm=None): - try: - set_core_affinity([0, 1, 2, 3]) - except Exception: - cloudlog.exception("mapd: failed to set core affinity") - mapd = MapD() - rk = Ratekeeper(1., print_delay_threshold=None) # Keeps rate at 1 hz - - # *** setup messaging - if sm is None: - sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal', 'carControl']) - if pm is None: - pm = messaging.PubMaster(['liveMapDataSP']) - - while True: - sm.update() - mapd.udpate_state(sm) - mapd.update_gps(sm) - mapd.updated_osm_data() - mapd.update_route() - mapd.publish(pm, sm) - rk.keep_time() - - -def main(sm=None, pm=None): - mapd_thread(sm, pm) - - -if __name__ == "__main__": - main() diff --git a/selfdrive/mapd/sp_osm_db.json b/selfdrive/mapd/sp_osm_db.json deleted file mode 100644 index 75562fe505..0000000000 --- a/selfdrive/mapd/sp_osm_db.json +++ /dev/null @@ -1,92 +0,0 @@ -{ - "== None ==": { - "name": "", - "waypoint": "", - "url": "" - }, - "Australia": { - "name": "australia", - "waypoint": "514911884", - "url": "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EathcAhPP8dHiTdBiFPIGYoB7zwlUR79OkSTH5dnS-9Shg?e=9cS6er&download=1" - }, - "Brazil": { - "name": "brazil", - "waypoint": "124199196", - "url": "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EXWcrd8ukahDtoFDXVOxvkYBjGGa9b29fVvRdJlADv3YdA?e=jwabo2&download=1" - }, - "Canada": { - "name": "canada", - "waypoint": "68588664", - "url": "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/ETpwTMZftJpCkep2goPmzUQBl5SW_YfHBKLTd7w5CUagkA?e=dshC10&download=1" - }, - "GCC States": { - "name": "gcc-states", - "waypoint": "69021390", - "url": "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EWtFOVR9mj5CoRpATYR0_tUBWNEcy5jeasp8zO0ZYdduRg?e=Cg0l2Z&download=1" - }, - "Germany": { - "name": "germany", - "waypoint": "461526153", - "url": "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EbNzQuA8jHBCuGch1WY4cuoBJfK1S6m5odj5qMnOu3HA3Q?e=cNDn30&download=1" - }, - "Malaysia, Singapore, Brunei": { - "name": "malaysia-singapore-brunei", - "waypoint": "1112741782", - "url": "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EQb9bq_YkE1NuFx0jp-52zoBnAbdo4nyLXMnEY1pUbCB8g?e=CoFiUJ&download=1" - }, - "New Zealand": { - "name": "new-zealand", - "waypoint": "154430132", - "url": "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EVI6eMuNa4dJpXg-r92Vw1EBq5RxR83_7Z5C56AYftg8xQ?e=xouRBl&download=1" - }, - "South Africa": { - "name": "south-africa", - "waypoint": "2729449", - "url": "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EQzbZeus-yBNsgP1KQyLn9wBYZVMMknVoHdB7_ewulLbdA?e=DrnKkb&download=1" - }, - "Spain": { - "name": "spain", - "waypoint": "4263034", - "url": "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/Ee21EfLqZxxLjksDCB_2dscBXKN9Kch8ZOYz3cIWnVpBTg?e=LWBJ1n&download=1" - }, - "Taiwan": { - "name": "taiwan", - "waypoint": "198637969", - "url": "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EXgP3Z5_lsBPiQe-aXOOSY4Bu-r54Z_3kKKmT8-IqLO42A?e=tOhUYw&download=1" - }, - "Turkey": { - "name": "turkey", - "waypoint": "698359658", - "url": "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EWJRrYpV7I1IvOW0LUWrDsUBjEjuKRTla0hA4yRbsVX2xw?e=RGcu8n&download=1" - }, - "US - Florida": { - "name": "florida", - "waypoint": "147221754", - "url": "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/ET9rAvCaretAnO4TjocAddkBFxAziblxblAn9YWzGk5hTw?e=tts5Oz&download=1" - }, - "US - Midwest": { - "name": "us-midwest", - "waypoint": "1059596607", - "url": "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EaDYnugxokRHhOukMndJuZMBKz-kzPfM6C_-iQcki-r94Q?e=oUX6hz&download=1" - }, - "US - Northeast": { - "name": "us-northeast", - "waypoint": "575213527", - "url": "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/ERrlIKNuT4ZIuUOfr4a3BwABLfd7X1wSq9nUrk9buKrIgA?e=HHIT8R&download=1" - }, - "US - Pacific": { - "name": "us-pacific", - "waypoint": "112909709", - "url": "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EQOzoMksk9tAluahQ2qm3rQBHOZD03qFU8Aw4o-TFUTuJA?e=CcBOxL&download=1" - }, - "US - South": { - "name": "us-south", - "waypoint": "243729876", - "url": "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/ETizacxh8lFNk8NMVRyKCEsBMMYZl1iq0r-D4IWoOAd_DQ?e=EVIElm&download=1" - }, - "US - West": { - "name": "us-west", - "waypoint": "30023440", - "url": "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/ETTRiSKwlEVKoRHtylruy7wBKgITKXdaCUAkJMuGAN2_mA?e=7jlglL&download=1" - } -} diff --git a/selfdrive/mapd/test/mock_data.py b/selfdrive/mapd/test/mock_data.py deleted file mode 100644 index ea26da4e46..0000000000 --- a/selfdrive/mapd/test/mock_data.py +++ /dev/null @@ -1,266 +0,0 @@ -from selfdrive.mapd.lib.WayCollection import WayCollection -from selfdrive.mapd.lib.geo import vectors, R -from selfdrive.mapd.lib.NodesData import _MIN_NODE_DISTANCE, _ADDED_NODES_DIST, _SPLINE_EVAL_STEP, \ - _MIN_SPEED_SECTION_LENGTH, nodes_raw_data_array_for_wr, node_calculations, is_wr_a_valid_divertion_from_node, \ - spline_curvature_calculations, speed_limits_for_curvatures_data -from scipy.interpolate import splev, splprep -import numpy as np -import overpy - - -class MockNodesData(): - def __init__(self, way_coords): - self.degrees = np.array(way_coords) - self.radians = np.radians(self.degrees) - - # ***************** - # Expected code implementation nodes_data - self.v = vectors(self.radians) * R - self.d = np.linalg.norm(self.v, axis=1) - self.b = np.arctan2(self.v[:, 0], self.v[:, 1]) - self.v = np.concatenate(([[0., 0.]], self.v)) - self.dp = np.concatenate(([0.], self.d)) - self.dn = np.concatenate((self.d, [0.])) - self.dr = np.cumsum(self.dp, axis=0) - self.b = np.concatenate((self.b, [self.b[-1]])) - - # Expected code implementation spline_curvature_calculations - vect = self.v - dist_prev = self.dp - too_far_idxs = np.nonzero(self.dp >= _MIN_NODE_DISTANCE)[0] - for idx in too_far_idxs[::-1]: - dp = dist_prev[idx] # distance of vector that needs to be replaced by higher resolution vectors. - n = int(np.ceil(dp / _ADDED_NODES_DIST)) # number of vectors that need to be added. - new_v = vect[idx, :] / n # new relative vector to insert. - vect = np.delete(vect, idx, axis=0) # remove the relative vector to be replaced by the insertion of new vectors. - vect = np.insert(vect, [idx] * n, [new_v] * n, axis=0) # insert n new relative vectors - ds = np.cumsum(dist_prev, axis=0) - vs = np.cumsum(vect, axis=0) - tck, u = splprep([vs[:, 0], vs[:, 1]]) # pylint: disable=W0632 - n = max(int(ds[-1] / _SPLINE_EVAL_STEP), len(u)) - unew = np.arange(0, n + 1) / n - d1 = splev(unew, tck, der=1) - d2 = splev(unew, tck, der=2) - num = d1[0] * d2[1] - d1[1] * d2[0] - den = (d1[0]**2 + d1[1]**2)**(1.5) - self.curv = num / den - self.curv_ds = unew * ds[-1] - # ***************** - - -class MockCurveSection(): - def __init__(self, func, di=0., df=1000., step=10.): - self.di = di - self.df = df - self.n = (df - di) // step - self.u = np.arange(0, self.n + 1) / self.n - self.curv_ds = self.u * (df - di) + di - self.curv = func(self.u) - self.curv_abs = np.abs(self.curv) - self.curv_sec = np.column_stack((self.curv_abs, np.sign(self.curv), self.curv_ds)) - - -class MockOSMQueryResponse(): - def __init__(self, xml_path, query_center): - self.api = overpy.Overpass() - self.query_center = np.radians(np.array(query_center)) - - with open(xml_path, 'r') as f: - overpass_xml = f.read() - self.ways = self.api.parse_xml(overpass_xml).ways - - self.wayCollection = WayCollection(self.ways, self.query_center) - -class MockRouteData(): - def __init__(self, way_ids, way_collection, first_node_id): # way)ids must be in order forming a route. - self.wrs = [next(wr for wr in way_collection.way_relations if wr.id == way_id) for way_id in way_ids] - self.way_collection = way_collection - self.first_node_id = first_node_id - - def reset(self): - way_relations = self.wrs - wr_index = self.way_collection.wr_index - - # Nodes Data processing expects way relations to be updated with direction before running. - for idx, wr in enumerate(way_relations): - if idx == 0: - wr.update_direction_from_starting_node(self.first_node_id) - else: - wr.update_direction_from_starting_node(way_relations[idx - 1].last_node.id) - - # ***** Expected calculations - self._nodes_data = np.array([]) - self._divertions = [[]] - self._curvature_speed_sections_data = np.array([]) - way_count = len(way_relations) - if way_count == 0: - return - # We want all the nodes from the last way section - nodes_data = nodes_raw_data_array_for_wr(way_relations[-1]) - # For the ways before the last in the route we want all the nodes but the last, as that one is the first on - # the next section. Collect them, append last way node data and concatenate the numpy arrays. - if way_count > 1: - wrs_data = tuple([nodes_raw_data_array_for_wr(wr, drop_last=True) for wr in way_relations[:-1]]) - wrs_data += (nodes_data,) - nodes_data = np.concatenate(wrs_data) - # Get a subarray with lat, lon to compute the remaining node values. - lat_lon_array = nodes_data[:, [1, 2]] - points = np.radians(lat_lon_array) - # Ensure we have more than 3 points, if not calculations are not possible. - if len(points) <= 3: - return - vect, dist_prev, dist_next, dist_route, bearing = node_calculations(points) - # append calculations to nodes_data - # nodes_data structure: [id, lat, lon, speed_limit, x, y, dist_prev, dist_next, dist_route, bearing] - self._nodes_data = np.column_stack((nodes_data, vect, dist_prev, dist_next, dist_route, bearing)) - # Build route diversion options data from the wr_index. - wr_ids = [wr.id for wr in way_relations] - self._divertions = [[wr for wr in wr_index.way_relations_with_edge_node_id(node_id) - if is_wr_a_valid_divertion_from_node(wr, node_id, wr_ids)] - for node_id in nodes_data[:, 0]] - # Store calculcations for curvature sections speed limits. We need more than 3 points to be able to process. - # _curvature_speed_sections_data structure: [dist_start, dist_stop, speed_limits, curv_sign] - if len(vect) > 3: - self._curv, self._curv_ds = spline_curvature_calculations(vect, dist_prev) - self._curvature_speed_sections_data = speed_limits_for_curvatures_data(self._curv, self._curv_ds) - # ***** - - -# Test data in degrees from this road: -# https://www.google.de/maps/@52.209263,13.8723137,13z -_WAY_NODES_COORDS_01 = [ - [52.1933703, 13.8723799], - [52.1939477, 13.8711273], - [52.1942004, 13.8705818], - [52.1945408, 13.8698496], - [52.1948447, 13.8691873], - [52.1950772, 13.8685726], - [52.1951168, 13.8684641], - [52.1956681, 13.8670323], - [52.1958716, 13.8664936], - [52.1964366, 13.8649875], - [52.1969283, 13.8636040], - [52.1970203, 13.8634430], - [52.1975486, 13.8626307], - [52.1976354, 13.8624971], - [52.1977827, 13.8621795], - [52.1978564, 13.8619220], - [52.1981843, 13.8604497], - [52.1982614, 13.8602140], - [52.1983351, 13.8600595], - [52.1992768, 13.8579824], - [52.1995107, 13.8574321], - [52.1995948, 13.8572604], - [52.1996818, 13.8571155], - [52.1998000, 13.8570029], - [52.2000659, 13.8568236], - [52.2003868, 13.8566005], - [52.2007182, 13.8564460], - [52.2008760, 13.8564117], - [52.2009865, 13.8564117], - [52.2011390, 13.8564202], - [52.2012267, 13.8564496], - [52.2012544, 13.8564577], - [52.2013179, 13.8564803], - [52.2020491, 13.8571756], - [52.2026014, 13.8576991], - [52.2027592, 13.8578879], - [52.2027960, 13.8579309], - [52.2028960, 13.8580939], - [52.2030170, 13.8583343], - [52.2036587, 13.8597076], - [52.2052946, 13.8633039], - [52.2064332, 13.8658435], - [52.2067856, 13.8666332], - [52.2068961, 13.8668477], - [52.2070777, 13.8670890], - [52.2073723, 13.8674409], - [52.2077457, 13.8679387], - [52.2083874, 13.8687455], - [52.2093341, 13.8699214], - [52.2099652, 13.8707540], - [52.2102282, 13.8712089], - [52.2104228, 13.8715694], - [52.2106122, 13.8718955], - [52.2107619, 13.8721756], - [52.2108695, 13.8723771], - [52.2110747, 13.8727610], - [52.2111514, 13.8729047], - [52.2114010, 13.8733718], - [52.2114694, 13.8735006], - [52.2115430, 13.8736636], - [52.2116086, 13.8737571], - [52.2116770, 13.8738172], - [52.2117611, 13.8738515], - [52.2118664, 13.8738566], - [52.2119322, 13.8738439], - [52.2121058, 13.8737924], - [52.2122583, 13.8737495], - [52.2123265, 13.8737260], - [52.2124213, 13.8736894], - [52.2127466, 13.8734888], - [52.2128263, 13.8734491], - [52.2131313, 13.8733117], - [52.2133943, 13.8731830], - [52.2136625, 13.8731057], - [52.2139465, 13.8730456], - [52.2143619, 13.8730113], - [52.2148773, 13.8729942], - [52.2152275, 13.8730325], - [52.2153110, 13.8730398], - [52.2157442, 13.8730848], - [52.2158833, 13.8731036]] - - -mockNodesData01 = MockNodesData(_WAY_NODES_COORDS_01) - -# OSM Query around B96 south of Berlin -mockOSMResponse01 = MockOSMQueryResponse('selfdrive/mapd/test/mock_osm_response_01.xml', - [52.31400353586984, 13.447158941786366]) - -# OSM Query on curvy town area south of Germany. -mockOSMResponse02 = MockOSMQueryResponse('selfdrive/mapd/test/mock_osm_response_02.xml', - [48.16573269276522, 9.81418473659117]) - -mockWayCollection01 = WayCollection(mockOSMResponse01.ways, mockOSMResponse01.query_center) -mockWayCollection02 = WayCollection(mockOSMResponse02.ways, mockOSMResponse02.query_center) - -# Normal curvy Way. way id: 179532213 with 35 Nodes. -mockOSMWay_01_01_LongCurvy = next(way for way in mockOSMResponse01.ways if way.id == 179532213) - -# Looped way. way id: 29233907 -mockOSMWay_01_02_Loop = next(way for way in mockOSMResponse01.ways if way.id == 29233907) - -# Complex curvy road through town with intersections. way id:178450395 -mockOSMWay_02_01_CurvyTownWithIntersections = next(way for way in mockOSMResponse02.ways if way.id == 178450395) - -# Valid diversion for way 02_01 at node: 34785115. way id: 27955186 -mockOSMWay_02_02_Divertion_34785115 = next(way for way in mockOSMResponse02.ways if way.id == 27955186) - -# 3 node way. way id: 807781992 -mockOSMWay_02_03_Short_3_node_way = next(way for way in mockOSMResponse02.ways if way.id == 807781992) - -# data composing route 01 in way collection 02 -mockRouteData_02_01 = MockRouteData([60890967, 737120246, 601406617, 60890971, 178450395], mockWayCollection02, - first_node_id=201962346) - -# data composing route 02 in way collection 02. Single WR -mockRouteData_02_02_single_wr = MockRouteData([178450395], mockWayCollection02, first_node_id=762086638) - -# data composing route 03 in way collection 02. Multiple speed limits -mockRouteData_02_03 = MockRouteData([158799549, 798805532, 28707704, 158797898, 602249535, 602249536, 825823509, - 178449088, 916462523, 158796386], mockWayCollection02, - first_node_id=252601829) - -# 1000mt section with one full sin cycle as curv values. -mockCurveSectionSin = MockCurveSection(lambda x: np.sin(x * 2 * np.pi)) - -# 200mt section with changing curvature rate. -mockCurveSteepCurvChange = MockCurveSection(lambda x: 0.05 * x**3 - 0.007 * x**2 + 0.001 * x, df=200) - -# _MIN_SPEED_SECTION_LENGTH section with changing curvature rate. -mockCurveSteepCurvChangeShort = MockCurveSection( - lambda x: 0.05 * x**3 - 0.007 * x**2 + 0.001 * x, df=_MIN_SPEED_SECTION_LENGTH) - -# 200mt section with smooth changing curvature rate. no deviation over 2. -mockCurveSmoothCurveChange = MockCurveSection(lambda x: 0.0002 * x**3 - 0.001 * x**2 + 0.6 * x, df=200) diff --git a/selfdrive/mapd/test/mock_osm_response_01.xml b/selfdrive/mapd/test/mock_osm_response_01.xml deleted file mode 100644 index 7d9a8cf56f..0000000000 --- a/selfdrive/mapd/test/mock_osm_response_01.xml +++ /dev/null @@ -1,9908 +0,0 @@ - - - -The data included in this document is from www.openstreetmap.org. The data is made available under ODbL. - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/selfdrive/mapd/test/mock_osm_response_02.xml b/selfdrive/mapd/test/mock_osm_response_02.xml deleted file mode 100644 index 85886017a1..0000000000 --- a/selfdrive/mapd/test/mock_osm_response_02.xml +++ /dev/null @@ -1,11529 +0,0 @@ - - - -The data included in this document is from www.openstreetmap.org. The data is made available under ODbL. - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/selfdrive/mapd/test/test_NodesData.py b/selfdrive/mapd/test/test_NodesData.py deleted file mode 100644 index 0138173b90..0000000000 --- a/selfdrive/mapd/test/test_NodesData.py +++ /dev/null @@ -1,354 +0,0 @@ -import unittest -import numpy as np -from selfdrive.mapd.lib.geo import DIRECTION -from common.conversions import Conversions as CV -from selfdrive.mapd.lib.WayRelation import WayRelation -from selfdrive.mapd.lib.NodesData import nodes_raw_data_array_for_wr, node_calculations, \ - spline_curvature_calculations, split_speed_section_by_sign, split_speed_section_by_curv_degree, speed_section, \ - speed_limits_for_curvatures_data, is_wr_a_valid_divertion_from_node, SpeedLimitSection, TurnSpeedLimitSection, \ - NodesData, NodeDataIdx -from selfdrive.mapd.test.mock_data import mockOSMWay_01_01_LongCurvy, mockNodesData01, mockCurveSectionSin, \ - mockCurveSteepCurvChange, mockCurveSteepCurvChangeShort, mockCurveSmoothCurveChange, \ - mockOSMWay_02_01_CurvyTownWithIntersections, mockOSMWay_02_02_Divertion_34785115, mockOSMWay_02_03_Short_3_node_way, \ - mockRouteData_02_01, mockRouteData_02_02_single_wr, mockRouteData_02_03 -from numpy.testing import assert_array_almost_equal - - -class TestNodesDataFileFunctions(unittest.TestCase): - def test_nodes_raw_data_array_for_wr(self): - wr = WayRelation(mockOSMWay_01_01_LongCurvy) - data_e = np.array([(n.id, n.lat, n.lon, wr.speed_limit) for n in wr.way.nodes], dtype=float) - data = nodes_raw_data_array_for_wr(wr) - - assert_array_almost_equal(data, data_e) - - def test_nodes_raw_data_array_for_wr_flips_when_backwards(self): - wr = WayRelation(mockOSMWay_01_01_LongCurvy) - wr.direction = DIRECTION.BACKWARD - - data_e = np.array([(n.id, n.lat, n.lon, wr.speed_limit) for n in wr.way.nodes], dtype=float) - data_e = np.flip(data_e, axis=0) - - data = nodes_raw_data_array_for_wr(wr) - - assert_array_almost_equal(data, data_e) - - def test_nodes_raw_data_array_for_wr_drops_last(self): - wr = WayRelation(mockOSMWay_01_01_LongCurvy) - data_e = np.array([(n.id, n.lat, n.lon, wr.speed_limit) for n in wr.way.nodes], dtype=float)[:-1] - data = nodes_raw_data_array_for_wr(wr, drop_last=True) - - assert_array_almost_equal(data, data_e) - - def test_node_calculations(self): - points = mockNodesData01.radians - - v, dp, dn, dr, b = node_calculations(points) - - assert_array_almost_equal(v, mockNodesData01.v) - assert_array_almost_equal(dp, mockNodesData01.dp) - assert_array_almost_equal(dn, mockNodesData01.dn) - assert_array_almost_equal(dr, mockNodesData01.dr) - assert_array_almost_equal(b, mockNodesData01.b) - - def test_node_calculations_index_error(self): - points = mockNodesData01.radians[:2] - - with self.assertRaises(IndexError): - node_calculations(points) - - def test_spline_curvature_calculations(self): - vect = mockNodesData01.v - dist_prev = mockNodesData01.dp - - curv, curv_ds = spline_curvature_calculations(vect, dist_prev) - - assert_array_almost_equal(curv, mockNodesData01.curv) - assert_array_almost_equal(curv_ds, mockNodesData01.curv_ds) - - def test_spline_curvature_calculations_with_route_data(self): - mockRouteData_02_01.reset() - nodes_data = mockRouteData_02_01._nodes_data - vect = np.column_stack((nodes_data[:, 4], nodes_data[:, 5])) - dist_prev = nodes_data[:, 6] - - curv, curv_ds = spline_curvature_calculations(vect, dist_prev) - - assert_array_almost_equal(curv, mockRouteData_02_01._curv) - assert_array_almost_equal(curv_ds, mockRouteData_02_01._curv_ds) - - def test_split_speed_section_by_sign(self): - curv_sec = mockCurveSectionSin.curv_sec - new_secs = split_speed_section_by_sign(curv_sec) - - # 3 sections with matching initial and final distance - self.assertEqual(len(new_secs), 3) - self.assertEqual(new_secs[0][0][2], mockCurveSectionSin.di) - self.assertEqual(new_secs[2][-1][2], mockCurveSectionSin.df) - - # All new sections has same sign internally - for sec in new_secs: - self.assertEqual(np.average(sec, axis=0)[1], sec[0][1]) - - # Sections change sign - for idx in range(2): - self.assertNotEqual(new_secs[idx][0][1], new_secs[idx + 1][0][1]) - - # total items consistency - lengths = [len(sec) for sec in new_secs] - self.assertEqual(len(curv_sec), sum(lengths)) - - def test_split_speed_section_by_curv_degree(self): - curv_sec = mockCurveSteepCurvChange.curv_sec - new_secs = split_speed_section_by_curv_degree(curv_sec) - - # 3 sections with matching initial and final distance - self.assertEqual(len(new_secs), 3) - self.assertEqual(new_secs[0][0][2], mockCurveSteepCurvChange.di) - self.assertEqual(new_secs[2][-1][2], mockCurveSteepCurvChange.df) - - # Sections split at the right points - split_dist = [sec[-1][2] for sec in new_secs] - self.assertListEqual(split_dist, [50., 150., 200.]) - - def test_split_speed_section_by_curv_degree_does_nothing_if_short(self): - curv_sec = mockCurveSteepCurvChangeShort.curv_sec - new_secs = split_speed_section_by_curv_degree(curv_sec) - - self.assertEqual(len(new_secs), 1) - assert_array_almost_equal(curv_sec, new_secs[0]) - - def test_split_speed_section_by_curv_degree_does_nothing_if_no_substantial_change(self): - curv_sec = mockCurveSmoothCurveChange.curv_sec - new_secs = split_speed_section_by_curv_degree(curv_sec) - - self.assertEqual(len(new_secs), 1) - assert_array_almost_equal(curv_sec, new_secs[0]) - - def test_speed_section(self): - curv_sec = mockCurveSectionSin.curv_sec - - speed_secs = speed_section(curv_sec) - expected = np.array([0., 1000., 1.51657509, 1.]) - - assert_array_almost_equal(speed_secs, expected) - - def test_speed_limits_for_curvatures_data(self): - curv = mockCurveSectionSin.curv - curv_ds = mockCurveSectionSin.curv_ds - - expected = np.array([ - [10., 490., 1.51657509, 1.], - [510., 990., 1.51657509, -1.]]) - limits = speed_limits_for_curvatures_data(curv, curv_ds) - - assert_array_almost_equal(limits, expected) - - def test_is_wr_a_valid_divertion_from_node(self): - wr = WayRelation(mockOSMWay_02_01_CurvyTownWithIntersections) - mockOSMWay_02_02_Divertion_34785115.tags['oneway'] = 'yes' - wr_div = WayRelation(mockOSMWay_02_02_Divertion_34785115) - - # False if id already in route - wr_ids = [wr.id, wr_div.id] - self.assertFalse(is_wr_a_valid_divertion_from_node(wr_div, 34785115, wr_ids)) - - # True if id not in route, node_id is edge and not prohibited - wr_ids = [wr.id, 11111, 22222] - self.assertTrue(is_wr_a_valid_divertion_from_node(wr_div, 34785115, wr_ids)) - - # False if id not in route, node_id is edge but prohibited (wrong direction from node 319503453) - self.assertFalse(is_wr_a_valid_divertion_from_node(wr_div, 319503453, wr_ids)) - - # False if id not in route, node_id is not edge - self.assertFalse(is_wr_a_valid_divertion_from_node(wr_div, 44444, wr_ids)) - - -class TestSpeedLimitSection(unittest.TestCase): - def test_speed_limit_section_init(self): - section = SpeedLimitSection(10., 20., 50.) - - self.assertEqual(section.start, 10.) - self.assertEqual(section.end, 20.) - self.assertEqual(section.value, 50.) - - -class TestTurnSpeedLimitSection(unittest.TestCase): - def test_turn_speed_limit_section_init(self): - section = TurnSpeedLimitSection(10., 20., 50., -1.) - - self.assertEqual(section.start, 10.) - self.assertEqual(section.end, 20.) - self.assertEqual(section.value, 50.) - self.assertEqual(section.curv_sign, -1.) - - -class TestNodesData(unittest.TestCase): - def test_init_with_empty_list(self): - nodesData = NodesData([], {}) - - self.assertEqual(len(nodesData._nodes_data), 0) - num_diverstions = sum([len(d) for d in nodesData._divertions]) - self.assertEqual(num_diverstions, 0) - self.assertEqual(len(nodesData._curvature_speed_sections_data), 0) - - def test_init_with_single_wr_includes_all_wr_nodes(self): - mockRouteData_02_02_single_wr.reset() - way_relations = mockRouteData_02_02_single_wr.wrs - wr_index = mockRouteData_02_02_single_wr.way_collection.wr_index - - nodesData = NodesData(way_relations, wr_index) - - assert_array_almost_equal(nodesData._nodes_data, mockRouteData_02_02_single_wr._nodes_data) - assert_array_almost_equal(nodesData._curvature_speed_sections_data, - mockRouteData_02_02_single_wr._curvature_speed_sections_data) - self.assertListEqual(nodesData._divertions, mockRouteData_02_02_single_wr._divertions) - self.assertEqual(len(nodesData._nodes_data), len(way_relations[0].way.nodes)) - self.assertEqual(len(nodesData._curvature_speed_sections_data), 6) - num_diverstions = sum([len(d) for d in nodesData._divertions]) - self.assertEqual(num_diverstions, 6) - - def test_init_with_less_than_4_nodes(self): - wr_t = WayRelation(mockOSMWay_02_03_Short_3_node_way) - - nodesData = NodesData([wr_t], {}) - - self.assertEqual(len(nodesData._nodes_data), 0) - num_diverstions = sum([len(d) for d in nodesData._divertions]) - self.assertEqual(num_diverstions, 0) - self.assertEqual(len(nodesData._curvature_speed_sections_data), 0) - - def test_init_with_multiple_wr(self): - mockRouteData_02_01.reset() - way_relations = mockRouteData_02_01.wrs - wr_index = mockRouteData_02_01.way_collection.wr_index - - nodesData = NodesData(way_relations, wr_index) - - assert_array_almost_equal(nodesData._nodes_data, mockRouteData_02_01._nodes_data) - assert_array_almost_equal(nodesData._curvature_speed_sections_data, mockRouteData_02_01._curvature_speed_sections_data) - self.assertListEqual(nodesData._divertions, mockRouteData_02_01._divertions) - self.assertEqual(len(nodesData._curvature_speed_sections_data), 9) - num_diverstions = sum([len(d) for d in nodesData._divertions]) - self.assertEqual(num_diverstions, 14) - - def test_count(self): - mockRouteData_02_01.reset() - way_relations = mockRouteData_02_01.wrs - wr_index = mockRouteData_02_01.way_collection.wr_index - num_n = sum([len(wr.way.nodes) for wr in way_relations]) - len(way_relations) + 1 - - nodesData = NodesData(way_relations, wr_index) - - self.assertEqual(nodesData.count, num_n) - - def test_get_on_empty(self): - wr_t = WayRelation(mockOSMWay_02_03_Short_3_node_way) - - nodesData = NodesData([wr_t], {}) - assert_array_almost_equal(nodesData.get(NodeDataIdx.node_id), np.array([])) - - def test_get_values(self): - mockRouteData_02_01.reset() - way_relations = mockRouteData_02_01.wrs - wr_index = mockRouteData_02_01.way_collection.wr_index - - nodesData = NodesData(way_relations, wr_index) - - assert_array_almost_equal(nodesData.get(NodeDataIdx.node_id), mockRouteData_02_01._nodes_data[:, 0]) - assert_array_almost_equal(nodesData.get(NodeDataIdx.lat), mockRouteData_02_01._nodes_data[:, 1]) - assert_array_almost_equal(nodesData.get(NodeDataIdx.lon), mockRouteData_02_01._nodes_data[:, 2]) - assert_array_almost_equal(nodesData.get(NodeDataIdx.speed_limit), mockRouteData_02_01._nodes_data[:, 3]) - assert_array_almost_equal(nodesData.get(NodeDataIdx.x), mockRouteData_02_01._nodes_data[:, 4]) - assert_array_almost_equal(nodesData.get(NodeDataIdx.y), mockRouteData_02_01._nodes_data[:, 5]) - assert_array_almost_equal(nodesData.get(NodeDataIdx.dist_prev), mockRouteData_02_01._nodes_data[:, 6]) - assert_array_almost_equal(nodesData.get(NodeDataIdx.dist_next), mockRouteData_02_01._nodes_data[:, 7]) - assert_array_almost_equal(nodesData.get(NodeDataIdx.dist_route), mockRouteData_02_01._nodes_data[:, 8]) - assert_array_almost_equal(nodesData.get(NodeDataIdx.bearing), mockRouteData_02_01._nodes_data[:, 9]) - - def test_speed_limits_ahead_from_empty(self): - wr_t = WayRelation(mockOSMWay_02_03_Short_3_node_way) - - nodesData = NodesData([wr_t], {}) - self.assertEqual(len(nodesData.speed_limits_ahead(1, 10.)), 0) - - def test_speed_limits_ahead(self): - mockRouteData_02_03.reset() - way_relations = mockRouteData_02_03.wrs - wr_index = mockRouteData_02_03.way_collection.wr_index - - nodesData = NodesData(way_relations, wr_index) - - # empty when ahead_idx is none. - self.assertEqual(len(nodesData.speed_limits_ahead(None, 10.)), 0) - - # All limist from 0 - all_limits = nodesData.speed_limits_ahead(1, nodesData.get(NodeDataIdx.dist_next)[0]) - self.assertEqual(len(all_limits), 4) # 4 limits on this mock road. - self.assertListEqual([sl.value for sl in all_limits], [v * CV.KPH_TO_MS for v in [50, 100, 50, 100]]) - for idx, sl in enumerate(all_limits): - self.assertTrue(sl.end > sl.start) - self.assertTrue(sl.value > 0.) - if idx == 0: - self.assertEqual(sl.start, 0.) - else: - self.assertEqual(sl.start, all_limits[idx - 1].end) - self.assertNotEqual(sl.value, all_limits[idx - 1].value) - - def test_distance_to_end_from_empty(self): - wr_t = WayRelation(mockOSMWay_02_03_Short_3_node_way) - - nodesData = NodesData([wr_t], {}) - self.assertIsNone(nodesData.distance_to_end(1, 10.)) - - def test_distance_to_end(self): - mockRouteData_02_03.reset() - way_relations = mockRouteData_02_03.wrs - wr_index = mockRouteData_02_03.way_collection.wr_index - - nodesData = NodesData(way_relations, wr_index) - - # none when ahead_idx is none. - self.assertIsNone(nodesData.distance_to_end(None, 10.)) - - # From the beginning - expected = np.sum(nodesData.get(NodeDataIdx.dist_next)) - self.assertAlmostEqual(nodesData.distance_to_end(1, nodesData.get(NodeDataIdx.dist_next)[0]), expected) - self.assertAlmostEqual(nodesData.get(NodeDataIdx.dist_route)[-1], expected) - - # From the node next to last - expected = nodesData.get(NodeDataIdx.dist_next)[-2] - self.assertAlmostEqual(nodesData.distance_to_end(nodesData.count - 2, 0.), expected) - - def test_distance_to_node(self): - mockRouteData_02_03.reset() - way_relations = mockRouteData_02_03.wrs - wr_index = mockRouteData_02_03.way_collection.wr_index - - nodesData = NodesData(way_relations, wr_index) - dist_to_node_ahead = 10. - node_id = 1887995486 # Some node id in the middle of the way. idx 50 - node_idx = np.nonzero(nodesData.get(NodeDataIdx.node_id) == node_id)[0][0] - - # none when ahead_idx is none. - self.assertIsNone(nodesData.distance_to_node(node_id, None, dist_to_node_ahead)) - - # From the beginning - expected = nodesData.get(NodeDataIdx.dist_route)[node_idx] - self.assertAlmostEqual(nodesData.distance_to_node(node_id, 1, nodesData.get(NodeDataIdx.dist_next)[0]), expected) - - # From the end - expected = -np.sum(nodesData.get(NodeDataIdx.dist_next)[node_idx:]) - self.assertAlmostEqual(nodesData.distance_to_node(node_id, len(nodesData.get(NodeDataIdx.node_id)) - 1, 0.), expected) - - # From some node behind including dist to node ahead - ahead_idx = node_idx - 10 - expected = np.sum(nodesData.get(NodeDataIdx.dist_next)[ahead_idx:node_idx]) + dist_to_node_ahead - self.assertAlmostEqual(nodesData.distance_to_node(node_id, ahead_idx, dist_to_node_ahead), expected) - - # From some node ahead including dist to node ahead - ahead_idx = node_idx + 10 - expected = -np.sum(nodesData.get(NodeDataIdx.dist_next)[node_idx:ahead_idx]) + dist_to_node_ahead - self.assertAlmostEqual(nodesData.distance_to_node(node_id, ahead_idx, dist_to_node_ahead), expected) - -# TODO: Missing tests for curvatures_speed_limit_sections_ahead and possible_divertions diff --git a/selfdrive/mapd/test/test_WayRelation.py b/selfdrive/mapd/test/test_WayRelation.py deleted file mode 100644 index 0e3ea030c6..0000000000 --- a/selfdrive/mapd/test/test_WayRelation.py +++ /dev/null @@ -1,651 +0,0 @@ -import copy -import unittest -import numpy as np -from unittest import mock -from numpy.testing import assert_array_almost_equal -from datetime import datetime as dt, timezone, timedelta -from common.conversions import Conversions as CV -from selfdrive.mapd.lib.WayRelation import WayRelation, is_osm_time_condition_active, \ - conditional_speed_limit_for_osm_tag_limit_string, speed_limit_for_osm_tag_limit_string -from selfdrive.mapd.config import LANE_WIDTH -from selfdrive.mapd.lib.geo import DIRECTION, R, vectors -from selfdrive.mapd.test.mock_data import mockOSMWay_01_01_LongCurvy, mockOSMWay_01_02_Loop, \ - mockOSMWay_02_01_CurvyTownWithIntersections - - -class TestWayRelationFileFunctions(unittest.TestCase): - def test_speed_limit_for_osm_tag_limit_string(self): - values = [ - None, # Invalid - "1000", # Invalid - "60 kph", # Invalid - "100", - "30 mph", - "DE:zone:40", - "DE:zone:50 mph", - "AR:urban", - "CZ:pedestrian_zone", - "DK:urban", - "DK:rural", - "DK:motorway", - "DE:living_street", - "DE:residential", - "DE:urban", - "DE:rural", - "DE:trunk", # No limit - "DE:motorway", # No limit - "GB:nsl_restricted", - "GB:nsl_single", - "GB:nsl_dual", - "GB:motorway", - "GB:invalid", # Invalid - ] - - expected = [ - 0., - 0., - 0., - 100. * CV.KPH_TO_MS, - 30. * CV.MPH_TO_MS, - 40. * CV.KPH_TO_MS, - 50. * CV.MPH_TO_MS, - 40. * CV.KPH_TO_MS, - 20. * CV.KPH_TO_MS, - 50. * CV.KPH_TO_MS, - 80. * CV.KPH_TO_MS, - 130. * CV.KPH_TO_MS, - 7. * CV.KPH_TO_MS, - 30. * CV.KPH_TO_MS, - 50. * CV.KPH_TO_MS, - 100. * CV.KPH_TO_MS, - 0., - 0., - 30. * CV.MPH_TO_MS, - 60. * CV.MPH_TO_MS, - 70. * CV.MPH_TO_MS, - 70. * CV.MPH_TO_MS, - 0., - ] - - result = [speed_limit_for_osm_tag_limit_string(sls) for sls in values] - - self.assertEqual(result, expected) - - @mock.patch('selfdrive.mapd.lib.WayRelation.dt') - def test_is_osm_time_condition_active(self, mock_dt): - tz = timezone(timedelta(hours=1), 'berlin') - wed_10_10_am = dt(2021, 9, 1, 10, 10, 0) - mock_dt.now.return_value = wed_10_10_am - mock_dt.tzinfo = tz - mock_dt.combine = dt.combine - mock_dt.strptime = dt.strptime - - values = [ - "WE", # Invalid - "We", - "Mo", - "Fr", - "Tu-Th", - "10:00", # Invalid - "10:00-10:30", - "We 10:00-10:30", - "SU 10:00-10:30", # Valid, SU string not considered a day string. - "Sa 10:00-10:30", - "Tu-Th 10:00-10:30", - ] - - expected = [ - False, # Invalid - True, - False, - False, - True, - False, # Invalid - True, - True, - True, - False, - True, - ] - - result = [is_osm_time_condition_active(cs) for cs in values] - - self.assertEqual(result, expected) - - @mock.patch('selfdrive.mapd.lib.WayRelation.dt') - def test_conditional_speed_limit_for_osm_tag_limit_string(self, mock_dt): - tz = timezone(timedelta(hours=1), 'berlin') - wed_10_10_am = dt(2021, 9, 1, 10, 10, 0) - mock_dt.now.return_value = wed_10_10_am - mock_dt.tzinfo = tz - mock_dt.combine = dt.combine - mock_dt.strptime = dt.strptime - - values = [ - None, # Invalid - "Hola", # Invalid - "100 @ (WE)", # Invalid - "x @ (We)", # Invalid - "100 @ (We)", - "100 @ (Mo)", - "100 @ (Fr)", - "100 @ (Tu-Th)", - "100 @ (10:00)", # Invalid - "100 @ (10:00-10:30)", - "100 @ (We 10:00-10:30)", - "100 @ (SU 10:00-10:30)", # Valid, SU string not considered a day string. - "100 @ (Sa 10:00-10:30)", - "100 @ (Tu-Th 10:00-10:30)", - "100 @ (Mo-Th;Su)", - "100 @ (Mo Th;Fr-Sa)", - "100 @ (Fr-Su;Mo-Tu)", - "100 @ (10:00-10:30;15:00-16:00)", - "100 @ (We;Mo-Tu)", - "100 @ (We 10:00-10:30;Th 15:00-16:00)", - "100 @ (Tu 10:00-10:30;Th 15:00-16:00)", - ] - - _100 = 100. * CV.KPH_TO_MS - - expected = [ - 0., # Invalid - 0., # Invalid - 0., # Invalid - 0., # Invalid - _100, - 0., - 0., - _100, - 0., # Invalid - _100, - _100, - _100, - 0., - _100, - _100, - _100, - 0., - _100, - _100, - _100, - 0. - ] - - result = [conditional_speed_limit_for_osm_tag_limit_string(ls) for ls in values] - - self.assertEqual(result, expected) - - -class TestWayRelation(unittest.TestCase): - def test_way_relation_init(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - - nodes_np_expected = np.radians(np.array([[node.lat, node.lon] for node in wayRelation.way.nodes], dtype=float)) - v = vectors(wayRelation._nodes_np) - way_distances_expected = np.linalg.norm(v * R, axis=1) - way_bearings_expected = np.arctan2(v[:, 0], v[:, 1]) - bbox_expected = np.array([ - [0.91321784, 0.2346417], - [0.91344672, 0.23475751]]) - - self.assertEqual(wayRelation.way.id, 179532213) - self.assertIsNone(wayRelation.parent_wr_id) - self.assertEqual(wayRelation.direction, DIRECTION.NONE) - self.assertEqual(wayRelation._speed_limit, None) - self.assertEqual(wayRelation._one_way, 'yes') - self.assertEqual(wayRelation.name, None) - self.assertEqual(wayRelation.ref, 'B 96') - self.assertEqual(wayRelation.highway_type, 'trunk') - self.assertEqual(wayRelation.highway_rank, 10) - self.assertEqual(wayRelation.lanes, 2) - assert_array_almost_equal(wayRelation._nodes_np, nodes_np_expected) - assert_array_almost_equal(wayRelation._way_distances, way_distances_expected) - assert_array_almost_equal(wayRelation._way_bearings, way_bearings_expected) - assert_array_almost_equal(wayRelation.bbox, bbox_expected) - self.assertEqual(wayRelation.edge_nodes_ids, [wayRelation.way.nodes[0].id, wayRelation.way.nodes[-1].id]) - - def test_way_relation_init_with_parent(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy, parent=WayRelation(mockOSMWay_01_02_Loop)) - - self.assertEqual(wayRelation.way.id, 179532213) - self.assertEqual(wayRelation.parent_wr_id, 29233907) - - def test_way_relation_equality(self): - wayRelation1 = WayRelation(mockOSMWay_01_01_LongCurvy) - wayRelation2 = copy.copy(wayRelation1) - wayRelation3 = copy.deepcopy(wayRelation1) - wayRelation3.way.id = 123 - - self.assertEqual(wayRelation1, wayRelation2) - self.assertNotEqual(wayRelation1, wayRelation3) - - def test_way_relation_reset_location_variables(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - self.make_wayRelation_location_dirty(wayRelation) - - wayRelation.reset_location_variables() - - self.assert_wayRelation_variables_reset(wayRelation) - - def test_way_relation_id(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - - self.assertEqual(wayRelation.id, 179532213) - - def test_way_relation_road_name(self): - # road name when no tag for name or ref - wayRelation = WayRelation(mockOSMWay_01_02_Loop) - self.assertIsNone(wayRelation.road_name) - # road name based on ref tag - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - self.assertEqual(wayRelation.road_name, "B 96") - # road name based on name tag - wayRelation = WayRelation(mockOSMWay_02_01_CurvyTownWithIntersections) - self.assertEqual(wayRelation.road_name, "Hauptstraße") - - def test_way_relation_update_resets_on_update(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - self.make_wayRelation_location_dirty(wayRelation) - location_rad = np.array([0., 0.]) # Location outside bbox - - wayRelation.update(location_rad, 0., 10.) - - self.assertFalse(wayRelation.is_location_in_bbox(location_rad)) - self.assert_wayRelation_variables_reset(wayRelation) - - def test_way_relation_update_only_resets_if_no_possible_found(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - location_rad = wayRelation.bbox[0] # Location inside bbox but outside actual way (due to padding) - - wayRelation.update(location_rad, 0., 10.) - - self.assertTrue(wayRelation.is_location_in_bbox(location_rad)) - self.assert_wayRelation_variables_reset(wayRelation) - - def test_way_relation_updates_in_the_correct_direction_with_correct_property_values(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - location_rad = np.radians(np.array([52.32855593146639, 13.445320150125069])) - bearing_rad = 0. - - wayRelation.update(location_rad, bearing_rad, 10.) - - self.assertTrue(wayRelation.is_location_in_bbox(location_rad)) - self.assertEqual(wayRelation.direction, DIRECTION.FORWARD) - self.assertEqual(wayRelation.ahead_idx, 17) - self.assertEqual(wayRelation.behind_idx, 16) - self.assertAlmostEqual(wayRelation._distance_to_way, 3.43290781621360) - self.assertAlmostEqual(wayRelation._active_bearing_delta, 0.320717420388962) - self.assertAlmostEqual(wayRelation.distance_to_node_ahead, 25.4998961709014) - self.assertTrue(wayRelation.active) - self.assertFalse(wayRelation.diverting) - assert_array_almost_equal(wayRelation.location_rad, location_rad) - self.assertEqual(wayRelation.bearing_rad, bearing_rad) - self.assertIsNone(wayRelation._speed_limit) - - bearing_rad = 180. - - wayRelation.update(location_rad, bearing_rad, 10.) - - self.assertTrue(wayRelation.is_location_in_bbox(location_rad)) - self.assertEqual(wayRelation.direction, DIRECTION.BACKWARD) - self.assertEqual(wayRelation.ahead_idx, 16) - self.assertEqual(wayRelation.behind_idx, 17) - self.assertAlmostEqual(wayRelation._distance_to_way, 3.43290781621360) - self.assertAlmostEqual(wayRelation._active_bearing_delta, 0.9507682562504284) - self.assertAlmostEqual(wayRelation.distance_to_node_ahead, 11.11623371145368) - self.assertTrue(wayRelation.active) - self.assertFalse(wayRelation.diverting) - assert_array_almost_equal(wayRelation.location_rad, location_rad) - self.assertEqual(wayRelation.bearing_rad, bearing_rad) - self.assertIsNone(wayRelation._speed_limit) - - def test_way_relation_updates_with_location_closest_to_way_when_multiple_possible(self): - wayRelation = WayRelation(mockOSMWay_01_02_Loop) - location_rad = np.radians(np.array([52.313303275461564, 13.437729236325788])) - bearing_rad = np.radians(10.) - - wayRelation.update(location_rad, bearing_rad, 10.) - - self.assertTrue(wayRelation.is_location_in_bbox(location_rad)) - self.assertEqual(wayRelation.direction, DIRECTION.BACKWARD) - self.assertEqual(wayRelation.ahead_idx, 26) - self.assertEqual(wayRelation.behind_idx, 27) - self.assertAlmostEqual(wayRelation._distance_to_way, 10.151775235257011) - self.assertAlmostEqual(wayRelation._active_bearing_delta, 0.06371131069242782) - self.assertAlmostEqual(wayRelation.distance_to_node_ahead, 10.174073707120915) - self.assertTrue(wayRelation.active) - self.assertFalse(wayRelation.diverting) - assert_array_almost_equal(wayRelation.location_rad, location_rad) - self.assertEqual(wayRelation.bearing_rad, bearing_rad) - self.assertIsNone(wayRelation._speed_limit) - - def test_way_relation_updates_will_become_inactive_if_too_far_from_way(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - # Location is 24.9 mts away from the way. There are 2 Lanes in this way. - location_rad = np.radians(np.array([52.328634560607746, 13.445609877522788])) - location_stdev = 5.5 # threshold is 4 * location_stdev + LANE_WIDTH - distance_threshold = 4. * location_stdev + wayRelation.lanes * LANE_WIDTH / 2. - - wayRelation.update(location_rad, 0., location_stdev) - self.assertTrue(wayRelation.active) - self.assertLess(wayRelation._distance_to_way, distance_threshold) - - location_stdev = 5. - - wayRelation.update(location_rad, 0., location_stdev) - self.assertFalse(wayRelation.active) - - def test_way_relation_updates_will_update_diverting_correctly(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - # Location is 24.9 mts away from the way. There are 2 Lanes in this way. - location_rad = np.radians(np.array([52.328634560607746, 13.445609877522788])) - location_stdev = 11. - distance_threshold = 2. * location_stdev + wayRelation.lanes * LANE_WIDTH / 2. - - wayRelation.update(location_rad, 0., location_stdev) - - self.assertLess(wayRelation._distance_to_way, distance_threshold) - self.assertFalse(wayRelation.diverting) - - location_stdev = 10. - distance_threshold = 2. * location_stdev + wayRelation.lanes * LANE_WIDTH / 2. - - wayRelation.update(location_rad, 0., location_stdev) - - self.assertGreater(wayRelation._distance_to_way, distance_threshold) - self.assertTrue(wayRelation.diverting) - - def test_way_relation_update_direction_from_starting_node_resets_speed_limit(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - wayRelation._speed_limit = 10. - - wayRelation.update_direction_from_starting_node(wayRelation.way.nodes[0].id) - - self.assertIsNone(wayRelation._speed_limit) - - def test_way_relation_update_direction_from_starting_node_updates_correctly(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - wayRelation.update_direction_from_starting_node(wayRelation.way.nodes[0].id) - self.assertEqual(wayRelation.direction, DIRECTION.FORWARD) - - wayRelation.update_direction_from_starting_node(wayRelation.way.nodes[-1].id) - self.assertEqual(wayRelation.direction, DIRECTION.BACKWARD) - - wayRelation.update_direction_from_starting_node(0) - self.assertEqual(wayRelation.direction, DIRECTION.NONE) - - def test_way_relation_is_location_in_bbox(self): - wayRelation = WayRelation(mockOSMWay_01_02_Loop) - bbox = wayRelation.bbox - - loc_avg = np.average(bbox, axis=0) - loc_min = np.min(bbox, axis=0) - loc_max = np.max(bbox, axis=0) - - locations = [ - loc_avg, - loc_min, - loc_max, - [loc_avg[0], loc_min[1]], - [loc_avg[0], loc_max[1]], - [loc_min[0], loc_avg[1]], - [loc_max[0], loc_avg[1]], - loc_min - 0.1, - loc_max + 0.1, - [loc_avg[0], loc_min[1] - 0.1], - [loc_avg[0], loc_max[1] + 0.1], - [loc_min[0] - 0.1, loc_avg[1]], - [loc_max[0] + 0.1, loc_avg[1]], - ] - - is_in = [wayRelation.is_location_in_bbox(loc) for loc in locations] - - self.assertEqual(is_in, [True, True, True, True, True, True, True, False, False, False, False, False, False]) - - def test_way_relation_speed_limit_when_set(self): - wayRelation = WayRelation(mockOSMWay_01_02_Loop) - wayRelation._speed_limit = 10. - - self.assertEqual(wayRelation.speed_limit, 10.) - - @mock.patch('selfdrive.mapd.lib.WayRelation.dt') - def test_way_relation_speed_limit_conditional(self, mock_dt): - tz = timezone(timedelta(hours=1), 'berlin') - wed_10_10_am = dt(2021, 9, 1, 10, 10, 0) - mock_dt.now.return_value = wed_10_10_am - mock_dt.tzinfo = tz - mock_dt.combine = dt.combine - mock_dt.strptime = dt.strptime - - # Reset all tags before teting - mockOSMWay_01_02_Loop.tags = {} - wayRelation = WayRelation(mockOSMWay_01_02_Loop) - - # No Value - self.assertEqual(wayRelation.speed_limit, 0.) - - # Value on both directions - wayRelation._speed_limit = None - wayRelation.way.tags["maxspeed:conditional"] = "100 @ (We 10:00-10:30)" - self.assertEqual(wayRelation.speed_limit, 100. * CV.KPH_TO_MS) - - # Value on forward - wayRelation.way.tags.pop("maxspeed:conditional") - wayRelation._speed_limit = None - wayRelation.direction = DIRECTION.FORWARD - self.assertEqual(wayRelation.speed_limit, 0.) - - wayRelation._speed_limit = None - wayRelation.way.tags["maxspeed:forward:conditional"] = "100 @ (We 10:00-10:30)" - self.assertEqual(wayRelation.speed_limit, 100. * CV.KPH_TO_MS) - - # Value on backward - wayRelation._speed_limit = None - wayRelation.direction = DIRECTION.BACKWARD - self.assertEqual(wayRelation.speed_limit, 0.) - - wayRelation._speed_limit = None - wayRelation.way.tags["maxspeed:backward:conditional"] = "100 @ (We 10:00-10:30)" - self.assertEqual(wayRelation.speed_limit, 100. * CV.KPH_TO_MS) - - def test_way_relation_speed_limit_maxspeed(self): - # Reset all tags before teting - mockOSMWay_01_02_Loop.tags = {} - wayRelation = WayRelation(mockOSMWay_01_02_Loop) - - # No Value - self.assertEqual(wayRelation.speed_limit, 0.) - - # Value on both directions - wayRelation._speed_limit = None - wayRelation.way.tags["maxspeed"] = "100" - self.assertEqual(wayRelation.speed_limit, 100. * CV.KPH_TO_MS) - - # Value on forward - wayRelation.way.tags.pop("maxspeed") - wayRelation._speed_limit = None - wayRelation.direction = DIRECTION.FORWARD - self.assertEqual(wayRelation.speed_limit, 0.) - - wayRelation._speed_limit = None - wayRelation.way.tags["maxspeed:forward"] = "100" - self.assertEqual(wayRelation.speed_limit, 100. * CV.KPH_TO_MS) - - # Value on backward - wayRelation._speed_limit = None - wayRelation.direction = DIRECTION.BACKWARD - self.assertEqual(wayRelation.speed_limit, 0.) - - wayRelation._speed_limit = None - wayRelation.way.tags["maxspeed:backward"] = "100" - self.assertEqual(wayRelation.speed_limit, 100. * CV.KPH_TO_MS) - - def test_way_relation_active_bearing_delta_reflects_internal_value(self): - wayRelation = WayRelation(mockOSMWay_01_02_Loop) - wayRelation._active_bearing_delta = 10. - self.assertEqual(wayRelation.active_bearing_delta, 10.) - - def test_way_relation_is_one_way(self): - # Setup initial tags - mockOSMWay_01_02_Loop.tags = { - 'oneway': 'yes', - 'highway': 'unclassified' - } - wayRelation = WayRelation(mockOSMWay_01_02_Loop) - - # oneway = yes - self.assertTrue(wayRelation.is_one_way) - - # oneway non existing - wayRelation._one_way = None - self.assertFalse(wayRelation.is_one_way) - - # highway = motorway - wayRelation.highway_type = 'motorway' - self.assertTrue(wayRelation.is_one_way) - - def test_way_relation_is_prohibited(self): - # Setup initial tags - mockOSMWay_01_02_Loop.tags = { - 'oneway': 'yes' - } - wayRelation = WayRelation(mockOSMWay_01_02_Loop) - - # Direction undefined - wayRelation.direction = DIRECTION.NONE - self.assertTrue(wayRelation.is_prohibited) - - # oneway = yes - wayRelation.direction = DIRECTION.BACKWARD - self.assertTrue(wayRelation.is_prohibited) - - wayRelation.direction = DIRECTION.FORWARD - self.assertFalse(wayRelation.is_prohibited) - - # oneway non existing - wayRelation._one_way = None - self.assertFalse(wayRelation.is_one_way) - - wayRelation.direction = DIRECTION.BACKWARD - self.assertFalse(wayRelation.is_prohibited) - - def test_way_relation_distance_to_way_reflects_internal_value(self): - wayRelation = WayRelation(mockOSMWay_01_02_Loop) - wayRelation._distance_to_way = 10. - self.assertEqual(wayRelation.distance_to_way, 10.) - - def test_way_relation_node_ahead(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - # ahead_ids is None on init - self.assertIsNone(wayRelation.node_ahead) - - wayRelation.ahead_idx = 15 - self.assertEqual(wayRelation.node_ahead, wayRelation.way.nodes[15]) - - def test_way_relation_last_node(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - # direction is NONE on init - self.assertIsNone(wayRelation.last_node) - - # forward - wayRelation.direction = DIRECTION.FORWARD - self.assertEqual(wayRelation.last_node, wayRelation.way.nodes[-1]) - - # backward - wayRelation.direction = DIRECTION.BACKWARD - self.assertEqual(wayRelation.last_node, wayRelation.way.nodes[0]) - - def test_way_relation_last_node_coordinates(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - # direction is NONE on init - self.assertIsNone(wayRelation.last_node_coordinates) - - # forward - wayRelation.direction = DIRECTION.FORWARD - coords = np.radians(np.array([wayRelation.way.nodes[-1].lat, wayRelation.way.nodes[-1].lon], dtype=float)) - assert_array_almost_equal(wayRelation.last_node_coordinates, coords) - - # backward - wayRelation.direction = DIRECTION.BACKWARD - coords = np.radians(np.array([wayRelation.way.nodes[0].lat, wayRelation.way.nodes[0].lon], dtype=float)) - assert_array_almost_equal(wayRelation.last_node_coordinates, coords) - - def test_way_relation_node_before_edge_coordinates(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - - coords = wayRelation.node_before_edge_coordinates(0) - assert_array_almost_equal(coords, np.array([0., 0.])) - - coords = wayRelation.node_before_edge_coordinates(wayRelation.way.nodes[0].id) - coords_e = np.radians(np.array([wayRelation.way.nodes[1].lat, wayRelation.way.nodes[1].lon], dtype=float)) - assert_array_almost_equal(coords, coords_e) - - coords = wayRelation.node_before_edge_coordinates(wayRelation.way.nodes[-1].id) - coords_e = np.radians(np.array([wayRelation.way.nodes[-2].lat, wayRelation.way.nodes[-2].lon], dtype=float)) - assert_array_almost_equal(coords, coords_e) - - def test_way_relation_split_no_matching_node(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - - wrs = wayRelation.split(0) - self.assertEqual(len(wrs), 0) - - def test_way_relation_split_use_correct_ids(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - - wrs = wayRelation.split(wayRelation._nodes_ids[5], [-100, -200]) - self.assertEqual(wrs[0].id, -100) - self.assertEqual(wrs[1].id, -200) - - def test_way_relation_split_on_edge_node(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - edge_node_ids = wayRelation.edge_nodes_ids - - for edge_node_id in edge_node_ids: - wrs = wayRelation.split(edge_node_id) - self.assertEqual(len(wrs), 1) - self.assertEqual(wrs[0], wayRelation) - self.assertEqual(wrs[0].way.tags, wayRelation.way.tags) - - def test_way_relation_split_on_internal_node(self): - wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy) - way_ids = [-10, -20] - - for idx, node_id in enumerate(wayRelation._nodes_ids): - if idx == 0 or idx == len(wayRelation._nodes_ids) - 1: - continue - wrs = wayRelation.split(node_id, way_ids) - self.assertEqual(len(wrs), 2) - assert_array_almost_equal(wrs[0]._nodes_ids, wayRelation._nodes_ids[:idx + 1]) - assert_array_almost_equal(wrs[1]._nodes_ids, wayRelation._nodes_ids[idx:]) - self.assertIn(node_id, wrs[0].edge_nodes_ids) - self.assertIn(node_id, wrs[1].edge_nodes_ids) - self.assertEqual(wrs[0].way.tags, wayRelation.way.tags) - self.assertEqual(wrs[1].way.tags, wayRelation.way.tags) - self.assertEqual(way_ids, [wr.id for wr in wrs]) - - # Helpers - def make_wayRelation_location_dirty(self, wayRelation): - wayRelation.distance_to_node_ahead = 10. - wayRelation.location_rad = 0.8 - wayRelation.bearing_rad = 2. - wayRelation.active = True - wayRelation.diverting = True - wayRelation.ahead_idx = 5 - wayRelation.behind_idx = 4 - wayRelation._active_bearing_delta = 3. - wayRelation._distance_to_way = 20. - - def assert_wayRelation_variables_reset(self, wayRelation): - self.assertEqual(wayRelation.distance_to_node_ahead, 0.) - self.assertIsNone(wayRelation.location_rad) - self.assertIsNone(wayRelation.bearing_rad) - self.assertFalse(wayRelation.active) - self.assertFalse(wayRelation.diverting) - self.assertIsNone(wayRelation.ahead_idx) - self.assertIsNone(wayRelation.behind_idx) - self.assertIsNone(wayRelation._active_bearing_delta) - self.assertIsNone(wayRelation._distance_to_way) - - def wayRelation_mid_point_rad(self, wayRelation): - return np.average(wayRelation.bbox, axis=0) diff --git a/selfdrive/mapd/test/test_WayRelationIndex.py b/selfdrive/mapd/test/test_WayRelationIndex.py deleted file mode 100644 index f8ef212bc3..0000000000 --- a/selfdrive/mapd/test/test_WayRelationIndex.py +++ /dev/null @@ -1,74 +0,0 @@ -import unittest -from selfdrive.mapd.lib.WayRelationIndex import WayRelationIndex -from selfdrive.mapd.test.mock_data import mockWayCollection01 - - -class TestWayRelationIndex(unittest.TestCase): - def test_init_and_add(self): - wrs = mockWayCollection01.way_relations - wr_index = WayRelationIndex(wrs) - - # expected init logic, including add logic. - edge_nodes_index_dict = {} - full_nodes_index_dict = {} - for wr in wrs: - for node in wr.way.nodes: - node_id = node.id - full_nodes_index_dict[node_id] = full_nodes_index_dict.get(node_id, []) + [wr] - if node_id in wr.edge_nodes_ids: - edge_nodes_index_dict[node_id] = edge_nodes_index_dict.get(node_id, []) + [wr] - - # assert logic delivers same result - self.assertDictEqual(edge_nodes_index_dict, wr_index._edge_nodes_index_dict) - self.assertDictEqual(full_nodes_index_dict, wr_index._full_nodes_index_dict) - self.assertEqual(len(wr_index._edge_nodes_index_dict), 586) - self.assertEqual(len(wr_index._full_nodes_index_dict), 2342) - - def test_remove(self): - wrs = mockWayCollection01.way_relations - wr_index = WayRelationIndex(wrs) - - wr_to_remove = wrs[0] - affected_full_node_ids = [nodesData.id for nodesData in wr_to_remove.way.nodes] - affected_edge_node_ids = wr_to_remove.edge_nodes_ids - - initial_full_lists = [wr_index._full_nodes_index_dict[ndid] for ndid in affected_full_node_ids] - initial_edge_lists = [wr_index._edge_nodes_index_dict[ndid] for ndid in affected_edge_node_ids] - - expected_final_full_lists = [[wr for wr in li if wr is not wr_to_remove] for li in initial_full_lists] - expected_final_edge_lists = [[wr for wr in li if wr is not wr_to_remove] for li in initial_edge_lists] - - wr_index.remove(wr_to_remove) - - final_full_lists = [wr_index._full_nodes_index_dict[ndid] for ndid in affected_full_node_ids] - final_edge_lists = [wr_index._edge_nodes_index_dict[ndid] for ndid in affected_edge_node_ids] - - for idx, li in enumerate(final_full_lists): - self.assertListEqual(li, expected_final_full_lists[idx]) - - for idx, li in enumerate(final_edge_lists): - self.assertListEqual(li, expected_final_edge_lists[idx]) - - def test_way_relations_with_edge_node_id(self): - wr_index = WayRelationIndex([]) - ref_dict = { - 0: ["fake_wr1", "fake_wr2"], - 1: ["fake_wr3"], - 3: ["fake_wr4", "fake_wr5", "fake_wr6"], - } - wr_index._edge_nodes_index_dict = ref_dict - - for key, li in ref_dict.items(): - self.assertListEqual(li, wr_index.way_relations_with_edge_node_id(key)) - - def test_way_relations_with_node_id(self): - wr_index = WayRelationIndex([]) - ref_dict = { - 0: ["fake_wr1", "fake_wr2"], - 1: ["fake_wr3"], - 3: ["fake_wr4", "fake_wr5", "fake_wr6"], - } - wr_index._full_nodes_index_dict = ref_dict - - for key, li in ref_dict.items(): - self.assertListEqual(li, wr_index.way_relations_with_node_id(key)) diff --git a/selfdrive/mapd/test/test_geo.py b/selfdrive/mapd/test/test_geo.py deleted file mode 100644 index a18fe717b0..0000000000 --- a/selfdrive/mapd/test/test_geo.py +++ /dev/null @@ -1,234 +0,0 @@ -import unittest -from selfdrive.mapd.lib.geo import vectors, ref_vectors, bearing_to_points, distance_to_points -import numpy as np -from numpy.testing import assert_array_almost_equal -from selfdrive.mapd.test.mock_data import mockNodesData01 - - -class TestMapsdGeoLibrary(unittest.TestCase): - def test_vectors(self): - points = mockNodesData01.radians - expected = np.array([ - [-1.34011951e-05, 1.00776468e-05], - [-5.83610920e-06, 4.41046897e-06], - [-7.83348567e-06, 5.94114032e-06], - [-7.08560788e-06, 5.30408795e-06], - [-6.57632550e-06, 4.05791838e-06], - [-1.16077872e-06, 6.91151252e-07], - [-1.53178098e-05, 9.62215139e-06], - [-5.76314175e-06, 3.55176643e-06], - [-1.61124141e-05, 9.86127759e-06], - [-1.48006628e-05, 8.58192512e-06], - [-1.72237209e-06, 1.60570482e-06], - [-8.68985228e-06, 9.22062311e-06], - [-1.42922812e-06, 1.51494711e-06], - [-3.39761486e-06, 2.57087743e-06], - [-2.75467373e-06, 1.28631255e-06], - [-1.57501989e-05, 5.72309451e-06], - [-2.52143954e-06, 1.34565295e-06], - [-1.65278643e-06, 1.28630942e-06], - [-2.22196114e-05, 1.64360838e-05], - [-5.88675934e-06, 4.08234746e-06], - [-1.83673390e-06, 1.46782408e-06], - [-1.55004206e-06, 1.51843800e-06], - [-1.20451533e-06, 2.06298011e-06], - [-1.91801338e-06, 4.64083285e-06], - [-2.38653483e-06, 5.60076524e-06], - [-1.65269781e-06, 5.78402290e-06], - [-3.66908309e-07, 2.75412965e-06], - [0.00000000e+00, 1.92858882e-06], - [9.09242615e-08, 2.66162711e-06], - [3.14490354e-07, 1.53065382e-06], - [8.66452477e-08, 4.83456208e-07], - [2.41750593e-07, 1.10828411e-06], - [7.43745228e-06, 1.27618831e-05], - [5.59968054e-06, 9.63947367e-06], - [2.01951467e-06, 2.75413219e-06], - [4.59952643e-07, 6.42281301e-07], - [1.74353749e-06, 1.74533121e-06], - [2.57144338e-06, 2.11185266e-06], - [1.46893187e-05, 1.11999169e-05], - [3.84659229e-05, 2.85527952e-05], - [2.71627936e-05, 1.98727946e-05], - [8.44632540e-06, 6.15058628e-06], - [2.29420323e-06, 1.92859222e-06], - [2.58083439e-06, 3.16952222e-06], - [3.76373643e-06, 5.14174911e-06], - [5.32416098e-06, 6.51707770e-06], - [8.62890928e-06, 1.11998258e-05], - [1.25762497e-05, 1.65231340e-05], - [8.90452991e-06, 1.10148240e-05], - [4.86505726e-06, 4.59023120e-06], - [3.85545276e-06, 3.39642031e-06], - [3.48753893e-06, 3.30566145e-06], - [2.99557303e-06, 2.61276368e-06], - [2.15496788e-06, 1.87797727e-06], - [4.10564937e-06, 3.58142649e-06], - [1.53680853e-06, 1.33866906e-06], - [4.99540175e-06, 4.35635790e-06], - [1.37744970e-06, 1.19380643e-06], - [1.74319821e-06, 1.28456429e-06], - [9.99931238e-07, 1.14493663e-06], - [6.42735560e-07, 1.19380547e-06], - [3.66818436e-07, 1.46782199e-06], - [5.45413874e-08, 1.83783170e-06], - [-1.35818548e-07, 1.14842666e-06], - [-5.50758101e-07, 3.02989178e-06], - [-4.58785270e-07, 2.66162724e-06], - [-2.51315555e-07, 1.19031459e-06], - [-3.91409773e-07, 1.65457223e-06], - [-2.14525206e-06, 5.67755902e-06], - [-4.24558096e-07, 1.39102753e-06], - [-1.46936730e-06, 5.32325561e-06], - [-1.37632061e-06, 4.59021715e-06], - [-8.26642899e-07, 4.68097349e-06], - [-6.42702724e-07, 4.95673534e-06], - [-3.66796960e-07, 7.25009780e-06], - [-1.82861669e-07, 8.99542699e-06], - [4.09564134e-07, 6.11214315e-06], - [7.80629912e-08, 1.45734993e-06], - [4.81205526e-07, 7.56076647e-06], - [2.01036346e-07, 2.42775302e-06]]) - - v = vectors(points) - assert_array_almost_equal(v, expected) - - def test_ref_vectors(self): - points = mockNodesData01.radians - expected = np.array([ - [1.59924145e-04, -1.07153714e-04], - [1.46520873e-04, -9.70788297e-05], - [1.40683931e-04, -9.26694631e-05], - [1.32849368e-04, -8.67297434e-05], - [1.25762852e-04, -8.14268689e-05], - [1.19185869e-04, -7.73700167e-05], - [1.18024984e-04, -7.66790438e-05], - [1.02705711e-04, -6.70592230e-05], - [9.69420991e-05, -6.35082196e-05], - [8.08284530e-05, -5.36489556e-05], - [6.60268961e-05, -4.50685727e-05], - [6.43043874e-05, -4.34630144e-05], - [5.56137708e-05, -3.42431117e-05], - [5.41844341e-05, -3.27282671e-05], - [5.07866397e-05, -3.01576270e-05], - [4.80318817e-05, -2.88714948e-05], - [3.22813286e-05, -2.31493755e-05], - [2.97598330e-05, -2.18038275e-05], - [2.81069973e-05, -2.05175815e-05], - [5.88679032e-06, -4.08230278e-06], - [0.00000000e+00, 0.00000000e+00], - [-1.83673390e-06, 1.46782408e-06], - [-3.38677236e-06, 2.98626574e-06], - [-4.59127869e-06, 5.04925111e-06], - [-6.50926460e-06, 9.69009532e-06], - [-8.89575243e-06, 1.52908806e-05], - [-1.05483839e-05, 2.10749224e-05], - [-1.09152548e-05, 2.38290571e-05], - [-1.09152276e-05, 2.57576459e-05], - [-1.08242659e-05, 2.84192717e-05], - [-1.05097542e-05, 2.99499212e-05], - [-1.04231024e-05, 3.04333762e-05], - [-1.01813369e-05, 3.15416571e-05], - [-2.74371711e-06, 4.43034426e-05], - [2.85599752e-06, 5.39428964e-05], - [4.87550206e-06, 5.66970360e-05], - [5.33545066e-06, 5.73393202e-05], - [7.07897615e-06, 5.90846634e-05], - [9.65040026e-06, 6.11965396e-05], - [2.43395796e-05, 7.23966392e-05], - [6.28046063e-05, 1.00950641e-04], - [8.99657904e-05, 1.20825635e-04], - [9.84114021e-05, 1.26977201e-04], - [1.00705361e-04, 1.28906084e-04], - [1.03285783e-04, 1.32075942e-04], - [1.07048835e-04, 1.37218192e-04], - [1.12372096e-04, 1.43736004e-04], - [1.20999382e-04, 1.54937080e-04], - [1.33573053e-04, 1.71462176e-04], - [1.42475686e-04, 1.82478533e-04], - [1.47339899e-04, 1.87069658e-04], - [1.51194707e-04, 1.90466811e-04], - [1.54681601e-04, 1.93773152e-04], - [1.57676653e-04, 1.96386513e-04], - [1.59831239e-04, 1.98264929e-04], - [1.63936150e-04, 2.01847201e-04], - [1.65472675e-04, 2.03186195e-04], - [1.70467147e-04, 2.07543619e-04], - [1.71844334e-04, 2.08737728e-04], - [1.73587247e-04, 2.10022678e-04], - [1.74586922e-04, 2.11167839e-04], - [1.75229389e-04, 2.12361789e-04], - [1.75595876e-04, 2.13829694e-04], - [1.75650001e-04, 2.15667538e-04], - [1.75513922e-04, 2.16815933e-04], - [1.74962478e-04, 2.19845700e-04], - [1.74503092e-04, 2.22507224e-04], - [1.74251509e-04, 2.23697482e-04], - [1.73859727e-04, 2.25351966e-04], - [1.71713202e-04, 2.31029044e-04], - [1.71288336e-04, 2.32419977e-04], - [1.69817793e-04, 2.37742908e-04], - [1.68440467e-04, 2.42332824e-04], - [1.67612807e-04, 2.47013617e-04], - [1.66969033e-04, 2.51970213e-04], - [1.66600674e-04, 2.59220232e-04], - [1.66415880e-04, 2.68215619e-04], - [1.66824132e-04, 2.74327850e-04], - [1.66901881e-04, 2.75785216e-04], - [1.67381459e-04, 2.83346086e-04], - [1.67581971e-04, 2.85773882e-04]]) - - v = ref_vectors(points[20], points) - assert_array_almost_equal(v, expected) - - def test_bearing_to_points(self): - points = mockNodesData01.radians - expected = np.array([ - 2.16112265, 2.15595027, 2.15326799, 2.14916735, 2.14538642, - 2.14657678, 2.14694997, 2.1492257, 2.1507589, 2.15676899, - 2.16973441, 2.1651606, 2.12270237, 2.11416356, 2.10665211, - 2.11201708, 2.19291574, 2.2031069, 2.20136186, 2.17712517, - 0., -0.8965745, -0.84815954, -0.73792895, -0.59150953, - -0.5269061, -0.46406215, -0.42954043, -0.4008254, -0.36391371, - -0.33748609, -0.32996807, -0.31223189, -0.06185112, 0.05289544, - 0.08578116, 0.0927833, 0.11924233, 0.15640718, 0.32432622, - 0.55653415, 0.64003094, 0.6593301, 0.66319086, 0.66367982, - 0.66251077, 0.66354137, 0.66302176, 0.66181884, 0.66291139, - 0.66714676, 0.67095594, 0.67367984, 0.6765003, 0.67847961, - 0.68212344, 0.68345356, 0.68762778, 0.68876073, 0.69070183, - 0.69085143, 0.68988665, 0.68753177, 0.68348884, 0.68051081, - 0.67220053, 0.66506824, 0.66177969, 0.65712162, 0.63916951, - 0.6351146, 0.62025347, 0.60741567, 0.59618923, 0.58521935, - 0.57122582, 0.55532475, 0.54636839, 0.54422542, 0.53357655, - 0.53037033]) - - v = bearing_to_points(points[20], points) - assert_array_almost_equal(v, expected) - - def test_distance_to_points(self): - points = mockNodesData01.radians - expected = np.array([ - 1226.82569068, 1120.13820773, 1073.61121415, 1011.10016574, - 954.81557436, 905.58045038, 896.97734399, 781.7102819, - 738.58271117, 618.26145463, 509.47052142, 494.6403804, - 416.22483123, 403.42108699, 376.42615499, 357.15106681, - 253.15957483, 235.11572972, 221.77439728, 45.65465979, - 0., 14.98414, 28.77606056, 43.49299446, - 74.39463425, 112.74005248, 150.19482607, 167.03665191, - 178.28443483, 193.80834084, 202.28154097, 205.01173833, - 211.22777104, 282.88676739, 344.25957352, 362.66370657, - 367.00206795, 379.23951996, 394.82505328, 486.76073331, - 757.70254732, 960.03439155, 1023.81434529, 1042.49401713, - 1068.53770096, 1109.12696535, 1162.74555108, 1252.847351, - 1385.17179405, 1475.42502599, 1517.57849916, 1549.79838056, - 1580.12405964, 1605.05483058, 1622.98937809, 1657.19268821, - 1669.99157205, 1711.63883132, 1723.09133393, 1736.47655688, - 1746.16073119, 1754.63481838, 1763.34186103, 1772.62691273, - 1777.76189094, 1790.62024447, 1802.11488235, 1807.1040605, - 1813.90756815, 1834.49265566, 1840.00708445, 1861.96087374, - 1880.81678093, 1902.42091191, 1926.37194131, 1963.78301115, - 2011.62679077, 2046.18028824, 2054.37811294, 2097.30347724, - 2111.28586072]) - - v = distance_to_points(points[20], points) - assert_array_almost_equal(v, expected) diff --git a/selfdrive/mapd_manager.py b/selfdrive/mapd_manager.py new file mode 100644 index 0000000000..09b8a97496 --- /dev/null +++ b/selfdrive/mapd_manager.py @@ -0,0 +1,162 @@ +import json +import time +import platform + +import numpy as np + +from openpilot.common.params_pyx import Params +from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert +from openpilot.selfdrive.navd.helpers import Coordinate +from openpilot.selfdrive.sunnypilot.live_map_data import QUERY_RADIUS +from openpilot.common.realtime import Ratekeeper, set_core_affinity +from openpilot.system.swaglog import cloudlog +import os +import glob +import shutil + +from openpilot.selfdrive.sunnypilot.live_map_data.osm_map_data import OsmMapData +from openpilot.selfdrive.sunnypilot.live_map_data import R + +# PFEIFER - MAPD {{ +params = Params() +mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else params +# }} PFEIFER - MAPD + +COMMON_DIR = '/data/media/0/osm' +MAPD_BIN_DIR = '/data/openpilot/third_party/pfeiferj-mapd' +MAPD_PATH = os.path.join(MAPD_BIN_DIR, 'mapd') + + +def get_files_for_cleanup(): + paths = [ + f"{COMMON_DIR}/db", + f"{COMMON_DIR}/v*" + ] + files_to_remove = [] + for path in paths: + if os.path.exists(path): + files = glob.glob(path + '/**', recursive=True) + files_to_remove.extend(files) + # check for version and mapd files + if not os.path.isfile(MAPD_PATH): + files_to_remove.append(MAPD_PATH) + return files_to_remove + + +def cleanup_OLD_OSM_data(files_to_remove): + for file in files_to_remove: + # Remove trailing slash if path is file + if file.endswith('/') and os.path.isfile(file[:-1]): + file = file[:-1] + # Try to remove as file or symbolic link first + if os.path.islink(file) or os.path.isfile(file): + os.remove(file) + elif os.path.isdir(file): # If it's a directory + shutil.rmtree(file, ignore_errors=False) + + +def _get_current_bounding_box(self, radius: float): + self.last_query_radius = radius + # Calculate the bounding box coordinates for the bbox containing the circle around location. + bbox_angle = float(np.degrees(radius / R)) + + lat = float(self.last_gps.latitude) + lon = float(self.last_gps.longitude) + + return { + "min_lat": lat - bbox_angle, + "min_lon": lon - bbox_angle, + "max_lat": lat + bbox_angle, + "max_lon": lon + bbox_angle, + } + + +def _request_refresh_osm_bounds_data(self): + self.last_refresh_loc = Coordinate(self.last_gps.latitude, self.last_gps.longitude) + self.last_query_radius = QUERY_RADIUS + current_bounding_box = self._get_current_bounding_box(self.last_query_radius) + mem_params.put("OSMDownloadBounds", json.dumps(current_bounding_box)) + + +def request_refresh_osm_location_data(nations: [str], states: [str] = None): + params.put("OsmDownloadedDate", str(time.time())) + params.put_bool("OsmDbUpdatesCheck", False) + + osm_download_locations = json.dumps({ + "nations": nations, + "states": states or [] + }) + + print(f"Downloading maps for {osm_download_locations}") + mem_params.put("OSMDownloadLocations", osm_download_locations) + + +def filter_nations_and_states(nations: [str], states: [str] = None): + """Filters and prepares nation and state data for OSM map download. + + If the nation is 'US' and a specific state is provided, the nation 'US' is removed from the list. + If the nation is 'US' and the state is 'All', the 'All' is removed from the list. + The idea behind these filters is that if a specific state in the US is provided, + there's no need to download map data for the entire US. Conversely, + if the state is unspecified (i.e., 'All'), we intend to download map data for the whole US, + and 'All' isn't a valid state name, so it's removed. + + Parameters: + nations (list): A list of nations for which the map data is to be downloaded. + states (list, optional): A list of states for which the map data is to be downloaded. Defaults to None. + + Returns: + tuple: Two lists. The first list is filtered nations and the second list is filtered states. + """ + + if "US" in nations and states and not any(x.lower() == "all" for x in states): + # If a specific state in the US is provided, remove 'US' from nations + nations.remove("US") + elif "US" in nations and states and any(x.lower() == "all" for x in states): + # If 'All' is provided as a state (case invariant), remove those instances from states + states = [x for x in states if x.lower() != "all"] + elif "US" not in nations and states and any(x.lower() == "all" for x in states): + states.remove("All") + return nations, states or [] + + +def update_osm_db(): + # last_downloaded_date = float(params.get('OsmDownloadedDate', encoding='utf-8') or 0.0) + # if params.get_bool("OsmDbUpdatesCheck") or time.time() - last_downloaded_date >= 604800: # 7 days * 24 hours/day * 60 + if params.get_bool("OsmDbUpdatesCheck"): + cleanup_OLD_OSM_data(get_files_for_cleanup()) + country = params.get('OsmLocationName', encoding='utf-8') + state = params.get('OsmStateName', encoding='utf-8') or "All" + filtered_nations, filtered_states = filter_nations_and_states([country], [state]) + request_refresh_osm_location_data(filtered_nations, filtered_states) + + if not mem_params.get("OSMDownloadBounds"): + mem_params.put("OSMDownloadBounds", "") + + if not mem_params.get("LastGPSPosition"): + mem_params.put("LastGPSPosition", "{}") + + +def main_thread(sm=None, pm=None): + try: + set_core_affinity([0, 1, 2, 3]) + except Exception: + cloudlog.exception("mapd: failed to set core affinity") + rk = Ratekeeper(1, print_delay_threshold=None) + live_map_sp = OsmMapData() + + while True: + show_alert = get_files_for_cleanup() and params.get_bool("OsmLocal") + set_offroad_alert("Offroad_OSMUpdateRequired", show_alert, "This alert will be cleared when new maps are downloaded.") + + update_osm_db() + live_map_sp.tick() + rk.keep_time() + + +def main(): + main_thread() + + +if __name__ == "__main__": + main() diff --git a/selfdrive/mapd/test/__init__.py b/selfdrive/sunnypilot/__init__.py similarity index 100% rename from selfdrive/mapd/test/__init__.py rename to selfdrive/sunnypilot/__init__.py diff --git a/selfdrive/sunnypilot/live_map_data/__init__.py b/selfdrive/sunnypilot/live_map_data/__init__.py new file mode 100644 index 0000000000..593d12c65b --- /dev/null +++ b/selfdrive/sunnypilot/live_map_data/__init__.py @@ -0,0 +1,18 @@ +from cereal import custom +from openpilot.system.swaglog import cloudlog + +LOOK_AHEAD_HORIZON_TIME = 15. # s. Time horizon for look ahead of turn speed sections to provide on liveMapDataSP msg. +_DEBUG = False +_CLOUDLOG_DEBUG = False +ROAD_NAME_TIMEOUT = 30 # secs +DataType = custom.LiveMapDataSP.DataType +R = 6373000.0 # approximate radius of earth in mts +QUERY_RADIUS = 3000 # mts. Radius to use on OSM data queries. +QUERY_RADIUS_OFFLINE = 2250 # mts. Radius to use on offline OSM data queries. + + +def get_debug(msg, log_to_cloud=True): + if _CLOUDLOG_DEBUG and log_to_cloud: + cloudlog.debug(msg) + if _DEBUG: + print(msg) diff --git a/selfdrive/sunnypilot/live_map_data/base_map_data.py b/selfdrive/sunnypilot/live_map_data/base_map_data.py new file mode 100644 index 0000000000..ed7f74644c --- /dev/null +++ b/selfdrive/sunnypilot/live_map_data/base_map_data.py @@ -0,0 +1,106 @@ +import math +from abc import abstractmethod, ABC + +from cereal import custom, messaging +from openpilot.selfdrive.navd.helpers import Coordinate +from openpilot.selfdrive.sunnypilot.live_map_data import get_debug + + +class BaseMapData(ABC): + def __init__(self): + self._last_gps: Coordinate | None = None + self._gps_sock = None + self._data_type = custom.LiveMapDataSP.DataType.default + self._sub_master = messaging.SubMaster(['liveLocationKalman', 'carControl']) + self._pub_master = messaging.PubMaster(['liveMapDataSP']) + + @abstractmethod + def update_location(self, current_location: Coordinate): + pass + + @abstractmethod + def get_current_speed_limit(self) -> float: + pass + + @abstractmethod + def get_next_speed_limit_and_distance(self) -> (float, float): + pass + + @abstractmethod + def get_current_road_name(self) -> str: + pass + + def _is_gps_data_valid(self) -> bool: + all_sock_alive = self._sub_master.all_alive(service_list=[self._gps_sock]) + all_sock_valid = self._sub_master.all_valid(service_list=[self._gps_sock]) + return all_sock_alive and all_sock_valid + + def get_current_location(self) -> Coordinate | None: + self._gps_sock = "liveLocationKalman" + if not self._sub_master.updated[self._gps_sock] or not self._sub_master.valid[self._gps_sock]: + return None + + _last_gps = self._sub_master[self._gps_sock] + # ignore the message if the fix is invalid + if not _last_gps.positionGeodetic.valid: + return None + + kalman_bearing_deg = math.degrees(_last_gps.calibratedOrientationNED.value[2]) + kalman_speed = _last_gps.velocityCalibrated.value[2] + kalman_latitude = _last_gps.positionGeodetic.value[0]; + kalman_longitude = _last_gps.positionGeodetic.value[1] + + result = Coordinate(kalman_latitude, kalman_longitude) + result.annotations['unixTimestampMillis'] = _last_gps.unixTimestampMillis + result.annotations['speed'] = kalman_speed + result.annotations['bearingDeg'] = kalman_bearing_deg + result.annotations['accuracy'] = 1 # Hardcoded since liveLocationKalman does not report this. + result.annotations['bearingAccuracyDeg'] = 1. # you'll need to assign this if available + + return result + + def get_live_map_data_sp(self, speed_limit, next_speed_limit, next_speed_limit_distance, current_road_name): + last_gps = self.get_current_location() + map_data_msg = messaging.new_message('liveMapDataSP') + map_data_msg.valid = self._is_gps_data_valid() + + live_map_data = map_data_msg.liveMapDataSP + if last_gps: + live_map_data.lastGpsTimestamp = last_gps.annotations.get('unixTimestampMillis', 0) + live_map_data.lastGpsLatitude = last_gps.latitude + live_map_data.lastGpsLongitude = last_gps.longitude + live_map_data.lastGpsSpeed = last_gps.annotations.get('speed', 0) + live_map_data.lastGpsBearingDeg = last_gps.annotations.get('bearingDeg', 0) + live_map_data.lastGpsAccuracy = last_gps.annotations.get('accuracy', 0) + live_map_data.lastGpsBearingAccuracyDeg = last_gps.annotations.get('bearingAccuracyDeg', 0) + + live_map_data.speedLimitValid = bool(speed_limit > 0) + live_map_data.speedLimit = speed_limit + live_map_data.speedLimitAheadValid = bool(next_speed_limit > 0) + live_map_data.speedLimitAhead = float(next_speed_limit) + live_map_data.speedLimitAheadDistance = float(next_speed_limit_distance) + live_map_data.currentRoadName = str(current_road_name) + live_map_data.dataType = self._data_type + + return map_data_msg + + def publish(self): + speed_limit = self.get_current_speed_limit() + current_road_name = self.get_current_road_name() + next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance() + + live_map_data_sp = self.get_live_map_data_sp( + speed_limit, + next_speed_limit, + next_speed_limit_distance, + current_road_name + ) + + self._pub_master.send('liveMapDataSP', live_map_data_sp) + get_debug(f"SRC: [{self.__class__.__name__}] | SLC: [{speed_limit}] | NSL: [{next_speed_limit}] | NSLD: [{next_speed_limit_distance}] | CRN: [{current_road_name}] | GPS: [{self._last_gps}] Annotations: [{', '.join(f'{key}: {value}' for key, value in self._last_gps.annotations.items()) if self._last_gps else []}]") + + def tick(self): + self._sub_master.update() + self._last_gps = self.get_current_location() + self.update_location(self._last_gps) + self.publish() diff --git a/selfdrive/sunnypilot/live_map_data/debug.py b/selfdrive/sunnypilot/live_map_data/debug.py new file mode 100644 index 0000000000..035f21077a --- /dev/null +++ b/selfdrive/sunnypilot/live_map_data/debug.py @@ -0,0 +1,50 @@ +# DISCLAIMER: This code is intended principally for development and debugging purposes. +# Although it provides a standalone entry point to the program, users should refer +# to the actual implementations for consumption. Usage outside of development scenarios +# is not advised and could lead to unpredictable results. + +import threading +import traceback + +from cereal import messaging +from openpilot.common.realtime import set_core_affinity +from openpilot.selfdrive.controls.lib.sunnypilot.common import Policy +from openpilot.selfdrive.controls.lib.sunnypilot.speed_limit_resolver import SpeedLimitResolver +from openpilot.selfdrive.sunnypilot.live_map_data import get_debug +from openpilot.system.swaglog import cloudlog + + +def excepthook(args): + get_debug(f'MapD: Threading exception:\n{args}') + traceback.print_exception(args.exc_type, args.exc_value, args.exc_traceback) + + +def live_map_data_sp_thread(): + try: + set_core_affinity([0, 1, 2, 3]) + except Exception: + cloudlog.exception("mapd: failed to set core affinity") + + while True: + live_map_data_sp_thread_debug() + + +def live_map_data_sp_thread_debug(): + _sub_master = messaging.SubMaster(['carState', 'navInstruction', 'liveLocationKalman', 'liveMapDataSP', 'longitudinalPlanSP']) + _sub_master.update() + + v_ego = _sub_master['carState'].vEgo + long_spl = _sub_master['longitudinalPlanSP'].speedLimit + _policy = Policy.car_state_priority + _resolver = SpeedLimitResolver(_policy) + _speed_limit, _distance, _source = _resolver.resolve(v_ego, long_spl, _sub_master) + print(_speed_limit, _distance, _source, " <-> ", long_spl) + + +def main(): + threading.excepthook = excepthook + live_map_data_sp_thread() + + +if __name__ == "__main__": + main() diff --git a/selfdrive/sunnypilot/live_map_data/osm_map_data.py b/selfdrive/sunnypilot/live_map_data/osm_map_data.py new file mode 100644 index 0000000000..0f39b2287f --- /dev/null +++ b/selfdrive/sunnypilot/live_map_data/osm_map_data.py @@ -0,0 +1,50 @@ +import json +import platform + +from openpilot.common.params_pyx import Params +from openpilot.selfdrive.navd.helpers import Coordinate +from openpilot.selfdrive.sunnypilot.live_map_data import DataType +from openpilot.selfdrive.sunnypilot.live_map_data.base_map_data import BaseMapData + + +class OsmMapData(BaseMapData): + def __init__(self): + super().__init__() + self.last_gps = Coordinate(0.0, 0.0) + self.params = Params() + self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params + self.data_type = DataType.offline + + def update_location(self, current_location): + self.last_gps = current_location + if not self.last_gps: + return + + last_gps_position_for_osm = { + "latitude": self.last_gps.latitude, + "longitude": self.last_gps.longitude, + "bearing": self.last_gps.annotations.get("bearingDeg", 0) + } + self.mem_params.put("LastGPSPosition", json.dumps(last_gps_position_for_osm)) + + def get_current_speed_limit(self): + speed_limit = self.mem_params.get("MapSpeedLimit", encoding='utf8') + return float(speed_limit) if speed_limit else 0.0 + + def get_current_road_name(self): + current_road_name = self.mem_params.get("RoadName", encoding='utf8') + return current_road_name if current_road_name else "" + + def get_next_speed_limit_and_distance(self): + next_speed_limit_section_str = self.mem_params.get("NextMapSpeedLimit", encoding='utf8') + next_speed_limit_section = json.loads(next_speed_limit_section_str) if next_speed_limit_section_str else {} + next_speed_limit = next_speed_limit_section.get('speedlimit', 0.0) + next_speed_limit_latitude = next_speed_limit_section.get('latitude') + next_speed_limit_longitude = next_speed_limit_section.get('longitude') + next_speed_limit_distance = 0 + + if next_speed_limit_latitude and next_speed_limit_longitude: + next_speed_limit_coordinates = Coordinate(next_speed_limit_latitude, next_speed_limit_longitude) + next_speed_limit_distance = (self.last_gps or Coordinate(0, 0)).distance_to(next_speed_limit_coordinates) + + return next_speed_limit, next_speed_limit_distance diff --git a/selfdrive/sunnypilot/live_map_data/standalone.py b/selfdrive/sunnypilot/live_map_data/standalone.py new file mode 100644 index 0000000000..a65e3e373a --- /dev/null +++ b/selfdrive/sunnypilot/live_map_data/standalone.py @@ -0,0 +1,40 @@ +# DISCLAIMER: This code is intended principally for development and debugging purposes. +# Although it provides a standalone entry point to the program, users should refer +# to the actual implementations for consumption. Usage outside of development scenarios +# is not advised and could lead to unpredictable results. + +import threading +import traceback + +from openpilot.common.realtime import Ratekeeper, set_core_affinity +from openpilot.selfdrive.sunnypilot.live_map_data import get_debug +from openpilot.selfdrive.sunnypilot.live_map_data.osm_map_data import OsmMapData +from openpilot.system.swaglog import cloudlog + + +def excepthook(args): + get_debug(f'MapD: Threading exception:\n{args}') + traceback.print_exception(args.exc_type, args.exc_value, args.exc_traceback) + + +def live_map_data_sp_thread(): + try: + set_core_affinity([0, 1, 2, 3]) + except Exception: + cloudlog.exception("mapd: failed to set core affinity") + + live_map_sp = OsmMapData() + rk = Ratekeeper(1, print_delay_threshold=None) + + while True: + live_map_sp.tick() + rk.keep_time() + + +def main(): + threading.excepthook = excepthook + live_map_data_sp_thread() + + +if __name__ == "__main__": + main() diff --git a/selfdrive/ui/qt/offroad/sunnypilot/locations_fetcher.h b/selfdrive/ui/qt/offroad/sunnypilot/locations_fetcher.h new file mode 100644 index 0000000000..1e5b20139f --- /dev/null +++ b/selfdrive/ui/qt/offroad/sunnypilot/locations_fetcher.h @@ -0,0 +1,52 @@ +#pragma once + +#include +#include // for std::sort +#include +#include + +#include "selfdrive/ui/qt/offroad/sunnypilot/json_fetcher.h" + +static const std::tuple defaultLocation = std::make_tuple("== None ==", ""); +// New class LocationsFetcher that handles web requests and JSON parsing +class LocationsFetcher { +public: + inline std::vector> + getLocationsFromURL(const QUrl &url, const std::tuple &customLocation = defaultLocation) const { + // Initialize an empty vector to hold the locations + std::vector> locations; + + JsonFetcher fetcher; + QJsonObject json = fetcher.getJsonFromURL(url.toString()); + + for (auto it = json.begin(); it != json.end(); ++it) { + QString code = it.key(); + QJsonObject obj = it.value().toObject(); + QString fullName = obj["full_name"].toString(); + + locations.push_back(std::make_tuple(fullName, code, QString(), QString())); + } + // Sort locations by full name + std::sort(locations.begin(), locations.end(), [](const auto &lhs, const auto &rhs) { + return std::get < 0 > (lhs) < std::get < 0 > (rhs); // Compare full names + }); + // Optionally, you can now add defaultName entry at the beginning + locations.insert(locations.begin(), std::tuple_cat(customLocation, std::make_tuple("", ""))); + return locations; + } + + inline std::vector> + getLocationsFromURL(const QString &url, const std::tuple &customLocation = defaultLocation) const { + return getLocationsFromURL(QUrl(url), customLocation); + } + + inline std::vector> + getOsmLocations(const std::tuple &customLocation = defaultLocation) const { + return getLocationsFromURL("https://raw.githubusercontent.com/pfeiferj/openpilot-mapd/main/nation_bounding_boxes.json", customLocation); + } + + inline std::vector> + getUsStatesLocations(const std::tuple &customLocation = defaultLocation) const { + return getLocationsFromURL("https://raw.githubusercontent.com/pfeiferj/openpilot-mapd/main/us_states_bounding_boxes.json", customLocation); + } +}; diff --git a/selfdrive/ui/qt/offroad/sunnypilot/osm_settings.cc b/selfdrive/ui/qt/offroad/sunnypilot/osm_settings.cc index e7d6112ab9..958d338138 100644 --- a/selfdrive/ui/qt/offroad/sunnypilot/osm_settings.cc +++ b/selfdrive/ui/qt/offroad/sunnypilot/osm_settings.cc @@ -3,107 +3,266 @@ OsmPanel::OsmPanel(QWidget *parent) : QFrame(parent) { main_layout = new QStackedLayout(this); - ListWidget *list = new ListWidget(this, false); - // param, title, desc, icon - std::vector> toggle_defs{ - { - "OsmLocalDb", - tr("Use Offline Database"), - "", - "../assets/offroad/icon_blank.png", - } - }; - - for (auto &[param, title, desc, icon] : toggle_defs) { - auto toggle = new ParamControl(param, title, desc, icon, this); - - list->addItem(toggle); - toggles[param.toStdString()] = toggle; - } - - osmUpdateLbl = new QLabel(tr("Database updates can be downloaded while the car is off.")); - osmUpdateLbl->setStyleSheet("font-size: 50px; font-weight: 400; text-align: left; padding-top: 30px; padding-bottom: 30px;"); - list->addItem(osmUpdateLbl); - - osmUpdateBtn = new ButtonControl(tr("OpenStreetMap Database Update"), tr("CHECK")); - connect(osmUpdateBtn, &ButtonControl::clicked, [=]() { - if (ConfirmationDialog::confirm(tr("Are you sure you want to reboot and perform a database update?\nEstimated time: 30-90 minutes"), tr("Reboot"), parent)) { - params.putBool("OsmDbUpdatesCheck", true); - Hardware::reboot(); - } - }); - list->addItem(osmUpdateBtn); - - osmDownloadBtn = new ButtonControl(tr("OpenStreetMap Database"), tr("SELECT")); - connect(osmDownloadBtn, &ButtonControl::clicked, [=]() { - std::vector> locations = getOsmLocations(); - QString initTitle = QString::fromStdString(params.get("OsmLocationTitle")); - QString currentTitle = ((initTitle == "== None ==") || (initTitle.length() == 0)) ? "== None ==" : initTitle; - - QStringList locationTitles; - for (auto& loc : locations) { - locationTitles.push_back(std::get<0>(loc)); - } - - QString selection = MultiOptionDialog::getSelection(tr("Select your location"), locationTitles, currentTitle, this); - if (!selection.isEmpty()) { - params.put("OsmLocal", "1"); - params.put("OsmLocationTitle", selection.toStdString()); - for (auto& loc : locations) { - if (std::get<0>(loc) == selection) { - params.put("OsmLocationName", std::get<1>(loc).toStdString()); - params.put("OsmWayTest", std::get<2>(loc).toStdString()); - params.put("OsmLocationUrl", std::get<3>(loc).toStdString()); - break; - } - } - osmDownloadBtn->setValue(selection); - if (selection != "== None ==") { - if (ConfirmationDialog::confirm(tr("Are you sure you want to reboot to start downloading the selected database? Estimated time: 30-90 minutes"), tr("Reboot"), parent)) { - params.putBool("OsmDbUpdatesCheck", true); - Hardware::reboot(); - } - } - } - updateLabels(); - }); - list->addItem(osmDownloadBtn); + const auto list = new ListWidget(this, false); + list->addItem(mapdVersion = new LabelControl(tr("Mapd Version"), "Loading...")); + list->addItem(setupOsmDeleteMapsButton(parent)); + list->addItem(offlineMapsETA = new LabelControl(tr("Offline Maps ETA"), "")); + list->addItem(offlineMapsElapsed = new LabelControl(tr("Time Elapsed"), "")); + list->addItem(setupOsmUpdateButton(parent)); + list->addItem(setupOsmDownloadButton(parent)); + list->addItem(setupUsStatesButton(parent)); connect(uiState(), &UIState::offroadTransition, [=](bool offroad) { - is_onroad = !offroad; updateLabels(); }); + timer = new QTimer(this); + connect(timer, &QTimer::timeout, this, QOverload<>::of(&OsmPanel::updateLabels)); + timer->start(FAST_REFRESH_INTERVAL); // Time specified in milliseconds. updateLabels(); osmScreen = new QWidget(this); - QVBoxLayout* vlayout = new QVBoxLayout(osmScreen); + auto *vlayout = new QVBoxLayout(osmScreen); vlayout->setContentsMargins(50, 20, 50, 20); - vlayout->addWidget(new ScrollView(list, this), 1); main_layout->addWidget(osmScreen); } -void OsmPanel::showEvent(QShowEvent *event) { - updateLabels(); +ButtonControl *OsmPanel::setupOsmDeleteMapsButton(QWidget *parent) { + osmDeleteMapsBtn = new ButtonControl(tr("Downloaded Maps"), tr("Delete Maps")); // Updated on updateLabels() + connect(osmDeleteMapsBtn, &ButtonControl::clicked, [=]() { + if (showConfirmationDialog(parent, "This will delete ALL downloaded maps\n\nAre you sure you want to delete all the maps?", "Yes, delete all the maps.")) { + QtConcurrent::run([=]() { + QDir dir(MAP_PATH); + osmDeleteMapsBtn->setEnabled(false); + osmDeleteMapsBtn->setText("Deleting..."); + dir.removeRecursively(); + updateMapSize(); + osmDeleteMapsBtn->setEnabled(true); + osmDeleteMapsBtn->setText("DELETE"); + }); + updateLabels(); + } + }); + return osmDeleteMapsBtn; } +ButtonControl *OsmPanel::setupOsmUpdateButton(QWidget *parent) { + osmUpdateBtn = new ButtonControl(tr("Database Update"), tr("CHECK")); // Updated on updateLabels() + connect(osmUpdateBtn, &ButtonControl::clicked, [=]() { + if (osm_download_in_progress && !download_failed_state) { + updateLabels(); + } else if (showConfirmationDialog(parent)) { + osm_download_in_progress = true; + params.putBool("OsmDbUpdatesCheck", true); + updateLabels(); + } + }); + return osmUpdateBtn; +} + +ButtonControl *OsmPanel::setupOsmDownloadButton(QWidget *parent) { + osmDownloadBtn = new ButtonControl(tr("Country"), tr("SELECT")); + connect(osmDownloadBtn, &ButtonControl::clicked, [=]() { + osmDownloadBtn->setEnabled(false); + osmDownloadBtn->setValue("Fetching Country list..."); + const std::vector> locations = getOsmLocations(); + osmDownloadBtn->setEnabled(true); + osmDownloadBtn->setValue(""); + const QString initTitle = QString::fromStdString(params.get("OsmLocationTitle")); + const QString currentTitle = ((initTitle == "== None ==") || (initTitle.length() == 0)) ? "== None ==" : initTitle; + + QStringList locationTitles; + for (auto &loc: locations) { + locationTitles.push_back(std::get<0>(loc)); + } + + const QString selection = MultiOptionDialog::getSelection(tr("Country"), locationTitles, currentTitle, this); + if (!selection.isEmpty()) { + params.put("OsmLocal", "1"); + params.put("OsmLocationTitle", selection.toStdString()); + for (auto &loc: locations) { + if (std::get<0>(loc) == selection) { + params.put("OsmLocationName", std::get<1>(loc).toStdString()); + break; + } + } + if (params.get("OsmLocationName") == "US") { + usStatesBtn->click(); + return; + } else if (selection != "== None ==") { + if (showConfirmationDialog(parent)) { + osm_download_in_progress = true; + params.putBool("OsmDbUpdatesCheck", true); + updateLabels(); + } + } + } + updateLabels(); + }); + return osmDownloadBtn; +} + +ButtonControl *OsmPanel::setupUsStatesButton(QWidget *parent) { + usStatesBtn = new ButtonControl(tr("State"), tr("SELECT")); + connect(usStatesBtn, &ButtonControl::clicked, [=]() { + const std::tuple allStatesOption = std::make_tuple("All States (~4.8 GB)", "All"); + usStatesBtn->setEnabled(false); + usStatesBtn->setValue("Fetching State list..."); + const std::vector> locations = getUsStatesLocations(allStatesOption); + usStatesBtn->setEnabled(true); + usStatesBtn->setValue(""); + const QString initTitle = QString::fromStdString(params.get("OsmStateTitle")); + const QString currentTitle = ((initTitle == std::get<0>(allStatesOption)) || (initTitle.length() == 0)) ? "All" : initTitle; + + QStringList locationTitles; + for (auto &loc: locations) { + locationTitles.push_back(std::get<0>(loc)); + } + + const QString selection = MultiOptionDialog::getSelection(tr("State"), locationTitles, currentTitle, this); + if (!selection.isEmpty()) { + params.put("OsmStateTitle", selection.toStdString()); + for (auto &loc: locations) { + if (std::get<0>(loc) == selection) { + params.put("OsmStateName", std::get<1>(loc).toStdString()); + break; + } + } + usStatesBtn->setValue(selection); + if (showConfirmationDialog(parent)) { + osm_download_in_progress = true; + params.putBool("OsmDbUpdatesCheck", true); + updateLabels(); + } + } + updateLabels(); + }); + usStatesBtn->setVisible(false); // initially hidden + return usStatesBtn; +} + +void OsmPanel::showEvent(QShowEvent *event) { + updateLabels(); // For snappier feeling + if (!timer->isActive()) { + timer->start(FAST_REFRESH_INTERVAL); + } +} + +void OsmPanel::hideEvent(QHideEvent *event) { + if (timer->isActive()) { + timer->stop(); + } +} + + void OsmPanel::updateLabels() { if (!isVisible()) { return; } + mapd_version = params.get("MapdVersion"); + mapdVersion->setText(mapd_version.c_str()); - QString name = QString::fromStdString(params.get("OsmLocationName")); - if (!name.isEmpty()) { - osmUpdateBtn->setVisible(!is_onroad); + updateMapSize(); + osm_download_locations = mem_params.get("OSMDownloadLocations"); + osm_download_in_progress = !osm_download_locations.empty(); + + timer->setInterval(osm_download_in_progress ? FAST_REFRESH_INTERVAL : SLOW_REFRESH_INTERVAL); + LOGT("Timer Interval %d", timer->interval()); + + const std::string osmLastDownloadTimeStr = params.get("OsmDownloadedDate"); + if (!lastDownloadedTimePoint.has_value() && !osmLastDownloadTimeStr.empty()) { + const double osmLastDownloadTime = std::stod(osmLastDownloadTimeStr); + lastDownloadedTimePoint = std::chrono::system_clock::from_time_t(static_cast(osmLastDownloadTime)); + } + + osmDownloadBtn->setEnabled(!osm_download_in_progress); + usStatesBtn->setEnabled(!osm_download_in_progress); + + updateDownloadProgress(); + + const QString locationName = QString::fromStdString(params.get("OsmLocationName")); + const bool isUs = !locationName.isEmpty() && locationName == "US"; + usStatesBtn->setVisible(isUs); + + if (!locationName.isEmpty()) { + if (!isUs) { + params.remove("OsmStateName"); + params.remove("OsmStateTitle"); + } + osmUpdateBtn->setVisible(true); } else { params.remove("OsmLocal"); + params.remove("OsmLocationName"); + params.remove("OsmLocationTitle"); + params.remove("OsmStateName"); + params.remove("OsmStateTitle"); osmUpdateBtn->setVisible(false); + usStatesBtn->setVisible(false); } - osmUpdateLbl->setVisible(is_onroad); - osmDownloadBtn->setEnabled(!is_onroad); osmDownloadBtn->setValue(QString::fromStdString(params.get("OsmLocationTitle"))); - + usStatesBtn->setValue(QString::fromStdString(params.get("OsmStateTitle"))); update(); } + +void OsmPanel::updateDownloadProgress() { + const auto pending_update_check = params.getBool("OsmDbUpdatesCheck"); + const QJsonObject osmDownloadProgress = QJsonDocument::fromJson(params.get("OSMDownloadProgress").c_str()).object(); + if (osm_download_in_progress && lastDownloadedTimePoint.has_value()) { + offlineMapsETA->setVisible(true); + offlineMapsElapsed->setVisible(true); + offlineMapsETA->setText(calculateETA(osmDownloadProgress, lastDownloadedTimePoint.value())); + offlineMapsElapsed->setText(calculateElapsedTime(osmDownloadProgress, lastDownloadedTimePoint.value())); + } else { + offlineMapsETA->setVisible(false); + offlineMapsElapsed->setVisible(false); + } + + const int total_files = extractIntFromJson(osmDownloadProgress, "total_files"); + const int downloaded_files = extractIntFromJson(osmDownloadProgress, "downloaded_files"); + download_failed_state = total_files && osm_download_in_progress && !lastDownloadedTimePoint.has_value() && downloaded_files < total_files; + + const auto updateButtonText = processUpdateStatus(pending_update_check, total_files, downloaded_files, osmDownloadProgress, download_failed_state); + + osmUpdateBtn->setValue(tr(updateButtonText.c_str())); + osmUpdateBtn->setText(tr(osm_download_in_progress && !download_failed_state ? "Check status" : "Force Update")); + osmDeleteMapsBtn->setValue(formatSize(mapsDirSize)); +} + +int OsmPanel::extractIntFromJson(const QJsonObject& json, const QString& key) { + return (json.contains(key)) ? json[key].toInt() : 0; +} + +std::string OsmPanel::processUpdateStatus(bool pending_update, int total_files, int downloaded_files, const QJsonObject& json, bool failed_state) { + if (pending_update && !osm_download_in_progress && !total_files) { + lastDownloadedTimePoint.reset(); + return "Download starting..."; + } else if (failed_state) { + return "Error: Invalid download. Retry."; + } else if (osm_download_in_progress && total_files > downloaded_files) { + return formatDownloadStatus(json).toStdString(); + } else if (osm_download_in_progress && downloaded_files >= total_files) { + osm_download_in_progress = false; + lastDownloadedTimePoint.reset(); + return "Download complete!"; + } + + if (lastDownloadedTimePoint.has_value()) { + QDateTime dateTime = QDateTime::fromTime_t(std::chrono::system_clock::to_time_t(lastDownloadedTimePoint.value())); //fromMSecsSinceEpoch(duration); + dateTime = dateTime.toLocalTime(); + return QString("%1").arg(dateTime.toString("yyyy-MM-dd HH:mm:ss")).toStdString(); + } + + return ""; +} + +void OsmPanel::updateMapSize() { + if (mapSizeFuture.has_value() && mapSizeFuture.value().isFinished()) { + mapsDirSize = mapSizeFuture.value().result(); + } + + if (!mapSizeFuture.has_value() || !mapSizeFuture.value().isRunning()) { + mapSizeFuture = QtConcurrent::run(getDirSize, MAP_PATH); + } +} diff --git a/selfdrive/ui/qt/offroad/sunnypilot/osm_settings.h b/selfdrive/ui/qt/offroad/sunnypilot/osm_settings.h index 29af9f492e..199b224b17 100644 --- a/selfdrive/ui/qt/offroad/sunnypilot/osm_settings.h +++ b/selfdrive/ui/qt/offroad/sunnypilot/osm_settings.h @@ -1,9 +1,25 @@ #pragma once -#include "selfdrive/ui/ui.h" -#include "selfdrive/ui/qt/widgets/controls.h" -#include "selfdrive/ui/qt/widgets/scrollview.h" +#include +#include +#include +#include +#include +#include +#include "common/swaglog.h" +#include "selfdrive/ui/qt/network/wifi_manager.h" +#include "selfdrive/ui/qt/widgets/controls.h" +#include "selfdrive/ui/qt/offroad/sunnypilot/locations_fetcher.h" +#include "selfdrive/ui/qt/widgets/scrollview.h" +#include "selfdrive/ui/qt/util.h" +#include "selfdrive/ui/ui.h" +#include "system/hardware/hw.h" + +constexpr int FAST_REFRESH_INTERVAL = 1000; // ms +constexpr int SLOW_REFRESH_INTERVAL = 5000; // ms + +static const QString MAP_PATH = Hardware::PC() ? QDir::homePath() + "/.comma/media/0/osm/offline/" : "/data/media/0/osm/offline/"; class OsmPanel : public QFrame { Q_OBJECT @@ -14,129 +30,190 @@ private: QStackedLayout* main_layout = nullptr; QWidget* osmScreen = nullptr; Params params; + Params mem_params{ Hardware::PC() ? "": "/dev/shm/params"}; std::map toggles; + std::optional> mapSizeFuture; + const SubMaster &sm = *uiState()->sm; - void showEvent(QShowEvent *event) override; - void updateLabels(); bool is_onroad = false; + std::string mapd_version; + bool isWifi() const {return sm["deviceState"].getDeviceState().getNetworkType() == cereal::DeviceState::NetworkType::WIFI; } + bool isMetered() const {return sm["deviceState"].getDeviceState().getNetworkMetered(); } + bool osm_download_in_progress = false; + bool download_failed_state = false; + quint64 mapsDirSize = 0; QLabel *osmUpdateLbl; ButtonControl *osmDownloadBtn; ButtonControl *osmUpdateBtn; + ButtonControl *usStatesBtn; + ButtonControl *osmDeleteMapsBtn; + ButtonControl *setupOsmDeleteMapsButton(QWidget *parent);; + ButtonControl* setupOsmUpdateButton(QWidget *parent); + ButtonControl* setupOsmDownloadButton(QWidget *parent); + ButtonControl* setupUsStatesButton(QWidget *parent); + QTimer *timer; + std::string osm_download_locations; +// void updateButtonControl(ButtonControl *btnControl, QWidget *parent, const QString &initTitle, const QString &allStatesOption); - inline std::vector> getOsmLocations() { - // location_title, location_name, waypoint, url - std::vector> locations{ - { - "== None ==", - "", - "", - "", - }, - { - "Australia", - "australia", - "514911884", - "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EathcAhPP8dHiTdBiFPIGYoB7zwlUR79OkSTH5dnS-9Shg?e=9cS6er&download=1", - }, - { - "Brazil", - "brazil", - "124199196", - "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EXWcrd8ukahDtoFDXVOxvkYBjGGa9b29fVvRdJlADv3YdA?e=jwabo2&download=1", - }, - { - "Canada", - "canada", - "68588664", - "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/ETpwTMZftJpCkep2goPmzUQBl5SW_YfHBKLTd7w5CUagkA?e=dshC10&download=1", - }, - { - "GCC States", - "gcc-states", - "69021390", - "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EWtFOVR9mj5CoRpATYR0_tUBWNEcy5jeasp8zO0ZYdduRg?e=Cg0l2Z&download=1", - }, - { - "Germany", - "germany", - "461526153", - "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EbNzQuA8jHBCuGch1WY4cuoBJfK1S6m5odj5qMnOu3HA3Q?e=cNDn30&download=1", - }, - { - "Malaysia, Singapore, Brunei", - "malaysia-singapore-brunei", - "1112741782", - "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EQb9bq_YkE1NuFx0jp-52zoBnAbdo4nyLXMnEY1pUbCB8g?e=CoFiUJ&download=1", - }, - { - "New Zealand", - "new-zealand", - "154430132", - "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EVI6eMuNa4dJpXg-r92Vw1EBq5RxR83_7Z5C56AYftg8xQ?e=xouRBl&download=1", - }, - { - "South Africa", - "south-africa", - "2729449", - "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EQzbZeus-yBNsgP1KQyLn9wBYZVMMknVoHdB7_ewulLbdA?e=DrnKkb&download=1", - }, - { - "Spain", - "spain", - "4263034", - "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/Ee21EfLqZxxLjksDCB_2dscBXKN9Kch8ZOYz3cIWnVpBTg?e=LWBJ1n&download=1", - }, - { - "Taiwan", - "taiwan", - "198637969", - "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EXgP3Z5_lsBPiQe-aXOOSY4Bu-r54Z_3kKKmT8-IqLO42A?e=tOhUYw&download=1", - }, - { - "Turkey", - "turkey", - "698359658", - "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EWJRrYpV7I1IvOW0LUWrDsUBjEjuKRTla0hA4yRbsVX2xw?e=RGcu8n&download=1", - }, - { - "US - Florida", - "florida", - "147221754", - "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/ET9rAvCaretAnO4TjocAddkBFxAziblxblAn9YWzGk5hTw?e=tts5Oz&download=1", - }, - { - "US - Midwest", - "us-midwest", - "1059596607", - "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EaDYnugxokRHhOukMndJuZMBKz-kzPfM6C_-iQcki-r94Q?e=oUX6hz&download=1", - }, - { - "US - Northeast", - "us-northeast", - "575213527", - "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/ERrlIKNuT4ZIuUOfr4a3BwABLfd7X1wSq9nUrk9buKrIgA?e=HHIT8R&download=1", - }, - { - "US - Pacific", - "us-pacific", - "112909709", - "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/EQOzoMksk9tAluahQ2qm3rQBHOZD03qFU8Aw4o-TFUTuJA?e=CcBOxL&download=1", - }, - { - "US - South", - "us-south", - "243729876", - "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/ETizacxh8lFNk8NMVRyKCEsBMMYZl1iq0r-D4IWoOAd_DQ?e=EVIElm&download=1", - }, - { - "US - West", - "us-west", - "30023440", - "https://thecorgirosie-my.sharepoint.com/:u:/g/personal/databases_sunnypilot_com/ETTRiSKwlEVKoRHtylruy7wBKgITKXdaCUAkJMuGAN2_mA?e=7jlglL&download=1", - }, - }; - return locations; + void showEvent(QShowEvent *event) override; + void hideEvent(QHideEvent* event) override; + void updateLabels(); + void updateDownloadProgress(); + static int extractIntFromJson(const QJsonObject& json, const QString& key); + std::string processUpdateStatus(bool pending_update_check, int total_files, int downloaded_files, const QJsonObject& json, bool failed_state); + + ConfirmationDialog* confirmationDialog; + LabelControl *mapdVersion; + LabelControl *offlineMapsStatus; + LabelControl *offlineMapsETA; + LabelControl *offlineMapsElapsed; + std::optional lastDownloadedTimePoint; + LocationsFetcher locationsFetcher; + void updateMapSize(); + + + bool showConfirmationDialog(QWidget *parent, + const QString &message = QString(), + const QString &confirmButtonText = QString()) const { + const auto _is_metered = isMetered(); + const QString warning_message = _is_metered ? tr("\n\nWarning: You are on a metered connection!") : QString(); + QString final_message = message.isEmpty() ? tr("This will start the download process and it might take a while to complete.") : message; + final_message += warning_message; // Append the warning message if the connection is metered + + const QString final_buttonText = confirmButtonText.isEmpty() ? (_is_metered ? tr("Continue on Metered") : tr("Start Download")) : confirmButtonText; + + return ConfirmationDialog::confirm(final_message, final_buttonText, parent); + } + + // Refactored methods + std::vector> getOsmLocations(const std::tuple& customLocation = defaultLocation) const { + return locationsFetcher.getOsmLocations(customLocation); + } + std::vector> getUsStatesLocations(const std::tuple& customLocation = defaultLocation) const { + return locationsFetcher.getUsStatesLocations(customLocation); + } + + static QString formatTime(const long timeInSeconds) { + const long minutes = timeInSeconds / 60; + const long seconds = timeInSeconds % 60; + + QString formattedTime; + if (minutes > 0) { + formattedTime = QString::number(minutes) + "m "; + } + formattedTime += QString::number(seconds) + "s"; + return formattedTime; + } + + static QString calculateElapsedTime(const QJsonObject &jsonData, const std::chrono::system_clock::time_point &startTime) { + using namespace std::chrono; + if (!jsonData.contains("total_files") || !jsonData.contains("downloaded_files")) + return "Calculating..."; + + const int totalFiles = jsonData["total_files"].toInt(); + const int downloadedFiles = jsonData["downloaded_files"].toInt(); + + if (downloadedFiles >= totalFiles || totalFiles <= 0) return "Downloaded"; + + const long elapsed = duration_cast(system_clock::now() - startTime).count(); + + if (elapsed == 0 || downloadedFiles == 0) return "Calculating..."; + + return formatTime(elapsed); + } + + static QString calculateETA(const QJsonObject &jsonData, const std::chrono::system_clock::time_point &startTime) { + using namespace std::chrono; + static steady_clock::time_point lastUpdateTime = steady_clock::now(); + static std::deque rateHistory; + + constexpr int minDataPoints = 3; + constexpr int historySize = 10; + + static QString lastETA = "Calculating ETA..."; + + if (duration_cast(steady_clock::now() - lastUpdateTime).count() < 1) { + return lastETA; + } + + if (!jsonData.contains("total_files") || !jsonData.contains("downloaded_files")) + return lastETA; + + const int totalFiles = jsonData["total_files"].toInt(); + const int downloadedFiles = jsonData["downloaded_files"].toInt(); + + if (totalFiles <= 0 || downloadedFiles >= totalFiles) { + return totalFiles <= 0 ? "Ready" : "Downloaded"; + } + + const long elapsed = duration_cast(system_clock::now() - startTime).count(); + if (elapsed == 0 || downloadedFiles == 0) return lastETA; + + const double rate = downloadedFiles / static_cast(elapsed); + if (rateHistory.size() >= historySize) rateHistory.pop_front(); + rateHistory.push_back(rate); + + if (rateHistory.size() < minDataPoints) return lastETA; + + double weightedSum = 0; + for (int i = 0, weight = 1; i < rateHistory.size(); ++i, ++weight) { + weightedSum += rateHistory[i] * weight; + } + const double avgRate = 2 * weightedSum / (rateHistory.size() * (rateHistory.size() + 1)); + + const long remainingTime = static_cast((totalFiles - downloadedFiles) / avgRate); + if (remainingTime <= 0) return lastETA; + + lastETA = formatTime(remainingTime) + " remaining"; + lastUpdateTime = steady_clock::now(); + return lastETA; + } + + static QString formatDownloadStatus(const QJsonObject &json) { + + if (!json.contains("total_files") || !json.contains("downloaded_files")) + return ""; + + const int total_files = json["total_files"].toInt(); + const int downloaded_files = json["downloaded_files"].toInt(); + + if (total_files <= 0) return "Ready"; + if (downloaded_files >= total_files) return "Downloaded"; + + const int percentage = static_cast(100.0 * downloaded_files / total_files); + return QString::asprintf("%d/%d (%d%%)", downloaded_files, total_files, percentage); + } + + QString formatSize(quint64 size) const { + if (size == 0 && (!mapSizeFuture.has_value() || mapSizeFuture.value().isRunning())) { + return QString("Calculating..."); + } + + constexpr qint64 kb = 1024; + constexpr qint64 mb = 1024 * kb; + constexpr qint64 gb = 1024 * mb; + + if (size < gb) { + const double sizeMB = size / static_cast(mb); + return QString::number(sizeMB, 'f', 2) + " MB"; + } else { + const double sizeGB = size / static_cast(gb); + return QString::number(sizeGB, 'f', 2) + " GB"; + } + } + + static quint64 getDirSize(QString dirPath) { + quint64 size = 0; + const QString actualDirPath = dirPath.startsWith("~") ? dirPath.replace(0, 1, QDir::homePath()) : dirPath; + QDirIterator it(actualDirPath, QDir::Files | QDir::Dirs | QDir::NoDotAndDotDot | QDir::NoSymLinks, QDirIterator::Subdirectories); + while (it.hasNext()) { + it.next(); + if (it.fileInfo().isFile()) { + size += it.fileInfo().size(); + } + } + return size; } }; diff --git a/selfdrive/ui/qt/offroad/sunnypilot/sunnypilot_settings.cc b/selfdrive/ui/qt/offroad/sunnypilot/sunnypilot_settings.cc index d75ef3d205..3143cfe524 100644 --- a/selfdrive/ui/qt/offroad/sunnypilot/sunnypilot_settings.cc +++ b/selfdrive/ui/qt/offroad/sunnypilot/sunnypilot_settings.cc @@ -482,6 +482,9 @@ void SunnypilotPanel::updateToggles() { customTorqueControl->setVisible(false); } } + + m_tsc->setVisible(false); // TODO: temporarily disable M-TSC until the reimplementation is in place. Remove this line to re-enable the toggle. + m_tsc->setEnabled(false); // TODO: temporarily disable M-TSC until the reimplementation is in place. Remove this line to re-enable the toggle. } TorqueFriction::TorqueFriction() : SPOptionControl ( diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 4416e58338..b2999d050a 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -963,13 +963,10 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { // Bottom bar road name if (showDebugUI && !roadName.isEmpty()) { - const int h = 38; - QRect bar_rc(rect().left(), rect().top(), rect().width(), h); - p.setPen(Qt::NoPen); - p.setBrush(QColor(0, 0, 0, 100)); - p.drawRect(bar_rc); - p.setFont(InterFont(28, QFont::Bold)); - drawCenteredText(p, bar_rc.center().x(), bar_rc.center().y(), roadName, QColor(255, 255, 255, 200)); + int font_size = splitPanelVisible ? 38 : 50; + int h = splitPanelVisible ? 18 : 26; + p.setFont(InterFont(font_size, QFont::Bold)); + drawRoadNameText(p, rect().center().x(), h, roadName, QColor(255, 255, 255, 255)); } // Turn Speed Sign @@ -1016,6 +1013,22 @@ void AnnotatedCameraWidget::drawCenteredText(QPainter &p, int x, int y, const QS p.drawText(real_rect, Qt::AlignCenter, text); } +void AnnotatedCameraWidget::drawRoadNameText(QPainter &p, int x, int y, const QString &text, QColor color) { + QRect real_rect = p.fontMetrics().boundingRect(text); + real_rect.moveCenter({x, y}); + + QRect real_rect_adjusted(real_rect); + real_rect_adjusted.adjust(-UI_ROAD_NAME_MARGIN_X, 5, UI_ROAD_NAME_MARGIN_X, 0); + QPainterPath path; + path.addRoundedRect(real_rect_adjusted, 10, 10); + p.setPen(Qt::NoPen); + p.setBrush(QColor(0, 0, 0, 100)); + p.drawPath(path); + + p.setPen(color); + p.drawText(real_rect, Qt::AlignCenter, text); +} + void AnnotatedCameraWidget::drawVisionTurnControllerUI(QPainter &p, int x, int y, int size, const QColor &color, const QString &vision_speed, int alpha) { QRect rvtc(x, y, size, size); diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 8b444002e7..a64700fd6d 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -127,6 +127,7 @@ private: void speedLimitSignPulse(int frame); void speedLimitWarning(QPainter &p, QRect sign_rect, const int sign_margin); void mousePressEvent(QMouseEvent* e) override; + void drawRoadNameText(QPainter &p, int x, int y, const QString &text, QColor color); Params params; QVBoxLayout *main_layout; diff --git a/selfdrive/ui/qt/widgets/controls.h b/selfdrive/ui/qt/widgets/controls.h index 6b3ad20de8..636cfba830 100644 --- a/selfdrive/ui/qt/widgets/controls.h +++ b/selfdrive/ui/qt/widgets/controls.h @@ -109,6 +109,7 @@ public: ButtonControl(const QString &title, const QString &text, const QString &desc = "", QWidget *parent = nullptr); inline void setText(const QString &text) { btn.setText(text); } inline QString text() const { return btn.text(); } + inline void click() { btn.click(); } signals: void clicked(); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 3cb9bc3368..3f7bbcb5f8 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -20,6 +20,8 @@ const int UI_BORDER_SIZE = 30; const int UI_HEADER_HEIGHT = 420; +const int UI_ROAD_NAME_MARGIN_X = 14; + struct FeatureStatusText { const QStringList dlp_list_text = { "Laneful", "Laneless", "Auto" }; const QStringList gac_list_text = { "Maniac", "Aggressive", "Standard", "Relaxed" }; diff --git a/third_party/pfeiferj-mapd/README.md b/third_party/pfeiferj-mapd/README.md new file mode 100644 index 0000000000..a814f11260 --- /dev/null +++ b/third_party/pfeiferj-mapd/README.md @@ -0,0 +1,2 @@ +# MAPD implementation by pfeiferj +https://github.com/pfeiferj/openpilot-mapd/releases/ diff --git a/third_party/pfeiferj-mapd/mapd b/third_party/pfeiferj-mapd/mapd new file mode 100755 index 0000000000..4d07be8f32 Binary files /dev/null and b/third_party/pfeiferj-mapd/mapd differ