mirror of
https://github.com/MoreTore/openpilot.git
synced 2026-09-14 19:44:12 +08:00
openpilot v0.6.5 release
This commit is contained in:
Vehicle Researcher
@@ -1,9 +1,11 @@
|
||||
#!/usr/bin/env python
|
||||
#!/usr/bin/env python3
|
||||
import binascii
|
||||
import os
|
||||
import sys
|
||||
from collections import defaultdict
|
||||
from common.realtime import sec_since_boot
|
||||
|
||||
import selfdrive.messaging as messaging
|
||||
from common.realtime import sec_since_boot
|
||||
from selfdrive.services import service_list
|
||||
|
||||
|
||||
@@ -24,9 +26,9 @@ def can_printer(bus=0, max_msg=None, addr="127.0.0.1"):
|
||||
if sec_since_boot() - lp > 0.1:
|
||||
dd = chr(27) + "[2J"
|
||||
dd += "%5.2f\n" % (sec_since_boot() - start)
|
||||
for k,v in sorted(zip(msgs.keys(), map(lambda x: x[-1].encode("hex"), msgs.values()))):
|
||||
for k,v in sorted(zip(msgs.keys(), map(lambda x: binascii.hexlify(x[-1]), msgs.values()))):
|
||||
if max_msg is None or k < max_msg:
|
||||
dd += "%s(%6d) %s\n" % ("%04X(%4d)" % (k,k),len(msgs[k]), v)
|
||||
dd += "%s(%6d) %s\n" % ("%04X(%4d)" % (k,k),len(msgs[k]), v.decode('ascii'))
|
||||
print(dd)
|
||||
lp = sec_since_boot()
|
||||
|
||||
@@ -39,4 +41,3 @@ if __name__ == "__main__":
|
||||
can_printer(int(sys.argv[1]))
|
||||
else:
|
||||
can_printer()
|
||||
|
||||
|
||||
Executable
+18
@@ -0,0 +1,18 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
# put 2 fingeprints and print the diffs
|
||||
f1 = {
|
||||
168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8
|
||||
}
|
||||
|
||||
f2 = {
|
||||
168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8
|
||||
}
|
||||
|
||||
for k in f1:
|
||||
if k not in f2 or f1[k] != f2[k]:
|
||||
print(k, "not in f2")
|
||||
|
||||
for k in f2:
|
||||
if k not in f1 or f2[k] != f1[k]:
|
||||
print(k, "not in f1")
|
||||
@@ -1,4 +1,4 @@
|
||||
#!/usr/bin/env python2
|
||||
#!/usr/bin/env python3
|
||||
import psutil
|
||||
import time
|
||||
import os
|
||||
@@ -94,7 +94,7 @@ if __name__ == "__main__":
|
||||
for k, stat in stats.items():
|
||||
if len(stat['cpu_samples']) <= 0:
|
||||
continue
|
||||
for name, samples in stat['cpu_samples'].iteritems():
|
||||
for name, samples in stat['cpu_samples'].items():
|
||||
samples = np.array(samples)
|
||||
avg = samples.mean()
|
||||
c = samples.size
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
#!/usr/bin/env python2
|
||||
#!/usr/bin/env python3
|
||||
import sys
|
||||
import argparse
|
||||
import zmq
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
#!/usr/bin/env python
|
||||
#!/usr/bin/env python3
|
||||
|
||||
# simple script to get a vehicle fingerprint.
|
||||
|
||||
|
||||
@@ -1,26 +0,0 @@
|
||||
CC = clang
|
||||
CXX = clang++
|
||||
|
||||
WARN_FLAGS = -Werror=implicit-function-declaration \
|
||||
-Werror=incompatible-pointer-types \
|
||||
-Werror=int-conversion \
|
||||
-Werror=return-type \
|
||||
-Werror=format-extra-args
|
||||
|
||||
CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS)
|
||||
|
||||
.PHONY: all
|
||||
all: libvisionipc.so
|
||||
|
||||
visionipc.o: ../../common/visionipc.c ../../common/visionipc.h
|
||||
@echo "[ CC ] $@"
|
||||
$(CC) $(CFLAGS) -MMD \
|
||||
-I../.. -I../../.. \
|
||||
-c -o '$@' ../../common/visionipc.c
|
||||
|
||||
libvisionipc.so: visionipc.o
|
||||
$(CC) -shared -fPIC -o '$@' visionipc.o
|
||||
|
||||
.PHONY: clean
|
||||
clean:
|
||||
rm visionipc.o libvisionipc.so
|
||||
@@ -1,101 +0,0 @@
|
||||
#!/usr/bin/env python
|
||||
import os
|
||||
import subprocess
|
||||
from cffi import FFI
|
||||
|
||||
import numpy as np
|
||||
|
||||
gf_dir = os.path.dirname(os.path.abspath(__file__))
|
||||
|
||||
subprocess.check_call(["make"], cwd=gf_dir)
|
||||
|
||||
|
||||
ffi = FFI()
|
||||
ffi.cdef("""
|
||||
|
||||
typedef enum VisionStreamType {
|
||||
VISION_STREAM_RGB_BACK,
|
||||
VISION_STREAM_RGB_FRONT,
|
||||
VISION_STREAM_YUV,
|
||||
VISION_STREAM_YUV_FRONT,
|
||||
VISION_STREAM_MAX,
|
||||
} VisionStreamType;
|
||||
|
||||
typedef struct VisionUIInfo {
|
||||
int big_box_x, big_box_y;
|
||||
int big_box_width, big_box_height;
|
||||
int transformed_width, transformed_height;
|
||||
|
||||
int front_box_x, front_box_y;
|
||||
int front_box_width, front_box_height;
|
||||
} VisionUIInfo;
|
||||
|
||||
typedef struct VisionStreamBufs {
|
||||
VisionStreamType type;
|
||||
|
||||
int width, height, stride;
|
||||
size_t buf_len;
|
||||
|
||||
union {
|
||||
VisionUIInfo ui_info;
|
||||
} buf_info;
|
||||
} VisionStreamBufs;
|
||||
|
||||
typedef struct VIPCBuf {
|
||||
int fd;
|
||||
size_t len;
|
||||
void* addr;
|
||||
} VIPCBuf;
|
||||
|
||||
typedef struct VIPCBufExtra {
|
||||
// only for yuv
|
||||
uint32_t frame_id;
|
||||
uint64_t timestamp_eof;
|
||||
} VIPCBufExtra;
|
||||
|
||||
typedef struct VisionStream {
|
||||
int ipc_fd;
|
||||
int last_idx;
|
||||
int last_type;
|
||||
int num_bufs;
|
||||
VisionStreamBufs bufs_info;
|
||||
VIPCBuf *bufs;
|
||||
} VisionStream;
|
||||
|
||||
int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, VisionStreamBufs *out_bufs_info);
|
||||
VIPCBuf* visionstream_get(VisionStream *s, VIPCBufExtra *out_extra);
|
||||
void visionstream_destroy(VisionStream *s);
|
||||
|
||||
"""
|
||||
)
|
||||
|
||||
clib = ffi.dlopen(os.path.join(gf_dir, "libvisionipc.so"))
|
||||
|
||||
|
||||
def getframes(front=False):
|
||||
s = ffi.new("VisionStream*")
|
||||
buf_info = ffi.new("VisionStreamBufs*")
|
||||
|
||||
if front:
|
||||
stream_type = clib.VISION_STREAM_RGB_FRONT
|
||||
else:
|
||||
stream_type = clib.VISION_STREAM_RGB_BACK
|
||||
|
||||
err = clib.visionstream_init(s, stream_type, True, buf_info)
|
||||
assert err == 0
|
||||
|
||||
w = buf_info.width
|
||||
h = buf_info.height
|
||||
assert buf_info.stride == w*3
|
||||
assert buf_info.buf_len == w*h*3
|
||||
|
||||
while True:
|
||||
buf = clib.visionstream_get(s, ffi.NULL)
|
||||
|
||||
pbuf = ffi.buffer(buf.addr, buf.len)
|
||||
yield np.frombuffer(pbuf, dtype=np.uint8).reshape((h, w, 3))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
for buf in getframes():
|
||||
print("{0} {1}".format(buf.shape, buf[101, 101]))
|
||||
Executable
+49
@@ -0,0 +1,49 @@
|
||||
#!/usr/bin/env python3
|
||||
import numpy as np
|
||||
|
||||
from selfdrive.messaging import SubMaster
|
||||
|
||||
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
|
||||
|
||||
|
||||
|
||||
sm = SubMaster(['thermal', 'procLog'])
|
||||
|
||||
last_temp = 0.0
|
||||
total_times = [0., 0., 0., 0.]
|
||||
busy_times = [0., 0., 0.0, 0.]
|
||||
|
||||
|
||||
while True:
|
||||
sm.update()
|
||||
|
||||
if sm.updated['thermal']:
|
||||
t = sm['thermal']
|
||||
last_temp = np.mean([t.cpu0, t.cpu1, t.cpu2, t.cpu3]) / 10.
|
||||
|
||||
if sm.updated['procLog']:
|
||||
m = sm['procLog']
|
||||
|
||||
cores = [0., 0., 0., 0.]
|
||||
total_times_new = [0., 0., 0., 0.]
|
||||
busy_times_new = [0., 0., 0.0, 0.]
|
||||
|
||||
for c in m.cpuTimes:
|
||||
n = c.cpuNum
|
||||
total_times_new[n] = cputime_total(c)
|
||||
busy_times_new[n] = cputime_busy(c)
|
||||
|
||||
for n in range(4):
|
||||
t_busy = busy_times_new[n] - busy_times[n]
|
||||
t_total = total_times_new[n] - total_times[n]
|
||||
cores[n] = t_busy / t_total
|
||||
|
||||
total_times = total_times_new[:]
|
||||
busy_times = busy_times_new[:]
|
||||
|
||||
print("CPU %.2f%% - Temp %.2f" % (100. * np.mean(cores), last_temp ))
|
||||
Executable
+111
@@ -0,0 +1,111 @@
|
||||
#!/usr/bin/env python3
|
||||
import matplotlib
|
||||
matplotlib.use('TkAgg')
|
||||
|
||||
import sys
|
||||
from selfdrive.services import service_list
|
||||
import selfdrive.messaging as messaging
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
# debug liateral MPC by plotting its trajectory. To receive liveLongitudinalMpc packets,
|
||||
# set on LOG_MPC env variable and run plannerd on a replay
|
||||
|
||||
|
||||
def mpc_vwr_thread(addr="127.0.0.1"):
|
||||
|
||||
plt.ion()
|
||||
fig = plt.figure(figsize=(15, 20))
|
||||
ax = fig.add_subplot(131)
|
||||
aa = fig.add_subplot(132, sharey=ax)
|
||||
ap = fig.add_subplot(133, sharey=ax)
|
||||
|
||||
ax.set_xlim([-10, 10])
|
||||
ax.set_ylim([0., 100.])
|
||||
aa.set_xlim([-20., 20])
|
||||
ap.set_xlim([-5, 5])
|
||||
|
||||
ax.set_xlabel('x [m]')
|
||||
ax.set_ylabel('y [m]')
|
||||
aa.set_xlabel('steer_angle [deg]')
|
||||
ap.set_xlabel('asset angle [deg]')
|
||||
ax.grid(True)
|
||||
aa.grid(True)
|
||||
ap.grid(True)
|
||||
|
||||
path_x = np.arange(0, 100)
|
||||
mpc_path_x = np.arange(0, 49)
|
||||
|
||||
p_path_y = np.zeros(100)
|
||||
|
||||
l_path_y = np.zeros(100)
|
||||
r_path_y = np.zeros(100)
|
||||
mpc_path_y = np.zeros(49)
|
||||
mpc_steer_angle = np.zeros(49)
|
||||
mpc_psi = np.zeros(49)
|
||||
|
||||
line1, = ax.plot(mpc_path_y, mpc_path_x)
|
||||
# line1b, = ax.plot(mpc_path_y, mpc_path_x, 'o')
|
||||
|
||||
lineP, = ax.plot(p_path_y, path_x)
|
||||
lineL, = ax.plot(l_path_y, path_x)
|
||||
lineR, = ax.plot(r_path_y, path_x)
|
||||
line3, = aa.plot(mpc_steer_angle, mpc_path_x)
|
||||
line4, = ap.plot(mpc_psi, mpc_path_x)
|
||||
ax.invert_xaxis()
|
||||
aa.invert_xaxis()
|
||||
plt.show()
|
||||
|
||||
|
||||
# *** log ***
|
||||
livempc = messaging.sub_sock(service_list['liveMpc'].port, addr=addr)
|
||||
model = messaging.sub_sock(service_list['model'].port, addr=addr)
|
||||
path_plan_sock = messaging.sub_sock(service_list['pathPlan'].port, addr=addr)
|
||||
|
||||
while 1:
|
||||
lMpc = messaging.recv_sock(livempc, wait=True)
|
||||
md = messaging.recv_sock(model)
|
||||
pp = messaging.recv_sock(path_plan_sock)
|
||||
|
||||
if md is not None:
|
||||
p_poly = np.array(md.model.path.poly)
|
||||
l_poly = np.array(md.model.leftLane.poly)
|
||||
r_poly = np.array(md.model.rightLane.poly)
|
||||
|
||||
p_path_y = np.polyval(p_poly, path_x)
|
||||
l_path_y = np.polyval(r_poly, path_x)
|
||||
r_path_y = np.polyval(l_poly, path_x)
|
||||
|
||||
if pp is not None:
|
||||
p_path_y = np.polyval(pp.pathPlan.dPoly, path_x)
|
||||
lineP.set_xdata(p_path_y)
|
||||
lineP.set_ydata(path_x)
|
||||
|
||||
if lMpc is not None:
|
||||
mpc_path_x = list(lMpc.liveMpc.x)[1:]
|
||||
mpc_path_y = list(lMpc.liveMpc.y)[1:]
|
||||
mpc_steer_angle = list(lMpc.liveMpc.delta)[1:]
|
||||
mpc_psi = list(lMpc.liveMpc.psi)[1:]
|
||||
|
||||
line1.set_xdata(mpc_path_y)
|
||||
line1.set_ydata(mpc_path_x)
|
||||
lineL.set_xdata(l_path_y)
|
||||
lineL.set_ydata(path_x)
|
||||
lineR.set_xdata(r_path_y)
|
||||
lineR.set_ydata(path_x)
|
||||
line3.set_xdata(np.asarray(mpc_steer_angle)*180./np.pi * 14)
|
||||
line3.set_ydata(mpc_path_x)
|
||||
line4.set_xdata(np.asarray(mpc_psi)*180./np.pi)
|
||||
line4.set_ydata(mpc_path_x)
|
||||
|
||||
aa.relim()
|
||||
aa.autoscale_view(True, scaley=True, scalex=True)
|
||||
|
||||
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()
|
||||
Executable
+110
@@ -0,0 +1,110 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import matplotlib
|
||||
matplotlib.use('TkAgg')
|
||||
|
||||
import sys
|
||||
from selfdrive.services import service_list
|
||||
import selfdrive.messaging as messaging
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
N = 21
|
||||
|
||||
# debug longitudinal MPC by plotting its trajectory. To receive liveLongitudinalMpc packets,
|
||||
# set on LOG_MPC env variable and run plannerd on a replay
|
||||
|
||||
def plot_longitudinal_mpc(addr="127.0.0.1"):
|
||||
# *** log ***
|
||||
livempc = messaging.sub_sock(service_list['liveLongitudinalMpc'].port, addr=addr, conflate=True)
|
||||
radarstate = messaging.sub_sock(service_list['radarState'].port, addr=addr, conflate=True)
|
||||
|
||||
plt.ion()
|
||||
fig = plt.figure()
|
||||
|
||||
t = np.hstack([np.arange(0.0, 0.8, 0.2), np.arange(0.8, 10.6, 0.6)])
|
||||
|
||||
p_x_ego = fig.add_subplot(3, 2, 1)
|
||||
p_v_ego = fig.add_subplot(3, 2, 3)
|
||||
p_a_ego = fig.add_subplot(3, 2, 5)
|
||||
# p_x_l = fig.add_subplot(3, 2, 2)
|
||||
# p_a_l = fig.add_subplot(3, 2, 6)
|
||||
p_d_l = fig.add_subplot(3, 2, 2)
|
||||
p_d_l_v = fig.add_subplot(3, 2, 4)
|
||||
p_d_l_vv = fig.add_subplot(3, 2, 6)
|
||||
|
||||
p_v_ego.set_ylim([0, 30])
|
||||
p_a_ego.set_ylim([-4, 4])
|
||||
p_d_l.set_ylim([-1, 10])
|
||||
|
||||
p_x_ego.set_title('x')
|
||||
p_v_ego.set_title('v')
|
||||
p_a_ego.set_title('a')
|
||||
p_d_l.set_title('rel dist')
|
||||
|
||||
l_x_ego, = p_x_ego.plot(t, np.zeros(N))
|
||||
l_v_ego, = p_v_ego.plot(t, np.zeros(N))
|
||||
l_a_ego, = p_a_ego.plot(t, np.zeros(N))
|
||||
l_x_l, = p_x_ego.plot(t, np.zeros(N))
|
||||
l_v_l, = p_v_ego.plot(t, np.zeros(N))
|
||||
l_a_l, = p_a_ego.plot(t, np.zeros(N))
|
||||
l_d_l, = p_d_l.plot(t, np.zeros(N))
|
||||
l_d_l_v, = p_d_l_v.plot(np.zeros(N))
|
||||
l_d_l_vv, = p_d_l_vv.plot(np.zeros(N))
|
||||
p_x_ego.legend(['ego', 'l'])
|
||||
p_v_ego.legend(['ego', 'l'])
|
||||
p_a_ego.legend(['ego', 'l'])
|
||||
p_d_l_v.set_xlabel('d_rel')
|
||||
p_d_l_v.set_ylabel('v_rel')
|
||||
p_d_l_v.set_ylim([-20, 20])
|
||||
p_d_l_v.set_xlim([0, 100])
|
||||
p_d_l_vv.set_xlabel('d_rel')
|
||||
p_d_l_vv.set_ylabel('v_rel')
|
||||
p_d_l_vv.set_ylim([-5, 5])
|
||||
p_d_l_vv.set_xlim([10, 40])
|
||||
|
||||
while True:
|
||||
lMpc = messaging.recv_sock(livempc, wait=True)
|
||||
rs = messaging.recv_sock(radarstate, wait=True)
|
||||
|
||||
if lMpc is not None:
|
||||
|
||||
if lMpc.liveLongitudinalMpc.mpcId != 1:
|
||||
continue
|
||||
|
||||
x_ego = list(lMpc.liveLongitudinalMpc.xEgo)
|
||||
v_ego = list(lMpc.liveLongitudinalMpc.vEgo)
|
||||
a_ego = list(lMpc.liveLongitudinalMpc.aEgo)
|
||||
x_l = list(lMpc.liveLongitudinalMpc.xLead)
|
||||
v_l = list(lMpc.liveLongitudinalMpc.vLead)
|
||||
# a_l = list(lMpc.liveLongitudinalMpc.aLead)
|
||||
a_l = rs.radarState.leadOne.aLeadK * np.exp(-lMpc.liveLongitudinalMpc.aLeadTau * t**2 / 2)
|
||||
#print(min(a_ego), lMpc.liveLongitudinalMpc.qpIterations)
|
||||
|
||||
l_x_ego.set_ydata(x_ego)
|
||||
l_v_ego.set_ydata(v_ego)
|
||||
l_a_ego.set_ydata(a_ego)
|
||||
|
||||
l_x_l.set_ydata(x_l)
|
||||
l_v_l.set_ydata(v_l)
|
||||
l_a_l.set_ydata(a_l)
|
||||
|
||||
l_d_l.set_ydata(np.array(x_l) - np.array(x_ego))
|
||||
l_d_l_v.set_ydata(np.array(v_l) - np.array(v_ego))
|
||||
l_d_l_v.set_xdata(np.array(x_l) - np.array(x_ego))
|
||||
l_d_l_vv.set_ydata(np.array(v_l) - np.array(v_ego))
|
||||
l_d_l_vv.set_xdata(np.array(x_l) - np.array(x_ego))
|
||||
|
||||
p_x_ego.relim()
|
||||
p_x_ego.autoscale_view(True, scaley=True, scalex=True)
|
||||
fig.canvas.draw()
|
||||
fig.canvas.flush_events()
|
||||
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
if len(sys.argv) > 1:
|
||||
plot_longitudinal_mpc(sys.argv[1])
|
||||
else:
|
||||
plot_longitudinal_mpc()
|
||||
Executable
+129
@@ -0,0 +1,129 @@
|
||||
#! /usr/bin/env python
|
||||
import matplotlib.pyplot as plt
|
||||
from selfdrive.controls.lib.lateral_mpc import libmpc_py
|
||||
from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT
|
||||
import math
|
||||
|
||||
libmpc = libmpc_py.libmpc
|
||||
libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, 1.)
|
||||
|
||||
cur_state = libmpc_py.ffi.new("state_t *")
|
||||
cur_state[0].x = 0.0
|
||||
cur_state[0].y = 0.0
|
||||
cur_state[0].psi = 0.0
|
||||
cur_state[0].delta = 0.0
|
||||
|
||||
mpc_solution = libmpc_py.ffi.new("log_t *")
|
||||
xx = []
|
||||
yy = []
|
||||
deltas = []
|
||||
psis = []
|
||||
times = []
|
||||
|
||||
curvature_factor = 0.3
|
||||
v_ref = 1.0 * 20.12 # 45 mph
|
||||
|
||||
LANE_WIDTH = 3.7
|
||||
p = [0.0, 0.0, 0.0, 0.0]
|
||||
p_l = p[:]
|
||||
p_l[3] += LANE_WIDTH / 2.0
|
||||
|
||||
p_r = p[:]
|
||||
p_r[3] -= LANE_WIDTH / 2.0
|
||||
|
||||
|
||||
l_poly = libmpc_py.ffi.new("double[4]", p_l)
|
||||
r_poly = libmpc_py.ffi.new("double[4]", p_r)
|
||||
p_poly = libmpc_py.ffi.new("double[4]", p)
|
||||
|
||||
l_prob = 1.0
|
||||
r_prob = 1.0
|
||||
p_prob = 1.0
|
||||
|
||||
for i in range(1):
|
||||
cur_state[0].delta = math.radians(510. / 13.)
|
||||
libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob,
|
||||
curvature_factor, v_ref, LANE_WIDTH)
|
||||
|
||||
timesi = []
|
||||
ct = 0
|
||||
for i in range(21):
|
||||
timesi.append(ct)
|
||||
if i <= 4:
|
||||
ct += 0.05
|
||||
else:
|
||||
ct += 0.15
|
||||
|
||||
|
||||
xi = list(mpc_solution[0].x)
|
||||
yi = list(mpc_solution[0].y)
|
||||
psii = list(mpc_solution[0].psi)
|
||||
deltai = list(mpc_solution[0].delta)
|
||||
print("COST: ", mpc_solution[0].cost)
|
||||
|
||||
|
||||
plt.figure(0)
|
||||
plt.subplot(3, 1, 1)
|
||||
plt.plot(timesi, psii)
|
||||
plt.ylabel('psi')
|
||||
plt.grid(True)
|
||||
plt.subplot(3, 1, 2)
|
||||
plt.plot(timesi, deltai)
|
||||
plt.ylabel('delta')
|
||||
plt.grid(True)
|
||||
plt.subplot(3, 1, 3)
|
||||
plt.plot(timesi, yi)
|
||||
plt.ylabel('y')
|
||||
plt.grid(True)
|
||||
plt.show()
|
||||
|
||||
|
||||
#### UNCOMMENT TO CHECK ITERATIVE SOLUTION
|
||||
####
|
||||
####for i in range(100):
|
||||
#### libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob,
|
||||
#### curvature_factor, v_ref, LANE_WIDTH)
|
||||
#### print "x", list(mpc_solution[0].x)
|
||||
#### print "y", list(mpc_solution[0].y)
|
||||
#### print "delta", list(mpc_solution[0].delta)
|
||||
#### print "psi", list(mpc_solution[0].psi)
|
||||
#### # cur_state[0].x = mpc_solution[0].x[1]
|
||||
#### # cur_state[0].y = mpc_solution[0].y[1]
|
||||
#### # cur_state[0].psi = mpc_solution[0].psi[1]
|
||||
#### cur_state[0].delta = radians(200 / 13.)#mpc_solution[0].delta[1]
|
||||
####
|
||||
#### xx.append(cur_state[0].x)
|
||||
#### yy.append(cur_state[0].y)
|
||||
#### psis.append(cur_state[0].psi)
|
||||
#### deltas.append(cur_state[0].delta)
|
||||
#### times.append(i * 0.05)
|
||||
####
|
||||
####
|
||||
####def f(x):
|
||||
#### return p_poly[0] * x**3 + p_poly[1] * x**2 + p_poly[2] * x + p_poly[3]
|
||||
####
|
||||
####
|
||||
##### planned = map(f, xx)
|
||||
##### plt.figure(1)
|
||||
##### plt.plot(yy, xx, 'r-')
|
||||
##### plt.plot(planned, xx, 'b--', linewidth=0.5)
|
||||
##### plt.axes().set_aspect('equal', 'datalim')
|
||||
##### plt.gca().invert_xaxis()
|
||||
####
|
||||
##### planned = map(f, map(float, list(mpc_solution[0].x)[1:]))
|
||||
##### plt.figure(1)
|
||||
##### plt.plot(map(float, list(mpc_solution[0].y)[1:]), map(float, list(mpc_solution[0].x)[1:]), 'r-')
|
||||
##### plt.plot(planned, map(float, list(mpc_solution[0].x)[1:]), 'b--', linewidth=0.5)
|
||||
##### plt.axes().set_aspect('equal', 'datalim')
|
||||
##### plt.gca().invert_xaxis()
|
||||
####
|
||||
####plt.figure(2)
|
||||
####plt.subplot(2, 1, 1)
|
||||
####plt.plot(times, psis)
|
||||
####plt.ylabel('psi')
|
||||
####plt.subplot(2, 1, 2)
|
||||
####plt.plot(times, deltas)
|
||||
####plt.ylabel('delta')
|
||||
####
|
||||
####
|
||||
####plt.show()
|
||||
Executable
+186
@@ -0,0 +1,186 @@
|
||||
#! /usr/bin/env python
|
||||
import numpy as np
|
||||
from collections import OrderedDict
|
||||
import matplotlib.pyplot as plt
|
||||
from selfdrive.car.honda.interface import CarInterface
|
||||
from selfdrive.controls.lib.lateral_mpc import libmpc_py
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
|
||||
# plot lateral MPC trajectory by defining boundary conditions:
|
||||
# lane lines, p_poly and vehicle states. Use this script to tune MPC costs
|
||||
|
||||
libmpc = libmpc_py.libmpc
|
||||
|
||||
mpc_solution = libmpc_py.ffi.new("log_t *")
|
||||
|
||||
points_l = np.array([1.1049711, 1.1053879, 1.1073375, 1.1096942, 1.1124474, 1.1154714, 1.1192677, 1.1245866, 1.1321017, 1.1396152, 1.146443, 1.1555313, 1.1662073, 1.1774249, 1.1888939, 1.2009926, 1.2149779, 1.2300836, 1.2450289, 1.2617753, 1.2785473, 1.2974714, 1.3151019, 1.3331807, 1.3545501, 1.3763691, 1.3983455, 1.4215056, 1.4446729, 1.4691089, 1.4927692, 1.5175346, 1.5429921, 1.568854, 1.5968665, 1.6268958, 1.657122, 1.6853137, 1.7152609, 1.7477539, 1.7793678, 1.8098511, 1.8428392, 1.8746407, 1.9089606, 1.9426043, 1.9775689, 2.0136933, 2.0520134, 2.0891454])
|
||||
|
||||
points_r = np.array([-2.4442139, -2.4449506, -2.4448867, -2.44377, -2.4422617, -2.4393811, -2.4374201, -2.4334245, -2.4286852, -2.4238286, -2.4177458, -2.4094386, -2.3994849, -2.3904033, -2.380136, -2.3699453, -2.3594661, -2.3474073, -2.3342307, -2.3194637, -2.3046403, -2.2881098, -2.2706163, -2.2530098, -2.235604, -2.2160542, -2.1967411, -2.1758952, -2.1544619, -2.1325269, -2.1091819, -2.0850561, -2.0621953, -2.0364127, -2.0119917, -1.9851667, -1.9590458, -1.9306552, -1.9024918, -1.8745357, -1.8432863, -1.8131843, -1.7822732, -1.7507075, -1.7180918, -1.6845931, -1.650871, -1.6157099, -1.5787286, -1.5418037])
|
||||
|
||||
|
||||
points_c = (points_l + points_r) / 2.0
|
||||
|
||||
def compute_path_pinv():
|
||||
deg = 3
|
||||
x = np.arange(50.0)
|
||||
X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T
|
||||
pinv = np.linalg.pinv(X)
|
||||
return pinv
|
||||
|
||||
|
||||
def model_polyfit(points):
|
||||
path_pinv = compute_path_pinv()
|
||||
return np.dot(path_pinv, map(float, points))
|
||||
|
||||
|
||||
xx = []
|
||||
yy = []
|
||||
deltas = []
|
||||
psis = []
|
||||
times = []
|
||||
|
||||
CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING")
|
||||
VM = VehicleModel(CP)
|
||||
|
||||
v_ref = 32.00 # 45 mph
|
||||
curvature_factor = VM.curvature_factor(v_ref)
|
||||
print(curvature_factor)
|
||||
|
||||
LANE_WIDTH = 3.9
|
||||
p_l = map(float, model_polyfit(points_l))
|
||||
p_r = map(float, model_polyfit(points_r))
|
||||
p_p = map(float, model_polyfit(points_c))
|
||||
|
||||
l_poly = libmpc_py.ffi.new("double[4]", p_l)
|
||||
r_poly = libmpc_py.ffi.new("double[4]", p_r)
|
||||
p_poly = libmpc_py.ffi.new("double[4]", p_p)
|
||||
l_prob = 1.0
|
||||
r_prob = 1.0
|
||||
p_prob = 1.0 # This is always 1
|
||||
|
||||
|
||||
mpc_x_points = np.linspace(0., 2.5*v_ref, num=50)
|
||||
points_poly_l = np.polyval(p_l, mpc_x_points)
|
||||
points_poly_r = np.polyval(p_r, mpc_x_points)
|
||||
points_poly_p = np.polyval(p_p, mpc_x_points)
|
||||
print(points_poly_l)
|
||||
|
||||
lanes_x = np.linspace(0, 49)
|
||||
|
||||
cur_state = libmpc_py.ffi.new("state_t *")
|
||||
cur_state[0].x = 0.0
|
||||
cur_state[0].y = 0.5
|
||||
cur_state[0].psi = 0.0
|
||||
cur_state[0].delta = 0.0
|
||||
|
||||
xs = []
|
||||
ys = []
|
||||
deltas = []
|
||||
titles = [
|
||||
'Steer rate cost',
|
||||
'Heading cost',
|
||||
'Lane cost',
|
||||
'Path cost',
|
||||
]
|
||||
|
||||
# Steer rate cost
|
||||
sol_x = OrderedDict()
|
||||
sol_y = OrderedDict()
|
||||
delta = OrderedDict()
|
||||
for cost in np.logspace(-1, 1.0, 5):
|
||||
libmpc.init(1.0, 3.0, 1.0, cost)
|
||||
for _ in range(10):
|
||||
libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob,
|
||||
curvature_factor, v_ref, LANE_WIDTH)
|
||||
sol_x[cost] = map(float, list(mpc_solution[0].x))
|
||||
sol_y[cost] = map(float, list(mpc_solution[0].y))
|
||||
delta[cost] = map(float, list(mpc_solution[0].delta))
|
||||
xs.append(sol_x)
|
||||
ys.append(sol_y)
|
||||
deltas.append(delta)
|
||||
|
||||
# Heading cost
|
||||
sol_x = OrderedDict()
|
||||
sol_y = OrderedDict()
|
||||
delta = OrderedDict()
|
||||
for cost in np.logspace(-1, 1.0, 5):
|
||||
libmpc.init(1.0, 3.0, cost, 1.0)
|
||||
for _ in range(10):
|
||||
libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob,
|
||||
curvature_factor, v_ref, LANE_WIDTH)
|
||||
sol_x[cost] = map(float, list(mpc_solution[0].x))
|
||||
sol_y[cost] = map(float, list(mpc_solution[0].y))
|
||||
delta[cost] = map(float, list(mpc_solution[0].delta))
|
||||
xs.append(sol_x)
|
||||
ys.append(sol_y)
|
||||
deltas.append(delta)
|
||||
|
||||
# Lane cost
|
||||
sol_x = OrderedDict()
|
||||
sol_y = OrderedDict()
|
||||
delta = OrderedDict()
|
||||
for cost in np.logspace(-1, 2.0, 5):
|
||||
libmpc.init(1.0, cost, 1.0, 1.0)
|
||||
for _ in range(10):
|
||||
libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob,
|
||||
curvature_factor, v_ref, LANE_WIDTH)
|
||||
sol_x[cost] = map(float, list(mpc_solution[0].x))
|
||||
sol_y[cost] = map(float, list(mpc_solution[0].y))
|
||||
delta[cost] = map(float, list(mpc_solution[0].delta))
|
||||
xs.append(sol_x)
|
||||
ys.append(sol_y)
|
||||
deltas.append(delta)
|
||||
|
||||
|
||||
# Path cost
|
||||
sol_x = OrderedDict()
|
||||
sol_y = OrderedDict()
|
||||
delta = OrderedDict()
|
||||
for cost in np.logspace(-1, 1.0, 5):
|
||||
libmpc.init(cost, 3.0, 1.0, 1.0)
|
||||
for _ in range(10):
|
||||
libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob,
|
||||
curvature_factor, v_ref, LANE_WIDTH)
|
||||
sol_x[cost] = map(float, list(mpc_solution[0].x))
|
||||
sol_y[cost] = map(float, list(mpc_solution[0].y))
|
||||
delta[cost] = map(float, list(mpc_solution[0].delta))
|
||||
xs.append(sol_x)
|
||||
ys.append(sol_y)
|
||||
deltas.append(delta)
|
||||
|
||||
|
||||
|
||||
plt.figure()
|
||||
|
||||
for i in range(len(xs)):
|
||||
ax = plt.subplot(2, 2, i + 1)
|
||||
sol_x = xs[i]
|
||||
sol_y = ys[i]
|
||||
for cost in sol_x.keys():
|
||||
plt.plot(sol_x[cost], sol_y[cost])
|
||||
|
||||
plt.plot(lanes_x, points_r, '.b')
|
||||
plt.plot(lanes_x, points_l, '.b')
|
||||
plt.plot(lanes_x, (points_l + points_r) / 2.0, '--g')
|
||||
plt.plot(mpc_x_points, points_poly_l, 'b')
|
||||
plt.plot(mpc_x_points, points_poly_r, 'b')
|
||||
plt.plot(mpc_x_points, (points_poly_l + points_poly_r) / 2.0, 'g')
|
||||
plt.legend(map(lambda x: str(round(x, 2)), sol_x.keys()) + ['right', 'left', 'center'], loc=3)
|
||||
plt.title(titles[i])
|
||||
plt.grid(True)
|
||||
# ax.set_aspect('equal', 'datalim')
|
||||
|
||||
|
||||
plt.figure()
|
||||
for i in range(len(xs)):
|
||||
plt.subplot(2, 2, i + 1)
|
||||
sol_x = xs[i]
|
||||
delta = deltas[i]
|
||||
|
||||
for cost in sol_x.keys():
|
||||
plt.plot(delta[cost])
|
||||
plt.title(titles[i])
|
||||
plt.legend(map(lambda x: str(round(x, 2)), sol_x.keys()), loc=3)
|
||||
plt.grid(True)
|
||||
|
||||
plt.show()
|
||||
Executable
+168
@@ -0,0 +1,168 @@
|
||||
#! /usr/bin/env python
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from selfdrive.controls.lib.longitudinal_mpc import libmpc_py
|
||||
from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG
|
||||
import math
|
||||
|
||||
# plot liongitudinal MPC trajectory by defining boundary conditions:
|
||||
# ego and lead vehicles state. Use this script to tune MPC costs
|
||||
|
||||
def RW(v_ego, v_l):
|
||||
TR = 1.8
|
||||
G = 9.81
|
||||
return (v_ego * TR - (v_l - v_ego) * TR + v_ego*v_ego/(2*G) - v_l*v_l / (2*G))
|
||||
|
||||
|
||||
def NORM_RW_ERROR(v_ego, v_l, p):
|
||||
return (RW(v_ego, v_l) + 4.0 - p)
|
||||
return (RW(v_ego, v_l) + 4.0 - p) / (np.sqrt(v_ego + 0.5) + 0.1)
|
||||
|
||||
|
||||
v_ego = 20.0
|
||||
a_ego = 0
|
||||
|
||||
x_lead = 10.0
|
||||
v_lead = 20.0
|
||||
a_lead = -3.0
|
||||
a_lead_tau = 0.
|
||||
|
||||
# v_ego = 7.02661012716
|
||||
# a_ego = -1.26143024772
|
||||
|
||||
# x_lead = 29.625 + 20
|
||||
# v_lead = 0.725235462189 + 1
|
||||
# a_lead = -1.00025629997
|
||||
|
||||
# a_lead_tau = 2.90729817665
|
||||
|
||||
min_a_lead_tau = (a_lead**2 * math.pi) / (2 * (v_lead + 0.01)**2)
|
||||
min_a_lead_tau = 0.0
|
||||
|
||||
print(a_lead_tau, min_a_lead_tau)
|
||||
a_lead_tau = max(a_lead_tau, min_a_lead_tau)
|
||||
|
||||
ffi, libmpc = libmpc_py.get_libmpc(1)
|
||||
libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
|
||||
libmpc.init_with_simulation(v_ego, x_lead, v_lead, a_lead, a_lead_tau)
|
||||
|
||||
cur_state = ffi.new("state_t *")
|
||||
cur_state[0].x_ego = 0.0
|
||||
cur_state[0].v_ego = v_ego
|
||||
cur_state[0].a_ego = a_ego
|
||||
cur_state[0].x_l = x_lead
|
||||
cur_state[0].v_l = v_lead
|
||||
|
||||
mpc_solution = ffi.new("log_t *")
|
||||
|
||||
for _ in range(10):
|
||||
print(libmpc.run_mpc(cur_state, mpc_solution, a_lead_tau, a_lead))
|
||||
|
||||
|
||||
for i in range(21):
|
||||
print("t: %.2f\t x_e: %.2f\t v_e: %.2f\t a_e: %.2f\t" % (mpc_solution[0].t[i], mpc_solution[0].x_ego[i], mpc_solution[0].v_ego[i], mpc_solution[0].a_ego[i]))
|
||||
print("x_l: %.2f\t v_l: %.2f\t \t" % (mpc_solution[0].x_l[i], mpc_solution[0].v_l[i]))
|
||||
|
||||
t = np.hstack([np.arange(0., 1.0, 0.2), np.arange(1.0, 10.1, 0.6)])
|
||||
|
||||
print(map(float, mpc_solution[0].x_ego)[-1])
|
||||
print(map(float, mpc_solution[0].x_l)[-1] - map(float, mpc_solution[0].x_ego)[-1])
|
||||
|
||||
plt.figure(figsize=(8, 8))
|
||||
|
||||
plt.subplot(4, 1, 1)
|
||||
x_l = np.array(map(float, mpc_solution[0].x_l))
|
||||
plt.plot(t, map(float, mpc_solution[0].x_ego))
|
||||
plt.plot(t, x_l)
|
||||
plt.legend(['ego', 'lead'])
|
||||
plt.title('x')
|
||||
plt.grid()
|
||||
|
||||
plt.subplot(4, 1, 2)
|
||||
v_ego = np.array(map(float, mpc_solution[0].v_ego))
|
||||
v_l = np.array(map(float, mpc_solution[0].v_l))
|
||||
plt.plot(t, v_ego)
|
||||
plt.plot(t, v_l)
|
||||
plt.legend(['ego', 'lead'])
|
||||
plt.ylim([-1, max(max(v_ego), max(v_l))])
|
||||
plt.title('v')
|
||||
plt.grid()
|
||||
|
||||
plt.subplot(4, 1, 3)
|
||||
plt.plot(t, map(float, mpc_solution[0].a_ego))
|
||||
plt.plot(t, map(float, mpc_solution[0].a_l))
|
||||
plt.legend(['ego', 'lead'])
|
||||
plt.title('a')
|
||||
plt.grid()
|
||||
|
||||
|
||||
plt.subplot(4, 1, 4)
|
||||
d_l = np.array(map(float, mpc_solution[0].x_l)) - np.array(map(float, mpc_solution[0].x_ego))
|
||||
desired = 4.0 + RW(v_ego, v_l)
|
||||
|
||||
plt.plot(t, d_l)
|
||||
plt.plot(t, desired, '--')
|
||||
plt.ylim(-1, max(max(desired), max(d_l)))
|
||||
plt.legend(['relative distance', 'desired distance'])
|
||||
plt.grid()
|
||||
|
||||
plt.show()
|
||||
|
||||
# c1 = np.exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l))
|
||||
# c2 = np.exp(4.5 - d_l)
|
||||
# print(c1)
|
||||
# print(c2)
|
||||
|
||||
# plt.figure()
|
||||
# plt.plot(t, c1, label="NORM_RW_ERROR")
|
||||
# plt.plot(t, c2, label="penalty function")
|
||||
# plt.legend()
|
||||
|
||||
# ## OLD MPC
|
||||
# a_lead_tau = 1.5
|
||||
# a_lead_tau = max(a_lead_tau, -a_lead / (v_lead + 0.01))
|
||||
|
||||
# ffi, libmpc = libmpc_py.get_libmpc(1)
|
||||
# libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
|
||||
# libmpc.init_with_simulation(v_ego, x_lead, v_lead, a_lead, a_lead_tau)
|
||||
|
||||
# cur_state = ffi.new("state_t *")
|
||||
# cur_state[0].x_ego = 0.0
|
||||
# cur_state[0].v_ego = v_ego
|
||||
# cur_state[0].a_ego = a_ego
|
||||
# cur_state[0].x_lead = x_lead
|
||||
# cur_state[0].v_lead = v_lead
|
||||
# cur_state[0].a_lead = a_lead
|
||||
|
||||
# mpc_solution = ffi.new("log_t *")
|
||||
|
||||
# for _ in range(10):
|
||||
# print libmpc.run_mpc(cur_state, mpc_solution, a_lead_tau)
|
||||
|
||||
# t = np.hstack([np.arange(0., 1.0, 0.2), np.arange(1.0, 10.1, 0.6)])
|
||||
|
||||
# print(map(float, mpc_solution[0].x_ego)[-1])
|
||||
# print(map(float, mpc_solution[0].x_lead)[-1] - map(float, mpc_solution[0].x_ego)[-1])
|
||||
# plt.subplot(4, 2, 2)
|
||||
# plt.plot(t, map(float, mpc_solution[0].x_ego))
|
||||
# plt.plot(t, map(float, mpc_solution[0].x_lead))
|
||||
# plt.legend(['ego', 'lead'])
|
||||
# plt.title('x')
|
||||
|
||||
# plt.subplot(4, 2, 4)
|
||||
# plt.plot(t, map(float, mpc_solution[0].v_ego))
|
||||
# plt.plot(t, map(float, mpc_solution[0].v_lead))
|
||||
# plt.legend(['ego', 'lead'])
|
||||
# plt.title('v')
|
||||
|
||||
# plt.subplot(4, 2, 6)
|
||||
# plt.plot(t, map(float, mpc_solution[0].a_ego))
|
||||
# plt.plot(t, map(float, mpc_solution[0].a_lead))
|
||||
# plt.legend(['ego', 'lead'])
|
||||
# plt.title('a')
|
||||
|
||||
|
||||
# plt.subplot(4, 2, 8)
|
||||
# plt.plot(t, np.array(map(float, mpc_solution[0].x_lead)) - np.array(map(float, mpc_solution[0].x_ego)))
|
||||
|
||||
# plt.show()
|
||||
Executable
+27
@@ -0,0 +1,27 @@
|
||||
#!/usr/bin/env python3
|
||||
from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_known_cars
|
||||
import selfdrive.messaging as messaging
|
||||
|
||||
|
||||
# Prius and Leuxs es 300H
|
||||
fingerprint = {898: 8, 905: 8, 810: 2, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 921: 8, 800: 8, 944: 8, 1570: 8, 1059: 1, 36: 8, 37: 8, 550: 8, 295: 8, 296: 8, 170: 8, 1071: 8, 560: 7, 945: 8, 562: 6, 180: 8, 1077: 8, 950: 8, 951: 8, 953: 8, 1595: 8, 1084: 8, 829: 2, 1086: 8, 1568: 8, 452: 8, 581: 5, 1057: 8, 713: 8, 971: 7, 975: 5, 1571: 8, 466: 8, 467: 8, 1572: 8, 1114: 8, 933: 8, 863: 8, 608: 8, 993: 8, 610: 8, 955: 8, 166: 8, 1056: 8, 956: 8, 1132: 8, 1085: 8, 552: 4, 1779: 8, 1017: 8, 1020: 8, 426: 6, 1279: 8}
|
||||
|
||||
# rav4 2019 and corolla tss2
|
||||
fingerprint = {896: 8, 898: 8, 976: 1, 1541: 8, 905: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1552: 8, 1553: 8, 1556: 8, 921: 8, 1056: 8, 544: 4, 1570: 8, 1059: 1, 36: 8, 37: 8, 550: 8, 552: 4, 170: 8, 812: 8, 944: 8, 945: 8, 562: 6, 180: 8, 1077: 8, 951: 8, 824: 8, 1076: 8, 186: 4, 955: 8, 956: 8, 705: 8, 452: 8, 1592: 8, 464: 8, 1571: 8, 466: 8, 467: 8, 761: 8, 728: 8, 1572: 8, 1114: 8, 933: 8, 800: 8, 608: 8, 865: 8, 610: 8, 1595: 8, 1745: 8, 764: 8, 1002: 8, 1649: 8, 1779: 8, 1568: 8, 1017: 8, 1279: 8, 1020: 8, 810: 2, 426: 6}
|
||||
|
||||
# rav4 2019 and corolla tss2
|
||||
fingerprint = {896: 8, 898: 8, 900: 6, 976: 1, 1541: 8, 902: 6, 905: 8, 810: 2, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1552: 8, 1553: 8, 1556: 8, 1571: 8, 921: 8, 1056: 8, 544: 4, 1570: 8, 1059: 1, 36: 8, 37: 8, 550: 8, 935: 8, 552: 4, 170: 8, 812: 8, 944: 8, 945: 8, 562: 6, 180: 8, 1077: 8, 951: 8, 1592: 8, 1076: 8, 186: 4, 955: 8, 956: 8, 1001: 8, 705: 8, 452: 8, 1788: 8, 464: 8, 824: 8, 466: 8, 467: 8, 761: 8, 728: 8, 1572: 8, 1114: 8, 933: 8, 800: 8, 608: 8, 865: 8, 610: 8, 1595: 8, 934: 8, 998: 5, 1745: 8, 1000: 8, 764: 8, 1002: 8, 999: 7, 1789: 8, 1649: 8, 1779: 8, 1568: 8, 1017: 8, 1786: 8, 1787: 8, 1020: 8, 426: 6, 1279: 8}
|
||||
|
||||
candidate_cars = all_known_cars()
|
||||
|
||||
|
||||
for addr, l in fingerprint.items():
|
||||
dat = messaging.new_message()
|
||||
dat.init('can', 1)
|
||||
|
||||
msg = dat.can[0]
|
||||
msg.address = addr
|
||||
msg.dat = " " * l
|
||||
|
||||
candidate_cars = eliminate_incompatible_cars(msg, candidate_cars)
|
||||
print(candidate_cars)
|
||||
@@ -1,4 +1,4 @@
|
||||
#!/usr/bin/env python
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
This tool can be used to quickly changes the values in a JSON file used for tuning
|
||||
Keys like in vim:
|
||||
|
||||
Reference in New Issue
Block a user