#!/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()