AIRO-945 Adding integration test and plugging into CI (#18)

Added a simple integration test  into the Project, which can be executed with the Test Runner panel.  I added instructions for running the test to the run_example readme, and I added a yamato config to execute this test automatically against PRs to dev and main.

.yamato/start-ros.bash

@@ -0,0 +1,12 @@
+source /opt/ros/galactic/setup.bash
+
+set -e
+
+# Assuming this script is invoked from the root of the repository...
+DIR_ORIGINAL=$PWD
+cd ros2_docker/colcon_ws
+colcon build
+source install/local_setup.bash
+cd "$DIR_ORIGINAL"
+# TODO: Expose a parameter to disable RViz since we don't need it here
+ros2 launch unity_slam_example unity_slam_example.py &

.yamato/yamato-config.yml

@@ -0,0 +1,28 @@
+name: Nav2 SLAM Example Tests
+agent:
+  type: Unity::VM
+  image: robotics/ci-ros2-galactic-ubuntu20:v0.0.2-nav2slam-848832
+  flavor: i1.large
+variables:
+  PATH: /root/.local/bin:/home/bokken/bin:/home/bokken/.local/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin:/sbin:/home/bokken/.npm-global/bin
+commands:
+  - git submodule update --init --recursive
+  # Ensure audio is disabled. Unity built-in audio fails to initialize in our Bokken image.
+  - "sed -i -e '/m_DisableAudio/ s/: .*/: 1/' ./Nav2SLAMExampleProject/ProjectSettings/AudioManager.asset"
+  - python3 -m pip install unity-downloader-cli --index-url https://artifactory.prd.it.unity3d.com/artifactory/api/pypi/pypi/simple --upgrade
+  - unity-downloader-cli -u 2020.3.11f1 -c editor -c StandaloneSupport-IL2CPP -c Linux --wait --published
+  - git clone git@github.cds.internal.unity3d.com:unity/utr.git utr
+  #TODO: Determine how best to capture ROS logging as test artifacts
+  - /bin/bash .yamato/start-ros.bash
+  - utr/utr --testproject=./Nav2SLAMExampleProject --editor-location=.Editor --reruncount=0
+    --artifacts_path=test-results --suite=playmode --platform=Editor --testfilter IntegrationTests.NavigationIntegrationTests
+
+triggers:
+  cancel_old_ci: true
+  expression: |
+    (pull_request.target in ["main", "dev"] AND
+    NOT pull_request.changes.all match ["**/*.md","**/*.jpg","**/*.jpeg","**/*.gif","**/*.pdf"])
+artifacts:
+  logs:
+    paths:
+      - "test-results/**/*"

Nav2SLAMExampleProject/Assets/Resources/ROSConnectionPrefab.prefab

@@ -54,7 +54,7 @@ MonoBehaviour:
   m_KeepaliveTime: 5
   m_NetworkTimeoutSeconds: 2
   m_SleepTimeSeconds: 0.01
-  m_ShowHUD: 1
+  m_ShowHUD: 0
 --- !u!114 &-8745016548381094262
 MonoBehaviour:
   m_ObjectHideFlags: 0

Nav2SLAMExampleProject/Assets/RosMessages/Unity.Robotics.Nav2Example.Messages.asmdef

@@ -0,0 +1,16 @@
+{
+    "name": "Unity.Robotics.Nav2Example.Messages",
+    "rootNamespace": "",
+    "references": [
+        "GUID:625bfc588fb96c74696858f2c467e978"
+    ],
+    "includePlatforms": [],
+    "excludePlatforms": [],
+    "allowUnsafeCode": false,
+    "overrideReferences": false,
+    "precompiledReferences": [],
+    "autoReferenced": true,
+    "defineConstraints": [],
+    "versionDefines": [],
+    "noEngineReferences": false
+}

Nav2SLAMExampleProject/Assets/RosMessages/Unity.Robotics.Nav2Example.Messages.asmdef.meta

@@ -0,0 +1,7 @@
+fileFormatVersion: 2
+guid: 18b61dc105331d14c9d7a5ba828092b6
+AssemblyDefinitionImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Nav2SLAMExampleProject/Assets/Scenes/Test.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 66a7b6e1a4ebbbf478afa327a1720953
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Nav2SLAMExampleProject/Assets/Scenes/Test/SimpleObstacleCourse.unity.meta

@@ -0,0 +1,7 @@
+fileFormatVersion: 2
+guid: 3ac50540e9c5fc4408b358ac551a7fea
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Nav2SLAMExampleProject/Assets/Scripts/AGVController.cs

@@ -45,7 +45,6 @@ namespace RosSharp.Control
         {
             rosLinear = (float)cmdVel.linear.x;
             rosAngular = (float)cmdVel.angular.z;
-            Debug.Log("Linear : " + rosLinear + " Angular : " + rosAngular);
             lastCmdReceived = Time.time;
         }
 

Nav2SLAMExampleProject/Assets/Scripts/Tests/PlayMode.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 89e4feb62f8c25742b753fe55d07b630
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Nav2SLAMExampleProject/Assets/Scripts/Tests/PlayMode/PlayMode.asmdef

@@ -0,0 +1,23 @@
+{
+    "name": "PlayMode",
+    "rootNamespace": "",
+    "references": [
+        "UnityEngine.TestRunner",
+        "UnityEditor.TestRunner",
+        "Unity.Robotics.ROSTCPConnector",
+        "Unity.Robotics.Nav2Example"
+    ],
+    "includePlatforms": [],
+    "excludePlatforms": [],
+    "allowUnsafeCode": false,
+    "overrideReferences": true,
+    "precompiledReferences": [
+        "nunit.framework.dll"
+    ],
+    "autoReferenced": false,
+    "defineConstraints": [
+        "UNITY_INCLUDE_TESTS"
+    ],
+    "versionDefines": [],
+    "noEngineReferences": false
+}

Nav2SLAMExampleProject/Assets/Scripts/Tests/PlayMode/WaypointIntegrationTest.cs

@@ -0,0 +1,170 @@
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using NUnit.Framework;
+using Unity.Robotics.Core;
+using Unity.Robotics.ROSTCPConnector;
+using UnityEngine;
+using UnityEngine.SceneManagement;
+using UnityEngine.TestTools;
+using Unity.Robotics.ROSTCPConnector.MessageGeneration;
+using Unity.Robotics.ROSTCPConnector.ROSGeometry;
+
+namespace IntegrationTests 
+{
+    class WaypointTracker
+    {
+        internal Transform CurrentWaypoint => m_Waypoints[m_CurrentWaypointIdx];
+        internal int Count => m_Waypoints.Count;
+        
+        const string k_WaypointTag = "Waypoint"; 
+        List<Transform> m_Waypoints;
+        int m_CurrentWaypointIdx;
+
+        internal WaypointTracker()
+        {
+            var waypoints = GameObject.FindGameObjectsWithTag(k_WaypointTag).ToList();
+            waypoints.Sort((g, o) => string.Compare(g.name, o.name));
+            m_Waypoints = waypoints.Select(w => w.transform).ToList();
+            m_CurrentWaypointIdx = -1;
+            if (m_Waypoints.Count == 0)
+            {
+                Debug.LogWarning(
+                    $"Found no GameObjects tagged with {k_WaypointTag} in {SceneManager.GetActiveScene().name}");
+            }
+        }
+
+        internal bool NextWaypoint()
+        {
+            m_CurrentWaypointIdx++;
+            return m_CurrentWaypointIdx < m_Waypoints.Count;
+        }
+    }
+
+    [TestFixture, Explicit, Category("IntegrationTests")]
+    public class NavigationIntegrationTests
+    {
+        // We don't use Path.Combine here because asset paths are always defined with forward-slash
+        const string k_TestSceneAssetPath = "Scenes/Test/";
+        static readonly List<string> k_TestSceneNames = new List<string>
+        {
+            "SimpleObstacleCourse"
+        };
+
+        // TODO: Use a "TestParameters" MonoBehaviour to hold these parameters in a GameObject per-scene,
+        //       so they can be tuned without re-compiling code
+        const string k_RobotTag = "robot";
+
+        const string k_RobotBaseName = "base_footprint/base_link";
+        const string k_GoalPoseFrameId = "map";
+        const string k_GoalPoseTopic = "/goal_pose";
+        static readonly string k_GoalPoseMessageName = 
+            MessageRegistry.GetRosMessageName<RosMessageTypes.Geometry.PoseStampedMsg>();
+        
+        const float k_Nav2InitializeTime = 5.0f;
+        const float k_SleepBetweenWaypointsTime = 2.0f;
+        
+        // Used to define a timeout for waypoint navigation based on distances between steps
+        const float k_MinimumSpeedExpected = 0.15f;
+
+        // How close the TurtleBot must get to the navigation target to be successful
+        const float k_DistanceSuccessThreshold = 0.4f;
+
+        [UnityTearDown]
+        public IEnumerator TearDown()
+        {
+            ROSConnection.instance.Disconnect();
+            yield return null;
+        }
+
+        static string GetScenePath(string sceneName)
+        {
+            return k_TestSceneAssetPath + sceneName;
+        }
+
+        static bool IsCloseEnough(Transform expected, Transform actual)
+        {
+            return (expected.position - actual.position).magnitude < k_DistanceSuccessThreshold;
+        }
+
+        static void ToRosMsg(Transform transform, out RosMessageTypes.Geometry.PoseMsg poseMsg)
+        {
+            poseMsg = new RosMessageTypes.Geometry.PoseMsg();
+            poseMsg.position = transform.position.To<FLU>();
+            poseMsg.orientation = transform.rotation.To<FLU>();
+        }
+
+        static RosMessageTypes.Geometry.PoseStampedMsg ToRosMsg(Transform transform)
+        {
+            ToRosMsg(transform, out var pose);
+            var msg = new RosMessageTypes.Geometry.PoseStampedMsg
+            {
+                header =
+                {
+                    stamp = new TimeStamp(Clock.time),
+                    frame_id = k_GoalPoseFrameId
+                },
+                pose = pose
+            };
+            return msg;
+        }
+
+        [UnityTest]
+        public IEnumerator TurtleBotOnObstacleCourse_NavigateWaypoints_Succeeds(
+            [ValueSource(nameof(k_TestSceneNames))] string sceneName)
+        {
+            var scenePath = GetScenePath(sceneName);
+            //SceneManager.LoadScene(scenePath);
+            yield return SceneManager.LoadSceneAsync(scenePath);
+            
+            var ros = ROSConnection.instance;
+            ros.ConnectOnStart = true;
+            
+            var robots = GameObject.FindGameObjectsWithTag(k_RobotTag);
+            Assert.AreEqual(1, robots.Length,
+                $"There should be exactly one object tagged '{k_RobotTag}' in the scene, but {scenePath} had {robots.Length}");
+            var robot = robots[0].transform.Find(k_RobotBaseName)?.gameObject;
+            Assert.IsNotNull(robot, 
+                $"Expected {robots[0].name} to have a child object named {k_RobotBaseName} but it did not.");
+            
+            var waypoints = new WaypointTracker();
+            Assert.Less(0, waypoints.Count, 
+                $"Every test scene is expected to have at least one waypoint, but {scenePath} had none.");
+
+            yield return new EnterPlayMode();
+            // TODO: Implement some sort of confirmation mechanism on ROS side rather than use arbitrary sleep
+            yield return new WaitForSeconds(k_Nav2InitializeTime);
+            
+            ros.RegisterPublisher(k_GoalPoseTopic, k_GoalPoseMessageName);
+
+            while (waypoints.NextWaypoint())
+            {
+                var timeNavigationStarted = Time.realtimeSinceStartup;
+                var waypoint = waypoints.CurrentWaypoint;
+                var waypointTf = waypoint.transform;
+                var robotTf = robot.transform;
+                var distance = (waypointTf.position - robotTf.position).magnitude;
+                var timeout = distance / k_MinimumSpeedExpected;
+                
+                
+                ros.Send(k_GoalPoseTopic, ToRosMsg(waypointTf));
+
+                yield return new WaitUntil(() => 
+                    IsCloseEnough(waypointTf, robotTf) ||
+                    Time.realtimeSinceStartup - timeNavigationStarted > timeout);
+                
+                Assert.IsTrue(IsCloseEnough(waypointTf.transform, robot.transform), 
+                    $"Robot did not reach {waypoint.name} in the time allotted ({timeout} seconds).");
+                
+                // Because our success threshold may not match the navigation stack's success threshold, we "sleep" for
+                // a small amount of time to ensure the nav stack has time to complete its route
+                yield return new WaitForSeconds(k_SleepBetweenWaypointsTime);
+            }
+            
+            Debug.Log($"Test completed successfully for {scenePath}");
+            yield return null;
+        }
+
+    }
+
+}

Nav2SLAMExampleProject/Assets/Scripts/Tests/PlayMode/WaypointIntegrationTest.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 7f0aa2fbbd1c09a4abfdb24bfae4e666
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Nav2SLAMExampleProject/Assets/Scripts/Tests/Tests.asmdef

@@ -1,6 +0,0 @@
-{
-    "name": "Tests",
-    "optionalUnityReferences": [
-        "TestAssemblies"
-    ]
-}

Nav2SLAMExampleProject/Assets/Scripts/Unity.Robotics.Nav2Example.asmdef

@@ -0,0 +1,18 @@
+{
+    "name": "Unity.Robotics.Nav2Example",
+    "rootNamespace": "",
+    "references": [
+        "GUID:625bfc588fb96c74696858f2c467e978",
+        "GUID:b1ef917f7a8a86a4eb639ec2352edbf8",
+        "GUID:18b61dc105331d14c9d7a5ba828092b6"
+    ],
+    "includePlatforms": [],
+    "excludePlatforms": [],
+    "allowUnsafeCode": false,
+    "overrideReferences": false,
+    "precompiledReferences": [],
+    "autoReferenced": true,
+    "defineConstraints": [],
+    "versionDefines": [],
+    "noEngineReferences": false
+}

Nav2SLAMExampleProject/Assets/Scripts/Unity.Robotics.Nav2Example.asmdef.meta

@@ -0,0 +1,7 @@
+fileFormatVersion: 2
+guid: a2f7fde49569bab48b9253060c5a530b
+AssemblyDefinitionImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Nav2SLAMExampleProject/Assets/Settings/UniversalRenderPipelineAsset.asset

@@ -23,30 +23,30 @@ MonoBehaviour:
   m_RequireOpaqueTexture: 0
   m_OpaqueDownsampling: 1
   m_SupportsTerrainHoles: 1
-  m_SupportsHDR: 0
+  m_SupportsHDR: 1
   m_MSAA: 8
-  m_RenderScale: 1
+  m_RenderScale: 2
   m_MainLightRenderingMode: 1
   m_MainLightShadowsSupported: 1
   m_MainLightShadowmapResolution: 4096
-  m_AdditionalLightsRenderingMode: 1
+  m_AdditionalLightsRenderingMode: 2
-  m_AdditionalLightsPerObjectLimit: 4
+  m_AdditionalLightsPerObjectLimit: 6
   m_AdditionalLightShadowsSupported: 1
   m_AdditionalLightsShadowmapResolution: 512
   m_ShadowDistance: 50
-  m_ShadowCascadeCount: 1
+  m_ShadowCascadeCount: 3
-  m_Cascade2Split: 0.25
+  m_Cascade2Split: 0.136
   m_Cascade3Split: {x: 0.1, y: 0.3}
   m_Cascade4Split: {x: 0.067, y: 0.2, z: 0.467}
-  m_ShadowDepthBias: 1
+  m_ShadowDepthBias: 1.53
-  m_ShadowNormalBias: 1
+  m_ShadowNormalBias: 0
   m_SoftShadowsSupported: 1
   m_UseSRPBatcher: 1
   m_SupportsDynamicBatching: 0
   m_MixedLightingSupported: 1
   m_DebugLevel: 0
   m_UseAdaptivePerformance: 1
-  m_ColorGradingMode: 0
+  m_ColorGradingMode: 1
   m_ColorGradingLutSize: 32
   m_ShadowType: 1
   m_LocalShadowsSupported: 0

Nav2SLAMExampleProject/Assets/turtlebot3/Materials/light_black.mat

@@ -9,7 +9,7 @@ Material:
   m_PrefabAsset: {fileID: 0}
   m_Name: light_black
   m_Shader: {fileID: 4800000, guid: 933532a4fcc9baf4fa0491de14d08ed7, type: 3}
-  m_ShaderKeywords: 
+  m_ShaderKeywords: _SMOOTHNESS_TEXTURE_ALBEDO_CHANNEL_A
   m_LightmapFlags: 4
   m_EnableInstancingVariants: 0
   m_DoubleSidedGI: 0
@@ -91,14 +91,14 @@ Material:
     - _GlossMapScale: 1
     - _Glossiness: 0.75
     - _GlossyReflections: 1
-    - _Metallic: 1
+    - _Metallic: 0.972
     - _Mode: 0
     - _OcclusionStrength: 1
     - _Parallax: 0.02
     - _QueueOffset: 0
     - _ReceiveShadows: 1
-    - _Smoothness: 0.35
+    - _Smoothness: 0.267
-    - _SmoothnessTextureChannel: 0
+    - _SmoothnessTextureChannel: 1
     - _SpecularHighlights: 1
     - _SrcBlend: 1
     - _Surface: 0
@@ -106,7 +106,7 @@ Material:
     - _WorkflowMode: 1
     - _ZWrite: 1
     m_Colors:
-    - _BaseColor: {r: 0.4, g: 0.4, b: 0.4, a: 1}
+    - _BaseColor: {r: 0.16981131, g: 0.16901031, b: 0.16901031, a: 1}
     - _Color: {r: 0.4, g: 0.4, b: 0.4, a: 1}
     - _EmissionColor: {r: 0, g: 0, b: 0, a: 1}
     - _SpecColor: {r: 0.19999996, g: 0.19999996, b: 0.19999996, a: 1}

Nav2SLAMExampleProject/ProjectSettings/EditorBuildSettings.asset

@@ -4,5 +4,14 @@
 EditorBuildSettings:
   m_ObjectHideFlags: 0
   serializedVersion: 2
-  m_Scenes: []
+  m_Scenes:
+  - enabled: 1
+    path: Assets/Scenes/SimpleWarehouseScene.unity
+    guid: 853b6c7d78dda3144b2d4b6c45b2498a
+  - enabled: 1
+    path: Assets/Scenes/BasicScene.unity
+    guid: 4124fab2d5d19c8499effae5c804eb62
+  - enabled: 1
+    path: Assets/Scenes/Test/SimpleObstacleCourse.unity
+    guid: 3ac50540e9c5fc4408b358ac551a7fea
   m_configObjects: {}

Nav2SLAMExampleProject/ProjectSettings/TagManager.asset

@@ -3,7 +3,9 @@
 --- !u!78 &1
 TagManager:
   serializedVersion: 2
-  tags: []
+  tags:
+  - robot
+  - Waypoint
   layers:
   - Default
   - TransparentFX

readmes/run_example.md

@@ -80,6 +80,16 @@ You may also modify the parameters of the LaserScan sensor and observe how diffe
 
  ---
 
+ ### Automate It
+
+Selecting `Window->General->Test Runner` from the drop-down menu at the top will open a panel which displays any Tests defined for the Project. 
+
+![](images/test.png)
+
+This is an example integration test which, when executed, will open a simple scene with a few waypoints defined in the hierarchy.  The test script will publish each waypoint in order, as goal poses, and evaluate "Success" based on the Turtlebot's ability to navigate to each waypoint within the time limit. To see this test execute, ensure you have freshly launched ros2 environment (`ros2 launch unity_slam_example unity_slam_example.py`) and then simply double-click the test in the Test Runner panel.  The source code for this test is located in `Assets/Scripts/Tests/PlayMode/WaypointIntegrationTest.cs`.
+
+ ---
+
 ### Learn more about this Unity Scene
 
 For more information about how the different components in this simulation function, and how the ROS2 environment is set up, we have a separate [page](explanation.md) that goes into more granular detail.