# Conflicts: # selfdrive/car/volkswagen/interface.py # selfdrive/car/volkswagen/values.pypull/20489/head
Jason Young
4 years ago
commit
a3ddd1fcb6
23 changed files with 435 additions and 171 deletions
@ -1,7 +1,7 @@ |
|||||||
{ |
{ |
||||||
"ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-c082c7c3365829b9df9cbbc5b20a9aed3cc5c98ebb17351e7e00e6285072c403.zip", |
"ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-969e22c42e5c6314e54bc3ccaa5c6a684f3130a53a7a70e0cea9f1453ceb0b06.zip", |
||||||
"ota_hash": "c082c7c3365829b9df9cbbc5b20a9aed3cc5c98ebb17351e7e00e6285072c403", |
"ota_hash": "969e22c42e5c6314e54bc3ccaa5c6a684f3130a53a7a70e0cea9f1453ceb0b06", |
||||||
"recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-a7c7d5d54b9f3afa6ae3d22ceab44f018b819c02443e4a09578089fbdb2ec4ee.img", |
"recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-9c784a24826c25df315d0ace864224478e9c0e86b904f5d1f8e18ea1037e842b.img", |
||||||
"recovery_len": 15209772, |
"recovery_len": 15209772, |
||||||
"recovery_hash": "a7c7d5d54b9f3afa6ae3d22ceab44f018b819c02443e4a09578089fbdb2ec4ee" |
"recovery_hash": "9c784a24826c25df315d0ace864224478e9c0e86b904f5d1f8e18ea1037e842b" |
||||||
} |
} |
||||||
|
|||||||
@ -1 +1 @@ |
|||||||
Subproject commit ad9ecefe65ce4ea7bcaf3bfe692ee2a3fc6b0b63 |
Subproject commit f146aa367b82f041b4eadcaab0bb2fc79e388b94 |
||||||
@ -0,0 +1,68 @@ |
|||||||
|
#!/usr/bin/env python3 |
||||||
|
|
||||||
|
import time |
||||||
|
import unittest |
||||||
|
import os |
||||||
|
import numpy as np |
||||||
|
|
||||||
|
from selfdrive.test.helpers import with_processes |
||||||
|
from selfdrive.camerad.snapshot.snapshot import get_snapshots |
||||||
|
|
||||||
|
from selfdrive.hardware import EON, TICI |
||||||
|
|
||||||
|
TEST_TIME = 45 |
||||||
|
REPEAT = 5 |
||||||
|
|
||||||
|
os.environ["SEND_ROAD"] = "1" |
||||||
|
os.environ["SEND_DRIVER"] = "1" |
||||||
|
if TICI: |
||||||
|
os.environ["SEND_WIDE_ROAD"] = "1" |
||||||
|
|
||||||
|
class TestCamerad(unittest.TestCase): |
||||||
|
@classmethod |
||||||
|
def setUpClass(cls): |
||||||
|
if not (EON or TICI): |
||||||
|
raise unittest.SkipTest |
||||||
|
|
||||||
|
def _numpy_rgb2gray(self, im): |
||||||
|
ret = np.clip(im[:,:,2] * 0.114 + im[:,:,1] * 0.587 + im[:,:,0] * 0.299, 0, 255).astype(np.uint8) |
||||||
|
return ret |
||||||
|
|
||||||
|
def _is_exposure_okay(self, i, med_mean=np.array([[0.2,0.4],[0.2,0.6]])): |
||||||
|
h, w = i.shape[:2] |
||||||
|
i = i[h//10:9*h//10,w//10:9*w//10] |
||||||
|
med_ex, mean_ex = med_mean |
||||||
|
i = self._numpy_rgb2gray(i) |
||||||
|
i_median = np.median(i) / 255. |
||||||
|
i_mean = np.mean(i) / 255. |
||||||
|
print([i_median, i_mean]) |
||||||
|
return med_ex[0] < i_median < med_ex[1] and mean_ex[0] < i_mean < mean_ex[1] |
||||||
|
|
||||||
|
|
||||||
|
@with_processes(['camerad']) |
||||||
|
def test_camera_operation(self): |
||||||
|
print("checking image outputs") |
||||||
|
|
||||||
|
start = time.time() |
||||||
|
passed = 0 |
||||||
|
while(time.time() - start < TEST_TIME and passed < REPEAT): |
||||||
|
rpic, dpic = get_snapshots(frame="roadCameraState", front_frame="driverCameraState") |
||||||
|
|
||||||
|
res = self._is_exposure_okay(rpic) |
||||||
|
res = res and self._is_exposure_okay(dpic) |
||||||
|
|
||||||
|
if TICI: |
||||||
|
wpic, _ = get_snapshots(frame="wideRoadCameraState") |
||||||
|
res = res and self._is_exposure_okay(wpic) |
||||||
|
|
||||||
|
if passed > 0 and not res: |
||||||
|
passed = -passed # fails test if any failure after first sus |
||||||
|
break |
||||||
|
|
||||||
|
passed += int(res) |
||||||
|
time.sleep(2) |
||||||
|
print(passed) |
||||||
|
self.assertTrue(passed >= REPEAT) |
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
unittest.main() |
||||||
@ -0,0 +1,150 @@ |
|||||||
|
#!/usr/bin/env python3 |
||||||
|
# pylint: skip-file |
||||||
|
# flake8: noqa |
||||||
|
# type: ignore |
||||||
|
|
||||||
|
import math |
||||||
|
import multiprocessing |
||||||
|
|
||||||
|
import numpy as np |
||||||
|
from tqdm import tqdm |
||||||
|
|
||||||
|
from selfdrive.locationd.paramsd import ParamsLearner, States |
||||||
|
from tools.lib.logreader import LogReader |
||||||
|
from tools.lib.route import Route |
||||||
|
|
||||||
|
ROUTE = "b2f1615665781088|2021-03-14--17-27-47" |
||||||
|
PLOT = True |
||||||
|
|
||||||
|
|
||||||
|
def load_segment(segment_name): |
||||||
|
print(f"Loading {segment_name}") |
||||||
|
if segment_name is None: |
||||||
|
return [] |
||||||
|
|
||||||
|
try: |
||||||
|
return list(LogReader(segment_name)) |
||||||
|
except ValueError as e: |
||||||
|
print(f"Error parsing {segment_name}: {e}") |
||||||
|
return [] |
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
route = Route(ROUTE) |
||||||
|
|
||||||
|
msgs = [] |
||||||
|
with multiprocessing.Pool(24) as pool: |
||||||
|
for d in pool.map(load_segment, route.log_paths()): |
||||||
|
msgs += d |
||||||
|
|
||||||
|
for m in msgs: |
||||||
|
if m.which() == 'carParams': |
||||||
|
CP = m.carParams |
||||||
|
break |
||||||
|
|
||||||
|
params = { |
||||||
|
'carFingerprint': CP.carFingerprint, |
||||||
|
'steerRatio': CP.steerRatio, |
||||||
|
'stiffnessFactor': 1.0, |
||||||
|
'angleOffsetAverageDeg': 0.0, |
||||||
|
} |
||||||
|
|
||||||
|
for m in msgs: |
||||||
|
if m.which() == 'liveParameters': |
||||||
|
params['steerRatio'] = m.liveParameters.steerRatio |
||||||
|
params['angleOffsetAverageDeg'] = m.liveParameters.angleOffsetAverageDeg |
||||||
|
break |
||||||
|
|
||||||
|
for m in msgs: |
||||||
|
if m.which() == 'carState': |
||||||
|
last_carstate = m |
||||||
|
break |
||||||
|
|
||||||
|
print(params) |
||||||
|
learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverageDeg'])) |
||||||
|
msgs = [m for m in tqdm(msgs) if m.which() in ('liveLocationKalman', 'carState', 'liveParameters')] |
||||||
|
msgs = sorted(msgs, key=lambda m: m.logMonoTime) |
||||||
|
|
||||||
|
ts = [] |
||||||
|
ts_log = [] |
||||||
|
results = [] |
||||||
|
results_log = [] |
||||||
|
for m in tqdm(msgs): |
||||||
|
if m.which() == 'carState': |
||||||
|
last_carstate = m |
||||||
|
|
||||||
|
elif m.which() == 'liveLocationKalman': |
||||||
|
t = last_carstate.logMonoTime / 1e9 |
||||||
|
learner.handle_log(t, 'carState', last_carstate.carState) |
||||||
|
|
||||||
|
t = m.logMonoTime / 1e9 |
||||||
|
learner.handle_log(t, 'liveLocationKalman', m.liveLocationKalman) |
||||||
|
|
||||||
|
x = learner.kf.x |
||||||
|
sr = float(x[States.STEER_RATIO]) |
||||||
|
st = float(x[States.STIFFNESS]) |
||||||
|
ao_avg = math.degrees(x[States.ANGLE_OFFSET]) |
||||||
|
ao = ao_avg + math.degrees(x[States.ANGLE_OFFSET_FAST]) |
||||||
|
r = [sr, st, ao_avg, ao] |
||||||
|
if any(math.isnan(v) for v in r): |
||||||
|
print("NaN", t) |
||||||
|
|
||||||
|
ts.append(t) |
||||||
|
results.append(r) |
||||||
|
|
||||||
|
elif m.which() == 'liveParameters': |
||||||
|
t = m.logMonoTime / 1e9 |
||||||
|
mm = m.liveParameters |
||||||
|
|
||||||
|
r = [mm.steerRatio, mm.stiffnessFactor, mm.angleOffsetAverageDeg, mm.angleOffsetDeg] |
||||||
|
if any(math.isnan(v) for v in r): |
||||||
|
print("NaN in log", t) |
||||||
|
ts_log.append(t) |
||||||
|
results_log.append(r) |
||||||
|
|
||||||
|
results = np.asarray(results) |
||||||
|
results_log = np.asarray(results_log) |
||||||
|
|
||||||
|
if PLOT: |
||||||
|
import matplotlib.pyplot as plt |
||||||
|
plt.figure() |
||||||
|
|
||||||
|
plt.subplot(3, 2, 1) |
||||||
|
plt.plot(ts, results[:, 0], label='Steer Ratio') |
||||||
|
plt.grid() |
||||||
|
plt.ylim([0, 20]) |
||||||
|
plt.legend() |
||||||
|
|
||||||
|
plt.subplot(3, 2, 3) |
||||||
|
plt.plot(ts, results[:, 1], label='Stiffness') |
||||||
|
plt.ylim([0, 2]) |
||||||
|
plt.grid() |
||||||
|
plt.legend() |
||||||
|
|
||||||
|
plt.subplot(3, 2, 5) |
||||||
|
plt.plot(ts, results[:, 2], label='Angle offset (average)') |
||||||
|
plt.plot(ts, results[:, 3], label='Angle offset (instant)') |
||||||
|
plt.ylim([-5, 5]) |
||||||
|
plt.grid() |
||||||
|
plt.legend() |
||||||
|
|
||||||
|
plt.subplot(3, 2, 2) |
||||||
|
plt.plot(ts_log, results_log[:, 0], label='Steer Ratio') |
||||||
|
plt.grid() |
||||||
|
plt.ylim([0, 20]) |
||||||
|
plt.legend() |
||||||
|
|
||||||
|
plt.subplot(3, 2, 4) |
||||||
|
plt.plot(ts_log, results_log[:, 1], label='Stiffness') |
||||||
|
plt.ylim([0, 2]) |
||||||
|
plt.grid() |
||||||
|
plt.legend() |
||||||
|
|
||||||
|
plt.subplot(3, 2, 6) |
||||||
|
plt.plot(ts_log, results_log[:, 2], label='Angle offset (average)') |
||||||
|
plt.plot(ts_log, results_log[:, 3], label='Angle offset (instant)') |
||||||
|
plt.ylim([-5, 5]) |
||||||
|
plt.grid() |
||||||
|
plt.legend() |
||||||
|
plt.show() |
||||||
|
|
||||||
Loading…
Reference in new issue