From fc65a8ea8f9f374343cd3eb8c59fc955b75b013a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Mon, 7 Apr 2025 19:23:11 -0700 Subject: [PATCH] paramsd: cache backwards compatibility (#34991) * LiveParametersV2 key * Param key * Fix tests * Remove old params if error * Update exception message * Update the test * Add test for corrupted byte format in v1 * Rename params_reader => params --- common/params_keys.h | 1 + selfdrive/locationd/paramsd.py | 32 +++++++++++++----------- selfdrive/locationd/test/test_paramsd.py | 13 +++++++++- 3 files changed, 30 insertions(+), 16 deletions(-) diff --git a/common/params_keys.h b/common/params_keys.h index 2b540b744c..fd701cda18 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -72,6 +72,7 @@ inline static std::unordered_map keys = { {"LastUpdateException", CLEAR_ON_MANAGER_START}, {"LastUpdateTime", PERSISTENT}, {"LiveParameters", PERSISTENT}, + {"LiveParametersV2", PERSISTENT}, {"LiveTorqueParameters", PERSISTENT | DONT_LOG}, {"LocationFilterInitialState", PERSISTENT}, {"LongitudinalManeuverMode", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 243abc5c08..a7712ba1d9 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -200,26 +200,28 @@ def check_valid_with_hysteresis(current_valid: bool, val: float, threshold: floa # TODO: Remove this function after few releases (added in 0.9.9) -def migrate_cached_vehicle_params_if_needed(params_reader: Params): - last_parameters_data = params_reader.get("LiveParameters") - if last_parameters_data is None: +def migrate_cached_vehicle_params_if_needed(params: Params): + last_parameters_data_old = params.get("LiveParameters") + last_parameters_data = params.get("LiveParametersV2") + if last_parameters_data_old is None or last_parameters_data is not None: return try: - last_parameters_dict = json.loads(last_parameters_data) + last_parameters_dict = json.loads(last_parameters_data_old) last_parameters_msg = messaging.new_message('liveParameters') last_parameters_msg.liveParameters.valid = True last_parameters_msg.liveParameters.steerRatio = last_parameters_dict['steerRatio'] last_parameters_msg.liveParameters.stiffnessFactor = last_parameters_dict['stiffnessFactor'] last_parameters_msg.liveParameters.angleOffsetAverageDeg = last_parameters_dict['angleOffsetAverageDeg'] - params_reader.put("LiveParameters", last_parameters_msg.to_bytes()) - except Exception: - pass + params.put("LiveParametersV2", last_parameters_msg.to_bytes()) + except Exception as e: + cloudlog.error(f"Failed to perform parameter migration: {e}") + params.remove("LiveParameters") -def retrieve_initial_vehicle_params(params_reader: Params, CP: car.CarParams, replay: bool, debug: bool): - last_parameters_data = params_reader.get("LiveParameters") - last_carparams_data = params_reader.get("CarParamsPrevRoute") +def retrieve_initial_vehicle_params(params: Params, CP: car.CarParams, replay: bool, debug: bool): + last_parameters_data = params.get("LiveParametersV2") + last_carparams_data = params.get("CarParamsPrevRoute") steer_ratio, stiffness_factor, angle_offset_deg, p_initial = CP.steerRatio, 1.0, 0.0, None @@ -267,12 +269,12 @@ def main(): pm = messaging.PubMaster(['liveParameters']) sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState'], poll='livePose') - params_reader = Params() - CP = messaging.log_from_bytes(params_reader.get("CarParams", block=True), car.CarParams) + params = Params() + CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) - migrate_cached_vehicle_params_if_needed(params_reader) + migrate_cached_vehicle_params_if_needed(params) - steer_ratio, stiffness_factor, angle_offset_deg, pInitial = retrieve_initial_vehicle_params(params_reader, CP, REPLAY, DEBUG) + steer_ratio, stiffness_factor, angle_offset_deg, pInitial = retrieve_initial_vehicle_params(params, CP, REPLAY, DEBUG) learner = VehicleParamsLearner(CP, steer_ratio, stiffness_factor, np.radians(angle_offset_deg), pInitial) while True: @@ -288,7 +290,7 @@ def main(): msg_dat = msg.to_bytes() if sm.frame % 1200 == 0: # once a minute - params_reader.put_nonblocking("LiveParameters", msg_dat) + params.put_nonblocking("LiveParametersV2", msg_dat) pm.send('liveParameters', msg_dat) diff --git a/selfdrive/locationd/test/test_paramsd.py b/selfdrive/locationd/test/test_paramsd.py index 3c2bf9249a..2129bf4386 100644 --- a/selfdrive/locationd/test/test_paramsd.py +++ b/selfdrive/locationd/test/test_paramsd.py @@ -28,7 +28,7 @@ class TestParamsd: CP = next(m for m in lr if m.which() == "carParams").carParams msg = get_random_live_parameters(CP) - params.put("LiveParameters", msg.to_bytes()) + params.put("LiveParametersV2", msg.to_bytes()) params.put("CarParamsPrevRoute", CP.as_builder().to_bytes()) migrate_cached_vehicle_params_if_needed(params) # this is not tested here but should not mess anything up or throw an error @@ -49,9 +49,20 @@ class TestParamsd: msg = get_random_live_parameters(CP) params.put("LiveParameters", json.dumps(msg.liveParameters.to_dict())) params.put("CarParamsPrevRoute", CP.as_builder().to_bytes()) + params.remove("LiveParametersV2") migrate_cached_vehicle_params_if_needed(params) sr, sf, offset, _ = retrieve_initial_vehicle_params(params, CP, replay=True, debug=True) np.testing.assert_allclose(sr, msg.liveParameters.steerRatio) np.testing.assert_allclose(sf, msg.liveParameters.stiffnessFactor) np.testing.assert_allclose(offset, msg.liveParameters.angleOffsetAverageDeg) + assert params.get("LiveParametersV2") is not None + + def test_read_saved_params_corrupted_old_format(self): + params = Params() + params.put("LiveParameters", b'\x00\x00\x02\x00\x01\x00:F\xde\xed\xae;') + params.remove("LiveParametersV2") + + migrate_cached_vehicle_params_if_needed(params) + assert params.get("LiveParameters") is None + assert params.get("LiveParametersV2") is None