# 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_hash": "c082c7c3365829b9df9cbbc5b20a9aed3cc5c98ebb17351e7e00e6285072c403", |
||||
"recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-a7c7d5d54b9f3afa6ae3d22ceab44f018b819c02443e4a09578089fbdb2ec4ee.img", |
||||
"ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-969e22c42e5c6314e54bc3ccaa5c6a684f3130a53a7a70e0cea9f1453ceb0b06.zip", |
||||
"ota_hash": "969e22c42e5c6314e54bc3ccaa5c6a684f3130a53a7a70e0cea9f1453ceb0b06", |
||||
"recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-9c784a24826c25df315d0ace864224478e9c0e86b904f5d1f8e18ea1037e842b.img", |
||||
"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