Merge pull request #26 from lge-ros2/develop

Merge 'develop' branch into 'master'

Assets/Scripts/Devices/Camera.CameraData.cs

@@ -122,18 +122,18 @@ static public int GetImageDepth(in string imageFormat)
 			return depth;
 		}
 
-		private struct CamData
+		private struct CamImageData
 		{
-			private byte[] imageBuffer;
+			private NativeArray<byte> imageBuffer;
 
 			private Texture2D cameraImage;
 
 			public void AllocateTexture(in int width, in int height, in string imageFormat)
 			{
 				var isLinear = false;
+				var textureFormat = TextureFormat.RGB24;
 
 				var format = GetPixelFormat(imageFormat);
-				var textureFormat = TextureFormat.RGB24;
 				switch (format)
 				{
 					case PixelFormat.L_INT8:
@@ -156,38 +156,36 @@ public void AllocateTexture(in int width, in int height, in string imageFormat)
 
 					case PixelFormat.RGB_INT8:
 					default:
-						textureFormat = TextureFormat.RGB24;
 						break;
 				}
+
 				cameraImage = new Texture2D(width, height, textureFormat, false, isLinear);
 			}
 
-			public void SetTextureData(in NativeArray<byte> buffer)
+			public void SetTextureBufferData(in NativeArray<byte> buffer)
+			{
+				imageBuffer = buffer;
+			}
+
+			public int GetImageDataLength()
 			{
-				imageBuffer = buffer.ToArray();
+				return imageBuffer.Length;
 			}
 
-			public byte[] GetTextureData()
+			public byte[] GetImageData()
 			{
-				return imageBuffer;
+				return imageBuffer.ToArray();
 			}
 
 			public void SaveRawImageData(in string path, in string name)
 			{
-				cameraImage.LoadImage(imageBuffer);
+				cameraImage.SetPixelData(imageBuffer, 0);
 				cameraImage.Apply();
-
 				var bytes = cameraImage.EncodeToPNG();
-				var fileName = string.Format("{0}/{1}", path, name);
+				var fileName = string.Format("{0}/{1}.png", path, name);
 				System.IO.File.WriteAllBytes(fileName, bytes);
 			}
 		}
 
-		private CamData camData;
-
-		public byte[] GetCamImageData()
-		{
-			return camData.GetTextureData();
-		}
 	}
 }

Assets/Scripts/Devices/Camera.cs

@@ -35,6 +35,7 @@ public partial class Camera : Device
 		protected RenderTextureFormat targetRTformat;
 		protected RenderTextureReadWrite targetRTrwmode;
 		protected TextureFormat readbackDstFormat;
+		private CamImageData camData;
 
 		void OnPreRender()
 		{
@@ -173,12 +174,12 @@ private void SetupCamera()
 
 		private IEnumerator CameraWorker()
 		{
-			var waitForSeconds = new WaitForSeconds(UpdatePeriod * adjustCapturingRate);
+			var image = imageStamped.Image;
+			var waitForSeconds = new WaitForSeconds(WaitPeriod());
 
 			while (true)
 			{
 				cam.enabled = true;
-
 				cam.Render();
 
 				var readback = AsyncGPUReadback.Request(cam.targetTexture, 0, readbackDstFormat);
@@ -197,13 +198,19 @@ private IEnumerator CameraWorker()
 
 				if (readback.done)
 				{
-					camData.SetTextureData(readback.GetData<byte>());
+					camData.SetTextureBufferData(readback.GetData<byte>());
 
-					if (parameters.save_enabled)
+					if (image.Data.Length == camData.GetImageDataLength())
 					{
-						var saveName = name + "_" + Time.time;
-						camData.SaveRawImageData(parameters.save_path, saveName);
-						// Debug.LogFormat("{0}|{1} captured", parameters.save_path, saveName);
+						// Debug.Log(imageStamped.Image.Height + "," + imageStamped.Image.Width);
+						image.Data = camData.GetImageData();
+
+						if (parameters.save_enabled)
+						{
+							var saveName = name + "_" + Time.time;
+							camData.SaveRawImageData(parameters.save_path, saveName);
+							// Debug.LogFormat("{0}|{1} captured", parameters.save_path, saveName);
+						}
 					}
 				}
 
@@ -226,15 +233,6 @@ protected override IEnumerator MainDeviceWorker()
 
 		protected override void GenerateMessage()
 		{
-			var image = imageStamped.Image;
-			var imageData = camData.GetTextureData();
-
-			if (imageData != null && image.Data.Length == imageData.Length)
-			{
-				image.Data = imageData;
-			}
-			// Debug.Log(imageStamped.Image.Height + "," + imageStamped.Image.Width);
-
 			DeviceHelper.SetCurrentTime(imageStamped.Time);
 			PushData<messages.ImageStamped>(imageStamped);
 		}
@@ -243,5 +241,10 @@ public messages.CameraSensor GetCameraInfo()
 		{
 			return sensorInfo;
 		}
+
+		public messages.Image GetImageMessage()
+		{
+			return (imageStamped == null || imageStamped.Image == null)? null:imageStamped.Image;
+		}
 	}
 }

Assets/Scripts/Devices/Contact.cs

@@ -64,13 +64,7 @@ protected override void OnStart()
 
 		protected override IEnumerator OnVisualize()
 		{
-			var waitForEndOfFrame = new WaitForEndOfFrame();
-			var waitForSeconds = new WaitForSeconds(UpdatePeriod);
-
-			while (true)
-			{
-				yield return waitForSeconds;
-			}
+			yield return null;
 		}
 
 		protected override void InitializeMessages()

Assets/Scripts/Devices/Device.cs

@@ -12,12 +12,13 @@
 
 public abstract class Device : MonoBehaviour
 {
+	private const int maxQueue = 3;
+
 	public string deviceName = string.Empty;
 
-	private const int maxQueue = 3;
-	private BlockingCollection<MemoryStream> memoryStreamOutboundQueue;
+	private BlockingCollection<MemoryStream> memoryStreamOutboundQueue = new BlockingCollection<MemoryStream>(maxQueue);
 
-	private MemoryStream memoryStream = null;
+	private MemoryStream memoryStream = new MemoryStream();
 
 	protected const float SEC2MSEC = 1000.0f;
 
@@ -29,7 +30,8 @@ public abstract class Device : MonoBehaviour
 
 	private float transportingTimeSeconds = 0;
 
-	public float adjustCapturingRate = 0.85f;
+	[Range(0, 1.0f)]
+	public float waitingPeriodRatio = 1.0f;
 
 	void OnDestroy()
 	{
@@ -59,8 +61,6 @@ public bool EnableVisualize
 
 	void Awake()
 	{
-		memoryStreamOutboundQueue = new BlockingCollection<MemoryStream>(maxQueue);
-		memoryStream = new MemoryStream();
 		ResetDataStream();
 
 		OnAwake();
@@ -94,10 +94,10 @@ void Start()
 
 	protected float WaitPeriod(in float messageGenerationTime = 0)
 	{
-		var waitTime = UpdatePeriod - messageGenerationTime - TransportingTime;
+		var waitTime = (UpdatePeriod * waitingPeriodRatio) - messageGenerationTime - TransportingTime;
 		// Debug.LogFormat(deviceName + ": waitTime({0}) = period({1}) - elapsedTime({2}) - TransportingTime({3})",
 		// 	waitTime.ToString("F5"), UpdatePeriod.ToString("F5"), messageGenerationTime.ToString("F5"), TransportingTime.ToString("F5"));
-		return (waitTime < 0)? 0:waitTime;
+		return (waitTime < 0) ? 0 : waitTime;
 	}
 
 	public void SetUpdateRate(in float value)

Assets/Scripts/Devices/Lidar.LaserCameraData.cs

@@ -22,7 +22,8 @@ struct LaserCamData
 
 			private int imageWidth;
 			private int imageHeight;
-			private byte[] imageBuffer;
+
+			private NativeArray<byte> imageBuffer;
 
 			public double[] output;
 
@@ -33,17 +34,12 @@ public void AllocateBuffer(in int width, in int height)
 			{
 				imageWidth = width;
 				imageHeight = height;
-				imageBuffer = new byte[width * height * colorFormatUnitSize];
-
 				output = new double[imageWidth];
 			}
 
 			public void SetBufferData(in NativeArray<byte> buffer)
 			{
-				if (imageBuffer != null)
-				{
-					buffer.CopyTo(imageBuffer);
-				}
+				imageBuffer = buffer;
 			}
 
 			public void ResolveLaserRanges(in float rangeMax = 1.0f)

Assets/Scripts/Devices/Lidar.cs

@@ -195,6 +195,8 @@ private void SetupLaserCameraData()
 			laserStartAngle = defaultRotationOffset + (float)angleMin;
 			laserEndAngle = defaultRotationOffset + (float)angleMax;
 			laserTotalAngle = (float)(angleMax - angleMin);
+
+			waitingPeriodRatio = 0.85f;
 		}
 
 		private IEnumerator LaserCameraWorker()
@@ -240,20 +242,14 @@ private IEnumerator LaserCameraWorker()
 					{
 						var data = laserCamData[dataIndex];
 						data.SetBufferData(readback.GetData<byte>());
+						data.ResolveLaserRanges((float)rangeMax);
 					}
 					else
 					{
 						Debug.LogWarning("AsyncGPUReadBackback Request was failed, dataIndex: " + dataIndex);
 					}
 				}
 
-				for (var dataIndex = 0; dataIndex < numberOfLaserCamData; dataIndex++)
-				{
-					yield return null;
-					var data = laserCamData[dataIndex];
-					data.ResolveLaserRanges((float)rangeMax);
-				}
-
 				sw.Stop();
 
 				yield return new WaitForSeconds(WaitPeriod((float)sw.Elapsed.TotalSeconds));
@@ -268,7 +264,6 @@ protected override IEnumerator MainDeviceWorker()
 				sw.Restart();
 				GenerateMessage();
 				sw.Stop();
-
 				yield return new WaitForSeconds(WaitPeriod((float)sw.Elapsed.TotalSeconds));
 			}
 		}
@@ -359,7 +354,7 @@ protected override IEnumerator OnVisualize()
 					var rayRotation = Quaternion.AngleAxis((float)(rayAngleH), lidarLink.up) * lidarLink.forward;
 					var rayStart = (rayRotation * (float)rangeMin) + lidarSensorWorldPosition;
 
-					var ccwIndex = (uint)rangeData.Length - hScanIndex - 1;
+					var ccwIndex = (uint)(rangeData.Length - hScanIndex - 1);
 					var rayData = rangeData[ccwIndex];
 
 					var rayDistance = (rayData == Mathf.Infinity) ? (float)rangeMax : (rayData - (float)rangeMin);

Assets/Scripts/Devices/MultiCamera.cs

@@ -31,7 +31,6 @@ protected override void OnStart()
 			foreach (var camParameters in parameters.list)
 			{
 				AddCamera(camParameters);
-				InitializeCamMessages(camParameters);
 			}
 		}
 
@@ -59,19 +58,25 @@ protected override void InitializeMessages()
 			imagesStamped.Time = new messages.Time();
 		}
 
-		protected override void GenerateMessage()
+		private void InitializeCamerasMessage()
 		{
-			var images = imagesStamped.Images;
-
-			for (int index = 0; index < images.Count; index++)
+			foreach (var cam in cameras)
 			{
-				var image = images[index];
-				var imageData = cameras[index].GetCamImageData();
-				if (imageData != null && image.Data.Length == imageData.Length)
+				var image = cam.GetImageMessage();
+				if (image == null)
 				{
-					image.Data = imageData;
+					break;
 				}
-				// Debug.Log(imagesStamped.Image.Height + "," + imagesStamped.Image.Width);
+
+				imagesStamped.Images.Add(image);
+			}
+		}
+
+		protected override void GenerateMessage()
+		{
+			if (imagesStamped.Images.Count != cameras.Count)
+			{
+				InitializeCamerasMessage();
 			}
 
 			DeviceHelper.SetCurrentTime(imagesStamped.Time);
@@ -94,18 +99,8 @@ private void AddCamera(in SDF.Camera parameters)
 			newCam.runningDeviceWork = false;
 			newCam.deviceName = "MultiCamera::" + parameters.name;
 			newCam.parameters = parameters;
-			cameras.Add(newCam);
-		}
 
-		private void InitializeCamMessages(in SDF.Camera parameters)
-		{
-			var image = new messages.Image();
-			image.Width = (uint)parameters.image_width;
-			image.Height = (uint)parameters.image_height;
-			image.PixelFormat = (uint)Camera.GetPixelFormat(parameters.image_format);
-			image.Step = image.Width * (uint)Camera.GetImageDepth(parameters.image_format);
-			image.Data = new byte[image.Height * image.Step];
-			imagesStamped.Images.Add(image);
+			cameras.Add(newCam);
 		}
 
 		public messages.CameraSensor GetCameraInfo(in string cameraName)

Assets/Scripts/Devices/Sonar.cs

@@ -41,7 +41,13 @@ protected override void OnAwake()
 		{
 			deviceName = name;
 			sonarLink = transform.parent;
-			adjustCapturingRate = 0.95f;
+			waitingPeriodRatio = 0.95f;
+
+			var sonar = sonarStamped.Sonar;
+			sonar.Frame = deviceName;
+			sonar.Radius = radius;
+			sonar.RangeMin = rangeMin;
+			sonar.RangeMax = rangeMax;
 		}
 
 		protected override void OnStart()
@@ -109,12 +115,6 @@ protected override void InitializeMessages()
 			sonarStamped.Sonar.WorldPose.Position = new messages.Vector3d();
 			sonarStamped.Sonar.WorldPose.Orientation = new messages.Quaternion();
 			sonarStamped.Sonar.Contact = new messages.Vector3d();
-
-			var sonar = sonarStamped.Sonar;
-			sonar.Frame = deviceName;
-			sonar.Radius = radius;
-			sonar.RangeMin = rangeMin;
-			sonar.RangeMax = rangeMax;
 		}
 
 		protected override IEnumerator MainDeviceWorker()

Assets/Scripts/Tools/SDF/Joint.cs

@@ -146,6 +146,10 @@ protected override void ParseElements()
 					}
 					break;
 
+				case "fixed":
+					// Console.WriteLine("[{0}] P:{1} C:{2}", Type, parent, child);
+					break;
+
 				default:
 					Console.WriteLine("Invalid Type [{0}] P:{1} C:{2}", Type, parent, child);
 					break;