Merge pull request #12 from lge-ros2/develop

Merge 'develop' branch into 'master'

Assets/Scripts/CustomPlugins/CameraPlugin.cs → Assets/Scripts/DevicePlugins/CameraPlugin.cs

@@ -7,7 +7,7 @@
 using UnityEngine;
 using Stopwatch = System.Diagnostics.Stopwatch;
 
-public class CameraPlugin : CustomPlugin
+public class CameraPlugin : DevicePlugin
 {
 	public string partName = string.Empty;
 

Assets/Scripts/CustomPlugins/DepthCameraPlugin.cs → Assets/Scripts/DevicePlugins/DepthCameraPlugin.cs

@@ -7,7 +7,7 @@
 using UnityEngine;
 using Stopwatch = System.Diagnostics.Stopwatch;
 
-public class DepthCameraPlugin : CustomPlugin
+public class DepthCameraPlugin : DevicePlugin
 {
 	public string partName = string.Empty;
 

Assets/Scripts/CustomPlugins/CustomPlugin.cs → Assets/Scripts/DevicePlugins/DevicePlugin.cs

@@ -12,7 +12,7 @@
 using System;
 using UnityEngine;
 
-public abstract class CustomPlugin : DeviceTransporter
+public abstract class DevicePlugin : DeviceTransporter
 {
 	public string modelName = String.Empty;
 
@@ -22,7 +22,7 @@ public abstract class CustomPlugin : DeviceTransporter
 
 	private List<Thread> threadList = null;
 
-	protected CustomPlugin()
+	protected DevicePlugin()
 	{
 		threadList = new List<Thread>();
 	}

Assets/Scripts/CustomPlugins/ElevatorSystem.Elevator.cs → Assets/Scripts/DevicePlugins/ElevatorSystem.Elevator.cs

@@ -6,7 +6,7 @@
 
 using UnityEngine;
 
-public partial class ElevatorSystem : CustomPlugin
+public partial class ElevatorSystem : DevicePlugin
 {
 	private enum ElevatorState {STOP = 0, UPWARD, DOWNWARD};
 	private struct ElevatorEntity

Assets/Scripts/CustomPlugins/ElevatorSystem.cs → Assets/Scripts/DevicePlugins/ElevatorSystem.cs

@@ -13,7 +13,7 @@
 using Param = gazebo.msgs.Param;
 using Any = gazebo.msgs.Any;
 
-public partial class ElevatorSystem : CustomPlugin
+public partial class ElevatorSystem : DevicePlugin
 {
 	private enum ElevatorTaskState {DOOR_OPEN, DOOR_CLOSE, STANDBY, PROCESSING, DONE}
 

Assets/Scripts/CustomPlugins/GpsPlugin.cs → Assets/Scripts/DevicePlugins/GpsPlugin.cs

@@ -7,7 +7,7 @@
 using UnityEngine;
 using Stopwatch = System.Diagnostics.Stopwatch;
 
-public class GpsPlugin : CustomPlugin
+public class GpsPlugin : DevicePlugin
 {
 	public string partName = string.Empty;
 

Assets/Scripts/CustomPlugins/LaserPlugin.cs → Assets/Scripts/DevicePlugins/LaserPlugin.cs

@@ -7,7 +7,7 @@
 using UnityEngine;
 using Stopwatch = System.Diagnostics.Stopwatch;
 
-public class LaserPlugin : CustomPlugin
+public class LaserPlugin : DevicePlugin
 {
 	public string partName = string.Empty;
 

Assets/Scripts/CustomPlugins/MultiCameraPlugin.cs → Assets/Scripts/DevicePlugins/MultiCameraPlugin.cs

@@ -7,7 +7,7 @@
 using UnityEngine;
 using Stopwatch = System.Diagnostics.Stopwatch;
 
-public class MultiCameraPlugin : CustomPlugin
+public class MultiCameraPlugin : DevicePlugin
 {
 	public string partName = string.Empty;
 

Assets/Scripts/CustomPlugins/README.md → Assets/Scripts/DevicePlugins/README.md

@@ -1,4 +1,4 @@
-# CustomPlugins
+# DevicePlugins
 
 These plugin scripts are for sensor connection.
 

Assets/Scripts/CustomPlugins/RobotControl.cs → Assets/Scripts/DevicePlugins/RobotControl.cs

@@ -8,7 +8,7 @@
 using UnityEngine;
 using Stopwatch = System.Diagnostics.Stopwatch;
 
-public class RobotControl : CustomPlugin
+public class RobotControl : DevicePlugin
 {
 	private MicomInput micomInput = null;
 	private MicomSensor micomSensor = null;

Assets/Scripts/CustomPlugins/UnityRosInit.cs → Assets/Scripts/DevicePlugins/UnityRosInit.cs

@@ -4,11 +4,9 @@
  * SPDX-License-Identifier: MIT
  */
 
-using System.Threading;
-using System;
 using UnityEngine;
 
-public class UnityRosInit : CustomPlugin
+public class UnityRosInit : DevicePlugin
 {
 	private Clock clock = null;
 

Assets/Scripts/Devices/Camera.CameraData.cs

@@ -109,7 +109,6 @@ static public uint GetImageDepth(in string imageFormat)
 		private struct CamData
 		{
 			private Texture2D cameraImage;
-			private Rect pixelSource;
 
 			public int ImageWidth
 			{
@@ -154,19 +153,6 @@ public void AllocateTexture(in int width, in int height, in string imageFormat)
 				}
 
 				cameraImage = new Texture2D(width, height, textureFormat, false, isLinear);
-				pixelSource = new Rect(0, 0, width, height);
-			}
-
-			public void SetTextureData(in RenderTexture rt)
-			{
-				if (rt != null)
-				{
-					var currentRenderTexture = RenderTexture.active;
-					RenderTexture.active = rt;
-					cameraImage.ReadPixels(pixelSource, 0, 0);
-					cameraImage.Apply();
-					RenderTexture.active = currentRenderTexture;
-				}
 			}
 
 			public void SetTextureData(in NativeArray<byte> buffer)

Assets/Scripts/Devices/Camera.cs

@@ -6,6 +6,7 @@
 
 using System.Collections;
 using UnityEngine;
+using UnityEngine.Rendering;
 using Stopwatch = System.Diagnostics.Stopwatch;
 
 namespace SensorDevices
@@ -74,7 +75,7 @@ private void SetupCamera()
 			cam.nearClipPlane = (float)parameters.clip.near;
 			cam.farClipPlane = (float)parameters.clip.far;
 
-			var targetRTname = "CameraTexture";
+			string targetRTname;
 			var targetRTdepth = 0;
 			var targetRTformat = RenderTextureFormat.ARGB32;
 			var targetRTrwmode = RenderTextureReadWrite.sRGB;
@@ -86,8 +87,21 @@ private void SetupCamera()
 				targetRTformat = RenderTextureFormat.RFloat;
 				targetRTrwmode = RenderTextureReadWrite.Linear;
 			}
+			else
+			{
+				targetRTname = "CameraTexture";
+			}
+
+			var targetRT = new RenderTexture(parameters.image_width, parameters.image_height, targetRTdepth, targetRTformat, targetRTrwmode)
+			{
+				dimension = UnityEngine.Rendering.TextureDimension.Tex2D,
+				antiAliasing = 1,
+				useMipMap = false,
+				useDynamicScale = false,
+				wrapMode = TextureWrapMode.Clamp,
+				filterMode = FilterMode.Bilinear,
+			};
 
-			var targetRT = new RenderTexture(parameters.image_width, parameters.image_height, targetRTdepth, targetRTformat, targetRTrwmode);
 			targetRT.name = targetRTname;
 			cam.targetTexture = targetRT;
 
@@ -116,7 +130,17 @@ private IEnumerator CameraWorker()
 				cam.Render();
 				GL.invertCulling = oldCulling;
 
-				camData.SetTextureData(cam.targetTexture);
+				var readback = AsyncGPUReadback.Request(cam.targetTexture, 0, TextureFormat.RGB24);
+				yield return new WaitUntil(() => readback.done);
+
+				if (readback.hasError)
+				{
+					Debug.LogError("Failed to read GPU texture");
+					continue;
+				}
+				Debug.Assert(readback.done);
+
+				camData.SetTextureData(readback.GetData<byte>());
 
 				if (parameters.save_enabled)
 				{

Assets/Scripts/Devices/Lidar.LaserCameraData.cs

@@ -16,7 +16,6 @@ struct LaserCamData
 			private int index;
 			private float centerAngle;
 			private Texture2D cameraImage;
-			private Rect pixelSource;
 
 			public float CenterAngle
 			{
@@ -38,19 +37,6 @@ public void AllocateTexture(in int dataIndex, in int width, in int height)
 			{
 				index = dataIndex;
 				cameraImage = new Texture2D(width, height, TextureFormat.RGBA32, false, true);
-				pixelSource = new Rect(0, 0, width, height);
-			}
-
-			public void SetTextureData(in RenderTexture rt)
-			{
-				if (rt != null)
-				{
-					var currentRenderTexture = RenderTexture.active;
-					RenderTexture.active = rt;
-					cameraImage.ReadPixels(pixelSource, 0, 0);
-					cameraImage.Apply();
-					RenderTexture.active = currentRenderTexture;
-				}
 			}
 
 			public void SetTextureData(in NativeArray<byte> buffer)

Assets/Scripts/Devices/Lidar.cs

@@ -7,6 +7,7 @@
 using System.Collections;
 using System;
 using UnityEngine;
+using UnityEngine.Rendering;
 using Stopwatch = System.Diagnostics.Stopwatch;
 
 namespace SensorDevices
@@ -194,7 +195,17 @@ private IEnumerator LaserCameraWorker()
 					laserCamera.transform.localRotation = Quaternion.Euler(axisRotation);
 					laserCamera.Render();
 
-					data.SetTextureData(laserCamera.targetTexture);
+					var readback = AsyncGPUReadback.Request(laserCamera.targetTexture, 0, TextureFormat.RGBA32);
+					yield return new WaitUntil(() => readback.done);
+
+					if (readback.hasError)
+					{
+						Debug.LogError("Failed to read GPU texture");
+						continue;
+					}
+					Debug.Assert(readback.done);
+
+					data.SetTextureData(readback.GetData<byte>());
 
 					laserCamera.enabled = false;
 

Assets/Scripts/Devices/Sonar.cs

@@ -6,7 +6,6 @@
 
 using System.Collections;
 using System.Collections.Generic;
-using System;
 using UnityEngine;
 using Stopwatch = System.Diagnostics.Stopwatch;
 
@@ -27,6 +26,8 @@ public partial class Sonar : Device
 		[Range(0, 100)]
 		public double radius = 0;
 
+		public double detectedDistance = 0;
+
 		private List<Vector3> meshSensorRegionVertices = new List<Vector3>();
 
 		private float sensorTimeElapsed = 0.0f;
@@ -68,6 +69,16 @@ protected override void OnStart()
 			meshCollider.convex = true;
 			meshCollider.isTrigger = true;
 
+			ResolveSensingArea(meshCollider);
+
+			// const MeshColliderCookingOptions cookingOptions
+			// 	= MeshColliderCookingOptions.EnableMeshCleaning|MeshColliderCookingOptions.WeldColocatedVertices;
+			// meshCollider.cookingOptions = cookingOptions;
+			// meshCollider.hideFlags |= HideFlags.NotEditable;
+		}
+
+		private void ResolveSensingArea(in MeshCollider meshCollider)
+		{
 			// preserve the vertex points of the sensing area
 			var localToWorld = sonarLink.localToWorldMatrix;
 			for (var i = 0; i < meshCollider.sharedMesh.vertices.Length; i++)
@@ -81,11 +92,6 @@ protected override void OnStart()
 
 				meshSensorRegionVertices.Add(meshCollider.sharedMesh.vertices[i]);
 			}
-
-			// const MeshColliderCookingOptions cookingOptions
-			// 	= MeshColliderCookingOptions.EnableMeshCleaning|MeshColliderCookingOptions.WeldColocatedVertices;
-			// meshCollider.cookingOptions = cookingOptions;
-			// meshCollider.hideFlags |= HideFlags.NotEditable;
 		}
 
 		private void TranslateDetectionArea(Mesh mesh, in float offset)
@@ -94,6 +100,7 @@ private void TranslateDetectionArea(Mesh mesh, in float offset)
 			for (var i = 0; i < vertices.Length; i++)
 			{
 				vertices[i].y += offset;
+				vertices[i].y *= -1;
 			}
 			mesh.vertices = vertices;
 		}
@@ -157,33 +164,37 @@ void OnTriggerStay(Collider other)
 
 			var sonar = sonarStamped.Sonar;
 			var sensorStartPoint = sonarLink.position;
-			sensorStartPoint.z += sensorStartOffset;
+			sensorStartPoint.z -= sensorStartOffset;
 
 			var detectedRange = (float)rangeMax;
 			var contactPoint = Vector3.zero;
 			var contactDirection = Vector3.zero;
 			var localToWorld = sonarLink.localToWorldMatrix;
 
+			// Debug.Log("Hit Points: " + meshSensorRegionVertices.Count);
 			for (var i = 0; i < meshSensorRegionVertices.Count; i++)
 			{
 				var targetPoint = localToWorld.MultiplyPoint3x4(meshSensorRegionVertices[i]);
-				var direction = (targetPoint - sensorStartPoint).normalized;
+				var direction = (targetPoint - sensorStartPoint);
+
 				if (Physics.Raycast(sensorStartPoint, direction, out var hitInfo))
 				{
 					// Debug.DrawRay(sensorStartPoint, direction, Color.magenta, 0.01f);
 					// Debug.Log("Hit Point of contact: " + hitInfo.point);
 					var hitPoint = hitInfo.point;
 					var hitDistance = Vector3.Distance(sensorStartPoint, hitPoint);
+
 					if ((hitDistance < detectedRange) && (hitDistance > (float)rangeMin))
 					{
-						// Debug.Log("Hit Point of contact: " + hitInfo.point + "|" + distance.ToString("F4"));
+						// Debug.Log("Hit Point " + i + " of contact: " + hitInfo.point + "|" + hitDistance.ToString("F4"));
 						detectedRange = hitDistance;
 						contactDirection = direction;
 						contactPoint = hitPoint;
 					}
 				}
 			}
 
+			detectedDistance = detectedRange;
 			sonar.Range = detectedRange;
 			DeviceHelper.SetVector3d(sonar.Contact, contactPoint);
 			// Debug.Log(other.name + " |Stay| " + "," + detectedRange.ToString("F5") + ", " + contactPoint);
@@ -232,14 +243,14 @@ protected override IEnumerator OnVisualize()
 
 			while (true)
 			{
-				sensorStartPoint.Set(sonarLink.position.x, sonarLink.position.y, sonarLink.position.z + sensorStartOffset);
+				sensorStartPoint.Set(sonarLink.position.x, sonarLink.position.y, sonarLink.position.z - sensorStartOffset);
 
 				var direction = (GetDetectedPoint() - sensorStartPoint).normalized;
 				var detectedRange = GetDetectedRange();
 
 				if (detectedRange < rangeMax && !direction.Equals(Vector3.zero))
 				{
-					Debug.DrawRay(sensorStartPoint, direction * detectedRange, Color.magenta, visualDrawDuration);
+					Debug.DrawRay(sensorStartPoint, direction * detectedRange, Color.blue, visualDrawDuration);
 				}
 				yield return waitForSeconds;
 			}

Assets/Scripts/ModelLoader.cs

@@ -207,7 +207,7 @@ private IEnumerator ResetSimulation()
 			plugin.Reset();
 		}
 
-		foreach (var plugin in modelsRoot.GetComponentsInChildren<CustomPlugin>())
+		foreach (var plugin in modelsRoot.GetComponentsInChildren<DevicePlugin>())
 		{
 			plugin.Reset();
 		}