diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py index 7f5541b8c295be..f7780834b05b61 100755 --- a/selfdrive/locationd/locationd.py +++ b/selfdrive/locationd/locationd.py @@ -8,6 +8,7 @@ from collections import defaultdict from cereal import log, messaging +from cereal.services import SERVICE_LIST from openpilot.common.transformations.orientation import rot_from_euler from openpilot.common.realtime import config_realtime_process from openpilot.common.params import Params @@ -23,8 +24,10 @@ MAX_FILTER_REWIND_TIME = 0.8 # s MAX_SENSOR_TIME_DIFF = 0.1 # s YAWRATE_CROSS_ERR_CHECK_FACTOR = 30 -INPUT_INVALID_THRESHOLD = 0.5 -INPUT_INVALID_DECAY = 0.9993 # ~10 secs to resume after a bad input +INPUT_INVALID_THRESHOLD = 0.5 # 0 bad inputs ignored +TIMING_INVALID_THRESHOLD = 2.5 # 2 bad timings ignored +INPUT_INVALID_DECAY = 0.9993 # ~10 secs to resume after exceeding allowed bad inputs by one (at 100hz) +TIMING_INVALID_DECAY = 0.9990 # ~2 secs to resume after exceeding allowed bad timings by one (at 100hz) POSENET_STD_INITIAL_VALUE = 10.0 POSENET_STD_HIST_HALF = 20 @@ -265,10 +268,13 @@ def main(): estimator = LocationEstimator(DEBUG) filter_initialized = False - critcal_services = ["accelerometer", "gyroscope", "liveCalibration", "cameraOdometry"] - observation_timing_invalid = False + critcal_services = ["accelerometer", "gyroscope", "cameraOdometry"] + observation_timing_invalid = defaultdict(int) observation_input_invalid = defaultdict(int) + input_invalid_decay = {s: INPUT_INVALID_DECAY ** (100. / SERVICE_LIST[s].frequency) for s in critcal_services} + timing_invalid_decay = {s: TIMING_INVALID_DECAY ** (100. / SERVICE_LIST[s].frequency) for s in critcal_services} + initial_pose = params.get("LocationFilterInitialState") if initial_pose is not None: initial_pose = json.loads(initial_pose) @@ -282,8 +288,6 @@ def main(): acc_msgs, gyro_msgs = (messaging.drain_sock(sock) for sock in sensor_sockets) if filter_initialized: - observation_timing_invalid = False - msgs = [] for msg in acc_msgs + gyro_msgs: t, valid, which, data = msg.logMonoTime, msg.valid, msg.which(), getattr(msg, msg.which()) @@ -298,18 +302,23 @@ def main(): if valid: t = log_mono_time * 1e-9 res = estimator.handle_log(t, which, msg) + if which not in critcal_services: + continue + if res == HandleLogResult.TIMING_INVALID: - observation_timing_invalid = True + observation_timing_invalid[which] += 1 elif res == HandleLogResult.INPUT_INVALID: observation_input_invalid[which] += 1 else: - observation_input_invalid[which] *= INPUT_INVALID_DECAY + observation_input_invalid[which] *= input_invalid_decay[which] + observation_timing_invalid[which] *= timing_invalid_decay[which] else: filter_initialized = sm.all_checks() and sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION) if sm.updated["cameraOdometry"]: critical_service_inputs_valid = all(observation_input_invalid[s] < INPUT_INVALID_THRESHOLD for s in critcal_services) - inputs_valid = sm.all_valid() and critical_service_inputs_valid and not observation_timing_invalid + critical_service_timing_valid = all(observation_timing_invalid[s] < TIMING_INVALID_THRESHOLD for s in critcal_services) + inputs_valid = sm.all_valid() and critical_service_inputs_valid and critical_service_timing_valid sensors_valid = sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION) msg = estimator.get_msg(sensors_valid, inputs_valid, filter_initialized) diff --git a/selfdrive/locationd/test/test_locationd_scenarios.py b/selfdrive/locationd/test/test_locationd_scenarios.py index 166d715c347475..e0ff96a362861c 100644 --- a/selfdrive/locationd/test/test_locationd_scenarios.py +++ b/selfdrive/locationd/test/test_locationd_scenarios.py @@ -17,6 +17,7 @@ 'sensors_flag': ['sensorsOK'], } JUNK_IDX = 100 +CONSISTENT_SPIKES_COUNT = 10 class Scenario(Enum): @@ -25,6 +26,8 @@ class Scenario(Enum): GYRO_SPIKE_MIDWAY = 'gyro_spike_midway' ACCEL_OFF = 'accel_off' ACCEL_SPIKE_MIDWAY = 'accel_spike_midway' + SENSOR_TIMING_SPIKE_MIDWAY = 'timing_spikes' + SENSOR_TIMING_CONSISTENT_SPIKES = 'timing_consistent_spikes' def get_select_fields_data(logs): @@ -43,6 +46,17 @@ def get_nested_keys(msg, keys): return data +def modify_logs_midway(logs, which, count, fn): + non_which = [x for x in logs if x.which() != which] + which = [x for x in logs if x.which() == which] + temps = which[len(which) // 2:len(which) // 2 + count] + for i, temp in enumerate(temps): + temp = temp.as_builder() + fn(temp) + which[len(which) // 2 + i] = temp.as_reader() + return sorted(non_which + which, key=lambda x: x.logMonoTime) + + def run_scenarios(scenario, logs): if scenario == Scenario.BASE: pass @@ -51,23 +65,23 @@ def run_scenarios(scenario, logs): logs = sorted([x for x in logs if x.which() != 'gyroscope'], key=lambda x: x.logMonoTime) elif scenario == Scenario.GYRO_SPIKE_MIDWAY: - non_gyro = [x for x in logs if x.which() not in 'gyroscope'] - gyro = [x for x in logs if x.which() in 'gyroscope'] - temp = gyro[len(gyro) // 2].as_builder() - temp.gyroscope.gyroUncalibrated.v[0] += 3.0 - gyro[len(gyro) // 2] = temp.as_reader() - logs = sorted(non_gyro + gyro, key=lambda x: x.logMonoTime) + def gyro_spike(msg): + msg.gyroscope.gyroUncalibrated.v[0] += 3.0 + logs = modify_logs_midway(logs, 'gyroscope', 1, gyro_spike) elif scenario == Scenario.ACCEL_OFF: logs = sorted([x for x in logs if x.which() != 'accelerometer'], key=lambda x: x.logMonoTime) elif scenario == Scenario.ACCEL_SPIKE_MIDWAY: - non_accel = [x for x in logs if x.which() not in 'accelerometer'] - accel = [x for x in logs if x.which() in 'accelerometer'] - temp = accel[len(accel) // 2].as_builder() - temp.accelerometer.acceleration.v[0] += 10.0 - accel[len(accel) // 2] = temp.as_reader() - logs = sorted(non_accel + accel, key=lambda x: x.logMonoTime) + def acc_spike(msg): + msg.accelerometer.acceleration.v[0] += 10.0 + logs = modify_logs_midway(logs, 'accelerometer', 1, acc_spike) + + elif scenario == Scenario.SENSOR_TIMING_SPIKE_MIDWAY or scenario == Scenario.SENSOR_TIMING_CONSISTENT_SPIKES: + def timing_spike(msg): + msg.accelerometer.timestamp -= int(0.150 * 1e9) + count = 1 if scenario == Scenario.SENSOR_TIMING_SPIKE_MIDWAY else CONSISTENT_SPIKES_COUNT + logs = modify_logs_midway(logs, 'accelerometer', count, timing_spike) replayed_logs = replay_process_with_name(name='locationd', lr=logs) return get_select_fields_data(logs), get_select_fields_data(replayed_logs) @@ -122,7 +136,7 @@ def test_gyro_spikes(self): assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.35)) assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55)) assert np.diff(replayed_data['inputs_flag'])[499] == -1.0 - assert np.diff(replayed_data['inputs_flag'])[696] == 1.0 + assert np.diff(replayed_data['inputs_flag'])[704] == 1.0 def test_accel_off(self): """ @@ -146,3 +160,21 @@ def test_accel_spikes(self): orig_data, replayed_data = run_scenarios(Scenario.ACCEL_SPIKE_MIDWAY, self.logs) assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.35)) assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55)) + + def test_single_timing_spike(self): + """ + Test: timing of 150ms off for the single accelerometer message in the middle of the segment + Expected Result: the message is ignored, and inputsOK is False for that time + """ + orig_data, replayed_data = run_scenarios(Scenario.SENSOR_TIMING_SPIKE_MIDWAY, self.logs) + assert np.all(replayed_data['inputs_flag'] == orig_data['inputs_flag']) + assert np.all(replayed_data['sensors_flag'] == orig_data['sensors_flag']) + + def test_consistent_timing_spikes(self): + """ + Test: consistent timing spikes for N accelerometer messages in the middle of the segment + Expected Result: inputsOK becomes False after N of bad measurements + """ + orig_data, replayed_data = run_scenarios(Scenario.SENSOR_TIMING_CONSISTENT_SPIKES, self.logs) + assert np.diff(replayed_data['inputs_flag'])[500] == -1.0 + assert np.diff(replayed_data['inputs_flag'])[787] == 1.0