From 25e5c57dfe06b848ffd2c4619b5ca04c1cf18f88 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Harald=20Sch=C3=A4fer?= Date: Thu, 28 Sep 2023 14:27:50 -0700 Subject: [PATCH] calibrationd: Pitch spread different tolerance (#30065) * calibrationd: Pitch spread different tolerance * Improve calibrationd tests old-commit-hash: 2162d149e8c8c6a4ac3b1acaf9462ff2f61aefbc --- selfdrive/locationd/calibrationd.py | 6 +- selfdrive/locationd/test/test_calibrationd.py | 120 +++++++----------- 2 files changed, 53 insertions(+), 73 deletions(-) diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 6469ece402..566373d7b8 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -31,7 +31,8 @@ SMOOTH_CYCLES = 10 BLOCK_SIZE = 100 INPUTS_NEEDED = 5 # Minimum blocks needed for valid calibration INPUTS_WANTED = 50 # We want a little bit more than we need for stability -MAX_ALLOWED_SPREAD = np.radians(2) +MAX_ALLOWED_YAW_SPREAD = np.radians(2) +MAX_ALLOWED_PITCH_SPREAD = np.radians(4) RPY_INIT = np.array([0.0,0.0,0.0]) WIDE_FROM_DEVICE_EULER_INIT = np.array([0.0, 0.0, 0.0]) HEIGHT_INIT = np.array([1.22]) @@ -156,7 +157,8 @@ class Calibrator: # If spread is too high, assume mounting was changed and reset to last block. # Make the transition smooth. Abrupt transitions are not good for feedback loop through supercombo model. # TODO: add height spread check with smooth transition too - if max(self.calib_spread) > MAX_ALLOWED_SPREAD and self.cal_status == log.LiveCalibrationData.Status.calibrated: + spread_too_high = self.calib_spread[1] > MAX_ALLOWED_PITCH_SPREAD or self.calib_spread[2] > MAX_ALLOWED_YAW_SPREAD + if spread_too_high and self.cal_status == log.LiveCalibrationData.Status.calibrated: self.reset(self.rpys[self.block_idx - 1], valid_blocks=1, smooth_from=self.rpy) self.cal_status = log.LiveCalibrationData.Status.recalibrating diff --git a/selfdrive/locationd/test/test_calibrationd.py b/selfdrive/locationd/test/test_calibrationd.py index a5eedaf99a..e2db094397 100755 --- a/selfdrive/locationd/test/test_calibrationd.py +++ b/selfdrive/locationd/test/test_calibrationd.py @@ -8,8 +8,28 @@ import cereal.messaging as messaging from cereal import log from openpilot.common.params import Params from openpilot.selfdrive.locationd.calibrationd import Calibrator, INPUTS_NEEDED, INPUTS_WANTED, BLOCK_SIZE, MIN_SPEED_FILTER, \ - MAX_YAW_RATE_FILTER, SMOOTH_CYCLES, HEIGHT_INIT - + MAX_YAW_RATE_FILTER, SMOOTH_CYCLES, HEIGHT_INIT, MAX_ALLOWED_PITCH_SPREAD, MAX_ALLOWED_YAW_SPREAD + + +def process_messages(c, cam_odo_calib, cycles, + cam_odo_speed=MIN_SPEED_FILTER + 1, + carstate_speed=MIN_SPEED_FILTER + 1, + cam_odo_yr=0.0, + cam_odo_speed_std=1e-3, + cam_odo_height_std=1e-3): + old_rpy_weight_prev = 0.0 + for _ in range(cycles): + assert (old_rpy_weight_prev - c.old_rpy_weight < 1/SMOOTH_CYCLES + 1e-3) + old_rpy_weight_prev = c.old_rpy_weight + c.handle_v_ego(carstate_speed) + c.handle_cam_odom([cam_odo_speed, + np.sin(cam_odo_calib[2]) * cam_odo_speed, + -np.sin(cam_odo_calib[1]) * cam_odo_speed], + [0.0, 0.0, cam_odo_yr], + [0.0, 0.0, 0.0], + [cam_odo_speed_std, cam_odo_speed_std, cam_odo_speed_std], + [0.0, 0.0, HEIGHT_INIT.item()], + [cam_odo_height_std, cam_odo_height_std, cam_odo_height_std]) class TestCalibrationd(unittest.TestCase): @@ -28,14 +48,7 @@ class TestCalibrationd(unittest.TestCase): def test_calibration_basics(self): c = Calibrator(param_put=False) - for _ in range(BLOCK_SIZE * INPUTS_WANTED): - c.handle_v_ego(MIN_SPEED_FILTER + 1) - c.handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [1e-3, 1e-3, 1e-3], - [0.0, 0.0, HEIGHT_INIT.item()], - [1e-3, 1e-3, 1e-3]) + process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED) self.assertEqual(c.valid_blocks, INPUTS_WANTED) np.testing.assert_allclose(c.rpy, np.zeros(3)) np.testing.assert_allclose(c.height, HEIGHT_INIT) @@ -44,22 +57,8 @@ class TestCalibrationd(unittest.TestCase): def test_calibration_low_speed_reject(self): c = Calibrator(param_put=False) - for _ in range(BLOCK_SIZE * INPUTS_WANTED): - c.handle_v_ego(MIN_SPEED_FILTER - 1) - c.handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [1e-3, 1e-3, 1e-3], - [0.0, 0.0, HEIGHT_INIT.item()], - [1e-3, 1e-3, 1e-3]) - for _ in range(BLOCK_SIZE * INPUTS_WANTED): - c.handle_v_ego(MIN_SPEED_FILTER + 1) - c.handle_cam_odom([MIN_SPEED_FILTER - 1, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [1e-3, 1e-3, 1e-3], - [0.0, 0.0, HEIGHT_INIT.item()], - [1e-3, 1e-3, 1e-3]) + process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_speed=MIN_SPEED_FILTER - 1) + process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, carstate_speed=MIN_SPEED_FILTER - 1) self.assertEqual(c.valid_blocks, 0) np.testing.assert_allclose(c.rpy, np.zeros(3)) np.testing.assert_allclose(c.height, HEIGHT_INIT) @@ -67,14 +66,7 @@ class TestCalibrationd(unittest.TestCase): def test_calibration_yaw_rate_reject(self): c = Calibrator(param_put=False) - for _ in range(BLOCK_SIZE * INPUTS_WANTED): - c.handle_v_ego(MIN_SPEED_FILTER + 1) - c.handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0], - [0.0, 0.0, MAX_YAW_RATE_FILTER ], - [0.0, 0.0, 0.0], - [1e-3, 1e-3, 1e-3], - [0.0, 0.0, HEIGHT_INIT.item()], - [1e-3, 1e-3, 1e-3]) + process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_yr=MAX_YAW_RATE_FILTER) self.assertEqual(c.valid_blocks, 0) np.testing.assert_allclose(c.rpy, np.zeros(3)) np.testing.assert_allclose(c.height, HEIGHT_INIT) @@ -82,58 +74,44 @@ class TestCalibrationd(unittest.TestCase): def test_calibration_speed_std_reject(self): c = Calibrator(param_put=False) - for _ in range(BLOCK_SIZE * INPUTS_WANTED): - c.handle_v_ego(MIN_SPEED_FILTER + 1) - c.handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [1e3, 1e3, 1e3], - [0.0, 0.0, HEIGHT_INIT.item()], - [1e-3, 1e-3, 1e-3]) + process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_speed_std=1e3) self.assertEqual(c.valid_blocks, INPUTS_NEEDED) np.testing.assert_allclose(c.rpy, np.zeros(3)) def test_calibration_speed_std_height_reject(self): c = Calibrator(param_put=False) - for _ in range(BLOCK_SIZE * INPUTS_WANTED): - c.handle_v_ego(MIN_SPEED_FILTER + 1) - c.handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [1e-3, 1e-3, 1e-3], - [0.0, 0.0, HEIGHT_INIT.item()], - [1e3, 1e3, 1e3]) + process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_height_std=1e3) self.assertEqual(c.valid_blocks, INPUTS_NEEDED) np.testing.assert_allclose(c.rpy, np.zeros(3)) def test_calibration_auto_reset(self): c = Calibrator(param_put=False) - for _ in range(BLOCK_SIZE * INPUTS_WANTED): - c.handle_v_ego(MIN_SPEED_FILTER + 1) - c.handle_cam_odom([MIN_SPEED_FILTER + 1, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [1e-3, 1e-3, 1e-3], - [0.0, 0.0, HEIGHT_INIT.item()], - [1e-3, 1e-3, 1e-3]) - self.assertEqual(c.valid_blocks, INPUTS_WANTED) + process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED) + self.assertEqual(c.valid_blocks, INPUTS_NEEDED) + np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0], atol=1e-3) + process_messages(c, [0.0, MAX_ALLOWED_PITCH_SPREAD*0.9, MAX_ALLOWED_YAW_SPREAD*0.9], BLOCK_SIZE + 10) + self.assertEqual(c.valid_blocks, INPUTS_NEEDED + 1) + self.assertEqual(c.cal_status, log.LiveCalibrationData.Status.calibrated) + + c = Calibrator(param_put=False) + process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED) + self.assertEqual(c.valid_blocks, INPUTS_NEEDED) + np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0]) + process_messages(c, [0.0, MAX_ALLOWED_PITCH_SPREAD*1.1, 0.0], BLOCK_SIZE + 10) + self.assertEqual(c.valid_blocks, 1) + self.assertEqual(c.cal_status, log.LiveCalibrationData.Status.recalibrating) + np.testing.assert_allclose(c.rpy, [0.0, MAX_ALLOWED_PITCH_SPREAD*1.1, 0.0], atol=1e-2) + + c = Calibrator(param_put=False) + process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED) + self.assertEqual(c.valid_blocks, INPUTS_NEEDED) np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0]) - old_rpy_weight_prev = 0.0 - for _ in range(BLOCK_SIZE + 10): - self.assertLess(old_rpy_weight_prev - c.old_rpy_weight, 1/SMOOTH_CYCLES + 1e-3) - old_rpy_weight_prev = c.old_rpy_weight - c.handle_v_ego(MIN_SPEED_FILTER + 1) - c.handle_cam_odom([MIN_SPEED_FILTER + 1, -0.05 * MIN_SPEED_FILTER, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [1e-3, 1e-3, 1e-3], - [0.0, 0.0, HEIGHT_INIT.item()], - [1e-3, 1e-3, 1e-3]) + process_messages(c, [0.0, 0.0, MAX_ALLOWED_YAW_SPREAD*1.1], BLOCK_SIZE + 10) self.assertEqual(c.valid_blocks, 1) self.assertEqual(c.cal_status, log.LiveCalibrationData.Status.recalibrating) - np.testing.assert_allclose(c.rpy, [0.0, 0.0, -0.05], atol=1e-2) + np.testing.assert_allclose(c.rpy, [0.0, 0.0, MAX_ALLOWED_YAW_SPREAD*1.1], atol=1e-2) if __name__ == "__main__": unittest.main()