process_replay: liveLocationKalman migration (#33472)

* migrate_livePose

* Remove unused param

* Dont migrate if livePose is there

selfdrive/test/process_replay/migration.py

@@ -9,13 +9,14 @@ from panda import Panda
 
 
 # TODO: message migration should happen in-place
-def migrate_all(lr, old_logtime=False, manager_states=False, panda_states=False, camera_states=False):
+def migrate_all(lr, manager_states=False, panda_states=False, camera_states=False):
-  msgs = migrate_sensorEvents(lr, old_logtime)
+  msgs = migrate_sensorEvents(lr)
-  msgs = migrate_carParams(msgs, old_logtime)
+  msgs = migrate_carParams(msgs)
   msgs = migrate_gpsLocation(msgs)
   msgs = migrate_deviceState(msgs)
   msgs = migrate_carOutput(msgs)
   msgs = migrate_controlsState(msgs)
+  msgs = migrate_liveLocationKalman(msgs)
   if manager_states:
     msgs = migrate_managerState(msgs)
   if panda_states:
@@ -27,6 +28,33 @@ def migrate_all(lr, old_logtime=False, manager_states=False, panda_states=False,
   return msgs
 
 
+def migrate_liveLocationKalman(lr):
+  # migration needed only for routes before livePose
+  if any(msg.which() == 'livePose' for msg in lr):
+    return lr
+
+  all_msgs = []
+  for msg in lr:
+    if msg.which() != 'liveLocationKalmanDEPRECATED':
+      all_msgs.append(msg)
+      continue
+
+    m = messaging.new_message('livePose')
+    m.valid = msg.valid
+    m.logMonoTime = msg.logMonoTime
+    for field in ["orientationNED", "velocityDevice", "accelerationDevice", "angularVelocityDevice"]:
+      lp_field, llk_field = getattr(m.livePose, field), getattr(msg.liveLocationKalmanDEPRECATED, field)
+      lp_field.x, lp_field.y, lp_field.z = llk_field.value
+      lp_field.xStd, lp_field.yStd, lp_field.zStd = llk_field.std
+      lp_field.valid = llk_field.valid
+    for flag in ["inputsOK", "posenetOK", "sensorsOK"]:
+      setattr(m.livePose, flag, getattr(msg.liveLocationKalmanDEPRECATED, flag))
+
+    all_msgs.append(m.as_reader())
+
+  return all_msgs
+
+
 def migrate_controlsState(lr):
   ret = []
   last_cs = None
@@ -225,7 +253,7 @@ def migrate_cameraStates(lr):
   return all_msgs
 
 
-def migrate_carParams(lr, old_logtime=False):
+def migrate_carParams(lr):
   all_msgs = []
   for msg in lr:
     if msg.which() == 'carParams':
@@ -233,7 +261,6 @@ def migrate_carParams(lr, old_logtime=False):
       CP.carParams.carFingerprint = MIGRATION.get(CP.carParams.carFingerprint, CP.carParams.carFingerprint)
       for car_fw in CP.carParams.carFw:
         car_fw.brand = CP.carParams.carName
-      if old_logtime:
       CP.logMonoTime = msg.logMonoTime
       msg = CP.as_reader()
     all_msgs.append(msg)
@@ -241,7 +268,7 @@ def migrate_carParams(lr, old_logtime=False):
   return all_msgs
 
 
-def migrate_sensorEvents(lr, old_logtime=False):
+def migrate_sensorEvents(lr):
   all_msgs = []
   for msg in lr:
     if msg.which() != 'sensorEventsDEPRECATED':
@@ -265,7 +292,6 @@ def migrate_sensorEvents(lr, old_logtime=False):
 
       m = messaging.new_message(sensor_service)
       m.valid = True
-      if old_logtime:
       m.logMonoTime = msg.logMonoTime
 
       m_dat = getattr(m, sensor_service)
@@ -273,7 +299,6 @@ def migrate_sensorEvents(lr, old_logtime=False):
       m_dat.sensor = evt.sensor
       m_dat.type = evt.type
       m_dat.source = evt.source
-      if old_logtime:
       m_dat.timestamp = evt.timestamp
       setattr(m_dat, evt.which(), getattr(evt, evt.which()))
 

selfdrive/test/process_replay/process_replay.py

@@ -649,7 +649,7 @@ def replay_process(
   else:
     cfgs = [cfg]
 
-  all_msgs = migrate_all(lr, old_logtime=True,
+  all_msgs = migrate_all(lr,
                          manager_states=True,
                          panda_states=any("pandaStates" in cfg.pubs for cfg in cfgs),
                          camera_states=any(len(cfg.vision_pubs) != 0 for cfg in cfgs))