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