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