From bc3b34069466022adcff6d28e467dc18fa8eeb6c Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Thu, 30 Jul 2020 15:33:22 -0700 Subject: [PATCH] Misc locationd improvements (#1714) * I like this more * rewind less * bump rednose * falling off windshield detectopr * adjust thresholds * this is a soft disable now * move that * process replay fixes * update refs Co-authored-by: Comma Device Co-authored-by: Adeeb Shihadeh old-commit-hash: 8e2d344135b9a1102503432d651e6bf599d9fa42 --- rednose_repo | 2 +- selfdrive/controls/lib/events.py | 14 ++--- selfdrive/locationd/locationd.py | 51 +++++++++++-------- selfdrive/locationd/models/live_kf.py | 2 +- selfdrive/test/process_replay/compare_logs.py | 14 ++--- selfdrive/test/process_replay/ref_commit | 2 +- .../test/process_replay/test_processes.py | 6 ++- 7 files changed, 49 insertions(+), 42 deletions(-) diff --git a/rednose_repo b/rednose_repo index 307c771fca..2e556b8219 160000 --- a/rednose_repo +++ b/rednose_repo @@ -1 +1 @@ -Subproject commit 307c771fcaa5e301ccb18d3ab9fdedddd675c569 +Subproject commit 2e556b8219185708ed974a4b6502796607d7ce0d diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index bb7ce16566..eb943c1acc 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -530,15 +530,6 @@ EVENTS = { duration_hud_alert=0.), }, - EventName.posenetInvalid: { - ET.WARNING: Alert( - "TAKE CONTROL", - "Vision Model Output Uncertain", - AlertStatus.userPrompt, AlertSize.mid, - Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning1, .4, 2., 3.), - ET.NO_ENTRY: NoEntryAlert("Vision Model Output Uncertain"), - }, - EventName.focusRecoverActive: { ET.WARNING: Alert( "TAKE CONTROL", @@ -659,6 +650,11 @@ EVENTS = { ET.NO_ENTRY : NoEntryAlert("Driving model lagging"), }, + EventName.posenetInvalid: { + ET.SOFT_DISABLE: SoftDisableAlert("Vision Model Output Uncertain"), + ET.NO_ENTRY: NoEntryAlert("Vision Model Output Uncertain"), + }, + EventName.lowMemory: { ET.SOFT_DISABLE: SoftDisableAlert("Low Memory: Reboot Your Device"), ET.PERMANENT: Alert( diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py index 3a5d515538..283d14732a 100755 --- a/selfdrive/locationd/locationd.py +++ b/selfdrive/locationd/locationd.py @@ -1,7 +1,6 @@ #!/usr/bin/env python3 import numpy as np import sympy as sp - import cereal.messaging as messaging import common.transformations.coordinates as coord from common.transformations.orientation import ecef_euler_from_ned, \ @@ -23,6 +22,7 @@ from rednose.helpers.sympy_helpers import euler_rotate VISION_DECIMATION = 2 SENSOR_DECIMATION = 10 +POSENET_STD_HIST = 40 def to_float(arr): @@ -52,7 +52,7 @@ class Localizer(): self.kf = LiveKalman(GENERATED_DIR) self.reset_kalman() - self.max_age = .2 # seconds + self.max_age = .1 # seconds self.disabled_logs = disabled_logs self.calib = np.zeros(3) self.device_from_calib = np.eye(3) @@ -63,6 +63,7 @@ class Localizer(): self.posenet_invalid_count = 0 self.posenet_speed = 0 self.car_speed = 0 + self.posenet_stds = 10*np.ones((POSENET_STD_HIST)) self.converter = coord.LocalCoord.from_ecef(self.kf.x[States.ECEF_POS]) @@ -113,8 +114,8 @@ class Localizer(): fix = messaging.log.LiveLocationKalman.new_message() fix.positionGeodetic.value = to_float(fix_pos_geo) - #fix.positionGeodetic.std = to_float(fix_pos_geo_std) - #fix.positionGeodetic.valid = True + fix.positionGeodetic.std = to_float(np.nan*np.zeros(3)) + fix.positionGeodetic.valid = True fix.positionECEF.value = to_float(fix_ecef) fix.positionECEF.std = to_float(fix_ecef_std) fix.positionECEF.valid = True @@ -122,8 +123,8 @@ class Localizer(): fix.velocityECEF.std = to_float(vel_ecef_std) fix.velocityECEF.valid = True fix.velocityNED.value = to_float(ned_vel) - #fix.velocityNED.std = to_float(ned_vel_std) - #fix.velocityNED.valid = True + fix.velocityNED.std = to_float(np.nan*np.zeros(3)) + fix.velocityNED.valid = True fix.velocityDevice.value = to_float(vel_device) fix.velocityDevice.std = to_float(vel_device_std) fix.velocityDevice.valid = True @@ -135,11 +136,11 @@ class Localizer(): fix.orientationECEF.std = to_float(orientation_ecef_std) fix.orientationECEF.valid = True fix.calibratedOrientationECEF.value = to_float(calibrated_orientation_ecef) - #fix.calibratedOrientationECEF.std = to_float(calibrated_orientation_ecef_std) - #fix.calibratedOrientationECEF.valid = True + fix.calibratedOrientationECEF.std = to_float(np.nan*np.zeros(3)) + fix.calibratedOrientationECEF.valid = True fix.orientationNED.value = to_float(orientation_ned) - #fix.orientationNED.std = to_float(orientation_ned_std) - #fix.orientationNED.valid = True + fix.orientationNED.std = to_float(np.nan*np.zeros(3)) + fix.orientationNED.valid = True fix.angularVelocityDevice.value = to_float(predicted_state[States.ANGULAR_VELOCITY]) fix.angularVelocityDevice.std = to_float(predicted_std[States.ANGULAR_VELOCITY_ERR]) fix.angularVelocityDevice.valid = True @@ -155,14 +156,23 @@ class Localizer(): fix.accelerationCalibrated.valid = True return fix - def liveLocationMsg(self, time): + def liveLocationMsg(self): fix = self.msg_from_state(self.converter, self.calib_from_device, self.H, self.kf.x, self.kf.P) - if abs(self.posenet_speed - self.car_speed) > max(0.4 * self.car_speed, 5.0): - self.posenet_invalid_count += 1 + #if abs(self.posenet_speed - self.car_speed) > max(0.4 * self.car_speed, 5.0): + # self.posenet_invalid_count += 1 + #else: + # self.posenet_invalid_count = 0 + #fix.posenetOK = self.posenet_invalid_count < 4 + + # experimentally found these values + old_mean, new_mean = np.mean(self.posenet_stds[:POSENET_STD_HIST//2]), np.mean(self.posenet_stds[POSENET_STD_HIST//2:]) + std_spike = new_mean/old_mean > 4 and new_mean > 5 + + if std_spike and self.car_speed > 5: + fix.posenetOK = False else: - self.posenet_invalid_count = 0 - fix.posenetOK = self.posenet_invalid_count < 4 + fix.posenetOK = True #fix.gpsWeek = self.time.week #fix.gpsTimeOfWeek = self.time.tow @@ -178,15 +188,10 @@ class Localizer(): def update_kalman(self, time, kind, meas, R=None): try: - self.kf.predict_and_observe(time, kind, meas, R=R) + self.kf.predict_and_observe(time, kind, meas, R) except KalmanError: cloudlog.error("Error in predict and observe, kalman reset") self.reset_kalman() - #idx = bisect_right([x[0] for x in self.observation_buffer], time) - #self.observation_buffer.insert(idx, (time, kind, meas)) - #while len(self.observation_buffer) > 0 and self.observation_buffer[-1][0] - self.observation_buffer[0][0] > self.max_age: - # else: - # self.observation_buffer.pop(0) def handle_gps(self, current_time, log): # ignore the message if the fix is invalid @@ -244,6 +249,8 @@ class Localizer(): trans_device = self.device_from_calib.dot(log.trans) trans_device_std = self.device_from_calib.dot(log.transStd) self.posenet_speed = np.linalg.norm(trans_device) + self.posenet_stds[:-1] = self.posenet_stds[1:] + self.posenet_stds[-1] = trans_device_std[0] self.update_kalman(current_time, ObservationKind.CAMERA_ODO_TRANSLATION, np.concatenate([trans_device, 10*trans_device_std])) @@ -322,7 +329,7 @@ def locationd_thread(sm, pm, disabled_logs=None): msg = messaging.new_message('liveLocationKalman') msg.logMonoTime = t - msg.liveLocationKalman = localizer.liveLocationMsg(t * 1e-9) + msg.liveLocationKalman = localizer.liveLocationMsg() msg.liveLocationKalman.inputsOK = sm.all_alive_and_valid() msg.liveLocationKalman.sensorsOK = sm.alive['sensorEvents'] and sm.valid['sensorEvents'] diff --git a/selfdrive/locationd/models/live_kf.py b/selfdrive/locationd/models/live_kf.py index a6a6eaf0b1..d2153487b2 100755 --- a/selfdrive/locationd/models/live_kf.py +++ b/selfdrive/locationd/models/live_kf.py @@ -206,7 +206,7 @@ class LiveKalman(): ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.diag([.2**2, .2**2, .2**2, .2**2])} # init filter - self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err) + self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err, max_rewind_age=0.2) @property def x(self): diff --git a/selfdrive/test/process_replay/compare_logs.py b/selfdrive/test/process_replay/compare_logs.py index 4845575238..8cb4b2453f 100755 --- a/selfdrive/test/process_replay/compare_logs.py +++ b/selfdrive/test/process_replay/compare_logs.py @@ -55,11 +55,12 @@ def remove_ignored_fields(msg, ignore): def compare_logs(log1, log2, ignore_fields=None, ignore_msgs=None, tolerance=None): if ignore_fields is None: ignore_fields = [] - if ignore_msgs is None: ignore_msgs = [] + log1, log2 = [list(filter(lambda m: m.which() not in ignore_msgs, log)) for log in (log1, log2)] - assert len(log1) == len(log2), "logs are not same length: " + str(len(log1)) + " VS " + str(len(log2)) + if len(log1) != len(log2): + raise Exception(f"logs are not same length: {len(log1)} VS {len(log2)}") diff = [] for msg1, msg2 in tqdm(zip(log1, log2)): @@ -77,11 +78,12 @@ def compare_logs(log1, log2, ignore_fields=None, ignore_msgs=None, tolerance=Non tolerance = EPSILON if tolerance is None else tolerance dd = dictdiffer.diff(msg1_dict, msg2_dict, ignore=ignore_fields) - # Dictiffer only supports relative tolerance, we also want to check for absolute + # Dictdiffer only supports relative tolerance, we also want to check for absolute def outside_tolerance(diff): - a, b = diff[2] - if isinstance(a, numbers.Number) and isinstance(b, numbers.Number): - return abs(a - b) > max(tolerance, tolerance * max(abs(a), abs(b))) + if diff[0] == "change": + a, b = diff[2] + if isinstance(a, numbers.Number) and isinstance(b, numbers.Number): + return abs(a - b) > max(tolerance, tolerance * max(abs(a), abs(b))) return True dd = list(filter(outside_tolerance, dd)) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 0c8b6156f5..479987e9fa 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -f3261f36402ad9bc3003feb9de61a04a634f30e1 \ No newline at end of file +d8944d6eb13c30c2fd5bcc1af5dbad07472c42c0 \ No newline at end of file diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index 8156269a8a..73939b0d97 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -71,8 +71,10 @@ def test_process(cfg, lr, cmp_log_fn, ignore_fields=None, ignore_msgs=None): segment = cmp_log_fn.split("/")[-1].split("_")[0] raise Exception("Route never enabled: %s" % segment) - return compare_logs(cmp_log_msgs, log_msgs, ignore_fields+cfg.ignore, ignore_msgs, cfg.tolerance) - + try: + return compare_logs(cmp_log_msgs, log_msgs, ignore_fields+cfg.ignore, ignore_msgs, cfg.tolerance) + except Exception as e: + return str(e) def format_diff(results, ref_commit): diff1, diff2 = "", ""