Align lidar and cam time stamps

Runtime/Tx/CompressedImageTx.cs

@@ -5,6 +5,7 @@
 using Unity.Collections;
 using UnityEngine;
 using UnityEngine.Rendering;
+using Time = std_msgs.Time;
 
 namespace ProBridge.Tx.Sensor
 {
@@ -34,10 +35,11 @@ public enum Format
         private RenderTexture renderTexture;
         private Texture2D texture2D;
         private TJCompressor compressor;
-        private bool newFrameAvailable;
+        private bool newRawTextureAvailable;
+        private bool newFrameAvailable = false;
         private NativeArray<Color32> imageData;
 
-        private byte[] rawTextureData;
+        private (byte[], Time) rawTextureData;
         private Thread jpegCompressionThread;
 
         private DateTime lastRenderTime;
@@ -75,31 +77,40 @@ protected override void OnStart()
             jpegCompressionThread.Start();
         }
 
+
         void RenderLoop()
         {
             renderCamera.Render();
-            AsyncGPUReadback.Request(renderTexture, 0, TextureFormat.RGB24, OnCompleteReadback);
+            Time frameTimestamp = ProBridgeServer.SimTime;
+            AsyncGPUReadback.Request(renderTexture, 0, TextureFormat.RGB24, (request) => OnCompleteReadback(request, frameTimestamp));
         }
 
         protected override ProBridge.Msg GetMsg(TimeSpan ts)
         {
             data.format = format.ToString();
 
-            if (format == Format.png && newFrameAvailable)
+            if (format == Format.png && newRawTextureAvailable)
             {
                 // TODO: optimize png encoding
+                data.header.stamp = ProBridgeServer.SimTime;
                 data.data = texture2D.EncodeToPNG();
-                newFrameAvailable = false;
-
+                newRawTextureAvailable = false;
+                
                 UpdateFrameRate();
+                return base.GetMsg(data.header.stamp);
             }
 
-            data.data ??= new byte[] { 0, 0, 0};
+            if (format == Format.jpeg && newFrameAvailable)
+            {
+                newFrameAvailable = false;
+                return base.GetMsg(data.header.stamp);
+
+            }
 
-            return base.GetMsg(ts);
+            return null;
         }
 
-        private void OnCompleteReadback(AsyncGPUReadbackRequest request)
+        private void OnCompleteReadback(AsyncGPUReadbackRequest request, Time frameTimestamp)
         {
             if (request.hasError)
             {
@@ -115,26 +126,30 @@ private void OnCompleteReadback(AsyncGPUReadbackRequest request)
 
             if (format == Format.jpeg)
             {
-                rawTextureData = texture2D.GetRawTextureData();
+                rawTextureData.Item1 = texture2D.GetRawTextureData();
+                rawTextureData.Item2 = frameTimestamp;
             }
 
-            newFrameAvailable = true;
+            newRawTextureAvailable = true;
         }
 
 
         private void JpegCompressor()
         {
             while (true)
             {
-                if (newFrameAvailable)
+                if (newRawTextureAvailable)
                 {
-                    data.data = compressor.Compress(rawTextureData, 0,
+                    data.data = compressor.Compress(rawTextureData.Item1, 0,
                         textureWidth,
                         textureHeight, TJPixelFormat.RGB, TJSubsamplingOption.Chrominance420,
                         (int)CompressionQuality,
                         TJFlags.FastDct | TJFlags.BottomUp);
+                    data.header.stamp = rawTextureData.Item2;
 
-                    newFrameAvailable = false;
+                    newRawTextureAvailable = false;
+
+                    newFrameAvailable = true;
 
                     UpdateFrameRate();
                 }

Runtime/Tx/RaycastLiDARTx.cs

@@ -37,7 +37,7 @@ public class RaycastLiDARTx : ProBridgeTxStamped<PointCloud2>
         NativeQueue<PointXYZI>.ParallelWriter tempQueueWriter;
         NativeArray<PointXYZI> tempPointsInput;
         private bool sensorReady = false;
-
+        private TimeSpan __dataTime;
 
         protected override void OnStart()
         {
@@ -114,6 +114,7 @@ private ScanPattern DownSampleScanPattern(ScanPattern scanPattern, float downSam
         private void OnSensorUpdated()
         {
             sensorReady = true;
+            __dataTime = ProBridgeServer.SimTime;
         }
 
         private void CalculateFieldsOffset()
@@ -174,7 +175,7 @@ protected override ProBridge.Msg GetMsg(TimeSpan ts)
             tempData.Dispose();
             tempPointsInput.Dispose();
 
-            return base.GetMsg(ts);
+            return base.GetMsg(__dataTime);
         }
 
 
@@ -236,4 +237,4 @@ private uint GetTypeSize(PointField field)
             return typeSize;
         }
     }
-}
+}