From 2048a6e3d3e2e41dd8b237efc2bac20e1c339225 Mon Sep 17 00:00:00 2001 From: George Hotz Date: Fri, 17 Jan 2020 11:23:21 -0800 Subject: [PATCH] selfdrive/debug old-commit-hash: f467642a1cd4b47d70a0fe3ae894e11350f6e28c --- selfdrive/debug/__init__.py | 0 selfdrive/debug/can_printer.py | 42 ++++ selfdrive/debug/check_freq.py | 43 ++++ selfdrive/debug/compare_fingerprints.py | 18 ++ selfdrive/debug/cpu_usage_stat.py | 120 +++++++++++ selfdrive/debug/dump.py | 64 ++++++ selfdrive/debug/get_fingerprint.py | 29 +++ selfdrive/debug/internal/__init__.py | 0 selfdrive/debug/internal/benchmark_sleep.py | 21 ++ selfdrive/debug/internal/camera_small_box.py | 113 +++++++++++ .../debug/internal/convert_csv_to_route.py | 98 +++++++++ selfdrive/debug/internal/cycle_alerts.py | 53 +++++ selfdrive/debug/internal/design_lqr.py | 32 +++ .../debug/internal/eval_live_location.py | 48 +++++ .../internal/get_fingerprint_from_route.py | 33 ++++ selfdrive/debug/internal/gps_planner.py | 89 +++++++++ .../debug/internal/honda_sniff_steering.py | 113 +++++++++++ .../internal/measure_torque_time_to_max.py | 57 ++++++ .../debug/internal/messaging/boost/.gitignore | 2 + .../debug/internal/messaging/boost/Makefile | 61 ++++++ .../internal/messaging/boost/receiver.cc | 54 +++++ .../debug/internal/messaging/boost/sender.cc | 62 ++++++ .../internal/messaging/nanomsg/.gitignore | 2 + .../debug/internal/messaging/nanomsg/Makefile | 65 ++++++ .../internal/messaging/nanomsg/receiver.cc | 48 +++++ .../internal/messaging/nanomsg/sender.cc | 73 +++++++ .../debug/internal/messaging/nng/.gitignore | 2 + .../debug/internal/messaging/nng/Makefile | 68 +++++++ .../debug/internal/messaging/nng/receiver.cc | 56 ++++++ .../debug/internal/messaging/nng/sender.cc | 78 ++++++++ .../debug/internal/messaging/zmq/.gitignore | 2 + .../debug/internal/messaging/zmq/Makefile | 71 +++++++ .../debug/internal/messaging/zmq/receiver.cc | 49 +++++ .../debug/internal/messaging/zmq/sender.cc | 65 ++++++ .../debug/internal/messaging_benchmark.py | 17 ++ .../mock_process/fake_UiNavigationEvent.py | 21 ++ .../mock_process/fake_controls_state.py | 22 +++ .../mock_process/fake_fusion_state.py | 27 +++ .../debug/internal/mock_process/fake_gps.py | 46 +++++ .../mock_process/fake_gps_external.py | 27 +++ .../internal/mock_process/fake_liveMpc.py | 22 +++ .../mock_process/fake_trafficsignd.py | 22 +++ .../debug/internal/model_timing_checker.py | 41 ++++ selfdrive/debug/internal/polyfit_bench.py | 96 +++++++++ selfdrive/debug/internal/power_monitor.py | 64 ++++++ .../debug/internal/realtime_benchmark.py | 22 +++ .../internal/run_segment_through_visiond.py | 79 ++++++++ selfdrive/debug/internal/send_alert.py | 22 +++ selfdrive/debug/internal/status.py | 107 ++++++++++ selfdrive/debug/internal/time_to_collision.py | 83 ++++++++ selfdrive/debug/internal/topdown.py | 178 +++++++++++++++++ selfdrive/debug/internal/toyota/blindspot.py | 152 ++++++++++++++ selfdrive/debug/internal/toyota/blindspot2.py | 151 ++++++++++++++ .../debug/internal/toyota/blinker_toyota.py | 41 ++++ .../internal/toyota/continental_radar.py | 61 ++++++ .../internal/toyota/simple_can_printer.py | 26 +++ selfdrive/debug/internal/toyota/sonar.py | 159 +++++++++++++++ selfdrive/debug/internal/vehicle_model_sim.py | 54 +++++ selfdrive/debug/live_cpu_and_temp.py | 49 +++++ selfdrive/debug/mpc/live_lateral_mpc.py | 110 +++++++++++ selfdrive/debug/mpc/live_longitudinal_mpc.py | 106 ++++++++++ selfdrive/debug/mpc/test_mpc_wobble.py | 129 ++++++++++++ selfdrive/debug/mpc/tune_lateral.py | 186 ++++++++++++++++++ selfdrive/debug/mpc/tune_longitudinal.py | 168 ++++++++++++++++ selfdrive/debug/show_matching_cars.py | 27 +++ selfdrive/debug/tuner.py | 67 +++++++ 66 files changed, 4113 insertions(+) create mode 100644 selfdrive/debug/__init__.py create mode 100755 selfdrive/debug/can_printer.py create mode 100755 selfdrive/debug/check_freq.py create mode 100755 selfdrive/debug/compare_fingerprints.py create mode 100755 selfdrive/debug/cpu_usage_stat.py create mode 100755 selfdrive/debug/dump.py create mode 100755 selfdrive/debug/get_fingerprint.py create mode 100644 selfdrive/debug/internal/__init__.py create mode 100644 selfdrive/debug/internal/benchmark_sleep.py create mode 100755 selfdrive/debug/internal/camera_small_box.py create mode 100755 selfdrive/debug/internal/convert_csv_to_route.py create mode 100644 selfdrive/debug/internal/cycle_alerts.py create mode 100755 selfdrive/debug/internal/design_lqr.py create mode 100755 selfdrive/debug/internal/eval_live_location.py create mode 100755 selfdrive/debug/internal/get_fingerprint_from_route.py create mode 100755 selfdrive/debug/internal/gps_planner.py create mode 100755 selfdrive/debug/internal/honda_sniff_steering.py create mode 100755 selfdrive/debug/internal/measure_torque_time_to_max.py create mode 100644 selfdrive/debug/internal/messaging/boost/.gitignore create mode 100644 selfdrive/debug/internal/messaging/boost/Makefile create mode 100644 selfdrive/debug/internal/messaging/boost/receiver.cc create mode 100644 selfdrive/debug/internal/messaging/boost/sender.cc create mode 100644 selfdrive/debug/internal/messaging/nanomsg/.gitignore create mode 100644 selfdrive/debug/internal/messaging/nanomsg/Makefile create mode 100644 selfdrive/debug/internal/messaging/nanomsg/receiver.cc create mode 100644 selfdrive/debug/internal/messaging/nanomsg/sender.cc create mode 100644 selfdrive/debug/internal/messaging/nng/.gitignore create mode 100644 selfdrive/debug/internal/messaging/nng/Makefile create mode 100644 selfdrive/debug/internal/messaging/nng/receiver.cc create mode 100644 selfdrive/debug/internal/messaging/nng/sender.cc create mode 100644 selfdrive/debug/internal/messaging/zmq/.gitignore create mode 100644 selfdrive/debug/internal/messaging/zmq/Makefile create mode 100644 selfdrive/debug/internal/messaging/zmq/receiver.cc create mode 100644 selfdrive/debug/internal/messaging/zmq/sender.cc create mode 100755 selfdrive/debug/internal/messaging_benchmark.py create mode 100755 selfdrive/debug/internal/mock_process/fake_UiNavigationEvent.py create mode 100755 selfdrive/debug/internal/mock_process/fake_controls_state.py create mode 100755 selfdrive/debug/internal/mock_process/fake_fusion_state.py create mode 100755 selfdrive/debug/internal/mock_process/fake_gps.py create mode 100755 selfdrive/debug/internal/mock_process/fake_gps_external.py create mode 100755 selfdrive/debug/internal/mock_process/fake_liveMpc.py create mode 100755 selfdrive/debug/internal/mock_process/fake_trafficsignd.py create mode 100755 selfdrive/debug/internal/model_timing_checker.py create mode 100644 selfdrive/debug/internal/polyfit_bench.py create mode 100755 selfdrive/debug/internal/power_monitor.py create mode 100755 selfdrive/debug/internal/realtime_benchmark.py create mode 100755 selfdrive/debug/internal/run_segment_through_visiond.py create mode 100644 selfdrive/debug/internal/send_alert.py create mode 100644 selfdrive/debug/internal/status.py create mode 100644 selfdrive/debug/internal/time_to_collision.py create mode 100755 selfdrive/debug/internal/topdown.py create mode 100755 selfdrive/debug/internal/toyota/blindspot.py create mode 100755 selfdrive/debug/internal/toyota/blindspot2.py create mode 100755 selfdrive/debug/internal/toyota/blinker_toyota.py create mode 100755 selfdrive/debug/internal/toyota/continental_radar.py create mode 100755 selfdrive/debug/internal/toyota/simple_can_printer.py create mode 100755 selfdrive/debug/internal/toyota/sonar.py create mode 100644 selfdrive/debug/internal/vehicle_model_sim.py create mode 100755 selfdrive/debug/live_cpu_and_temp.py create mode 100755 selfdrive/debug/mpc/live_lateral_mpc.py create mode 100755 selfdrive/debug/mpc/live_longitudinal_mpc.py create mode 100755 selfdrive/debug/mpc/test_mpc_wobble.py create mode 100755 selfdrive/debug/mpc/tune_lateral.py create mode 100755 selfdrive/debug/mpc/tune_longitudinal.py create mode 100755 selfdrive/debug/show_matching_cars.py create mode 100755 selfdrive/debug/tuner.py diff --git a/selfdrive/debug/__init__.py b/selfdrive/debug/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/debug/can_printer.py b/selfdrive/debug/can_printer.py new file mode 100755 index 0000000000..f06db79545 --- /dev/null +++ b/selfdrive/debug/can_printer.py @@ -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() diff --git a/selfdrive/debug/check_freq.py b/selfdrive/debug/check_freq.py new file mode 100755 index 0000000000..75c0b8f89a --- /dev/null +++ b/selfdrive/debug/check_freq.py @@ -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 diff --git a/selfdrive/debug/compare_fingerprints.py b/selfdrive/debug/compare_fingerprints.py new file mode 100755 index 0000000000..a0c80a740e --- /dev/null +++ b/selfdrive/debug/compare_fingerprints.py @@ -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") diff --git a/selfdrive/debug/cpu_usage_stat.py b/selfdrive/debug/cpu_usage_stat.py new file mode 100755 index 0000000000..160f4f9d55 --- /dev/null +++ b/selfdrive/debug/cpu_usage_stat.py @@ -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 + )) diff --git a/selfdrive/debug/dump.py b/selfdrive/debug/dump.py new file mode 100755 index 0000000000..26ce350530 --- /dev/null +++ b/selfdrive/debug/dump.py @@ -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) diff --git a/selfdrive/debug/get_fingerprint.py b/selfdrive/debug/get_fingerprint.py new file mode 100755 index 0000000000..9ece7d7268 --- /dev/null +++ b/selfdrive/debug/get_fingerprint.py @@ -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)) diff --git a/selfdrive/debug/internal/__init__.py b/selfdrive/debug/internal/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/debug/internal/benchmark_sleep.py b/selfdrive/debug/internal/benchmark_sleep.py new file mode 100644 index 0000000000..7774ce8d97 --- /dev/null +++ b/selfdrive/debug/internal/benchmark_sleep.py @@ -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))) diff --git a/selfdrive/debug/internal/camera_small_box.py b/selfdrive/debug/internal/camera_small_box.py new file mode 100755 index 0000000000..082e711baf --- /dev/null +++ b/selfdrive/debug/internal/camera_small_box.py @@ -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) diff --git a/selfdrive/debug/internal/convert_csv_to_route.py b/selfdrive/debug/internal/convert_csv_to_route.py new file mode 100755 index 0000000000..f97e4617cb --- /dev/null +++ b/selfdrive/debug/internal/convert_csv_to_route.py @@ -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]) diff --git a/selfdrive/debug/internal/cycle_alerts.py b/selfdrive/debug/internal/cycle_alerts.py new file mode 100644 index 0000000000..6c5036fb63 --- /dev/null +++ b/selfdrive/debug/internal/cycle_alerts.py @@ -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) diff --git a/selfdrive/debug/internal/design_lqr.py b/selfdrive/debug/internal/design_lqr.py new file mode 100755 index 0000000000..0304df5aa1 --- /dev/null +++ b/selfdrive/debug/internal/design_lqr.py @@ -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)) diff --git a/selfdrive/debug/internal/eval_live_location.py b/selfdrive/debug/internal/eval_live_location.py new file mode 100755 index 0000000000..527162f36a --- /dev/null +++ b/selfdrive/debug/internal/eval_live_location.py @@ -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() + + diff --git a/selfdrive/debug/internal/get_fingerprint_from_route.py b/selfdrive/debug/internal/get_fingerprint_from_route.py new file mode 100755 index 0000000000..27e84a8ed3 --- /dev/null +++ b/selfdrive/debug/internal/get_fingerprint_from_route.py @@ -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 ") + sys.exit(1) + + route = sys.argv[1] + route = Route(route) + lr = MultiLogIterator(route.log_paths(), wraparound=False) + get_fingerprint(lr) diff --git a/selfdrive/debug/internal/gps_planner.py b/selfdrive/debug/internal/gps_planner.py new file mode 100755 index 0000000000..7204a726b8 --- /dev/null +++ b/selfdrive/debug/internal/gps_planner.py @@ -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() diff --git a/selfdrive/debug/internal/honda_sniff_steering.py b/selfdrive/debug/internal/honda_sniff_steering.py new file mode 100755 index 0000000000..2dc32d90b1 --- /dev/null +++ b/selfdrive/debug/internal/honda_sniff_steering.py @@ -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() + diff --git a/selfdrive/debug/internal/measure_torque_time_to_max.py b/selfdrive/debug/internal/measure_torque_time_to_max.py new file mode 100755 index 0000000000..65f13afc84 --- /dev/null +++ b/selfdrive/debug/internal/measure_torque_time_to_max.py @@ -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 diff --git a/selfdrive/debug/internal/messaging/boost/.gitignore b/selfdrive/debug/internal/messaging/boost/.gitignore new file mode 100644 index 0000000000..5a1c58ff00 --- /dev/null +++ b/selfdrive/debug/internal/messaging/boost/.gitignore @@ -0,0 +1,2 @@ +sender +receiver diff --git a/selfdrive/debug/internal/messaging/boost/Makefile b/selfdrive/debug/internal/messaging/boost/Makefile new file mode 100644 index 0000000000..2713d0a5a3 --- /dev/null +++ b/selfdrive/debug/internal/messaging/boost/Makefile @@ -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 diff --git a/selfdrive/debug/internal/messaging/boost/receiver.cc b/selfdrive/debug/internal/messaging/boost/receiver.cc new file mode 100644 index 0000000000..417e8c89b5 --- /dev/null +++ b/selfdrive/debug/internal/messaging/boost/receiver.cc @@ -0,0 +1,54 @@ +#include +#include +#include +#include + +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; +} diff --git a/selfdrive/debug/internal/messaging/boost/sender.cc b/selfdrive/debug/internal/messaging/boost/sender.cc new file mode 100644 index 0000000000..d2e34b06a7 --- /dev/null +++ b/selfdrive/debug/internal/messaging/boost/sender.cc @@ -0,0 +1,62 @@ +#include +#include +#include +#include +#include +#include + +#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(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; +} diff --git a/selfdrive/debug/internal/messaging/nanomsg/.gitignore b/selfdrive/debug/internal/messaging/nanomsg/.gitignore new file mode 100644 index 0000000000..b18ec44691 --- /dev/null +++ b/selfdrive/debug/internal/messaging/nanomsg/.gitignore @@ -0,0 +1,2 @@ +receiver +sender diff --git a/selfdrive/debug/internal/messaging/nanomsg/Makefile b/selfdrive/debug/internal/messaging/nanomsg/Makefile new file mode 100644 index 0000000000..63416e40ed --- /dev/null +++ b/selfdrive/debug/internal/messaging/nanomsg/Makefile @@ -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 diff --git a/selfdrive/debug/internal/messaging/nanomsg/receiver.cc b/selfdrive/debug/internal/messaging/nanomsg/receiver.cc new file mode 100644 index 0000000000..56033f6ff6 --- /dev/null +++ b/selfdrive/debug/internal/messaging/nanomsg/receiver.cc @@ -0,0 +1,48 @@ +#include +#include +#include +#include +#include + +#include +#include +#include + +#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; +} diff --git a/selfdrive/debug/internal/messaging/nanomsg/sender.cc b/selfdrive/debug/internal/messaging/nanomsg/sender.cc new file mode 100644 index 0000000000..a916690d48 --- /dev/null +++ b/selfdrive/debug/internal/messaging/nanomsg/sender.cc @@ -0,0 +1,73 @@ +#include +#include +#include +#include + +#include +#include +#include + + +#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(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; +} diff --git a/selfdrive/debug/internal/messaging/nng/.gitignore b/selfdrive/debug/internal/messaging/nng/.gitignore new file mode 100644 index 0000000000..b18ec44691 --- /dev/null +++ b/selfdrive/debug/internal/messaging/nng/.gitignore @@ -0,0 +1,2 @@ +receiver +sender diff --git a/selfdrive/debug/internal/messaging/nng/Makefile b/selfdrive/debug/internal/messaging/nng/Makefile new file mode 100644 index 0000000000..9188251041 --- /dev/null +++ b/selfdrive/debug/internal/messaging/nng/Makefile @@ -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 diff --git a/selfdrive/debug/internal/messaging/nng/receiver.cc b/selfdrive/debug/internal/messaging/nng/receiver.cc new file mode 100644 index 0000000000..06b8d63d6a --- /dev/null +++ b/selfdrive/debug/internal/messaging/nng/receiver.cc @@ -0,0 +1,56 @@ +#include +#include +#include +#include +#include + +#include +#include +#include + +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; +} diff --git a/selfdrive/debug/internal/messaging/nng/sender.cc b/selfdrive/debug/internal/messaging/nng/sender.cc new file mode 100644 index 0000000000..6448ceb681 --- /dev/null +++ b/selfdrive/debug/internal/messaging/nng/sender.cc @@ -0,0 +1,78 @@ +#include +#include +#include +#include + +#include +#include +#include + + +#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(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; +} diff --git a/selfdrive/debug/internal/messaging/zmq/.gitignore b/selfdrive/debug/internal/messaging/zmq/.gitignore new file mode 100644 index 0000000000..b18ec44691 --- /dev/null +++ b/selfdrive/debug/internal/messaging/zmq/.gitignore @@ -0,0 +1,2 @@ +receiver +sender diff --git a/selfdrive/debug/internal/messaging/zmq/Makefile b/selfdrive/debug/internal/messaging/zmq/Makefile new file mode 100644 index 0000000000..8a83c2567e --- /dev/null +++ b/selfdrive/debug/internal/messaging/zmq/Makefile @@ -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 diff --git a/selfdrive/debug/internal/messaging/zmq/receiver.cc b/selfdrive/debug/internal/messaging/zmq/receiver.cc new file mode 100644 index 0000000000..b1b2dcffc1 --- /dev/null +++ b/selfdrive/debug/internal/messaging/zmq/receiver.cc @@ -0,0 +1,49 @@ +#include +#include +#include + +#include + +// #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; +} diff --git a/selfdrive/debug/internal/messaging/zmq/sender.cc b/selfdrive/debug/internal/messaging/zmq/sender.cc new file mode 100644 index 0000000000..1052543e76 --- /dev/null +++ b/selfdrive/debug/internal/messaging/zmq/sender.cc @@ -0,0 +1,65 @@ +#include +#include +#include + +#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(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; +} diff --git a/selfdrive/debug/internal/messaging_benchmark.py b/selfdrive/debug/internal/messaging_benchmark.py new file mode 100755 index 0000000000..d7acdd5c04 --- /dev/null +++ b/selfdrive/debug/internal/messaging_benchmark.py @@ -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() diff --git a/selfdrive/debug/internal/mock_process/fake_UiNavigationEvent.py b/selfdrive/debug/internal/mock_process/fake_UiNavigationEvent.py new file mode 100755 index 0000000000..50eb31bb65 --- /dev/null +++ b/selfdrive/debug/internal/mock_process/fake_UiNavigationEvent.py @@ -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() diff --git a/selfdrive/debug/internal/mock_process/fake_controls_state.py b/selfdrive/debug/internal/mock_process/fake_controls_state.py new file mode 100755 index 0000000000..143ec23212 --- /dev/null +++ b/selfdrive/debug/internal/mock_process/fake_controls_state.py @@ -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() diff --git a/selfdrive/debug/internal/mock_process/fake_fusion_state.py b/selfdrive/debug/internal/mock_process/fake_fusion_state.py new file mode 100755 index 0000000000..be08e1c4da --- /dev/null +++ b/selfdrive/debug/internal/mock_process/fake_fusion_state.py @@ -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() diff --git a/selfdrive/debug/internal/mock_process/fake_gps.py b/selfdrive/debug/internal/mock_process/fake_gps.py new file mode 100755 index 0000000000..e540ef52b4 --- /dev/null +++ b/selfdrive/debug/internal/mock_process/fake_gps.py @@ -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) + diff --git a/selfdrive/debug/internal/mock_process/fake_gps_external.py b/selfdrive/debug/internal/mock_process/fake_gps_external.py new file mode 100755 index 0000000000..34a91530d7 --- /dev/null +++ b/selfdrive/debug/internal/mock_process/fake_gps_external.py @@ -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) diff --git a/selfdrive/debug/internal/mock_process/fake_liveMpc.py b/selfdrive/debug/internal/mock_process/fake_liveMpc.py new file mode 100755 index 0000000000..cf4ecbf8e1 --- /dev/null +++ b/selfdrive/debug/internal/mock_process/fake_liveMpc.py @@ -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() diff --git a/selfdrive/debug/internal/mock_process/fake_trafficsignd.py b/selfdrive/debug/internal/mock_process/fake_trafficsignd.py new file mode 100755 index 0000000000..7d5a06046c --- /dev/null +++ b/selfdrive/debug/internal/mock_process/fake_trafficsignd.py @@ -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() diff --git a/selfdrive/debug/internal/model_timing_checker.py b/selfdrive/debug/internal/model_timing_checker.py new file mode 100755 index 0000000000..53218ad10e --- /dev/null +++ b/selfdrive/debug/internal/model_timing_checker.py @@ -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 + diff --git a/selfdrive/debug/internal/polyfit_bench.py b/selfdrive/debug/internal/polyfit_bench.py new file mode 100644 index 0000000000..2909a45b4f --- /dev/null +++ b/selfdrive/debug/internal/polyfit_bench.py @@ -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)) diff --git a/selfdrive/debug/internal/power_monitor.py b/selfdrive/debug/internal/power_monitor.py new file mode 100755 index 0000000000..74a7e2ba0d --- /dev/null +++ b/selfdrive/debug/internal/power_monitor.py @@ -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") diff --git a/selfdrive/debug/internal/realtime_benchmark.py b/selfdrive/debug/internal/realtime_benchmark.py new file mode 100755 index 0000000000..0e48c9a92c --- /dev/null +++ b/selfdrive/debug/internal/realtime_benchmark.py @@ -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) diff --git a/selfdrive/debug/internal/run_segment_through_visiond.py b/selfdrive/debug/internal/run_segment_through_visiond.py new file mode 100755 index 0000000000..c2470ec117 --- /dev/null +++ b/selfdrive/debug/internal/run_segment_through_visiond.py @@ -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)) diff --git a/selfdrive/debug/internal/send_alert.py b/selfdrive/debug/internal/send_alert.py new file mode 100644 index 0000000000..699d51e8e1 --- /dev/null +++ b/selfdrive/debug/internal/send_alert.py @@ -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) diff --git a/selfdrive/debug/internal/status.py b/selfdrive/debug/internal/status.py new file mode 100644 index 0000000000..8c38665a89 --- /dev/null +++ b/selfdrive/debug/internal/status.py @@ -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() diff --git a/selfdrive/debug/internal/time_to_collision.py b/selfdrive/debug/internal/time_to_collision.py new file mode 100644 index 0000000000..76c00e3ac1 --- /dev/null +++ b/selfdrive/debug/internal/time_to_collision.py @@ -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() diff --git a/selfdrive/debug/internal/topdown.py b/selfdrive/debug/internal/topdown.py new file mode 100755 index 0000000000..e94b362828 --- /dev/null +++ b/selfdrive/debug/internal/topdown.py @@ -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() + diff --git a/selfdrive/debug/internal/toyota/blindspot.py b/selfdrive/debug/internal/toyota/blindspot.py new file mode 100755 index 0000000000..32d9b38194 --- /dev/null +++ b/selfdrive/debug/internal/toyota/blindspot.py @@ -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) diff --git a/selfdrive/debug/internal/toyota/blindspot2.py b/selfdrive/debug/internal/toyota/blindspot2.py new file mode 100755 index 0000000000..864849dff2 --- /dev/null +++ b/selfdrive/debug/internal/toyota/blindspot2.py @@ -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) diff --git a/selfdrive/debug/internal/toyota/blinker_toyota.py b/selfdrive/debug/internal/toyota/blinker_toyota.py new file mode 100755 index 0000000000..d1faf81505 --- /dev/null +++ b/selfdrive/debug/internal/toyota/blinker_toyota.py @@ -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) diff --git a/selfdrive/debug/internal/toyota/continental_radar.py b/selfdrive/debug/internal/toyota/continental_radar.py new file mode 100755 index 0000000000..74219a74f0 --- /dev/null +++ b/selfdrive/debug/internal/toyota/continental_radar.py @@ -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')) diff --git a/selfdrive/debug/internal/toyota/simple_can_printer.py b/selfdrive/debug/internal/toyota/simple_can_printer.py new file mode 100755 index 0000000000..77d6e2900d --- /dev/null +++ b/selfdrive/debug/internal/toyota/simple_can_printer.py @@ -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) diff --git a/selfdrive/debug/internal/toyota/sonar.py b/selfdrive/debug/internal/toyota/sonar.py new file mode 100755 index 0000000000..ccc64280b8 --- /dev/null +++ b/selfdrive/debug/internal/toyota/sonar.py @@ -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) diff --git a/selfdrive/debug/internal/vehicle_model_sim.py b/selfdrive/debug/internal/vehicle_model_sim.py new file mode 100644 index 0000000000..755b9b023b --- /dev/null +++ b/selfdrive/debug/internal/vehicle_model_sim.py @@ -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) diff --git a/selfdrive/debug/live_cpu_and_temp.py b/selfdrive/debug/live_cpu_and_temp.py new file mode 100755 index 0000000000..7286841a84 --- /dev/null +++ b/selfdrive/debug/live_cpu_and_temp.py @@ -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 )) diff --git a/selfdrive/debug/mpc/live_lateral_mpc.py b/selfdrive/debug/mpc/live_lateral_mpc.py new file mode 100755 index 0000000000..48be5be9c6 --- /dev/null +++ b/selfdrive/debug/mpc/live_lateral_mpc.py @@ -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() diff --git a/selfdrive/debug/mpc/live_longitudinal_mpc.py b/selfdrive/debug/mpc/live_longitudinal_mpc.py new file mode 100755 index 0000000000..b5b4361e13 --- /dev/null +++ b/selfdrive/debug/mpc/live_longitudinal_mpc.py @@ -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() diff --git a/selfdrive/debug/mpc/test_mpc_wobble.py b/selfdrive/debug/mpc/test_mpc_wobble.py new file mode 100755 index 0000000000..5f9f1b3355 --- /dev/null +++ b/selfdrive/debug/mpc/test_mpc_wobble.py @@ -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() diff --git a/selfdrive/debug/mpc/tune_lateral.py b/selfdrive/debug/mpc/tune_lateral.py new file mode 100755 index 0000000000..4a8ec2ce0b --- /dev/null +++ b/selfdrive/debug/mpc/tune_lateral.py @@ -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() diff --git a/selfdrive/debug/mpc/tune_longitudinal.py b/selfdrive/debug/mpc/tune_longitudinal.py new file mode 100755 index 0000000000..0af444631d --- /dev/null +++ b/selfdrive/debug/mpc/tune_longitudinal.py @@ -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() diff --git a/selfdrive/debug/show_matching_cars.py b/selfdrive/debug/show_matching_cars.py new file mode 100755 index 0000000000..df2b703635 --- /dev/null +++ b/selfdrive/debug/show_matching_cars.py @@ -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) diff --git a/selfdrive/debug/tuner.py b/selfdrive/debug/tuner.py new file mode 100755 index 0000000000..b8a023ba29 --- /dev/null +++ b/selfdrive/debug/tuner.py @@ -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