From 772d21bc033193eab238e45b149ce076d984f304 Mon Sep 17 00:00:00 2001 From: Hamid Younesy Date: Thu, 22 Apr 2021 16:43:08 -0700 Subject: [PATCH 01/12] recreated dev-sensors From 9e6e356f0e6e16b1fc15f69b2f213399022339e2 Mon Sep 17 00:00:00 2001 From: vidurvij-Unity <60901103+vidurvij-Unity@users.noreply.github.com> Date: Fri, 7 May 2021 11:43:43 -0700 Subject: [PATCH 02/12] Vidur/sensor parse (#89) * Adding sensor parser and test * Added empty element to the test file * Add code blocks * Adding test XML in code * Adding value equivalence tests * Update names according to naming convenetion * Format and test updates * Adding space for loop code blocks * Formatting update --- .../Runtime/RosSharpDefinitions/Robot.cs | 10 ++ .../Runtime/RosSharpDefinitions/Sensor.cs | 60 +++++++++ .../RosSharpDefinitions/Sensor.cs.meta | 11 ++ .../Tests/Runtime/SensorTests.cs | 123 ++++++++++++++++++ .../Tests/Runtime/SensorTests.cs.meta | 11 ++ 5 files changed, 215 insertions(+) create mode 100644 com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Sensor.cs create mode 100644 com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Sensor.cs.meta create mode 100644 com.unity.robotics.urdf-importer/Tests/Runtime/SensorTests.cs create mode 100644 com.unity.robotics.urdf-importer/Tests/Runtime/SensorTests.cs.meta diff --git a/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Robot.cs b/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Robot.cs index c8388f1d..39840d40 100644 --- a/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Robot.cs +++ b/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Robot.cs @@ -32,6 +32,7 @@ public class Robot public List links; public List joints; + public List sensors; public List plugins; public List> ignoreCollisionPair; @@ -48,6 +49,7 @@ public Robot(string filename) links = ReadLinks(node); joints = ReadJoints(node); plugins = ReadPlugins(node); + sensors = ReadSensors(node); ignoreCollisionPair = ReadDisableCollision(node); @@ -107,6 +109,14 @@ from child in node.Elements("joint") return joints.ToList(); } + private static List ReadSensors(XElement node) + { + var sensors = + from child in node.Elements("sensor") + select new Sensor(child); + return sensors.ToList(); + } + private List ReadPlugins(XElement node) { var plugins = diff --git a/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Sensor.cs b/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Sensor.cs new file mode 100644 index 00000000..cb3d6d2f --- /dev/null +++ b/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Sensor.cs @@ -0,0 +1,60 @@ +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Threading; +using System.Xml; +using System.Xml.Linq; +using System; +using System.Numerics; + + +namespace RosSharp.Urdf +{ + public class Sensor + { + public string name { get; set; } + public string type { get; set; } + public string topic { get; set; } + private static string k_BaseKey = "sensor"; + private static string k_AttributeDelimit = "@"; + private static string k_ElementDelimit = "/"; + public int updateRate { get; set; } + public Dictionary elements { get; set; } + + public Sensor(XElement node) + { + elements = new Dictionary(); + foreach (XAttribute attribute in node.Attributes()) + { + AddAttribute(attribute, k_BaseKey); + } + foreach (XElement element in node.Elements()) + { + AddElement(element,k_BaseKey); + } + } + + public void AddAttribute(XAttribute attribute, string key) + { + string currentKey = key + k_AttributeDelimit + attribute.Name; + elements.Add(currentKey, attribute.Value); + } + + public void AddElement(XElement element, string key) + { + string currentKey = key + k_ElementDelimit + element.Name; + if (element.Elements().Count() == 0 && !(element.Value == "")) + { + elements.Add(currentKey,element.Value); + } + foreach (XAttribute attribute in element.Attributes()) + { + AddAttribute(attribute, currentKey); + } + foreach (XElement ele in element.Elements()) + { + AddElement(ele,currentKey); + } + } + } +} diff --git a/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Sensor.cs.meta b/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Sensor.cs.meta new file mode 100644 index 00000000..c08b9a31 --- /dev/null +++ b/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Sensor.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 73a330672d3fd4a8d90e405ac776b0bd +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.urdf-importer/Tests/Runtime/SensorTests.cs b/com.unity.robotics.urdf-importer/Tests/Runtime/SensorTests.cs new file mode 100644 index 00000000..d53d8591 --- /dev/null +++ b/com.unity.robotics.urdf-importer/Tests/Runtime/SensorTests.cs @@ -0,0 +1,123 @@ +using System.Collections; +using System.IO; +using System.Collections.Generic; +using NUnit.Framework; +using UnityEngine; +using UnityEngine.TestTools; +using RosSharp.Urdf; +using System.Xml; +using System.Xml.Linq; + + +namespace RosSharp.Urdf.Sensors.Test +{ + public class SensorTests + { + // A Test behaves as an ordinary method + private TextReader GetSample() + { + string sampleDoc = "\n" + + " \n" + + " \n" + + " /tmp/camera_save_tutorial\n" + + " \n" + + " 1.047\n" + + " \n" + + " 1920\n" + + " 1080\n" + + " \n" + + " \n" + + " 0.1\n" + + " 100\n" + + " \n" + + " \n" + + " 1\n" + + " 30\n" + + " \n" + + " \n" + + "\n"; + return new StringReader(sampleDoc); + } + [Test] + public void TotalDataTest() + { + + int totalData = 13; + XDocument xdoc = XDocument.Load(GetSample()); + Sensor sensor = new Sensor(xdoc.Element("sensor")); + Assert.AreEqual(totalData,sensor.elements.Count); + } + + [Test] + public void NumberofAttributeTest() + { + int totalAttributes = 5; + int numberofAttributes = 0; + XDocument xdoc = XDocument.Load(GetSample()); + Sensor sensor = new Sensor(xdoc.Element("sensor")); + foreach (var key in sensor.elements.Keys) + { + if (key.Contains("@")) + { + numberofAttributes++; + } + } + Assert.AreEqual(totalAttributes , numberofAttributes); + } + + [Test] + public void NumberofElementsTest() + { + int totalElements = 8; + int numberofElements = 0; + XDocument xdoc = XDocument.Load(GetSample()); + Sensor sensor = new Sensor(xdoc.Element("sensor")); + foreach (var key in sensor.elements.Keys) + { + if (!(key.Contains("@"))) + { + numberofElements++; + } + } + Assert.AreEqual(totalElements , numberofElements); + } + + [Test] + public void NoEmptyValue() + { + int emptyValue = 0; + XDocument xdoc = XDocument.Load(GetSample()); + Sensor sensor = new Sensor(xdoc.Element("sensor")); + foreach (var value in sensor.elements.Values) + { + if (value == "" || value == null) + { + emptyValue++; + } + } + Assert.AreEqual(emptyValue , 0); + } + + [Test] + public void CorrectAttributeValues() + { + XDocument xdoc = XDocument.Load(GetSample()); + XElement sensor = xdoc.Element("sensor"); + Sensor testSensor = new Sensor(sensor); + Assert.AreEqual(testSensor.elements["sensor@name"],"my_camera"); + Assert.AreEqual(testSensor.elements["sensor@type"],"camera"); + Assert.AreEqual(testSensor.elements["sensor/camera/save@enabled"],"true"); + Assert.AreEqual(testSensor.elements["sensor/camera/save/path"],"/tmp/camera_save_tutorial"); + Assert.AreEqual(testSensor.elements["sensor/camera/horizontal_fov"],"1.047"); + Assert.AreEqual(testSensor.elements["sensor/camera/image/width"],"1920"); + Assert.AreEqual(testSensor.elements["sensor/camera/image/height"],"1080"); + Assert.AreEqual(testSensor.elements["sensor/camera/clip/near"],"0.1"); + Assert.AreEqual(testSensor.elements["sensor/camera/clip/far"],"100"); + Assert.AreEqual(testSensor.elements["sensor/always_on"],"1"); + Assert.AreEqual(testSensor.elements["sensor/update_rate"],"30"); + Assert.AreEqual(testSensor.elements["sensor/plugin@name"],"test_plugin"); + Assert.AreEqual(testSensor.elements["sensor/plugin@filename"],"test_filename"); + + } + } +} diff --git a/com.unity.robotics.urdf-importer/Tests/Runtime/SensorTests.cs.meta b/com.unity.robotics.urdf-importer/Tests/Runtime/SensorTests.cs.meta new file mode 100644 index 00000000..f9e795a3 --- /dev/null +++ b/com.unity.robotics.urdf-importer/Tests/Runtime/SensorTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 437821fecd5504c429617bb469e6bdf1 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: From eebe9d2d90ffdd56e45a91c4d9aba1530840a2aa Mon Sep 17 00:00:00 2001 From: vidurvij-Unity Date: Mon, 7 Jun 2021 16:45:55 -0700 Subject: [PATCH 03/12] Adding systemgraph and sensors --- .gitmodules | 12 +++ TestUrdfImporter/Assets/Test URDF export.meta | 8 ++ .../Assets/fetch_description.meta | 8 ++ .../Assets/fetch_description/robots.meta | 8 ++ .../franka_panda_description-master.meta | 8 ++ TestUrdfImporter/Assets/turtlebot3.meta | 8 ++ .../turtlebot3/turtlebot3_description.meta | 8 ++ TestUrdfImporter/Assets/ur3_with_gripper.meta | 8 ++ .../ur3_with_gripper/ur_description.meta | 8 ++ .../ur_description/meshes.meta | 8 ++ TestUrdfImporter/Assets/urdf.meta | 8 ++ TestUrdfImporter/Packages/Mechatronics | 1 + TestUrdfImporter/Packages/ROS-TCP-Connector | 1 + TestUrdfImporter/Packages/RoboticsSensors | 1 + TestUrdfImporter/Packages/URDF-Importer | 1 + TestUrdfImporter/Packages/manifest.json | 18 ++-- TestUrdfImporter/Packages/packages-lock.json | 96 +++++++++++-------- .../Settings.json | 7 ++ .../ProjectSettings/ProjectSettings.asset | 15 ++- .../ProjectSettings/ProjectVersion.txt | 4 +- .../ProjectSettings/URPProjectSettings.asset | 2 +- 21 files changed, 185 insertions(+), 53 deletions(-) create mode 100644 .gitmodules create mode 100644 TestUrdfImporter/Assets/Test URDF export.meta create mode 100644 TestUrdfImporter/Assets/fetch_description.meta create mode 100644 TestUrdfImporter/Assets/fetch_description/robots.meta create mode 100644 TestUrdfImporter/Assets/franka_panda_description-master.meta create mode 100644 TestUrdfImporter/Assets/turtlebot3.meta create mode 100644 TestUrdfImporter/Assets/turtlebot3/turtlebot3_description.meta create mode 100644 TestUrdfImporter/Assets/ur3_with_gripper.meta create mode 100644 TestUrdfImporter/Assets/ur3_with_gripper/ur_description.meta create mode 100644 TestUrdfImporter/Assets/ur3_with_gripper/ur_description/meshes.meta create mode 100644 TestUrdfImporter/Assets/urdf.meta create mode 160000 TestUrdfImporter/Packages/Mechatronics create mode 160000 TestUrdfImporter/Packages/ROS-TCP-Connector create mode 160000 TestUrdfImporter/Packages/RoboticsSensors create mode 160000 TestUrdfImporter/Packages/URDF-Importer create mode 100644 TestUrdfImporter/ProjectSettings/Packages/com.unity.testtools.codecoverage/Settings.json diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 00000000..5caa152b --- /dev/null +++ b/.gitmodules @@ -0,0 +1,12 @@ +[submodule "TestUrdfImporter/Packages/URDF-Importer"] + path = TestUrdfImporter/Packages/URDF-Importer + url = https://github.com/Unity-Technologies/URDF-Importer.git +[submodule "TestUrdfImporter/Packages/RoboticsSensors"] + path = TestUrdfImporter/Packages/RoboticsSensors + url = git@github.cds.internal.unity3d.com:unity/RoboticsSensors.git +[submodule "TestUrdfImporter/Packages/ROS-TCP-Connector"] + path = TestUrdfImporter/Packages/ROS-TCP-Connector + url = https://github.com/Unity-Technologies/ROS-TCP-Connector +[submodule "TestUrdfImporter/Packages/Mechatronics"] + path = TestUrdfImporter/Packages/Mechatronics + url = git@github.cds.internal.unity3d.com:unity/Mechatronics.git diff --git a/TestUrdfImporter/Assets/Test URDF export.meta b/TestUrdfImporter/Assets/Test URDF export.meta new file mode 100644 index 00000000..4caf8445 --- /dev/null +++ b/TestUrdfImporter/Assets/Test URDF export.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: a51b350ae06a149eb9ae6de042800922 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/TestUrdfImporter/Assets/fetch_description.meta b/TestUrdfImporter/Assets/fetch_description.meta new file mode 100644 index 00000000..f4e7802c --- /dev/null +++ b/TestUrdfImporter/Assets/fetch_description.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: ad7aef710df90482aad37204e6ec99c8 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/TestUrdfImporter/Assets/fetch_description/robots.meta b/TestUrdfImporter/Assets/fetch_description/robots.meta new file mode 100644 index 00000000..051a3cc4 --- /dev/null +++ b/TestUrdfImporter/Assets/fetch_description/robots.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0a8089b559f304f789bc56276704d0d0 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/TestUrdfImporter/Assets/franka_panda_description-master.meta b/TestUrdfImporter/Assets/franka_panda_description-master.meta new file mode 100644 index 00000000..7d637b9c --- /dev/null +++ b/TestUrdfImporter/Assets/franka_panda_description-master.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: b9cc269490bde4ac38f6ab7595edee29 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/TestUrdfImporter/Assets/turtlebot3.meta b/TestUrdfImporter/Assets/turtlebot3.meta new file mode 100644 index 00000000..bd415ad4 --- /dev/null +++ b/TestUrdfImporter/Assets/turtlebot3.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: feed3a2414df041568e19c941b769a20 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/TestUrdfImporter/Assets/turtlebot3/turtlebot3_description.meta b/TestUrdfImporter/Assets/turtlebot3/turtlebot3_description.meta new file mode 100644 index 00000000..b0fe64a8 --- /dev/null +++ b/TestUrdfImporter/Assets/turtlebot3/turtlebot3_description.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: a6212a5f530ae49c0ba44ebff2a0a9f7 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/TestUrdfImporter/Assets/ur3_with_gripper.meta b/TestUrdfImporter/Assets/ur3_with_gripper.meta new file mode 100644 index 00000000..f717caae --- /dev/null +++ b/TestUrdfImporter/Assets/ur3_with_gripper.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: bdd20656863574499a2f5243015e2a9b +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/TestUrdfImporter/Assets/ur3_with_gripper/ur_description.meta b/TestUrdfImporter/Assets/ur3_with_gripper/ur_description.meta new file mode 100644 index 00000000..9c5d88c9 --- /dev/null +++ b/TestUrdfImporter/Assets/ur3_with_gripper/ur_description.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 074f158bd1506474f8e2883808b74d50 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/TestUrdfImporter/Assets/ur3_with_gripper/ur_description/meshes.meta b/TestUrdfImporter/Assets/ur3_with_gripper/ur_description/meshes.meta new file mode 100644 index 00000000..c0b16083 --- /dev/null +++ b/TestUrdfImporter/Assets/ur3_with_gripper/ur_description/meshes.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 430a18699e46549d8b16905ad991de8a +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/TestUrdfImporter/Assets/urdf.meta b/TestUrdfImporter/Assets/urdf.meta new file mode 100644 index 00000000..f26ce948 --- /dev/null +++ b/TestUrdfImporter/Assets/urdf.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 28ebdaa059f274e3591ee47d45db65f4 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/TestUrdfImporter/Packages/Mechatronics b/TestUrdfImporter/Packages/Mechatronics new file mode 160000 index 00000000..b68fa34b --- /dev/null +++ b/TestUrdfImporter/Packages/Mechatronics @@ -0,0 +1 @@ +Subproject commit b68fa34be9912dcfd5ea81108b5c097e687f23af diff --git a/TestUrdfImporter/Packages/ROS-TCP-Connector b/TestUrdfImporter/Packages/ROS-TCP-Connector new file mode 160000 index 00000000..ab45157a --- /dev/null +++ b/TestUrdfImporter/Packages/ROS-TCP-Connector @@ -0,0 +1 @@ +Subproject commit ab45157ad3c11a5bce500ec181c2472895cd54dd diff --git a/TestUrdfImporter/Packages/RoboticsSensors b/TestUrdfImporter/Packages/RoboticsSensors new file mode 160000 index 00000000..5ab1bf54 --- /dev/null +++ b/TestUrdfImporter/Packages/RoboticsSensors @@ -0,0 +1 @@ +Subproject commit 5ab1bf5430295ade480b0a29b57e284438811b02 diff --git a/TestUrdfImporter/Packages/URDF-Importer b/TestUrdfImporter/Packages/URDF-Importer new file mode 160000 index 00000000..d90212e1 --- /dev/null +++ b/TestUrdfImporter/Packages/URDF-Importer @@ -0,0 +1 @@ +Subproject commit d90212e1cde3900f3dad80a975a882b87a908dc7 diff --git a/TestUrdfImporter/Packages/manifest.json b/TestUrdfImporter/Packages/manifest.json index 992f3529..c6cb739e 100644 --- a/TestUrdfImporter/Packages/manifest.json +++ b/TestUrdfImporter/Packages/manifest.json @@ -1,15 +1,17 @@ { "dependencies": { - "com.unity.collab-proxy": "1.3.9", - "com.unity.ide.rider": "2.0.7", - "com.unity.ide.visualstudio": "2.0.3", - "com.unity.ide.vscode": "1.2.2", - "com.unity.render-pipelines.universal": "10.0.0-preview.26", + "com.unity.collab-proxy": "1.5.7", + "com.unity.ide.rider": "3.0.6", + "com.unity.ide.visualstudio": "2.0.8", + "com.unity.ide.vscode": "1.2.3", + "com.unity.robotics.sensors": "file:/Users/vidur.vij/Desktop/RoboticsSensors/com.unity.robotics.sensors", "com.unity.robotics.urdf-importer": "file:../../com.unity.robotics.urdf-importer", - "com.unity.test-framework": "1.1.20", + "com.unity.systemgraph": "file:Mechatronics/com.unity.systemgraph", + "com.unity.systemgraph.authoring": "file:Mechatronics/com.unity.systemgraph.authoring", + "com.unity.test-framework": "1.1.24", "com.unity.testtools.codecoverage": "1.0.0", - "com.unity.textmeshpro": "3.0.1", - "com.unity.timeline": "1.4.3", + "com.unity.textmeshpro": "3.0.6", + "com.unity.timeline": "1.5.5", "com.unity.ugui": "1.0.0", "com.unity.modules.ai": "1.0.0", "com.unity.modules.androidjni": "1.0.0", diff --git a/TestUrdfImporter/Packages/packages-lock.json b/TestUrdfImporter/Packages/packages-lock.json index 5cf2e780..4eb89bf1 100644 --- a/TestUrdfImporter/Packages/packages-lock.json +++ b/TestUrdfImporter/Packages/packages-lock.json @@ -1,10 +1,24 @@ { "dependencies": { + "com.unity.addressables": { + "version": "1.16.19", + "depth": 1, + "source": "registry", + "dependencies": { + "com.unity.scriptablebuildpipeline": "1.15.2", + "com.unity.modules.assetbundle": "1.0.0", + "com.unity.modules.unitywebrequest": "1.0.0", + "com.unity.modules.unitywebrequestassetbundle": "1.0.0" + }, + "url": "https://packages.unity.com" + }, "com.unity.collab-proxy": { - "version": "1.3.9", + "version": "1.5.7", "depth": 0, "source": "registry", - "dependencies": {}, + "dependencies": { + "com.unity.nuget.newtonsoft-json": "2.0.0" + }, "url": "https://packages.unity.com" }, "com.unity.editorcoroutines": { @@ -22,54 +36,52 @@ "url": "https://packages.unity.com" }, "com.unity.ide.rider": { - "version": "2.0.7", + "version": "3.0.6", "depth": 0, "source": "registry", "dependencies": { - "com.unity.test-framework": "1.1.1" + "com.unity.ext.nunit": "1.0.6" }, "url": "https://packages.unity.com" }, "com.unity.ide.visualstudio": { - "version": "2.0.3", + "version": "2.0.8", "depth": 0, "source": "registry", - "dependencies": {}, + "dependencies": { + "com.unity.test-framework": "1.1.9" + }, "url": "https://packages.unity.com" }, "com.unity.ide.vscode": { - "version": "1.2.2", + "version": "1.2.3", "depth": 0, "source": "registry", "dependencies": {}, "url": "https://packages.unity.com" }, - "com.unity.mathematics": { - "version": "1.1.0", + "com.unity.nuget.newtonsoft-json": { + "version": "2.0.0", "depth": 1, "source": "registry", "dependencies": {}, "url": "https://packages.unity.com" }, - "com.unity.render-pipelines.core": { - "version": "10.0.0-preview.30", - "depth": 1, - "source": "registry", - "dependencies": { - "com.unity.ugui": "1.0.0" - }, - "url": "https://packages.unity.com" + "com.unity.robotics.ros-tcp-connector": { + "version": "file:ROS-TCP-Connector", + "depth": 0, + "source": "embedded", + "dependencies": {} }, - "com.unity.render-pipelines.universal": { - "version": "10.0.0-preview.26", + "com.unity.robotics.sensors": { + "version": "file:/Users/vidur.vij/Desktop/RoboticsSensors/com.unity.robotics.sensors", "depth": 0, - "source": "registry", + "source": "local", "dependencies": { - "com.unity.mathematics": "1.1.0", - "com.unity.render-pipelines.core": "10.0.0-preview.30", - "com.unity.shadergraph": "10.0.0-preview.27" - }, - "url": "https://packages.unity.com" + "com.unity.addressables": "1.6.2", + "com.unity.systemgraph": "0.0.1-preview.2", + "com.unity.robotics.ros-tcp-connector": "0.1.2-preview" + } }, "com.unity.robotics.urdf-importer": { "version": "file:../../com.unity.robotics.urdf-importer", @@ -79,32 +91,38 @@ "com.unity.editorcoroutines": "1.0.0" } }, - "com.unity.searcher": { - "version": "4.3.1", + "com.unity.scriptablebuildpipeline": { + "version": "1.15.2", "depth": 2, "source": "registry", "dependencies": {}, "url": "https://packages.unity.com" }, "com.unity.settings-manager": { - "version": "1.0.1", + "version": "1.0.3", "depth": 1, "source": "registry", "dependencies": {}, "url": "https://packages.unity.com" }, - "com.unity.shadergraph": { - "version": "10.0.0-preview.27", - "depth": 1, - "source": "registry", + "com.unity.systemgraph": { + "version": "file:Mechatronics/com.unity.systemgraph", + "depth": 0, + "source": "local", "dependencies": { - "com.unity.render-pipelines.core": "10.0.0-preview.30", - "com.unity.searcher": "4.3.1" - }, - "url": "https://packages.unity.com" + "com.unity.addressables": "1.6.2" + } + }, + "com.unity.systemgraph.authoring": { + "version": "file:Mechatronics/com.unity.systemgraph.authoring", + "depth": 0, + "source": "local", + "dependencies": { + "com.unity.systemgraph": "1.0.0-exp.3" + } }, "com.unity.test-framework": { - "version": "1.1.20", + "version": "1.1.24", "depth": 0, "source": "registry", "dependencies": { @@ -125,7 +143,7 @@ "url": "https://packages.unity.com" }, "com.unity.textmeshpro": { - "version": "3.0.1", + "version": "3.0.6", "depth": 0, "source": "registry", "dependencies": { @@ -134,7 +152,7 @@ "url": "https://packages.unity.com" }, "com.unity.timeline": { - "version": "1.4.3", + "version": "1.5.5", "depth": 0, "source": "registry", "dependencies": { diff --git a/TestUrdfImporter/ProjectSettings/Packages/com.unity.testtools.codecoverage/Settings.json b/TestUrdfImporter/ProjectSettings/Packages/com.unity.testtools.codecoverage/Settings.json new file mode 100644 index 00000000..ad11087f --- /dev/null +++ b/TestUrdfImporter/ProjectSettings/Packages/com.unity.testtools.codecoverage/Settings.json @@ -0,0 +1,7 @@ +{ + "m_Name": "Settings", + "m_Path": "ProjectSettings/Packages/com.unity.testtools.codecoverage/Settings.json", + "m_Dictionary": { + "m_DictionaryValues": [] + } +} \ No newline at end of file diff --git a/TestUrdfImporter/ProjectSettings/ProjectSettings.asset b/TestUrdfImporter/ProjectSettings/ProjectSettings.asset index b16d1f09..3b9044c9 100644 --- a/TestUrdfImporter/ProjectSettings/ProjectSettings.asset +++ b/TestUrdfImporter/ProjectSettings/ProjectSettings.asset @@ -3,7 +3,7 @@ --- !u!129 &1 PlayerSettings: m_ObjectHideFlags: 0 - serializedVersion: 21 + serializedVersion: 22 productGUID: 3f43b7478172b48c28d431d637aa5f82 AndroidProfiler: 0 AndroidFilterTouchesWhenObscured: 0 @@ -350,6 +350,7 @@ PlayerSettings: switchScreenResolutionBehavior: 2 switchUseCPUProfiler: 0 switchUseGOLDLinker: 0 + switchLTOSetting: 0 switchApplicationID: 0x01004b9000490000 switchNSODependencies: switchTitleNames_0: @@ -367,6 +368,7 @@ PlayerSettings: switchTitleNames_12: switchTitleNames_13: switchTitleNames_14: + switchTitleNames_15: switchPublisherNames_0: switchPublisherNames_1: switchPublisherNames_2: @@ -382,6 +384,7 @@ PlayerSettings: switchPublisherNames_12: switchPublisherNames_13: switchPublisherNames_14: + switchPublisherNames_15: switchIcons_0: {fileID: 0} switchIcons_1: {fileID: 0} switchIcons_2: {fileID: 0} @@ -397,6 +400,7 @@ PlayerSettings: switchIcons_12: {fileID: 0} switchIcons_13: {fileID: 0} switchIcons_14: {fileID: 0} + switchIcons_15: {fileID: 0} switchSmallIcons_0: {fileID: 0} switchSmallIcons_1: {fileID: 0} switchSmallIcons_2: {fileID: 0} @@ -412,6 +416,7 @@ PlayerSettings: switchSmallIcons_12: {fileID: 0} switchSmallIcons_13: {fileID: 0} switchSmallIcons_14: {fileID: 0} + switchSmallIcons_15: {fileID: 0} switchManualHTML: switchAccessibleURLs: switchLegalInformation: @@ -475,6 +480,8 @@ PlayerSettings: switchNetworkInterfaceManagerInitializeEnabled: 1 switchPlayerConnectionEnabled: 1 switchUseNewStyleFilepaths: 0 + switchUseMicroSleepForYield: 1 + switchMicroSleepForYieldTime: 25 ps4NPAgeRating: 12 ps4NPTitleSecret: ps4NPTrophyPackPath: @@ -580,7 +587,8 @@ PlayerSettings: webGLLinkerTarget: 1 webGLThreadsSupport: 0 webGLDecompressionFallback: 0 - scriptingDefineSymbols: {} + scriptingDefineSymbols: + 1: UNITY_CCU;ALLOW_AUTHORING additionalCompilerArguments: {} platformArchitecture: {} scriptingBackend: @@ -591,11 +599,11 @@ PlayerSettings: suppressCommonWarnings: 1 allowUnsafeCode: 0 useDeterministicCompilation: 1 - useReferenceAssemblies: 1 enableRoslynAnalyzers: 1 additionalIl2CppArgs: scriptingRuntimeVersion: 1 gcIncremental: 1 + assemblyVersionValidation: 1 gcWBarrierValidation: 0 apiCompatibilityLevelPerPlatform: Standalone: 3 @@ -645,6 +653,7 @@ PlayerSettings: XboxOneCapability: [] XboxOneGameRating: {} XboxOneIsContentPackage: 0 + XboxOneEnhancedXboxCompatibilityMode: 0 XboxOneEnableGPUVariability: 1 XboxOneSockets: {} XboxOneSplashScreen: {fileID: 0} diff --git a/TestUrdfImporter/ProjectSettings/ProjectVersion.txt b/TestUrdfImporter/ProjectSettings/ProjectVersion.txt index ccec5356..f015b2d3 100644 --- a/TestUrdfImporter/ProjectSettings/ProjectVersion.txt +++ b/TestUrdfImporter/ProjectSettings/ProjectVersion.txt @@ -1,2 +1,2 @@ -m_EditorVersion: 2020.2.0b9 -m_EditorVersionWithRevision: 2020.2.0b9 (ef2968fa77ae) +m_EditorVersion: 2021.1.9f1 +m_EditorVersionWithRevision: 2021.1.9f1 (7a790e367ab3) diff --git a/TestUrdfImporter/ProjectSettings/URPProjectSettings.asset b/TestUrdfImporter/ProjectSettings/URPProjectSettings.asset index 91791f36..3077404f 100644 --- a/TestUrdfImporter/ProjectSettings/URPProjectSettings.asset +++ b/TestUrdfImporter/ProjectSettings/URPProjectSettings.asset @@ -12,4 +12,4 @@ MonoBehaviour: m_Script: {fileID: 11500000, guid: 247994e1f5a72c2419c26a37e9334c01, type: 3} m_Name: m_EditorClassIdentifier: - m_LastMaterialVersion: 2 + m_LastMaterialVersion: 4 From bf31303edb9668351bed1210182a419158c28e83 Mon Sep 17 00:00:00 2001 From: vidurvij-Unity Date: Tue, 13 Jul 2021 15:31:34 -0700 Subject: [PATCH 04/12] Change Sensor namespace --- com.unity.robotics.urdf-importer.meta | 3 +++ .../Runtime/RosSharpDefinitions/Sensor.cs | 2 +- com.unity.robotics.urdf-importer/Tests/Runtime/SensorTests.cs | 4 ++-- 3 files changed, 6 insertions(+), 3 deletions(-) create mode 100644 com.unity.robotics.urdf-importer.meta diff --git a/com.unity.robotics.urdf-importer.meta b/com.unity.robotics.urdf-importer.meta new file mode 100644 index 00000000..2bced377 --- /dev/null +++ b/com.unity.robotics.urdf-importer.meta @@ -0,0 +1,3 @@ +fileFormatVersion: 2 +guid: 362b4d503012461693396f3dca41e51e +timeCreated: 1626215050 \ No newline at end of file diff --git a/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Sensor.cs b/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Sensor.cs index cb3d6d2f..f418a622 100644 --- a/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Sensor.cs +++ b/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Sensor.cs @@ -8,7 +8,7 @@ using System.Numerics; -namespace RosSharp.Urdf +namespace Unity.Robotics.UrdfImporter { public class Sensor { diff --git a/com.unity.robotics.urdf-importer/Tests/Runtime/SensorTests.cs b/com.unity.robotics.urdf-importer/Tests/Runtime/SensorTests.cs index d53d8591..8417f796 100644 --- a/com.unity.robotics.urdf-importer/Tests/Runtime/SensorTests.cs +++ b/com.unity.robotics.urdf-importer/Tests/Runtime/SensorTests.cs @@ -4,12 +4,12 @@ using NUnit.Framework; using UnityEngine; using UnityEngine.TestTools; -using RosSharp.Urdf; +using Unity.Robotics.UrdfImporter; using System.Xml; using System.Xml.Linq; -namespace RosSharp.Urdf.Sensors.Test +namespace Unity.Robotics.UrdfImporter.Sensors.Test { public class SensorTests { From bb2b08cfb714d0b6b964953c65014b2bd13bfe74 Mon Sep 17 00:00:00 2001 From: vidurvij-Unity <60901103+vidurvij-Unity@users.noreply.github.com> Date: Tue, 14 Sep 2021 11:01:31 -0700 Subject: [PATCH 05/12] Vidur/tf (#142) * Added Sensor import functionality * Adding play mode context * Adding editor and playmode context * Removing async code * Catching sensor Exception in URDF * Adding Default parameters in case inertia is missing * Updating namespace in sensor files * Add Import of transfromSensor * Adding joint sensor in robot root * Integrating transform sensor import * update naming * Remove compile errors * Correcting capitalization errors * Update AssimpNet.xml * Update Unity.Robotics.URDFImporter.asmdef * Formatting corrections * Update UrdfRobotExtensions.cs --- com.unity.robotics.urdf-importer.meta | 2 +- .../Runtime/Controller/Controller.cs | 13 +-- .../Runtime/Extensions/UrdfLinkExtensions.cs | 11 ++- .../Runtime/Extensions/UrdfRobotExtensions.cs | 90 ++++++++++++++----- .../Runtime/Extensions/UrdfSensorExtension.cs | 49 ++++++++++ .../Extensions/UrdfSensorExtension.cs.meta | 3 + .../Extensions/UrdfSensorsExtensions.cs | 52 +++++++++++ .../Extensions/UrdfSensorsExtensions.cs.meta | 11 +++ .../Runtime/RosSharpDefinitions/Link.cs | 10 +++ .../Runtime/RosSharpDefinitions/Robot.cs | 11 +-- .../Unity.Robotics.URDFImporter.asmdef | 3 +- .../Runtime/UrdfComponents/UrdfComparator.cs | 4 +- .../Runtime/UrdfComponents/UrdfInertial.cs | 15 ++++ .../UrdfComponents/UrdfJoints/UrdfJoint.cs | 18 ++-- .../UrdfJoints/UrdfJointContinuous.cs | 2 +- .../UrdfJoints/UrdfJointFloating.cs | 2 +- .../UrdfJoints/UrdfJointPlanar.cs | 4 +- .../UrdfJoints/UrdfJointPrismatic.cs | 6 +- .../UrdfJoints/UrdfJointRevolute.cs | 4 +- .../Runtime/UrdfComponents/UrdfSensor.cs | 9 ++ .../Runtime/UrdfComponents/UrdfSensor.cs.meta | 3 + .../Runtime/UrdfComponents/UrdfSensors.cs | 9 ++ .../UrdfComponents/UrdfSensors.cs.meta | 3 + .../Tests/Runtime/Assets/URDF/cube/cube.urdf | 3 + 24 files changed, 276 insertions(+), 61 deletions(-) create mode 100644 com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorExtension.cs create mode 100644 com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorExtension.cs.meta create mode 100644 com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs create mode 100644 com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs.meta create mode 100644 com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfSensor.cs create mode 100644 com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfSensor.cs.meta create mode 100644 com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfSensors.cs create mode 100644 com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfSensors.cs.meta diff --git a/com.unity.robotics.urdf-importer.meta b/com.unity.robotics.urdf-importer.meta index 2bced377..638ccbf7 100644 --- a/com.unity.robotics.urdf-importer.meta +++ b/com.unity.robotics.urdf-importer.meta @@ -1,3 +1,3 @@ fileFormatVersion: 2 guid: 362b4d503012461693396f3dca41e51e -timeCreated: 1626215050 \ No newline at end of file +timeCreated: 1626215050 diff --git a/com.unity.robotics.urdf-importer/Runtime/Controller/Controller.cs b/com.unity.robotics.urdf-importer/Runtime/Controller/Controller.cs index 5475d5a8..154350be 100644 --- a/com.unity.robotics.urdf-importer/Runtime/Controller/Controller.cs +++ b/com.unity.robotics.urdf-importer/Runtime/Controller/Controller.cs @@ -160,11 +160,14 @@ private void UpdateDirection(int jointIndex) /// Index of the part in the Articulation chain private void StoreJointColors(int index) { - Renderer[] materialLists = articulationChain[index].transform.GetChild(0).GetComponentsInChildren(); - prevColor = new Color[materialLists.Length]; - for (int counter = 0; counter < materialLists.Length; counter++) - { - prevColor[counter] = MaterialExtensions.GetMaterialColor(materialLists[counter]); + if (articulationChain.Length > 0) + { + Renderer[] materialLists = articulationChain[index].transform.GetChild(0).GetComponentsInChildren(); + prevColor = new Color[materialLists.Length]; + for (int counter = 0; counter < materialLists.Length; counter++) + { + prevColor[counter] = MaterialExtensions.GetMaterialColor(materialLists[counter]); + } } } diff --git a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfLinkExtensions.cs b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfLinkExtensions.cs index a6059af5..63bc64d4 100644 --- a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfLinkExtensions.cs +++ b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfLinkExtensions.cs @@ -23,9 +23,7 @@ public static GameObject Create(Transform parent, Link link = null, Joint joint GameObject linkObject = new GameObject("link"); linkObject.transform.SetParentAndAlign(parent); UrdfLink urdfLink = linkObject.AddComponent(); - UrdfVisualsExtensions.Create(linkObject.transform, link?.visuals); - UrdfCollisionsExtensions.Create(linkObject.transform, link?.collisions); - + if (link != null) { urdfLink.ImportLinkData(link, joint); @@ -37,6 +35,10 @@ public static GameObject Create(Transform parent, Link link = null, Joint joint UnityEditor.EditorGUIUtility.PingObject(linkObject); #endif } + + UrdfVisualsExtensions.Create(linkObject.transform, link?.visuals); + UrdfCollisionsExtensions.Create(linkObject.transform, link?.collisions); + UrdfSensorsExtensions.Create(linkObject.transform, link?.sensors); return linkObject; } @@ -59,7 +61,10 @@ private static void ImportLinkData(this UrdfLink urdfLink, Link link, Joint join UrdfJoint.Create(urdfLink.gameObject, UrdfJoint.GetJointType(joint.type), joint); } else if (joint != null) + { + UrdfInertial.Create(urdfLink.gameObject); UrdfJoint.Create(urdfLink.gameObject, UrdfJoint.GetJointType(joint.type), joint); + } } diff --git a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfRobotExtensions.cs b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfRobotExtensions.cs index afe22508..dd5916d3 100644 --- a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfRobotExtensions.cs +++ b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfRobotExtensions.cs @@ -16,6 +16,7 @@ limitations under the License. using System; using System.IO; using System.Linq; +using Unity.Robotics.Sensors; using Unity.Robotics.UrdfImporter.Control; #if UNITY_EDITOR using UnityEditor; @@ -71,7 +72,7 @@ private static ImportPipelineData ImportPipelineInit(string filename, ImportSett im.wasRuntimeMode = RuntimeUrdf.IsRuntimeMode(); im.forceRuntimeMode = forceRuntimeMode; - if (forceRuntimeMode) + if (forceRuntimeMode) { RuntimeUrdf.SetRuntimeMode(true); } @@ -81,20 +82,23 @@ private static ImportPipelineData ImportPipelineInit(string filename, ImportSett if (!UrdfAssetPathHandler.IsValidAssetPath(im.robot.filename)) { Debug.LogError("URDF file and ressources must be placed in Assets Folder:\n" + Application.dataPath); - if (forceRuntimeMode) - { // set runtime mode back to what it was + if (forceRuntimeMode) + { + // set runtime mode back to what it was RuntimeUrdf.SetRuntimeMode(im.wasRuntimeMode); } + return null; } + return im; } // Creates the robot game object. - private static void ImportPipelineCreateObject(ImportPipelineData im) + private static void ImportPipelineCreateObject(ImportPipelineData im) { im.robotGameObject = new GameObject(im.robot.name); - + importsettings = im.settings; im.settings.totalLinks = im.robot.links.Count; @@ -104,8 +108,9 @@ private static void ImportPipelineCreateObject(ImportPipelineData im) im.robotGameObject.AddComponent(); im.robotGameObject.AddComponent(); - if (RuntimeUrdf.IsRuntimeMode()) - {// In runtime mode, we have to disable controller while robot is being constructed. + if (RuntimeUrdf.IsRuntimeMode()) + { + // In runtime mode, we have to disable controller while robot is being constructed. im.robotGameObject.GetComponent().enabled = false; } @@ -114,17 +119,18 @@ private static void ImportPipelineCreateObject(ImportPipelineData im) UrdfAssetPathHandler.SetPackageRoot(Path.GetDirectoryName(im.robot.filename)); UrdfMaterial.InitializeRobotMaterials(im.robot); UrdfPlugins.Create(im.robotGameObject.transform, im.robot.plugins); + AddJointSensor(im.robotGameObject); } // Creates the stack of robot joints. Should be called iteratively until false is returned. private static bool ProcessJointStack(ImportPipelineData im) { - if (im.importStack == null) + if (im.importStack == null) { im.importStack = new Stack>(); im.importStack.Push(new Tuple(im.robot.root, im.robotGameObject.transform, null)); } - + if (im.importStack.Count != 0) { Tuple currentLink = im.importStack.Pop(); @@ -135,8 +141,10 @@ private static bool ProcessJointStack(ImportPipelineData im) Link child = childJoint.ChildLink; im.importStack.Push(new Tuple(child, importedLink.transform, childJoint)); } + return true; } + return false; } @@ -152,8 +160,9 @@ private static void ImportPipelinePostCreate(ImportPipelineData im) CorrectAxis(im.robotGameObject); CreateCollisionExceptions(im.robot, im.robotGameObject); - if (im.forceRuntimeMode) - { // set runtime mode back to what it was + if (im.forceRuntimeMode) + { + // set runtime mode back to what it was RuntimeUrdf.SetRuntimeMode(im.wasRuntimeMode); } } @@ -211,7 +220,8 @@ public static GameObject CreateRuntime(string filename, ImportSettings settings) ImportPipelineCreateObject(im); while (ProcessJointStack(im)) - {// process the stack until finished. + { + // process the stack until finished. } ImportPipelinePostCreate(im); @@ -222,7 +232,7 @@ public static GameObject CreateRuntime(string filename, ImportSettings settings) public static void CorrectAxis(GameObject robot) { UrdfRobot robotScript = robot.GetComponent(); - if (robotScript == null) + if (robotScript == null) { Debug.LogError("Robot has no UrdfRobot component attached. Abandon correcting axis"); return; @@ -232,6 +242,7 @@ public static void CorrectAxis(GameObject robot) { return; } + Quaternion correctYtoZ = Quaternion.Euler(-90, 0, 90); Quaternion correctZtoY = Quaternion.Inverse((correctYtoZ)); Quaternion correction = new Quaternion(); @@ -254,11 +265,12 @@ public static void CorrectAxis(GameObject robot) foreach (UrdfCollision collision in collisionMeshList) { - if (robotScript.choosenAxis != ImportSettings.axisType.zAxis) + if (robotScript.choosenAxis != ImportSettings.axisType.zAxis) { collision.transform.localRotation = collision.transform.localRotation * correction; } } + robotScript.SetOrientation(); } @@ -275,6 +287,7 @@ private static void CreateCollisionExceptions(Robot robot, GameObject robotGameO CollisionList.Add(new CollisionIgnore(collisionObject1, collisionObject2)); } } + robotGameObject.GetComponent().collisionExceptions = CollisionList; } @@ -322,16 +335,19 @@ private static Robot ExportRobotData(this UrdfRobot urdfRobot) "Ok"); return null; } + robot.links.Add(urdfLink.ExportLinkData()); linkNames.Add(urdfLink.name); - //Joint export + //Joints export UrdfJoint urdfJoint = urdfLink.gameObject.GetComponent(); if (urdfJoint != null) robot.joints.Add(urdfJoint.ExportJointData()); else if (!urdfLink.IsBaseLink) + { //Make sure that links with no rigidbodies are still connected to the robot by a default joint robot.joints.Add(UrdfJoint.ExportDefaultJoint(urdfLink.transform)); + } } robot.materials = UrdfMaterial.Materials.Values.ToList(); @@ -368,8 +384,8 @@ public static void CreateTag() SerializedProperty t = tagsProp.GetArrayElementAtIndex(i); if (t.stringValue.Equals(FKRobot.k_TagName)) { - found = true; - break; + found = true; + break; } } @@ -381,7 +397,7 @@ public static void CreateTag() n.stringValue = FKRobot.k_TagName; } - tagManager.ApplyModifiedProperties(); + tagManager.ApplyModifiedProperties(); #endif } @@ -393,11 +409,11 @@ static void SetTag(GameObject go) } catch (Exception) { - Debug.LogError($"Unable to find tag '{FKRobot.k_TagName}'." + - $"Add a tag '{FKRobot.k_TagName}' in the Project Settings in Unity Editor."); + Debug.LogError($"Unable to find tag '{FKRobot.k_TagName}'." + + $"Add a tag '{FKRobot.k_TagName}' in the Project Settings in Unity Editor."); return; } - + if (!go) return; @@ -410,5 +426,37 @@ static void SetTag(GameObject go) Debug.LogError($"Unable to set the GameObject '{go.name}' tag to '{FKRobot.k_TagName}'."); } } + + static void AddJointSensor(GameObject robot) + { + Dictionary settings = new Dictionary { { "sensor/topic", robot.name + "/JointState" } }; + SensorFactory.InstantiateSensor("joint", settings).transform.SetParentAndAlign(robot.transform); + + static void SetTag(GameObject go) + { + try + { + GameObject.FindWithTag(FKRobot.k_TagName); + } + catch (Exception) + { + Debug.LogError($"Unable to find tag '{FKRobot.k_TagName}'." + + $"Add a tag '{FKRobot.k_TagName}' in the Project Settings in Unity Editor."); + return; + } + + if (!go) + return; + + try + { + go.tag = FKRobot.k_TagName; + } + catch (Exception) + { + Debug.LogError($"Unable to set the GameObject '{go.name}' tag to '{FKRobot.k_TagName}'."); + } + } + } } } diff --git a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorExtension.cs b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorExtension.cs new file mode 100644 index 00000000..c807e3ac --- /dev/null +++ b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorExtension.cs @@ -0,0 +1,49 @@ +using System; +using UnityEngine; +using Unity.Robotics.Sensors; + +namespace Unity.Robotics.UrdfImporter +{ + public static class UrdfSensorExtension + { + const string k_PoseKey = "sensor/pose"; + const string k_NameKey = "sensor@name"; + const string k_TypeKey = "sensor@type"; + const string k_PluginKey = "sensor/plugin@name"; + public static UrdfSensor Create(Transform parent, Sensor sensor) + { + GameObject sensorObject = new GameObject("unnamed"); + sensorObject.transform.SetParentAndAlign(parent); + UrdfSensor urdfSensor = sensorObject.AddComponent(); + urdfSensor.sensorType = sensor.elements[k_TypeKey]; + ImportSensorData(sensorObject.transform, sensor); + return urdfSensor; + } + + static void ImportSensorData(Transform sensorObject, Sensor sensor) + { + if (sensor.elements.ContainsKey(k_PoseKey)) + { + string originString = sensor.elements[k_PoseKey]; + string[] originPose = originString.Split(' '); + double[] xyz = new[] { Convert.ToDouble(originPose[0]), Convert.ToDouble(originPose[1]), Convert.ToDouble(originPose[2]) }; + double[] rpy = new[] { Convert.ToDouble(originPose[3]), Convert.ToDouble(originPose[4]), Convert.ToDouble(originPose[5]) }; + Origin origin = new Origin(xyz, rpy); + UrdfOrigin.ImportOriginData(sensorObject.transform, origin); + } + + sensorObject.name = sensor.elements[k_NameKey]; + GameObject sensorGameObject; + if (sensor.elements.ContainsKey(k_PluginKey) && String.IsNullOrEmpty(sensor.elements[k_PluginKey])) + { + sensorGameObject = SensorFactory.InstantiateSensor(sensor.elements[k_TypeKey], sensor.elements[k_PluginKey],sensor.elements); + } + else + { + sensorGameObject = SensorFactory.InstantiateSensor(sensor.elements[k_TypeKey], sensor.elements); + } + + sensorGameObject.transform.SetParentAndAlign(sensorObject); + } + } +} diff --git a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorExtension.cs.meta b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorExtension.cs.meta new file mode 100644 index 00000000..09877cbd --- /dev/null +++ b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorExtension.cs.meta @@ -0,0 +1,3 @@ +fileFormatVersion: 2 +guid: 731733b86bde4ec5bd3880c2bdf7786c +timeCreated: 1623153275 \ No newline at end of file diff --git a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs new file mode 100644 index 00000000..a955abdd --- /dev/null +++ b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs @@ -0,0 +1,52 @@ +using System; +using System.Collections.Generic; +using Unity.Robotics.Sensors; +using UnityEngine; + +namespace Unity.Robotics.UrdfImporter +{ + public static class UrdfSensorsExtensions + { + const string k_SensorTopic = "sensor/topic"; + public static UrdfSensors Create(Transform parent, List sensors = null) + { + GameObject sensorsObject = new GameObject("Sensors"); + sensorsObject.transform.SetParentAndAlign(parent); + UrdfSensors urdfSensors = sensorsObject.AddComponent(); + + sensorsObject.hideFlags = HideFlags.NotEditable; + urdfSensors.hideFlags = HideFlags.None; + + if (sensors != null) + { + foreach (Sensor sensor in sensors) + { + try + { + UrdfSensorExtension.Create(urdfSensors.transform, sensor); + } + catch (Exception e) + { + Debug.LogError($"Failed loading `{sensor.name}`"); + } + } + } + + + if (parent.GetComponent() != null) + { + GameObject transformSensor = AddTransformSensor(parent); + transformSensor.transform.SetParentAndAlign(sensorsObject.transform); + } + + return urdfSensors; + } + + static GameObject AddTransformSensor(Transform parent) + { + string topicName = "/"+parent.root.name + "/" + parent.name + "/TransformStamped"; + Dictionary settings = new Dictionary ();//{ { k_SensorTopic, topicName } }; + return SensorFactory.InstantiateSensor("transform",settings); + } + } +} diff --git a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs.meta b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs.meta new file mode 100644 index 00000000..735b959c --- /dev/null +++ b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a147aaee313004756acdc342220a77b7 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Link.cs b/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Link.cs index 4b0a8f5b..12d14c02 100644 --- a/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Link.cs +++ b/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Link.cs @@ -26,12 +26,14 @@ public class Link public List visuals; public List collisions; public List joints; + public List sensors; public Link(XElement node) { name = (string)node.Attribute("name"); // required inertial = (node.Element("inertial") != null) ? new Inertial(node.Element("inertial")) : null; // optional visuals = readVisuals(node); // multiple + sensors = ReadSensors(node); collisions = readCollisions(node); // optional } @@ -75,6 +77,14 @@ from child in node.Elements("visual") select new Visual(child); return visuals.ToList(); } + + private static List ReadSensors(XElement node) + { + var sensors = + from child in node.Elements("sensor") + select new Sensor(child); + return sensors.ToList(); + } public class Inertial { diff --git a/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Robot.cs b/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Robot.cs index 812efd5a..d6c2bc9e 100644 --- a/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Robot.cs +++ b/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Robot.cs @@ -32,7 +32,6 @@ public class Robot public List links; public List joints; - public List sensors; public List plugins; public List> ignoreCollisionPair; @@ -49,7 +48,6 @@ public Robot(string filename) links = ReadLinks(node); joints = ReadJoints(node); plugins = ReadPlugins(node); - sensors = ReadSensors(node); ignoreCollisionPair = ReadDisableCollision(node); @@ -108,14 +106,7 @@ from child in node.Elements("joint") select new Joint(child); return joints.ToList(); } - - private static List ReadSensors(XElement node) - { - var sensors = - from child in node.Elements("sensor") - select new Sensor(child); - return sensors.ToList(); - } + private List ReadPlugins(XElement node) { diff --git a/com.unity.robotics.urdf-importer/Runtime/Unity.Robotics.URDFImporter.asmdef b/com.unity.robotics.urdf-importer/Runtime/Unity.Robotics.URDFImporter.asmdef index 8ef6ea1d..05e58f3a 100644 --- a/com.unity.robotics.urdf-importer/Runtime/Unity.Robotics.URDFImporter.asmdef +++ b/com.unity.robotics.urdf-importer/Runtime/Unity.Robotics.URDFImporter.asmdef @@ -2,7 +2,8 @@ "name": "Unity.Robotics.UrdfImporter", "rootNamespace": "Unity.Robotics.UrdfImporter.Urdf", "references": [ - "GUID:6a697808d7c80a549b57420070d6c4f3" + "vhacd", + "Unity.Robotics.Sensors" ], "includePlatforms": [ "Editor", diff --git a/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfComparator.cs b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfComparator.cs index 3453e798..d9474450 100644 --- a/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfComparator.cs +++ b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfComparator.cs @@ -167,7 +167,7 @@ private bool CompareLink( Link source, Link exported, int indent) Joint jointExported = exported.joints.Find(x => x.name == jointSource.name); // Check for no match if (jointExported == null) { - linkLog.AppendLine(String.Format("{0}Joint Not Found in Exported: Joint Name:{1,12}",Indent(indent + 1),jointSource.name)); + linkLog.AppendLine(String.Format("{0}Joints Not Found in Exported: Joints Name:{1,12}", Indent(indent + 1),jointSource.name)); return false; } if (jointExported != null && !CompareJoint(jointSource, jointExported, indent)) @@ -188,7 +188,7 @@ private bool CompareLink( Link source, Link exported, int indent) /// private bool CompareJoint( Joint source, Joint exported, int indent) // This function does not test for Mimic, Calibration and SafetyController as they are not imported in Unity { - linkLog.AppendLine("\n\n********Joint*****\n"); + linkLog.AppendLine("\n\n********Joints*****\n"); bool jointNameEqual = (source.name == exported.name); linkLog.AppendLine(String.Format("{0}Name:", Indent(indent))); diff --git a/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfInertial.cs b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfInertial.cs index 09b363f4..17579951 100644 --- a/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfInertial.cs +++ b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfInertial.cs @@ -60,6 +60,10 @@ public static void Create(GameObject linkObject, Link.Inertial inertial = null) urdfInertial.useUrdfData = true; } + else + { + urdfInertial.SetDefaultInertialValues(); + } urdfInertial.displayInertiaGizmo = false; } @@ -220,6 +224,17 @@ private static Quaternion ToQuaternion(Vector3 eigenvector0, Vector3 eigenvector return new Quaternion(qx, qy, qz, qw); } + public void SetDefaultInertialValues() + { + ArticulationBody robotLink = GetComponent(); + robotLink.mass = minMass; + robotLink.inertiaTensor = new Vector3(MinInertia, MinInertia, MinInertia); + robotLink.inertiaTensorRotation = Quaternion.identity; + this.centerOfMass = Vector3.zero; + this.inertiaTensor = robotLink.inertiaTensor; + this.inertiaTensorRotation = robotLink.inertiaTensorRotation; + } + #endregion #region Export diff --git a/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfJoints/UrdfJoint.cs b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfJoints/UrdfJoint.cs index ba8ff679..7ffddadc 100644 --- a/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfJoints/UrdfJoint.cs +++ b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfJoints/UrdfJoint.cs @@ -20,7 +20,7 @@ namespace Unity.Robotics.UrdfImporter #if UNITY_2020_1_OR_NEWER [RequireComponent(typeof(ArticulationBody))] #else - [RequireComponent(typeof(Joint))] + [RequireComponent(typeof(Joints))] #endif public abstract class UrdfJoint : MonoBehaviour { @@ -40,7 +40,7 @@ public enum JointTypes protected ArticulationBody unityJoint; protected Vector3 axisofMotion; #else - protected UnityEngine.Joint unityJoint; + protected UnityEngine.Joints unityJoint; #endif public string jointName; @@ -101,7 +101,7 @@ private static UrdfJoint AddCorrectJointType(GameObject linkObject, JointTypes j #if UNITY_2020_1_OR_NEWER #else - UnityEngine.Joint unityJoint = linkObject.GetComponent(); + UnityEngine.Joints unityJoint = linkObject.GetComponent(); unityJoint.connectedBody = linkObject.transform.parent.gameObject.GetComponent(); unityJoint.autoConfigureConnectedAnchor = true; #endif @@ -112,7 +112,7 @@ private static UrdfJoint AddCorrectJointType(GameObject linkObject, JointTypes j /// /// Changes the type of the joint /// - /// Joint whose type is to be changed + /// Joints whose type is to be changed /// Type of the new joint public static void ChangeJointType(GameObject linkObject, JointTypes newJointType) { @@ -122,7 +122,7 @@ public static void ChangeJointType(GameObject linkObject, JointTypes newJointTyp #if UNITY_2020_1_OR_NEWER linkObject.transform.DestroyImmediateIfExists(); #else - linkObject.transform.DestroyImmediateIfExists(); + linkObject.transform.DestroyImmediateIfExists(); #endif AddCorrectJointType(linkObject, newJointType); } @@ -134,7 +134,7 @@ public void Start() #if UNITY_2020_1_OR_NEWER unityJoint = GetComponent(); #else - unityJoint = GetComponent(); + unityJoint = GetComponent(); #endif } @@ -229,7 +229,7 @@ public Joint ExportJointData() #if UNITY_2020_1_OR_NEWER unityJoint = GetComponent(); #else - unityJoint = GetComponent(); + unityJoint = GetComponent(); #endif CheckForUrdfCompatibility(); @@ -280,7 +280,7 @@ protected virtual bool IsJointAxisDefined() else return true; #else - UnityEngine.Joint joint = GetComponent(); + UnityEngine.Joints joint = GetComponent(); return !(Math.Abs(joint.axis.x) < Tolerance && Math.Abs(joint.axis.y) < Tolerance && Math.Abs(joint.axis.z) < Tolerance); @@ -311,7 +311,7 @@ protected static Joint.Axis GetAxisData(Vector3 axis) private void CheckForUrdfCompatibility() { if (!AreLimitsCorrect()) - Debug.LogWarning("Limits are not defined correctly for Joint " + jointName + " in Link " + name + + Debug.LogWarning("Limits are not defined correctly for Joints " + jointName + " in Link " + name + ". This may cause problems when visualizing the robot in RVIZ or Gazebo.", gameObject); if (!IsJointAxisDefined()) diff --git a/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfJoints/UrdfJointContinuous.cs b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfJoints/UrdfJointContinuous.cs index 6a8d3f99..5ec24526 100644 --- a/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfJoints/UrdfJointContinuous.cs +++ b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfJoints/UrdfJointContinuous.cs @@ -109,7 +109,7 @@ protected override Joint ExportSpecificJointData(Joint joint) #else joint.axis = GetAxisData(unityJoint.axis); - joint.dynamics = new Joint.Dynamics( + joint.dynamics = new Joints.Dynamics( ((HingeJoint)unityJoint).spring.damper, ((HingeJoint)unityJoint).spring.spring); #endif diff --git a/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfJoints/UrdfJointFloating.cs b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfJoints/UrdfJointFloating.cs index a9281ba5..efb6516b 100644 --- a/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfJoints/UrdfJointFloating.cs +++ b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfJoints/UrdfJointFloating.cs @@ -25,7 +25,7 @@ public static UrdfJoint Create(GameObject linkObject) UrdfJointFloating urdfJoint = linkObject.AddComponent(); #if UNITY_2020_1_OR_NEWER urdfJoint.unityJoint = linkObject.AddComponent(); - //Doesnt have any equivalent Articulatiob Joint + //Doesnt have any equivalent Articulatiob Joints #else urdfJoint.UnityJoint = linkObject.AddComponent(); #endif diff --git a/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfJoints/UrdfJointPlanar.cs b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfJoints/UrdfJointPlanar.cs index 73299826..585eaab9 100644 --- a/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfJoints/UrdfJointPlanar.cs +++ b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfJoints/UrdfJointPlanar.cs @@ -116,7 +116,7 @@ protected override Joint ExportSpecificJointData(Joint joint) #else ConfigurableJoint configurableJoint = (ConfigurableJoint)unityJoint; joint.axis = GetAxisData(Vector3.Cross(configurableJoint.axis, configurableJoint.secondaryAxis)); - joint.dynamics = new Joint.Dynamics(configurableJoint.xDrive.positionDamper, configurableJoint.xDrive.positionSpring); + joint.dynamics = new Joints.Dynamics(configurableJoint.xDrive.positionDamper, configurableJoint.xDrive.positionSpring); joint.limit = ExportLimitData(); #endif return joint; @@ -129,7 +129,7 @@ protected override Joint.Limit ExportLimitData() return new Joint.Limit(drive.lowerLimit, drive.upperLimit, EffortLimit, VelocityLimit); #else ConfigurableJoint configurableJoint = (ConfigurableJoint)unityJoint; - return new Joint.Limit( + return new Joints.Limit( Math.Round(-configurableJoint.linearLimit.limit, RoundDigits), Math.Round(configurableJoint.linearLimit.limit, RoundDigits), EffortLimit, VelocityLimit); diff --git a/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfJoints/UrdfJointPrismatic.cs b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfJoints/UrdfJointPrismatic.cs index 8e0ec91c..f7332791 100644 --- a/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfJoints/UrdfJointPrismatic.cs +++ b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfJoints/UrdfJointPrismatic.cs @@ -164,7 +164,7 @@ protected override Joint ExportSpecificJointData(Joint joint) ConfigurableJoint configurableJoint = (ConfigurableJoint)unityJoint; joint.axis = GetAxisData(configurableJoint.axis); - joint.dynamics = new Joint.Dynamics(configurableJoint.xDrive.positionDamper, configurableJoint.xDrive.positionSpring); + joint.dynamics = new Joints.Dynamics(configurableJoint.xDrive.positionDamper, configurableJoint.xDrive.positionSpring); joint.limit = ExportLimitData(); #endif return joint; @@ -188,11 +188,11 @@ protected override Joint.Limit ExportLimitData() #if UNITY_2020_2_OR_NEWER return new Joint.Limit(drive.lowerLimit, drive.upperLimit, drive.forceLimit, unityJoint.maxLinearVelocity); #elif UNITY_2020_1 - return new Joint.Limit(drive.lowerLimit, drive.upperLimit, drive.forceLimit, maxLinearVelocity); + return new Joints.Limit(drive.lowerLimit, drive.upperLimit, drive.forceLimit, maxLinearVelocity); #endif #else PrismaticJointLimitsManager prismaticLimits = GetComponent(); - return new Joint.Limit( + return new Joints.Limit( Math.Round(prismaticLimits.PositionLimitMin, RoundDigits), Math.Round(prismaticLimits.PositionLimitMax, RoundDigits), EffortLimit, diff --git a/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfJoints/UrdfJointRevolute.cs b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfJoints/UrdfJointRevolute.cs index a79aad48..38fad11d 100644 --- a/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfJoints/UrdfJointRevolute.cs +++ b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfJoints/UrdfJointRevolute.cs @@ -111,7 +111,7 @@ protected override Joint ExportSpecificJointData(Joint joint) joint.limit = ExportLimitData(); #else joint.axis = GetAxisData(unityJoint.axis); - joint.dynamics = new Joint.Dynamics(((HingeJoint)unityJoint).spring.damper, ((HingeJoint)unityJoint).spring.spring); + joint.dynamics = new Joints.Dynamics(((HingeJoint)unityJoint).spring.damper, ((HingeJoint)unityJoint).spring.spring); joint.limit = ExportLimitData(); #endif @@ -137,7 +137,7 @@ protected override Joint.Limit ExportLimitData() return new Joint.Limit(drive.lowerLimit * Mathf.Deg2Rad, drive.upperLimit * Mathf.Deg2Rad, drive.forceLimit, unityJoint.maxAngularVelocity); #else HingeJointLimitsManager hingeJointLimits = GetComponent(); - return new Joint.Limit( + return new Joints.Limit( Math.Round(hingeJointLimits.LargeAngleLimitMin * Mathf.Deg2Rad, RoundDigits), Math.Round(hingeJointLimits.LargeAngleLimitMax * Mathf.Deg2Rad, RoundDigits), EffortLimit, diff --git a/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfSensor.cs b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfSensor.cs new file mode 100644 index 00000000..48c964ea --- /dev/null +++ b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfSensor.cs @@ -0,0 +1,9 @@ +using UnityEngine; + +namespace Unity.Robotics.UrdfImporter +{ + public class UrdfSensor: MonoBehaviour + { + public string sensorType; + } +} diff --git a/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfSensor.cs.meta b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfSensor.cs.meta new file mode 100644 index 00000000..557320df --- /dev/null +++ b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfSensor.cs.meta @@ -0,0 +1,3 @@ +fileFormatVersion: 2 +guid: ef24a4a610d14afebc9f7ca9195c44e1 +timeCreated: 1623155570 \ No newline at end of file diff --git a/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfSensors.cs b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfSensors.cs new file mode 100644 index 00000000..d90eed4a --- /dev/null +++ b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfSensors.cs @@ -0,0 +1,9 @@ +using UnityEngine; + +namespace Unity.Robotics.UrdfImporter +{ + public class UrdfSensors: MonoBehaviour + { + + } +} diff --git a/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfSensors.cs.meta b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfSensors.cs.meta new file mode 100644 index 00000000..eac229de --- /dev/null +++ b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfSensors.cs.meta @@ -0,0 +1,3 @@ +fileFormatVersion: 2 +guid: a7e8ef314757414dad08752a90a8c94a +timeCreated: 1623153177 \ No newline at end of file diff --git a/com.unity.robotics.urdf-importer/Tests/Runtime/Assets/URDF/cube/cube.urdf b/com.unity.robotics.urdf-importer/Tests/Runtime/Assets/URDF/cube/cube.urdf index 5d2a516e..ff4dcdf3 100644 --- a/com.unity.robotics.urdf-importer/Tests/Runtime/Assets/URDF/cube/cube.urdf +++ b/com.unity.robotics.urdf-importer/Tests/Runtime/Assets/URDF/cube/cube.urdf @@ -13,5 +13,8 @@ + + 100 + \ No newline at end of file From 1ad9817a5009376fb37f09f5d8af861718224c67 Mon Sep 17 00:00:00 2001 From: Hamid Younesy Date: Sat, 18 Sep 2021 14:17:05 -0700 Subject: [PATCH 06/12] updating package version for dev-sensors for clarity (#144) --- com.unity.robotics.urdf-importer/package.json | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/com.unity.robotics.urdf-importer/package.json b/com.unity.robotics.urdf-importer/package.json index 4752a801..da50e43e 100644 --- a/com.unity.robotics.urdf-importer/package.json +++ b/com.unity.robotics.urdf-importer/package.json @@ -1,11 +1,10 @@ { "name": "com.unity.robotics.urdf-importer", - "version": "0.5.0-preview", + "version": "0.5.0-exp.1+sensors", "displayName": "URDF Importer", "description": "Facilitates importing configurations from the Universal Robot Description Format", - "unity": "2020.2", - "unityRelease": "0b9", + "unity": "2020.3", "dependencies": { "com.unity.editorcoroutines": "1.0.0" - } + } } From a2d384458da57ac53ff05b3f27c158722fd5608f Mon Sep 17 00:00:00 2001 From: vidurvij-Unity <60901103+vidurvij-Unity@users.noreply.github.com> Date: Wed, 26 Jan 2022 13:20:53 -0800 Subject: [PATCH 07/12] Update Transform sensor topic name during import. (#172) --- .../Runtime/Extensions/UrdfSensorsExtensions.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs index a955abdd..1501b628 100644 --- a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs +++ b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs @@ -45,7 +45,7 @@ public static UrdfSensors Create(Transform parent, List sensors = null) static GameObject AddTransformSensor(Transform parent) { string topicName = "/"+parent.root.name + "/" + parent.name + "/TransformStamped"; - Dictionary settings = new Dictionary ();//{ { k_SensorTopic, topicName } }; + Dictionary settings = new Dictionary { { k_SensorTopic, topicName } }; return SensorFactory.InstantiateSensor("transform",settings); } } From 5abd66b9f8eb7a996ef82cf92843421fbfcefd9c Mon Sep 17 00:00:00 2001 From: vidurvij-Unity <60901103+vidurvij-Unity@users.noreply.github.com> Date: Fri, 4 Feb 2022 10:33:53 -0800 Subject: [PATCH 08/12] AIRO-1698 Change yamato config to initiate tests on dev-sensors (#180) * Change yamato config to initiate tests on dev-sensors * Removing dependency on RoboticSensors if package not detected. --- .yamato/yamato-config.yml | 10 ++-- TestUrdfImporter/Packages/manifest.json | 3 -- TestUrdfImporter/Packages/packages-lock.json | 51 ------------------- .../Runtime/Extensions/UrdfLinkExtensions.cs | 7 ++- .../Runtime/Extensions/UrdfRobotExtensions.cs | 7 +++ .../Runtime/Extensions/UrdfSensorExtension.cs | 2 + .../Extensions/UrdfSensorsExtensions.cs | 2 + .../Unity.Robotics.URDFImporter.asmdef | 8 ++- 8 files changed, 30 insertions(+), 60 deletions(-) diff --git a/.yamato/yamato-config.yml b/.yamato/yamato-config.yml index 9d960d9e..8638173f 100644 --- a/.yamato/yamato-config.yml +++ b/.yamato/yamato-config.yml @@ -22,9 +22,13 @@ commands: triggers: cancel_old_ci: true expression: | - ((pull_request.target eq "main" OR pull_request.target eq "dev") - AND NOT pull_request.push.changes.all match "**/*.md") OR - (push.branch eq "main" OR push.branch eq "dev") + (pull_request.target eq "main" AND + NOT pull_request.push.changes.all match "**/*.md") OR + (pull_request.target eq "dev" AND + NOT pull_request.push.changes.all match "**/*.md") OR + (pull_request.target eq "dev-sensors" AND + NOT pull_request.push.changes.all match "**/*.md") + artifacts: logs: paths: diff --git a/TestUrdfImporter/Packages/manifest.json b/TestUrdfImporter/Packages/manifest.json index c6cb739e..2c85fd53 100644 --- a/TestUrdfImporter/Packages/manifest.json +++ b/TestUrdfImporter/Packages/manifest.json @@ -4,10 +4,7 @@ "com.unity.ide.rider": "3.0.6", "com.unity.ide.visualstudio": "2.0.8", "com.unity.ide.vscode": "1.2.3", - "com.unity.robotics.sensors": "file:/Users/vidur.vij/Desktop/RoboticsSensors/com.unity.robotics.sensors", "com.unity.robotics.urdf-importer": "file:../../com.unity.robotics.urdf-importer", - "com.unity.systemgraph": "file:Mechatronics/com.unity.systemgraph", - "com.unity.systemgraph.authoring": "file:Mechatronics/com.unity.systemgraph.authoring", "com.unity.test-framework": "1.1.24", "com.unity.testtools.codecoverage": "1.0.0", "com.unity.textmeshpro": "3.0.6", diff --git a/TestUrdfImporter/Packages/packages-lock.json b/TestUrdfImporter/Packages/packages-lock.json index 4eb89bf1..460a71d2 100644 --- a/TestUrdfImporter/Packages/packages-lock.json +++ b/TestUrdfImporter/Packages/packages-lock.json @@ -1,17 +1,5 @@ { "dependencies": { - "com.unity.addressables": { - "version": "1.16.19", - "depth": 1, - "source": "registry", - "dependencies": { - "com.unity.scriptablebuildpipeline": "1.15.2", - "com.unity.modules.assetbundle": "1.0.0", - "com.unity.modules.unitywebrequest": "1.0.0", - "com.unity.modules.unitywebrequestassetbundle": "1.0.0" - }, - "url": "https://packages.unity.com" - }, "com.unity.collab-proxy": { "version": "1.5.7", "depth": 0, @@ -67,22 +55,6 @@ "dependencies": {}, "url": "https://packages.unity.com" }, - "com.unity.robotics.ros-tcp-connector": { - "version": "file:ROS-TCP-Connector", - "depth": 0, - "source": "embedded", - "dependencies": {} - }, - "com.unity.robotics.sensors": { - "version": "file:/Users/vidur.vij/Desktop/RoboticsSensors/com.unity.robotics.sensors", - "depth": 0, - "source": "local", - "dependencies": { - "com.unity.addressables": "1.6.2", - "com.unity.systemgraph": "0.0.1-preview.2", - "com.unity.robotics.ros-tcp-connector": "0.1.2-preview" - } - }, "com.unity.robotics.urdf-importer": { "version": "file:../../com.unity.robotics.urdf-importer", "depth": 0, @@ -91,13 +63,6 @@ "com.unity.editorcoroutines": "1.0.0" } }, - "com.unity.scriptablebuildpipeline": { - "version": "1.15.2", - "depth": 2, - "source": "registry", - "dependencies": {}, - "url": "https://packages.unity.com" - }, "com.unity.settings-manager": { "version": "1.0.3", "depth": 1, @@ -105,22 +70,6 @@ "dependencies": {}, "url": "https://packages.unity.com" }, - "com.unity.systemgraph": { - "version": "file:Mechatronics/com.unity.systemgraph", - "depth": 0, - "source": "local", - "dependencies": { - "com.unity.addressables": "1.6.2" - } - }, - "com.unity.systemgraph.authoring": { - "version": "file:Mechatronics/com.unity.systemgraph.authoring", - "depth": 0, - "source": "local", - "dependencies": { - "com.unity.systemgraph": "1.0.0-exp.3" - } - }, "com.unity.test-framework": { "version": "1.1.24", "depth": 0, diff --git a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfLinkExtensions.cs b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfLinkExtensions.cs index bbd36b8f..579f639a 100644 --- a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfLinkExtensions.cs +++ b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfLinkExtensions.cs @@ -23,8 +23,6 @@ public static GameObject Create(Transform parent, Link link = null, Joint joint GameObject linkObject = new GameObject("link"); linkObject.transform.SetParentAndAlign(parent); UrdfLink urdfLink = linkObject.AddComponent(); - UrdfCollisionsExtensions.Create(linkObject.transform, link?.collisions); - UrdfVisualsExtensions.Create(linkObject.transform, link?.visuals); if (link != null) { @@ -38,7 +36,12 @@ public static GameObject Create(Transform parent, Link link = null, Joint joint #endif } + + UrdfVisualsExtensions.Create(linkObject.transform, link?.visuals); + UrdfCollisionsExtensions.Create(linkObject.transform, link?.collisions); +#if ROBOTICS_SENSORS UrdfSensorsExtensions.Create(linkObject.transform, link?.sensors); +#endif return linkObject; } diff --git a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfRobotExtensions.cs b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfRobotExtensions.cs index 9df42e5c..e39bf096 100644 --- a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfRobotExtensions.cs +++ b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfRobotExtensions.cs @@ -16,7 +16,10 @@ limitations under the License. using System; using System.IO; using System.Linq; + +#if ROBOTICS_SENSORS using Unity.Robotics.Sensors; +#endif using Unity.Robotics.UrdfImporter.Control; #if UNITY_EDITOR using UnityEditor; @@ -118,7 +121,9 @@ private static void ImportPipelineCreateObject(ImportPipelineData im) UrdfAssetPathHandler.SetPackageRoot(Path.GetDirectoryName(im.robot.filename)); UrdfMaterial.InitializeRobotMaterials(im.robot); UrdfPlugins.Create(im.robotGameObject.transform, im.robot.plugins); +#if ROBOTICS_SENSORS AddJointSensor(im.robotGameObject); +#endif } // Creates the stack of robot joints. Should be called iteratively until false is returned. @@ -427,6 +432,7 @@ static void SetTag(GameObject go) } } +#if ROBOTICS_SENSORS static void AddJointSensor(GameObject robot) { Dictionary settings = new Dictionary { { "sensor/topic", robot.name + "/JointState" } }; @@ -458,5 +464,6 @@ static void SetTag(GameObject go) } } } +#endif } } diff --git a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorExtension.cs b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorExtension.cs index c807e3ac..78036b29 100644 --- a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorExtension.cs +++ b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorExtension.cs @@ -1,3 +1,4 @@ +#if ROBOTICS_SENSORS using System; using UnityEngine; using Unity.Robotics.Sensors; @@ -47,3 +48,4 @@ static void ImportSensorData(Transform sensorObject, Sensor sensor) } } } +#endif diff --git a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs index 1501b628..983fad4f 100644 --- a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs +++ b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs @@ -1,3 +1,4 @@ +#if ROBOTICS_SENSORS using System; using System.Collections.Generic; using Unity.Robotics.Sensors; @@ -50,3 +51,4 @@ static GameObject AddTransformSensor(Transform parent) } } } +#endif \ No newline at end of file diff --git a/com.unity.robotics.urdf-importer/Runtime/Unity.Robotics.URDFImporter.asmdef b/com.unity.robotics.urdf-importer/Runtime/Unity.Robotics.URDFImporter.asmdef index 05e58f3a..a1aa9c3e 100644 --- a/com.unity.robotics.urdf-importer/Runtime/Unity.Robotics.URDFImporter.asmdef +++ b/com.unity.robotics.urdf-importer/Runtime/Unity.Robotics.URDFImporter.asmdef @@ -17,6 +17,12 @@ "precompiledReferences": [], "autoReferenced": true, "defineConstraints": [], - "versionDefines": [], + "versionDefines": [ + { + "name": "com.unity.robotics.sensors", + "expression": "", + "define": "ROBOTICS_SENSORS" + } + ], "noEngineReferences": false } \ No newline at end of file From 4c575219fb51dc6a1e38dec1047db8665f62030a Mon Sep 17 00:00:00 2001 From: vidurvij-Unity <60901103+vidurvij-Unity@users.noreply.github.com> Date: Fri, 4 Feb 2022 11:49:59 -0800 Subject: [PATCH 09/12] Adding TF Broadcaster to an imported robot (#173) * Adding TF Broadcaster to an imported robot * Update UrdfRobotExtensions.cs * Update UrdfRobotExtensions.cs --- .../Runtime/Extensions/UrdfRobotExtensions.cs | 47 +++++++++---------- 1 file changed, 22 insertions(+), 25 deletions(-) diff --git a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfRobotExtensions.cs b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfRobotExtensions.cs index e39bf096..77b491ec 100644 --- a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfRobotExtensions.cs +++ b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfRobotExtensions.cs @@ -123,6 +123,7 @@ private static void ImportPipelineCreateObject(ImportPipelineData im) UrdfPlugins.Create(im.robotGameObject.transform, im.robot.plugins); #if ROBOTICS_SENSORS AddJointSensor(im.robotGameObject); + AddTfBroadcaster(im.robotGameObject); #endif } @@ -436,32 +437,28 @@ static void SetTag(GameObject go) static void AddJointSensor(GameObject robot) { Dictionary settings = new Dictionary { { "sensor/topic", robot.name + "/JointState" } }; - SensorFactory.InstantiateSensor("joint", settings).transform.SetParentAndAlign(robot.transform); - - static void SetTag(GameObject go) + var sensor = SensorFactory.InstantiateSensor("joint", settings); + if (sensor == null) { - try - { - GameObject.FindWithTag(FKRobot.k_TagName); - } - catch (Exception) - { - Debug.LogError($"Unable to find tag '{FKRobot.k_TagName}'." + - $"Add a tag '{FKRobot.k_TagName}' in the Project Settings in Unity Editor."); - return; - } - - if (!go) - return; - - try - { - go.tag = FKRobot.k_TagName; - } - catch (Exception) - { - Debug.LogError($"Unable to set the GameObject '{go.name}' tag to '{FKRobot.k_TagName}'."); - } + Debug.LogWarning("JointSensor is not loaded."); + } + else + { + sensor.transform.SetParentAndAlign(robot.transform); + } + } + + static void AddTfBroadcaster(GameObject robot) + { + Dictionary settings = new Dictionary (); + var sensor = SensorFactory.InstantiateSensor("TF", settings); + if (sensor == null) + { + Debug.LogWarning("TFBroadcaster is not loaded."); + } + else + { + sensor.transform.SetParentAndAlign(robot.transform); } } #endif From bd929453d4baa149e9a404fbec9c830d9c5035e6 Mon Sep 17 00:00:00 2001 From: vidurvij-Unity <60901103+vidurvij-Unity@users.noreply.github.com> Date: Fri, 4 Feb 2022 16:54:27 -0800 Subject: [PATCH 10/12] AIRO-1483 AIRO-1480 Sensor export (#156) * Create RunTimeExportTests.cs.meta * Adding export tests * Adding cleanup code * Renaming Test Function * Adding sensor import code to URDF Importer * Adding sensor pose export. * Adding sensor export test * Adding storage of ununsed settings during sensor import * passing unused parameter to InstantiateSensor * Update UrdfRobotExtensions.cs * Hide RoboticSensors code if package not present. * Add RoboticSensors dependence in Unit tests Co-authored-by: hyounesy --- .../ProjectSettings/TagManager.asset | 3 +- .../Runtime/Extensions/UrdfLinkExtensions.cs | 3 + .../Runtime/Extensions/UrdfRobotExtensions.cs | 4 +- .../Runtime/Extensions/UrdfSensorExtension.cs | 27 +- .../Extensions/UrdfSensorsExtensions.cs | 13 +- .../Runtime/RosSharpDefinitions/Link.cs | 3 + .../Runtime/RosSharpDefinitions/Sensor.cs | 45 ++- .../Runtime/UrdfComponents/UrdfSensor.cs | 2 + .../Tests/Editor/UrdfExportTests.meta | 3 + .../Editor/UrdfExportTests/UrdfExportTests.cs | 366 ++++++++++++++++++ .../UrdfExportTests/UrdfExportTests.cs.meta | 11 + .../Tests/Runtime/Assets/URDF/cube/cube.urdf | 1 + .../Runtime/Assets/URDF/cube/cube_export.urdf | 47 +++ .../Assets/URDF/cube/cube_export.urdf.meta | 7 + ...Robotics.UrdfImporter.Runtime.Tests.asmdef | 5 + 15 files changed, 520 insertions(+), 20 deletions(-) create mode 100644 com.unity.robotics.urdf-importer/Tests/Editor/UrdfExportTests.meta create mode 100644 com.unity.robotics.urdf-importer/Tests/Editor/UrdfExportTests/UrdfExportTests.cs create mode 100644 com.unity.robotics.urdf-importer/Tests/Editor/UrdfExportTests/UrdfExportTests.cs.meta create mode 100644 com.unity.robotics.urdf-importer/Tests/Runtime/Assets/URDF/cube/cube_export.urdf create mode 100644 com.unity.robotics.urdf-importer/Tests/Runtime/Assets/URDF/cube/cube_export.urdf.meta diff --git a/TestUrdfImporter/ProjectSettings/TagManager.asset b/TestUrdfImporter/ProjectSettings/TagManager.asset index 1c92a784..d82c6cbf 100644 --- a/TestUrdfImporter/ProjectSettings/TagManager.asset +++ b/TestUrdfImporter/ProjectSettings/TagManager.asset @@ -3,7 +3,8 @@ --- !u!78 &1 TagManager: serializedVersion: 2 - tags: [] + tags: + - robot layers: - Default - TransparentFX diff --git a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfLinkExtensions.cs b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfLinkExtensions.cs index 579f639a..cbb38f65 100644 --- a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfLinkExtensions.cs +++ b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfLinkExtensions.cs @@ -80,6 +80,9 @@ public static Link ExportLinkData(this UrdfLink urdfLink) { visuals = urdfLink.GetComponentInChildren().ExportVisualsData(), collisions = urdfLink.GetComponentInChildren().ExportCollisionsData(), +#if ROBOTICS_SENSORS + sensors = urdfLink.GetComponentInChildren().ExportSensorsData(), +#endif inertial = urdfInertial == null ? null : urdfInertial.ExportInertialData() }; diff --git a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfRobotExtensions.cs b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfRobotExtensions.cs index 77b491ec..1cb050ba 100644 --- a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfRobotExtensions.cs +++ b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfRobotExtensions.cs @@ -437,7 +437,7 @@ static void SetTag(GameObject go) static void AddJointSensor(GameObject robot) { Dictionary settings = new Dictionary { { "sensor/topic", robot.name + "/JointState" } }; - var sensor = SensorFactory.InstantiateSensor("joint", settings); + var sensor = SensorFactory.InstantiateSensor("joint", settings, out Dictionary unusedSettings); if (sensor == null) { Debug.LogWarning("JointSensor is not loaded."); @@ -451,7 +451,7 @@ static void AddJointSensor(GameObject robot) static void AddTfBroadcaster(GameObject robot) { Dictionary settings = new Dictionary (); - var sensor = SensorFactory.InstantiateSensor("TF", settings); + var sensor = SensorFactory.InstantiateSensor("TF", settings, out Dictionary unusedSettings); if (sensor == null) { Debug.LogWarning("TFBroadcaster is not loaded."); diff --git a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorExtension.cs b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorExtension.cs index 78036b29..78dd7a1e 100644 --- a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorExtension.cs +++ b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorExtension.cs @@ -1,5 +1,7 @@ #if ROBOTICS_SENSORS using System; +using System.Collections.Generic; +using System.Linq; using UnityEngine; using Unity.Robotics.Sensors; @@ -35,17 +37,38 @@ static void ImportSensorData(Transform sensorObject, Sensor sensor) sensorObject.name = sensor.elements[k_NameKey]; GameObject sensorGameObject; + Dictionary unusedSettings; if (sensor.elements.ContainsKey(k_PluginKey) && String.IsNullOrEmpty(sensor.elements[k_PluginKey])) { - sensorGameObject = SensorFactory.InstantiateSensor(sensor.elements[k_TypeKey], sensor.elements[k_PluginKey],sensor.elements); + sensorGameObject = SensorFactory.InstantiateSensor(sensor.elements[k_TypeKey], sensor.elements[k_PluginKey],sensor.elements, out unusedSettings); } else { - sensorGameObject = SensorFactory.InstantiateSensor(sensor.elements[k_TypeKey], sensor.elements); + sensorGameObject = SensorFactory.InstantiateSensor(sensor.elements[k_TypeKey], sensor.elements, out unusedSettings); } + sensorObject.GetComponent().unusedSettings = unusedSettings; sensorGameObject.transform.SetParentAndAlign(sensorObject); } + + public static Sensor ExportSensorData(this UrdfSensor urdfSensor) + { + var sensorProperties = SensorFactory.GetSensorSettingsAsDictionary(urdfSensor.gameObject); + foreach (var unusedSettings in urdfSensor.unusedSettings.Where(unusedSettings => unusedSettings.Key != k_NameKey && unusedSettings.Key != k_PoseKey && unusedSettings.Key != k_TypeKey)) + sensorProperties.Add(unusedSettings.Key,unusedSettings.Value); + sensorProperties.Add(k_NameKey,urdfSensor.name); + sensorProperties.Add(k_TypeKey,urdfSensor.sensorType); + var sensorPose = UrdfOrigin.ExportOriginData(urdfSensor.transform); + if (sensorPose != null) + { + string poseString = string.Join(" ", string.Join(" ", sensorPose.Xyz ?? new double[] { 0, 0, 0 }), string.Join(" ", sensorPose.Rpy ?? new double[] { 0, 0, 0 })); + sensorProperties.Add(k_PoseKey,poseString); + } + return new Sensor() + { + elements = sensorProperties + }; + } } } #endif diff --git a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs index 983fad4f..66563779 100644 --- a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs +++ b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs @@ -1,6 +1,7 @@ #if ROBOTICS_SENSORS using System; using System.Collections.Generic; +using System.Linq; using Unity.Robotics.Sensors; using UnityEngine; @@ -9,6 +10,7 @@ namespace Unity.Robotics.UrdfImporter public static class UrdfSensorsExtensions { const string k_SensorTopic = "sensor/topic"; + const string k_SensorName = "sensor/name"; public static UrdfSensors Create(Transform parent, List sensors = null) { GameObject sensorsObject = new GameObject("Sensors"); @@ -28,7 +30,7 @@ public static UrdfSensors Create(Transform parent, List sensors = null) } catch (Exception e) { - Debug.LogError($"Failed loading `{sensor.name}`"); + Debug.LogError($"Failed loading `{sensor.elements[k_SensorName]}`"); } } } @@ -47,7 +49,14 @@ static GameObject AddTransformSensor(Transform parent) { string topicName = "/"+parent.root.name + "/" + parent.name + "/TransformStamped"; Dictionary settings = new Dictionary { { k_SensorTopic, topicName } }; - return SensorFactory.InstantiateSensor("transform",settings); + return SensorFactory.InstantiateSensor("transform",settings, out Dictionary unusedSettings); + } + + public static List ExportSensorsData(this UrdfSensors urdfSensors) + { + UrdfSensor[] urdfSensorsList = urdfSensors.GetComponentsInChildren(); + + return urdfSensorsList.Select(urdfSensor => urdfSensor.ExportSensorData()).ToList(); } } } diff --git a/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Link.cs b/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Link.cs index 64c3c451..9f459ef2 100644 --- a/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Link.cs +++ b/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Link.cs @@ -45,6 +45,7 @@ public Link(string name, Inertial inertial = null) visuals = new List(); collisions = new List(); joints = new List(); + sensors = new List(); } public void WriteToUrdf(XmlWriter writer) @@ -58,6 +59,8 @@ public void WriteToUrdf(XmlWriter writer) visual.WriteToUrdf(writer); foreach (var collision in collisions) collision.WriteToUrdf(writer); + foreach (var sensor in sensors) + sensor.WriteToUrdf(writer); writer.WriteEndElement(); } diff --git a/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Sensor.cs b/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Sensor.cs index f418a622..096d2097 100644 --- a/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Sensor.cs +++ b/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Sensor.cs @@ -1,24 +1,16 @@ using System.Collections.Generic; -using System.IO; using System.Linq; -using System.Threading; -using System.Xml; using System.Xml.Linq; using System; -using System.Numerics; - +using System.Xml; namespace Unity.Robotics.UrdfImporter { public class Sensor { - public string name { get; set; } - public string type { get; set; } - public string topic { get; set; } - private static string k_BaseKey = "sensor"; - private static string k_AttributeDelimit = "@"; - private static string k_ElementDelimit = "/"; - public int updateRate { get; set; } + static string k_BaseKey = "sensor"; + static char k_AttributeDelimit = '@'; + static char k_ElementDelimit = '/'; public Dictionary elements { get; set; } public Sensor(XElement node) @@ -33,6 +25,11 @@ public Sensor(XElement node) AddElement(element,k_BaseKey); } } + + public Sensor() + { + + } public void AddAttribute(XAttribute attribute, string key) { @@ -43,7 +40,7 @@ public void AddAttribute(XAttribute attribute, string key) public void AddElement(XElement element, string key) { string currentKey = key + k_ElementDelimit + element.Name; - if (element.Elements().Count() == 0 && !(element.Value == "")) + if (!element.Elements().Any() && element.Value != "") { elements.Add(currentKey,element.Value); } @@ -56,5 +53,27 @@ public void AddElement(XElement element, string key) AddElement(ele,currentKey); } } + + public void WriteToUrdf(XmlWriter writer) + { + ExportElement("sensor",writer); + } + + void ExportElement(string elementKey, XmlWriter writer) + { + char[] delimiterChars = { k_AttributeDelimit, k_ElementDelimit }; + string elementName = elementKey.Split(delimiterChars).Last(); + writer.WriteStartElement(elementName); + var attributeList = elements.Select(x => x).Where(x => x.Key.StartsWith(elementKey + k_AttributeDelimit)).ToList(); + foreach(var attribute in attributeList) + writer.WriteAttributeString(attribute.Key.Substring((elementKey + k_AttributeDelimit).Length),attribute.Value); + if(elements.Keys.Contains(elementKey)) + writer.WriteString(elements[elementKey]); + var elementList = elements.Select(x => x).Where(x => x.Key.StartsWith(elementKey + k_ElementDelimit)).ToList(); + foreach(var element in elementList) + ExportElement(element.Key,writer); + writer.WriteEndElement(); + } + } } diff --git a/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfSensor.cs b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfSensor.cs index 48c964ea..82e0d084 100644 --- a/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfSensor.cs +++ b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfSensor.cs @@ -1,3 +1,4 @@ +using System.Collections.Generic; using UnityEngine; namespace Unity.Robotics.UrdfImporter @@ -5,5 +6,6 @@ namespace Unity.Robotics.UrdfImporter public class UrdfSensor: MonoBehaviour { public string sensorType; + public Dictionary unusedSettings; } } diff --git a/com.unity.robotics.urdf-importer/Tests/Editor/UrdfExportTests.meta b/com.unity.robotics.urdf-importer/Tests/Editor/UrdfExportTests.meta new file mode 100644 index 00000000..343c43b3 --- /dev/null +++ b/com.unity.robotics.urdf-importer/Tests/Editor/UrdfExportTests.meta @@ -0,0 +1,3 @@ +fileFormatVersion: 2 +guid: 4d855383ef6b4996a448d94db2ac3221 +timeCreated: 1635488791 \ No newline at end of file diff --git a/com.unity.robotics.urdf-importer/Tests/Editor/UrdfExportTests/UrdfExportTests.cs b/com.unity.robotics.urdf-importer/Tests/Editor/UrdfExportTests/UrdfExportTests.cs new file mode 100644 index 00000000..b2a03fda --- /dev/null +++ b/com.unity.robotics.urdf-importer/Tests/Editor/UrdfExportTests/UrdfExportTests.cs @@ -0,0 +1,366 @@ +using System; +using System.Collections.Generic; +using System.IO; +using NUnit.Framework; +using UnityEditor; +using UnityEngine; +using Assert = UnityEngine.Assertions.Assert; + +namespace Unity.Robotics.UrdfImporter +{ + public class UrdfExportTests + { + Robot m_Original; + Robot m_Export; + string m_UrdfFilePath = $"{Application.dataPath.Substring(0, Application.dataPath.Length - "Assets".Length)}" + + "Packages/com.unity.robotics.urdf-importer/Tests/Runtime/Assets/URDF/cube/"; + string m_OriginalUrdfFile = "cube_export.urdf"; + + [Test] + public void ExportRobotToUrdf_UrdfFile_ExportedFileEqual() + { + var robotEnumerator = UrdfRobotExtensions.Create(Path.Combine(m_UrdfFilePath,m_OriginalUrdfFile), ImportSettings.DefaultSettings()); + robotEnumerator.MoveNext(); + var robot = robotEnumerator.Current; + UrdfRobot robotObject = robot.GetComponent(); + Directory.CreateDirectory(m_UrdfFilePath + "tmp"); + robotObject.ExportRobotToUrdf(m_UrdfFilePath, "tmp"); + string exportedFilePath = Path.Combine(m_UrdfFilePath, "tmp", robot.name + ".urdf"); + m_Original = new Robot(Path.Combine(m_UrdfFilePath, m_OriginalUrdfFile)); + m_Export = new Robot(exportedFilePath); + Compare(); + } + + void Compare() + { + Assert.AreEqual(m_Original.links.Count, m_Export.links.Count, $"Number of links in exported and source file are unequal: {m_Export.links.Count}, {m_Original.links.Count}"); + Assert.AreEqual(m_Original.links.Count, m_Export.links.Count, $"Number of joints in exported and source file are unequal: {m_Export.joints.Count}, {m_Original.joints.Count}"); + + CompareLink(m_Original.root, m_Export.root); + } + + /// + /// Compares two links of a robot + /// + /// First link to be compared + /// Second link to be compared + /// + void CompareLink(Link source, Link exported) + { + Assert.AreEqual(source.name, exported.name, $"Joint names are not the same: Expected: {source.name} Actual: {exported.name}"); + CompareInertial(source.inertial, exported.inertial); + + Assert.AreEqual(source.visuals.Count, exported.visuals.Count, $"Number of visual meshes are unequal: Expected: {source.visuals.Count} Actual: {exported.visuals.Count}"); + for (var i = 0; i < source.visuals.Count; i++) CompareVisual(source.visuals[i], exported.visuals[i]); + + Assert.AreEqual(source.collisions.Count, exported.collisions.Count, $"Number of collision meshes are unequal: Expected: {source.collisions.Count} Actual: {exported.collisions.Count}"); + for (var i = 0; i < source.collisions.Count; i++) CompareCollisions(source.collisions[i], exported.collisions[i]); + +#if ROBOTICS_SENSORS + + Assert.AreEqual(source.sensors.Count, exported.sensors.Count, $"Number of sensors are unequal: Expected: {source.sensors.Count} Actual: {exported.sensors.Count}"); + for (var i = 0; i < source.sensors.Count; i++) CompareSensors(source.sensors[i], exported.sensors[i]); +#endif + Assert.AreEqual(source.joints.Count, exported.joints.Count, $"Number of joints connected to {source.name} link are unequal: Expected: {source.joints.Count} Actual: {exported.joints.Count}"); + foreach (var jointSource in source.joints) + { + var jointExported = exported.joints.Find(x => x.name == jointSource.name); // Check for no match + Assert.IsFalse(jointExported == null, $"Joint name not found in the exported URDF file"); + CompareJoint(jointSource, jointExported); + } + } + + /// + /// Compares two joint of a robot + /// + /// First joint to be compared + /// Second joint to be compared + /// + void CompareJoint(Joint source, Joint exported) // This function does not test for Mimic, Calibration and SafetyController as they are not imported in Unity + { + Assert.AreEqual(source.name, exported.name, $"Name of the joint is unequal: Expected: {source.name} Actual: {exported.name}"); + Assert.AreEqual(source.type, exported.type, $"Joint type is unequal: Expected: {source.type} Actual: {exported.type}"); + + CompareOrigin(source.origin, exported.origin); + Assert.AreEqual(source.parent, exported.parent, $"Parent name is unequal: Expected: {source.parent} Actual: {exported.parent}"); + + CompareAxis(source.axis, exported.axis); + CompareDynamics(source.dynamics, exported.dynamics); + if (source.type == "revolute" || source.type == "prismatic") CompareLimit(source.limit, exported.limit); + + Assert.AreEqual(source.child, exported.child, $"Child name is unequal: Expected: {source.child} Actual: {exported.child}"); + if (source.child != null) + CompareLink(source.ChildLink, exported.ChildLink); + } + + /// + /// Compares inertial information of two links + /// + /// First link's inertial information to be compared + /// Second link's inertial information to be compared + /// + static void CompareInertial(Link.Inertial source, Link.Inertial exported) + { + if (source == null && exported == null) return; + + Assert.IsFalse(source == null && exported != null || source != null && exported == null, $"Both joints' inertial is not null"); + Assert.IsTrue(source.mass.EqualsDelta(exported.mass, .005), $"Mass is unequal: Expected: {source.mass} Actual: {exported.mass}"); + + CompareOrigin(source.origin, exported.origin); + } + + /// + /// Compares two origin information of two links + /// + /// First link's origin information to be compared + /// Second link's origin information to be compared + /// + static void CompareOrigin(Origin source, Origin exported) + { + if (source == null && exported == null) return; + + Assert.IsTrue(source != null && exported == null && source.Xyz.ToVector3() == new Vector3(0, 0, 0) && source.Rpy.ToVector3() == new Vector3(0, 0, 0) + || exported != null && source == null && exported.Xyz.ToVector3() == new Vector3(0, 0, 0) && exported.Rpy.ToVector3() == new Vector3(0, 0, 0) + || source != null && exported != null); + + if (source != null && exported != null) + { + // XYZ checks + Assert.IsTrue(source.Xyz != null && exported.Xyz != null && source.Xyz.DoubleDeltaCompare(exported.Xyz, 0.01) || source.Xyz == null && exported.Xyz == null || source.Xyz == null && exported.Xyz.ToVector3() == new Vector3(0, 0, 0) || source.Xyz.ToVector3() == new Vector3(0, 0, 0) && exported.Xyz == null); + //RPY checks + Assert.IsTrue(source?.Rpy != null && exported?.Rpy != null && RpyCheck(source.Rpy, exported.Rpy, .05) || source.Rpy == null && exported.Rpy == null || source.Rpy == null && exported.Rpy.ToVector3() == new Vector3(0, 0, 0) || source.Rpy.ToVector3() == new Vector3(0, 0, 0) && exported.Rpy == null); + } + } + + /// + /// Compares visual information of two links + /// + /// First link's visual information to be compared + /// Second link's visual information to be compared + /// + static void CompareVisual(Link.Visual source, Link.Visual exported) + { + Assert.AreEqual(source.name, exported.name, $"Name of the visual is unequal: Expected: {source.name} Actual: {exported.name}"); + CompareOrigin(source.origin, exported.origin); + CompareGeometry(source.geometry, exported.geometry); + + if (source.material != null && exported.material != null) + CompareMaterial(source.material, exported.material); + //else + // Assert.IsTrue(source.material == null && exported.material == null || source.material == null && exported.material.name == "Default-Material" || exported.material == null && source.material?.name == "Default-Material"); + } + + /// + /// Compares geometry information of two visuals + /// + /// First visual's geometry information to be compared + /// Second visual's geometry information to be compared + /// + static void CompareGeometry(Link.Geometry source, Link.Geometry exported) + { + if (source.box != null) + { + Assert.IsTrue(exported != null && source.box.size.DoubleDeltaCompare(exported.box.size, .0001), "Box size is not equal"); + return; + } + + if (source.cylinder != null) + { + Assert.IsTrue(exported.cylinder != null); + Assert.IsTrue(source.cylinder.radius.EqualsDelta(exported.cylinder.radius, .001), "Cylinder radius is not equal"); + Assert.IsTrue(source.cylinder.length.EqualsDelta(exported.cylinder.length, .001), "Cylinder length is not equal"); + return; + } + + if (source.sphere != null) + { + Assert.IsTrue(exported.sphere != null && source.sphere.radius.EqualsDelta(exported.sphere.radius, .0001), "Sphere radius is not equal"); + return; + } + + if (source.mesh != null) + { + Assert.IsTrue(exported.mesh != null); + Assert.IsTrue(Path.GetFileName(source.mesh.filename) == Path.GetFileName(exported.mesh.filename), "Filename is not same"); + if (source.mesh.scale != null && exported.mesh.scale != null) + for (var i = 0; i < source.mesh.scale.Length; i++) + Assert.IsTrue(source.mesh.scale[i].EqualsDelta(exported.mesh.scale[i], .0001), "Mesh scale is not equal"); + else + Assert.IsTrue(source.mesh.scale == null && exported.mesh.scale == null || exported.mesh.scale == null && source.mesh.scale.DoubleDeltaCompare(new double[] { 1, 1, 1 }, 0) || source.mesh.scale == null && exported.mesh.scale.DoubleDeltaCompare(new double[] { 1, 1, 1 }, 0)); + } + } + + /// + /// Compares material information of two visuals + /// + /// First visual's material information to be compared + /// Second visual's material information to be compared + /// + static void CompareMaterial(Link.Visual.Material source, Link.Visual.Material exported) + { + Assert.AreEqual(source.name, exported.name, $"Name of the material is unequal: Expected: {source.name} Actual: {exported.name}"); + + if (source.color != null && exported.color != null) + for (var i = 0; i < 3; i++) + Assert.IsTrue(source.color.rgba[i].EqualsDelta(exported.color.rgba[i], .005), "Material not equal"); + else + Assert.IsTrue(source.color == null && exported.color == null); + + if (source.texture != null && exported.texture != null) + Assert.IsTrue(source.texture.filename != exported.texture.filename, "Texture names are not equal"); + else + Assert.IsTrue(source.texture == null && source.texture == null); + } + + /// + /// Compares collision information of two links + /// + /// First link's collision information to be compared + /// Second link's collision information to be compared + /// + static void CompareCollisions(Link.Collision source, Link.Collision exported) + { + Assert.IsTrue(source.name == exported.name, "Collision mesh name not equal"); + CompareOrigin(source.origin, exported.origin); + CompareGeometry(source.geometry, exported.geometry); + } + +#if ROBOTICS_SENSORS + /// + /// Compares sensor information + /// + /// + /// + static void CompareSensors(Sensor source, Sensor exported) + { + var sourceSettings = source.elements; + var exportedSettings = exported.elements; + string poseKey = "sensor/pose"; + string zeroPose = "0 0 0 0 0 0"; + Assert.IsTrue((!sourceSettings.ContainsKey(poseKey) && !exportedSettings.ContainsKey(poseKey)) || + (sourceSettings.ContainsKey(poseKey) && !exportedSettings.ContainsKey(poseKey) && sourceSettings[poseKey] == zeroPose) || + (exportedSettings.ContainsKey(poseKey) && !sourceSettings.ContainsKey(poseKey) && exportedSettings[poseKey] == zeroPose) || + (sourceSettings.ContainsKey(poseKey) && exportedSettings.ContainsKey(poseKey))); + + if(sourceSettings.ContainsKey(poseKey) && exportedSettings.ContainsKey(poseKey)) + { + string[] sourcePose = sourceSettings[poseKey].Split(); + string[] exportedPose = exportedSettings[poseKey].Split(); + + //Pose XYZ check + Vector3 sourceXYZ = new Vector3(float.Parse(sourcePose[0]), float.Parse(sourcePose[1]), float.Parse(sourcePose[2])); + Vector3 exportedXYZ = new Vector3(float.Parse(exportedPose[0]), float.Parse(exportedPose[1]), float.Parse(exportedPose[2])); + Assert.AreEqual(sourceXYZ, exportedXYZ); + + //Pose RPY check + Vector3 sourceRPY = new Vector3(float.Parse(sourcePose[3]), float.Parse(sourcePose[4]), float.Parse(sourcePose[5])); + Vector3 exportedRPY = new Vector3(float.Parse(exportedPose[3]), float.Parse(exportedPose[4]), float.Parse(exportedPose[5])); + Quaternion sourceQuat = new Quaternion(); + sourceQuat.eulerAngles = sourceRPY; + Quaternion exportedQuat = new Quaternion(); + exportedQuat.eulerAngles = exportedRPY; + Assert.AreEqual(sourceQuat, exportedQuat); + } + + sourceSettings.Remove(poseKey); + exportedSettings.Remove(poseKey); + + string testString = ""; + string testString2 = ""; + foreach (string key in sourceSettings.Keys) + testString = string.Join(" ", testString,key); + foreach (string key in exportedSettings.Keys) + testString2 = string.Join(" ", testString2,key); + Debug.Log(testString); + Debug.Log(testString2); + + //Assert.AreEqual(sourceSettings.Count,exportedSettings.Count); // To discuss: It is possible that the sensor may prefab may have extra fields during export. + foreach (var key in sourceSettings.Keys) + { + Assert.IsTrue(exportedSettings.ContainsKey(key), $"Key Name: {key}"); + Assert.AreEqual(sourceSettings[key],exportedSettings[key]); + } + } +#endif + + /// + /// Compares axis information of two links + /// + /// First joint's axis information to be compared + /// Second joint's axis information to be compared + /// + static void CompareAxis(Joint.Axis source, Joint.Axis exported) + { + Assert.IsTrue(source == null && exported == null || (source == null && exported != null && exported.xyz.DoubleDeltaCompare(new double[] { 1, 0, 0 }, 0)) + || (exported == null && source != null && source.xyz.DoubleDeltaCompare(new double[] { 1, 0, 0 }, 0)) + || (source != null && exported !=null && source.xyz.DoubleDeltaCompare(exported.xyz, .001))); + } + + /// + /// Compares dynamics information of two links + /// + /// First link's dynamics information to be compared + /// Second link's dynamics information to be compared + /// + static void CompareDynamics(Joint.Dynamics source, Joint.Dynamics exported) + { + Assert.IsTrue(source == null && exported == null || + source != null && exported != null && + (!double.IsNaN(source.damping) && !double.IsNaN(exported.damping) && source.damping.EqualsDelta(exported.damping, .001) || double.IsNaN(source.damping) && exported.damping == 0 || double.IsNaN(exported.damping) && source.damping == 0 || double.IsNaN(source.damping) && double.IsNaN(exported.damping)) && + (!double.IsNaN(source.friction) && !double.IsNaN(exported.friction) && source.friction.EqualsDelta(exported.friction, .001) || double.IsNaN(source.friction) && exported.friction == 0 || double.IsNaN(exported.friction) && source.friction == 0 || double.IsNaN(source.friction) && double.IsNaN(exported.friction)) || + (source == null && exported.damping == 0 && exported.friction == 0 || exported == null && source.damping == 0 && source.friction == 0) + ); + } + + /// + /// Compares limit information of two joints + /// + /// First joint's limit information to be compared + /// Second joint's limit information to be compared + /// + static void CompareLimit(Joint.Limit source, Joint.Limit exported) + { + //Lower + Assert.IsFalse(double.IsNaN(source.lower) && exported.lower != 0 || source.lower != 0 && double.IsNaN(exported.lower), "Nullity Check failed"); + Assert.IsTrue(source.lower.EqualsDelta(exported.lower, .05), "Lower Limit is not equal"); + + //Upper + Assert.IsFalse(double.IsNaN(source.upper) && exported.upper != 0 || source.upper != 0 && double.IsNaN(exported.upper), "Nullity Check failed"); + Assert.IsTrue(source.upper.EqualsDelta(exported.upper, .05), "Lower Limit is not equal"); + + Assert.IsTrue(source.effort.EqualsDelta(exported.effort, .05), "Effort is not equal"); + Assert.IsTrue(source.velocity.EqualsDelta(exported.velocity, .05), "Velocity is not equal"); + } + + /// + /// Function to compare Roll, Pitch Yaw. + /// It is implemented to take into account equality of angles. + /// + /// First RPY array + /// Second RPY array + /// Amount difference allowed in comparison + /// + static bool RpyCheck(double[] source, double[] exported, double delta) + { + for (var i = 0; i < 3; i++) + if (!(source[i].EqualsDelta(exported[i], delta) || source[i] > 0 || source[i].EqualsDelta(exported[i] + 2 * Mathf.PI, delta) || source[i] <= 0 || source[i].EqualsDelta(exported[i] - 2 * Mathf.PI, delta))) + return false; + return true; + } + + [TearDown] + public void TearDown() + { + var outFailedPaths = new List(); + AssetDatabase.DeleteAssets( + new[] + { + "Packages/com.unity.robotics.urdf-importer/Tests/Runtime/Assets/URDF/cube/tmp", + "Assets/Tests/", + "Packages/com.unity.robotics.urdf-importer/Tests/Runtime/Assets/URDF/cube/Materials", + "Packages/com.unity.robotics.urdf-importer/Tests/Runtime/Assets/URDF/cube/meshes/cube_1.asset", + "Packages/com.unity.robotics.urdf-importer/Tests/Runtime/Assets/URDF/cube/meshes/cube_1.asset.meta" + }, outFailedPaths); + } + } +} diff --git a/com.unity.robotics.urdf-importer/Tests/Editor/UrdfExportTests/UrdfExportTests.cs.meta b/com.unity.robotics.urdf-importer/Tests/Editor/UrdfExportTests/UrdfExportTests.cs.meta new file mode 100644 index 00000000..0bfd3637 --- /dev/null +++ b/com.unity.robotics.urdf-importer/Tests/Editor/UrdfExportTests/UrdfExportTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: dd54eaa18f4c94b8bb2b4a13b3dbca82 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.urdf-importer/Tests/Runtime/Assets/URDF/cube/cube.urdf b/com.unity.robotics.urdf-importer/Tests/Runtime/Assets/URDF/cube/cube.urdf index ff4dcdf3..ca57a4ba 100644 --- a/com.unity.robotics.urdf-importer/Tests/Runtime/Assets/URDF/cube/cube.urdf +++ b/com.unity.robotics.urdf-importer/Tests/Runtime/Assets/URDF/cube/cube.urdf @@ -15,6 +15,7 @@ 100 + eg \ No newline at end of file diff --git a/com.unity.robotics.urdf-importer/Tests/Runtime/Assets/URDF/cube/cube_export.urdf b/com.unity.robotics.urdf-importer/Tests/Runtime/Assets/URDF/cube/cube_export.urdf new file mode 100644 index 00000000..32372df2 --- /dev/null +++ b/com.unity.robotics.urdf-importer/Tests/Runtime/Assets/URDF/cube/cube_export.urdf @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 100 + + + + + + + + + \ No newline at end of file diff --git a/com.unity.robotics.urdf-importer/Tests/Runtime/Assets/URDF/cube/cube_export.urdf.meta b/com.unity.robotics.urdf-importer/Tests/Runtime/Assets/URDF/cube/cube_export.urdf.meta new file mode 100644 index 00000000..e709d486 --- /dev/null +++ b/com.unity.robotics.urdf-importer/Tests/Runtime/Assets/URDF/cube/cube_export.urdf.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 15ecf916f0fcd4656a09b619323a0c6f +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.robotics.urdf-importer/Tests/Runtime/Unity.Robotics.UrdfImporter.Runtime.Tests.asmdef b/com.unity.robotics.urdf-importer/Tests/Runtime/Unity.Robotics.UrdfImporter.Runtime.Tests.asmdef index c7d4ab64..bbcaf035 100644 --- a/com.unity.robotics.urdf-importer/Tests/Runtime/Unity.Robotics.UrdfImporter.Runtime.Tests.asmdef +++ b/com.unity.robotics.urdf-importer/Tests/Runtime/Unity.Robotics.UrdfImporter.Runtime.Tests.asmdef @@ -23,6 +23,11 @@ "expression": "", "define": "URP_PRESENT" }, + { + "name": "com.unity.robotics.sensors", + "expression": "", + "define": "ROBOTICS_SENSORS" + }, { "name": "com.unity.render-pipelines.high-definition", "expression": "", From 2a1bad3ac8f6801f567410f69910fd4665001849 Mon Sep 17 00:00:00 2001 From: Hamid Younesy Date: Thu, 10 Mar 2022 10:49:17 -0800 Subject: [PATCH 11/12] AIRO-1651 dev-sensors: fix plugin (#184) * fix issues with loading sensor as plugin * Added GetValueOrDefault extension for ddictionary --- .../Runtime/Extensions/UrdfSensorExtension.cs | 39 ++++++++++++++----- .../Extensions/UrdfSensorsExtensions.cs | 9 ++--- .../Runtime/RosSharpDefinitions/Sensor.cs | 7 ++-- 3 files changed, 37 insertions(+), 18 deletions(-) diff --git a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorExtension.cs b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorExtension.cs index 78dd7a1e..73b3f31c 100644 --- a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorExtension.cs +++ b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorExtension.cs @@ -11,11 +11,23 @@ public static class UrdfSensorExtension { const string k_PoseKey = "sensor/pose"; const string k_NameKey = "sensor@name"; + public static string NameKey => k_NameKey; const string k_TypeKey = "sensor@type"; - const string k_PluginKey = "sensor/plugin@name"; + const string k_PluginNameKey = "sensor/plugin@name"; + const string k_PluginFilenameKey = "sensor/plugin@filename"; + public static UrdfSensor Create(Transform parent, Sensor sensor) { - GameObject sensorObject = new GameObject("unnamed"); + if (!sensor.elements.ContainsKey(k_NameKey)) + { + throw new Exception("No 'name' attribute specified for the sensor."); + } + if (!sensor.elements.ContainsKey(k_TypeKey)) + { + throw new Exception($"No 'type' attribute specified for the sensor name='{sensor.elements[k_NameKey]}'."); + } + + GameObject sensorObject = new GameObject(sensor.elements[k_NameKey]); sensorObject.transform.SetParentAndAlign(parent); UrdfSensor urdfSensor = sensorObject.AddComponent(); urdfSensor.sensorType = sensor.elements[k_TypeKey]; @@ -35,18 +47,21 @@ static void ImportSensorData(Transform sensorObject, Sensor sensor) UrdfOrigin.ImportOriginData(sensorObject.transform, origin); } - sensorObject.name = sensor.elements[k_NameKey]; - GameObject sensorGameObject; - Dictionary unusedSettings; - if (sensor.elements.ContainsKey(k_PluginKey) && String.IsNullOrEmpty(sensor.elements[k_PluginKey])) + if (string.IsNullOrEmpty(sensor.elements.GetValueOrDefault(k_PluginNameKey)) && !string.IsNullOrEmpty(sensor.elements.GetValueOrDefault(k_PluginFilenameKey))) { - sensorGameObject = SensorFactory.InstantiateSensor(sensor.elements[k_TypeKey], sensor.elements[k_PluginKey],sensor.elements, out unusedSettings); + throw new Exception($"No name attribute specified for the plugin filename='{sensor.elements[k_PluginFilenameKey]}' of sensor '{sensorObject.name}'."); } - else + + if (!string.IsNullOrEmpty(sensor.elements.GetValueOrDefault(k_PluginNameKey)) && string.IsNullOrEmpty(sensor.elements.GetValueOrDefault(k_PluginFilenameKey))) { - sensorGameObject = SensorFactory.InstantiateSensor(sensor.elements[k_TypeKey], sensor.elements, out unusedSettings); + throw new Exception($"No filename attribute specified for the plugin name='{sensor.elements[k_PluginNameKey]}' of sensor '{sensorObject.name}'."); } + Dictionary unusedSettings; + var sensorGameObject = string.IsNullOrEmpty(sensor.elements.GetValueOrDefault(k_PluginFilenameKey)) ? + SensorFactory.InstantiateSensor(sensor.elements[k_TypeKey], sensor.elements, out unusedSettings) : + SensorFactory.InstantiateSensor(sensor.elements[k_TypeKey], sensor.elements[k_PluginFilenameKey], sensor.elements, out unusedSettings); + sensorObject.GetComponent().unusedSettings = unusedSettings; sensorGameObject.transform.SetParentAndAlign(sensorObject); } @@ -70,5 +85,11 @@ public static Sensor ExportSensorData(this UrdfSensor urdfSensor) }; } } + + public static class Extensions + { + public static TValue GetValueOrDefault(this Dictionary dict,TKey key) + => dict.TryGetValue(key, out var value) ? value : default(TValue); + } } #endif diff --git a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs index 66563779..ce6330f6 100644 --- a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs +++ b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs @@ -10,7 +10,7 @@ namespace Unity.Robotics.UrdfImporter public static class UrdfSensorsExtensions { const string k_SensorTopic = "sensor/topic"; - const string k_SensorName = "sensor/name"; + public static UrdfSensors Create(Transform parent, List sensors = null) { GameObject sensorsObject = new GameObject("Sensors"); @@ -30,12 +30,11 @@ public static UrdfSensors Create(Transform parent, List sensors = null) } catch (Exception e) { - Debug.LogError($"Failed loading `{sensor.elements[k_SensorName]}`"); + Debug.LogError($"Failed loading sensor '{sensor.elements.GetValueOrDefault(UrdfSensorExtension.NameKey)}'.\n{e}"); } } } - if (parent.GetComponent() != null) { GameObject transformSensor = AddTransformSensor(parent); @@ -47,9 +46,9 @@ public static UrdfSensors Create(Transform parent, List sensors = null) static GameObject AddTransformSensor(Transform parent) { - string topicName = "/"+parent.root.name + "/" + parent.name + "/TransformStamped"; + string topicName = "/" + parent.root.name + "/" + parent.name + "/TransformStamped"; Dictionary settings = new Dictionary { { k_SensorTopic, topicName } }; - return SensorFactory.InstantiateSensor("transform",settings, out Dictionary unusedSettings); + return SensorFactory.InstantiateSensor("transform",settings, out Dictionary _); } public static List ExportSensorsData(this UrdfSensors urdfSensors) diff --git a/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Sensor.cs b/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Sensor.cs index 096d2097..0de1c2c0 100644 --- a/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Sensor.cs +++ b/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Sensor.cs @@ -8,9 +8,9 @@ namespace Unity.Robotics.UrdfImporter { public class Sensor { - static string k_BaseKey = "sensor"; - static char k_AttributeDelimit = '@'; - static char k_ElementDelimit = '/'; + const string k_BaseKey = "sensor"; + const char k_AttributeDelimit = '@'; + const char k_ElementDelimit = '/'; public Dictionary elements { get; set; } public Sensor(XElement node) @@ -28,7 +28,6 @@ public Sensor(XElement node) public Sensor() { - } public void AddAttribute(XAttribute attribute, string key) From 1667add87a9629bb1d6500143f0c4901c523389c Mon Sep 17 00:00:00 2001 From: vidurvij-Unity <60901103+vidurvij-Unity@users.noreply.github.com> Date: Thu, 7 Apr 2022 15:47:48 -0700 Subject: [PATCH 12/12] Remove addition of transform sensor during robot import.#193) --- .../Runtime/Extensions/UrdfSensorsExtensions.cs | 6 ------ 1 file changed, 6 deletions(-) diff --git a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs index ce6330f6..c7ca8081 100644 --- a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs +++ b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs @@ -35,12 +35,6 @@ public static UrdfSensors Create(Transform parent, List sensors = null) } } - if (parent.GetComponent() != null) - { - GameObject transformSensor = AddTransformSensor(parent); - transformSensor.transform.SetParentAndAlign(sensorsObject.transform); - } - return urdfSensors; }