From ed69033e66a02a332d739a22ca20a4710e17acc4 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Thu, 9 Apr 2020 11:43:14 -0700 Subject: [PATCH] Remove unmaintained mapd code old-commit-hash: d38a9521025f872a2710f311573249cc9f0f8733 --- selfdrive/mapd/__init__.py | 0 selfdrive/mapd/default_speeds.json | 106 ------ selfdrive/mapd/default_speeds_generator.py | 240 -------------- selfdrive/mapd/mapd.py | 295 ----------------- selfdrive/mapd/mapd_helpers.py | 364 --------------------- 5 files changed, 1005 deletions(-) delete mode 100644 selfdrive/mapd/__init__.py delete mode 100644 selfdrive/mapd/default_speeds.json delete mode 100755 selfdrive/mapd/default_speeds_generator.py delete mode 100755 selfdrive/mapd/mapd.py delete mode 100644 selfdrive/mapd/mapd_helpers.py diff --git a/selfdrive/mapd/__init__.py b/selfdrive/mapd/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/selfdrive/mapd/default_speeds.json b/selfdrive/mapd/default_speeds.json deleted file mode 100644 index 7126c27fc..000000000 --- a/selfdrive/mapd/default_speeds.json +++ /dev/null @@ -1,106 +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": "7", - "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" -} diff --git a/selfdrive/mapd/default_speeds_generator.py b/selfdrive/mapd/default_speeds_generator.py deleted file mode 100755 index aa2e55f68..000000000 --- a/selfdrive/mapd/default_speeds_generator.py +++ /dev/null @@ -1,240 +0,0 @@ -#!/usr/bin/env python3 -import json - -DEFAULT_OUTPUT_FILENAME = "default_speeds_by_region.json" - -def main(filename = DEFAULT_OUTPUT_FILENAME): - countries = [] - - """ - -------------------------------------------------- - US - United State of America - -------------------------------------------------- - """ - US = Country("US") # First step, create the country using the ISO 3166 two letter code - countries.append(US) # Second step, add the country to countries list - - """ Default rules """ - # Third step, add some default rules for the country - # Speed limit rules are based on OpenStreetMaps (OSM) tags. - # The dictionary {...} defines the tag_name: value - # if a road in OSM has a tag with the name tag_name and this value, the speed limit listed below will be applied. - # The text at the end is the speed limit (use no unit for km/h) - # Rules apply in the order in which they are written for each country - # Rules for specific regions (states) take priority over country rules - # If you modify existing country rules, you must update all existing states without that rule to use the old rule - US.add_rule({"highway": "motorway"}, "65 mph") # On US roads with the tag highway and value motorway, the speed limit will default to 65 mph - US.add_rule({"highway": "trunk"}, "55 mph") - US.add_rule({"highway": "primary"}, "55 mph") - US.add_rule({"highway": "secondary"}, "45 mph") - US.add_rule({"highway": "tertiary"}, "35 mph") - US.add_rule({"highway": "unclassified"}, "55 mph") - US.add_rule({"highway": "residential"}, "25 mph") - US.add_rule({"highway": "service"}, "25 mph") - US.add_rule({"highway": "motorway_link"}, "55 mph") - US.add_rule({"highway": "trunk_link"}, "55 mph") - US.add_rule({"highway": "primary_link"}, "55 mph") - US.add_rule({"highway": "secondary_link"}, "45 mph") - US.add_rule({"highway": "tertiary_link"}, "35 mph") - US.add_rule({"highway": "living_street"}, "15 mph") - - """ States """ - new_york = US.add_region("New York") # Fourth step, add a state/region to country - new_york.add_rule({"highway": "primary"}, "45 mph") # Fifth step , add rules to the state. See the text above for how to write rules - new_york.add_rule({"highway": "secondary"}, "55 mph") - new_york.add_rule({"highway": "tertiary"}, "55 mph") - new_york.add_rule({"highway": "residential"}, "30 mph") - new_york.add_rule({"highway": "primary_link"}, "45 mph") - new_york.add_rule({"highway": "secondary_link"}, "55 mph") - new_york.add_rule({"highway": "tertiary_link"}, "55 mph") - # All if not written by the state, the rules will default to the country rules - - #california = US.add_region("California") - # California uses only the default US rules - - michigan = US.add_region("Michigan") - michigan.add_rule({"highway": "motorway"}, "70 mph") - - oregon = US.add_region("Oregon") - oregon.add_rule({"highway": "motorway"}, "55 mph") - oregon.add_rule({"highway": "secondary"}, "35 mph") - oregon.add_rule({"highway": "tertiary"}, "30 mph") - oregon.add_rule({"highway": "service"}, "15 mph") - oregon.add_rule({"highway": "secondary_link"}, "35 mph") - oregon.add_rule({"highway": "tertiary_link"}, "30 mph") - - south_dakota = US.add_region("South Dakota") - south_dakota.add_rule({"highway": "motorway"}, "80 mph") - south_dakota.add_rule({"highway": "trunk"}, "70 mph") - south_dakota.add_rule({"highway": "primary"}, "65 mph") - south_dakota.add_rule({"highway": "trunk_link"}, "70 mph") - south_dakota.add_rule({"highway": "primary_link"}, "65 mph") - - wisconsin = US.add_region("Wisconsin") - wisconsin.add_rule({"highway": "trunk"}, "65 mph") - wisconsin.add_rule({"highway": "tertiary"}, "45 mph") - wisconsin.add_rule({"highway": "unclassified"}, "35 mph") - wisconsin.add_rule({"highway": "trunk_link"}, "65 mph") - wisconsin.add_rule({"highway": "tertiary_link"}, "45 mph") - - """ - -------------------------------------------------- - AU - Australia - -------------------------------------------------- - """ - AU = Country("AU") - countries.append(AU) - - """ Default rules """ - AU.add_rule({"highway": "motorway"}, "100") - AU.add_rule({"highway": "trunk"}, "80") - AU.add_rule({"highway": "primary"}, "80") - AU.add_rule({"highway": "secondary"}, "50") - AU.add_rule({"highway": "tertiary"}, "50") - AU.add_rule({"highway": "unclassified"}, "80") - AU.add_rule({"highway": "residential"}, "50") - AU.add_rule({"highway": "service"}, "40") - AU.add_rule({"highway": "motorway_link"}, "90") - AU.add_rule({"highway": "trunk_link"}, "80") - AU.add_rule({"highway": "primary_link"}, "80") - AU.add_rule({"highway": "secondary_link"}, "50") - AU.add_rule({"highway": "tertiary_link"}, "50") - AU.add_rule({"highway": "living_street"}, "30") - - """ - -------------------------------------------------- - CA - Canada - -------------------------------------------------- - """ - CA = Country("CA") - countries.append(CA) - - """ Default rules """ - CA.add_rule({"highway": "motorway"}, "100") - CA.add_rule({"highway": "trunk"}, "80") - CA.add_rule({"highway": "primary"}, "80") - CA.add_rule({"highway": "secondary"}, "50") - CA.add_rule({"highway": "tertiary"}, "50") - CA.add_rule({"highway": "unclassified"}, "80") - CA.add_rule({"highway": "residential"}, "40") - CA.add_rule({"highway": "service"}, "40") - CA.add_rule({"highway": "motorway_link"}, "90") - CA.add_rule({"highway": "trunk_link"}, "80") - CA.add_rule({"highway": "primary_link"}, "80") - CA.add_rule({"highway": "secondary_link"}, "50") - CA.add_rule({"highway": "tertiary_link"}, "50") - CA.add_rule({"highway": "living_street"}, "20") - - - """ - -------------------------------------------------- - DE - Germany - -------------------------------------------------- - """ - DE = Country("DE") - countries.append(DE) - - """ Default rules """ - DE.add_rule({"highway": "motorway"}, "none") - DE.add_rule({"highway": "living_street"}, "10") - DE.add_rule({"highway": "residential"}, "30") - DE.add_rule({"zone:traffic": "DE:rural"}, "100") - DE.add_rule({"zone:traffic": "DE:urban"}, "50") - DE.add_rule({"zone:maxspeed": "DE:30"}, "30") - DE.add_rule({"zone:maxspeed": "DE:urban"}, "50") - DE.add_rule({"zone:maxspeed": "DE:rural"}, "100") - DE.add_rule({"zone:maxspeed": "DE:motorway"}, "none") - DE.add_rule({"bicycle_road": "yes"}, "30") - - - """ - -------------------------------------------------- - EE - Estonia - -------------------------------------------------- - """ - EE = Country("EE") - countries.append(EE) - - """ Default rules """ - EE.add_rule({"highway": "motorway"}, "90") - EE.add_rule({"highway": "trunk"}, "90") - EE.add_rule({"highway": "primary"}, "90") - EE.add_rule({"highway": "secondary"}, "50") - EE.add_rule({"highway": "tertiary"}, "50") - EE.add_rule({"highway": "unclassified"}, "90") - EE.add_rule({"highway": "residential"}, "40") - EE.add_rule({"highway": "service"}, "40") - EE.add_rule({"highway": "motorway_link"}, "90") - EE.add_rule({"highway": "trunk_link"}, "70") - EE.add_rule({"highway": "primary_link"}, "70") - EE.add_rule({"highway": "secondary_link"}, "50") - EE.add_rule({"highway": "tertiary_link"}, "50") - EE.add_rule({"highway": "living_street"}, "20") - - - """ --- DO NOT MODIFY CODE BELOW THIS LINE --- """ - """ --- ADD YOUR COUNTRY OR STATE ABOVE --- """ - - # Final step - write_json(countries, filename) - -def write_json(countries, filename = DEFAULT_OUTPUT_FILENAME): - out_dict = {} - for country in countries: - out_dict.update(country.jsonify()) - json_string = json.dumps(out_dict, indent=2) - with open(filename, "wb") as f: - f.write(json_string) - - -class Region(): - ALLOWABLE_TAG_KEYS = ["highway", "zone:traffic", "bicycle_road", "zone:maxspeed"] - ALLOWABLE_HIGHWAY_TYPES = ["motorway", "trunk", "primary", "secondary", "tertiary", "unclassified", "residential", "service", "motorway_link", "trunk_link", "primary_link", "secondary_link", "tertiary_link", "living_street"] - def __init__(self, name): - self.name = name - self.rules = [] - - def add_rule(self, tag_conditions, speed): - new_rule = {} - if not isinstance(tag_conditions, dict): - raise TypeError("Rule tag conditions must be dictionary") - if not all(tag_key in self.ALLOWABLE_TAG_KEYS for tag_key in tag_conditions): - raise ValueError("Rule tag keys must be in allowable tag kesy") # If this is by mistake, please update ALLOWABLE_TAG_KEYS - if 'highway' in tag_conditions: - if not tag_conditions['highway'] in self.ALLOWABLE_HIGHWAY_TYPES: - raise ValueError("Invalid Highway type {}".format(tag_conditions["highway"])) - new_rule['tags'] = tag_conditions - try: - new_rule['speed'] = str(speed) - except ValueError: - raise ValueError("Rule speed must be string") - self.rules.append(new_rule) - - def jsonify(self): - ret_dict = {} - ret_dict[self.name] = self.rules - return ret_dict - -class Country(Region): - ALLOWABLE_COUNTRY_CODES = ["AF","AX","AL","DZ","AS","AD","AO","AI","AQ","AG","AR","AM","AW","AU","AT","AZ","BS","BH","BD","BB","BY","BE","BZ","BJ","BM","BT","BO","BQ","BA","BW","BV","BR","IO","BN","BG","BF","BI","KH","CM","CA","CV","KY","CF","TD","CL","CN","CX","CC","CO","KM","CG","CD","CK","CR","CI","HR","CU","CW","CY","CZ","DK","DJ","DM","DO","EC","EG","SV","GQ","ER","EE","ET","FK","FO","FJ","FI","FR","GF","PF","TF","GA","GM","GE","DE","GH","GI","GR","GL","GD","GP","GU","GT","GG","GN","GW","GY","HT","HM","VA","HN","HK","HU","IS","IN","ID","IR","IQ","IE","IM","IL","IT","JM","JP","JE","JO","KZ","KE","KI","KP","KR","KW","KG","LA","LV","LB","LS","LR","LY","LI","LT","LU","MO","MK","MG","MW","MY","MV","ML","MT","MH","MQ","MR","MU","YT","MX","FM","MD","MC","MN","ME","MS","MA","MZ","MM","NA","NR","NP","NL","NC","NZ","NI","NE","NG","NU","NF","MP","NO","OM","PK","PW","PS","PA","PG","PY","PE","PH","PN","PL","PT","PR","QA","RE","RO","RU","RW","BL","SH","KN","LC","MF","PM","VC","WS","SM","ST","SA","SN","RS","SC","SL","SG","SX","SK","SI","SB","SO","ZA","GS","SS","ES","LK","SD","SR","SJ","SZ","SE","CH","SY","TW","TJ","TZ","TH","TL","TG","TK","TO","TT","TN","TR","TM","TC","TV","UG","UA","AE","GB","US","UM","UY","UZ","VU","VE","VN","VG","VI","WF","EH","YE","ZM","ZW"] - def __init__(self, ISO_3166_alpha_2): - Region.__init__(self, ISO_3166_alpha_2) - if ISO_3166_alpha_2 not in self.ALLOWABLE_COUNTRY_CODES: - raise ValueError("Not valid IOS 3166 country code") - self.regions = {} - - def add_region(self, name): - self.regions[name] = Region(name) - return self.regions[name] - - def jsonify(self): - ret_dict = {} - ret_dict[self.name] = {} - for r_name, region in self.regions.items(): - ret_dict[self.name].update(region.jsonify()) - ret_dict[self.name]['Default'] = self.rules - return ret_dict - - -if __name__ == '__main__': - main() diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py deleted file mode 100755 index 85862befa..000000000 --- a/selfdrive/mapd/mapd.py +++ /dev/null @@ -1,295 +0,0 @@ -#!/usr/bin/env python3 - -# Add phonelibs openblas to LD_LIBRARY_PATH if import fails -from common.basedir import BASEDIR -try: - from scipy import spatial -except ImportError as e: - import os - import sys - - - openblas_path = os.path.join(BASEDIR, "phonelibs/openblas/") - os.environ['LD_LIBRARY_PATH'] += ':' + openblas_path - - args = [sys.executable] - args.extend(sys.argv) - os.execv(sys.executable, args) - -DEFAULT_SPEEDS_BY_REGION_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds_by_region.json" -from selfdrive.mapd import default_speeds_generator -default_speeds_generator.main(DEFAULT_SPEEDS_BY_REGION_JSON_FILE) - -import os -import sys -import time -import zmq -import threading -import numpy as np -import overpy -from collections import defaultdict - -from common.params import Params -from common.transformations.coordinates import geodetic2ecef -from cereal.services import service_list -import cereal.messaging as messaging -from selfdrive.mapd.mapd_helpers import MAPS_LOOKAHEAD_DISTANCE, Way, circle_through_points -import selfdrive.crash as crash -from selfdrive.version import version, dirty - - -OVERPASS_API_URL = "https://overpass.kumi.systems/api/interpreter" -OVERPASS_HEADERS = { - 'User-Agent': 'NEOS (comma.ai)', - 'Accept-Encoding': 'gzip' -} - -last_gps = None -query_lock = threading.Lock() -last_query_result = None -last_query_pos = None -cache_valid = False - -def build_way_query(lat, lon, radius=50): - """Builds a query to find all highways within a given radius around a point""" - pos = " (around:%f,%f,%f)" % (radius, lat, lon) - lat_lon = "(%f,%f)" % (lat, lon) - q = """( - way - """ + pos + """ - [highway][highway!~"^(footway|path|bridleway|steps|cycleway|construction|bus_guideway|escape)$"]; - >;);out;""" + """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; - """ - return q - - -def query_thread(): - global last_query_result, last_query_pos, cache_valid - api = overpy.Overpass(url=OVERPASS_API_URL, headers=OVERPASS_HEADERS, timeout=10.) - - while True: - time.sleep(1) - if last_gps is not None: - fix_ok = last_gps.flags & 1 - if not fix_ok: - continue - - if last_query_pos is not None: - cur_ecef = geodetic2ecef((last_gps.latitude, last_gps.longitude, last_gps.altitude)) - prev_ecef = geodetic2ecef((last_query_pos.latitude, last_query_pos.longitude, last_query_pos.altitude)) - dist = np.linalg.norm(cur_ecef - prev_ecef) - if dist < 1000: #updated when we are 1km from the edge of the downloaded circle - continue - - if dist > 3000: - cache_valid = False - - q = build_way_query(last_gps.latitude, last_gps.longitude, radius=3000) - try: - new_result = api.query(q) - - # Build kd-tree - nodes = [] - real_nodes = [] - node_to_way = defaultdict(list) - location_info = {} - - for n in new_result.nodes: - nodes.append((float(n.lat), float(n.lon), 0)) - real_nodes.append(n) - - for way in new_result.ways: - for n in way.nodes: - node_to_way[n.id].append(way) - - for area in new_result.areas: - if area.tags.get('admin_level', '') == "2": - location_info['country'] = area.tags.get('ISO3166-1:alpha2', '') - if area.tags.get('admin_level', '') == "4": - location_info['region'] = area.tags.get('name', '') - - nodes = np.asarray(nodes) - nodes = geodetic2ecef(nodes) - tree = spatial.cKDTree(nodes) - - query_lock.acquire() - last_query_result = new_result, tree, real_nodes, node_to_way, location_info - last_query_pos = last_gps - cache_valid = True - query_lock.release() - - except Exception as e: - print(e) - query_lock.acquire() - last_query_result = None - query_lock.release() - - -def mapsd_thread(): - global last_gps - - gps_sock = messaging.sub_sock('gpsLocation', conflate=True) - gps_external_sock = messaging.sub_sock('gpsLocationExternal', conflate=True) - map_data_sock = messaging.pub_sock('liveMapData') - - cur_way = None - curvature_valid = False - curvature = None - upcoming_curvature = 0. - dist_to_turn = 0. - road_points = None - - while True: - gps = messaging.recv_one(gps_sock) - gps_ext = messaging.recv_one_or_none(gps_external_sock) - - if gps_ext is not None: - gps = gps_ext.gpsLocationExternal - else: - gps = gps.gpsLocation - - last_gps = gps - - fix_ok = gps.flags & 1 - if not fix_ok or last_query_result is None or not cache_valid: - cur_way = None - curvature = None - curvature_valid = False - upcoming_curvature = 0. - dist_to_turn = 0. - road_points = None - map_valid = False - else: - map_valid = True - lat = gps.latitude - lon = gps.longitude - heading = gps.bearing - speed = gps.speed - - query_lock.acquire() - cur_way = Way.closest(last_query_result, lat, lon, heading, cur_way) - if cur_way is not None: - pnts, curvature_valid = cur_way.get_lookahead(lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE) - - xs = pnts[:, 0] - ys = pnts[:, 1] - road_points = [float(x) for x in xs], [float(y) for y in ys] - - if speed < 10: - curvature_valid = False - if curvature_valid and pnts.shape[0] <= 3: - curvature_valid = False - - # The curvature is valid when at least MAPS_LOOKAHEAD_DISTANCE of road is found - if curvature_valid: - # Compute the curvature for each point - with np.errstate(divide='ignore'): - circles = [circle_through_points(*p) for p in zip(pnts, pnts[1:], pnts[2:])] - circles = np.asarray(circles) - radii = np.nan_to_num(circles[:, 2]) - radii[radii < 10] = np.inf - curvature = 1. / radii - - # Index of closest point - closest = np.argmin(np.linalg.norm(pnts, axis=1)) - dist_to_closest = pnts[closest, 0] # We can use x distance here since it should be close - - # Compute distance along path - dists = list() - dists.append(0) - for p, p_prev in zip(pnts, pnts[1:, :]): - dists.append(dists[-1] + np.linalg.norm(p - p_prev)) - dists = np.asarray(dists) - dists = dists - dists[closest] + dist_to_closest - dists = dists[1:-1] - - close_idx = np.logical_and(dists > 0, dists < 500) - dists = dists[close_idx] - curvature = curvature[close_idx] - - if len(curvature): - # TODO: Determine left or right turn - curvature = np.nan_to_num(curvature) - - # Outlier rejection - new_curvature = np.percentile(curvature, 90, interpolation='lower') - - k = 0.6 - upcoming_curvature = k * upcoming_curvature + (1 - k) * new_curvature - in_turn_indices = curvature > 0.8 * new_curvature - - if np.any(in_turn_indices): - dist_to_turn = np.min(dists[in_turn_indices]) - else: - dist_to_turn = 999 - else: - upcoming_curvature = 0. - dist_to_turn = 999 - - query_lock.release() - - dat = messaging.new_message('liveMapData') - - if last_gps is not None: - dat.liveMapData.lastGps = last_gps - - if cur_way is not None: - dat.liveMapData.wayId = cur_way.id - - # Speed limit - max_speed = cur_way.max_speed() - if max_speed is not None: - dat.liveMapData.speedLimitValid = True - dat.liveMapData.speedLimit = max_speed - - # TODO: use the function below to anticipate upcoming speed limits - #max_speed_ahead, max_speed_ahead_dist = cur_way.max_speed_ahead(max_speed, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE) - #if max_speed_ahead is not None and max_speed_ahead_dist is not None: - # dat.liveMapData.speedLimitAheadValid = True - # dat.liveMapData.speedLimitAhead = float(max_speed_ahead) - # dat.liveMapData.speedLimitAheadDistance = float(max_speed_ahead_dist) - - - advisory_max_speed = cur_way.advisory_max_speed() - if advisory_max_speed is not None: - dat.liveMapData.speedAdvisoryValid = True - dat.liveMapData.speedAdvisory = advisory_max_speed - - # Curvature - dat.liveMapData.curvatureValid = curvature_valid - dat.liveMapData.curvature = float(upcoming_curvature) - dat.liveMapData.distToTurn = float(dist_to_turn) - if road_points is not None: - dat.liveMapData.roadX, dat.liveMapData.roadY = road_points - if curvature is not None: - dat.liveMapData.roadCurvatureX = [float(x) for x in dists] - dat.liveMapData.roadCurvature = [float(x) for x in curvature] - - dat.liveMapData.mapValid = map_valid - - map_data_sock.send(dat.to_bytes()) - - -def main(): - params = Params() - dongle_id = params.get("DongleId") - crash.bind_user(id=dongle_id) - crash.bind_extra(version=version, dirty=dirty, is_eon=True) - crash.install() - - main_thread = threading.Thread(target=mapsd_thread) - main_thread.daemon = True - main_thread.start() - - q_thread = threading.Thread(target=query_thread) - q_thread.daemon = True - q_thread.start() - - while True: - time.sleep(0.1) - - -if __name__ == "__main__": - main() diff --git a/selfdrive/mapd/mapd_helpers.py b/selfdrive/mapd/mapd_helpers.py deleted file mode 100644 index 58a33ee41..000000000 --- a/selfdrive/mapd/mapd_helpers.py +++ /dev/null @@ -1,364 +0,0 @@ -import math -import json -import numpy as np -from datetime import datetime -from common.basedir import BASEDIR -from selfdrive.config import Conversions as CV -from common.transformations.coordinates import LocalCoord, geodetic2ecef - -LOOKAHEAD_TIME = 10. -MAPS_LOOKAHEAD_DISTANCE = 50 * LOOKAHEAD_TIME - -DEFAULT_SPEEDS_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds.json" -DEFAULT_SPEEDS = {} -with open(DEFAULT_SPEEDS_JSON_FILE, "rb") as f: - DEFAULT_SPEEDS = json.loads(f.read()) - -DEFAULT_SPEEDS_BY_REGION_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds_by_region.json" -DEFAULT_SPEEDS_BY_REGION = {} -with open(DEFAULT_SPEEDS_BY_REGION_JSON_FILE, "rb") as f: - DEFAULT_SPEEDS_BY_REGION = json.loads(f.read()) - -def circle_through_points(p1, p2, p3): - """Fits a circle through three points - Formulas from: http://www.ambrsoft.com/trigocalc/circle3d.htm""" - x1, y1, _ = p1 - x2, y2, _ = p2 - x3, y3, _ = p3 - - A = x1 * (y2 - y3) - y1 * (x2 - x3) + x2 * y3 - x3 * y2 - B = (x1**2 + y1**2) * (y3 - y2) + (x2**2 + y2**2) * (y1 - y3) + (x3**2 + y3**2) * (y2 - y1) - C = (x1**2 + y1**2) * (x2 - x3) + (x2**2 + y2**2) * (x3 - x1) + (x3**2 + y3**2) * (x1 - x2) - D = (x1**2 + y1**2) * (x3 * y2 - x2 * y3) + (x2**2 + y2**2) * (x1 * y3 - x3 * y1) + (x3**2 + y3**2) * (x2 * y1 - x1 * y2) - - return (-B / (2 * A), - C / (2 * A), np.sqrt((B**2 + C**2 - 4 * A * D) / (4 * A**2))) - -def parse_speed_unit(max_speed): - """Converts a maxspeed string to m/s based on the unit present in the input. - OpenStreetMap defaults to kph if no unit is present. """ - - if not max_speed: - return None - - conversion = CV.KPH_TO_MS - if 'mph' in max_speed: - max_speed = max_speed.replace(' mph', '') - conversion = CV.MPH_TO_MS - try: - return float(max_speed) * conversion - except ValueError: - return None - -def parse_speed_tags(tags): - """Parses tags on a way to find the maxspeed string""" - max_speed = None - - if 'maxspeed' in tags: - max_speed = tags['maxspeed'] - - if 'maxspeed:conditional' in tags: - try: - max_speed_cond, cond = tags['maxspeed:conditional'].split(' @ ') - cond = cond[1:-1] - - start, end = cond.split('-') - now = datetime.now() # TODO: Get time and timezone from gps fix so this will work correctly on replays - start = datetime.strptime(start, "%H:%M").replace(year=now.year, month=now.month, day=now.day) - end = datetime.strptime(end, "%H:%M").replace(year=now.year, month=now.month, day=now.day) - - if start <= now <= end: - max_speed = max_speed_cond - except ValueError: - pass - - if not max_speed and 'source:maxspeed' in tags: - max_speed = DEFAULT_SPEEDS.get(tags['source:maxspeed'], None) - if not max_speed and 'maxspeed:type' in tags: - max_speed = DEFAULT_SPEEDS.get(tags['maxspeed:type'], None) - - max_speed = parse_speed_unit(max_speed) - return max_speed - -def geocode_maxspeed(tags, location_info): - max_speed = None - try: - geocode_country = location_info.get('country', '') - geocode_region = location_info.get('region', '') - - 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: - pass - max_speed = parse_speed_unit(max_speed) - return max_speed - -class Way: - def __init__(self, way, query_results): - self.id = way.id - self.way = way - self.query_results = query_results - - points = list() - - for node in self.way.get_nodes(resolve_missing=False): - points.append((float(node.lat), float(node.lon), 0.)) - - self.points = np.asarray(points) - - @classmethod - def closest(cls, query_results, lat, lon, heading, prev_way=None): - results, tree, real_nodes, node_to_way, location_info = query_results - - cur_pos = geodetic2ecef((lat, lon, 0)) - nodes = tree.query_ball_point(cur_pos, 500) - - # If no nodes within 500m, choose closest one - if not nodes: - nodes = [tree.query(cur_pos)[1]] - - ways = [] - for n in nodes: - real_node = real_nodes[n] - ways += node_to_way[real_node.id] - ways = set(ways) - - closest_way = None - best_score = None - for way in ways: - way = Way(way, query_results) - points = way.points_in_car_frame(lat, lon, heading) - - on_way = way.on_way(lat, lon, heading, points) - if not on_way: - continue - - # Create mask of points in front and behind - x = points[:, 0] - y = points[:, 1] - angles = np.arctan2(y, x) - front = np.logical_and((-np.pi / 2) < angles, - angles < (np.pi / 2)) - behind = np.logical_not(front) - - dists = np.linalg.norm(points, axis=1) - - # Get closest point behind the car - dists_behind = np.copy(dists) - dists_behind[front] = np.NaN - closest_behind = points[np.nanargmin(dists_behind)] - - # Get closest point in front of the car - dists_front = np.copy(dists) - dists_front[behind] = np.NaN - closest_front = points[np.nanargmin(dists_front)] - - # fit line: y = a*x + b - x1, y1, _ = closest_behind - x2, y2, _ = closest_front - a = (y2 - y1) / max((x2 - x1), 1e-5) - b = y1 - a * x1 - - # With a factor of 60 a 20m offset causes the same error as a 20 degree heading error - # (A 20 degree heading offset results in an a of about 1/3) - score = abs(a) * 60. + abs(b) - - # Prefer same type of road - if prev_way is not None: - if way.way.tags.get('highway', '') == prev_way.way.tags.get('highway', ''): - score *= 0.5 - - if closest_way is None or score < best_score: - closest_way = way - best_score = score - - # Normal score is < 5 - if best_score > 50: - return None - - return closest_way - - def __str__(self): - return "%s %s" % (self.id, self.way.tags) - - def max_speed(self): - """Extracts the (conditional) speed limit from a way""" - if not self.way: - return None - - max_speed = parse_speed_tags(self.way.tags) - if not max_speed: - location_info = self.query_results[4] - max_speed = geocode_maxspeed(self.way.tags, location_info) - - return max_speed - - def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead): - """Look ahead for a max speed""" - if not self.way: - return None - - speed_ahead = None - speed_ahead_dist = None - lookahead_ways = 5 - way = self - for i in range(lookahead_ways): - way_pts = way.points_in_car_frame(lat, lon, heading) - - # Check current lookahead distance - max_dist = np.linalg.norm(way_pts[-1, :]) - - if max_dist > 2 * lookahead: - break - - if 'maxspeed' in way.way.tags: - spd = parse_speed_tags(way.way.tags) - if not spd: - location_info = self.query_results[4] - spd = geocode_maxspeed(way.way.tags, location_info) - if spd < current_speed_limit: - speed_ahead = spd - min_dist = np.linalg.norm(way_pts[1, :]) - speed_ahead_dist = min_dist - break - # Find next way - way = way.next_way() - if not way: - break - - return speed_ahead, speed_ahead_dist - - def advisory_max_speed(self): - if not self.way: - return None - - tags = self.way.tags - adv_speed = None - - if 'maxspeed:advisory' in tags: - adv_speed = tags['maxspeed:advisory'] - adv_speed = parse_speed_unit(adv_speed) - return adv_speed - - def on_way(self, lat, lon, heading, points=None): - if points is None: - points = self.points_in_car_frame(lat, lon, heading) - x = points[:, 0] - return np.min(x) < 0. and np.max(x) > 0. - - def closest_point(self, lat, lon, heading, points=None): - if points is None: - points = self.points_in_car_frame(lat, lon, heading) - i = np.argmin(np.linalg.norm(points, axis=1)) - return points[i] - - def distance_to_closest_node(self, lat, lon, heading, points=None): - if points is None: - points = self.points_in_car_frame(lat, lon, heading) - return np.min(np.linalg.norm(points, axis=1)) - - def points_in_car_frame(self, lat, lon, heading): - lc = LocalCoord.from_geodetic([lat, lon, 0.]) - - # Build rotation matrix - heading = math.radians(-heading + 90) - c, s = np.cos(heading), np.sin(heading) - rot = np.array([[c, s, 0.], [-s, c, 0.], [0., 0., 1.]]) - - # Convert to local coordinates - points_carframe = lc.geodetic2ned(self.points).T - - # Rotate with heading of car - points_carframe = np.dot(rot, points_carframe[(1, 0, 2), :]).T - - return points_carframe - - def next_way(self, backwards=False): - results, tree, real_nodes, node_to_way, location_info = self.query_results - - if backwards: - node = self.way.nodes[0] - else: - node = self.way.nodes[-1] - - ways = node_to_way[node.id] - - way = None - try: - # Simple heuristic to find next way - ways = [w for w in ways if w.id != self.id] - ways = [w for w in ways if w.nodes[0] == node] - - # Filter on highway tag - acceptable_tags = list() - cur_tag = self.way.tags['highway'] - acceptable_tags.append(cur_tag) - if cur_tag == 'motorway_link': - acceptable_tags.append('motorway') - acceptable_tags.append('trunk') - acceptable_tags.append('primary') - ways = [w for w in ways if w.tags['highway'] in acceptable_tags] - - # Filter on number of lanes - cur_num_lanes = int(self.way.tags['lanes']) - if len(ways) > 1: - ways_same_lanes = [w for w in ways if int(w.tags['lanes']) == cur_num_lanes] - if len(ways_same_lanes) == 1: - ways = ways_same_lanes - if len(ways) > 1: - ways = [w for w in ways if int(w.tags['lanes']) > cur_num_lanes] - if len(ways) == 1: - way = Way(ways[0], self.query_results) - - except (KeyError, ValueError): - pass - - return way - - def get_lookahead(self, lat, lon, heading, lookahead): - pnts = None - way = self - valid = False - - for i in range(5): - # Get new points and append to list - new_pnts = way.points_in_car_frame(lat, lon, heading) - - if pnts is None: - pnts = new_pnts - else: - pnts = np.vstack([pnts, new_pnts]) - - # Check current lookahead distance - max_dist = np.linalg.norm(pnts[-1, :]) - if max_dist > lookahead: - valid = True - - if max_dist > 2 * lookahead: - break - - # Find next way - way = way.next_way() - if not way: - break - - return pnts, valid