From ca5298710565c92da6607200ed4ea396b3b30231 Mon Sep 17 00:00:00 2001
From: yjkim046 <yjkim046@yahoo.com>
Date: Wed, 29 Apr 2020 16:31:19 +0900
Subject: [PATCH 01/14] Update README.md

---
 README.md | 31 ++++++++++++++-----------------
 1 file changed, 14 insertions(+), 17 deletions(-)

diff --git a/README.md b/README.md
index 5f3d1474..36292806 100644
--- a/README.md
+++ b/README.md
@@ -1,26 +1,23 @@
 # CLOiSim : Multi-Robot Simulator
 
-Happy to announce CLOiSim. It is a new multi-robot simulator to bring-up SDF(www.sdformat.org) file which contains 3d world environemnt and multi-robot descriptions. 
+Happy to announce CLOiSim. It is a new multi-robot simulator that uses an SDF(www.sdformat.org) file containing 3d world environemnts and robot descriptions.
 
-It may looks similar to Gazebo simulator, but we had a problems to load multi-robot which equipped multi-sensors in Gazebo.
-So that is why we initiated to utilize a 'Unity 3D'.
-I believe 'Unity' can provide a powerful resource handling and can see many possiblity in industrial area by Unity.
+The simulator is based on Unity 3D. It may look similar to Gazebo, where, unfortunately, we encountered performance problems while loading multiple robots equipped with multiple sensors.
 
-So, this project consist of 
+This project consists of 
 - [SDF](http://sdformat.org/spec?ver=1.7) Parser for C#
-- [SDF](http://sdformat.org/spec?ver=1.7) Implementation for Unity -> **Visual / Collision / Sensor / Physics for joints**
+- [SDF](http://sdformat.org/spec?ver=1.7) Robot Implementation for Unity -> **Visual / Collision / Sensor / Physics for joints**
 - [SDF](http://sdformat.org/spec?ver=1.7) Plugins for Unity
-- UI modules -> On screen information
-- Network modules -> transporting sensor data or control data
-- Web service -> control and manipulate simulation
+- UI modules -> Module for on-screen information
+- Network modules -> Module for transporting sensor data or control signal
+- Web service -> Module for controling simulation through a web interface
 
 
 ## Features
-Unfortunately, all sensor models are not fully developed yet.
-Because first target for simulation was 2-wheeled mobile robot with 2D lidar sensor.
-
-Here are sensor models that already implemented or ongoing.
-- [X] 2D Lidar Sensor
+The current release includes the features only for a 2-wheeled mobile robot with 2D LiDAR sensor.
+Other sensor models are work in progress.
+Here is the full list of models that is implemented or planned to be implemented.
+- [X] 2D LiDAR Sensor
 - [X] 2-Wheeled Motor
 - [X] Sonar sensor
 - [X] IMU
@@ -30,12 +27,12 @@ Here are sensor models that already implemented or ongoing.
 - [ ] 3D Lidar Sensor
 - [ ] Sensor noise model
 
-Plus, [SDF](http://sdformat.org/spec?ver=1.7) implementation only works on essenstial elements like `<model>`, `<link>`, `<visual>`, `<collision>`, `<joint>`,  etc.
-For example, kind of optional elmenets `<lights>`, `<audio>`, `<actor>`, `<state>` and so on are not implemented yet.
+Plus, [SDF](http://sdformat.org/spec?ver=1.7) works on the essential elements such as `<model>`, `<link>`, `<visual>`, `<collision>`, `<joint>`,  etc.
+It does not support optional elmenets like `<lights>`, `<audio>`, `<actor>`, `<state>`.
 
 
 ## How it works
-It's simple. Refer to core codes in 'Assets/Scripts'.
+Refer to core codes in 'Assets/Scripts'.
 * Load SDF file -> Parse SDF(simulation description) -> Implement and realize description
 
 Shaders are also used to get depth buffer information in a few sensor model.

From 0de5e6e17face729d4801aca0a08f2065e020c7e Mon Sep 17 00:00:00 2001
From: Hyunseok Yang <hyunseok7.yang@lge.com>
Date: Mon, 4 May 2020 17:38:19 +0900
Subject: [PATCH 02/14] Upgrade Unity editor version -> 2019.3.12f1

---
 ProjectSettings/ProjectVersion.txt | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/ProjectSettings/ProjectVersion.txt b/ProjectSettings/ProjectVersion.txt
index c7d3fa90..99a0e87f 100644
--- a/ProjectSettings/ProjectVersion.txt
+++ b/ProjectSettings/ProjectVersion.txt
@@ -1,2 +1,2 @@
-m_EditorVersion: 2019.3.11f1
-m_EditorVersionWithRevision: 2019.3.11f1 (ceef2d848e70)
+m_EditorVersion: 2019.3.12f1
+m_EditorVersionWithRevision: 2019.3.12f1 (84b23722532d)

From e27180ab270d236536cd11e860a8b10b6af76ee2 Mon Sep 17 00:00:00 2001
From: Hyunseok Yang <hyunseok7.yang@lge.com>
Date: Mon, 4 May 2020 17:45:26 +0900
Subject: [PATCH 03/14] Packages version updated

---
 Packages/manifest.json | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/Packages/manifest.json b/Packages/manifest.json
index b1817531..c6ace87f 100644
--- a/Packages/manifest.json
+++ b/Packages/manifest.json
@@ -4,8 +4,8 @@
     "com.unity.ide.vscode": "1.2.0",
     "com.unity.mathematics": "1.1.0",
     "com.unity.render-pipelines.core": "7.3.1",
-    "com.unity.searcher": "4.0.9",
-    "com.unity.test-framework": "1.1.13",
+    "com.unity.searcher": "4.1.0",
+    "com.unity.test-framework": "1.1.14",
     "com.unity.textmeshpro": "2.0.1",
     "com.unity.ugui": "1.0.0",
     "com.unity.modules.ai": "1.0.0",

From 8827f14eedcafdeeb230be903494857963d5edcf Mon Sep 17 00:00:00 2001
From: Hyunseok Yang <hyunseok7.yang@lge.com>
Date: Wed, 6 May 2020 10:52:40 +0900
Subject: [PATCH 04/14] Modify CustomPlugin - RobotControl   : Change xml path
 of plugin parameters

---
 Assets/Scripts/CustomPlugins/RobotControl.cs | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/Assets/Scripts/CustomPlugins/RobotControl.cs b/Assets/Scripts/CustomPlugins/RobotControl.cs
index 577ee32c..79efc333 100644
--- a/Assets/Scripts/CustomPlugins/RobotControl.cs
+++ b/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)

From f84f9c620272c3691e4378248717e2c1c230a24d Mon Sep 17 00:00:00 2001
From: Hyunseok Yang <hyunseok7.yang@lge.com>
Date: Thu, 7 May 2020 11:17:52 +0900
Subject: [PATCH 05/14] Update .gitignore file

---
 .gitignore | 1 +
 1 file changed, 1 insertion(+)

diff --git a/.gitignore b/.gitignore
index aaf2f7a3..1e99de12 100644
--- a/.gitignore
+++ b/.gitignore
@@ -42,6 +42,7 @@
 
 # Visual Studio Code user settings
 .vscode/
+*.code-workspace
 
 # Gradle cache directory
 .gradle/

From 21793237cb49bd84675112a5247a18f956bc1395 Mon Sep 17 00:00:00 2001
From: Hyunseok Yang <hyunseok7.yang@lge.com>
Date: Thu, 7 May 2020 13:54:38 +0900
Subject: [PATCH 06/14] Upgrade Unity editor version -> 2019.3.13f1

---
 ProjectSettings/ProjectVersion.txt | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/ProjectSettings/ProjectVersion.txt b/ProjectSettings/ProjectVersion.txt
index 99a0e87f..beea0cc3 100644
--- a/ProjectSettings/ProjectVersion.txt
+++ b/ProjectSettings/ProjectVersion.txt
@@ -1,2 +1,2 @@
-m_EditorVersion: 2019.3.12f1
-m_EditorVersionWithRevision: 2019.3.12f1 (84b23722532d)
+m_EditorVersion: 2019.3.13f1
+m_EditorVersionWithRevision: 2019.3.13f1 (d4ddf0d95db9)

From 6e7a2f9ec4e659d32501531e17d9c83749287bcf Mon Sep 17 00:00:00 2001
From: Hyunseok Yang <hyunseok7.yang@lge.com>
Date: Thu, 7 May 2020 13:55:30 +0900
Subject: [PATCH 07/14] Change default world name in MainScene

---
 Assets/Scenes/MainScene.unity | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/Assets/Scenes/MainScene.unity b/Assets/Scenes/MainScene.unity
index 78cd37ba..9d8df15f 100644
--- a/Assets/Scenes/MainScene.unity
+++ b/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

From 210fcc8b8b3adae723ab74a078d3f33aeaba7c78 Mon Sep 17 00:00:00 2001
From: Hyunseok Yang <hyunseok7.yang@lge.com>
Date: Wed, 13 May 2020 19:42:46 +0900
Subject: [PATCH 08/14] Modify SDF Parser - Root class   : add minor defensive
 code

---
 .gitignore                       |  6 +++
 Assets/Scripts/Tools/SDF/Root.cs | 68 ++++++++++++++++++++------------
 2 files changed, 49 insertions(+), 25 deletions(-)

diff --git a/.gitignore b/.gitignore
index 1e99de12..3babf649 100644
--- a/.gitignore
+++ b/.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
 
diff --git a/Assets/Scripts/Tools/SDF/Root.cs b/Assets/Scripts/Tools/SDF/Root.cs
index 6236d7e6..b209496c 100644
--- a/Assets/Scripts/Tools/SDF/Root.cs
+++ b/Assets/Scripts/Tools/SDF/Root.cs
@@ -16,9 +16,11 @@ public class Root
 	{
 		private Dictionary<string, Tuple<string, string>> resourceModelTable = null; // Model Name, (Model Path, Model File)
 
-		private string[] sdfVersions = { "1.6", "1.5", "1.4", "1.3", "1.2", string.Empty };
+		private string[] sdfVersions = {"1.7", "1.6", "1.5", "1.4", "1.3", "1.2", string.Empty};
 
-		XmlDocument doc = null;
+		private XmlDocument doc = null;
+
+		private string sdfVersion = "1.7";
 
 		private World world = null;
 
@@ -129,13 +131,17 @@ public void updateResourceModelTable()
 				foreach (var subDirectory in rootDirectory.GetDirectories())
 				{
 					if (subDirectory.Name.StartsWith("."))
+					{
 						continue;
+					}
 
 					//Console.WriteLine(subDirectory.Name + " => " + subDirectory.FullName);
 					var modelConfig = subDirectory.FullName + "/model.config";
 
 					if (!File.Exists(modelConfig))
+					{
 						continue;
+					}
 
 					try
 					{
@@ -143,7 +149,7 @@ public void updateResourceModelTable()
 					}
 					catch (XmlException e)
 					{
-						Console.WriteLine("Failed to Load model file - " + e.Message);
+						Console.WriteLine("Failed to Load model file(" + modelConfig  + ") - " + e.Message);
 						continue;
 					}
 
@@ -151,11 +157,10 @@ public void updateResourceModelTable()
 					var modelName = subDirectory.Name; //modelNode["name"].InnerText;
 
 					// Get Model root
-					XmlNode modelNode = modelConfigDoc.SelectSingleNode("model");
+					var modelNode = modelConfigDoc.SelectSingleNode("model");
 
 					// Get Model SDF file name
-					//XmlNode sdfNodeList =
-					string sdfFileName = null;
+					var sdfFileName = string.Empty;
 					foreach (var version in sdfVersions)
 					{
 						//Console.WriteLine(version);
@@ -163,12 +168,13 @@ public void updateResourceModelTable()
 						if (sdfNode != null)
 						{
 							sdfFileName = sdfNode.InnerText;
+							sdfVersion = version;
 							//Console.WriteLine(version + "," + sdfFileName);
 							break;
 						}
 					}
 
-					if (sdfFileName == null)
+					if (string.IsNullOrEmpty(sdfFileName))
 					{
 						Console.WriteLine(modelName + ": SDF FileName is empty!!");
 						continue;
@@ -209,11 +215,11 @@ private void ConvertUriToAbsolutePath()
 				string uri = node.InnerText;
 				if (uri.StartsWith("model://"))
 				{
-					string modelUri = uri.Replace("model://", string.Empty);
+					var modelUri = uri.Replace("model://", string.Empty);
 					var stringArray = modelUri.Split('/');
 
 					// Get Model name from Uri
-					string modelName = stringArray[0];
+					var modelName = stringArray[0];
 
 					// remove Model name in array
 					modelUri = string.Join("/", stringArray.Skip(1));
@@ -249,12 +255,12 @@ private void replaceAllIncludedModel()
 
 				foreach (XmlNode node in nodes)
 				{
-					XmlNode modelNode = GetIncludedModel(node);
+					var modelNode = GetIncludedModel(node);
 
 					if (modelNode != null)
 					{
 						// Console.WriteLine("Node - " + modelNode);
-						XmlNode importNode = doc.ImportNode(modelNode, true);
+						var importNode = doc.ImportNode(modelNode, true);
 						node.ParentNode.ReplaceChild(importNode, node);
 					}
 					else
@@ -267,28 +273,27 @@ private void replaceAllIncludedModel()
 
 		private XmlNode GetIncludedModel(XmlNode _node)
 		{
-			XmlNode nameNode = _node.SelectSingleNode("name");
-			string name = (nameNode == null) ? null : nameNode.InnerText;
+			var nameNode = _node.SelectSingleNode("name");
+			var name = (nameNode == null) ? null : nameNode.InnerText;
 
-			XmlNode poseNode = _node.SelectSingleNode("pose");
-			string pose = (poseNode == null) ? null : poseNode.InnerText;
+			var poseNode = _node.SelectSingleNode("pose");
+			var pose = (poseNode == null) ? null : poseNode.InnerText;
 
-			XmlNode staticNode = _node.SelectSingleNode("static");
-			string isStatic = (staticNode == null) ? null : staticNode.InnerText;
+			var staticNode = _node.SelectSingleNode("static");
+			var isStatic = (staticNode == null) ? null : staticNode.InnerText;
 
-			string uri = _node.SelectSingleNode("uri").InnerText;
+			var uri = _node.SelectSingleNode("uri").InnerText;
 
 			// Console.WriteLineFormat("{0} | {1} | {2} | {3}", name, uri, pose, isStatic);
 
 			Tuple<string, string> value;
-			string modelName = uri.Replace("model://", string.Empty);
+			var modelName = uri.Replace("model://", string.Empty);
 			if (resourceModelTable.TryGetValue(modelName, out value))
 			{
 				uri = value.Item1 + "/" + value.Item2;
 			}
 
-			XmlDocument modelSdfDoc = new XmlDocument();
-			XmlNode sdfNode = null;
+			var modelSdfDoc = new XmlDocument();
 
 			try
 			{
@@ -296,13 +301,22 @@ private XmlNode GetIncludedModel(XmlNode _node)
 			}
 			catch (XmlException e)
 			{
-				Console.WriteLine("Failed to Load included model(" + modelName + ") file - " + e.Message); ;
+				Console.WriteLine("Failed to Load included model(" + modelName + ") file - " + e.Message);
+				return null;
 			}
 
-			sdfNode = modelSdfDoc.SelectSingleNode("/sdf/model");
+			var sdfNode = modelSdfDoc.SelectSingleNode("/sdf/model");
+
+			if (sdfNode == null)
+			{
+				sdfNode = modelSdfDoc.SelectSingleNode("/sdf/light");
+			}
 
-				if (sdfNode == null)
-					sdfNode = modelSdfDoc.SelectSingleNode("/sdf/light");
+			var attributes = sdfNode.Attributes;
+			if (attributes.GetNamedItem("version") != null)
+			{
+				var modelSdfDocVersion = attributes.GetNamedItem("version").Value;
+			}
 
 			// Edit custom parameter
 			if (nameNode != null)
@@ -313,7 +327,9 @@ private XmlNode GetIncludedModel(XmlNode _node)
 			if (poseNode != null)
 			{
 				if (sdfNode.SelectSingleNode("pose") != null)
+				{
 					sdfNode.SelectSingleNode("pose").InnerText = pose;
+				}
 
 				else
 				{
@@ -326,7 +342,9 @@ private XmlNode GetIncludedModel(XmlNode _node)
 			if (staticNode != null)
 			{
 				if (sdfNode.SelectSingleNode("static") != null)
+				{
 					sdfNode.SelectSingleNode("static").InnerText = isStatic;
+				}
 				else
 				{
 					XmlElement elem = sdfNode.OwnerDocument.CreateElement("static");

From 76c885ba948b03068aa1f325613e6d13c18e1816 Mon Sep 17 00:00:00 2001
From: Hyunseok Yang <hyunseok7.yang@lge.com>
Date: Thu, 14 May 2020 16:28:42 +0900
Subject: [PATCH 09/14] Bug in SDF Parser - misunderstanding function
 overloading   -> GetAttribute

---
 Assets/Scripts/Tools/SDF/Entity.cs |  4 ++--
 Assets/Scripts/Tools/SDF/Sensor.cs | 31 +++++++++++++++++-------------
 2 files changed, 20 insertions(+), 15 deletions(-)

diff --git a/Assets/Scripts/Tools/SDF/Entity.cs b/Assets/Scripts/Tools/SDF/Entity.cs
index c75b62cf..7e10511a 100644
--- a/Assets/Scripts/Tools/SDF/Entity.cs
+++ b/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)
 			{
diff --git a/Assets/Scripts/Tools/SDF/Sensor.cs b/Assets/Scripts/Tools/SDF/Sensor.cs
index 58167e31..c4e6cc5a 100644
--- a/Assets/Scripts/Tools/SDF/Sensor.cs
+++ b/Assets/Scripts/Tools/SDF/Sensor.cs
@@ -59,7 +59,7 @@ private Ray ParseRay()
 			var ray = new Ray();
 
 			var pose = GetValue<string>("ray/pose");
-			var relative_to = GetAttribute<string>("ray/pose", "relative_to");
+			var relative_to = GetAttributeInPath<string>("ray/pose", "relative_to");
 
 			ray.ParsePose(pose, relative_to);
 
@@ -111,12 +111,17 @@ private Camera ParseCamera(in int index = 1)
 			var camera = new Camera();
 
 			var pose = GetValue<string>(cameraElement + "/pose");
-			var relative_to = GetAttribute<string>(cameraElement + "/pose", "relative_to");
+			var relative_to = GetAttributeInPath<string>(cameraElement + "/pose", "relative_to");
 
 			camera.ParsePose(pose, relative_to);
 
-			camera.name = Name;
-			camera.type = GetAttribute<string>(cameraElement, "name");
+			camera.name = GetAttributeInPath<string>(cameraElement, "name");
+			if (string.IsNullOrEmpty(camera.name))
+			{
+				camera.name = Name;
+			}
+
+			camera.type = Type;
 			camera.horizontal_fov = GetValue<double>(cameraElement + "/horizontal_fov");
 			camera.image_width = GetValue<int>(cameraElement + "/image/width");
 			camera.image_height = GetValue<int>(cameraElement + "/image/height");
@@ -126,7 +131,7 @@ private Camera ParseCamera(in int index = 1)
 
 			if (IsValidNode(cameraElement + "/save"))
 			{
-				camera.save_enabled = GetAttribute<bool>(cameraElement + "/save", "enabled", false);
+				camera.save_enabled = GetAttributeInPath<bool>(cameraElement + "/save", "enabled", false);
 				camera.save_path = GetValue<string>(cameraElement + "/save/path");
 			}
 
@@ -196,7 +201,7 @@ private Sonar ParseSonar()
 			var sonar = new Sonar();
 
 			var pose = GetValue<string>("sonar/pose");
-			var relative_to = GetAttribute<string>("sonar/pose", "relative_to");
+			var relative_to = GetAttributeInPath<string>("sonar/pose", "relative_to");
 
 			sonar.ParsePose(pose, relative_to);
 
@@ -220,7 +225,7 @@ private IMU ParseIMU()
 			var imu = new IMU();
 
 			var pose = GetValue<string>("imu/pose");
-			var relative_to = GetAttribute<string>("imu/pose", "relative_to");
+			var relative_to = GetAttributeInPath<string>("imu/pose", "relative_to");
 
 			imu.ParsePose(pose, relative_to);
 
@@ -231,16 +236,16 @@ private IMU ParseIMU()
 
 			if (IsValidNode("imu/angular_velocity"))
 			{
-				imu.angular_velocity_x.type = GetAttribute<string>("imu/angular_velocity/x/noise", "type");
-				imu.angular_velocity_y.type = GetAttribute<string>("imu/angular_velocity/y/noise", "type");
-				imu.angular_velocity_z.type = GetAttribute<string>("imu/angular_velocity/z/noise", "type");
+				imu.angular_velocity_x.type = GetAttributeInPath<string>("imu/angular_velocity/x/noise", "type");
+				imu.angular_velocity_y.type = GetAttributeInPath<string>("imu/angular_velocity/y/noise", "type");
+				imu.angular_velocity_z.type = GetAttributeInPath<string>("imu/angular_velocity/z/noise", "type");
 			}
 
 			if (IsValidNode("imu/linear_acceleration"))
 			{
-				imu.linear_acceleration_x.type = GetAttribute<string>("imu/linear_acceleration/x/noise", "type");
-				imu.linear_acceleration_y.type = GetAttribute<string>("imu/linear_acceleration/y/noise", "type");
-				imu.linear_acceleration_z.type = GetAttribute<string>("imu/linear_acceleration/z/noise", "type");
+				imu.linear_acceleration_x.type = GetAttributeInPath<string>("imu/linear_acceleration/x/noise", "type");
+				imu.linear_acceleration_y.type = GetAttributeInPath<string>("imu/linear_acceleration/y/noise", "type");
+				imu.linear_acceleration_z.type = GetAttributeInPath<string>("imu/linear_acceleration/z/noise", "type");
 			}
 			return imu;
 		}

From c81efad0feb1cf86276108f77afe28009199a4bf Mon Sep 17 00:00:00 2001
From: Hyunseok Yang <hyunseok7.yang@lge.com>
Date: Mon, 18 May 2020 17:33:24 +0900
Subject: [PATCH 10/14] change FPS update period in SimulationInfo Display

---
 Assets/Scripts/UI/SimulationDisplay.cs | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/Assets/Scripts/UI/SimulationDisplay.cs b/Assets/Scripts/UI/SimulationDisplay.cs
index 0032bbb0..ab5826dc 100644
--- a/Assets/Scripts/UI/SimulationDisplay.cs
+++ b/Assets/Scripts/UI/SimulationDisplay.cs
@@ -3,7 +3,7 @@
  *
  * SPDX-License-Identifier: MIT
  */
- 
+
 using System;
 using System.Text;
 using UnityEngine;
@@ -18,7 +18,7 @@ public class SimulationDisplay : MonoBehaviour
 	private float dt = 0.0F;
 	private float fps = 0.0F;
 
-	private const float fpsUpdatePeriod = 1.0F;
+	private const float fpsUpdatePeriod = 0.5F;
 
 	// Start is called before the first frame update
 	void Awake()

From 5d84bdf1d71a4781346d477b1b21aa9e964c1332 Mon Sep 17 00:00:00 2001
From: Hyunseok Yang <hyunseok7.yang@lge.com>
Date: Mon, 18 May 2020 17:35:07 +0900
Subject: [PATCH 11/14] Update MultiCamera sensor device Add MultiCamera plugin
 for transportation

---
 .../CustomPlugins/MultiCameraPlugin.cs        |  51 ++++++++
 .../CustomPlugins/MultiCameraPlugin.cs.meta   |  11 ++
 Assets/Scripts/Devices/Camera.CameraData.cs   |   5 +
 Assets/Scripts/Devices/Camera.cs              |   3 +-
 Assets/Scripts/Devices/Lidar.cs               |   3 +
 Assets/Scripts/Devices/MultiCamera.cs         | 117 ++++--------------
 .../Tools/SDFImporter/SDFImplement.Sensor.cs  |   5 +-
 7 files changed, 99 insertions(+), 96 deletions(-)
 create mode 100644 Assets/Scripts/CustomPlugins/MultiCameraPlugin.cs
 create mode 100644 Assets/Scripts/CustomPlugins/MultiCameraPlugin.cs.meta

diff --git a/Assets/Scripts/CustomPlugins/MultiCameraPlugin.cs b/Assets/Scripts/CustomPlugins/MultiCameraPlugin.cs
new file mode 100644
index 00000000..dfe9c98e
--- /dev/null
+++ b/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);
+		}
+	}
+}
\ No newline at end of file
diff --git a/Assets/Scripts/CustomPlugins/MultiCameraPlugin.cs.meta b/Assets/Scripts/CustomPlugins/MultiCameraPlugin.cs.meta
new file mode 100644
index 00000000..e2b9f42e
--- /dev/null
+++ b/Assets/Scripts/CustomPlugins/MultiCameraPlugin.cs.meta
@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: ebc9b00419b18fb70b7abe8073aa45de
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
diff --git a/Assets/Scripts/Devices/Camera.CameraData.cs b/Assets/Scripts/Devices/Camera.CameraData.cs
index 3b819661..84900162 100644
--- a/Assets/Scripts/Devices/Camera.CameraData.cs
+++ b/Assets/Scripts/Devices/Camera.CameraData.cs
@@ -189,5 +189,10 @@ public void SaveRawImageData(in string path, in string name)
 		}
 
 		private CamData camData;
+
+		public byte[] GetCamImageData()
+		{
+			return camData.GetTextureData();
+		}
 	}
 }
\ No newline at end of file
diff --git a/Assets/Scripts/Devices/Camera.cs b/Assets/Scripts/Devices/Camera.cs
index 2cc11fcd..312740b9 100644
--- a/Assets/Scripts/Devices/Camera.cs
+++ b/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;
diff --git a/Assets/Scripts/Devices/Lidar.cs b/Assets/Scripts/Devices/Lidar.cs
index 1d6a8649..60c56ed3 100644
--- a/Assets/Scripts/Devices/Lidar.cs
+++ b/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;
diff --git a/Assets/Scripts/Devices/MultiCamera.cs b/Assets/Scripts/Devices/MultiCamera.cs
index da8481c5..248c185c 100644
--- a/Assets/Scripts/Devices/MultiCamera.cs
+++ b/Assets/Scripts/Devices/MultiCamera.cs
@@ -6,8 +6,6 @@
 
 using System.Collections;
 using System.Collections.Generic;
-using System;
-using Unity.Collections;
 using UnityEngine;
 using Stopwatch = System.Diagnostics.Stopwatch;
 
@@ -15,10 +13,12 @@ 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,17 @@ 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 (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);
diff --git a/Assets/Scripts/Tools/SDFImporter/SDFImplement.Sensor.cs b/Assets/Scripts/Tools/SDFImporter/SDFImplement.Sensor.cs
index 2af00083..b605a2b5 100644
--- a/Assets/Scripts/Tools/SDFImporter/SDFImplement.Sensor.cs
+++ b/Assets/Scripts/Tools/SDFImporter/SDFImplement.Sensor.cs
@@ -113,11 +113,8 @@ public static Device AddMultiCamera(in SDF.Cameras element, in GameObject target
 
 			var multicamera = newSensorObject.AddComponent<SensorDevices.MultiCamera>();
 			multicamera.deviceName = GetFrameName(newSensorObject);
+			multicamera.parameters = element;
 
-			foreach (var cam in element.list)
-			{
-				multicamera.AddCamera(cam);
-			}
 			return multicamera;
 		}
 

From 44a42a6d711779a03b661f07e2ff57dac3a262e2 Mon Sep 17 00:00:00 2001
From: Hyunseok Yang <hyunseok7.yang@lge.com>
Date: Thu, 21 May 2020 13:57:59 +0900
Subject: [PATCH 12/14] Upgrade Unity editor version -> 2019.3.14f1

---
 ProjectSettings/ProjectVersion.txt | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/ProjectSettings/ProjectVersion.txt b/ProjectSettings/ProjectVersion.txt
index beea0cc3..3814b772 100644
--- a/ProjectSettings/ProjectVersion.txt
+++ b/ProjectSettings/ProjectVersion.txt
@@ -1,2 +1,2 @@
-m_EditorVersion: 2019.3.13f1
-m_EditorVersionWithRevision: 2019.3.13f1 (d4ddf0d95db9)
+m_EditorVersion: 2019.3.14f1
+m_EditorVersionWithRevision: 2019.3.14f1 (2b330bf6d2d8)

From c99e981d334647cf98e8999ad59eb18fc20324fc Mon Sep 17 00:00:00 2001
From: Hyunseok Yang <hyunseok7.yang@lge.com>
Date: Fri, 22 May 2020 19:25:22 +0900
Subject: [PATCH 13/14] Add defensive code for camera sensor device - check
 null data

---
 Assets/Scripts/Devices/Camera.CameraData.cs | 2 +-
 Assets/Scripts/Devices/MultiCamera.cs       | 3 ++-
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/Assets/Scripts/Devices/Camera.CameraData.cs b/Assets/Scripts/Devices/Camera.CameraData.cs
index 84900162..5be31e8e 100644
--- a/Assets/Scripts/Devices/Camera.CameraData.cs
+++ b/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)
diff --git a/Assets/Scripts/Devices/MultiCamera.cs b/Assets/Scripts/Devices/MultiCamera.cs
index 248c185c..e606f5cf 100644
--- a/Assets/Scripts/Devices/MultiCamera.cs
+++ b/Assets/Scripts/Devices/MultiCamera.cs
@@ -88,7 +88,8 @@ 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;
 				}

From 86abad50d66af15e4c5c8bb9e1e432da17b22332 Mon Sep 17 00:00:00 2001
From: Hyunseok Yang <hyunseok7.yang@lge.com>
Date: Fri, 22 May 2020 19:26:45 +0900
Subject: [PATCH 14/14] Update software version info in project settings

---
 ProjectSettings/ProjectSettings.asset | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/ProjectSettings/ProjectSettings.asset b/ProjectSettings/ProjectSettings.asset
index aaaec1f0..7f46f0a4 100644
--- a/ProjectSettings/ProjectSettings.asset
+++ b/ProjectSettings/ProjectSettings.asset
@@ -119,7 +119,7 @@ PlayerSettings:
     16:10: 1
     16:9: 1
     Others: 1
-  bundleVersion: 1.0.1
+  bundleVersion: 1.1.0
   preloadedAssets: []
   metroInputSource: 0
   wsaTransparentSwapchain: 0