Merge pull request #10 from lge-ros2/develop

Merge 'develop' branch into 'master'

.gitignore

@@ -1,3 +1,9 @@
+#
+# Copyright (c) 2020 LG Electronics Inc.
+#
+# SPDX-License-Identifier: MIT
+#
+
 # Created by https://www.gitignore.io/api/unity
 # Edit at https://www.gitignore.io/?templates=unity
 
@@ -42,6 +48,7 @@
 
 # Visual Studio Code user settings
 .vscode/
+*.code-workspace
 
 # Gradle cache directory
 .gradle/

Assets/Scenes/MainScene.unity

@@ -768,7 +768,7 @@ MonoBehaviour:
   doNotLoad: 0
   pauseOnStart: 0
   clearAllOnStart: 1
-  worldFileName: seocho_tower_multi.world
+  worldFileName: lg_seocho.world
 --- !u!114 &249819590
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -3253,7 +3253,7 @@ Camera:
     y: 0
     width: 1
     height: 1
-  near clip plane: 0.8
+  near clip plane: 0.6
   far clip plane: 500
   field of view: 60
   orthographic: 0

Assets/Scripts/CustomPlugins/MultiCameraPlugin.cs

@@ -0,0 +1,51 @@
+/*
+ * Copyright (c) 2020 LG Electronics Inc.
+ *
+ * SPDX-License-Identifier: MIT
+ */
+
+using UnityEngine;
+using Stopwatch = System.Diagnostics.Stopwatch;
+
+public class MultiCameraPlugin : CustomPlugin
+{
+	public string partName = string.Empty;
+
+	private SensorDevices.MultiCamera cam = null;
+
+	protected override void OnAwake()
+	{
+		partName = DeviceHelper.GetPartName(gameObject);
+
+		string hashKey = MakeHashKey(partName);
+		if (!RegisterTxDevice(hashKey))
+		{
+			Debug.LogError("Failed to register for CameraPlugin - " + hashKey);
+		}
+	}
+
+	protected override void OnStart()
+	{
+		cam = gameObject.GetComponent<SensorDevices.MultiCamera>();
+
+		AddThread(Sender);
+	}
+
+	private void Sender()
+	{
+		Stopwatch sw = new Stopwatch();
+		while (true)
+		{
+			if (cam == null)
+			{
+				continue;
+			}
+
+			var datastreamToSend = cam.PopData();
+			sw.Restart();
+			Publish(datastreamToSend);
+			sw.Stop();
+			cam.SetTransportTime((float)sw.Elapsed.TotalSeconds);
+		}
+	}
+}

Assets/Scripts/CustomPlugins/RobotControl.cs

@@ -60,8 +60,8 @@ protected override void OnStart()
 		var debugging = GetPluginValue<bool>("debug", false);
 		micomInput.EnableDebugging = debugging;
 
-		var motorFriction = GetPluginValue<float>("motor_friction", 0.1f);
-		var brakeFriction = GetPluginValue<float>("brake_friction", 0.1f);
+		var motorFriction = GetPluginValue<float>("wheel/friction/motor", 0.1f); // Currently not used
+		var brakeFriction = GetPluginValue<float>("wheel/friction/brake", 0.1f); // Currently not used
 
 		var modelList = GetComponentsInChildren<ModelPlugin>();
 		foreach (var model in modelList)

Assets/Scripts/Devices/Camera.CameraData.cs

@@ -177,7 +177,7 @@ public void SetTextureData(in NativeArray<byte> buffer)
 
 			public byte[] GetTextureData()
 			{
-				return cameraImage.GetRawTextureData();
+				return (cameraImage is null)? null : cameraImage.GetRawTextureData();
 			}
 
 			public void SaveRawImageData(in string path, in string name)
@@ -189,5 +189,10 @@ public void SaveRawImageData(in string path, in string name)
 		}
 
 		private CamData camData;
+
+		public byte[] GetCamImageData()
+		{
+			return camData.GetTextureData();
+		}
 	}
 }

Assets/Scripts/Devices/Camera.cs

@@ -5,8 +5,6 @@
  */
 
 using System.Collections;
-using System;
-using Unity.Collections;
 using UnityEngine;
 using Stopwatch = System.Diagnostics.Stopwatch;
 
@@ -116,6 +114,7 @@ private void SetupCamera()
 		private IEnumerator CameraWorker()
 		{
 			var waitForSeconds = new WaitForSeconds(UpdatePeriod * adjustCapturingRate);
+
 			while (true)
 			{
 				var oldCulling = GL.invertCulling;

Assets/Scripts/Devices/Lidar.cs

@@ -194,10 +194,13 @@ private IEnumerator LaserCameraWorker()
 				{
 					var data = laserCamData[dataIndex];
 					axisRotation.y = data.CenterAngle;
+
 					laserCamera.transform.localRotation = Quaternion.Euler(axisRotation);
 					laserCamera.Render();
+
 					data.SetTextureData(laserCamera.targetTexture);
 					// data.SaveRawImageData(name);
+
 					laserCamera.enabled = false;
 
 					yield return waitForSeconds;

Assets/Scripts/Devices/MultiCamera.cs

@@ -6,19 +6,19 @@
 
 using System.Collections;
 using System.Collections.Generic;
-using System;
-using Unity.Collections;
 using UnityEngine;
 using Stopwatch = System.Diagnostics.Stopwatch;
 
 namespace SensorDevices
 {
 	public partial class MultiCamera : Device
 	{
-		private List<SensorDevices.Camera> cameras = new List<SensorDevices.Camera>();
+		public List<SensorDevices.Camera> cameras = new List<SensorDevices.Camera>();
 
 		private gazebo.msgs.ImagesStamped imagesStamped;
 
+		public SDF.Cameras parameters = null;
+
 		private Transform multiCamLink = null;
 
 		public MultiCamera()
@@ -30,24 +30,17 @@ public MultiCamera()
 
 		protected override void OnStart()
 		{
-			foreach (var cam in cameras)
+			foreach (var camParameters in parameters.list)
 			{
-				// cam = gameObject.AddComponent<UnityEngine.Camera>();
-				// cam.transform.Rotate(Vector3.up, 90.0000000000f);
-				// cameraLink = transform.parent;
-				// InitializeMessages();
+				AddCamera(camParameters);
+				InitializeMessages(camParameters);
 			}
-			// if (cam)
-			// {
-			// 	SetupCamera();
-			// 	StartCoroutine(CameraWorker());
-			// }
 		}
 
-		public void AddCamera(in SDF.Camera parameters)
+		private void AddCamera(in SDF.Camera parameters)
 		{
 			var newCamObject = new GameObject();
-			newCamObject.name = "Camera::" + parameters.type;
+			newCamObject.name = "Camera::" + parameters.name;
 
 			var newCamTransform = newCamObject.transform;
 			newCamTransform.position = Vector3.zero;
@@ -63,77 +56,15 @@ public void AddCamera(in SDF.Camera parameters)
 			cameras.Add(newCam);
 		}
 
-		private void InitializeMessages()
+		private void InitializeMessages(in SDF.Camera parameters)
 		{
-			// var pixelFormat = GetPixelFormat();
-			// var depth = GetImageDepth();
-
-			var image = imagesStamped.Images;
-			// image.Width = (uint)parameters.image_width;
-			// image.Height = (uint)parameters.image_height;
-			// image.PixelFormat = (uint)pixelFormat;
-			// image.Step = image.Width * depth;
-			// image.Data = new byte[image.Height * image.Step];
-		}
-
-		private void SetupCamera()
-		{
-			// var depthShader = Shader.Find("Sensor/DepthShader");
-			// depthMaterial = new Material(depthShader);
-
-			// cam.ResetWorldToCameraMatrix();
-			// cam.ResetProjectionMatrix();
-
-			// cam.allowHDR = true;
-			// cam.allowMSAA = false;
-			// cam.targetDisplay = 0;
-			// cam.stereoTargetEye = StereoTargetEyeMask.None;
-
-			// cam.orthographic = false;
-			// cam.nearClipPlane = (float)parameters.clip.near;
-			// cam.farClipPlane = (float)parameters.clip.far;
-
-			// var targetRT = new RenderTexture(parameters.image_width, parameters.image_height, 16, RenderTextureFormat.ARGB32, RenderTextureReadWrite.sRGB);
-			// targetRT.name = "CameraTexture";
-			// cam.targetTexture = targetRT;
-
-			// var camHFov = (float)parameters.horizontal_fov * Mathf.Rad2Deg;
-			// var camVFov = DeviceHelper.HorizontalToVerticalFOV(camHFov, cam.aspect);
-			// cam.fieldOfView = camVFov;
-
-			// var projMatrix = DeviceHelper.MakeCustomProjectionMatrix(camHFov, camVFov, cam.nearClipPlane, cam.farClipPlane);
-
-			// // // Invert for gazebo msg
-			// var invertMatrix = Matrix4x4.Scale(new Vector3(	1, -1, 1));
-			// cam.projectionMatrix = projMatrix * invertMatrix;
-			// cam.enabled = false;
-			// // cam.hideFlags |= HideFlags.NotEditable;
-
-			// camData.AllocateTexture(parameters.image_width, parameters.image_height);
-		}
-
-		private IEnumerator CameraWorker()
-		{
-			while (true)
-			{
-				// var oldCulling = GL.invertCulling;
-				// GL.invertCulling = !oldCulling;
-				// cam.Render();
-				// GL.invertCulling = oldCulling;
-				// camData.SetTextureData(cam.targetTexture);
-
-				// if (parameters.save_enabled)
-				// {
-				// 	var saveName = name + "_" + Time.time;
-				// 	camData.SaveRawImageData(parameters.save_path, saveName);
-				// 	// Debug.LogFormat("{0}|{1} captured", parameters.save_path, saveName);
-				// }
-				// cam.enabled = false;
-
-				// imageData = camData.GetTextureData();
-
-				// yield return new WaitForSeconds(UpdatePeriod * adjustCapturingRate);
-			}
+			var image = new gazebo.msgs.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 * Camera.GetImageDepth(parameters.image_format);
+			image.Data = new byte[image.Height * image.Step];
+			imagesStamped.Images.Add(image);
 		}
 
 		protected override IEnumerator MainDeviceWorker()
@@ -152,11 +83,18 @@ protected override IEnumerator MainDeviceWorker()
 		protected override void GenerateMessage()
 		{
 			var images = imagesStamped.Images;
-			// if (image.Data.Length == imageData.Length)
-			// {
-			// 	image.Data = imageData;
-			// }
-			// // Debug.Log(imagesStamped.Image.Height + "," + imagesStamped.Image.Width);
+
+			for (int index = 0; index < images.Count; index++)
+			{
+				var image = images[index];
+				var imageData = cameras[index].GetCamImageData();
+
+				if (imageData != null && (image.Data.Length == imageData.Length))
+				{
+					image.Data = imageData;
+				}
+				// Debug.Log(imagesStamped.Image.Height + "," + imagesStamped.Image.Width);
+			}
 
 			DeviceHelper.SetCurrentTime(imagesStamped.Time);
 			PushData<gazebo.msgs.ImagesStamped>(imagesStamped);

Assets/Scripts/Tools/SDF/Entity.cs

@@ -135,7 +135,7 @@ protected XmlNodeList GetNodes(in string xpath)
 			return ConvertValueType<T>(attributes[attributeName].Value);
 		}
 
-		protected T GetAttribute<T>(in string xpath, in string attributeName, in T defaultValue = default(T))
+		protected T GetAttributeInPath<T>(in string xpath, in string attributeName, in T defaultValue = default(T))
 		{
 			var node = GetNode(xpath);
 			if (node != null)
@@ -215,7 +215,7 @@ private void ParsePose()
 			var value = GetValue<string>("pose");
 
 			// TBD : for SDF 1.7
-			relative_to = GetAttribute<string>("pose", "relative_to");
+			relative_to = GetAttributeInPath<string>("pose", "relative_to");
 
 			if (value != null)
 			{