diff --git a/.gitignore b/.gitignore index 35691c34b6..f4932ed4f0 100644 --- a/.gitignore +++ b/.gitignore @@ -10,6 +10,7 @@ venv/ .vscode* model2.png a.out +.hypothesis *.dylib *.DSYM diff --git a/Pipfile b/Pipfile index beb4ef227f..dd38abc51e 100644 --- a/Pipfile +++ b/Pipfile @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:690153f35d93395bae301cfcd607d141203117839855754381973636a0b8a846 -size 1887 +oid sha256:803c2edc32d1e8fbfb92207f5d8f073d3c89da025e3658e6c2f1c9ec649f5e0d +size 1904 diff --git a/Pipfile.lock b/Pipfile.lock index 6accb633cf..f099acdb39 100644 --- a/Pipfile.lock +++ b/Pipfile.lock @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:2965278134cfd52e027ef646eaf43b92e856fa895cfa67c81fac2ffe7887d32f -size 202729 +oid sha256:050349245ca49edc60182ef21c32281623b5a5bd7c16ab1b1a8e48aecd99cc08 +size 203871 diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 183b997616..1873340495 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -14,7 +14,7 @@ from cereal import car, log from cereal.services import service_list from common.params import Params from selfdrive.car.fingerprints import FW_VERSIONS -from selfdrive.car.car_helpers import get_car +from selfdrive.car.car_helpers import get_car, interfaces from selfdrive.manager.process import PythonProcess from selfdrive.manager.process_config import managed_processes @@ -148,7 +148,7 @@ class FakePubMaster(messaging.PubMaster): return dat -def fingerprint(msgs, fsm, can_sock): +def fingerprint(msgs, fsm, can_sock, fingerprint): print("start fingerprinting") fsm.wait_on_getitem = True @@ -172,14 +172,18 @@ def fingerprint(msgs, fsm, can_sock): print("finished fingerprinting") -def get_car_params(msgs, fsm, can_sock): - can = FakeSocket(wait=False) - sendcan = FakeSocket(wait=False) +def get_car_params(msgs, fsm, can_sock, fingerprint): + if fingerprint: + CarInterface, _, _ = interfaces[fingerprint] + CP = CarInterface.get_params(fingerprint) + else: + can = FakeSocket(wait=False) + sendcan = FakeSocket(wait=False) - canmsgs = [msg for msg in msgs if msg.which() == 'can'] - for m in canmsgs[:300]: - can.send(m.as_builder().to_bytes()) - _, CP = get_car(can, sendcan) + canmsgs = [msg for msg in msgs if msg.which() == 'can'] + for m in canmsgs[:300]: + can.send(m.as_builder().to_bytes()) + _, CP = get_car(can, sendcan) Params().put("CarParams", CP.to_bytes()) def controlsd_rcv_callback(msg, CP, cfg, fsm): @@ -328,14 +332,14 @@ CONFIGS = [ ] -def replay_process(cfg, lr): +def replay_process(cfg, lr, fingerprint=None): if cfg.fake_pubsubmaster: - return python_replay_process(cfg, lr) + return python_replay_process(cfg, lr, fingerprint) else: - return cpp_replay_process(cfg, lr) + return cpp_replay_process(cfg, lr, fingerprint) -def python_replay_process(cfg, lr): +def python_replay_process(cfg, lr, fingerprint=None): sub_sockets = [s for _, sub in cfg.pub_sub.items() for s in sub] pub_sockets = [s for s in cfg.pub_sub.keys() if s != 'can'] @@ -356,8 +360,6 @@ def python_replay_process(cfg, lr): params.put_bool("CommunityFeaturesToggle", True) os.environ['NO_RADAR_SLEEP'] = "1" - os.environ['SKIP_FW_QUERY'] = "" - os.environ['FINGERPRINT'] = "" # TODO: remove after getting new route for civic & accord migration = { @@ -365,14 +367,20 @@ def python_replay_process(cfg, lr): "HONDA ACCORD 2018 SPORT 2T": "HONDA ACCORD 2018 2T", } - for msg in lr: - if msg.which() == 'carParams': - car_fingerprint = migration.get(msg.carParams.carFingerprint, msg.carParams.carFingerprint) - if len(msg.carParams.carFw) and (car_fingerprint in FW_VERSIONS): - params.put("CarParamsCache", msg.carParams.as_builder().to_bytes()) - else: - os.environ['SKIP_FW_QUERY'] = "1" - os.environ['FINGERPRINT'] = car_fingerprint + if fingerprint is not None: + os.environ['SKIP_FW_QUERY'] = "1" + os.environ['FINGERPRINT'] = fingerprint + else: + os.environ['SKIP_FW_QUERY'] = "" + os.environ['FINGERPRINT'] = "" + for msg in lr: + if msg.which() == 'carParams': + car_fingerprint = migration.get(msg.carParams.carFingerprint, msg.carParams.carFingerprint) + if len(msg.carParams.carFw) and (car_fingerprint in FW_VERSIONS): + params.put("CarParamsCache", msg.carParams.as_builder().to_bytes()) + else: + os.environ['SKIP_FW_QUERY'] = "1" + os.environ['FINGERPRINT'] = car_fingerprint assert(type(managed_processes[cfg.proc_name]) is PythonProcess) managed_processes[cfg.proc_name].prepare() @@ -385,7 +393,7 @@ def python_replay_process(cfg, lr): if cfg.init_callback is not None: if 'can' not in list(cfg.pub_sub.keys()): can_sock = None - cfg.init_callback(all_msgs, fsm, can_sock) + cfg.init_callback(all_msgs, fsm, can_sock, fingerprint) CP = car.CarParams.from_bytes(params.get("CarParams", block=True)) @@ -421,7 +429,7 @@ def python_replay_process(cfg, lr): return log_msgs -def cpp_replay_process(cfg, lr): +def cpp_replay_process(cfg, lr, fingerprint=None): sub_sockets = [s for _, sub in cfg.pub_sub.items() for s in sub] # We get responses here pm = messaging.PubMaster(cfg.pub_sub.keys()) sockets = {s: messaging.sub_sock(s, timeout=1000) for s in sub_sockets} diff --git a/selfdrive/test/process_replay/test_fuzzy.py b/selfdrive/test/process_replay/test_fuzzy.py new file mode 100755 index 0000000000..3f9ac2839b --- /dev/null +++ b/selfdrive/test/process_replay/test_fuzzy.py @@ -0,0 +1,180 @@ +#!/usr/bin/env python3 +import sys +import unittest +from collections import Counter + +import hypothesis.strategies as st +import numpy as np +from hypothesis import assume, given, settings + +from cereal import log +from selfdrive.car.toyota.values import CAR as TOYOTA +from selfdrive.test.process_replay.process_replay import (CONFIGS, + replay_process) + + +def get_process_config(process): + return [cfg for cfg in CONFIGS if cfg.proc_name == process][0] + + +def get_event_union_strategy(r, name): + return st.fixed_dictionaries({ + 'valid': st.booleans(), + 'logMonoTime': st.integers(min_value=0, max_value=2**64-1), + name: r[name[0].upper() + name[1:]], + }) + + +def get_strategy_for_events(event_types, finite=False): + # TODO: generate automatically based on capnp definitions + def floats(**kwargs): + allow_nan = False if finite else None + allow_infinity = False if finite else None + return st.floats(**kwargs, allow_nan=allow_nan, allow_infinity=allow_infinity) + + r = {} + r['liveLocationKalman.Measurement'] = st.fixed_dictionaries({ + 'value': st.lists(floats(), min_size=3, max_size=3), + 'std': st.lists(floats(), min_size=3, max_size=3), + 'valid': st.booleans(), + }) + r['LiveLocationKalman'] = st.fixed_dictionaries({ + 'angularVelocityCalibrated': r['liveLocationKalman.Measurement'], + 'inputsOK': st.booleans(), + 'posenetOK': st.booleans(), + }) + r['CarState'] = st.fixed_dictionaries({ + 'vEgo': floats(width=32), + 'vEgoRaw': floats(width=32), + 'steeringPressed': st.booleans(), + 'steeringAngleDeg': floats(width=32), + }) + r['CameraOdometry'] = st.fixed_dictionaries({ + 'frameId': st.integers(min_value=0, max_value=2**32-1), + 'timestampEof': st.integers(min_value=0, max_value=2**64-1), + 'trans': st.lists(floats(width=32), min_size=3, max_size=3), + 'rot': st.lists(floats(width=32), min_size=3, max_size=3), + 'transStd': st.lists(floats(width=32), min_size=3, max_size=3), + 'rotStd': st.lists(floats(width=32), min_size=3, max_size=3), + }) + r['SensorEventData.SensorVec'] = st.fixed_dictionaries({ + 'v': st.lists(floats(width=32), min_size=3, max_size=3), + 'status': st.integers(min_value=0, max_value=1), + }) + r['SensorEventData_gyro'] = st.fixed_dictionaries({ + 'version': st.just(1), + 'sensor': st.just(5), + 'type': st.just(16), # BMX055 + 'timestamp': st.integers(min_value=0, max_value=2**63-1), + 'source': st.just(8), + 'gyroUncalibrated': r['SensorEventData.SensorVec'], + }) + r['SensorEventData_accel'] = st.fixed_dictionaries({ + 'version': st.just(1), + 'sensor': st.just(1), + 'type': st.just(1), # BMX055 + 'timestamp': st.integers(min_value=0, max_value=2**63-1), + 'source': st.just(8), + 'acceleration': r['SensorEventData.SensorVec'], + }) + r['SensorEvents'] = st.lists(st.one_of(r['SensorEventData_gyro'], r['SensorEventData_accel']), min_size=1) + r['GpsLocationExternal'] = st.fixed_dictionaries({ + 'flags': st.integers(min_value=0, max_value=1), + 'latitude': floats(), + 'longitude': floats(), + 'altitude': floats(), + 'speed': floats(width=32), + 'bearingDeg': floats(width=32), + 'accuracy': floats(width=32), + 'timestamp': st.integers(min_value=0, max_value=2**63-1), + 'source': st.just(6), # Ublox + 'vNED': st.lists(floats(width=32), min_size=3, max_size=3), + 'verticalAccuracy': floats(width=32), + 'bearingAccuracyDeg': floats(width=32), + 'speedAccuracy': floats(width=32), + }) + r['LiveCalibration'] = st.fixed_dictionaries({ + 'calStatus': st.integers(min_value=0, max_value=1), + 'rpyCalib': st.lists(floats(width=32), min_size=3, max_size=3), + }) + + return st.lists(st.one_of(*[get_event_union_strategy(r, n) for n in event_types])) + + +def get_strategy_for_process(process, finite=False): + return get_strategy_for_events(get_process_config(process).pub_sub.keys(), finite) + + +def convert_to_lr(msgs): + return [log.Event.new_message(**m).as_reader() for m in msgs] + + +def assume_all_services_present(cfg, lr): + tps = Counter([m.which() for m in lr]) + for p in cfg.pub_sub: + assume(tps[p] > 0) + + +def is_finite(d, exclude=[], prefix=""): # pylint: disable=dangerous-default-value + ret = True + for k, v in d.items(): + name = prefix + f"{k}" + if name in exclude: + continue + + if isinstance(v, dict): + ret = ret and is_finite(v, exclude, name + ".") + else: + try: + ret = ret and np.isfinite(v).all() + except TypeError: + pass + + return ret + + +def test_process(dat, name): + cfg = get_process_config(name) + lr = convert_to_lr(dat) + assume_all_services_present(cfg, lr) + return replay_process(cfg, lr, TOYOTA.COROLLA_TSS2) + + +class TestFuzzy(unittest.TestCase): + @given(get_strategy_for_process('paramsd')) + @settings(deadline=1000) + def test_paramsd(self, dat): + for r in test_process(dat, 'paramsd'): + lp = r.liveParameters.to_dict() + assert is_finite(lp) + + @given(get_strategy_for_process('locationd', finite=True)) + @settings(deadline=10000) + def test_locationd(self, dat): + exclude = [ + 'positionGeodetic.std', + 'velocityNED.std', + 'orientationNED.std', + 'calibratedOrientationECEF.std', + ] + for r in test_process(dat, 'locationd'): + lp = r.liveLocationKalman.to_dict() + assert is_finite(lp, exclude) + + +if __name__ == "__main__": + procs = { + 'locationd': TestFuzzy().test_locationd, + 'paramsd': TestFuzzy().test_paramsd, + } + + if len(sys.argv) != 2: + print("Usage: ./test_fuzzy.py ") + sys.exit(0) + + proc = sys.argv[1] + if proc not in procs: + print(f"{proc} not available") + sys.exit(0) + else: + procs[proc]()