spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

selfdrive/debug

pull/960/head
George Hotz 2020-01-17 11:23:21 -08:00
parent da079d47d7
commit f467642a1c
66 changed files with 4113 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

0
selfdrive/debug/__init__.py 100644
View File

42
selfdrive/debug/can_printer.py 100755
View File

@ -0,0 +1,42 @@
#!/usr/bin/env python3
import binascii
import os
import sys
from collections import defaultdict
import cereal.messaging as messaging
from common.realtime import sec_since_boot
def can_printer(bus=0, max_msg=None, addr="127.0.0.1"):
logcan = messaging.sub_sock('can', addr=addr)
start = sec_since_boot()
lp = sec_since_boot()
msgs = defaultdict(list)
canbus = int(os.getenv("CAN", bus))
while 1:
can_recv = messaging.drain_sock(logcan, wait_for_one=True)
for x in can_recv:
for y in x.can:
if y.src == canbus:
msgs[y.address].append(y.dat)
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: 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.decode('ascii'))
print(dd)
lp = sec_since_boot()
if __name__ == "__main__":
if len(sys.argv) > 3:
can_printer(int(sys.argv[1]), int(sys.argv[2]), sys.argv[3])
elif len(sys.argv) > 2:
can_printer(int(sys.argv[1]), int(sys.argv[2]))
elif len(sys.argv) > 1:
can_printer(int(sys.argv[1]))
else:
can_printer()

43
selfdrive/debug/check_freq.py 100755
View File

@ -0,0 +1,43 @@
#!/usr/bin/env python3
import argparse
import numpy as np
from collections import defaultdict, deque
from common.realtime import sec_since_boot
import cereal.messaging as messaging
if __name__ == "__main__":
context = messaging.Context()
poller = messaging.Poller()
parser = argparse.ArgumentParser()
parser.add_argument("socket", type=str, nargs='*', help="socket name")
args = parser.parse_args()
socket_names = args.socket
sockets = {}
rcv_times = defaultdict(lambda: deque(maxlen=100))
t = sec_since_boot()
for name in socket_names:
sock = messaging.sub_sock(name, poller=poller)
sockets[sock] = name
prev_print = t
while True:
for socket in poller.poll(100):
msg = messaging.recv_one(socket)
name = msg.which()
t = sec_since_boot()
rcv_times[name].append(msg.logMonoTime / 1e9)
if t - prev_print > 1:
print()
for name in socket_names:
dts = np.diff(rcv_times[name])
mean = np.mean(dts)
print("%s: Freq %.2f Hz, Min %.2f%%, Max %.2f%%" % (name, 1.0 / mean, np.min(dts) / mean * 100, np.max(dts) / mean * 100))
prev_print = t

18
selfdrive/debug/compare_fingerprints.py 100755
View File

@ -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")

120
selfdrive/debug/cpu_usage_stat.py 100755
View File

@ -0,0 +1,120 @@
#!/usr/bin/env python3
import psutil
import time
import os
import sys
import numpy as np
import argparse
import re
from collections import defaultdict
'''
System tools like top/htop can only show current cpu usage values, so I write this script to do statistics jobs.
Features:
Use psutil library to sample cpu usage(avergage for all cores) of OpenPilot processes, at a rate of 5 samples/sec.
Do cpu usage statistics periodically, 5 seconds as a cycle.
Caculate the average cpu usage within this cycle.
Caculate minumium/maximium/accumulated_average cpu usage as long term inspections.
Monitor multiple processes simuteneously.
Sample usage:
root@localhost:/data/openpilot$ python selfdrive/debug/cpu_usage_stat.py boardd,ubloxd
('Add monitored proc:', './boardd')
('Add monitored proc:', 'python locationd/ubloxd.py')
boardd: 1.96%, min: 1.96%, max: 1.96%, acc: 1.96%
ubloxd.py: 0.39%, min: 0.39%, max: 0.39%, acc: 0.39%
'''
# Do statistics every 5 seconds
PRINT_INTERVAL = 5
SLEEP_INTERVAL = 0.2
monitored_proc_names = [
'ubloxd', 'thermald', 'uploader', 'deleter', 'controlsd', 'plannerd', 'radard', 'mapd', 'loggerd' , 'logmessaged', 'tombstoned',
'logcatd', 'proclogd', 'boardd', 'pandad', './ui', 'ui', 'calibrationd', 'params_learner', 'modeld', 'monitoringd', 'camerad', 'sensord', 'updated', 'gpsd', 'athena']
cpu_time_names = ['user', 'system', 'children_user', 'children_system']
timer = getattr(time, 'monotonic', time.time)
def get_arg_parser():
parser = argparse.ArgumentParser(
description="Unlogger and UI",
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument("proc_names", nargs="?", default='',
help="Process names to be monitored, comma seperated")
parser.add_argument("--list_all", action='store_true',
help="Show all running processes' cmdline")
parser.add_argument("--detailed_times", action='store_true',
help="show cpu time details (split by user, system, child user, child system)")
return parser
if __name__ == "__main__":
args = get_arg_parser().parse_args(sys.argv[1:])
if args.list_all:
for p in psutil.process_iter():
print('cmdline', p.cmdline(), 'name', p.name())
sys.exit(0)
if len(args.proc_names) > 0:
monitored_proc_names = args.proc_names.split(',')
monitored_procs = []
stats = {}
for p in psutil.process_iter():
if p == psutil.Process():
continue
matched = any([l for l in p.cmdline() if any([pn for pn in monitored_proc_names if re.match(r'.*{}.*'.format(pn), l, re.M | re.I)])])
if matched:
k = ' '.join(p.cmdline())
print('Add monitored proc:', k)
stats[k] = {'cpu_samples': defaultdict(list), 'min': defaultdict(lambda: None), 'max': defaultdict(lambda: None),
'avg': defaultdict(lambda: 0.0), 'last_cpu_times': None, 'last_sys_time':None}
stats[k]['last_sys_time'] = timer()
stats[k]['last_cpu_times'] = p.cpu_times()
monitored_procs.append(p)
i = 0
interval_int = int(PRINT_INTERVAL / SLEEP_INTERVAL)
while True:
for p in monitored_procs:
k = ' '.join(p.cmdline())
cur_sys_time = timer()
cur_cpu_times = p.cpu_times()
cpu_times = np.subtract(cur_cpu_times, stats[k]['last_cpu_times']) / (cur_sys_time - stats[k]['last_sys_time'])
stats[k]['last_sys_time'] = cur_sys_time
stats[k]['last_cpu_times'] = cur_cpu_times
cpu_percent = 0
for num, name in enumerate(cpu_time_names):
stats[k]['cpu_samples'][name].append(cpu_times[num])
cpu_percent += cpu_times[num]
stats[k]['cpu_samples']['total'].append(cpu_percent)
time.sleep(SLEEP_INTERVAL)
i += 1
if i % interval_int == 0:
l = []
for k, stat in stats.items():
if len(stat['cpu_samples']) <= 0:
continue
for name, samples in stat['cpu_samples'].items():
samples = np.array(samples)
avg = samples.mean()
c = samples.size
min_cpu = np.amin(samples)
max_cpu = np.amax(samples)
if stat['min'][name] is None or min_cpu < stat['min'][name]:
stat['min'][name] = min_cpu
if stat['max'][name] is None or max_cpu > stat['max'][name]:
stat['max'][name] = max_cpu
stat['avg'][name] = (stat['avg'][name] * (i - c) + avg * c) / (i)
stat['cpu_samples'][name] = []
msg = 'avg: {1:.2%}, min: {2:.2%}, max: {3:.2%} {0}'.format(os.path.basename(k), stat['avg']['total'], stat['min']['total'], stat['max']['total'])
if args.detailed_times:
for stat_type in ['avg', 'min', 'max']:
msg += '\n {}: {}'.format(stat_type, [name + ':' + str(round(stat[stat_type][name]*100, 2)) for name in cpu_time_names])
l.append((os.path.basename(k), stat['avg']['total'], msg))
l.sort(key= lambda x: -x[1])
for x in l:
print(x[2])
print('avg sum: {0:.2%} over {1} samples {2} seconds\n'.format(
sum([stat['avg']['total'] for k, stat in stats.items()]), i, i * SLEEP_INTERVAL
))

64
selfdrive/debug/dump.py 100755
View File

@ -0,0 +1,64 @@
#!/usr/bin/env python3
import os
import sys
import argparse
import json
from hexdump import hexdump
from cereal import log
import cereal.messaging as messaging
from cereal.services import service_list
if __name__ == "__main__":
parser = argparse.ArgumentParser(description='Sniff a communcation socket')
parser.add_argument('--pipe', action='store_true')
parser.add_argument('--raw', action='store_true')
parser.add_argument('--json', action='store_true')
parser.add_argument('--dump-json', action='store_true')
parser.add_argument('--no-print', action='store_true')
parser.add_argument('--addr', default='127.0.0.1')
parser.add_argument('--values', help='values to monitor (instead of entire event)')
parser.add_argument("socket", type=str, nargs='*', help="socket name")
args = parser.parse_args()
if args.addr != "127.0.0.1":
os.environ["ZMQ"] = "1"
messaging.context = messaging.Context()
poller = messaging.Poller()
for m in args.socket if len(args.socket) > 0 else service_list:
sock = messaging.sub_sock(m, poller, addr=args.addr)
values = None
if args.values:
values = [s.strip().split(".") for s in args.values.split(",")]
while 1:
polld = poller.poll(1000)
for sock in polld:
msg = sock.receive()
evt = log.Event.from_bytes(msg)
if not args.no_print:
if args.pipe:
sys.stdout.write(msg)
sys.stdout.flush()
elif args.raw:
hexdump(msg)
elif args.json:
print(json.loads(msg))
elif args.dump_json:
print(json.dumps(evt.to_dict()))
elif values:
print("logMonotime = {}".format(evt.logMonoTime))
for value in values:
if hasattr(evt, value[0]):
item = evt
for key in value:
item = getattr(item, key)
print("{} = {}".format(".".join(value), item))
print("")
else:
print(evt)

29
selfdrive/debug/get_fingerprint.py 100755
View File

@ -0,0 +1,29 @@
#!/usr/bin/env python3
# simple script to get a vehicle fingerprint.
# Instructions:
# - connect to a Panda
# - run selfdrive/boardd/boardd
# - launching this script
# - turn on the car in STOCK MODE (set giraffe switches properly).
# Note: it's very important that the car is in stock mode, in order to collect a complete fingerprint
# - since some messages are published at low frequency, keep this script running for at least 30s,
# until all messages are received at least once
import cereal.messaging as messaging
logcan = messaging.sub_sock('can')
msgs = {}
while True:
lc = messaging.recv_sock(logcan, True)
for c in lc.can:
# read also msgs sent by EON on CAN bus 0x80 and filter out the
# addr with more than 11 bits
if c.src in [0, 2] and c.address < 0x800:
msgs[c.address] = len(c.dat)
fingerprint = ', '.join("%d: %d" % v for v in sorted(msgs.items()))
print("number of messages {0}:".format(len(msgs)))
print("fingerprint {0}".format(fingerprint))

0
selfdrive/debug/internal/__init__.py 100644
View File

21
selfdrive/debug/internal/benchmark_sleep.py 100644
View File

@ -0,0 +1,21 @@
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)))

113
selfdrive/debug/internal/camera_small_box.py 100755
View File

@ -0,0 +1,113 @@
#!/usr/bin/env python3
import os
from common.basedir import BASEDIR
os.environ['BASEDIR'] = BASEDIR
SCALE = 3
import argparse
import zmq
import pygame
import numpy as np
import cv2
import sys
import traceback
from collections import namedtuple
from cereal import car
from common.params import Params
from common.lazy_property import lazy_property
from cereal.messaging import sub_sock, recv_one_or_none, recv_one
from cereal.services import service_list
_BB_OFFSET = 290, 332
_BB_TO_FULL_FRAME = np.asarray([[1., 0., _BB_OFFSET[0]], [0., 1., _BB_OFFSET[1]],
[0., 0., 1.]])
_FULL_FRAME_TO_BB = np.linalg.inv(_BB_TO_FULL_FRAME)
_FULL_FRAME_SIZE = 1164, 874
def pygame_modules_have_loaded():
return pygame.display.get_init() and pygame.font.get_init()
def ui_thread(addr, frame_address):
context = zmq.Context()
pygame.init()
pygame.font.init()
assert pygame_modules_have_loaded()
size = (640 * SCALE, 480 * SCALE)
pygame.display.set_caption("comma one debug UI")
screen = pygame.display.set_mode(size, pygame.DOUBLEBUF)
camera_surface = pygame.surface.Surface((640 * SCALE, 480 * SCALE), 0, 24).convert()
frame = context.socket(zmq.SUB)
frame.connect(frame_address or "tcp://%s:%d" % (addr, 'frame'))
frame.setsockopt(zmq.SUBSCRIBE, "")
img = np.zeros((480, 640, 3), dtype='uint8')
imgff = np.zeros((_FULL_FRAME_SIZE[1], _FULL_FRAME_SIZE[0], 3), dtype=np.uint8)
while 1:
list(pygame.event.get())
screen.fill((64, 64, 64))
# ***** frame *****
fpkt = recv_one(frame)
yuv_img = fpkt.frame.image
if fpkt.frame.transform:
yuv_transform = np.array(fpkt.frame.transform).reshape(3, 3)
else:
# assume frame is flipped
yuv_transform = np.array([[-1.0, 0.0, _FULL_FRAME_SIZE[0] - 1],
[0.0, -1.0, _FULL_FRAME_SIZE[1] - 1], [0.0, 0.0, 1.0]])
if yuv_img and len(yuv_img) == _FULL_FRAME_SIZE[0] * _FULL_FRAME_SIZE[1] * 3 // 2:
yuv_np = np.frombuffer(
yuv_img, dtype=np.uint8).reshape(_FULL_FRAME_SIZE[1] * 3 // 2, -1)
cv2.cvtColor(yuv_np, cv2.COLOR_YUV2RGB_I420, dst=imgff)
cv2.warpAffine(
imgff,
np.dot(yuv_transform, _BB_TO_FULL_FRAME)[:2], (img.shape[1], img.shape[0]),
dst=img,
flags=cv2.WARP_INVERSE_MAP)
else:
img.fill(0)
height, width = img.shape[:2]
img_resized = cv2.resize(
img, (SCALE * width, SCALE * height), interpolation=cv2.INTER_CUBIC)
# *** blits ***
pygame.surfarray.blit_array(camera_surface, img_resized.swapaxes(0, 1))
screen.blit(camera_surface, (0, 0))
# this takes time...vsync or something
pygame.display.flip()
def get_arg_parser():
parser = argparse.ArgumentParser(
description="Show replay data in a UI.",
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument(
"ip_address",
nargs="?",
default="127.0.0.1",
help="The ip address on which to receive zmq messages.")
parser.add_argument(
"--frame-address",
default=None,
help="The ip address on which to receive zmq messages.")
return parser
if __name__ == "__main__":
args = get_arg_parser().parse_args(sys.argv[1:])
ui_thread(args.ip_address, args.frame_address)

98
selfdrive/debug/internal/convert_csv_to_route.py 100755
View File

@ -0,0 +1,98 @@
#!/usr/bin/env python
from common.kalman.ned import ecef2geodetic
import csv
import numpy as np
import webbrowser
import os
import sys
import json
import numpy.linalg as LA
import gmplot
from dateutil.parser import parse
from common.numpy_helpers import deep_interp
# import cvxpy as cvx
MPH_TO_MS = 0.44704
def downsample(positions, speeds, start_idx, end_idx, dist):
# TODO: save headings too
track = []
last_position = positions[start_idx]
valid_indeces = []
track_speeds = []
for pi in range(start_idx, end_idx):
# only save points that are at least 10 cm far away
if LA.norm(positions[pi] - last_position) >= dist:
#print LA.norm(positions[pi] - last_position)
last_position = positions[pi]
track.append(positions[pi])
valid_indeces.append(pi)
track_speeds.append(speeds[pi])
print(-start_idx + end_idx, len(valid_indeces))
# this compare the original point count vs the filtered count
track = np.array(track)
track_speeds = np.array(track_speeds)
return track, track_speeds
def converter(date):
filename = "/home/batman/one/selfdrive/locationd/liveloc_dumps/" + date + "/canonical.csv" # Point one (OK!)
c = csv.DictReader(open(filename, 'rb'), delimiter=',')
start_time = None
t = []
ll_positions = []
positions = []
sats = []
flag = []
speeds = []
for row in c:
t.append(float(row['pctime']))
x = float(row['ecefX'])
y = float(row['ecefY'])
z = float(row['ecefZ'])
ecef = np.array((x, y, z))
speeds.append(float(row['velSpeed']))
pos = ecef2geodetic(ecef)
ll_positions.append(pos)
positions.append(ecef)
t = np.array(t)
ll_positions = np.array(ll_positions)
positions = np.array(positions)
#distances = ll_positions[:,0:2] - START_POS[:2]
#i_start = np.argmin(LA.norm(distances, axis=1))
#for i in range(i_start + 500):
# distances[i] += np.array([100, 100])
#i_end = np.argmin(LA.norm(distances, axis=1))
i_start = 0
i_end = len(positions)
print(i_start, i_end)
track, track_speeds = downsample(positions, speeds, i_start, i_end, 0.2)
ll_track = np.array([ecef2geodetic(pos) for pos in track])
track_struct = {}
print(track_speeds.shape)
print(track.shape)
track_struct['race'] = np.hstack((track,
np.expand_dims(track_speeds, axis=1),
np.zeros((len(track_speeds), 1))))
f = open('/home/batman/one/selfdrive/controls/tracks/loop_city.npy', 'w')
np.save(f, track_struct)
f.close()
print("SAVED!")
if __name__ == "__main__":
converter(sys.argv[1])

53
selfdrive/debug/internal/cycle_alerts.py 100644
View File

@ -0,0 +1,53 @@
# USAGE: python cycle_alerts.py [duration_millis=1000]
# Then start manager
import argparse
import time
import zmq
import cereal.messaging as messaging
from cereal.services import service_list
from selfdrive.controls.lib.alerts import ALERTS
def now_millis(): return time.time() * 1000
default_alerts = sorted(ALERTS, key=lambda alert: (alert.alert_size, len(alert.alert_text_2)))
def cycle_alerts(duration_millis, alerts=None):
if alerts is None:
alerts = default_alerts
controls_state = messaging.pub_sock('controlsState')
last_pop_millis = now_millis()
alert = alerts.pop()
while 1:
if (now_millis() - last_pop_millis) > duration_millis:
alerts.insert(0, alert)
alert = alerts.pop()
last_pop_millis = now_millis()
print('sending {}'.format(str(alert)))
dat = messaging.new_message()
dat.init('controlsState')
dat.controlsState.alertType = alert.alert_type
dat.controlsState.alertText1 = alert.alert_text_1
dat.controlsState.alertText2 = alert.alert_text_2
dat.controlsState.alertSize = alert.alert_size
dat.controlsState.alertStatus = alert.alert_status
dat.controlsState.alertSound = alert.audible_alert
controls_state.send(dat.to_bytes())
time.sleep(0.01)
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument('--duration', type=int, default=1000)
parser.add_argument('--alert-types', nargs='+')
args = parser.parse_args()
alerts = None
if args.alert_types:
alerts = [next(a for a in ALERTS if a.alert_type==alert_type) for alert_type in args.alert_types]
cycle_alerts(args.duration, alerts=alerts)

32
selfdrive/debug/internal/design_lqr.py 100755
View File

@ -0,0 +1,32 @@
#!/usr/bin/env python3
import numpy as np
import control
dt = 0.01
A = np.array([[ 0. , 1. ], [-0.78823806, 1.78060701]])
B = np.array([[-2.23399437e-05], [ 7.58330763e-08]])
C = np.array([[1., 0.]])
# Kalman tuning
Q = np.diag([1, 1])
R = np.atleast_2d(1e5)
(_, _, L) = control.dare(A.T, C.T, Q, R)
L = L.T
# LQR tuning
Q = np.diag([2e5, 1e-5])
R = np.atleast_2d(1)
(_, _, K) = control.dare(A, B, Q, R)
A_cl = (A - B.dot(K))
sys = control.ss(A_cl, B, C, 0, dt)
dc_gain = control.dcgain(sys)
print(("self.A = np." + A.__repr__()).replace('\n', ''))
print(("self.B = np." + B.__repr__()).replace('\n', ''))
print(("self.C = np." + C.__repr__()).replace('\n', ''))
print(("self.K = np." + K.__repr__()).replace('\n', ''))
print(("self.L = np." + L.__repr__()).replace('\n', ''))
print("self.dc_gain = " + str(dc_gain))

48
selfdrive/debug/internal/eval_live_location.py 100755
View File

@ -0,0 +1,48 @@
#!/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()

33
selfdrive/debug/internal/get_fingerprint_from_route.py 100755
View File

@ -0,0 +1,33 @@
#!/usr/bin/env python3
import sys
from tools.lib.logreader import MultiLogIterator
from xx.chffr.lib.route import Route
def get_fingerprint(lr):
can_msgs = [m for m in lr if m.which() == 'can']
msgs = {}
for msg in can_msgs:
for c in msg.can:
# read also msgs sent by EON on CAN bus 0x80 and filter out the
# addr with more than 11 bits
if c.src % 0x80 == 0 and c.address < 0x800:
msgs[c.address] = len(c.dat)
fingerprint = ', '.join("%d: %d" % v for v in sorted(msgs.items()))
print("number of messages {0}:".format(len(msgs)))
print("fingerprint {0}".format(fingerprint))
if __name__ == "__main__":
if len(sys.argv) < 2:
print("Usage: ./get_fingerprint_internal.py <route>")
sys.exit(1)
route = sys.argv[1]
route = Route(route)
lr = MultiLogIterator(route.log_paths(), wraparound=False)
get_fingerprint(lr)

89
selfdrive/debug/internal/gps_planner.py 100755
View File

@ -0,0 +1,89 @@
#!/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()

113
selfdrive/debug/internal/honda_sniff_steering.py 100755
View File

@ -0,0 +1,113 @@
#!/usr/bin/env python3
import os
import zmq
import cereal.messaging as messaging
from cereal.services import service_list
from panda.lib.panda import Panda
from hexdump import hexdump
import time
def raw_panda():
p = Panda()
print(p)
p.set_uart_baud(2, 9600)
p.set_uart_baud(3, 9600)
p.set_uart_parity(2, 1)
p.set_uart_parity(3, 1)
p.set_uart_callback(2, 1)
p.set_uart_callback(3, 1)
idx = 0
while 1:
"""
dat = p.serial_read(2)
if len(dat) > 0:
print "2:",
hexdump(dat)
dat = p.serial_read(3)
if len(dat) > 0:
print "3:",
hexdump(dat)
print "read done, waiting"
time.sleep(0.01)
"""
if idx%2 == 1:
dat = "\x20\x80\xc0\xa0"
else:
dat = "\x00\x80\xc0\xc0"
p.can_send(0, dat, 8)
for r in p.can_recv():
if r[-1] in [8, 9]:
print(r[-1], r[2].encode("hex"))
time.sleep(0.01)
idx += 1
if __name__ == "__main__":
#raw_panda()
#exit(0)
logcan = messaging.sub_sock('can')
t1 = []
t2 = []
t3 = []
while len(t1) < 1000 or os.uname()[-1] == "aarch64":
rr = messaging.recv_sock(logcan, wait=True)
for c in rr.can:
if c.src in [9] and len(c.dat) == 5:
aa = map(lambda x: ord(x)&0x7f, c.dat)
# checksum
assert (-(aa[0]+aa[1]+aa[2]+aa[3]))&0x7f == aa[4]
#print map(bin, aa[0:4])
aa[0] &= ~0x20
aa[1] &= ~0x20
st = (aa[0] << 5) + aa[1]
if st >= 256:
st = -(512-st)
mt = ((aa[2] >> 3) << 7) + aa[3]
if mt >= 512:
mt = -(1024-mt)
print(st, mt)
t1.append(st)
t2.append(mt)
#print map(bin, aa), "apply", st
if c.src in [8] and len(c.dat) == 4:
aa = map(lambda x: ord(x)&0x7f, c.dat)
# checksum
assert (-(aa[0]+aa[1]+aa[2]))&0x7f == aa[3]
aa[0] &= ~0x20
aa[1] &= ~0x20
st = (aa[0] << 5) + aa[1]
if st >= 256:
st = -(512-st)
print(aa, "apply", st)
t3.append(st)
import matplotlib.pyplot as plt
plt.plot(t1)
plt.plot(t2)
plt.plot(t3)
plt.show()

57
selfdrive/debug/internal/measure_torque_time_to_max.py 100755
View File

@ -0,0 +1,57 @@
#!/usr/bin/env python3
import os
import sys
import argparse
import struct
from cereal import log
import cereal.messaging as messaging
from cereal.services import service_list
if __name__ == "__main__":
parser = argparse.ArgumentParser(description='Sniff a communcation socket')
parser.add_argument('--addr', default='127.0.0.1')
args = parser.parse_args()
if args.addr != "127.0.0.1":
os.environ["ZMQ"] = "1"
messaging.context = messaging.Context()
poller = messaging.Poller()
messaging.sub_sock('can', poller, addr=args.addr)
active = 0
start_t = 0
start_v = 0
max_v = 0
max_t = 0
window = [0] * 10
avg = 0
while 1:
polld = poller.poll(1000)
for sock in polld:
msg = sock.receive()
evt = log.Event.from_bytes(msg)
for item in evt.can:
if item.address == 0xe4 and item.src == 128:
torque_req = struct.unpack('!h', item.dat[0:2])[0]
# print(torque_req)
active = abs(torque_req) > 0
if abs(torque_req) < 100:
if max_v > 5:
print(f'{start_v} -> {max_v} = {round(max_v - start_v, 2)} over {round(max_t - start_t, 2)}s')
start_t = evt.logMonoTime / 1e9
start_v = avg
max_t = 0
max_v = 0
if item.address == 0x1ab and item.src == 0:
motor_torque = ((item.dat[0] & 0x3) << 8) + item.dat[1]
window.append(motor_torque)
window.pop(0)
avg = sum(window) / len(window)
#print(f'{evt.logMonoTime}: {avg}')
if active and avg > max_v + 0.5:
max_v = avg
max_t = evt.logMonoTime / 1e9

2
selfdrive/debug/internal/messaging/boost/.gitignore vendored 100644
View File

@ -0,0 +1,2 @@
sender
receiver

61
selfdrive/debug/internal/messaging/boost/Makefile 100644
View File

@ -0,0 +1,61 @@
CC = clang
CXX = clang++
ARCH := $(shell uname -m)
OS := $(shell uname -o)
BASEDIR = ../../../..
PHONELIBS = ../../../../phonelibs
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) -Wall
CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS) -Wall
ifeq ($(ARCH),aarch64)
CFLAGS += -mcpu=cortex-a57
CXXFLAGS += -mcpu=cortex-a57
endif
EXTRA_LIBS = -lpthread
ifeq ($(ARCH),x86_64)
BOOST_LIBS = -lboost_system -lboost_locale -lrt
else
EXTRA_LIBS += -llog -luuid
endif
.PHONY: all
all: sender receiver
receiver: receiver.o
@echo "[ LINK ] $@"
$(CXX) -fPIC -o '$@' $^ \
$(CEREAL_LIBS) \
$(BOOST_LIBS) \
$(EXTRA_LIBS)
sender: sender.o
@echo "[ LINK ] $@"
$(CXX) -fPIC -o '$@' $^ \
$(CEREAL_LIBS) \
$(BOOST_LIBS) \
$(EXTRA_LIBS)
%.o: %.cc
@echo "[ CXX ] $@"
$(CXX) $(CXXFLAGS) -MMD \
-Iinclude -I.. -I../.. \
-I../ \
-I../../ \
-c -o '$@' '$<'
.PHONY: clean
clean:
rm -f *.d sender receiver *.o

54
selfdrive/debug/internal/messaging/boost/receiver.cc 100644
View File

@ -0,0 +1,54 @@
#include <boost/interprocess/ipc/message_queue.hpp>
#include <iostream>
#include <vector>
#include <thread>
using namespace boost::interprocess;
#define N 1024
message_queue *sub_queue(const char *name){
while (true){
try {
message_queue *mq = new message_queue(open_only, name);
return mq;
}
catch(interprocess_exception &ex){
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
}
}
message_queue *pub_queue(const char *name){
message_queue::remove(name);
message_queue *mq = new message_queue(create_only, name, 100, N);
return mq;
}
int main ()
{
message_queue::remove("queue_1");
message_queue::remove("queue_2");
message_queue *pq = pub_queue("queue_2");
message_queue *sq = sub_queue("queue_1");
std::cout << "Ready" << std::endl;
unsigned int priority;
std::size_t recvd_size;
char * rcv_msg = new char[N];
while (true){
sq->receive(rcv_msg, N, recvd_size, priority);
assert(N == recvd_size);
pq->send(rcv_msg, N, 0);
}
return 0;
}

62
selfdrive/debug/internal/messaging/boost/sender.cc 100644
View File

@ -0,0 +1,62 @@
#include <boost/interprocess/ipc/message_queue.hpp>
#include <iostream>
#include <vector>
#include <thread>
#include <chrono>
#include <cassert>
#define N 1024
#define MSGS 1e5
using namespace boost::interprocess;
message_queue *sub_queue(const char *name){
while (true){
try {
message_queue *mq = new message_queue(open_only, name);
return mq;
}
catch(interprocess_exception &ex){
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
}
}
message_queue *pub_queue(const char *name){
message_queue::remove(name);
message_queue *mq = new message_queue(create_only, name, 100, N);
return mq;
}
int main ()
{
message_queue *pq = pub_queue("queue_1");
message_queue *sq = sub_queue("queue_2");
std::cout << "Ready" << std::endl;
auto start = std::chrono::steady_clock::now();
char * rcv_msg = new char[N];
char * snd_msg = new char[N];
unsigned int priority;
std::size_t recvd_size;
for (int i = 0; i < MSGS; i++){
sprintf(snd_msg, "%d", i);
pq->send(snd_msg, N, 0);
sq->receive(rcv_msg, N, recvd_size, priority);
}
auto end = std::chrono::steady_clock::now();
double elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count() / 1e9;
double throughput = ((double) MSGS / (double) elapsed);
std::cout << "Elapsed: " << elapsed << " s" << std::endl;
std::cout << "Throughput: " << throughput << " msg/s" << std::endl;
return 0;
}

2
selfdrive/debug/internal/messaging/nanomsg/.gitignore vendored 100644
View File

@ -0,0 +1,2 @@
receiver
sender

65
selfdrive/debug/internal/messaging/nanomsg/Makefile 100644
View File

@ -0,0 +1,65 @@
CC = clang
CXX = clang++
ARCH := $(shell uname -m)
OS := $(shell uname -o)
BASEDIR = ../../../..
PHONELIBS = ../../../../phonelibs
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) -Wall
CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS) -Wall
# NANOMSG_LIBS = -l:libnanomsg.a
ifeq ($(ARCH),aarch64)
CFLAGS += -mcpu=cortex-a57
CXXFLAGS += -mcpu=cortex-a57
endif
EXTRA_LIBS = -lpthread
ifeq ($(ARCH),x86_64)
NANOMSG_FLAGS = -I$(BASEDIR)/phonelibs/nanomsg/x64/include
NANOMSG_LIBS = -L$(BASEDIR)/phonelibs/nanomsg/x64/lib \
-lnanomsg -Wl,-rpath,$(BASEDIR)/phonelibs/nanomsg/x64/lib
else
EXTRA_LIBS += -llog -luuid
endif
.PHONY: all
all: sender receiver
receiver: receiver.o
@echo "[ LINK ] $@"
$(CXX) -fPIC -o '$@' $^ \
$(NANOMSG_LIBS) \
$(EXTRA_LIBS)
sender: sender.o
@echo "[ LINK ] $@"
$(CXX) -fPIC -o '$@' $^ \
$(NANOMSG_LIBS) \
$(EXTRA_LIBS)
%.o: %.cc
@echo "[ CXX ] $@"
$(CXX) $(CXXFLAGS) -MMD \
-Iinclude -I.. -I../.. \
$(NANOMSG_FLAGS) \
$(JSON11_FLAGS) \
$(JSON_FLAGS) \
-I../ \
-I../../ \
-c -o '$@' '$<'
.PHONY: clean
clean:
rm -f *.d sender receiver

48
selfdrive/debug/internal/messaging/nanomsg/receiver.cc 100644
View File

@ -0,0 +1,48 @@
#include <future>
#include <cassert>
#include <iostream>
#include <cstring>
#include <thread>
#include <nanomsg/nn.h>
#include <nanomsg/pubsub.h>
#include <nanomsg/tcp.h>
#define N 1024
int sub_sock(const char *endpoint) {
int sock = nn_socket(AF_SP, NN_SUB);
assert(sock >= 0);
nn_setsockopt(sock, NN_SUB, NN_SUB_SUBSCRIBE, "", 0);
assert(nn_connect(sock, endpoint) >= 0);
return sock;
}
int pub_sock(const char *endpoint){
int sock = nn_socket(AF_SP, NN_PUB);
assert(sock >= 0);
int b = 1;
nn_setsockopt(sock, NN_TCP, NN_TCP_NODELAY, &b, sizeof(b));
assert(nn_bind(sock, endpoint) >= 0);
return sock;
}
int main(int argc, char *argv[]) {
auto p_sock = pub_sock("tcp://*:10011");
auto s_sock = sub_sock("tcp://127.0.0.1:10010");
std::cout << "Ready!" << std::endl;
char * msg = new char[N];
while (true){
int bytes = nn_recv(s_sock, msg, N, 0);
nn_send(p_sock, msg, bytes, 0);
}
return 0;
}

73
selfdrive/debug/internal/messaging/nanomsg/sender.cc 100644
View File

@ -0,0 +1,73 @@
#include <iostream>
#include <cassert>
#include <chrono>
#include <thread>
#include <nanomsg/nn.h>
#include <nanomsg/pubsub.h>
#include <nanomsg/tcp.h>
#define N 1024
#define MSGS 1e5
int sub_sock(const char *endpoint) {
int sock = nn_socket(AF_SP, NN_SUB);
assert(sock >= 0);
nn_setsockopt(sock, NN_SUB, NN_SUB_SUBSCRIBE, "", 0);
int timeout = 100;
nn_setsockopt(sock, NN_SOL_SOCKET, NN_RCVTIMEO, &timeout , sizeof(timeout));
assert(nn_connect(sock, endpoint) >= 0);
return sock;
}
int pub_sock(const char *endpoint){
int sock = nn_socket(AF_SP, NN_PUB);
assert(sock >= 0);
int b = 1;
nn_setsockopt(sock, NN_TCP, NN_TCP_NODELAY, &b, sizeof(b));
assert(nn_bind(sock, endpoint) >= 0);
return sock;
}
int main(int argc, char *argv[]) {
auto p_sock = pub_sock("tcp://*:10010");
auto s_sock = sub_sock("tcp://127.0.0.1:10011");
std::cout << "Ready!" << std::endl;
// auto p_sock = pub_sock("ipc:///tmp/feeds/3");
// auto s_sock = sub_sock("ipc:///tmp/feeds/2");
char * msg = new char[N];
auto start = std::chrono::steady_clock::now();
for (int i = 0; i < MSGS; i++){
sprintf(msg, "%d", i);
nn_send(p_sock, msg, N, 0);
int bytes = nn_recv(s_sock, msg, N, 0);
if (bytes < 0) {
std::cout << "Timeout" << std::endl;
}
}
auto end = std::chrono::steady_clock::now();
double elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count() / 1e9;
double throughput = ((double) MSGS / (double) elapsed);
std::cout << "Elapsed: " << elapsed << " s" << std::endl;
std::cout << "Throughput: " << throughput << " msg/s" << std::endl;
return 0;
}

2
selfdrive/debug/internal/messaging/nng/.gitignore vendored 100644
View File

@ -0,0 +1,2 @@
receiver
sender

68
selfdrive/debug/internal/messaging/nng/Makefile 100644
View File

@ -0,0 +1,68 @@
CC = clang
CXX = clang++
ARCH := $(shell uname -m)
OS := $(shell uname -o)
BASEDIR = ../../../..
PHONELIBS = ../../../../phonelibs
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) -Wall
CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS) -Wall
NNG_LIBS = -l:libnng.a
ifeq ($(ARCH),aarch64)
CFLAGS += -mcpu=cortex-a57
CXXFLAGS += -mcpu=cortex-a57
endif
EXTRA_LIBS = -lpthread
ifeq ($(ARCH),x86_64)
ZMQ_FLAGS = -I$(BASEDIR)/phonelibs/nng/x64/include
NNG_LIBS = -L$(BASEDIR)/phonelibs/nng/x64/lib \
-l:libnng.a
else
EXTRA_LIBS += -llog -luuid
endif
.PHONY: all
all: sender receiver
receiver: receiver.o
@echo "[ LINK ] $@"
$(CXX) -fPIC -o '$@' $^ \
$(CEREAL_LIBS) \
$(NNG_LIBS) \
$(EXTRA_LIBS)
sender: sender.o
@echo "[ LINK ] $@"
$(CXX) -fPIC -o '$@' $^ \
$(CEREAL_LIBS) \
$(NNG_LIBS) \
$(EXTRA_LIBS)
%.o: %.cc
@echo "[ CXX ] $@"
$(CXX) $(CXXFLAGS) -MMD \
-Iinclude -I.. -I../.. \
$(CEREAL_CXXFLAGS) \
$(ZMQ_FLAGS) \
$(JSON11_FLAGS) \
$(JSON_FLAGS) \
-I../ \
-I../../ \
-c -o '$@' '$<'
.PHONY: clean
clean:
rm -f *.d sender receiver

56
selfdrive/debug/internal/messaging/nng/receiver.cc 100644
View File

@ -0,0 +1,56 @@
#include <future>
#include <cassert>
#include <iostream>
#include <cstring>
#include <thread>
#include <nng/nng.h>
#include <nng/protocol/pubsub0/pub.h>
#include <nng/protocol/pubsub0/sub.h>
nng_socket sub_sock(const char *endpoint) {
nng_socket sock;
int r;
r = nng_sub0_open(&sock);
assert(r == 0);
nng_setopt(sock, NNG_OPT_SUB_SUBSCRIBE, "", 0);
while (true){
r = nng_dial(sock, endpoint, NULL, 0);
if (r == 0){
break;
}
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
return sock;
}
nng_socket pub_sock(const char *endpoint){
nng_socket sock;
int r;
r = nng_pub0_open(&sock);
assert(r == 0);
r = nng_listen(sock, endpoint, NULL, 0);
assert(r == 0);
return sock;
}
int main(int argc, char *argv[]) {
// auto p_sock = pub_sock("tcp://*:10004");
// auto s_sock = sub_sock("tcp://127.0.0.1:10003");
auto p_sock = pub_sock("ipc:///tmp/feeds/2");
auto s_sock = sub_sock("ipc:///tmp/feeds/3");
while (true){
nng_msg *msg;
nng_recvmsg(s_sock, &msg, 0);
nng_sendmsg(p_sock, msg, 0);
}
return 0;
}

78
selfdrive/debug/internal/messaging/nng/sender.cc 100644
View File

@ -0,0 +1,78 @@
#include <iostream>
#include <cassert>
#include <chrono>
#include <thread>
#include <nng/nng.h>
#include <nng/protocol/pubsub0/pub.h>
#include <nng/protocol/pubsub0/sub.h>
#define N 1024
#define MSGS 1e5
nng_socket sub_sock(const char *endpoint) {
nng_socket sock;
int r;
r = nng_sub0_open(&sock);
assert(r == 0);
nng_setopt(sock, NNG_OPT_SUB_SUBSCRIBE, "", 0);
nng_setopt_ms(sock, NNG_OPT_RECVTIMEO, 100);
while (true){
r = nng_dial(sock, endpoint, NULL, 0);
if (r == 0){
break;
}
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
return sock;
}
nng_socket pub_sock(const char *endpoint){
nng_socket sock;
int r;
r = nng_pub0_open(&sock);
assert(r == 0);
r = nng_listen(sock, endpoint, NULL, 0);
assert(r == 0);
return sock;
}
int main(int argc, char *argv[]) {
// auto p_sock = pub_sock("tcp://*:10003");
// auto s_sock = sub_sock("tcp://127.0.0.1:10004");
auto p_sock = pub_sock("ipc:///tmp/feeds/3");
auto s_sock = sub_sock("ipc:///tmp/feeds/2");
auto start = std::chrono::steady_clock::now();
for (int i = 0; i < MSGS; i++){
nng_msg *msg;
nng_msg_alloc(&msg, N);
nng_sendmsg(p_sock, msg, 0);
nng_msg *rmsg;
int r = nng_recvmsg(s_sock, &rmsg, 0);
if (r) {
std::cout << "Timeout" << std::endl;
}
}
auto end = std::chrono::steady_clock::now();
double elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count() / 1e9;
double throughput = ((double) MSGS / (double) elapsed);
std::cout << "Elapsed: " << elapsed << " s" << std::endl;
std::cout << "Throughput: " << throughput << " msg/s" << std::endl;
return 0;
}

2
selfdrive/debug/internal/messaging/zmq/.gitignore vendored 100644
View File

@ -0,0 +1,2 @@
receiver
sender

71
selfdrive/debug/internal/messaging/zmq/Makefile 100644
View File

@ -0,0 +1,71 @@
CC = clang
CXX = clang++
ARCH := $(shell uname -m)
OS := $(shell uname -o)
BASEDIR = ../../../../../
PHONELIBS = ../../../../../phonelibs
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) -Wall
CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS) -Wall
ZMQ_LIBS = -l:libczmq.a -l:libzmq.a
ifeq ($(ARCH),aarch64)
CFLAGS += -mcpu=cortex-a57
CXXFLAGS += -mcpu=cortex-a57
ZMQ_LIBS += -lgnustl_shared
endif
EXTRA_LIBS = -lpthread
ifeq ($(ARCH),x86_64)
ZMQ_FLAGS = -I$(BASEDIR)/phonelibs/zmq/x64/include
ZMQ_LIBS = -L$(BASEDIR)/external/zmq/lib \
-l:libczmq.a -l:libzmq.a
ZMQ_SHARED_LIBS = -L$(BASEDIR)/external/zmq/lib \
-lczmq -lzmq
else
EXTRA_LIBS += -llog -luuid
endif
.PHONY: all
all: sender receiver
receiver: receiver.o
@echo "[ LINK ] $@"
$(CXX) -fPIC -o '$@' $^ \
$(CEREAL_LIBS) \
$(ZMQ_LIBS) \
$(EXTRA_LIBS)
sender: sender.o
@echo "[ LINK ] $@"
$(CXX) -fPIC -o '$@' $^ \
$(CEREAL_LIBS) \
$(ZMQ_LIBS) \
$(EXTRA_LIBS)
%.o: %.cc
@echo "[ CXX ] $@"
$(CXX) $(CXXFLAGS) -MMD \
-Iinclude -I.. -I../.. \
$(CEREAL_CXXFLAGS) \
$(ZMQ_FLAGS) \
$(JSON11_FLAGS) \
$(JSON_FLAGS) \
-I../ \
-I../../ \
-c -o '$@' '$<'
.PHONY: clean
clean:
rm -f *.d sender receiver

49
selfdrive/debug/internal/messaging/zmq/receiver.cc 100644
View File

@ -0,0 +1,49 @@
#include <future>
#include <iostream>
#include <cstring>
#include <zmq.h>
// #define IPC
void *sub_sock(void *ctx, const char *endpoint) {
void* sock = zmq_socket(ctx, ZMQ_SUB);
zmq_connect(sock, endpoint);
zmq_setsockopt(sock, ZMQ_SUBSCRIBE, "", 0);
return sock;
}
void *pub_sock(void *ctx, const char *endpoint){
void * sock = zmq_socket(ctx, ZMQ_PUB);
zmq_bind(sock, endpoint);
return sock;
}
int main(int argc, char *argv[]) {
auto ctx = zmq_ctx_new();
#ifdef IPC
auto s_sock = sub_sock(ctx, "ipc:///tmp/q0");
auto p_sock = pub_sock(ctx, "ipc:///tmp/q1");
#else
auto s_sock = sub_sock(ctx, "tcp://localhost:10005");
auto p_sock = pub_sock(ctx, "tcp://*:10004");
#endif
zmq_msg_t msg;
zmq_msg_init(&msg);
while (true){
zmq_msg_recv(&msg, s_sock, 0);
zmq_msg_send(&msg, p_sock, ZMQ_DONTWAIT);
}
zmq_msg_close(&msg);
zmq_close(p_sock);
zmq_close(s_sock);
return 0;
}

65
selfdrive/debug/internal/messaging/zmq/sender.cc 100644
View File

@ -0,0 +1,65 @@
#include <iostream>
#include <zmq.h>
#include <chrono>
#define N 1024
#define MSGS 1e5
// #define IPC
void *sub_sock(void *ctx, const char *endpoint) {
void* sock = zmq_socket(ctx, ZMQ_SUB);
zmq_connect(sock, endpoint);
zmq_setsockopt(sock, ZMQ_SUBSCRIBE, "", 0);
int timeout = 100;
zmq_setsockopt(sock, ZMQ_RCVTIMEO, &timeout, sizeof(int));
return sock;
}
void *pub_sock(void *ctx, const char *endpoint){
void * sock = zmq_socket(ctx, ZMQ_PUB);
zmq_bind(sock, endpoint);
return sock;
}
int main(int argc, char *argv[]) {
auto ctx = zmq_ctx_new();
#ifdef IPC
auto s_sock = sub_sock(ctx, "ipc:///tmp/q1");
auto p_sock = pub_sock(ctx, "ipc:///tmp/q0");
#else
auto s_sock = sub_sock(ctx, "tcp://127.0.0.1:10004");
auto p_sock = pub_sock(ctx, "tcp://*:10005");
#endif
zmq_msg_t msg;
zmq_msg_init_size (&msg, N);
auto start = std::chrono::steady_clock::now();
for (int i = 0; i < MSGS; i++){
zmq_msg_send(&msg, p_sock, ZMQ_DONTWAIT);
int r = zmq_msg_recv(&msg, s_sock, 0);
if (r) {
start = std::chrono::steady_clock::now();
std::cout << "Timeout" << std::endl;
}
}
auto end = std::chrono::steady_clock::now();
double elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count() / 1e9;
double throughput = ((double) MSGS / (double) elapsed);
std::cout << "Elapsed: " << elapsed << " s" << std::endl;
std::cout << "Throughput: " << throughput << " msg/s" << std::endl;
zmq_close(p_sock);
zmq_close(s_sock);
return 0;
}

17
selfdrive/debug/internal/messaging_benchmark.py 100755
View File

@ -0,0 +1,17 @@
#!/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()
dat.init('controlsState')
dt = time.time() - t
print("Init message %d its, %f s" % (N, dt))
if __name__ == "__main__":
init_message_bench()

21
selfdrive/debug/internal/mock_process/fake_UiNavigationEvent.py 100755
View File

@ -0,0 +1,21 @@
#!/usr/bin/env python3
import zmq
import time
from cereal.services import service_list
import cereal.messaging as messaging
from cereal import log
def mock():
traffic_events = messaging.pub_sock('uiNavigationEvent')
while 1:
m = messaging.new_message()
m.init('uiNavigationEvent')
m.uiNavigationEvent.type = log.UiNavigationEvent.Type.mergeRight
m.uiNavigationEvent.status = log.UiNavigationEvent.Status.active
m.uiNavigationEvent.distanceTo = 100.
traffic_events.send(m.to_bytes())
time.sleep(0.01)
if __name__=="__main__":
mock()

22
selfdrive/debug/internal/mock_process/fake_controls_state.py 100755
View File

@ -0,0 +1,22 @@
#!/usr/bin/env python3
import time
import zmq
from hexdump import hexdump
from common.realtime import Ratekeeper
import cereal.messaging as messaging
from cereal.services import service_list
if __name__ == "__main__":
controls_state = messaging.pub_sock('controlsState')
rk = Ratekeeper(100)
while 1:
dat = messaging.new_message()
dat.init('controlsState')
dat.controlsState.vEgo = 25.
dat.controlsState.enabled = True
controls_state.send(dat.to_bytes())
rk.keep_time()

27
selfdrive/debug/internal/mock_process/fake_fusion_state.py 100755
View File

@ -0,0 +1,27 @@
#!/usr/bin/env python3
import zmq
import time
from hexdump import hexdump
import cereal.messaging as messaging
from cereal.services import service_list
from cereal import log
def leadRange(start, end, step):
x = start
while x < end:
yield x
x += (x * step)
def mock_lead():
radarState = messaging.pub_sock('radarState')
while 1:
m = messaging.new_message()
m.init('radarState')
m.radarState.leadOne.status = True
for x in leadRange(3.0, 65.0, 0.005):
m.radarState.leadOne.dRel = x
radarState.send(m.to_bytes())
time.sleep(0.01)
if __name__=="__main__":
mock_lead()

46
selfdrive/debug/internal/mock_process/fake_gps.py 100755
View File

@ -0,0 +1,46 @@
# mock_gps.py: Publishes a generated path moving at 15m/s to gpsLocation
# USAGE: python mock_gps.py
# Then start manager
from itertools import cycle
import time
import zmq
from cereal import log
import cereal.messaging as messaging
from cereal.services import service_list
degrees_per_meter = 0.000009000009 # approximation
start_lat = 43.64199141443989
start_lng = -94.97520411931725
def gen_path(length_seconds, speed=15):
return [{"lat": start_lat,
"lng": start_lng + speed * i * degrees_per_meter, # moving along longitudinal axis at speed m/s
"speed": speed}
for i in range(1, length_seconds + 1)]
if __name__ == '__main__':
gpsLocation = messaging.pub_sock('gpsLocation')
path_stopped_5s = [{"lat": start_lat, "lng": start_lng, "speed": 0}] * 5
path_moving = gen_path(30, speed=15)
path_stopped_5s_then_moving = path_stopped_5s + path_moving
for point in cycle(path_stopped_5s_then_moving):
print('sending gpsLocation from point: {}'.format(str(point)))
dat = messaging.new_message()
dat.init('gpsLocation')
dat.gpsLocation.latitude = point['lat']
dat.gpsLocation.longitude = point['lng']
dat.gpsLocation.speed = point['speed']
dat.gpsLocation.flags = 0
dat.gpsLocation.altitude = 0
dat.gpsLocation.bearing = 0 # todo we can mock this
dat.gpsLocation.accuracy = 1
dat.gpsLocation.timestamp = int(time.time() * 1000)
dat.gpsLocation.source = log.GpsLocationData.SensorSource.android
gpsLocation.send(dat.to_bytes())
time.sleep(1)

27
selfdrive/debug/internal/mock_process/fake_gps_external.py 100755
View File

@ -0,0 +1,27 @@
#!/usr/bin/env python3
import time
import zmq
from cereal import log
import cereal.messaging as messaging
from cereal.services import service_list
if __name__ == '__main__':
gpsLocationExternal = messaging.pub_sock('gpsLocationExternal')
while True:
dat = messaging.new_message()
dat.init('gpsLocationExternal')
dat.gpsLocationExternal.latitude = 37.6513687
dat.gpsLocationExternal.longitude = -122.4535056
dat.gpsLocationExternal.speed = 28.2
dat.gpsLocationExternal.flags = 1
dat.gpsLocationExternal.altitude = 75.
dat.gpsLocationExternal.bearing = 145.5
dat.gpsLocationExternal.accuracy = 1.
dat.gpsLocationExternal.timestamp = int(time.time() * 1000)
dat.gpsLocationExternal.source = log.GpsLocationData.SensorSource.ublox
gpsLocationExternal.send(dat.to_bytes())
time.sleep(.1)

22
selfdrive/debug/internal/mock_process/fake_liveMpc.py 100755
View File

@ -0,0 +1,22 @@
#!/usr/bin/env python3
import zmq
import time
from hexdump import hexdump
import cereal.messaging as messaging
from cereal.services import service_list
from cereal import log
def mock_x():
liveMpc = messaging.pub_sock('liveMpc')
while 1:
m = messaging.new_message()
mx = []
m.init('liveMpc')
for x in range(0, 100):
mx.append(x*1.0)
m.liveMpc.x = mx
liveMpc.send(m.to_bytes())
if __name__=="__main__":
mock_x()

22
selfdrive/debug/internal/mock_process/fake_trafficsignd.py 100755
View File

@ -0,0 +1,22 @@
#!/usr/bin/env python3
import zmq
import time
from cereal.services import service_list
import cereal.messaging as messaging
from cereal import log
def mock():
traffic_events = messaging.pub_sock('trafficEvents')
while 1:
m = messaging.new_message()
m.init('trafficEvents', 1)
m.trafficEvents[0].type = log.TrafficEvent.Type.stopSign
m.trafficEvents[0].resuming = False
m.trafficEvents[0].distance = 100.
m.trafficEvents[0].action = log.TrafficEvent.Action.stop
traffic_events.send(m.to_bytes())
time.sleep(0.01)
if __name__=="__main__":
mock()

41
selfdrive/debug/internal/model_timing_checker.py 100755
View File

@ -0,0 +1,41 @@
#!/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

96
selfdrive/debug/internal/polyfit_bench.py 100644
View File

@ -0,0 +1,96 @@
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))

64
selfdrive/debug/internal/power_monitor.py 100755
View File

@ -0,0 +1,64 @@
#!/usr/bin/env python3
import os
import time
import sys
from datetime import datetime
def average(avg, sample):
# Weighted avg between existing value and new sample
return ((avg[0] * avg[1] + sample) / (avg[1] + 1), avg[1] + 1)
if __name__ == '__main__':
try:
if len(sys.argv) > 1 and sys.argv[1] == "--charge":
print("not disabling charging")
else:
print("disabling charging")
os.system('echo "0" > /sys/class/power_supply/battery/charging_enabled')
voltage_average = (0., 0) # average, count
current_average = (0., 0)
power_average = (0., 0)
capacity_average = (0., 0)
bat_temp_average = (0., 0)
start_time = datetime.now()
while 1:
with open("/sys/class/power_supply/bms/voltage_now") as f:
voltage = int(f.read()) / 1e6 # volts
with open("/sys/class/power_supply/bms/current_now") as f:
current = int(f.read()) / 1e3 # ma
power = voltage * current
with open("/sys/class/power_supply/bms/capacity_raw") as f:
capacity = int(f.read()) / 1e2 # percent
with open("/sys/class/power_supply/bms/temp") as f:
bat_temp = int(f.read()) / 1e1 # celsius
# compute averages
voltage_average = average(voltage_average, voltage)
current_average = average(current_average, current)
power_average = average(power_average, power)
capacity_average = average(capacity_average, capacity)
bat_temp_average = average(bat_temp_average, bat_temp)
print("%.2f volts %12.2f ma %12.2f mW %8.2f%% battery %8.1f degC" % (voltage, current, power, capacity, bat_temp))
time.sleep(0.1)
finally:
stop_time = datetime.now()
print("\n----------------------Average-----------------------------------")
voltage = voltage_average[0]
current = current_average[0]
power = power_average[0]
capacity = capacity_average[0]
bat_temp = bat_temp_average[0]
print("%.2f volts %12.2f ma %12.2f mW %8.2f%% battery %8.1f degC" % (voltage, current, power, capacity, bat_temp))
print(" {:.2f} Seconds {} samples".format((stop_time-start_time).total_seconds(), voltage_average[1]))
print("----------------------------------------------------------------")
# reenable charging
os.system('echo "1" > /sys/class/power_supply/battery/charging_enabled')
print("charging enabled\n")

22
selfdrive/debug/internal/realtime_benchmark.py 100755
View File

@ -0,0 +1,22 @@
#!/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)

79
selfdrive/debug/internal/run_segment_through_visiond.py 100755
View File

@ -0,0 +1,79 @@
#!/usr/bin/env python3
import argparse
import time
import os
from tqdm import tqdm
from cereal.messaging import PubMaster, recv_one, sub_sock
from cereal.services import service_list
from tools.lib.logreader import LogReader
from xx.chffr.lib.route import Route, RouteSegment
from tools.lib.route_framereader import RouteFrameReader
from common.column_store import save_dict_as_column_store
from xx.pipeline.lib.log_time_series import append_dict
from selfdrive.test.process_replay.compare_logs import save_log
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="Run visiond on segment")
parser.add_argument("segment_name", help="The segment to run")
parser.add_argument("output_path", help="The output file")
args = parser.parse_args()
segment = RouteSegment.from_canonical_name(args.segment_name)
route = Route(segment._name._route_name)
frame_id_lookup = {}
frame_reader = RouteFrameReader(route.camera_paths(), None, frame_id_lookup, readahead=True)
msgs = list(LogReader(segment.log_path))
pm = PubMaster(['liveCalibration', 'frame'])
model_sock = sub_sock('model')
# Read encodeIdx
for msg in msgs:
if msg.which() == 'encodeIdx':
frame_id_lookup[msg.encodeIdx.frameId] = (msg.encodeIdx.segmentNum, msg.encodeIdx.segmentId)
# Send some livecalibration messages to initalize visiond
for msg in msgs:
if msg.which() == 'liveCalibration':
pm.send('liveCalibration', msg.as_builder())
time.sleep(1.0)
values = {}
out_msgs = []
for msg in tqdm(msgs):
w = msg.which()
if w == 'liveCalibration':
pm.send(w, msg.as_builder())
if w == 'frame':
msg = msg.as_builder()
frame_id = msg.frame.frameId
img = frame_reader.get(frame_id, pix_fmt="rgb24")[:,:,::-1]
msg.frame.image = img.flatten().tobytes()
pm.send(w, msg)
model = recv_one(model_sock)
model = model.as_builder()
model.logMonoTime = 0
model = model.as_reader()
out_msgs.append(model)
save_log(args.output_path, out_msgs)
# tm = model.logMonoTime / 1.0e9
# model = model.model
# append_dict("model/data/path", tm, model.path.to_dict(), values)
# append_dict("model/data/left_lane", tm, model.leftLane.to_dict(), values)
# append_dict("model/data/right_lane", tm, model.rightLane.to_dict(), values)
# append_dict("model/data/lead", tm, model.lead.to_dict(), values)
# save_dict_as_column_store(values, os.path.join(args.output_path, "LiveVisionD", args.segment_name))

22
selfdrive/debug/internal/send_alert.py 100644
View File

@ -0,0 +1,22 @@
#!/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()
dat.init('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)

107
selfdrive/debug/internal/status.py 100644
View File

@ -0,0 +1,107 @@
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()

83
selfdrive/debug/internal/time_to_collision.py 100644
View File

@ -0,0 +1,83 @@
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()

178
selfdrive/debug/internal/topdown.py 100755
View File

@ -0,0 +1,178 @@
#!/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()

152
selfdrive/debug/internal/toyota/blindspot.py 100755
View File

@ -0,0 +1,152 @@
#!/usr/bin/env python3
import zmq
import time
import random
from collections import defaultdict, OrderedDict
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.car.toyota.toyotacan import make_can_msg
import cereal.messaging as messaging
from cereal.services import service_list
fields = range(0, 256)
fields = [105, 225]
field_results = defaultdict(lambda: "\x00\x00")
cur_field = 97
def send(sendcan, addr, m):
packet = make_can_msg(addr, m, 0, False)
packets = can_list_to_can_capnp([packet], msgtype='sendcan')
sendcan.send(packets.to_bytes())
def recv(can, addr):
received = False
r = []
while not received:
c = messaging.recv_one(can)
for msg in c.can:
if msg.address == addr:
r.append(msg)
received = True
return r
def recv_timeout(can, addr):
received = False
r = []
t = time.time()
while not received:
c = messaging.recv_one_or_none(can)
if c is not None:
for msg in c.can:
if msg.address == addr:
r.append(msg)
received = True
if time.time() - t > 0.05:
received = True
return r
def print_hex(d):
s = map(ord, d)
s = "".join(["{:02X}".format(b) for b in s])
print(s)
TYPES = {
0: 'single',
1: 'first',
2: 'consecutive',
3: 'flow'
}
FIRST = "\x42\x02\xA8\x01\x00\x00\x00\x00"
CONTINUE = "\x42\x30\x01\x00\x00\x00\x00\x00"
TEST_ON = "\x42\x02\x10\x60\x00\x00\x00\x00"
TEST_OFF = "\x42\x02\x10\x5F\x00\x00\x00\x00"
POLL = "\x42\x02\x21\x69\x00\x00\x00\x00"
prev_rcv_t = ""
recv_data = []
l = 0
index = 0
can = messaging.sub_sock('can')
sendcan = messaging.pub_sock('sendcan')
time.sleep(0.5)
send(sendcan, 1872, FIRST)
results = []
test_mode = False
while True:
# Send flow control if necessary
if prev_rcv_t == 'first' or prev_rcv_t == 'consecutive':
send(sendcan, 1872, CONTINUE)
received = recv_timeout(can, 1880)
if len(received) == 0:
print(chr(27) + "[2J")
print(time.time())
if results[0] != "\x7F\x21\x31":
field_results[cur_field] = results[0]
else:
fields.remove(cur_field)
for k in fields:
if field_results[k] == "\x00\x00":
continue
print(k, end=' ')
print_hex(field_results[k])
results = []
if not test_mode:
send(sendcan, 1872, TEST_ON)
test_mode = True
else:
cur_field = random.choice(fields)
send(sendcan, 1872, POLL.replace('\x69', chr(cur_field)))
for r in received:
data = r.dat
# Check message type
t = TYPES[ord(data[1]) >> 4]
if t == 'single':
l = ord(data[1]) & 0x0F
elif t == 'first':
a = ord(data[1]) & 0x0F
b = ord(data[2])
l = b + (a << 8)
recv_data = []
prev_rcv_t = t
if t == 'single':
recv_data = data[2: 2 + l]
results.append(recv_data)
if t == 'first':
index = 0
recv_data += data[3: min(8, 3 + l)]
if t == 'consecutive':
index += 1
assert index == ord(data[1]) & 0x0F
pending_l = l - len(recv_data)
recv_data += data[2: min(8, 2 + pending_l)]
if len(recv_data) == l:
prev_rcv_t = ""
results.append(recv_data)

151
selfdrive/debug/internal/toyota/blindspot2.py 100755
View File

@ -0,0 +1,151 @@
#!/usr/bin/env python3
import zmq
import time
import random
from collections import defaultdict, OrderedDict
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.car.toyota.toyotacan import make_can_msg
import cereal.messaging as messaging
from cereal.services import service_list
fields = range(0, 256)
# fields = [105, 225]
fields = [105]
field_results = defaultdict(lambda: "\x00\x00")
cur_field = 97
def send(sendcan, addr, m):
packet = make_can_msg(addr, m, 0, False)
packets = can_list_to_can_capnp([packet], msgtype='sendcan')
sendcan.send(packets.to_bytes())
def recv(can, addr):
received = False
r = []
while not received:
c = messaging.recv_one(can)
for msg in c.can:
if msg.address == addr:
r.append(msg)
received = True
return r
def recv_timeout(can, addr):
received = False
r = []
t = time.time()
while not received:
c = messaging.recv_one_or_none(can)
if c is not None:
for msg in c.can:
if msg.address == addr:
r.append(msg)
received = True
if time.time() - t > 0.05:
received = True
return r
def print_hex(d):
s = map(ord, d)
s = "".join(["{:02X}".format(b) for b in s])
print(s)
TYPES = {
0: 'single',
1: 'first',
2: 'consecutive',
3: 'flow'
}
FIRST = "\xFF\x02\xA8\x01\x00\x00\x00\x00"
CONTINUE = "\xFF\x30\x01\x00\x00\x00\x00\x00"
TEST_ON = "\xFF\x02\x10\x01\x00\x00\x00\x00"
POLL = "\xFF\x02\x21\x69\x00\x00\x00\x00"
prev_rcv_t = ""
recv_data = []
l = 0
index = 0
can = messaging.sub_sock('can')
sendcan = messaging.pub_sock('sendcan')
time.sleep(0.5)
send(sendcan, 1872, FIRST)
results = []
test_mode = False
while True:
# Send flow control if necessary
if prev_rcv_t == 'first' or prev_rcv_t == 'consecutive':
send(sendcan, 1872, CONTINUE)
received = recv_timeout(can, 1880)
if len(received) == 0:
print_hex(results[0])
# print chr(27) + "[2J"
# print time.time()
# if results[0] != "\x7F\x21\x31":
# field_results[cur_field] = results[0]
# else:
# fields.remove(cur_field)
# for k in fields:
# if field_results[k] == "\x00\x00":
# continue
# print k,
# print_hex(field_results[k])
results = []
if not test_mode:
send(sendcan, 1872, TEST_ON)
test_mode = True
else:
cur_field = random.choice(fields)
send(sendcan, 1872, POLL.replace('\x69', chr(cur_field)))
for r in received:
data = r.dat
# Check message type
t = TYPES[ord(data[1]) >> 4]
if t == 'single':
l = ord(data[1]) & 0x0F
elif t == 'first':
a = ord(data[1]) & 0x0F
b = ord(data[2])
l = b + (a << 8)
recv_data = []
prev_rcv_t = t
if t == 'single':
recv_data = data[2: 2 + l]
results.append(recv_data)
if t == 'first':
index = 0
recv_data += data[3: min(8, 3 + l)]
if t == 'consecutive':
index += 1
assert index == ord(data[1]) & 0x0F
pending_l = l - len(recv_data)
recv_data += data[2: min(8, 2 + pending_l)]
if len(recv_data) == l:
prev_rcv_t = ""
results.append(recv_data)

41
selfdrive/debug/internal/toyota/blinker_toyota.py 100755
View File

@ -0,0 +1,41 @@
#!/usr/bin/env python3
import zmq
import time
from collections import defaultdict, OrderedDict
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.car.toyota.toyotacan import make_can_msg
import cereal.messaging as messaging
from cereal.services import service_list
can = messaging.sub_sock('can')
sendcan = messaging.pub_sock('sendcan')
BEFORE = [
"\x10\x15\x30\x0B\x00\x00\x00\x00",
"\x21\x00\x00\x00\x00\x00\x00\x00",
]
LEFT = "\x22\x00\x00\x08\x00\x00\x00\x00"
RIGHT = "\x22\x00\x00\x04\x00\x00\x00\x00"
OFF = "\x22\x00\x00\x00\x00\x00\x00\x00"
AFTER = "\x23\x00\x00\x00\x00\x00\x00\x00"
i = 0
j = 0
while True:
i += 1
if i % 10 == 0:
j += 1
cur = RIGHT if j % 2 == 0 else OFF
can_list = [make_can_msg(1984, d, 0, False) for d in BEFORE]
can_list.append(make_can_msg(1984, cur, 0, False))
can_list.append(make_can_msg(1984, AFTER, 0, False))
for m in can_list:
sendcan.send(can_list_to_can_capnp([m], msgtype='sendcan').to_bytes())
time.sleep(0.01)

61
selfdrive/debug/internal/toyota/continental_radar.py 100755
View File

@ -0,0 +1,61 @@
#!/usr/bin/env python3
import zmq
import time
from collections import defaultdict, OrderedDict
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.car.toyota.toyotacan import make_can_msg
import cereal.messaging as messaging
from cereal.services import service_list
def send(sendcan, addr, m):
packet = make_can_msg(addr, m, 0, False)
packets = can_list_to_can_capnp([packet], msgtype='sendcan')
sendcan.send(packets.to_bytes())
def recv_timeout(can, addr):
received = False
r = []
t = time.time()
while not received:
c = messaging.recv_one_or_none(can)
if c is not None:
for msg in c.can:
if msg.address == addr:
r.append(msg)
received = True
if time.time() - t > 0.1:
received = True
return r
can = messaging.sub_sock('can')
sendcan = messaging.pub_sock('sendcan')
PUBLIC = 0
PRIVATE = 1
time.sleep(0.5)
# 1, 112
TEST_ON = "\xFF\x02\x10\x70\x00\x00\x00\x00"
POLL = "\xFF\x02\x21\x69\x00\x00\x00\x00"
send(sendcan, 1872, TEST_ON)
r = recv_timeout(can, 1880)
print(r)
for i in range(0, 256):
send(sendcan, 1872, POLL.replace('\x69', chr(i)))
r = recv_timeout(can, 1880)
if len(r):
print(i, end=' ')
for m in r:
print(m.dat.encode('hex'))

26
selfdrive/debug/internal/toyota/simple_can_printer.py 100755
View File

@ -0,0 +1,26 @@
#!/usr/bin/env python3
import zmq
from collections import OrderedDict
import cereal.messaging as messaging
from cereal.services import service_list
can = messaging.sub_sock('can')
addr = OrderedDict()
while True:
c = messaging.recv_one(can)
for msg in c.can:
s = map(ord, msg.dat)
s = "".join(["\\x{:02X}".format(b) for b in s])
s = "\"" + s + "\","
if msg.address == 1872:
print("s:", s)
if msg.address == 1880:
print("r:", s)
if msg.address not in addr:
addr[msg.address] = list()
if msg.dat not in addr[msg.address]:
addr[msg.address].append(s)

159
selfdrive/debug/internal/toyota/sonar.py 100755
View File

@ -0,0 +1,159 @@
#!/usr/bin/env python3
import sys
import zmq
import os
import time
import random
from collections import defaultdict, OrderedDict
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.car.toyota.toyotacan import make_can_msg
import cereal.messaging as messaging
from cereal.services import service_list
changing = []
fields = range(0, 256)
# fields = [225, 50, 39, 40]
fields = [50]
field_results = defaultdict(lambda: "\x00\x00")
cur_field = 97
def send(sendcan, addr, m):
packet = make_can_msg(addr, m, 0, False)
packets = can_list_to_can_capnp([packet], msgtype='sendcan')
sendcan.send(packets.to_bytes())
def recv(can, addr):
received = False
r = []
while not received:
c = messaging.recv_one(can)
for msg in c.can:
if msg.address == addr:
r.append(msg)
received = True
return r
def recv_timeout(can, addr):
received = False
r = []
t = time.time()
while not received:
c = messaging.recv_one_or_none(can)
if c is not None:
for msg in c.can:
if msg.address == addr:
r.append(msg)
received = True
if time.time() - t > 0.05:
received = True
return r
def print_hex(d):
s = map(ord, d)
s = "".join(["{:02X}".format(b) for b in s])
print(s)
TYPES = {
0: 'single',
1: 'first',
2: 'consecutive',
3: 'flow'
}
CONTINUE = "\x67\x30\x01\x00\x00\x00\x00\x00"
TEST_ON = "\x67\x02\x10\x74\x00\x00\x00\x00"
POLL = "\x67\x02\x21\x69\x00\x00\x00\x00"
# POLL = "\x67\x02\x10\x69\x00\x00\x00\x00"
prev_rcv_t = ""
recv_data = []
l = 0
index = 0
can = messaging.sub_sock('can')
sendcan = messaging.pub_sock('sendcan')
time.sleep(0.5)
results = []
test_mode = False
while True:
# Send flow control if necessary
if prev_rcv_t == 'first' or prev_rcv_t == 'consecutive':
send(sendcan, 1872, CONTINUE)
received = recv_timeout(can, 1880)
if len(received) == 0:
sys.stdout.flush()
print(chr(27) + "[2J")
print(time.time())
print(changing)
if len(results):
if results[0] != "\x7F\x21\x31":
old = field_results[cur_field]
if old != '\x00\x00' and old != results[0] and cur_field not in changing:
changing.append(cur_field)
field_results[cur_field] = results[0]
else:
fields.remove(cur_field)
for k in fields:
# if field_results[k] == "\x00\x00":
# continue
print(k, end=' ')
print_hex(field_results[k])
results = []
if not test_mode:
send(sendcan, 1872, TEST_ON)
test_mode = True
else:
cur_field = random.choice(fields)
send(sendcan, 1872, POLL.replace('\x69', chr(cur_field)))
for r in received:
data = r.dat
# Check message type
t = TYPES[ord(data[1]) >> 4]
if t == 'single':
l = ord(data[1]) & 0x0F
elif t == 'first':
a = ord(data[1]) & 0x0F
b = ord(data[2])
l = b + (a << 8)
recv_data = []
prev_rcv_t = t
if t == 'single':
recv_data = data[2: 2 + l]
results.append(recv_data)
if t == 'first':
index = 0
recv_data += data[3: min(8, 3 + l)]
if t == 'consecutive':
index += 1
assert index == ord(data[1]) & 0x0F
pending_l = l - len(recv_data)
recv_data += data[2: min(8, 2 + pending_l)]
if len(recv_data) == l:
prev_rcv_t = ""
results.append(recv_data)

54
selfdrive/debug/internal/vehicle_model_sim.py 100644
View File

@ -0,0 +1,54 @@
#!/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)

49
selfdrive/debug/live_cpu_and_temp.py 100755
View File

@ -0,0 +1,49 @@
#!/usr/bin/env python3
import numpy as np
from cereal.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 ))

110
selfdrive/debug/mpc/live_lateral_mpc.py 100755
View File

@ -0,0 +1,110 @@
#!/usr/bin/env python3
import matplotlib
matplotlib.use('TkAgg')
import sys
import cereal.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('liveMpc', addr=addr)
model = messaging.sub_sock('model', addr=addr)
path_plan_sock = messaging.sub_sock('pathPlan', 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()

106
selfdrive/debug/mpc/live_longitudinal_mpc.py 100755
View File

@ -0,0 +1,106 @@
#!/usr/bin/env python3
import sys
import cereal.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('liveLongitudinalMpc', addr=addr, conflate=True)
radarstate = messaging.sub_sock('radarState', 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()

129
selfdrive/debug/mpc/test_mpc_wobble.py 100755
View File

@ -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()

186
selfdrive/debug/mpc/tune_lateral.py 100755
View File

@ -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()

168
selfdrive/debug/mpc/tune_longitudinal.py 100755
View File

@ -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()

27
selfdrive/debug/show_matching_cars.py 100755
View File

@ -0,0 +1,27 @@
#!/usr/bin/env python3
from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_known_cars
import cereal.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)

67
selfdrive/debug/tuner.py 100755
View File

@ -0,0 +1,67 @@
#!/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:
- h: decrease by 0.05
- l: increase by 0.05
- k: move pointer up
- j: move pointer down
"""
import tty
import sys
import json
import termios
from collections import OrderedDict
FILENAME = '/data/tuning.json'
def read_tuning():
while True:
try:
return json.loads(open(FILENAME).read())
except:
pass
def main():
dat = json.loads(open(FILENAME, 'r').read())
dat = OrderedDict(sorted(dat.items(), key=lambda i: i[0]))
cur = 0
while True:
sys.stdout.write("\x1Bc")
for i, (k, v) in enumerate(dat.items()):
prefix = "> " if cur == i else " "
print((prefix + k).ljust(20) + "%.2f" % v)
key = sys.stdin.read(1)[0]
write = False
if key == "k":
cur = max(0, cur - 1)
elif key == "j":
cur = min(len(dat.keys()) - 1, cur + 1)
elif key == "l":
dat[dat.keys()[cur]] += 0.05
write = True
elif key == "h":
dat[dat.keys()[cur]] -= 0.05
write = True
elif key == "q":
break
if write:
open(FILENAME, 'w').write(json.dumps(dat))
if __name__ == "__main__":
orig_settings = termios.tcgetattr(sys.stdin)
tty.setcbreak(sys.stdin)
try:
main()
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, orig_settings)
except:
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, orig_settings)
raise