From 60e056caaec8583cb200165778bfc123cf801a43 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 27 Jun 2022 14:25:14 +0200 Subject: [PATCH 1/3] calibrationd: start faster by not waiting for carParams --- selfdrive/locationd/calibrationd.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index e092c939ae3126..64bcbc1804a615 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 @@ -62,7 +62,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() @@ -192,7 +192,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 @@ -212,7 +212,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']) @@ -232,6 +232,9 @@ def calibrationd_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[m if DEBUG and new_rpy is not None: print('got new rpy', new_rpy) + elif sm.updated['carParams']: + calibrator.not_car = sm['carParams'].notCar + # 4Hz driven by cameraOdometry if sm.frame % 5 == 0: calibrator.send_data(pm) From b55de9fdb2416e63ab554c95233a78219d8d3db9 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 27 Jun 2022 15:03:09 +0200 Subject: [PATCH 2/3] fix process replay --- selfdrive/locationd/calibrationd.py | 5 ++--- selfdrive/test/process_replay/process_replay.py | 3 ++- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 64bcbc1804a615..81bcc6ce1c9b8d 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -223,6 +223,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, @@ -232,9 +234,6 @@ def calibrationd_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[m if DEBUG and new_rpy is not None: print('got new rpy', new_rpy) - elif sm.updated['carParams']: - calibrator.not_car = sm['carParams'].notCar - # 4Hz driven by cameraOdometry if sm.frame % 5 == 0: calibrator.send_data(pm) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 62c5eb4826ac53..228f07c4c246f2 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -282,7 +282,8 @@ def ublox_rcv_callback(msg): proc_name="calibrationd", pub_sub={ "carState": ["liveCalibration"], - "cameraOdometry": [] + "cameraOdometry": [], + "carParams": [], }, ignore=["logMonoTime", "valid"], init_callback=get_car_params, From 2c15d0eec2a58a2d7ec4c7e26de6ce87e718140c Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 27 Jun 2022 15:05:11 +0200 Subject: [PATCH 3/3] update ref --- selfdrive/test/process_replay/ref_commit | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 662f8cc5b85247..292c41b9789abb 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -41161c8d151b0c2017214cad0aad3156533ab868 +b55de9fdb2416e63ab554c95233a78219d8d3db9 \ No newline at end of file