calibrationd: start faster by not waiting for carParams (#24976)

* calibrationd: start faster by not waiting for carParams

* fix process replay

* update ref
pull/24955/head
Willem Melching 3 years ago committed by GitHub
parent 684d4b75a1
commit de0c12e5af
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
  1. 10
      selfdrive/locationd/calibrationd.py
  2. 3
      selfdrive/test/process_replay/process_replay.py
  3. 2
      selfdrive/test/process_replay/ref_commit

@ -12,7 +12,7 @@ import capnp
import numpy as np import numpy as np
from typing import List, NoReturn, Optional from typing import List, NoReturn, Optional
from cereal import car, log from cereal import log
import cereal.messaging as messaging import cereal.messaging as messaging
from common.conversions import Conversions as CV from common.conversions import Conversions as CV
from common.params import Params, put_nonblocking from common.params import Params, put_nonblocking
@ -62,7 +62,7 @@ class Calibrator:
def __init__(self, param_put: bool = False): def __init__(self, param_put: bool = False):
self.param_put = param_put self.param_put = param_put
self.CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) self.not_car = False
# Read saved calibration # Read saved calibration
params = Params() params = Params()
@ -192,7 +192,7 @@ class Calibrator:
liveCalibration.rpyCalib = smooth_rpy.tolist() liveCalibration.rpyCalib = smooth_rpy.tolist()
liveCalibration.rpyCalibSpread = self.calib_spread.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) extrinsic_matrix = get_view_frame_from_road_frame(0, 0, 0, model_height)
liveCalibration.validBlocks = INPUTS_NEEDED liveCalibration.validBlocks = INPUTS_NEEDED
liveCalibration.calStatus = Calibration.CALIBRATED liveCalibration.calStatus = Calibration.CALIBRATED
@ -212,7 +212,7 @@ def calibrationd_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[m
set_realtime_priority(1) set_realtime_priority(1)
if sm is None: if sm is None:
sm = messaging.SubMaster(['cameraOdometry', 'carState'], poll=['cameraOdometry']) sm = messaging.SubMaster(['cameraOdometry', 'carState', 'carParams'], poll=['cameraOdometry'])
if pm is None: if pm is None:
pm = messaging.PubMaster(['liveCalibration']) pm = messaging.PubMaster(['liveCalibration'])
@ -223,6 +223,8 @@ def calibrationd_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[m
timeout = 0 if sm.frame == -1 else 100 timeout = 0 if sm.frame == -1 else 100
sm.update(timeout) sm.update(timeout)
calibrator.not_car = sm['carParams'].notCar
if sm.updated['cameraOdometry']: if sm.updated['cameraOdometry']:
calibrator.handle_v_ego(sm['carState'].vEgo) calibrator.handle_v_ego(sm['carState'].vEgo)
new_rpy = calibrator.handle_cam_odom(sm['cameraOdometry'].trans, new_rpy = calibrator.handle_cam_odom(sm['cameraOdometry'].trans,

@ -282,7 +282,8 @@ CONFIGS = [
proc_name="calibrationd", proc_name="calibrationd",
pub_sub={ pub_sub={
"carState": ["liveCalibration"], "carState": ["liveCalibration"],
"cameraOdometry": [] "cameraOdometry": [],
"carParams": [],
}, },
ignore=["logMonoTime", "valid"], ignore=["logMonoTime", "valid"],
init_callback=get_car_params, init_callback=get_car_params,

@ -1 +1 @@
41161c8d151b0c2017214cad0aad3156533ab868 b55de9fdb2416e63ab554c95233a78219d8d3db9
Loading…
Cancel
Save