modeld: prep for camera transform refactor (#31820)

* modeld: prep for camera transform refactor

* update refs

* add pub

* do setup

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)

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:

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(),