Merge from 'develop' into 'main' for CLOiSim-4.6.2 (#295)

lge-ros2 · Jul 5, 2024 · 5773f8a · 5773f8a
2 parents 3ad4c75 + 28cc53a
commit 5773f8a
Show file tree

Hide file tree

Showing 17 changed files with 184 additions and 167 deletions.
diff --git a/Assets/Scripts/Devices/Camera.cs b/Assets/Scripts/Devices/Camera.cs
@@ -93,6 +93,14 @@ protected override void OnStart()
 			}
 		}
 
+		protected override void OnReset()
+		{
+			lock (_readbackList)
+			{
+				_readbackList.Clear();
+			}
+		}
+
 		protected virtual void SetupTexture()
 		{
 			// Debug.Log("This is not a Depth Camera!");

diff --git a/Assets/Scripts/Devices/GPS.cs b/Assets/Scripts/Devices/GPS.cs
@@ -48,7 +48,7 @@ protected override void OnAwake()
 
 			_sphericalCoordinates = DeviceHelper.GetGlobalSphericalCoordinates();
 			_worldFrameOrientation = (Vector3.up * _sphericalCoordinates.HeadingAngle);
-			Debug.Log("worldFrameOrientation=" + _worldFrameOrientation.ToString("F3"));
+			// Debug.Log("worldFrameOrientation=" + _worldFrameOrientation.ToString("F3"));
 		}
 
 		protected override void OnStart()

diff --git a/Assets/Scripts/Devices/Lidar.cs b/Assets/Scripts/Devices/Lidar.cs
@@ -4,7 +4,9 @@
  * SPDX-License-Identifier: MIT
  */
 
+
 using System.Collections;
+using System.Collections.Generic;
 using System.Threading.Tasks;
 using System;
 using UnityEngine.Rendering.Universal;
@@ -51,7 +53,7 @@ public partial class Lidar : Device
 		private RTHandle _rtHandle = null;
 		private ParallelOptions _parallelOptions = null;
 
-		private AsyncLaserWork[] _asyncWorkList;
+		private List<AsyncLaserWork> _asyncWorkList = new List<AsyncLaserWork>();
 		private DepthData.CamBuffer[] _depthCamBuffers;
 		private LaserData.LaserCamData[] _laserCamData;
 		private LaserData.LaserDataOutput[] _laserDataOutput;
@@ -82,6 +84,14 @@ protected override void OnStart()
 			}
 		}
 
+		protected override void OnReset()
+		{
+			lock (_asyncWorkList)
+			{
+				_asyncWorkList.Clear();
+			}
+		}
+
 		protected new void OnDestroy()
 		{
 			_startLaserWork = false;
@@ -242,7 +252,6 @@ private void SetupLaserCameraData()
 			var isEven = (numberOfLaserCamData % 2 == 0) ? true : false;
 
 			_parallelOptions = new ParallelOptions { MaxDegreeOfParallelism = numberOfLaserCamData };
-			_asyncWorkList = new AsyncLaserWork[numberOfLaserCamData];
 			_depthCamBuffers = new DepthData.CamBuffer[numberOfLaserCamData];
 			_laserCamData = new LaserData.LaserCamData[numberOfLaserCamData];
 			_laserDataOutput = new LaserData.LaserDataOutput[numberOfLaserCamData];
@@ -305,8 +314,10 @@ private IEnumerator CaptureLaserCamera()
 						var capturedTime = (float)DeviceHelper.GetGlobalClock().SimTime;
 						var readbackRequest = AsyncGPUReadback.Request(laserCam.targetTexture, 0, GraphicsFormat.R8G8B8A8_UNorm, OnCompleteAsyncReadback);
 
-						if (_asyncWorkList.Length == numberOfLaserCamData)
-							_asyncWorkList[dataIndex] = new AsyncLaserWork(dataIndex, readbackRequest, capturedTime);
+						lock (_asyncWorkList)
+						{
+							_asyncWorkList.Add(new AsyncLaserWork(dataIndex, readbackRequest, capturedTime));
+						}
 
 						laserCam.enabled = false;
 						yield return null;
@@ -332,44 +343,48 @@ protected void OnCompleteAsyncReadback(AsyncGPUReadbackRequest request)
 			}
 			else if (request.done)
 			{
-				for (var i = 0; i < _asyncWorkList.Length; i++)
+				AsyncLaserWork asyncWork;
+
+				lock (_asyncWorkList)
 				{
-					var asyncWork = _asyncWorkList[i];
-					if (!asyncWork.request.Equals(request))
-						continue;
+					asyncWork = _asyncWorkList.Find(x => x.request.Equals(request));
+				}
 
-					var dataIndex = asyncWork.dataIndex;
-					var depthCamBuffer = _depthCamBuffers[dataIndex];
+				var dataIndex = asyncWork.dataIndex;
+				var depthCamBuffer = _depthCamBuffers[dataIndex];
 
-					depthCamBuffer.Allocate();
-					depthCamBuffer.raw = request.GetData<byte>();
+				depthCamBuffer.Allocate();
+				depthCamBuffer.raw = request.GetData<byte>();
 
-					if (depthCamBuffer.depth.IsCreated)
-					{
-						var jobHandleDepthCamBuffer = depthCamBuffer.Schedule(depthCamBuffer.Length(), BatchSize);
-						jobHandleDepthCamBuffer.Complete();
-
-						var laserCamData = _laserCamData[dataIndex];
-						laserCamData.depthBuffer = depthCamBuffer.depth;
-						laserCamData.Allocate();
+				if (depthCamBuffer.depth.IsCreated)
+				{
+					var jobHandleDepthCamBuffer = depthCamBuffer.Schedule(depthCamBuffer.Length(), BatchSize);
+					jobHandleDepthCamBuffer.Complete();
 
-						var jobHandle = laserCamData.Schedule(laserCamData.OutputLength(), BatchSize);
-						jobHandle.Complete();
+					var laserCamData = _laserCamData[dataIndex];
+					laserCamData.depthBuffer = depthCamBuffer.depth;
+					laserCamData.Allocate();
 
-						_laserDataOutput[dataIndex].data = laserCamData.GetLaserData();
-						_laserDataOutput[dataIndex].capturedTime = asyncWork.capturedTime;
-						_laserDataOutput[dataIndex].processingTime = (float)DeviceHelper.GlobalClock.SimTime - asyncWork.capturedTime;
+					var jobHandle = laserCamData.Schedule(laserCamData.OutputLength(), BatchSize);
+					jobHandle.Complete();
 
-						if (noise != null)
-						{
-							noise.Apply<double>(ref _laserDataOutput[dataIndex].data);
-						}
+					_laserDataOutput[dataIndex].data = laserCamData.GetLaserData();
+					_laserDataOutput[dataIndex].capturedTime = asyncWork.capturedTime;
+					_laserDataOutput[dataIndex].processingTime = (float)DeviceHelper.GlobalClock.SimTime - asyncWork.capturedTime;
 
-						laserCamData.Deallocate();
+					if (noise != null)
+					{
+						noise.Apply<double>(ref _laserDataOutput[dataIndex].data);
 					}
 
-					depthCamBuffer.Deallocate();
-					break;
+					laserCamData.Deallocate();
+				}
+
+				depthCamBuffer.Deallocate();
+
+				lock (_asyncWorkList)
+				{
+					_asyncWorkList.Remove(asyncWork);
 				}
 			}
 		}

diff --git a/Assets/Scripts/Devices/Modules/LaserData.cs b/Assets/Scripts/Devices/Modules/LaserData.cs
@@ -229,5 +229,4 @@ public AsyncLaserWork(in int dataIndex, in AsyncGPUReadbackRequest request, in f
 			this.capturedTime = capturedTime;
 		}
 	}
-
 }
-Original file line number
+Diff line change
@@ Expand Up @@
     			this.capturedTime = capturedTime;
     		}
     	}
     }