From 7ef251510c359b0b7e81f50c3a750929dee3924f Mon Sep 17 00:00:00 2001 From: Sherif Nafee Date: Fri, 21 Mar 2025 14:17:32 +0300 Subject: [PATCH 1/5] Update Lidar stamp accuracy --- Runtime/Tx/RaycastLiDARTx.cs | 3 +++ 1 file changed, 3 insertions(+) diff --git a/Runtime/Tx/RaycastLiDARTx.cs b/Runtime/Tx/RaycastLiDARTx.cs index 366e56e..2c8414e 100644 --- a/Runtime/Tx/RaycastLiDARTx.cs +++ b/Runtime/Tx/RaycastLiDARTx.cs @@ -73,6 +73,8 @@ protected override void OnStart() CalculateFieldsOffset(); base.OnStart(); + + autoAddStamp = false; } private ScanPattern ReduceScanPatternAngle(ScanPattern scanPattern, float minAzimuth, float maxAzimuth) @@ -114,6 +116,7 @@ private ScanPattern DownSampleScanPattern(ScanPattern scanPattern, float downSam private void OnSensorUpdated() { sensorReady = true; + data.header.stamp = ProBridgeServer.SimTime; } private void CalculateFieldsOffset() From be69762f02d2e1c7ada54a7a086003b041391617 Mon Sep 17 00:00:00 2001 From: Sherif Nafee Date: Fri, 21 Mar 2025 14:17:53 +0300 Subject: [PATCH 2/5] Update camera stamp accuracy --- Runtime/Tx/CompressedImageTx.cs | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/Runtime/Tx/CompressedImageTx.cs b/Runtime/Tx/CompressedImageTx.cs index 04cf1e2..3b01ef2 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 { @@ -37,7 +38,7 @@ public enum Format private bool newFrameAvailable; private NativeArray imageData; - private byte[] rawTextureData; + private (byte[], Time) rawTextureData; private Thread jpegCompressionThread; private DateTime lastRenderTime; @@ -73,12 +74,16 @@ protected override void OnStart() if (format != Format.jpeg) return; jpegCompressionThread = new Thread(JpegCompressor); jpegCompressionThread.Start(); + + autoAddStamp = false; } + 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) @@ -89,17 +94,18 @@ protected override ProBridge.Msg GetMsg(TimeSpan ts) { // TODO: optimize png encoding data.data = texture2D.EncodeToPNG(); + data.header.stamp = ProBridgeServer.SimTime; newFrameAvailable = false; UpdateFrameRate(); } - data.data ??= new byte[] { 0, 0, 0}; + data.data ??= new byte[] { 0, 0, 0}; return base.GetMsg(ts); } - private void OnCompleteReadback(AsyncGPUReadbackRequest request) + private void OnCompleteReadback(AsyncGPUReadbackRequest request, Time frameTimestamp) { if (request.hasError) { @@ -115,7 +121,8 @@ private void OnCompleteReadback(AsyncGPUReadbackRequest request) if (format == Format.jpeg) { - rawTextureData = texture2D.GetRawTextureData(); + rawTextureData.Item1 = texture2D.GetRawTextureData(); + rawTextureData.Item2 = frameTimestamp; } newFrameAvailable = true; @@ -128,11 +135,12 @@ private void JpegCompressor() { if (newFrameAvailable) { - 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; From 5d79f1c7ca4d27763eed6a576afbeae84fef4c64 Mon Sep 17 00:00:00 2001 From: Sherif Nafee Date: Tue, 25 Mar 2025 10:15:20 +0300 Subject: [PATCH 3/5] Remove the need for the auto stamp bool --- Runtime/Tx/CompressedImageTx.cs | 4 +--- Runtime/Tx/RaycastLiDARTx.cs | 4 +--- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/Runtime/Tx/CompressedImageTx.cs b/Runtime/Tx/CompressedImageTx.cs index 3b01ef2..3db3a0e 100644 --- a/Runtime/Tx/CompressedImageTx.cs +++ b/Runtime/Tx/CompressedImageTx.cs @@ -74,8 +74,6 @@ protected override void OnStart() if (format != Format.jpeg) return; jpegCompressionThread = new Thread(JpegCompressor); jpegCompressionThread.Start(); - - autoAddStamp = false; } @@ -102,7 +100,7 @@ protected override ProBridge.Msg GetMsg(TimeSpan ts) data.data ??= new byte[] { 0, 0, 0}; - return base.GetMsg(ts); + return base.GetMsg(data.header.stamp); } private void OnCompleteReadback(AsyncGPUReadbackRequest request, Time frameTimestamp) diff --git a/Runtime/Tx/RaycastLiDARTx.cs b/Runtime/Tx/RaycastLiDARTx.cs index 2c8414e..6db1d74 100644 --- a/Runtime/Tx/RaycastLiDARTx.cs +++ b/Runtime/Tx/RaycastLiDARTx.cs @@ -73,8 +73,6 @@ protected override void OnStart() CalculateFieldsOffset(); base.OnStart(); - - autoAddStamp = false; } private ScanPattern ReduceScanPatternAngle(ScanPattern scanPattern, float minAzimuth, float maxAzimuth) @@ -177,7 +175,7 @@ protected override ProBridge.Msg GetMsg(TimeSpan ts) tempData.Dispose(); tempPointsInput.Dispose(); - return base.GetMsg(ts); + return base.GetMsg(data.header.stamp); } From 3ee1a53efbdbd7b0ee871cfe449605c9fc709b64 Mon Sep 17 00:00:00 2001 From: Sherif Nafee Date: Tue, 25 Mar 2025 10:34:38 +0300 Subject: [PATCH 4/5] Update frame waiting method --- Runtime/Tx/CompressedImageTx.cs | 29 +++++++++++++++++++---------- 1 file changed, 19 insertions(+), 10 deletions(-) diff --git a/Runtime/Tx/CompressedImageTx.cs b/Runtime/Tx/CompressedImageTx.cs index 3db3a0e..6c8e1cc 100644 --- a/Runtime/Tx/CompressedImageTx.cs +++ b/Runtime/Tx/CompressedImageTx.cs @@ -35,7 +35,8 @@ 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[], Time) rawTextureData; @@ -88,19 +89,25 @@ 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.data = texture2D.EncodeToPNG(); data.header.stamp = ProBridgeServer.SimTime; - newFrameAvailable = false; - + data.data = texture2D.EncodeToPNG(); + 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(data.header.stamp); + return null; } private void OnCompleteReadback(AsyncGPUReadbackRequest request, Time frameTimestamp) @@ -123,7 +130,7 @@ private void OnCompleteReadback(AsyncGPUReadbackRequest request, Time frameTimes rawTextureData.Item2 = frameTimestamp; } - newFrameAvailable = true; + newRawTextureAvailable = true; } @@ -131,7 +138,7 @@ private void JpegCompressor() { while (true) { - if (newFrameAvailable) + if (newRawTextureAvailable) { data.data = compressor.Compress(rawTextureData.Item1, 0, textureWidth, @@ -140,7 +147,9 @@ private void JpegCompressor() TJFlags.FastDct | TJFlags.BottomUp); data.header.stamp = rawTextureData.Item2; - newFrameAvailable = false; + newRawTextureAvailable = false; + + newFrameAvailable = true; UpdateFrameRate(); } From 464ac050c4d9bb4ab52fb51003aadb6cd6a69b95 Mon Sep 17 00:00:00 2001 From: prony5 Date: Tue, 25 Mar 2025 20:12:29 +0300 Subject: [PATCH 5/5] Update RaycastLiDARTx.cs --- Runtime/Tx/RaycastLiDARTx.cs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Runtime/Tx/RaycastLiDARTx.cs b/Runtime/Tx/RaycastLiDARTx.cs index 6db1d74..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,7 +114,7 @@ private ScanPattern DownSampleScanPattern(ScanPattern scanPattern, float downSam private void OnSensorUpdated() { sensorReady = true; - data.header.stamp = ProBridgeServer.SimTime; + __dataTime = ProBridgeServer.SimTime; } private void CalculateFieldsOffset() @@ -175,7 +175,7 @@ protected override ProBridge.Msg GetMsg(TimeSpan ts) tempData.Dispose(); tempPointsInput.Dispose(); - return base.GetMsg(data.header.stamp); + return base.GetMsg(__dataTime); } @@ -237,4 +237,4 @@ private uint GetTypeSize(PointField field) return typeSize; } } -} \ No newline at end of file +}