You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
240 lines
6.9 KiB
240 lines
6.9 KiB
#!/usr/bin/env python
|
|
|
|
# Add phonelibs openblas to LD_LIBRARY_PATH if import fails
|
|
try:
|
|
from scipy import spatial
|
|
except ImportError as e:
|
|
import os
|
|
import sys
|
|
from common.basedir import BASEDIR
|
|
|
|
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)
|
|
|
|
import time
|
|
import zmq
|
|
import threading
|
|
import numpy as np
|
|
import overpy
|
|
from collections import defaultdict
|
|
|
|
from common.transformations.coordinates import geodetic2ecef
|
|
from selfdrive.services import service_list
|
|
import selfdrive.messaging as messaging
|
|
from mapd_helpers import LOOKAHEAD_TIME, MAPS_LOOKAHEAD_DISTANCE, Way, circle_through_points
|
|
|
|
|
|
OVERPASS_API_URL = "https://overpass.kumi.systems/api/interpreter"
|
|
OVERPASS_HEADERS = {
|
|
'User-Agent': 'NEOS (comma.ai)'
|
|
}
|
|
|
|
last_gps = None
|
|
query_lock = threading.Lock()
|
|
last_query_result = None
|
|
last_query_pos = None
|
|
|
|
|
|
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)
|
|
q = """(
|
|
way
|
|
""" + pos + """
|
|
[highway];
|
|
>;);out;
|
|
"""
|
|
return q
|
|
|
|
|
|
def query_thread():
|
|
global last_query_result, last_query_pos
|
|
api = overpy.Overpass(url=OVERPASS_API_URL, headers=OVERPASS_HEADERS, timeout=10.)
|
|
|
|
while True:
|
|
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:
|
|
continue
|
|
|
|
q = build_way_query(last_gps.latitude, last_gps.longitude, radius=2000)
|
|
try:
|
|
new_result = api.query(q)
|
|
|
|
# Build kd-tree
|
|
nodes = []
|
|
real_nodes = []
|
|
node_to_way = defaultdict(list)
|
|
|
|
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)
|
|
|
|
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
|
|
last_query_pos = last_gps
|
|
query_lock.release()
|
|
|
|
except Exception as e:
|
|
print e
|
|
query_lock.acquire()
|
|
last_query_result = None
|
|
query_lock.release()
|
|
time.sleep(1)
|
|
|
|
|
|
def mapsd_thread():
|
|
global last_gps
|
|
|
|
context = zmq.Context()
|
|
gps_sock = messaging.sub_sock(context, service_list['gpsLocation'].port, conflate=True)
|
|
gps_external_sock = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, conflate=True)
|
|
map_data_sock = messaging.pub_sock(context, service_list['liveMapData'].port)
|
|
|
|
cur_way = None
|
|
curvature_valid = False
|
|
curvature = None
|
|
upcoming_curvature = 0.
|
|
dist_to_turn = 0.
|
|
road_points = None
|
|
|
|
xx = np.arange(0, MAPS_LOOKAHEAD_DISTANCE, 10)
|
|
|
|
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:
|
|
cur_way = None
|
|
curvature = None
|
|
curvature_valid = False
|
|
upcoming_curvature = 0.
|
|
dist_to_turn = 0.
|
|
road_points = None
|
|
else:
|
|
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)
|
|
if cur_way is not None:
|
|
pnts, curvature_valid = cur_way.get_lookahead(last_query_result, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE)
|
|
|
|
xs = pnts[:, 0]
|
|
ys = pnts[:, 1]
|
|
road_points = map(float, xs), map(float, ys)
|
|
|
|
if speed < 10:
|
|
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
|
|
|
|
# TODO: Determine left or right turn
|
|
curvature = np.nan_to_num(curvature)
|
|
curvature_interp = np.interp(xx, dists[1:-1], curvature)
|
|
curvature_lookahead = curvature_interp[:int(speed * LOOKAHEAD_TIME / 10)]
|
|
|
|
# Outlier rejection
|
|
new_curvature = np.percentile(curvature_lookahead, 90)
|
|
|
|
k = 0.9
|
|
upcoming_curvature = k * upcoming_curvature + (1 - k) * new_curvature
|
|
in_turn_indices = curvature_interp > 0.8 * new_curvature
|
|
if np.any(in_turn_indices):
|
|
dist_to_turn = np.min(xx[in_turn_indices])
|
|
else:
|
|
dist_to_turn = 999
|
|
query_lock.release()
|
|
|
|
dat = messaging.new_message()
|
|
dat.init('liveMapData')
|
|
|
|
if last_gps is not None:
|
|
dat.liveMapData.lastGps = last_gps
|
|
|
|
if cur_way is not None:
|
|
dat.liveMapData.wayId = cur_way.id
|
|
|
|
# Seed limit
|
|
max_speed = cur_way.max_speed
|
|
if max_speed is not None:
|
|
dat.liveMapData.speedLimitValid = True
|
|
dat.liveMapData.speedLimit = 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 = map(float, xx)
|
|
dat.liveMapData.roadCurvature = map(float, curvature_interp)
|
|
|
|
map_data_sock.send(dat.to_bytes())
|
|
|
|
|
|
def main(gctx=None):
|
|
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()
|
|
|