From 7ca3e1235619927483fcf7fcd6f0a454d726e5af Mon Sep 17 00:00:00 2001
From: Devin Miller <devin.miller@unity3d.com>
Date: Fri, 19 Feb 2021 17:44:23 -0800
Subject: [PATCH] Fixing issue with Demo.cs not working with new URDF Importer
 co-routine

---
 .../Assets/DemoScripts/Demo.cs                | 74 ++++++++++---------
 1 file changed, 39 insertions(+), 35 deletions(-)

diff --git a/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs b/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs
index 8878ce80..348cb0c6 100644
--- a/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs
+++ b/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs
@@ -9,6 +9,7 @@
 using RosSharp;
 using RosSharp.Control;
 using RosSharp.Urdf.Editor;
+using Unity.EditorCoroutines.Editor;
 using UnityEditor;
 using UnityEngine;
 using Unity.Robotics.ROSTCPConnector;
@@ -75,8 +76,10 @@ void Awake()
     {
         EditorApplication.LockReloadAssemblies();
         SetupScene();
-        SetupRos();
-        CreateTrajectoryPlannerPubliser();
+        AddRosMessages();
+        ImportRobot();
+        CreateRosConnection();
+        CreateTrajectoryPlannerPublisher();
     }
 
     void Update()
@@ -104,31 +107,18 @@ void OnApplicationQuit()
     private void SetupScene()
     {
         // Instantiate the table, target, and target placement
-        GameObject table = InstantiatePrefab(tableName);
-        GameObject target = InstantiatePrefab(targetName);
-        GameObject targetPlacement = InstantiatePrefab(targetPlacementName);
+        InstantiatePrefab(tableName);
+        InstantiatePrefab(targetName);
+        InstantiatePrefab(targetPlacementName);
 
         // Adjust main camera
         GameObject camera = GameObject.Find(cameraName);
         camera.transform.position = cameraPosition;
         camera.transform.rotation = cameraRotation;
-
-        // Import Niryo One with URDF importer
-        string urdfFilepath = Path.Combine(Application.dataPath, urdfRelativeFilepath);
-        ImportRobot(urdfFilepath, ImportSettings.convexDecomposer.unity);
-
-        // Adjust robot parameters
-        Controller controller = GameObject.Find(niryoOneName).GetComponent<Controller>();
-        controller.stiffness = controllerStiffness;
-        controller.damping = controllerDamping;
-        controller.forceLimit = controllerForceLimit;
-        controller.speed = controllerSpeed;
-        controller.acceleration = controllerAcceleration;
-        GameObject.Find(baseLinkName).GetComponent<ArticulationBody>().immovable = true;
     }
 
     // Tutorial Part 2 without the Publisher object
-    private void SetupRos()
+    private void AddRosMessages()
     {
         if (generateRosMessages)
         {
@@ -152,19 +142,9 @@ private void SetupRos()
         scripts.AddRange(Directory.GetFiles(rosMessagesDirectory, scriptPattern, SearchOption.AllDirectories));
         scripts.AddRange(Directory.GetFiles(externalScriptsDirectory));
         RecompileScripts(scripts.ToArray());
-        
-        // Create RosConnect
-        GameObject rosConnect = new GameObject(rosConnectName);
-        rosConnection = rosConnect.AddComponent<ROSConnection>();
-        rosConnection.rosIPAddress = hostIP;
-        rosConnection.rosPort = hostPort;
-        rosConnection.overrideUnityIP = overrideUnityIP;
-        rosConnection.unityPort = unityPort;
-        rosConnection.awaitDataMaxRetries = awaitDataMaxRetries;
-        rosConnection.awaitDataSleepSeconds = awaitDataSleepSeconds;
     }
 
-    private void CreateTrajectoryPlannerPubliser()
+    private void CreateTrajectoryPlannerPublisher()
     {
         GameObject publisher = new GameObject(publisherName);
         dynamic planner = publisher.AddComponent(assembly.GetType(trajectoryPlannerType));
@@ -182,15 +162,39 @@ private GameObject InstantiatePrefab(string name)
         return gameObject;
     }
 
-    private void ImportRobot(string urdfFilepath, ImportSettings.convexDecomposer convexDecomposer)
+    private void CreateRosConnection()
     {
-        // Import Niryo One by URDF Importer
-        ImportSettings settings = new ImportSettings
+        // Create RosConnect
+        GameObject rosConnect = new GameObject(rosConnectName);
+        rosConnection = rosConnect.AddComponent<ROSConnection>();
+        rosConnection.rosIPAddress = hostIP;
+        rosConnection.rosPort = hostPort;
+        rosConnection.overrideUnityIP = overrideUnityIP;
+        rosConnection.unityPort = unityPort;
+        rosConnection.awaitDataMaxRetries = awaitDataMaxRetries;
+        rosConnection.awaitDataSleepSeconds = awaitDataSleepSeconds;
+    }
+
+    private void ImportRobot()
+    {
+        ImportSettings urdfImportSettings = new ImportSettings
         {
             choosenAxis = ImportSettings.axisType.yAxis,
-            convexMethod = convexDecomposer,
+            convexMethod = ImportSettings.convexDecomposer.unity
         };
-        UrdfRobotExtensions.Create(urdfFilepath, settings);
+        // Import Niryo One with URDF importer
+        string urdfFilepath = Path.Combine(Application.dataPath, urdfRelativeFilepath);
+        // Create is a coroutine that would usually run only in EditMode, so we need to force its execution here
+        var robotImporter = UrdfRobotExtensions.Create(urdfFilepath, urdfImportSettings, false);
+        while (robotImporter.MoveNext()) {}
+        // Adjust robot parameters
+        Controller controller = GameObject.Find(niryoOneName).GetComponent<Controller>();
+        controller.stiffness = controllerStiffness;
+        controller.damping = controllerDamping;
+        controller.forceLimit = controllerForceLimit;
+        controller.speed = controllerSpeed;
+        controller.acceleration = controllerAcceleration;
+        GameObject.Find(baseLinkName).GetComponent<ArticulationBody>().immovable = true;
     }
 
     // Credit to https://www.codeproject.com/Tips/715891/Compiling-Csharp-Code-at-Runtime