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/.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/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 753d7a50..2c85fd53 100644 --- a/TestUrdfImporter/Packages/manifest.json +++ b/TestUrdfImporter/Packages/manifest.json @@ -1,15 +1,14 @@ { "dependencies": { "com.unity.collab-proxy": "1.5.7", - "com.unity.ide.rider": "2.0.7", + "com.unity.ide.rider": "3.0.6", "com.unity.ide.visualstudio": "2.0.8", "com.unity.ide.vscode": "1.2.3", - "com.unity.render-pipelines.universal": "10.5.0", "com.unity.robotics.urdf-importer": "file:../../com.unity.robotics.urdf-importer", "com.unity.test-framework": "1.1.24", "com.unity.testtools.codecoverage": "1.0.0", "com.unity.textmeshpro": "3.0.6", - "com.unity.timeline": "1.4.8", + "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 21a60d1a..460a71d2 100644 --- a/TestUrdfImporter/Packages/packages-lock.json +++ b/TestUrdfImporter/Packages/packages-lock.json @@ -24,11 +24,11 @@ "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" }, @@ -48,13 +48,6 @@ "dependencies": {}, "url": "https://packages.unity.com" }, - "com.unity.mathematics": { - "version": "1.1.0", - "depth": 1, - "source": "registry", - "dependencies": {}, - "url": "https://packages.unity.com" - }, "com.unity.nuget.newtonsoft-json": { "version": "2.0.0", "depth": 1, @@ -62,26 +55,6 @@ "dependencies": {}, "url": "https://packages.unity.com" }, - "com.unity.render-pipelines.core": { - "version": "10.5.0", - "depth": 1, - "source": "registry", - "dependencies": { - "com.unity.ugui": "1.0.0" - }, - "url": "https://packages.unity.com" - }, - "com.unity.render-pipelines.universal": { - "version": "10.5.0", - "depth": 0, - "source": "registry", - "dependencies": { - "com.unity.mathematics": "1.1.0", - "com.unity.render-pipelines.core": "10.5.0", - "com.unity.shadergraph": "10.5.0" - }, - "url": "https://packages.unity.com" - }, "com.unity.robotics.urdf-importer": { "version": "file:../../com.unity.robotics.urdf-importer", "depth": 0, @@ -90,30 +63,13 @@ "com.unity.editorcoroutines": "1.0.0" } }, - "com.unity.searcher": { - "version": "4.3.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.5.0", - "depth": 1, - "source": "registry", - "dependencies": { - "com.unity.render-pipelines.core": "10.5.0", - "com.unity.searcher": "4.3.2" - }, - "url": "https://packages.unity.com" - }, "com.unity.test-framework": { "version": "1.1.24", "depth": 0, @@ -145,7 +101,7 @@ "url": "https://packages.unity.com" }, "com.unity.timeline": { - "version": "1.4.8", + "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 e610e282..f015b2d3 100644 --- a/TestUrdfImporter/ProjectSettings/ProjectVersion.txt +++ b/TestUrdfImporter/ProjectSettings/ProjectVersion.txt @@ -1,2 +1,2 @@ -m_EditorVersion: 2020.3.11f1 -m_EditorVersionWithRevision: 2020.3.11f1 (99c7afb366b3) +m_EditorVersion: 2021.1.9f1 +m_EditorVersionWithRevision: 2021.1.9f1 (7a790e367ab3) diff --git a/com.unity.robotics.urdf-importer.meta b/com.unity.robotics.urdf-importer.meta new file mode 100644 index 00000000..638ccbf7 --- /dev/null +++ b/com.unity.robotics.urdf-importer.meta @@ -0,0 +1,3 @@ +fileFormatVersion: 2 +guid: 362b4d503012461693396f3dca41e51e +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 8cbfc7c2..cbb38f65 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) { @@ -37,6 +35,13 @@ 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); +#if ROBOTICS_SENSORS + UrdfSensorsExtensions.Create(linkObject.transform, link?.sensors); +#endif return linkObject; } @@ -59,7 +64,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); + } } @@ -72,10 +80,13 @@ 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() }; return link; } } -} \ No newline at end of file +} diff --git a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfRobotExtensions.cs b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfRobotExtensions.cs index 8f3d0bfc..1cb050ba 100644 --- a/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfRobotExtensions.cs +++ b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfRobotExtensions.cs @@ -16,6 +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; @@ -86,8 +90,10 @@ private static ImportPipelineData ImportPipelineInit(string filename, ImportSett { // set runtime mode back to what it was RuntimeUrdf.SetRuntimeMode(im.wasRuntimeMode); } + return null; } + return im; } @@ -115,6 +121,10 @@ 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); + AddTfBroadcaster(im.robotGameObject); +#endif } // Creates the stack of robot joints. Should be called iteratively until false is returned. @@ -136,8 +146,10 @@ private static bool ProcessJointStack(ImportPipelineData im) Link child = childJoint.ChildLink; im.importStack.Push(new Tuple(child, importedLink.transform, childJoint)); } + return true; } + return false; } @@ -213,7 +225,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); @@ -235,6 +248,7 @@ public static void CorrectAxis(GameObject robot) { return; } + Quaternion correctYtoZ = Quaternion.Euler(-90, 0, 90); Quaternion correctZtoY = Quaternion.Inverse((correctYtoZ)); Quaternion correction = new Quaternion(); @@ -262,6 +276,7 @@ public static void CorrectAxis(GameObject robot) collision.transform.localRotation = collision.transform.localRotation * correction; } } + robotScript.SetOrientation(); } @@ -278,6 +293,7 @@ private static void CreateCollisionExceptions(Robot robot, GameObject robotGameO CollisionList.Add(new CollisionIgnore(collisionObject1, collisionObject2)); } } + robotGameObject.GetComponent().collisionExceptions = CollisionList; } @@ -325,16 +341,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(); @@ -413,5 +432,35 @@ static void SetTag(GameObject go) Debug.LogError($"Unable to set the GameObject '{go.name}' tag to '{FKRobot.k_TagName}'."); } } + +#if ROBOTICS_SENSORS + static void AddJointSensor(GameObject robot) + { + Dictionary settings = new Dictionary { { "sensor/topic", robot.name + "/JointState" } }; + var sensor = SensorFactory.InstantiateSensor("joint", settings, out Dictionary unusedSettings); + if (sensor == null) + { + 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, out Dictionary unusedSettings); + if (sensor == null) + { + Debug.LogWarning("TFBroadcaster is not loaded."); + } + else + { + sensor.transform.SetParentAndAlign(robot.transform); + } + } +#endif } } 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..73b3f31c --- /dev/null +++ b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorExtension.cs @@ -0,0 +1,95 @@ +#if ROBOTICS_SENSORS +using System; +using System.Collections.Generic; +using System.Linq; +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"; + public static string NameKey => k_NameKey; + const string k_TypeKey = "sensor@type"; + const string k_PluginNameKey = "sensor/plugin@name"; + const string k_PluginFilenameKey = "sensor/plugin@filename"; + + public static UrdfSensor Create(Transform parent, Sensor sensor) + { + 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]; + 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); + } + + if (string.IsNullOrEmpty(sensor.elements.GetValueOrDefault(k_PluginNameKey)) && !string.IsNullOrEmpty(sensor.elements.GetValueOrDefault(k_PluginFilenameKey))) + { + throw new Exception($"No name attribute specified for the plugin filename='{sensor.elements[k_PluginFilenameKey]}' of sensor '{sensorObject.name}'."); + } + + if (!string.IsNullOrEmpty(sensor.elements.GetValueOrDefault(k_PluginNameKey)) && string.IsNullOrEmpty(sensor.elements.GetValueOrDefault(k_PluginFilenameKey))) + { + 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); + } + + 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 + }; + } + } + + 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/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..c7ca8081 --- /dev/null +++ b/com.unity.robotics.urdf-importer/Runtime/Extensions/UrdfSensorsExtensions.cs @@ -0,0 +1,56 @@ +#if ROBOTICS_SENSORS +using System; +using System.Collections.Generic; +using System.Linq; +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 '{sensor.elements.GetValueOrDefault(UrdfSensorExtension.NameKey)}'.\n{e}"); + } + } + } + + 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, out Dictionary _); + } + + public static List ExportSensorsData(this UrdfSensors urdfSensors) + { + UrdfSensor[] urdfSensorsList = urdfSensors.GetComponentsInChildren(); + + return urdfSensorsList.Select(urdfSensor => urdfSensor.ExportSensorData()).ToList(); + } + } +} +#endif \ No newline at end of file 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 eb999faf..9f459ef2 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 } @@ -43,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) @@ -56,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(); } @@ -75,6 +80,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 368e0778..d6c2bc9e 100644 --- a/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Robot.cs +++ b/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Robot.cs @@ -106,6 +106,7 @@ from child in node.Elements("joint") select new Joint(child); return joints.ToList(); } + private List ReadPlugins(XElement node) { 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..0de1c2c0 --- /dev/null +++ b/com.unity.robotics.urdf-importer/Runtime/RosSharpDefinitions/Sensor.cs @@ -0,0 +1,78 @@ +using System.Collections.Generic; +using System.Linq; +using System.Xml.Linq; +using System; +using System.Xml; + +namespace Unity.Robotics.UrdfImporter +{ + public class Sensor + { + const string k_BaseKey = "sensor"; + const char k_AttributeDelimit = '@'; + const char k_ElementDelimit = '/'; + 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 Sensor() + { + } + + 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().Any() && 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); + } + } + + 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/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/Runtime/Unity.Robotics.URDFImporter.asmdef b/com.unity.robotics.urdf-importer/Runtime/Unity.Robotics.URDFImporter.asmdef index 8ef6ea1d..a1aa9c3e 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", @@ -16,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 diff --git a/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfComparator.cs b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfComparator.cs index 85e3a656..da0daefd 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 1ab1b0a6..06e4d350 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; } @@ -224,6 +228,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..82e0d084 --- /dev/null +++ b/com.unity.robotics.urdf-importer/Runtime/UrdfComponents/UrdfSensor.cs @@ -0,0 +1,11 @@ +using System.Collections.Generic; +using UnityEngine; + +namespace Unity.Robotics.UrdfImporter +{ + public class UrdfSensor: MonoBehaviour + { + public string sensorType; + public Dictionary unusedSettings; + } +} 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/Editor/UrdfExportTests/UrdfExportTests.cs b/com.unity.robotics.urdf-importer/Tests/Editor/UrdfExportTests/UrdfExportTests.cs index 54f5c7f2..e4023d86 100644 --- a/com.unity.robotics.urdf-importer/Tests/Editor/UrdfExportTests/UrdfExportTests.cs +++ b/com.unity.robotics.urdf-importer/Tests/Editor/UrdfExportTests/UrdfExportTests.cs @@ -58,6 +58,10 @@ void CompareLink(Link source, Link exported) 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) { @@ -226,11 +230,69 @@ static void CompareMaterial(Link.Visual.Material source, Link.Visual.Material ex /// static void CompareCollisions(Link.Collision source, Link.Collision exported) { - Assert.AreEqual(source.name,exported.name, "Collision mesh name not equal"); + Assert.AreEqual(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 /// 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..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 @@ -13,5 +13,9 @@ + + 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 index 654c2992..db7ed8b9 100644 --- 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 @@ -34,6 +34,9 @@ + + 100 + @@ -41,4 +44,4 @@ - \ No newline at end of file + 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..8417f796 --- /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 Unity.Robotics.UrdfImporter; +using System.Xml; +using System.Xml.Linq; + + +namespace Unity.Robotics.UrdfImporter.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: 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": "", diff --git a/com.unity.robotics.urdf-importer/package.json b/com.unity.robotics.urdf-importer/package.json index e7fac5fa..4c799714 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.2-preview", + "version": "0.5.2-preview+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" - } + } }