locationd: publish filter time (#37697)

* Include filter time in the message

* Move the line up

* Use nantonum

cereal/log.capnp

@@ -1426,6 +1426,8 @@ struct LivePose {
   posenetOK @5 :Bool = false;
   sensorsOK @6 :Bool = false;
 
+  timestamp @8 :UInt64;
+
   debugFilterState @7 :FilterState;
 
   struct XYZMeasurement {

selfdrive/locationd/locationd.py

@@ -239,6 +239,7 @@ class LocationEstimator:
     livePose.inputsOK = inputs_valid
     livePose.posenetOK = not std_spike or self.car_speed <= 5.0
     livePose.sensorsOK = sensors_valid
+    livePose.timestamp = int(np.nan_to_num(self.kf.t) * 1e9)
 
     return msg
 

selfdrive/test/process_replay/migration.py

@@ -175,6 +175,7 @@ def migrate_liveLocationKalman(msgs):
     m = messaging.new_message('livePose')
     m.valid = msg.valid
     m.logMonoTime = msg.logMonoTime
+    m.livePose.timestamp = 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 or nans