From 7144c0b862744c4a1f9611813ec0a1fcce3636e8 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 27 Jun 2022 15:34:36 +0200 Subject: [PATCH] calibrationd: start faster by not waiting for carParams (#24976) * calibrationd: start faster by not waiting for carParams * fix process replay * update ref --- selfdrive/locationd/calibrationd.py | 10 ++++++---- selfdrive/test/process_replay/process_replay.py | 3 ++- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 51c84b847db6e7..aade8a5d341796 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -12,7 +12,7 @@ import numpy as np from typing import List, NoReturn, Optional -from cereal import car, log +from cereal import log import cereal.messaging as messaging from common.conversions import Conversions as CV from common.params import Params, put_nonblocking @@ -63,7 +63,7 @@ class Calibrator: def __init__(self, param_put: bool = False): self.param_put = param_put - self.CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) + self.not_car = False # Read saved calibration params = Params() @@ -193,7 +193,7 @@ def get_msg(self) -> capnp.lib.capnp._DynamicStructBuilder: liveCalibration.rpyCalib = smooth_rpy.tolist() liveCalibration.rpyCalibSpread = self.calib_spread.tolist() - if self.CP.notCar: + if self.not_car: extrinsic_matrix = get_view_frame_from_road_frame(0, 0, 0, model_height) liveCalibration.validBlocks = INPUTS_NEEDED liveCalibration.calStatus = Calibration.CALIBRATED @@ -213,7 +213,7 @@ def calibrationd_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[m set_realtime_priority(1) if sm is None: - sm = messaging.SubMaster(['cameraOdometry', 'carState'], poll=['cameraOdometry']) + sm = messaging.SubMaster(['cameraOdometry', 'carState', 'carParams'], poll=['cameraOdometry']) if pm is None: pm = messaging.PubMaster(['liveCalibration']) @@ -224,6 +224,8 @@ def calibrationd_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[m timeout = 0 if sm.frame == -1 else 100 sm.update(timeout) + calibrator.not_car = sm['carParams'].notCar + if sm.updated['cameraOdometry']: calibrator.handle_v_ego(sm['carState'].vEgo) new_rpy = calibrator.handle_cam_odom(sm['cameraOdometry'].trans, diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 4691dcb402f3ee..2c76594500a3c7 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -277,7 +277,8 @@ def ublox_rcv_callback(msg): proc_name="calibrationd", pub_sub={ "carState": ["liveCalibration"], - "cameraOdometry": [] + "cameraOdometry": [], + "carParams": [], }, ignore=["logMonoTime", "valid"], init_callback=get_car_params,