Merge pull request #16 from lge-ros2/develop

Merge 'develop' branch into 'master'

Assets/Scripts/DevicePlugins/CameraPlugin.cs

@@ -5,7 +5,6 @@
  */
 
 using System.IO;
-using System.Text;
 using UnityEngine;
 using Stopwatch = System.Diagnostics.Stopwatch;
 using ProtoBuf;
@@ -43,6 +42,11 @@ protected override void OnStart()
 		AddThread(Sender);
 	}
 
+	void OnDestroy()
+	{
+		memoryStreamForCameraInfo.Dispose();
+	}
+
 	private void Sender()
 	{
 		Stopwatch sw = new Stopwatch();
@@ -67,19 +71,37 @@ private void Response()
 		{
 			var receivedBuffer = ReceiveRequest();
 
-			var receivedString = Encoding.UTF8.GetString(receivedBuffer, 0, receivedBuffer.Length);
+			var requestMessage = ParsingCameraInfoRequest(ref memoryStreamForCameraInfo, receivedBuffer);
 
 			// Debug.Log(subPartName + receivedString);
-			if (receivedBuffer != null && receivedString.Equals("request_camera_info"))
+			if (requestMessage != null && requestMessage.Name.Equals("request_camera_info"))
 			{
 				var cameraInfoMessage = cam.GetCameraInfo();
 
-				ClearMemoryStream(ref memoryStreamForCameraInfo);
-
-				Serializer.Serialize<messages.CameraSensor>(memoryStreamForCameraInfo, cameraInfoMessage);
+				SetCameraInfoResponse(ref memoryStreamForCameraInfo, cameraInfoMessage);
 
 				SendResponse(memoryStreamForCameraInfo);
 			}
 		}
 	}
+
+	public static messages.Param ParsingCameraInfoRequest(ref MemoryStream msCameraInfo, in byte[] receivedBuffer)
+	{
+		ClearMemoryStream(ref msCameraInfo);
+
+		msCameraInfo.Write(receivedBuffer, 0, receivedBuffer.Length);
+		msCameraInfo.Position = 0;
+
+		return Serializer.Deserialize<messages.Param>(msCameraInfo);
+	}
+
+	public static void SetCameraInfoResponse(ref MemoryStream msCameraInfo,in messages.CameraSensor sensorInfo)
+	{
+		if (sensorInfo != null)
+		{
+			ClearMemoryStream(ref msCameraInfo);
+
+			Serializer.Serialize<messages.CameraSensor>(msCameraInfo, sensorInfo);
+		}
+	}
 }

Assets/Scripts/DevicePlugins/DevicePlugin.cs

@@ -197,7 +197,7 @@ public void Reset()
 		OnReset();
 	}
 
-	protected void ClearMemoryStream(ref MemoryStream ms)
+	protected static void ClearMemoryStream(ref MemoryStream ms)
 	{
 		if (ms != null)
 		{

Assets/Scripts/DevicePlugins/MultiCameraPlugin.cs

@@ -4,21 +4,29 @@
  * SPDX-License-Identifier: MIT
  */
 
+using System.IO;
 using UnityEngine;
 using Stopwatch = System.Diagnostics.Stopwatch;
 
 public class MultiCameraPlugin : DevicePlugin
 {
+	private MemoryStream memoryStreamForCameraInfo = null;
 	private SensorDevices.MultiCamera cam = null;
 
 	protected override void OnAwake()
 	{
+		memoryStreamForCameraInfo = new MemoryStream();
 		cam = gameObject.GetComponent<SensorDevices.MultiCamera>();
+		partName = DeviceHelper.GetPartName(gameObject);
 	}
 
 	protected override void OnStart()
 	{
-		partName = DeviceHelper.GetPartName(gameObject);
+		var hashServiceKey = MakeHashKey(partName, "Info");
+		if (!RegisterServiceDevice(hashServiceKey))
+		{
+			Debug.LogError("Failed to register ElevatorSystem service - " + hashServiceKey);
+		}
 
 		var hashKey = MakeHashKey(partName);
 		if (!RegisterTxDevice(hashKey))
@@ -27,6 +35,12 @@ protected override void OnStart()
 		}
 
 		AddThread(Sender);
+		AddThread(Response);
+	}
+
+	void OnDestroy()
+	{
+		memoryStreamForCameraInfo.Dispose();
 	}
 
 	private void Sender()
@@ -46,4 +60,28 @@ private void Sender()
 			cam.SetTransportTime((float)sw.Elapsed.TotalSeconds);
 		}
 	}
+
+	private void Response()
+	{
+		while (true)
+		{
+			var receivedBuffer = ReceiveRequest();
+
+			var requestMessage = CameraPlugin.ParsingCameraInfoRequest(ref memoryStreamForCameraInfo, receivedBuffer);
+
+			if (requestMessage != null && requestMessage.Name.Equals("request_camera_info"))
+			{
+				var cameraName = requestMessage.Value.StringValue;
+
+				if (cameraName != null)
+				{
+					var cameraInfoMessage = cam.GetCameraInfo(cameraName);
+
+					CameraPlugin.SetCameraInfoResponse(ref memoryStreamForCameraInfo, cameraInfoMessage);
+				}
+
+				SendResponse(memoryStreamForCameraInfo);
+			}
+		}
+	}
 }

Assets/Scripts/Devices/MultiCamera.cs

@@ -67,7 +67,7 @@ protected override void GenerateMessage()
 			{
 				var image = images[index];
 				var imageData = cameras[index].GetCamImageData();
-				if (image.Data.Length == imageData.Length)
+				if (imageData != null && image.Data.Length == imageData.Length)
 				{
 					image.Data = imageData;
 				}
@@ -81,7 +81,7 @@ protected override void GenerateMessage()
 		private void AddCamera(in SDF.Camera parameters)
 		{
 			var newCamObject = new GameObject();
-			newCamObject.name = "Camera::" + parameters.name;
+			newCamObject.name = parameters.name;
 
 			var newCamTransform = newCamObject.transform;
 			newCamTransform.position = Vector3.zero;
@@ -107,5 +107,31 @@ private void InitializeCamMessages(in SDF.Camera parameters)
 			image.Data = new byte[image.Height * image.Step];
 			imagesStamped.Images.Add(image);
 		}
+
+		public messages.CameraSensor GetCameraInfo(in string cameraName)
+		{
+			for (var index = 0; index < cameras.Count; index++)
+			{
+				if (cameras[index].deviceName.Equals(("MultiCamera::" + cameraName)))
+				{
+					return GetCameraInfo(index);
+				}
+			}
+
+			return null;
+		}
+
+		public messages.CameraSensor GetCameraInfo(in int cameraIndex)
+		{
+			if (cameraIndex < cameras.Count)
+			{
+				return cameras[cameraIndex].GetCameraInfo();
+			}
+			else
+			{
+				Debug.LogWarning("unavailable camera index: " + cameraIndex);
+				return null;
+			}
+		}
 	}
 }

ProjectSettings/ProjectSettings.asset

@@ -121,7 +121,7 @@ PlayerSettings:
     16:10: 1
     16:9: 1
     Others: 1
-  bundleVersion: 1.3.2
+  bundleVersion: 1.4.0
   preloadedAssets: []
   metroInputSource: 0
   wsaTransparentSwapchain: 0