diff --git a/Runtime/Tx/CompressedImageTx.cs b/Runtime/Tx/CompressedImageTx.cs index 04cf1e2..6c8e1cc 100644 --- a/Runtime/Tx/CompressedImageTx.cs +++ b/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 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,10 +126,11 @@ private void OnCompleteReadback(AsyncGPUReadbackRequest request) if (format == Format.jpeg) { - rawTextureData = texture2D.GetRawTextureData(); + rawTextureData.Item1 = texture2D.GetRawTextureData(); + rawTextureData.Item2 = frameTimestamp; } - newFrameAvailable = true; + newRawTextureAvailable = true; } @@ -126,15 +138,18 @@ 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(); } diff --git a/Runtime/Tx/RaycastLiDARTx.cs b/Runtime/Tx/RaycastLiDARTx.cs index 366e56e..8985272 100644 --- a/Runtime/Tx/RaycastLiDARTx.cs +++ b/Runtime/Tx/RaycastLiDARTx.cs @@ -37,7 +37,7 @@ public class RaycastLiDARTx : ProBridgeTxStamped NativeQueue.ParallelWriter tempQueueWriter; NativeArray 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; } } -} \ No newline at end of file +}