mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-09-30 19:33:42 +08:00
Code Issues Packages Projects Releases Wiki Activity

dragonpilot beta3

Browse Source 
date: 2023-07-26T22:20:36
commit: c6d842c412052be1985b63d683c63be9dcb2b0eb
This commit is contained in:
dragonpilot
2023-07-26 22:14:57 +08:00
parent 67c2c03b43
commit 1abc7d7daa
536 changed files with 437552 additions and 24400 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/mapd/README.md
-6
View File
@@ -1,6 +0,0 @@
# MapD
The OpenStreetMap based speed logical by the [Move Fast team](https://github.com/move-fast) and the additional enhancement by [Dragonpilot team](https://github.com/dragonpilot-community/dragonpilot)
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). PS: Your traces get uploaded automatically to OSM.
© OpenStreetMap contributors
selfdrive/mapd/assets/osm-3s_v0.7.57.tar.xz
BIN
View File
Binary file not shown.
selfdrive/mapd/config.py
-1
View File
@@ -1,7 +1,6 @@
# Map query config
QUERY_RADIUS = 3000 # mts. Radius to use on OSM data queries.
QUERY_RADIUS_OFFLINE = 3000 # mts. Radius to use on offline OSM data queries. (4000 works for some region / TW is problematic)
MIN_DISTANCE_FOR_NEW_QUERY = 1000 # mts. Minimum distance to query area edge before issuing a new query.
FULL_STOP_MAX_SPEED = 1.39 # m/s Max speed for considering car is stopped.
LOOK_AHEAD_HORIZON_TIME = 15. # s. Time horizon for look ahead of turn speed sections to provide on liveMapData msg.
selfdrive/mapd/lib/NodesData.py
+3 -21
View File
@@ -2,11 +2,11 @@ import numpy as np
from enum import Enum
from selfdrive.mapd.lib.geo import DIRECTION, R, vectors
from scipy.interpolate import splev, splprep
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.6 # Maximum lateral acceleration in turns.
_MAX_LAT_ACC = 2.3 # 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.
@@ -22,24 +22,6 @@ def nodes_raw_data_array_for_wr(wr, drop_last=False):
sl = wr.speed_limit
data = np.array([(n.id, n.lat, n.lon, sl) for n in wr.way.nodes], dtype=float)
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' or node.tags['traffic_calming'] == 'hump':
data[count][3] = 2.24
# reverse the order if way direction is backwards
if wr.direction == DIRECTION.BACKWARD:
data = np.flip(data, axis=0)
@@ -223,7 +205,7 @@ def is_wr_a_valid_divertion_from_node(wr, node_id, wr_ids):
"""
Evaluates if the way relation `wr` is a valid divertion from node with id `node_id`.
A valid divertion 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
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:
selfdrive/mapd/lib/Route.py
+3 -3
View File
@@ -1,5 +1,5 @@
from selfdrive.mapd.lib.NodesData import NodesData, NodeDataIdx
from selfdrive.mapd.config import QUERY_RADIUS, QUERY_RADIUS_OFFLINE
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
@@ -13,7 +13,7 @@ _MAP_DATA_EDGE_DISTANCE = 50 # mts. Consider edge of map data from this distanc
class Route():
"""A set of consecutive way relations forming a default driving route.
"""
def __init__(self, current, wr_index, way_collection_id, query_center, is_offline):
def __init__(self, current, wr_index, way_collection_id, query_center):
"""Create a Route object from a given `wr_index` (Way relation index)
Args:
@@ -161,7 +161,7 @@ class Route():
# 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 if is_offline else QUERY_RADIUS_OFFLINE) - _MAP_DATA_EDGE_DISTANCE):
if dist_to_center > QUERY_RADIUS - _MAP_DATA_EDGE_DISTANCE:
break
# - Select next way.
selfdrive/mapd/lib/WayCollection.py
+3 -4
View File
@@ -11,7 +11,7 @@ _ACCEPTABLE_BEARING_DELTA_IND = 0.7071067811865475 # sin(pi/4) | 45 degrees acc
class WayCollection():
"""A collection of WayRelations to use for maps data analysis.
"""
def __init__(self, areas, ways, query_center, is_offline):
def __init__(self, ways, query_center):
"""Creates a WayCollection with a set of OSM way objects.
Args:
@@ -19,11 +19,10 @@ class WayCollection():
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.way_relations = [WayRelation(way) for way in ways]
self.query_center = query_center
self.wr_index = WayRelationIndex(self.way_relations)
self.is_offline = is_offline
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.
@@ -83,4 +82,4 @@ class WayCollection():
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, self.is_offline)
return Route(current, self.wr_index, self.id, self.query_center)
selfdrive/mapd/lib/WayRelation.py
+3 -53
View File
@@ -11,8 +11,6 @@ import json
_WAY_BBOX_PADING = 80. / R # 80 mts of pading 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())
@@ -41,8 +39,7 @@ _HIGHWAY_RANK = {
'tertiary_link': 41,
'unclassified': 50,
'residential': 60,
'living_street': 61,
'service': 62
'living_street': 61
}
@@ -143,56 +140,12 @@ def conditional_speed_limit_for_osm_tag_limit_string(limit_string):
# If we get here, no current date-time conditon 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 Exception:
return None
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):
def __init__(self, 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()
@@ -388,9 +341,6 @@ class WayRelation():
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
@@ -469,4 +419,4 @@ class WayRelation():
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]
return [WayRelation(way, parent=self) for way in ways]
selfdrive/mapd/lib/default_speeds.json
+1 -2
View File
@@ -28,8 +28,7 @@
"DK:urban": "50",
"DK:rural": "80",
"DK:motorway": "130",
"DE:living_street": "10",
"DE:service": "10",
"DE:living_street": "7",
"DE:residential": "30",
"DE:urban": "50",
"DE:rural": "100",
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/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 selfdrive.hardware import TICI
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_VERSION = "0.7.57" if TICI else "0.7.56"
OSM_QUERY = [f"{OSM_LOCAL_PATH}/v{OSM_VERSION}/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"dragonpilot-{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
+6 -42
View File
@@ -1,9 +1,7 @@
import overpy
import subprocess
import numpy as np
from common.params import Params
from selfdrive.mapd.lib.geo import R
from selfdrive.mapd.lib.helpers import is_local_osm_installed, OSM_QUERY
def create_way(way_id, node_ids, from_way):
"""
@@ -16,58 +14,24 @@ def create_way(way_id, node_ids, from_way):
class OSM():
def __init__(self):
self.api = overpy.Overpass()
self.areas = None
self.osm_local_db_enabled = is_local_osm_installed()
# self.api = overpy.Overpass(url='http://3.65.170.21/api/interpreter')
def is_offline(self):
return self.osm_local_db_enabled
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;
out;
"""
try:
if self.osm_local_db_enabled:
# print("Query OSM from Local Server")
# q = """
# way(""" + bbox_str + """)
# [highway]
# [highway!~"^(footway|path|corridor|bridleway|steps|cycleway|construction|bus_guideway|escape|service|track)$"];
# (._;>;);
# out;"""
cmd = OSM_QUERY
cmd.append(f"--request={q}")
completion = subprocess.run(cmd, check=True, capture_output=True)
ways = self.api.parse_xml(completion.stdout).ways
if self.areas is None:
# 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:
self.areas = self.api.query(area_q).areas
except Exception:
pass
areas = self.areas
else:
# print("Query OSM from remote Server")
query = self.api.query(q + area_q)
areas, ways = query.areas, query.ways
ways = self.api.query(q).ways
except Exception as e:
# print(f'Exception while querying OSM:\n{e}')
areas, ways = [],[]
print(f'Exception while querying OSM:\n{e}')
ways = []
return areas, ways
return ways
selfdrive/mapd/mapd.py
+8 -41
View File
@@ -4,18 +4,13 @@ from traceback import print_exception
import numpy as np
from time import strftime, gmtime
import cereal.messaging as messaging
from common.realtime import Ratekeeper, sec_since_boot, config_realtime_process
from common.realtime import 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, MIN_DISTANCE_FOR_NEW_QUERY, FULL_STOP_MAX_SPEED, LOOK_AHEAD_HORIZON_TIME, QUERY_RADIUS_OFFLINE
from selfdrive.mapd.config import QUERY_RADIUS, MIN_DISTANCE_FOR_NEW_QUERY, FULL_STOP_MAX_SPEED, LOOK_AHEAD_HORIZON_TIME
from system.swaglog import cloudlog
# dp
from common.params import Params
import json
ROAD_NAME_TIMEOUT = 30 # secs
_DEBUG = False
_CLOUDLOG_DEBUG = True
@@ -39,7 +34,6 @@ threading.excepthook = excepthook
class MapD():
def __init__(self):
self.osm = OSM()
self.is_offline = self.osm.is_offline()
self.way_collection = None
self.route = None
self.last_gps_fix_timestamp = 0
@@ -56,20 +50,6 @@ class MapD():
self._disengaging = False
self._query_thread = None
self._lock = threading.RLock()
self._road_name_last = ""
self._road_name_last_timed_out = 0.
# 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 = 'controlsState'
@@ -109,13 +89,13 @@ class MapD():
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 = osm.fetch_road_ways_around_location(lat, lon, radius)
ways = 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 conectivity issue.
# Will retry on next loop.
if len(ways) > 0:
new_way_collection = WayCollection(areas, ways, location_rad, osm.is_offline())
new_way_collection = WayCollection(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)
@@ -131,7 +111,7 @@ class MapD():
return
self._query_thread = threading.Thread(target=query, args=(self.osm, self.location_deg, self.location_rad,
QUERY_RADIUS if self.is_offline else QUERY_RADIUS_OFFLINE))
QUERY_RADIUS))
self._query_thread.start()
def updated_osm_data(self):
@@ -146,7 +126,7 @@ class MapD():
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 if self.is_offline else QUERY_RADIUS_OFFLINE) - MIN_DISTANCE_FOR_NEW_QUERY):
if distance_since_last < QUERY_RADIUS - MIN_DISTANCE_FOR_NEW_QUERY:
# do not query if are still not close to the border of previous query area
return
@@ -218,7 +198,7 @@ class MapD():
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 = "" if self.route.current_road_name is None else str(self.route.current_road_name).strip()
current_road_name = self.route.current_road_name
map_data_msg = messaging.new_message('liveMapData')
map_data_msg.valid = sm.all_alive(service_list=['gpsLocationExternal']) and \
@@ -251,19 +231,7 @@ class MapD():
map_data_msg.liveMapData.turnSpeedLimitsAheadDistances = [float(s.start) for s in next_turn_speed_limit_sections]
map_data_msg.liveMapData.turnSpeedLimitsAheadSigns = [float(s.curv_sign) for s in next_turn_speed_limit_sections]
# dp - cache road name to avoid name display blinking
if current_road_name == "":
sec = sec_since_boot()
if self._road_name_last_timed_out == 0.:
self._road_name_last_timed_out = sec + ROAD_NAME_TIMEOUT
if sec < self._road_name_last_timed_out:
current_road_name = self._road_name_last
else:
self._road_name_last_timed_out = 0.
self._road_name_last = current_road_name
map_data_msg.liveMapData.currentRoadName = current_road_name
map_data_msg.liveMapData.currentRoadName = str(current_road_name if current_road_name is not None else "")
pm.send('liveMapData', map_data_msg)
_debug(f'Mapd *****: Publish: \n{map_data_msg}\n********', log_to_cloud=False)
@@ -271,7 +239,6 @@ class MapD():
# provides live map data information
def mapd_thread(sm=None, pm=None):
config_realtime_process([2], 5)
mapd = MapD()
rk = Ratekeeper(1., print_delay_threshold=None) # Keeps rate at 1 hz