rely on carstate to be sure (#1363)
* rely on carstate to be sure * relies on carstate now too * don't know how else to make this work * update ref * clean up hacks * prev ref was weirdly bad? * lets try that from my machine Co-authored-by: Willem Melching <willem.melching@gmail.com>albatross
parent
e816467eb5
commit
a18832748c
|
@ -59,6 +59,7 @@ class Calibrator():
|
|||
self.valid_blocks = 0
|
||||
self.cal_status = Calibration.UNCALIBRATED
|
||||
self.just_calibrated = False
|
||||
self.v_ego = 0
|
||||
|
||||
# Read calibration
|
||||
calibration_params = Params().get("CalibrationParams")
|
||||
|
@ -88,8 +89,11 @@ class Calibrator():
|
|||
if start_status == Calibration.UNCALIBRATED and end_status == Calibration.CALIBRATED:
|
||||
self.just_calibrated = True
|
||||
|
||||
def handle_v_ego(self, v_ego):
|
||||
self.v_ego = v_ego
|
||||
|
||||
def handle_cam_odom(self, trans, rot, trans_std, rot_std):
|
||||
straight_and_fast = ((trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER))
|
||||
straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER))
|
||||
certain_if_calib = ((np.arctan2(trans_std[1], trans[0]) < MAX_VEL_ANGLE_STD) or
|
||||
(self.valid_blocks < INPUTS_NEEDED))
|
||||
if straight_and_fast and certain_if_calib:
|
||||
|
@ -132,7 +136,7 @@ class Calibrator():
|
|||
|
||||
def calibrationd_thread(sm=None, pm=None):
|
||||
if sm is None:
|
||||
sm = messaging.SubMaster(['cameraOdometry'])
|
||||
sm = messaging.SubMaster(['cameraOdometry', 'carState'])
|
||||
|
||||
if pm is None:
|
||||
pm = messaging.PubMaster(['liveCalibration'])
|
||||
|
@ -143,18 +147,22 @@ def calibrationd_thread(sm=None, pm=None):
|
|||
while 1:
|
||||
sm.update()
|
||||
|
||||
if sm.updated['carState']:
|
||||
calibrator.handle_v_ego(sm['carState'].vEgo)
|
||||
if send_counter % 25 == 0:
|
||||
calibrator.send_data(pm)
|
||||
send_counter += 1
|
||||
|
||||
if sm.updated['cameraOdometry']:
|
||||
new_vp = calibrator.handle_cam_odom(sm['cameraOdometry'].trans,
|
||||
sm['cameraOdometry'].rot,
|
||||
sm['cameraOdometry'].transStd,
|
||||
sm['cameraOdometry'].rotStd)
|
||||
if DEBUG and new_vp is not None:
|
||||
print('got new vp', new_vp)
|
||||
|
||||
# decimate outputs for efficiency
|
||||
if (send_counter % 5) == 0:
|
||||
calibrator.send_data(pm)
|
||||
send_counter += 1
|
||||
if DEBUG and new_vp is not None:
|
||||
print('got new vp', new_vp)
|
||||
|
||||
# decimate outputs for efficiency
|
||||
|
||||
|
||||
def main(sm=None, pm=None):
|
||||
|
|
|
@ -92,6 +92,7 @@ class FakeSubMaster(messaging.SubMaster):
|
|||
wait_for_event(self.update_ready)
|
||||
self.update_ready.clear()
|
||||
|
||||
|
||||
def update_msgs(self, cur_time, msgs):
|
||||
wait_for_event(self.update_called)
|
||||
self.update_called.clear()
|
||||
|
@ -186,8 +187,14 @@ def radar_rcv_callback(msg, CP, cfg, fsm):
|
|||
def calibration_rcv_callback(msg, CP, cfg, fsm):
|
||||
# calibrationd publishes 1 calibrationData every 5 cameraOdometry packets.
|
||||
# should_recv always true to increment frame
|
||||
recv_socks = ["liveCalibration"] if (fsm.frame + 1) % 5 == 0 else []
|
||||
return recv_socks, True
|
||||
if msg.which() == 'carState':
|
||||
if ((fsm.frame +1)% 25) == 0:
|
||||
recv_socks = ["liveCalibration"]
|
||||
else:
|
||||
recv_socks = []
|
||||
return recv_socks, True
|
||||
else:
|
||||
return [], False
|
||||
|
||||
|
||||
CONFIGS = [
|
||||
|
@ -225,7 +232,8 @@ CONFIGS = [
|
|||
ProcessConfig(
|
||||
proc_name="calibrationd",
|
||||
pub_sub={
|
||||
"cameraOdometry": ["liveCalibration"]
|
||||
"carState": ["liveCalibration"],
|
||||
"cameraOdometry": []
|
||||
},
|
||||
ignore=["logMonoTime", "valid"],
|
||||
init_callback=get_car_params,
|
||||
|
|
|
@ -1 +1 @@
|
|||
9b4b08487380a6741646e22e191fa5bd1615f2d3
|
||||
ea2820c0708a95e8f392d503c7b76090ade380b0
|
Loading…
Reference in New Issue