mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-09 15:42:11 +08:00
Remove unmaintained mapd code
old-commit-hash: d38a9521025f872a2710f311573249cc9f0f8733
This commit is contained in:
Willem Melching
@@ -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"
|
||||
}
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user