Willem Melching
5 years ago
parent
2325580259
commit
f135b7d4df
12 changed files with 0 additions and 776 deletions
@ -1,21 +0,0 @@ |
||||
import time |
||||
import numpy as np |
||||
|
||||
from common.realtime import sec_since_boot |
||||
|
||||
N = 1000 |
||||
|
||||
times = [] |
||||
for i in range(1000): |
||||
t1 = sec_since_boot() |
||||
time.sleep(0.01) |
||||
t2 = sec_since_boot() |
||||
dt = t2 - t1 |
||||
times.append(dt) |
||||
|
||||
|
||||
print("Mean", np.mean(times)) |
||||
print("Max", np.max(times)) |
||||
print("Min", np.min(times)) |
||||
print("Variance", np.var(times)) |
||||
print("STD", np.sqrt(np.var(times))) |
||||
@ -1,48 +0,0 @@ |
||||
#!/usr/bin/env python3 |
||||
import time |
||||
import sys |
||||
import argparse |
||||
import zmq |
||||
import json |
||||
import pyproj |
||||
import numpy as np |
||||
ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84') |
||||
lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84') |
||||
|
||||
import cereal.messaging as messaging |
||||
from cereal.services import service_list |
||||
|
||||
poller = zmq.Poller() |
||||
ll = messaging.sub_sock("liveLocation", poller) |
||||
tll = messaging.sub_sock("testLiveLocation", poller) |
||||
|
||||
l, tl = None, None |
||||
|
||||
lp = time.time() |
||||
|
||||
while 1: |
||||
polld = poller.poll(timeout=1000) |
||||
for sock, mode in polld: |
||||
if mode != zmq.POLLIN: |
||||
continue |
||||
if sock == ll: |
||||
l = messaging.recv_one(sock) |
||||
elif sock == tll: |
||||
tl = messaging.recv_one(sock) |
||||
if l is None or tl is None: |
||||
continue |
||||
|
||||
alt_err = np.abs(l.liveLocation.alt - tl.liveLocation.alt) |
||||
l1 = pyproj.transform(lla, ecef, l.liveLocation.lon, l.liveLocation.lat, l.liveLocation.alt) |
||||
l2 = pyproj.transform(lla, ecef, tl.liveLocation.lon, tl.liveLocation.lat, tl.liveLocation.alt) |
||||
|
||||
al1 = pyproj.transform(lla, ecef, l.liveLocation.lon, l.liveLocation.lat, l.liveLocation.alt) |
||||
al2 = pyproj.transform(lla, ecef, tl.liveLocation.lon, tl.liveLocation.lat, l.liveLocation.alt) |
||||
|
||||
tdiff = np.abs(l.logMonoTime - tl.logMonoTime) / 1e9 |
||||
|
||||
if time.time()-lp > 0.1: |
||||
print("tm: %f mse: %f mse(flat): %f alterr: %f" % (tdiff, np.mean((np.array(l1)-np.array(l2))**2), np.mean((np.array(al1)-np.array(al2))**2), alt_err)) |
||||
lp = time.time() |
||||
|
||||
|
||||
@ -1,89 +0,0 @@ |
||||
#!/usr/bin/env python3 |
||||
import sys |
||||
import time |
||||
|
||||
import matplotlib.pyplot as plt |
||||
import numpy as np |
||||
import cereal.messaging as messaging |
||||
import zmq |
||||
from common.transformations.coordinates import LocalCoord |
||||
from cereal.services import service_list |
||||
|
||||
SCALE = 20. |
||||
|
||||
def mpc_vwr_thread(addr="127.0.0.1"): |
||||
plt.ion() |
||||
fig = plt.figure(figsize=(15, 15)) |
||||
ax = fig.add_subplot(1,1,1) |
||||
ax.set_xlim([-SCALE, SCALE]) |
||||
ax.set_ylim([-SCALE, SCALE]) |
||||
ax.grid(True) |
||||
|
||||
line, = ax.plot([0.0], [0.0], ".b") |
||||
line2, = ax.plot([0.0], [0.0], 'r') |
||||
|
||||
ax.set_aspect('equal', 'datalim') |
||||
plt.show() |
||||
|
||||
live_location = messaging.sub_sock('liveLocation', addr=addr, conflate=True) |
||||
gps_planner_points = messaging.sub_sock('gpsPlannerPoints', conflate=True) |
||||
gps_planner_plan = messaging.sub_sock('gpsPlannerPlan', conflate=True) |
||||
|
||||
last_points = messaging.recv_one(gps_planner_points) |
||||
last_plan = messaging.recv_one(gps_planner_plan) |
||||
while True: |
||||
p = messaging.recv_one_or_none(gps_planner_points) |
||||
pl = messaging.recv_one_or_none(gps_planner_plan) |
||||
ll = messaging.recv_one(live_location).liveLocation |
||||
|
||||
if p is not None: |
||||
last_points = p |
||||
if pl is not None: |
||||
last_plan = pl |
||||
|
||||
if not last_plan.gpsPlannerPlan.valid: |
||||
time.sleep(0.1) |
||||
line2.set_color('r') |
||||
continue |
||||
|
||||
p0 = last_points.gpsPlannerPoints.points[0] |
||||
p0 = np.array([p0.x, p0.y, p0.z]) |
||||
|
||||
n = LocalCoord.from_geodetic(np.array([ll.lat, ll.lon, ll.alt])) |
||||
points = [] |
||||
print(len(last_points.gpsPlannerPoints.points)) |
||||
for p in last_points.gpsPlannerPoints.points: |
||||
ecef = np.array([p.x, p.y, p.z]) |
||||
points.append(n.ecef2ned(ecef)) |
||||
|
||||
points = np.vstack(points) |
||||
line.set_xdata(points[:, 1]) |
||||
line.set_ydata(points[:, 0]) |
||||
|
||||
y = np.matrix(np.arange(-100, 100.0, 0.5)) |
||||
x = -np.matrix(np.polyval(last_plan.gpsPlannerPlan.poly, y)) |
||||
xy = np.hstack([x.T, y.T]) |
||||
|
||||
cur_heading = np.radians(ll.heading - 90) |
||||
c, s = np.cos(cur_heading), np.sin(cur_heading) |
||||
R = np.array([[c, -s], [s, c]]) |
||||
xy = xy.dot(R) |
||||
|
||||
line2.set_xdata(xy[:, 1]) |
||||
line2.set_ydata(-xy[:, 0]) |
||||
line2.set_color('g') |
||||
|
||||
|
||||
ax.set_xlim([-SCALE, SCALE]) |
||||
ax.set_ylim([-SCALE, SCALE]) |
||||
|
||||
fig.canvas.draw() |
||||
fig.canvas.flush_events() |
||||
|
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
if len(sys.argv) > 1: |
||||
mpc_vwr_thread(sys.argv[1]) |
||||
else: |
||||
mpc_vwr_thread() |
||||
@ -1,16 +0,0 @@ |
||||
#!/usr/bin/env python3 |
||||
import time |
||||
import cereal.messaging as messaging |
||||
|
||||
|
||||
def init_message_bench(N=100000): |
||||
t = time.time() |
||||
for _ in range(N): |
||||
dat = messaging.new_message('controlsState') |
||||
|
||||
dt = time.time() - t |
||||
print("Init message %d its, %f s" % (N, dt)) |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
init_message_bench() |
||||
@ -1,41 +0,0 @@ |
||||
#!/usr/bin/env python3 |
||||
import zmq |
||||
|
||||
import cereal.messaging as messaging |
||||
from cereal.services import service_list |
||||
|
||||
if __name__ == "__main__": |
||||
poller = zmq.Poller() |
||||
|
||||
fsock = messaging.sub_sock("frame", poller) |
||||
msock = messaging.sub_sock("model", poller) |
||||
|
||||
frmTimes = {} |
||||
proc = [] |
||||
|
||||
last100 = [] |
||||
|
||||
while 1: |
||||
polld = poller.poll(timeout=1000) |
||||
for sock, mode in polld: |
||||
if mode != zmq.POLLIN: |
||||
continue |
||||
if sock == fsock: |
||||
f = messaging.recv_one(sock) |
||||
frmTimes[f.frame.frameId] = f.frame.timestampEof |
||||
else: |
||||
proc.append(messaging.recv_one(sock)) |
||||
nproc = [] |
||||
for mm in proc: |
||||
fid = mm.model.frameId |
||||
|
||||
if fid in frmTimes: |
||||
tm = (mm.logMonoTime-frmTimes[fid])/1e6 |
||||
del frmTimes[fid] |
||||
last100.append(tm) |
||||
last100 = last100[-100:] |
||||
print("%10d: %.2f ms min: %.2f max: %.2f" % (fid, tm, min(last100), max(last100))) |
||||
else: |
||||
nproc.append(mm) |
||||
proc = nproc |
||||
|
||||
@ -1,96 +0,0 @@ |
||||
import timeit |
||||
|
||||
import numpy as np |
||||
import numpy.linalg |
||||
from scipy.linalg import cho_factor, cho_solve |
||||
|
||||
# We are trying to solve the following system |
||||
# (A.T * A) * x = A.T * b |
||||
# Where x are the polynomial coefficients and b is are the input points |
||||
|
||||
# First we build A |
||||
deg = 3 |
||||
x = np.arange(50 * 1.0) |
||||
A = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T |
||||
|
||||
# The first way to solve this is using the pseudoinverse, which can be precomputed |
||||
# x = (A.T * A)^-1 * A^T * b = PINV b |
||||
PINV = np.linalg.pinv(A) |
||||
|
||||
# Another way is using the Cholesky decomposition |
||||
# We can note that at (A.T * A) is always positive definite |
||||
# By precomputing the Cholesky decomposition we can efficiently solve |
||||
# systems of the form (A.T * A) x = c |
||||
CHO = cho_factor(np.dot(A.T, A)) |
||||
|
||||
|
||||
def model_polyfit_old(points, deg=3): |
||||
A = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T |
||||
pinv = np.linalg.pinv(A) |
||||
return np.dot(pinv, map(float, points)) |
||||
|
||||
|
||||
def model_polyfit(points, deg=3): |
||||
A = np.vander(x, deg + 1) |
||||
pinv = np.linalg.pinv(A) |
||||
return np.dot(pinv, map(float, points)) |
||||
|
||||
|
||||
def model_polyfit_cho(points, deg=3): |
||||
A = np.vander(x, deg + 1) |
||||
cho = cho_factor(np.dot(A.T, A)) |
||||
c = np.dot(A.T, points) |
||||
return cho_solve(cho, c, check_finite=False) |
||||
|
||||
|
||||
def model_polyfit_np(points, deg=3): |
||||
return np.polyfit(x, points, deg) |
||||
|
||||
|
||||
def model_polyfit_lstsq(points, deg=3): |
||||
A = np.vander(x, deg + 1) |
||||
return np.linalg.lstsq(A, points, rcond=None)[0] |
||||
|
||||
|
||||
TEST_DATA = np.linspace(0, 5, num=50) + 1. |
||||
|
||||
|
||||
def time_pinv_old(): |
||||
model_polyfit_old(TEST_DATA) |
||||
|
||||
|
||||
def time_pinv(): |
||||
model_polyfit(TEST_DATA) |
||||
|
||||
|
||||
def time_cho(): |
||||
model_polyfit_cho(TEST_DATA) |
||||
|
||||
|
||||
def time_np(): |
||||
model_polyfit_np(TEST_DATA) |
||||
|
||||
|
||||
def time_lstsq(): |
||||
model_polyfit_lstsq(TEST_DATA) |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
# Verify correct results |
||||
pinv_old = model_polyfit_old(TEST_DATA) |
||||
pinv = model_polyfit(TEST_DATA) |
||||
cho = model_polyfit_cho(TEST_DATA) |
||||
numpy = model_polyfit_np(TEST_DATA) |
||||
lstsq = model_polyfit_lstsq(TEST_DATA) |
||||
|
||||
assert all(np.isclose(pinv, pinv_old)) |
||||
assert all(np.isclose(pinv, cho)) |
||||
assert all(np.isclose(pinv, numpy)) |
||||
assert all(np.isclose(pinv, lstsq)) |
||||
|
||||
# Run benchmark |
||||
print("Pseudo inverse (old)", timeit.timeit("time_pinv_old()", setup="from __main__ import time_pinv_old", number=10000)) |
||||
print("Pseudo inverse", timeit.timeit("time_pinv()", setup="from __main__ import time_pinv", number=10000)) |
||||
print("Cholesky", timeit.timeit("time_cho()", setup="from __main__ import time_cho", number=10000)) |
||||
print("Numpy leastsq", timeit.timeit("time_lstsq()", setup="from __main__ import time_lstsq", number=10000)) |
||||
print("Numpy polyfit", timeit.timeit("time_np()", setup="from __main__ import time_np", number=10000)) |
||||
@ -1,22 +0,0 @@ |
||||
#!/usr/bin/env python3 |
||||
import time |
||||
|
||||
from common.realtime import sec_since_boot, monotonic_time |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
N = 100000 |
||||
|
||||
t = time.time() |
||||
for _ in range(N): |
||||
monotonic_time() |
||||
dt = time.time() - t |
||||
|
||||
print("Monotonic", dt) |
||||
|
||||
t = time.time() |
||||
for _ in range(N): |
||||
sec_since_boot() |
||||
dt = time.time() - t |
||||
|
||||
print("Boot", dt) |
||||
@ -1,21 +0,0 @@ |
||||
#!/usr/bin/env python3 |
||||
import time |
||||
import zmq |
||||
from hexdump import hexdump |
||||
|
||||
import cereal.messaging as messaging |
||||
from cereal.services import service_list |
||||
|
||||
if __name__ == "__main__": |
||||
controls_state = messaging.pub_sock('controlsState') |
||||
|
||||
while 1: |
||||
dat = messaging.new_message('controlsState') |
||||
|
||||
dat.controlsState.alertText1 = "alert text 1" |
||||
dat.controlsState.alertText2 = "alert text 2" |
||||
dat.controlsState.alertType = "test" |
||||
dat.controlsState.alertSound = "chimeDisengage" |
||||
controls_state.send(dat.to_bytes()) |
||||
|
||||
time.sleep(0.01) |
||||
@ -1,107 +0,0 @@ |
||||
import os |
||||
import sys |
||||
|
||||
import zmq |
||||
from lru import LRU |
||||
|
||||
from cereal import log |
||||
from common.realtime import Ratekeeper |
||||
import cereal.messaging as messaging |
||||
from cereal.services import service_list |
||||
|
||||
def cputime_total(ct): |
||||
return ct.user+ct.nice+ct.system+ct.idle+ct.iowait+ct.irq+ct.softirq |
||||
|
||||
def cputime_busy(ct): |
||||
return ct.user+ct.nice+ct.system+ct.irq+ct.softirq |
||||
|
||||
def cpu_dtotal(l1, l2): |
||||
t1_total = sum(cputime_total(ct) for ct in l1.cpuTimes) |
||||
t2_total = sum(cputime_total(ct) for ct in l2.cpuTimes) |
||||
return t2_total - t1_total |
||||
|
||||
def cpu_percent(l1, l2): |
||||
dtotal = cpu_dtotal(l1, l2) |
||||
t1_busy = sum(cputime_busy(ct) for ct in l1.cpuTimes) |
||||
t2_busy = sum(cputime_busy(ct) for ct in l2.cpuTimes) |
||||
|
||||
dbusy = t2_busy - t1_busy |
||||
|
||||
if dbusy < 0 or dtotal <= 0: |
||||
return 0.0 |
||||
return dbusy / dtotal |
||||
|
||||
def proc_cpu_percent(proc1, proc2, l1, l2): |
||||
dtotal = cpu_dtotal(l1, l2) |
||||
|
||||
dproc = (proc2.cpuUser+proc2.cpuSystem) - (proc1.cpuUser+proc1.cpuSystem) |
||||
if dproc < 0: |
||||
return 0.0 |
||||
|
||||
return dproc / dtotal |
||||
|
||||
def display_cpu(pl1, pl2): |
||||
l1, l2 = pl1.procLog, pl2.procLog |
||||
|
||||
print(cpu_percent(l1, l2)) |
||||
|
||||
procs1 = dict((proc.pid, proc) for proc in l1.procs) |
||||
procs2 = dict((proc.pid, proc) for proc in l2.procs) |
||||
|
||||
procs_print = 4 |
||||
|
||||
procs_with_percent = sorted((proc_cpu_percent(procs1[proc.pid], proc, l1, l2), proc) for proc in l2.procs |
||||
if proc.pid in procs1) |
||||
for percent, proc in procs_with_percent[-1:-procs_print-1:-1]: |
||||
print(percent, proc.name) |
||||
|
||||
print() |
||||
|
||||
|
||||
def main(): |
||||
frame_cache = LRU(16) |
||||
md_cache = LRU(16) |
||||
plan_cache = LRU(16) |
||||
|
||||
frame_sock = messaging.sub_sock('frame') |
||||
md_sock = messaging.sub_sock('model') |
||||
plan_sock = messaging.sub_sock('plan') |
||||
controls_state_sock = messaging.sub_sock('controlsState') |
||||
|
||||
proc = messaging.sub_sock('procLog') |
||||
pls = [None, None] |
||||
|
||||
rk = Ratekeeper(10) |
||||
while True: |
||||
|
||||
for msg in messaging.drain_sock(frame_sock): |
||||
frame_cache[msg.frame.frameId] = msg |
||||
|
||||
for msg in messaging.drain_sock(md_sock): |
||||
md_cache[msg.logMonoTime] = msg |
||||
|
||||
for msg in messaging.drain_sock(plan_sock): |
||||
plan_cache[msg.logMonoTime] = msg |
||||
|
||||
controls_state = messaging.recv_sock(controls_state_sock) |
||||
if controls_state is not None: |
||||
plan_time = controls_state.controlsState.planMonoTime |
||||
if plan_time != 0 and plan_time in plan_cache: |
||||
plan = plan_cache[plan_time] |
||||
md_time = plan.plan.mdMonoTime |
||||
if md_time != 0 and md_time in md_cache: |
||||
md = md_cache[md_time] |
||||
frame_id = md.model.frameId |
||||
if frame_id != 0 and frame_id in frame_cache: |
||||
frame = frame_cache[frame_id] |
||||
print("controls lag: %.2fms" % ((controls_state.logMonoTime - frame.frame.timestampEof) / 1e6)) |
||||
|
||||
|
||||
pls = (pls+messaging.drain_sock(proc))[-2:] |
||||
if None not in pls: |
||||
display_cpu(*pls) |
||||
|
||||
rk.keep_time() |
||||
|
||||
if __name__ == "__main__": |
||||
main() |
||||
@ -1,83 +0,0 @@ |
||||
import numpy as np |
||||
import matplotlib.pyplot as plt |
||||
from mpl_toolkits.mplot3d import Axes3D |
||||
from matplotlib import cm |
||||
from matplotlib.ticker import LinearLocator, FormatStrFormatter |
||||
from scipy.optimize import minimize |
||||
|
||||
a = -9.81 |
||||
dt = 0.1 |
||||
|
||||
r = 2.0 |
||||
|
||||
v_ls = [] |
||||
x_ls = [] |
||||
v_egos = [] |
||||
|
||||
for vv_ego in np.arange(35, 40, 1): |
||||
for vv_l in np.arange(35, 40, 1): |
||||
for xx_l in np.arange(0, 100, 1.0): |
||||
x_l = xx_l |
||||
v_l = vv_l |
||||
v_ego = vv_ego |
||||
x_ego = 0.0 |
||||
|
||||
ttc = None |
||||
for t in np.arange(0, 100, dt): |
||||
x_l += v_l * dt |
||||
v_l += a * dt |
||||
v_l = max(v_l, 0.0) |
||||
|
||||
x_ego += v_ego * dt |
||||
if t > r: |
||||
v_ego += a * dt |
||||
v_ego = max(v_ego, 0.0) |
||||
|
||||
if x_ego >= x_l: |
||||
ttc = t |
||||
break |
||||
|
||||
if ttc is None: |
||||
if xx_l < 0.1: |
||||
break |
||||
|
||||
v_ls.append(vv_l) |
||||
x_ls.append(xx_l) |
||||
v_egos.append(vv_ego) |
||||
break |
||||
|
||||
|
||||
def eval_f(x, v_ego, v_l): |
||||
est = x[0] * v_l + x[1] * v_l**2 \ |
||||
+ x[2] * v_ego + x[3] * v_ego**2 |
||||
return est |
||||
|
||||
def f(x): |
||||
r = 0.0 |
||||
for v_ego, v_l, x_l in zip(v_egos, v_ls, x_ls): |
||||
est = eval_f(x, v_ego, v_l) |
||||
r += (x_l - est)**2 |
||||
|
||||
return r |
||||
|
||||
x0 = [0.5, 0.5, 0.5, 0.5] |
||||
res = minimize(f, x0, method='Nelder-Mead') |
||||
print(res) |
||||
print(res.x) |
||||
|
||||
g = 9.81 |
||||
t_r = 1.8 |
||||
|
||||
estimated = [4.0 + eval_f(res.x, v_ego, v_l) for (v_ego, v_l) in zip(v_egos, v_ls)] |
||||
new_formula = [4.0 + v_ego * t_r - (v_l - v_ego) * t_r + v_ego**2/(2*g) - v_l**2 / (2*g) for (v_ego, v_l) in zip(v_egos, v_ls)] |
||||
|
||||
fig = plt.figure() |
||||
ax = fig.add_subplot(111, projection='3d') |
||||
surf = ax.scatter(v_egos, v_ls, x_ls, s=1) |
||||
# surf = ax.scatter(v_egos, v_ls, estimated, s=1) |
||||
surf = ax.scatter(v_egos, v_ls, new_formula, s=1) |
||||
|
||||
ax.set_xlabel('v ego') |
||||
ax.set_ylabel('v lead') |
||||
ax.set_zlabel('min distance') |
||||
plt.show() |
||||
@ -1,178 +0,0 @@ |
||||
#!/usr/bin/env python3 |
||||
import sys |
||||
import math |
||||
import pygame |
||||
import pyproj |
||||
|
||||
import zmq |
||||
import cereal.messaging as messaging |
||||
from cereal.services import service_list |
||||
import numpy as np |
||||
|
||||
METER = 25 |
||||
YSCALE = 1 |
||||
|
||||
def to_grid(pt): |
||||
return (int(round(pt[0] * METER + 100)), int(round(pt[1] * METER * YSCALE + 500))) |
||||
|
||||
def gps_latlong_to_meters(gps_values, zero): |
||||
inProj = pyproj.Proj(init='epsg:4326') |
||||
outProj = pyproj.Proj(("+proj=tmerc +lat_0={:f} +lon_0={:f} +units=m" |
||||
" +k=1. +x_0=0 +y_0=0 +ellps=WGS84 +datum=WGS84 +no_defs" |
||||
"+towgs84=-90.7,-106.1,-119.2,4.09,0.218,-1.05,1.37").format(*zero)) |
||||
gps_x, gps_y = pyproj.transform(inProj, outProj, gps_values[1], gps_values[0]) |
||||
return gps_x, gps_y |
||||
|
||||
def rot(hrad): |
||||
return [[math.cos(hrad), -math.sin(hrad)], |
||||
[math.sin(hrad), math.cos(hrad)]] |
||||
|
||||
class Car(): |
||||
CAR_WIDTH = 2.0 |
||||
CAR_LENGTH = 4.5 |
||||
|
||||
def __init__(self, c): |
||||
self.car = pygame.Surface((METER*self.CAR_LENGTH*YSCALE, METER*self.CAR_LENGTH)) |
||||
self.car.set_alpha(64) |
||||
self.car.fill((0,0,0)) |
||||
self.car.set_colorkey((0,0,0)) |
||||
pygame.draw.rect(self.car, c, (METER*1.25*YSCALE, 0, METER*self.CAR_WIDTH*YSCALE, METER*self.CAR_LENGTH), 1) |
||||
|
||||
self.x = 0.0 |
||||
self.y = 0.0 |
||||
self.heading = 0.0 |
||||
|
||||
def from_car_frame(self, pts): |
||||
ret = [] |
||||
for x, y in pts: |
||||
rx, ry = np.dot(rot(math.radians(self.heading)), [x,y]) |
||||
ret.append((self.x + rx, self.y + ry)) |
||||
return ret |
||||
|
||||
def draw(self, screen): |
||||
cars = pygame.transform.rotate(self.car, 90-self.heading) |
||||
pt = (self.x - self.CAR_LENGTH/2, self.y - self.CAR_LENGTH/2) |
||||
screen.blit(cars, to_grid(pt)) |
||||
|
||||
|
||||
def ui_thread(addr="127.0.0.1"): |
||||
#from selfdrive.radar.nidec.interface import RadarInterface |
||||
#RI = RadarInterface() |
||||
|
||||
pygame.display.set_caption("comma top down UI") |
||||
size = (1920,1000) |
||||
screen = pygame.display.set_mode(size, pygame.DOUBLEBUF) |
||||
|
||||
liveLocation = messaging.sub_sock('liveLocation', addr=addr) |
||||
|
||||
#model = messaging.sub_sock('testModel', addr=addr) |
||||
model = messaging.sub_sock('model', addr=addr) |
||||
|
||||
plan = messaging.sub_sock('plan', addr=addr) |
||||
frame = messaging.sub_sock('frame', addr=addr) |
||||
liveTracks = messaging.sub_sock('liveTracks', addr=addr) |
||||
|
||||
car = Car((255,0,255)) |
||||
|
||||
base = None |
||||
|
||||
lb = [] |
||||
|
||||
ts_map = {} |
||||
|
||||
while 1: |
||||
lloc = messaging.recv_sock(liveLocation, wait=True) |
||||
lloc_ts = lloc.logMonoTime |
||||
lloc = lloc.liveLocation |
||||
|
||||
# 50 ms of lag |
||||
lb.append(lloc) |
||||
if len(lb) < 2: |
||||
continue |
||||
lb = lb[-1:] |
||||
|
||||
lloc = lb[0] |
||||
|
||||
# spacebar reset |
||||
for event in pygame.event.get(): |
||||
if event.type == pygame.KEYDOWN and event.key == pygame.K_SPACE: |
||||
base = None |
||||
|
||||
# offscreen reset |
||||
rp = to_grid((car.x, car.y)) |
||||
if rp[0] > (size[0] - 100) or rp[1] > (size[1] - 100) or rp[0] < 0 or rp[1] < 100: |
||||
base = None |
||||
|
||||
|
||||
if base == None: |
||||
screen.fill((10,10,10)) |
||||
base = lloc |
||||
|
||||
# transform pt into local |
||||
pt = gps_latlong_to_meters((lloc.lat, lloc.lon), (base.lat, base.lon)) |
||||
hrad = math.radians(270+base.heading) |
||||
pt = np.dot(rot(hrad), pt) |
||||
|
||||
car.x, car.y = pt[0], -pt[1] |
||||
car.heading = lloc.heading - base.heading |
||||
|
||||
#car.draw(screen) |
||||
pygame.draw.circle(screen, (192,64,192,128), to_grid((car.x, car.y)), 4) |
||||
|
||||
""" |
||||
lt = messaging.recv_sock(liveTracks, wait=False) |
||||
if lt is not None: |
||||
for track in lt.liveTracks: |
||||
pt = car.from_car_frame([[track.dRel, -track.yRel]])[0] |
||||
if track.stationary: |
||||
pygame.draw.circle(screen, (192,128,32,64), to_grid(pt), 1) |
||||
""" |
||||
|
||||
|
||||
""" |
||||
rr = RI.update() |
||||
for pt in rr.points: |
||||
cpt = car.from_car_frame([[pt.dRel + 2.7, -pt.yRel]])[0] |
||||
if (pt.vRel + lloc.speed) < 1.0: |
||||
pygame.draw.circle(screen, (192,128,32,64), to_grid(cpt), 1) |
||||
""" |
||||
|
||||
|
||||
for f in messaging.drain_sock(frame): |
||||
ts_map[f.frame.frameId] = f.frame.timestampEof |
||||
|
||||
def draw_model_data(mm, c): |
||||
pts = car.from_car_frame(zip(np.arange(0.0, 50.0), -np.array(mm))) |
||||
lt = 255 |
||||
for pt in pts: |
||||
screen.set_at(to_grid(pt), (c[0]*lt,c[1]*lt,c[2]*lt,lt)) |
||||
lt -= 2 |
||||
#pygame.draw.lines(screen, (c[0]*lt,c[1]*lt,c[2]*lt,lt), False, map(to_grid, pts), 1) |
||||
|
||||
md = messaging.recv_sock(model, wait=False) |
||||
if md: |
||||
if md.model.frameId in ts_map: |
||||
f_ts = ts_map[md.model.frameId] |
||||
print((lloc_ts - f_ts) * 1e-6,"ms") |
||||
|
||||
#draw_model_data(md.model.path.points, (1,0,0)) |
||||
if md.model.leftLane.prob > 0.3: |
||||
draw_model_data(md.model.leftLane.points, (0,1,0)) |
||||
if md.model.rightLane.prob > 0.3: |
||||
draw_model_data(md.model.rightLane.points, (0,1,0)) |
||||
#if md.model.leftLane.prob > 0.3 and md.model.rightLane.prob > 0.3: |
||||
# draw_model_data([(x+y)/2 for x,y in zip(md.model.leftLane.points, md.model.rightLane.points)], (1,1,0)) |
||||
|
||||
tplan = messaging.recv_sock(plan, wait=False) |
||||
if tplan: |
||||
pts = np.polyval(tplan.plan.dPoly, np.arange(0.0, 50.0)) |
||||
draw_model_data(pts, (1,1,1)) |
||||
|
||||
pygame.display.flip() |
||||
|
||||
if __name__ == "__main__": |
||||
if len(sys.argv) > 1: |
||||
ui_thread(sys.argv[1]) |
||||
else: |
||||
ui_thread() |
||||
|
||||
@ -1,54 +0,0 @@ |
||||
#!/usr/bin/env python3 |
||||
import numpy as np |
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel, calc_slip_factor |
||||
from selfdrive.car.honda.interface import CarInterface |
||||
|
||||
def mpc_path_prediction(sa, u, psi_0, dt, VM): |
||||
# sa and u needs to be numpy arrays |
||||
sa_w = sa * np.pi / 180. / VM.CP.steerRatio |
||||
x = np.zeros(len(sa)) |
||||
y = np.zeros(len(sa)) |
||||
psi = np.ones(len(sa)) * psi_0 |
||||
|
||||
for i in range(0, len(sa)-1): |
||||
x[i+1] = x[i] + np.cos(psi[i]) * u[i] * dt |
||||
y[i+1] = y[i] + np.sin(psi[i]) * u[i] * dt |
||||
psi[i+1] = psi[i] + sa_w[i] * u[i] * dt * VM.curvature_factor(u[i]) |
||||
|
||||
return x, y, psi |
||||
|
||||
|
||||
def model_path_prediction(sa, u, psi_0, dt, VM): |
||||
# steady state solution |
||||
sa_r = sa * np.pi / 180. |
||||
x = np.zeros(len(sa)) |
||||
y = np.zeros(len(sa)) |
||||
psi = np.ones(len(sa)) * psi_0 |
||||
for i in range(0, len(sa)-1): |
||||
|
||||
out = VM.steady_state_sol(sa_r[i], u[i]) |
||||
|
||||
x[i+1] = x[i] + np.cos(psi[i]) * u[i] * dt - np.sin(psi[i]) * out[0] * dt |
||||
y[i+1] = y[i] + np.sin(psi[i]) * u[i] * dt + np.cos(psi[i]) * out[0] * dt |
||||
psi[i+1] = psi[i] + out[1] * dt |
||||
|
||||
return x, y, psi |
||||
|
||||
if __name__ == "__main__": |
||||
CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING") |
||||
print(CP) |
||||
VM = VehicleModel(CP) |
||||
print(VM.steady_state_sol(.1, 0.15)) |
||||
print(calc_slip_factor(VM)) |
||||
print("Curv", VM.curvature_factor(30.)) |
||||
|
||||
dt = 0.05 |
||||
st = 20 |
||||
u = np.ones(st) * 1. |
||||
sa = np.ones(st) * 1. |
||||
|
||||
out = mpc_path_prediction(sa, u, dt, VM) |
||||
out_model = model_path_prediction(sa, u, dt, VM) |
||||
|
||||
print("mpc", out) |
||||
print("model", out_model) |
||||
Loading…
Reference in new issue