mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-08-06 13:15:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

mapd: New implementation and remove deprecated libraries

Browse Source
This commit is contained in:
DevTekVE
2023-12-19 22:14:06 +00:00
committed by Jason Wen
parent 7cdb6bb45d
commit 57dfee47b8
53 changed files with 1151 additions and 26343 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CHANGELOGS.md
+12
View File
@@ -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
common/params.cc
+16 -2
View File
@@ -178,6 +178,7 @@ std::unordered_map<std::string, uint32_t> 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<std::string, uint32_t> 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<std::string, uint32_t> 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
installer/custom/install_osm.sh
-35
View File
@@ -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}
installer/custom/install_osm_db.sh
-35
View File
@@ -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
launch_chffrplus.sh
+1 -6
View File
@@ -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
}
release/files_common
+6 -10
View File
@@ -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
selfdrive/controls/lib/alerts_offroad.json
+4
View File
@@ -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
}
}
selfdrive/controls/lib/sunnypilot/speed_limit_resolver.py
+21 -13
View File
@@ -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):
selfdrive/manager/custom_dep.py
-162
View File
@@ -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()
selfdrive/manager/local_osm_install.py
-72
View File
@@ -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()
selfdrive/manager/manager.py
+5
View File
@@ -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')))
selfdrive/manager/mapd_installer.py
Executable
+143
View File
@@ -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()
selfdrive/manager/process_config.py
+6 -1
View File
@@ -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),
selfdrive/mapd/README.md
-8
View File
@@ -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
selfdrive/mapd/assets/mapd_deps.tar.xz
BIN
View File
Binary file not shown.
selfdrive/mapd/assets/osm-3s_v0.7.57.tar.xz
BIN
View File
Binary file not shown.
selfdrive/mapd/config.py
-8
View File
@@ -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.
selfdrive/mapd/lib/NodesData.py
-452
View File
@@ -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
selfdrive/mapd/lib/Route.py
-343
View File
@@ -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
selfdrive/mapd/lib/WayCollection.py
-85
View File
@@ -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)
selfdrive/mapd/lib/WayRelation.py
-487
View File
@@ -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 `<restriction-value> @ (<condition>)` 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]
selfdrive/mapd/lib/WayRelationIndex.py
-34
View File
@@ -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, [])
selfdrive/mapd/lib/default_speeds.json
-112
View File
@@ -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"
}
selfdrive/mapd/lib/default_speeds_by_region.json
-624
View File
@@ -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"
}
}
]
}
}
selfdrive/mapd/lib/geo.py
-78
View File
@@ -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
selfdrive/mapd/lib/helpers.py
-102
View File
@@ -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
selfdrive/mapd/lib/osm.py
-71
View File
@@ -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
selfdrive/mapd/mapd.py
-294
View File
@@ -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()
selfdrive/mapd/sp_osm_db.json
-92
View File
@@ -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"
}
}
selfdrive/mapd/test/mock_data.py
-266
View File
@@ -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)
selfdrive/mapd/test/mock_osm_response_01.xml
-9908
View File
File diff suppressed because it is too large Load Diff
selfdrive/mapd/test/mock_osm_response_02.xml
-11529
View File
File diff suppressed because it is too large Load Diff
selfdrive/mapd/test/test_NodesData.py
-354
View File
@@ -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
selfdrive/mapd/test/test_WayRelation.py
-651
View File
@@ -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)
selfdrive/mapd/test/test_WayRelationIndex.py
-74
View File
@@ -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))
selfdrive/mapd/test/test_geo.py
-234
View File
@@ -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)
selfdrive/mapd_manager.py
+162
View File
@@ -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()
selfdrive/mapd/test/__init__.py → selfdrive/sunnypilot/__init__.py
View File
selfdrive/sunnypilot/live_map_data/__init__.py
+18
View File
@@ -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)
selfdrive/sunnypilot/live_map_data/base_map_data.py
+106
View File
@@ -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()
selfdrive/sunnypilot/live_map_data/debug.py
+50
View File
@@ -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()
selfdrive/sunnypilot/live_map_data/osm_map_data.py
+50
View File
@@ -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
selfdrive/sunnypilot/live_map_data/standalone.py
+40
View File
@@ -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()
selfdrive/ui/qt/offroad/sunnypilot/locations_fetcher.h
+52
View File
@@ -0,0 +1,52 @@
#pragma once
#include <QJsonObject>
#include <algorithm> // for std::sort
#include <deque>
#include <QDir>
#include "selfdrive/ui/qt/offroad/sunnypilot/json_fetcher.h"
static const std::tuple<QString, QString> defaultLocation = std::make_tuple("== None ==", "");
// New class LocationsFetcher that handles web requests and JSON parsing
class LocationsFetcher {
public:
inline std::vector<std::tuple<QString, QString, QString, QString>>
getLocationsFromURL(const QUrl &url, const std::tuple<QString, QString> &customLocation = defaultLocation) const {
// Initialize an empty vector to hold the locations
std::vector<std::tuple<QString, QString, QString, QString>> 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<std::tuple<QString, QString, QString, QString>>
getLocationsFromURL(const QString &url, const std::tuple<QString, QString> &customLocation = defaultLocation) const {
return getLocationsFromURL(QUrl(url), customLocation);
}
inline std::vector<std::tuple<QString, QString, QString, QString>>
getOsmLocations(const std::tuple<QString, QString> &customLocation = defaultLocation) const {
return getLocationsFromURL("https://raw.githubusercontent.com/pfeiferj/openpilot-mapd/main/nation_bounding_boxes.json", customLocation);
}
inline std::vector<std::tuple<QString, QString, QString, QString>>
getUsStatesLocations(const std::tuple<QString, QString> &customLocation = defaultLocation) const {
return getLocationsFromURL("https://raw.githubusercontent.com/pfeiferj/openpilot-mapd/main/us_states_bounding_boxes.json", customLocation);
}
};
selfdrive/ui/qt/offroad/sunnypilot/osm_settings.cc
+235 -76
View File
@@ -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<std::tuple<QString, QString, QString, QString>> 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<std::tuple<QString, QString, QString, QString>> 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<std::tuple<QString, QString, QString, QString>> 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<QString, QString> allStatesOption = std::make_tuple("All States (~4.8 GB)", "All");
usStatesBtn->setEnabled(false);
usStatesBtn->setValue("Fetching State list...");
const std::vector<std::tuple<QString, QString, QString, QString>> 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<std::time_t>(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);
}
}
selfdrive/ui/qt/offroad/sunnypilot/osm_settings.h
+195 -118
View File
@@ -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 <deque>
#include <chrono>
#include <optional>
#include <QDir>
#include <QFileInfo>
#include <QtConcurrent/QtConcurrent>
#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<std::string, ParamControl*> toggles;
std::optional<QFuture<quint64>> 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<std::tuple<QString, QString, QString, QString>> getOsmLocations() {
// location_title, location_name, waypoint, url
std::vector<std::tuple<QString, QString, QString, QString>> 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<std::chrono::system_clock::time_point> 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<std::tuple<QString, QString, QString, QString>> getOsmLocations(const std::tuple<QString, QString>& customLocation = defaultLocation) const {
return locationsFetcher.getOsmLocations(customLocation);
}
std::vector<std::tuple<QString, QString, QString, QString>> getUsStatesLocations(const std::tuple<QString, QString>& 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<seconds>(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<double> rateHistory;
constexpr int minDataPoints = 3;
constexpr int historySize = 10;
static QString lastETA = "Calculating ETA...";
if (duration_cast<seconds>(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<seconds>(system_clock::now() - startTime).count();
if (elapsed == 0 || downloadedFiles == 0) return lastETA;
const double rate = downloadedFiles / static_cast<double>(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<long>((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<int>(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<double>(mb);
return QString::number(sizeMB, 'f', 2) + " MB";
} else {
const double sizeGB = size / static_cast<double>(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;
}
};
selfdrive/ui/qt/offroad/sunnypilot/sunnypilot_settings.cc
+3
View File
@@ -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 (
selfdrive/ui/qt/onroad.cc
+20 -7
View File
@@ -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);
selfdrive/ui/qt/onroad.h
+1
View File
@@ -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;
selfdrive/ui/qt/widgets/controls.h
+1
View File
@@ -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();
selfdrive/ui/ui.h
+2
View File
@@ -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" };
third_party/pfeiferj-mapd/README.md
Vendored
+2
View File
@@ -0,0 +1,2 @@
# MAPD implementation by pfeiferj
https://github.com/pfeiferj/openpilot-mapd/releases/
third_party/pfeiferj-mapd/mapd
Vendored Executable
BIN
View File
Binary file not shown.