diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index e086b8aaf8..fe523dbd35 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -153,7 +153,7 @@ def main(demo=False): # messaging pm = PubMaster(["modelV2", "cameraOdometry"]) - sm = SubMaster(["carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction", "carControl"]) + sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction", "carControl"]) publish_state = PublishState() params = Params() @@ -225,7 +225,7 @@ def main(demo=False): is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId lateral_control_params = np.array([sm["carState"].vEgo, steer_delay], dtype=np.float32) - if sm.updated["liveCalibration"]: + if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']: device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) model_transform_main = get_warp_matrix(device_from_calib_euler, main_wide_camera, False).astype(np.float32) model_transform_extra = get_warp_matrix(device_from_calib_euler, True, True).astype(np.float32) diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py index 97b7c7c46c..94895285df 100755 --- a/selfdrive/test/process_replay/model_replay.py +++ b/selfdrive/test/process_replay/model_replay.py @@ -107,14 +107,17 @@ def model_replay(lr, frs): # modeld is using frame pairs modeld_logs = trim_logs_to_max_frames(lr, MAX_FRAMES, {"roadCameraState", "wideRoadCameraState"}, {"roadEncodeIdx", "wideRoadEncodeIdx", "carParams"}) dmodeld_logs = trim_logs_to_max_frames(lr, MAX_FRAMES, {"driverCameraState"}, {"driverEncodeIdx", "carParams"}) + if not SEND_EXTRA_INPUTS: - modeld_logs = [msg for msg in modeld_logs if msg.which() not in ["liveCalibration",]] - dmodeld_logs = [msg for msg in dmodeld_logs if msg.which() not in ["liveCalibration",]] - # initial calibration - cal_msg = next(msg for msg in lr if msg.which() == "liveCalibration").as_builder() - cal_msg.logMonoTime = lr[0].logMonoTime - modeld_logs.insert(0, cal_msg.as_reader()) - dmodeld_logs.insert(0, cal_msg.as_reader()) + modeld_logs = [msg for msg in modeld_logs if msg.which() != 'liveCalibration'] + dmodeld_logs = [msg for msg in dmodeld_logs if msg.which() != 'liveCalibration'] + + # initial setup + for s in ('liveCalibration', 'deviceState'): + msg = next(msg for msg in lr if msg.which() == s).as_builder() + msg.logMonoTime = lr[0].logMonoTime + modeld_logs.insert(1, msg.as_reader()) + dmodeld_logs.insert(1, msg.as_reader()) modeld = get_process_config("modeld") dmonitoringmodeld = get_process_config("dmonitoringmodeld") @@ -218,7 +221,8 @@ if __name__ == "__main__": results[TEST_ROUTE]["models"] = compare_logs(cmp_log, log_msgs, tolerance=tolerance, ignore_fields=ignore) diff_short, diff_long, failed = format_diff(results, log_paths, ref_commit) - print(diff_long) + if "CI" in os.environ: + print(diff_long) print('-------------\n'*5) print(diff_short) with open("model_diff.txt", "w") as f: diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 5119be0a8c..233f5d746a 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -546,7 +546,7 @@ CONFIGS = [ ), ProcessConfig( proc_name="modeld", - pubs=["roadCameraState", "wideRoadCameraState", "liveCalibration", "driverMonitoringState"], + pubs=["deviceState", "roadCameraState", "wideRoadCameraState", "liveCalibration", "driverMonitoringState"], subs=["modelV2", "cameraOdometry"], ignore=["logMonoTime", "modelV2.frameDropPerc", "modelV2.modelExecutionTime"], should_recv_callback=ModeldCameraSyncRcvCallback(),