Merge pull request #17 from lge-ros2/develop

Merge 'develop' branch into 'master'
lge-ros2 · Jul 26, 2020 · 9fa2638 · 9fa2638
2 parents ef7a420 + 68926e5
commit 9fa2638
Show file tree

Hide file tree

Showing 6 changed files with 74 additions and 56 deletions.
diff --git a/Assets/Scripts/DevicePlugins/MultiCameraPlugin.cs b/Assets/Scripts/DevicePlugins/MultiCameraPlugin.cs
@@ -45,7 +45,7 @@ void OnDestroy()
 
 	private void Sender()
 	{
-		Stopwatch sw = new Stopwatch();
+		var sw = new Stopwatch();
 		while (true)
 		{
 			if (cam == null)

diff --git a/Assets/Scripts/Devices/Camera.cs b/Assets/Scripts/Devices/Camera.cs
@@ -28,8 +28,6 @@ public partial class Camera : Device
 
 		protected UnityEngine.Camera cam = null;
 
-		public float adjustCapturingRate = 0.95f;
-
 		public bool runningDeviceWork = true;
 
 		protected string targetRTname;
@@ -136,6 +134,8 @@ private void SetupCamera()
 
 			cam.allowHDR = true;
 			cam.allowMSAA = false;
+			cam.allowDynamicResolution = true;
+			cam.useOcclusionCulling = true;
 			cam.targetDisplay = 0;
 			cam.stereoTargetEye = StereoTargetEyeMask.None;
 
@@ -182,10 +182,12 @@ private IEnumerator CameraWorker()
 				cam.Render();
 
 				var readback = AsyncGPUReadback.Request(cam.targetTexture, 0, readbackDstFormat);
+				cam.enabled = false;
 
-				yield return new WaitUntil(() => readback.done);
+				yield return null;
+
+				readback.WaitForCompletion();
 
-				cam.enabled = false;
 
 				if (readback.hasError)
 				{
@@ -194,13 +196,16 @@ private IEnumerator CameraWorker()
 				}
 				// Debug.Assert(request.done);
 
-				camData.SetTextureData(readback.GetData<byte>());
-
-				if (parameters.save_enabled)
+				if (readback.done)
 				{
-					var saveName = name + "_" + Time.time;
-					camData.SaveRawImageData(parameters.save_path, saveName);
-					// Debug.LogFormat("{0}|{1} captured", parameters.save_path, saveName);
+					camData.SetTextureData(readback.GetData<byte>());
+
+					if (parameters.save_enabled)
+					{
+						var saveName = name + "_" + Time.time;
+						camData.SaveRawImageData(parameters.save_path, saveName);
+						// Debug.LogFormat("{0}|{1} captured", parameters.save_path, saveName);
+					}
 				}
 
 				yield return waitForSeconds;

diff --git a/Assets/Scripts/Devices/Device.cs b/Assets/Scripts/Devices/Device.cs
@@ -29,6 +29,8 @@ public abstract class Device : MonoBehaviour
 
 	private float transportingTimeSeconds = 0;
 
+	public float adjustCapturingRate = 0.85f;
+
 	void OnDestroy()
 	{
 		if (memoryStreamOutboundQueue.IsCompleted)
@@ -41,8 +43,6 @@ void OnDestroy()
 
 	public float UpdatePeriod => 1f/updateRate;
 
-	public int UpdatePeriodInMillisecond => (int)(UpdatePeriod * SEC2MSEC);
-
 	public float TransportingTime => transportingTimeSeconds;
 
 	public bool EnableDebugging
@@ -211,18 +211,12 @@ protected bool PushData()
 
 	public MemoryStream PopData()
 	{
-		if (memoryStreamOutboundQueue == null)
+		if (memoryStreamOutboundQueue == null || memoryStreamOutboundQueue.Count == 0)
 		{
 			return null;
 		}
 
-		if (memoryStreamOutboundQueue.TryTake(out var item, UpdatePeriodInMillisecond))
-		{
-			return item;
-		}
-		else
-		{
-			return null;
-		}
+		var item = memoryStreamOutboundQueue.Take();
+		return item;
 	}
 }
diff --git a/Assets/Scripts/Devices/Lidar.cs b/Assets/Scripts/Devices/Lidar.cs
@@ -45,7 +45,7 @@ public partial class Lidar : Device
 		public Color rayColor = new Color(1, 0.1f, 0.1f, 0.2f);
 
 		private Transform lidarLink = null;
-		private UnityEngine.Camera laserCamera = null;
+		private UnityEngine.Camera laserCam = null;
 		private Material depthMaterial = null;
 
 		private const float defaultRotationOffset = 90.00000000000000f;
@@ -60,8 +60,6 @@ public partial class Lidar : Device
 
 		private LaserCamData[] laserCamData;
 
-		public float adjustWaitingPeriod = 0.80f;
-
 		void OnRenderImage(RenderTexture source, RenderTexture destination)
 		{
 			if (depthMaterial)
@@ -86,9 +84,9 @@ protected override void OnAwake()
 
 		protected override void OnStart()
 		{
-			laserCamera = gameObject.AddComponent<UnityEngine.Camera>();
+			laserCam = gameObject.AddComponent<UnityEngine.Camera>();
 
-			if (laserCamera)
+			if (laserCam)
 			{
 				SetupLaserCamera();
 
@@ -135,22 +133,24 @@ private void SetupLaserCamera()
 			var shader = Shader.Find("Sensor/Depth");
 			depthMaterial = new Material(shader);
 
-			laserCamera.backgroundColor = Color.white;
-			laserCamera.clearFlags = CameraClearFlags.SolidColor;
-			laserCamera.depthTextureMode = DepthTextureMode.Depth;
-			laserCamera.cullingMask = LayerMask.GetMask("Default");
+			laserCam.backgroundColor = Color.white;
+			laserCam.clearFlags = CameraClearFlags.SolidColor;
+			laserCam.depthTextureMode = DepthTextureMode.Depth;
+			laserCam.cullingMask = LayerMask.GetMask("Default");
 
-			laserCamera.allowHDR = true;
-			laserCamera.allowMSAA = false;
-			laserCamera.renderingPath = RenderingPath.DeferredLighting;
-			laserCamera.stereoTargetEye = StereoTargetEyeMask.None;
+			laserCam.allowHDR = true;
+			laserCam.allowMSAA = false;
+			laserCam.allowDynamicResolution = true;
+			laserCam.useOcclusionCulling = true;
+			laserCam.renderingPath = RenderingPath.DeferredLighting;
+			laserCam.stereoTargetEye = StereoTargetEyeMask.None;
 
-			laserCamera.orthographic = false;
-			laserCamera.nearClipPlane = (float)rangeMin;
-			laserCamera.farClipPlane = (float)rangeMax;
+			laserCam.orthographic = false;
+			laserCam.nearClipPlane = (float)rangeMin;
+			laserCam.farClipPlane = (float)rangeMax;
 
 			var projMatrix = DeviceHelper.MakeCustomProjectionMatrix(laserCameraHFov, laserCameraVFov, (float)rangeMin, (float)rangeMax);
-			laserCamera.projectionMatrix = projMatrix;
+			laserCam.projectionMatrix = projMatrix;
 
 			var renderTextrueWidth = Mathf.CeilToInt(laserCameraHFov / laserHAngleResolution);
 			var aspectRatio = Mathf.Tan(laserCameraVFov / 2 * Mathf.Deg2Rad) / Mathf.Tan(laserCameraHFov / 2 * Mathf.Deg2Rad);
@@ -160,11 +160,11 @@ private void SetupLaserCamera()
 				name = "LidarDepthTexture"
 			};
 
-			laserCamera.targetTexture = targetDepthRT;
+			laserCam.targetTexture = targetDepthRT;
 
-			laserCamera.enabled = false;
+			laserCam.enabled = false;
 
-			// laserCamera.hideFlags |= HideFlags.NotEditable;
+			// laserCam.hideFlags |= HideFlags.NotEditable;
 		}
 
 		private void SetupLaserCameraData()
@@ -174,7 +174,7 @@ private void SetupLaserCameraData()
 
 			laserCamData = new LaserCamData[numberOfLaserCamData];
 
-			var targetDepthRT = laserCamera.targetTexture;
+			var targetDepthRT = laserCam.targetTexture;
 			for (var index = 0; index < numberOfLaserCamData; index++)
 			{
 				var data = new LaserCamData();
@@ -187,7 +187,8 @@ private void SetupLaserCameraData()
 		private IEnumerator LaserCameraWorker()
 		{
 			var axisRotation = Vector3.zero;
-			var waitForSeconds = new WaitForSeconds(UpdatePeriod * adjustWaitingPeriod);
+			var waitForSeconds = new WaitForSeconds(UpdatePeriod * adjustCapturingRate);
+			var readbacks = new AsyncGPUReadbackRequest[numberOfLaserCamData];
 
 			while (true)
 			{
@@ -196,26 +197,40 @@ private IEnumerator LaserCameraWorker()
 					var data = laserCamData[dataIndex];
 					axisRotation.y = data.CenterAngle;
 
-					laserCamera.transform.localRotation = Quaternion.Euler(axisRotation);
+					laserCam.transform.localRotation = Quaternion.Euler(axisRotation);
+
+					laserCam.enabled = true;
 
-					laserCamera.enabled = true;
+					laserCam.Render();
 
-					laserCamera.Render();
+					readbacks[dataIndex] = AsyncGPUReadback.Request(laserCam.targetTexture, 0, TextureFormat.RGBA32);
 
-					var readback = AsyncGPUReadback.Request(laserCamera.targetTexture, 0, TextureFormat.RGBA32);
+					laserCam.enabled = false;
+				}
 
-					yield return new WaitUntil(() => readback.done);
+				yield return null;
 
-					laserCamera.enabled = false;
+				for (var dataIndex = 0; dataIndex < numberOfLaserCamData; dataIndex++)
+				{
+					var readback = readbacks[dataIndex];
+					readback.WaitForCompletion();
 
 					if (readback.hasError)
 					{
-						Debug.LogError("Failed to read GPU texture");
+						Debug.LogError("Failed to read GPU texture, dataIndex: " + dataIndex);
 						continue;
 					}
 					// Debug.Assert(readback.done);
 
-					data.SetBufferData(readback.GetData<byte>());
+					if (readback.done)
+					{
+						var data = laserCamData[dataIndex];
+						data.SetBufferData(readback.GetData<byte>());
+					}
+					else
+					{
+						Debug.LogWarning("AsyncGPUReadBackback Request was failed, dataIndex: " + dataIndex);
+					}
 				}
 
 				yield return waitForSeconds;
@@ -224,7 +239,7 @@ private IEnumerator LaserCameraWorker()
 
 		protected override IEnumerator MainDeviceWorker()
 		{
-			var waitForSeconds = new WaitForSeconds(UpdatePeriod * adjustWaitingPeriod);
+			var waitForSeconds = new WaitForSeconds(UpdatePeriod * adjustCapturingRate);
 			var sw = new Stopwatch();
 			while (true)
 			{

diff --git a/Assets/Scripts/Tools/SDF/Root.cs b/Assets/Scripts/Tools/SDF/Root.cs
@@ -292,9 +292,13 @@ private XmlNode GetIncludedModel(XmlNode _node)
 			{
 				uri = value.Item1 + "/" + value.Item2;
 			}
+			else
+			{
+				Console.WriteLine("Not exists in database: " + uri);
+				return null;
+			}
 
 			var modelSdfDoc = new XmlDocument();
-
 			try
 			{
 				modelSdfDoc.Load(uri);

diff --git a/ProjectSettings/ProjectVersion.txt b/ProjectSettings/ProjectVersion.txt
@@ -1,2 +1,2 @@
-m_EditorVersion: 2019.4.4f1
-m_EditorVersionWithRevision: 2019.4.4f1 (1f1dac67805b)
+m_EditorVersion: 2019.4.5f1
+m_EditorVersionWithRevision: 2019.4.5f1 (81610f64359c)