mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-06-24 23:42:05 +08:00
remove more unused debug scripts
This commit is contained in:
Willem Melching
@@ -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)
|
||||
Reference in New Issue
Block a user