Vehicle Researcher
6 years ago
parent
573a6915fc
commit
210db686bb
36 changed files with 825 additions and 159 deletions
Binary file not shown.
Binary file not shown.
@ -0,0 +1 @@ |
|||||||
|
libopenblas_armv8p-r0.2.19.so |
||||||
Binary file not shown.
@ -1 +1 @@ |
|||||||
#define COMMA_VERSION "0.5.6-release" |
#define COMMA_VERSION "0.5.7-release" |
||||||
|
|||||||
Binary file not shown.
@ -0,0 +1,240 @@ |
|||||||
|
#!/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() |
||||||
@ -0,0 +1,223 @@ |
|||||||
|
import math |
||||||
|
import numpy as np |
||||||
|
from datetime import datetime |
||||||
|
|
||||||
|
from selfdrive.config import Conversions as CV |
||||||
|
from common.transformations.coordinates import LocalCoord, geodetic2ecef |
||||||
|
|
||||||
|
LOOKAHEAD_TIME = 10. |
||||||
|
MAPS_LOOKAHEAD_DISTANCE = 50 * LOOKAHEAD_TIME |
||||||
|
|
||||||
|
|
||||||
|
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. """ |
||||||
|
|
||||||
|
conversion = CV.KPH_TO_MS |
||||||
|
if 'mph' in max_speed: |
||||||
|
max_speed = max_speed.replace(' mph', '') |
||||||
|
conversion = CV.MPH_TO_MS |
||||||
|
|
||||||
|
return float(max_speed) * conversion |
||||||
|
|
||||||
|
|
||||||
|
class Way: |
||||||
|
def __init__(self, way): |
||||||
|
self.id = way.id |
||||||
|
self.way = way |
||||||
|
|
||||||
|
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): |
||||||
|
results, tree, real_nodes, node_to_way = 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) |
||||||
|
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) |
||||||
|
if closest_way is None or score < best_score: |
||||||
|
closest_way = way |
||||||
|
best_score = score |
||||||
|
|
||||||
|
return closest_way |
||||||
|
|
||||||
|
def __str__(self): |
||||||
|
return "%s %s" % (self.id, self.way.tags) |
||||||
|
|
||||||
|
@property |
||||||
|
def max_speed(self): |
||||||
|
"""Extracts the (conditional) speed limit from a way""" |
||||||
|
if not self.way: |
||||||
|
return None |
||||||
|
|
||||||
|
tags = self.way.tags |
||||||
|
max_speed = None |
||||||
|
|
||||||
|
if 'maxspeed' in tags: |
||||||
|
max_speed = parse_speed_unit(tags['maxspeed']) |
||||||
|
|
||||||
|
if 'maxspeed:conditional' in tags: |
||||||
|
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 = parse_speed_unit(max_speed_cond) |
||||||
|
|
||||||
|
return max_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, query_results, lat, lon, heading, backwards=False): |
||||||
|
results, tree, real_nodes, node_to_way = 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 and w.tags['highway'] == self.way.tags['highway']] |
||||||
|
if len(ways) == 1: |
||||||
|
way = Way(ways[0]) |
||||||
|
except KeyError: |
||||||
|
pass |
||||||
|
|
||||||
|
return way |
||||||
|
|
||||||
|
def get_lookahead(self, query_results, 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(query_results, lat, lon, heading) |
||||||
|
if not way: |
||||||
|
break |
||||||
|
|
||||||
|
return pnts, valid |
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Loading…
Reference in new issue