Parcourir la source

Merge pull request #460 from aws-lumberyard-dev/urdf-importer-refactor

Updating ROS2 RobotImporter to use libsdformat
lumberyard-employee-dm il y a 1 an
Parent
commit
b1fc926126
40 fichiers modifiés avec 2050 ajouts et 899 suppressions
  1. 0 3
      Gems/ROS2/Code/CMakeLists.txt
  2. 2 1
      Gems/ROS2/Code/FindROS2.cmake
  3. 62 22
      Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp
  4. 0 1
      Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.h
  5. 73 47
      Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp
  6. 1 1
      Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.h
  7. 40 29
      Gems/ROS2/Code/Source/RobotImporter/URDF/ArticulationsMaker.cpp
  8. 3 1
      Gems/ROS2/Code/Source/RobotImporter/URDF/ArticulationsMaker.h
  9. 39 44
      Gems/ROS2/Code/Source/RobotImporter/URDF/CollidersMaker.cpp
  10. 6 5
      Gems/ROS2/Code/Source/RobotImporter/URDF/CollidersMaker.h
  11. 9 11
      Gems/ROS2/Code/Source/RobotImporter/URDF/InertialsMaker.cpp
  12. 1 1
      Gems/ROS2/Code/Source/RobotImporter/URDF/InertialsMaker.h
  13. 57 37
      Gems/ROS2/Code/Source/RobotImporter/URDF/JointsMaker.cpp
  14. 1 1
      Gems/ROS2/Code/Source/RobotImporter/URDF/JointsMaker.h
  15. 3 3
      Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.cpp
  16. 1 1
      Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.h
  17. 197 70
      Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp
  18. 8 5
      Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.h
  19. 26 72
      Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.cpp
  20. 31 12
      Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.h
  21. 44 45
      Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.cpp
  22. 9 6
      Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.h
  23. 36 0
      Gems/ROS2/Code/Source/RobotImporter/Utils/ErrorUtils.cpp
  24. 70 0
      Gems/ROS2/Code/Source/RobotImporter/Utils/ErrorUtils.h
  25. 6 1
      Gems/ROS2/Code/Source/RobotImporter/Utils/FilePath.cpp
  26. 434 189
      Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp
  27. 126 83
      Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h
  28. 22 13
      Gems/ROS2/Code/Source/RobotImporter/Utils/SourceAssetsStorage.cpp
  29. 10 9
      Gems/ROS2/Code/Source/RobotImporter/Utils/SourceAssetsStorage.h
  30. 9 9
      Gems/ROS2/Code/Source/RobotImporter/Utils/TypeConversions.cpp
  31. 4 4
      Gems/ROS2/Code/Source/RobotImporter/Utils/TypeConversions.h
  32. 40 9
      Gems/ROS2/Code/Source/RobotImporter/xacro/XacroUtils.cpp
  33. 2 2
      Gems/ROS2/Code/Source/RobotImporter/xacro/XacroUtils.h
  34. 48 30
      Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilder.cpp
  35. 3 1
      Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilder.h
  36. 42 5
      Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilderSettings.cpp
  37. 3 1
      Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilderSettings.h
  38. 578 124
      Gems/ROS2/Code/Tests/UrdfParserTest.cpp
  39. 2 0
      Gems/ROS2/Code/ros2_editor_files.cmake
  40. 2 1
      Gems/ROS2/Registry/sdfassetbuilder_settings.setreg

+ 0 - 3
Gems/ROS2/Code/CMakeLists.txt

@@ -140,9 +140,6 @@ if(PAL_TRAIT_BUILD_HOST_TOOLS)
                 3rdParty::sdformat
     )
 
-    find_package(urdfdom)
-    target_link_libraries(${gem_name}.Editor.Static PUBLIC urdfdom::urdfdom_model)
-
     ly_add_target(
         NAME ${gem_name}.Editor GEM_MODULE
         NAMESPACE Gem

+ 2 - 1
Gems/ROS2/Code/FindROS2.cmake

@@ -5,9 +5,10 @@
 
 # Note that this does not find any ros2 package in particular, but determines whether a distro is sourced properly
 # Can be extended to handle supported / unsupported distros
+message(STATUS "Ros Distro is \"$ENV{ROS_DISTRO}\"")
 if (NOT DEFINED ENV{ROS_DISTRO} OR NOT DEFINED ENV{AMENT_PREFIX_PATH})
     message(WARNING, "To build ROS2 Gem a ROS distribution needs to be sourced, but none detected")
     set(ROS2_FOUND FALSE)
     return()
 endif()
-set(ROS2_FOUND TRUE)
+set(ROS2_FOUND TRUE)

+ 62 - 22
Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp

@@ -7,14 +7,21 @@
  */
 
 #include "ROS2RobotImporterEditorSystemComponent.h"
-#include "RobotImporter/URDF/UrdfParser.h"
-#include "RobotImporter/Utils/FilePath.h"
+#include <RobotImporter/URDF/UrdfParser.h>
+#include <RobotImporter/Utils/FilePath.h>
+#include <RobotImporter/Utils/ErrorUtils.h>
 #include "RobotImporterWidget.h"
+#include <SdfAssetBuilder/SdfAssetBuilderSettings.h>
 #include <AzCore/Serialization/SerializeContext.h>
 #include <AzCore/Utils/Utils.h>
 #include <AzCore/std/chrono/chrono.h>
 #include <AzCore/std/string/string.h>
+#include <AzCore/StringFunc/StringFunc.h>
 #include <AzToolsFramework/API/ViewPaneOptions.h>
+
+
+#include <sdf/sdf.hh>
+
 #if !defined(Q_MOC_RUN)
 #include <QWindow>
 #endif
@@ -83,26 +90,38 @@ namespace ROS2
     {
         if (filePath.empty())
         {
-            AZ_Warning("ROS2EditorSystemComponent", false, "Path provided for prefab is empty");
+            AZ_Warning("ROS2RobotImporterEditorSystemComponent", false, "Path provided for prefab is empty");
             return false;
         }
         if (Utils::IsFileXacro(filePath))
         {
-            AZ_Warning("ROS2EditorSystemComponent", false, "XACRO formatted files are not supported");
+            AZ_Warning("ROS2RobotImporterEditorSystemComponent", false, "XACRO formatted files are not supported");
             return false;
         }
 
-        urdf::ModelInterfaceSharedPtr parsedUrdf = UrdfParser::ParseFromFile(filePath);
-        if (!parsedUrdf)
+        // Read the SDF Settings from the Settings Registry into a local struct
+        SdfAssetBuilderSettings sdfBuilderSettings;
+        sdfBuilderSettings.LoadSettings();
+        // Set the parser config settings for URDF content
+        sdf::ParserConfig parserConfig;
+        parserConfig.URDFSetPreserveFixedJoint(sdfBuilderSettings.m_urdfPreserveFixedJoints);
+
+        auto parsedUrdfOutcome = UrdfParser::ParseFromFile(filePath, parserConfig);
+        if (!parsedUrdfOutcome)
         {
-            const auto log = UrdfParser::GetUrdfParsingLog();
-            AZ_Warning("ROS2EditorSystemComponent", false, "URDF parsing failed. Refer to %s", log.c_str());
+            const AZStd::string log = Utils::JoinSdfErrorsToString(parsedUrdfOutcome.error());
+
+            AZ_Warning("ROS2RobotImporterEditorSystemComponent", false, "URDF parsing failed with errors:\nRefer to %s",
+                log.c_str());
             return false;
         }
 
-        auto collidersNames = Utils::GetMeshesFilenames(parsedUrdf->getRoot(), false, true);
-        auto visualNames = Utils::GetMeshesFilenames(parsedUrdf->getRoot(), true, false);
-        auto meshNames = Utils::GetMeshesFilenames(parsedUrdf->getRoot(), true, true);
+        // Urdf Root has been parsed successfully retrieve it from the Outcome
+        const sdf::Root& parsedUrdfRoot = parsedUrdfOutcome.value();
+
+        auto collidersNames = Utils::GetMeshesFilenames(&parsedUrdfRoot, false, true);
+        auto visualNames = Utils::GetMeshesFilenames(&parsedUrdfRoot, true, false);
+        auto meshNames = Utils::GetMeshesFilenames(&parsedUrdfRoot, true, true);
         AZStd::shared_ptr<Utils::UrdfAssetMap> urdfAssetsMapping = AZStd::make_shared<Utils::UrdfAssetMap>();
         if (importAssetWithUrdf)
         {
@@ -126,7 +145,7 @@ namespace ROS2
 
             if (loopTime - loopStartTime > assetLoopTimeout)
             {
-                AZ_Warning("ROS2EditorSystemComponent", false, "Loop waiting for assets timed out");
+                AZ_Warning("ROS2RobotImporterEditorSystemComponent", false, "Loop waiting for assets timed out");
                 break;
             }
 
@@ -136,19 +155,19 @@ namespace ROS2
                 auto sourceAssetFullPath = asset.m_availableAssetInfo.m_sourceAssetGlobalPath;
                 if (sourceAssetFullPath.empty())
                 {
-                    AZ_Warning("ROS2EditorSystemComponent", false, "Asset %s missing `sourceAssetFullPath`", name.c_str());
+                    AZ_Warning("ROS2RobotImporterEditorSystemComponent", false, "Asset %s missing `sourceAssetFullPath`", name.c_str());
                     continue;
                 }
                 using namespace AzToolsFramework;
                 using namespace AzToolsFramework::AssetSystem;
                 AZ::Outcome<AssetSystem::JobInfoContainer> result = AZ::Failure();
                 AssetSystemJobRequestBus::BroadcastResult(
-                    result, &AssetSystemJobRequestBus::Events::GetAssetJobsInfo, sourceAssetFullPath, true);
+                    result, &AssetSystemJobRequestBus::Events::GetAssetJobsInfo, sourceAssetFullPath.Native(), true);
 
                 if (!result.IsSuccess())
                 {
                     assetProcessorFailed = true;
-                    AZ_Error("ROS2EditorSystemComponent", false, "Asset System failed to reply with jobs infos");
+                    AZ_Error("ROS2RobotImporterEditorSystemComponent", false, "Asset System failed to reply with jobs infos");
                     break;
                 }
 
@@ -157,34 +176,55 @@ namespace ROS2
                 {
                     if (job.m_status == JobStatus::Queued || job.m_status == JobStatus::InProgress)
                     {
-                        AZ_Printf("ROS2EditorSystemComponent", "asset %s is being processed", sourceAssetFullPath.c_str());
+                        AZ_Printf("ROS2RobotImporterEditorSystemComponent", "asset %s is being processed", sourceAssetFullPath.c_str());
                         allAssetProcessed = false;
                     }
                     else
                     {
-                        AZ_Printf("ROS2EditorSystemComponent", "asset %s is done", sourceAssetFullPath.c_str());
+                        AZ_Printf("ROS2RobotImporterEditorSystemComponent", "asset %s is done", sourceAssetFullPath.c_str());
                     }
                 }
             }
 
             if (allAssetProcessed && !assetProcessorFailed)
             {
-                AZ_Printf("ROS2EditorSystemComponent", "All assets processed");
+                AZ_Printf("ROS2RobotImporterEditorSystemComponent", "All assets processed");
             }
         };
 
-        AZStd::string prefabName = AZStd::string(parsedUrdf->getName().c_str(), parsedUrdf->getName().size()) + ".prefab";
+        // Use the name of the first model tag in the SDF for the prefab
+        // Otherwise use the name of the first world tag in the SDF
+        AZStd::string prefabName;
+        if (const sdf::Model* model = parsedUrdfRoot.Model();
+            model != nullptr)
+        {
+            prefabName = AZStd::string(model->Name().c_str(), model->Name().size()) + ".prefab";
+        }
+
+        if (uint64_t urdfWorldCount = parsedUrdfRoot.WorldCount();
+            prefabName.empty() && urdfWorldCount > 0)
+        {
+            const sdf::World* parsedUrdfWorld = parsedUrdfRoot.WorldByIndex(0);
+            prefabName = AZStd::string(parsedUrdfWorld->Name().c_str(), parsedUrdfWorld->Name().size()) + ".prefab";
+        }
+
+        if (prefabName.empty())
+        {
+            AZ_Error("ROS2RobotImporterEditorSystemComponent", false, "URDF file converted to SDF %.*s contains no worlds."
+                " O3DE Prefab cannot be created", AZ_STRING_ARG(filePath));
+            return false;
+        }
+
 
         const AZ::IO::Path prefabPathRelative(AZ::IO::Path("Assets") / "Importer" / prefabName);
         const AZ::IO::Path prefabPath(AZ::IO::Path(AZ::Utils::GetProjectPath()) / prefabPathRelative);
-        AZStd::unique_ptr<URDFPrefabMaker> prefabMaker;
-        prefabMaker = AZStd::make_unique<URDFPrefabMaker>(filePath, parsedUrdf, prefabPath.String(), urdfAssetsMapping, useArticulation);
+        AZStd::unique_ptr<URDFPrefabMaker> prefabMaker = AZStd::make_unique<URDFPrefabMaker>(filePath, &parsedUrdfRoot, prefabPath.String(), urdfAssetsMapping, useArticulation);
 
         auto prefabOutcome = prefabMaker->CreatePrefabFromURDF();
 
         if (!prefabOutcome.IsSuccess())
         {
-            AZ_Error("ROS2EditorSystemComponent", false, "Unable to create Prefab from URDF file %s", filePath.data());
+            AZ_Error("ROS2RobotImporterEditorSystemComponent", false, "Unable to create Prefab from URDF file %s", filePath.data());
             return false;
         }
 

+ 0 - 1
Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.h

@@ -13,7 +13,6 @@
 #include <AzToolsFramework/Entity/EditorEntityContextBus.h>
 #include <ROS2/RobotImporter/RobotImporterBus.h>
 #include <RobotImporter/Utils/SourceAssetsStorage.h>
-#include <urdf_parser/urdf_parser.h>
 namespace ROS2
 {
 

+ 73 - 47
Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp

@@ -12,10 +12,12 @@
 #include <AzCore/Utils/Utils.h>
 
 #include "RobotImporterWidget.h"
-#include "URDF/URDFPrefabMaker.h"
-#include "URDF/UrdfParser.h"
-#include "Utils/FilePath.h"
-#include "Utils/RobotImporterUtils.h"
+#include <SdfAssetBuilder/SdfAssetBuilderSettings.h>
+#include <URDF/URDFPrefabMaker.h>
+#include <URDF/UrdfParser.h>
+#include <Utils/FilePath.h>
+#include <Utils/RobotImporterUtils.h>
+#include <Utils/ErrorUtils.h>
 #include <QApplication>
 #include <QScreen>
 #include <QTranslator>
@@ -81,73 +83,90 @@ namespace ROS2
 
     void RobotImporterWidget::OpenUrdf()
     {
+        UrdfParser::RootObjectOutcome parsedUrdfOutcome(AZStd::unexpect);
         QString report;
         if (!m_urdfPath.empty())
         {
             if (Utils::IsFileXacro(m_urdfPath))
             {
                 Utils::xacro::ExecutionOutcome outcome = Utils::xacro::ParseXacro(m_urdfPath.String(), m_params);
+                // Store off the URDF parsing outcome which will be output later in this function
+                parsedUrdfOutcome = outcome.m_urdfHandle;
                 if (outcome)
                 {
-                    m_parsedUrdf = outcome.m_urdfHandle;
                     report += "# " + tr("XACRO execution succeeded") + "\n";
                     m_assetPage->ClearAssetsList();
                 }
                 else
                 {
-                    report += "# " + tr("XACRO parsing failed") + "\n";
-                    report += "\n\n## " + tr("Command called") + "\n\n`" + QString::fromUtf8(outcome.m_called.data()) + "`";
-                    report += "\n\n" + tr("Process failed");
-                    report += "\n\n## " + tr("Error output") + "\n\n";
-                    report += "```\n";
-                    if (outcome.m_logErrorOutput.size())
+                    if (outcome.m_succeed)
                     {
-                        report +=
-                            QString::fromLocal8Bit(outcome.m_logErrorOutput.data(), static_cast<int>(outcome.m_logErrorOutput.size()));
+                        report += "# " + tr("XACRO execution succeeded, but URDF parsing failed") + "\n";
                     }
                     else
                     {
-                        report += tr("(EMPTY)");
+                        report += "# " + tr("XACRO parsing failed") + "\n";
+                        report += "\n\n## " + tr("Command called") + "\n\n`" + QString::fromUtf8(outcome.m_called.data()) + "`";
+                        report += "\n\n" + tr("Process failed");
+                        report += "\n\n## " + tr("Error output") + "\n\n";
+                        report += "```\n";
+                        if (outcome.m_logErrorOutput.size())
+                        {
+                            report +=
+                                QString::fromUtf8(outcome.m_logErrorOutput.data(), static_cast<int>(outcome.m_logErrorOutput.size()));
+                        }
+                        else
+                        {
+                            report += tr("(EMPTY)");
+                        }
+                        report += "\n```";
+                        report += "\n\n## " + tr("Standard output") + "\n\n";
+                        report += "```\n";
+                        if (outcome.m_logStandardOutput.size())
+                        {
+                            report += QString::fromUtf8(
+                                outcome.m_logStandardOutput.data(), static_cast<int>(outcome.m_logStandardOutput.size()));
+                        }
+                        else
+                        {
+                            report += tr("(EMPTY)");
+                        }
+                        report += "\n```";
+                        m_checkUrdfPage->ReportURDFResult(report, false);
+                        return;
                     }
-                    report += "\n```";
-                    report += "\n\n## " + tr("Standard output") + "\n\n";
-                    report += "```\n";
-                    if (outcome.m_logStandardOutput.size())
-                    {
-                        report += QString::fromLocal8Bit(
-                            outcome.m_logStandardOutput.data(), static_cast<int>(outcome.m_logStandardOutput.size()));
-                    }
-                    else
-                    {
-                        report += tr("(EMPTY)");
-                    }
-                    report += "\n```";
-                    m_checkUrdfPage->ReportURDFResult(report, false);
-                    m_parsedUrdf = nullptr;
-                    return;
                 }
             }
             else if (Utils::IsFileUrdf(m_urdfPath))
             {
                 // standard URDF
-                m_parsedUrdf = UrdfParser::ParseFromFile(m_urdfPath.Native());
+                // Read the SDF Settings from the Settings Registry into a local struct
+                SdfAssetBuilderSettings sdfBuilderSettings;
+                sdfBuilderSettings.LoadSettings();
+                // Set the parser config settings for URDF content
+                sdf::ParserConfig parserConfig;
+                parserConfig.URDFSetPreserveFixedJoint(sdfBuilderSettings.m_urdfPreserveFixedJoints);
+                parsedUrdfOutcome = UrdfParser::ParseFromFile(m_urdfPath, parserConfig);
             }
             else
             {
                 AZ_Assert(false, "Unknown file extension : %s \n", m_urdfPath.c_str());
             }
-            const auto log = UrdfParser::GetUrdfParsingLog();
-            if (m_parsedUrdf)
+            AZStd::string log;
+            bool urdfParsedSuccess = parsedUrdfOutcome.has_value();
+            if (urdfParsedSuccess)
             {
+                m_parsedUrdf = AZStd::move(parsedUrdfOutcome.value());
                 report += "# " + tr("The URDF was parsed and opened successfully") + "\n";
                 m_prefabMaker.reset();
                 // Report the status of skipping this page
                 AZ_Printf("Wizard", "Wizard skips m_checkUrdfPage since there is no errors in URDF\n");
-                m_meshNames = Utils::GetMeshesFilenames(m_parsedUrdf->getRoot(), true, true);
+                m_meshNames = Utils::GetMeshesFilenames(&m_parsedUrdf, true, true);
                 m_assetPage->ClearAssetsList();
             }
             else
             {
+                log = Utils::JoinSdfErrorsToString(parsedUrdfOutcome.error());
                 report += "# " + tr("The URDF was not opened") + "\n";
                 report += tr("URDF parser returned following errors:") + "\n\n";
             }
@@ -157,7 +176,7 @@ namespace ROS2
                 report += QString::fromUtf8(log.data(), int(log.size()));
                 report += "`";
             }
-            m_checkUrdfPage->ReportURDFResult(report, m_parsedUrdf != nullptr);
+            m_checkUrdfPage->ReportURDFResult(report, urdfParsedSuccess);
         }
     }
 
@@ -177,10 +196,10 @@ namespace ROS2
 
     void RobotImporterWidget::FillAssetPage()
     {
-        if (m_parsedUrdf && m_assetPage->IsEmpty())
+        if (m_parsedUrdf.Model() != nullptr && m_assetPage->IsEmpty())
         {
-            auto collidersNames = Utils::GetMeshesFilenames(m_parsedUrdf->getRoot(), false, true);
-            auto visualNames = Utils::GetMeshesFilenames(m_parsedUrdf->getRoot(), true, false);
+            auto collidersNames = Utils::GetMeshesFilenames(&m_parsedUrdf, false, true);
+            auto visualNames = Utils::GetMeshesFilenames(&m_parsedUrdf, true, false);
 
             AZ::Uuid::FixedString dirSuffix;
             if (!m_params.empty())
@@ -226,7 +245,7 @@ namespace ROS2
                     AZStd::string sourcePath(kNotFoundAz);
                     AZStd::string resolvedPath(kNotFoundAz);
                     QString productAssetText;
-                    auto crc = AZ::Crc32();
+                    AZ::Crc32 crc;
                     QString tooltip = kNotFound;
                     bool visual = visualNames.contains(meshPath);
                     bool collider = collidersNames.contains(meshPath);
@@ -247,8 +266,8 @@ namespace ROS2
                     {
                         const auto& asset = m_urdfAssetsMapping->at(meshPath);
                         sourceAssetUuid = asset.m_availableAssetInfo.m_sourceGuid;
-                        sourcePath = asset.m_availableAssetInfo.m_sourceAssetRelativePath;
-                        resolvedPath = asset.m_resolvedUrdfPath.data();
+                        sourcePath = asset.m_availableAssetInfo.m_sourceAssetRelativePath.String();
+                        resolvedPath = asset.m_resolvedUrdfPath.String();
                         crc = asset.m_urdfFileCRC;
                         tooltip = QString::fromUtf8(resolvedPath.data(), resolvedPath.size());
                     }
@@ -265,9 +284,9 @@ namespace ROS2
 
     void RobotImporterWidget::FillPrefabMakerPage()
     {
-        if (m_parsedUrdf)
+        if (m_parsedUrdf.Model() != nullptr)
         {
-            AZStd::string robotName = AZStd::string(m_parsedUrdf->getName().c_str(), m_parsedUrdf->getName().size()) + ".prefab";
+            AZStd::string robotName = AZStd::string(m_parsedUrdf.Model()->Name().c_str(), m_parsedUrdf.Model()->Name().size()) + ".prefab";
             m_prefabMakerPage->setProposedPrefabName(robotName);
             QWizard::button(PrefabCreationButtonId)->setText(tr("Create Prefab"));
             QWizard::setOption(HavePrefabCreationButton, true);
@@ -276,6 +295,8 @@ namespace ROS2
 
     bool RobotImporterWidget::validateCurrentPage()
     {
+        // If SDF file are desired to be brought in via the RobotImporter workflow
+        // an OpenSdf function would need to be added
         if (currentPage() == m_fileSelectPage)
         {
             m_params.clear();
@@ -287,7 +308,8 @@ namespace ROS2
                 m_xacroParamsPage->SetXacroParameters(m_params);
             }
             // no need to wait for param page - parse urdf now, nextId will skip unnecessary pages
-            if (m_params.empty())
+            if (const bool isFileUrdfOrXacro = Utils::IsFileXacro(m_urdfPath) || Utils::IsFileUrdf(m_urdfPath);
+                m_params.empty() && isFileUrdfOrXacro)
             {
                 OpenUrdf();
             }
@@ -296,7 +318,11 @@ namespace ROS2
         if (currentPage() == m_xacroParamsPage)
         {
             m_params = m_xacroParamsPage->GetXacroParameters();
-            OpenUrdf();
+            if (const bool isFileUrdfOrXacro = Utils::IsFileXacro(m_urdfPath) || Utils::IsFileUrdf(m_urdfPath);
+                isFileUrdfOrXacro)
+            {
+                OpenUrdf();
+            }
         }
         if (currentPage() == m_introPage)
         {
@@ -323,7 +349,7 @@ namespace ROS2
     {
         if ((currentPage() == m_fileSelectPage && m_params.empty()) || currentPage() == m_xacroParamsPage)
         {
-            if (m_parsedUrdf)
+            if (m_parsedUrdf.Model() != nullptr)
             {
                 if (m_meshNames.size() == 0)
                 {
@@ -372,7 +398,7 @@ namespace ROS2
         const bool useArticulation = m_prefabMakerPage->IsUseArticulations();
         m_prefabMaker = AZStd::make_unique<URDFPrefabMaker>(
             m_urdfPath.String(),
-            m_parsedUrdf,
+            &m_parsedUrdf,
             prefabPath.String(),
             m_urdfAssetsMapping,
             useArticulation,

+ 1 - 1
Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.h

@@ -69,7 +69,7 @@ namespace ROS2
         PrefabMakerPage* m_prefabMakerPage;
         XacroParamsPage* m_xacroParamsPage;
         AZ::IO::Path m_urdfPath;
-        urdf::ModelInterfaceSharedPtr m_parsedUrdf;
+        sdf::Root m_parsedUrdf{};
 
         //! User's choice to copy meshes during urdf import
         bool m_importAssetWithUrdf{ false };

+ 40 - 29
Gems/ROS2/Code/Source/RobotImporter/URDF/ArticulationsMaker.cpp

@@ -7,9 +7,10 @@
  */
 
 #include "ArticulationsMaker.h"
-#include "RobotImporter/Utils/DefaultSolverConfiguration.h"
 #include <AzCore/Component/EntityId.h>
 #include <AzToolsFramework/Entity/EditorEntityHelpers.h>
+#include <RobotImporter/Utils/DefaultSolverConfiguration.h>
+#include <RobotImporter/Utils/RobotImporterUtils.h>
 #include <RobotImporter/Utils/TypeConversions.h>
 #include <Source/EditorArticulationLinkComponent.h>
 
@@ -19,51 +20,59 @@ namespace ROS2
     namespace
     {
         using ArticulationCfg = PhysX::EditorArticulationLinkConfiguration;
-        static const AZStd::unordered_map<int, PhysX::ArticulationJointType> SupportedJointTypes{ {
-            { urdf::Joint::REVOLUTE, PhysX::ArticulationJointType::Hinge },
-            { urdf::Joint::CONTINUOUS, PhysX::ArticulationJointType::Hinge },
-            { urdf::Joint::PRISMATIC, PhysX::ArticulationJointType::Prismatic },
-            { urdf::Joint::FIXED, PhysX::ArticulationJointType::Fix },
+        static const AZStd::unordered_map<sdf::JointType, PhysX::ArticulationJointType> SupportedJointTypes{ {
+            { sdf::JointType::REVOLUTE, PhysX::ArticulationJointType::Hinge },
+            { sdf::JointType::CONTINUOUS, PhysX::ArticulationJointType::Hinge },
+            { sdf::JointType::PRISMATIC, PhysX::ArticulationJointType::Prismatic },
+            { sdf::JointType::FIXED, PhysX::ArticulationJointType::Fix },
         } };
     } // namespace
 
-    ArticulationCfg& AddToArticulationConfig(ArticulationCfg& articulationLinkConfiguration, const urdf::JointSharedPtr joint)
+    ArticulationCfg& AddToArticulationConfig(ArticulationCfg& articulationLinkConfiguration, const sdf::Joint* joint)
     {
         if (!joint)
         {
             return articulationLinkConfiguration;
         }
-        auto supportedArticulationType = SupportedJointTypes.find(joint->type);
+        auto supportedArticulationType = SupportedJointTypes.find(joint->Type());
         AZ_Warning(
             "ArticulationsMaker",
             supportedArticulationType != SupportedJointTypes.end(),
             "Articulations do not support type %d for URDF joint %s.",
-            joint->type,
-            joint->name.c_str());
+            static_cast<int>(joint->Type()),
+            joint->Name().c_str());
         if (supportedArticulationType != SupportedJointTypes.end())
         {
             const auto type = supportedArticulationType->second;
             articulationLinkConfiguration.m_articulationJointType = type;
             const AZ::Vector3 o3deJointDir{ 1.0, 0.0, 0.0 };
-            const AZ::Vector3 jointAxis = URDF::TypeConversions::ConvertVector3(joint->axis);
-            const auto quaternion =
-                jointAxis.IsZero() ? AZ::Quaternion::CreateIdentity() : AZ::Quaternion::CreateShortestArc(o3deJointDir, jointAxis);
+            AZ::Vector3 jointCoordinateAxis = AZ::Vector3::CreateZero();
+            auto quaternion = AZ::Quaternion::CreateIdentity();
+
+            const sdf::JointAxis* jointAxis = joint->Axis();
+            if (jointAxis != nullptr)
+            {
+                jointCoordinateAxis = URDF::TypeConversions::ConvertVector3(jointAxis->Xyz());
+                quaternion =
+                    jointCoordinateAxis.IsZero() ? AZ::Quaternion::CreateIdentity() : AZ::Quaternion::CreateShortestArc(o3deJointDir, jointCoordinateAxis);
+            }
+
             const AZ::Vector3 rotation = quaternion.GetEulerDegrees();
             articulationLinkConfiguration.m_localRotation = rotation;
 
-            if (joint->limits)
+            if (jointAxis)
             {
                 if (type == PhysX::ArticulationJointType::Hinge)
                 {
-                    const double limitUpper = AZ::RadToDeg(joint->limits->upper);
-                    const double limitLower = AZ::RadToDeg(joint->limits->lower);
+                    const double limitUpper = AZ::RadToDeg(jointAxis->Upper());
+                    const double limitLower = AZ::RadToDeg(jointAxis->Lower());
                     articulationLinkConfiguration.m_angularLimitNegative = limitLower;
                     articulationLinkConfiguration.m_angularLimitPositive = limitUpper;
                 }
                 else if (type == PhysX::ArticulationJointType::Prismatic)
                 {
-                    articulationLinkConfiguration.m_linearLimitLower = joint->limits->upper;
-                    articulationLinkConfiguration.m_linearLimitUpper = joint->limits->lower;
+                    articulationLinkConfiguration.m_linearLimitLower = jointAxis->Upper();
+                    articulationLinkConfiguration.m_linearLimitUpper = jointAxis->Lower();
                 }
             }
             else
@@ -74,28 +83,24 @@ namespace ROS2
         return articulationLinkConfiguration;
     }
 
-    ArticulationCfg& AddToArticulationConfig(ArticulationCfg& articulationLinkConfiguration, const urdf::InertialSharedPtr inertial)
+    ArticulationCfg& AddToArticulationConfig(ArticulationCfg& articulationLinkConfiguration, const gz::math::Inertiald& inertial)
     {
-        if (!inertial)
-        {
-            return articulationLinkConfiguration;
-        }
         articulationLinkConfiguration.m_solverPositionIterations =
             AZStd::max(articulationLinkConfiguration.m_solverPositionIterations, URDF::DefaultNumberPosSolver);
         articulationLinkConfiguration.m_solverVelocityIterations =
             AZStd::max(articulationLinkConfiguration.m_solverVelocityIterations, URDF::DefaultNumberVelSolver);
 
-        articulationLinkConfiguration.m_mass = inertial->mass;
-        articulationLinkConfiguration.m_centerOfMassOffset = URDF::TypeConversions::ConvertVector3(inertial->origin.position);
+        articulationLinkConfiguration.m_mass = inertial.MassMatrix().Mass();
+        articulationLinkConfiguration.m_centerOfMassOffset = URDF::TypeConversions::ConvertVector3(inertial.Pose().Pos());
 
-        if (!URDF::TypeConversions::ConvertQuaternion(inertial->origin.rotation).IsIdentity())
+        if (!URDF::TypeConversions::ConvertQuaternion(inertial.Pose().Rot()).IsIdentity())
         { // There is a rotation component in URDF that we are not able to apply
             AZ_Warning("AddArticulationLink", false, "Ignoring URDF inertial origin rotation (no such field in rigid body configuration)");
         }
         return articulationLinkConfiguration;
     }
 
-    void ArticulationsMaker::AddArticulationLink(const urdf::LinkSharedPtr link, AZ::EntityId entityId) const
+    void ArticulationsMaker::AddArticulationLink(const sdf::Model& model, const sdf::Link* link, AZ::EntityId entityId) const
     {
         AZ::Entity* entity = AzToolsFramework::GetEntityById(entityId);
         AZ_Assert(entity, "No entity for id %s", entityId.ToString().c_str());
@@ -103,8 +108,14 @@ namespace ROS2
         AZ_Trace("ArticulationsMaker", "Processing inertial for entity id: %s\n", entityId.ToString().c_str());
         PhysX::EditorArticulationLinkConfiguration articulationLinkConfiguration;
 
-        articulationLinkConfiguration = AddToArticulationConfig(articulationLinkConfiguration, link->inertial);
-        articulationLinkConfiguration = AddToArticulationConfig(articulationLinkConfiguration, link->parent_joint);
+        articulationLinkConfiguration = AddToArticulationConfig(articulationLinkConfiguration, link->Inertial());
+
+        constexpr bool getNestedModelJoints = true;
+        AZStd::string linkName(link->Name().c_str(), link->Name().size());
+        for (const sdf::Joint* joint : Utils::GetJointsForChildLink(model, linkName, getNestedModelJoints))
+        {
+            articulationLinkConfiguration = AddToArticulationConfig(articulationLinkConfiguration, joint);
+        }
 
         entity->CreateComponent<PhysX::EditorArticulationLinkComponent>(articulationLinkConfiguration);
     }

+ 3 - 1
Gems/ROS2/Code/Source/RobotImporter/URDF/ArticulationsMaker.h

@@ -20,8 +20,10 @@ namespace ROS2
     {
     public:
         //! Add zero or one inertial and joints elements to a given entity (depending on link content).
+        //! @param model SDF model object which can be queried to locate the joints needed to determine if the supplied
+        //!              link is a child link within a joint
         //! @param link A pointer to a parsed URDF link.
         //! @param entityId A non-active entity which will be populated according to inertial content.
-        void AddArticulationLink(urdf::LinkSharedPtr link, AZ::EntityId entityId) const;
+        void AddArticulationLink(const sdf::Model& model, const sdf::Link* link, AZ::EntityId entityId) const;
     };
 } // namespace ROS2

+ 39 - 44
Gems/ROS2/Code/Source/RobotImporter/URDF/CollidersMaker.cpp

@@ -64,41 +64,41 @@ namespace ROS2
         }
     }
 
-    void CollidersMaker::BuildColliders(urdf::LinkSharedPtr link)
+    void CollidersMaker::BuildColliders(const sdf::Link* link)
     {
-        for (auto collider : link->collision_array)
+        if (!link)
         {
-            BuildCollider(collider);
+            return;
         }
 
-        if (link->collision_array.empty())
+        for (uint64_t index = 0; index < link->CollisionCount(); index++)
         {
-            BuildCollider(link->collision);
+            BuildCollider(link->CollisionByIndex(index));
         }
     }
 
-    void CollidersMaker::BuildCollider(urdf::CollisionSharedPtr collision)
+    void CollidersMaker::BuildCollider(const sdf::Collision* collision)
     {
         if (!collision)
         { // it is ok not to have collision in a link
             return;
         }
 
-        auto geometry = collision->geometry;
-        bool isPrimitiveShape = geometry->type != urdf::Geometry::MESH;
+        auto geometry = collision->Geom();
+        bool isPrimitiveShape = geometry->Type() != sdf::GeometryType::MESH;
         if (!isPrimitiveShape)
         {
-            auto meshGeometry = std::dynamic_pointer_cast<urdf::Mesh>(geometry);
+            auto meshGeometry = geometry->MeshShape();
             if (!meshGeometry)
             {
                 return;
             }
-            const auto asset = PrefabMakerUtils::GetAssetFromPath(*m_urdfAssetsMapping, meshGeometry->filename);
+            const auto asset = PrefabMakerUtils::GetAssetFromPath(*m_urdfAssetsMapping, meshGeometry->Uri());
             if (!asset)
             {
                 return;
             }
-            const AZStd::string& azMeshPath = asset->m_sourceAssetGlobalPath;
+            const auto& azMeshPath = asset->m_sourceAssetGlobalPath;
 
             AZStd::shared_ptr<AZ::SceneAPI::Containers::Scene> scene;
             AZ::SceneAPI::Events::SceneSerializationBus::BroadcastResult(
@@ -110,7 +110,7 @@ namespace ROS2
                     false,
                     "Error loading collider. Invalid scene: %s, URDF path: %s",
                     azMeshPath.c_str(),
-                    meshGeometry->filename.c_str());
+                    meshGeometry->Uri().c_str());
                 return;
             }
 
@@ -205,13 +205,13 @@ namespace ROS2
         }
     }
 
-    void CollidersMaker::AddColliders(urdf::LinkSharedPtr link, AZ::EntityId entityId)
+    void CollidersMaker::AddColliders(const sdf::Model& model, const sdf::Link* link, AZ::EntityId entityId)
     {
         AZStd::string typeString = "collider";
-        const bool isWheelEntity = Utils::IsWheelURDFHeuristics(link);
+        const bool isWheelEntity = Utils::IsWheelURDFHeuristics(model, link);
         if (isWheelEntity)
         {
-            AZ_Printf(Internal::CollidersMakerLoggingTag, "Due to its name, %s is considered a wheel entity\n", link->name.c_str());
+            AZ_Printf(Internal::CollidersMakerLoggingTag, "Due to its name, %s is considered a wheel entity\n", link->Name().c_str());
             if (!m_wheelMaterial.GetId().IsValid())
             {
                 FindWheelMaterial();
@@ -220,21 +220,16 @@ namespace ROS2
         const AZ::Data::Asset<Physics::MaterialAsset> materialAsset =
             isWheelEntity ? m_wheelMaterial : AZ::Data::Asset<Physics::MaterialAsset>();
         size_t nameSuffixIndex = 0; // For disambiguation when multiple unnamed colliders are present. The order does not matter here
-        for (auto collider : link->collision_array)
+        for (uint64_t index = 0; index < link->CollisionCount(); index++)
         { // Add colliders (if any) from the collision array
             AddCollider(
-                collider, entityId, PrefabMakerUtils::MakeEntityName(link->name.c_str(), typeString, nameSuffixIndex), materialAsset);
+                link->CollisionByIndex(index), entityId, PrefabMakerUtils::MakeEntityName(link->Name().c_str(), typeString, nameSuffixIndex), materialAsset);
             nameSuffixIndex++;
         }
-
-        if (nameSuffixIndex == 0)
-        { // If there are no colliders in the array, the element member is used instead
-            AddCollider(link->collision, entityId, PrefabMakerUtils::MakeEntityName(link->name.c_str(), typeString), materialAsset);
-        }
     }
 
     void CollidersMaker::AddCollider(
-        urdf::CollisionSharedPtr collision,
+        const sdf::Collision* collision,
         AZ::EntityId entityId,
         const AZStd::string& generatedName,
         const AZ::Data::Asset<Physics::MaterialAsset>& materialAsset)
@@ -245,7 +240,7 @@ namespace ROS2
         }
         AZ_Trace(Internal::CollidersMakerLoggingTag, "Processing collisions for entity id:%s\n", entityId.ToString().c_str());
 
-        auto geometry = collision->geometry;
+        auto geometry = collision->Geom();
         if (!geometry)
         { // non-empty visual should have a geometry
             AZ_Warning(Internal::CollidersMakerLoggingTag, false, "No Geometry for a collider of entity %s", entityId.ToString().c_str());
@@ -256,25 +251,25 @@ namespace ROS2
     }
 
     void CollidersMaker::AddColliderToEntity(
-        urdf::CollisionSharedPtr collision, AZ::EntityId entityId, const AZ::Data::Asset<Physics::MaterialAsset>& materialAsset) const
+        const sdf::Collision* collision, AZ::EntityId entityId, const AZ::Data::Asset<Physics::MaterialAsset>& materialAsset) const
     {
         AZ::Entity* entity = AzToolsFramework::GetEntityById(entityId);
         AZ_Assert(entity, "AddColliderToEntity called with invalid entityId");
-        auto geometry = collision->geometry;
-        bool isPrimitiveShape = geometry->type != urdf::Geometry::MESH;
+        auto geometry = collision->Geom();
+        bool isPrimitiveShape = geometry->Type() != sdf::GeometryType::MESH;
 
         Physics::ColliderConfiguration colliderConfig;
 
         colliderConfig.m_materialSlots.SetMaterialAsset(0, materialAsset);
-        colliderConfig.m_position = URDF::TypeConversions::ConvertVector3(collision->origin.position);
-        colliderConfig.m_rotation = URDF::TypeConversions::ConvertQuaternion(collision->origin.rotation);
+        colliderConfig.m_position = URDF::TypeConversions::ConvertVector3(collision->RawPose().Pos());
+        colliderConfig.m_rotation = URDF::TypeConversions::ConvertQuaternion(collision->RawPose().Rot());
         if (!isPrimitiveShape)
         {
             AZ_Printf(Internal::CollidersMakerLoggingTag, "Adding mesh collider to %s\n", entityId.ToString().c_str());
-            auto meshGeometry = std::dynamic_pointer_cast<urdf::Mesh>(geometry);
+            auto meshGeometry = geometry->MeshShape();
             AZ_Assert(meshGeometry, "geometry is not meshGeometry");
 
-            auto asset = PrefabMakerUtils::GetAssetFromPath(*m_urdfAssetsMapping, meshGeometry->filename);
+            auto asset = PrefabMakerUtils::GetAssetFromPath(*m_urdfAssetsMapping, meshGeometry->Uri());
             if (!asset)
             {
                 return;
@@ -310,28 +305,28 @@ namespace ROS2
             return;
         }
 
-        AZ_Printf(Internal::CollidersMakerLoggingTag, "URDF geometry type: %d\n", geometry->type);
-        switch (geometry->type)
+        AZ_Printf(Internal::CollidersMakerLoggingTag, "URDF geometry type: %d\n", (int)geometry->Type());
+        switch (geometry->Type())
         {
-        case urdf::Geometry::SPHERE:
+        case sdf::GeometryType::SPHERE:
             {
-                auto sphereGeometry = std::dynamic_pointer_cast<urdf::Sphere>(geometry);
+                auto sphereGeometry = geometry->SphereShape();
                 AZ_Assert(sphereGeometry, "geometry is not sphereGeometry");
-                const Physics::SphereShapeConfiguration cfg{ static_cast<float>(sphereGeometry->radius) };
+                const Physics::SphereShapeConfiguration cfg{ static_cast<float>(sphereGeometry->Radius()) };
                 entity->CreateComponent<PhysX::EditorColliderComponent>(colliderConfig, cfg);
             }
             break;
-        case urdf::Geometry::BOX:
+        case sdf::GeometryType::BOX:
             {
-                const auto boxGeometry = std::dynamic_pointer_cast<urdf::Box>(geometry);
+                const auto boxGeometry = geometry->BoxShape();
                 AZ_Assert(boxGeometry, "geometry is not boxGeometry");
-                const Physics::BoxShapeConfiguration cfg{ URDF::TypeConversions::ConvertVector3(boxGeometry->dim) };
+                const Physics::BoxShapeConfiguration cfg{ URDF::TypeConversions::ConvertVector3(boxGeometry->Size()) };
                 entity->CreateComponent<PhysX::EditorColliderComponent>(colliderConfig, cfg);
             }
             break;
-        case urdf::Geometry::CYLINDER:
+        case sdf::GeometryType::CYLINDER:
             {
-                auto cylinderGeometry = std::dynamic_pointer_cast<urdf::Cylinder>(geometry);
+                auto cylinderGeometry = geometry->CylinderShape();
                 AZ_Assert(cylinderGeometry, "geometry is not cylinderGeometry");
                 Physics::BoxShapeConfiguration cfg;
                 auto* component = entity->CreateComponent<PhysX::EditorColliderComponent>(colliderConfig, cfg);
@@ -345,11 +340,11 @@ namespace ROS2
                     PhysX::EditorPrimitiveColliderComponentRequestBus::Event(
                         AZ::EntityComponentIdPair(entityId, component->GetId()),
                         &PhysX::EditorPrimitiveColliderComponentRequests::SetCylinderHeight,
-                        cylinderGeometry->length);
+                        cylinderGeometry->Length());
                     PhysX::EditorPrimitiveColliderComponentRequestBus::Event(
                         AZ::EntityComponentIdPair(entityId, component->GetId()),
                         &PhysX::EditorPrimitiveColliderComponentRequests::SetCylinderRadius,
-                        cylinderGeometry->radius);
+                        cylinderGeometry->Radius());
                     PhysX::EditorPrimitiveColliderComponentRequestBus::Event(
                         AZ::EntityComponentIdPair(entityId, component->GetId()),
                         &PhysX::EditorPrimitiveColliderComponentRequests::SetCylinderSubdivisionCount,
@@ -363,7 +358,7 @@ namespace ROS2
             }
             break;
         default:
-            AZ_Warning(Internal::CollidersMakerLoggingTag, false, "Unsupported collider geometry type: %d", geometry->type);
+            AZ_Warning(Internal::CollidersMakerLoggingTag, false, "Unsupported collider geometry type: %d", (int)geometry->Type());
             break;
         }
     }

+ 6 - 5
Gems/ROS2/Code/Source/RobotImporter/URDF/CollidersMaker.h

@@ -39,25 +39,26 @@ namespace ROS2
 
         //! Builds .pxmeshes for every collider in link collider mesh.
         //! @param link A parsed URDF tree link node which could hold information about colliders.
-        void BuildColliders(urdf::LinkSharedPtr link);
+        void BuildColliders(const sdf::Link* link);
         //! Add zero, one or many collider elements (depending on link content).
+        //! @param model An SDF model object provided by libsdformat from a parsed URDF
         //! @param link A parsed URDF tree link node which could hold information about colliders.
         //! @param entityId A non-active entity which will be affected.
-        void AddColliders(urdf::LinkSharedPtr link, AZ::EntityId entityId);
+        void AddColliders(const sdf::Model& model, const sdf::Link* link, AZ::EntityId entityId);
         //! Sends meshes required for colliders to asset processor.
         //! @param buildReadyCb Function to call when the processing finishes.
         void ProcessMeshes(BuildReadyCallback notifyBuildReadyCb);
 
     private:
         void FindWheelMaterial();
-        void BuildCollider(urdf::CollisionSharedPtr collision);
+        void BuildCollider(const sdf::Collision* collision);
         void AddCollider(
-            urdf::CollisionSharedPtr collision,
+            const sdf::Collision* collision,
             AZ::EntityId entityId,
             const AZStd::string& generatedName,
             const AZ::Data::Asset<Physics::MaterialAsset>& materialAsset);
         void AddColliderToEntity(
-            urdf::CollisionSharedPtr collision, AZ::EntityId entityId, const AZ::Data::Asset<Physics::MaterialAsset>& materialAsset) const;
+            const sdf::Collision* collision, AZ::EntityId entityId, const AZ::Data::Asset<Physics::MaterialAsset>& materialAsset) const;
 
         AZ::Data::Asset<Physics::MaterialAsset> m_wheelMaterial;
         AZStd::shared_ptr<Utils::UrdfAssetMap> m_urdfAssetsMapping;

+ 9 - 11
Gems/ROS2/Code/Source/RobotImporter/URDF/InertialsMaker.cpp

@@ -15,12 +15,8 @@
 
 namespace ROS2
 {
-    void InertialsMaker::AddInertial(urdf::InertialSharedPtr inertial, AZ::EntityId entityId) const
+    void InertialsMaker::AddInertial(const gz::math::Inertiald& inertial, AZ::EntityId entityId) const
     {
-        if (!inertial)
-        { // it is ok not to have inertia in a link
-            return;
-        }
         AZ_Trace("AddInertial", "Processing inertial for entity id: %s\n", entityId.ToString().c_str());
 
         AZ::Entity* entity = AzToolsFramework::GetEntityById(entityId);
@@ -31,22 +27,24 @@ namespace ROS2
         physxSpecificConfiguration.m_solverVelocityIterations =
             AZStd::max(physxSpecificConfiguration.m_solverVelocityIterations, URDF::DefaultNumberVelSolver);
 
-        rigidBodyConfiguration.m_mass = inertial->mass;
+        rigidBodyConfiguration.m_mass = inertial.MassMatrix().Mass();
         rigidBodyConfiguration.m_computeMass = false;
 
-        rigidBodyConfiguration.m_centerOfMassOffset = URDF::TypeConversions::ConvertVector3(inertial->origin.position);
+        rigidBodyConfiguration.m_centerOfMassOffset = URDF::TypeConversions::ConvertVector3(inertial.Pose().Pos());
         rigidBodyConfiguration.m_computeCenterOfMass = false;
 
-        if (!URDF::TypeConversions::ConvertQuaternion(inertial->origin.rotation).IsIdentity())
+        if (!URDF::TypeConversions::ConvertQuaternion(inertial.Pose().Rot()).IsIdentity())
         { // There is a rotation component in URDF that we are not able to apply
             AZ_Warning("AddInertial", false, "Ignoring URDF inertial origin rotation (no such field in rigid body configuration)");
         }
 
+        auto moi = inertial.Moi();
+
         // Inertia tensor is symmetrical
         auto inertiaMatrix = AZ::Matrix3x3::CreateFromRows(
-            AZ::Vector3(inertial->ixx, inertial->ixy, inertial->ixz),
-            AZ::Vector3(inertial->ixy, inertial->iyy, inertial->iyz),
-            AZ::Vector3(inertial->ixz, inertial->iyz, inertial->izz));
+            AZ::Vector3(moi(0, 0), moi(0, 1), moi(0, 2)),
+            AZ::Vector3(moi(0, 1), moi(1, 1), moi(1, 2)),
+            AZ::Vector3(moi(0, 2), moi(1, 2), moi(2, 2)));
         rigidBodyConfiguration.m_inertiaTensor = inertiaMatrix;
         rigidBodyConfiguration.m_computeInertiaTensor = false;
 

+ 1 - 1
Gems/ROS2/Code/Source/RobotImporter/URDF/InertialsMaker.h

@@ -20,6 +20,6 @@ namespace ROS2
         //! Add zero or one inertial elements to a given entity (depending on link content).
         //! @param inertial A pointer to a parsed URDF inertial structure, might be null.
         //! @param entityId A non-active entity which will be populated according to inertial content.
-        void AddInertial(urdf::InertialSharedPtr inertial, AZ::EntityId entityId) const;
+        void AddInertial(const gz::math::Inertiald& inertial, AZ::EntityId entityId) const;
     };
 } // namespace ROS2

+ 57 - 37
Gems/ROS2/Code/Source/RobotImporter/URDF/JointsMaker.cpp

@@ -18,9 +18,8 @@
 
 namespace ROS2
 {
-
     JointsMaker::JointsMakerResult JointsMaker::AddJointComponent(
-        urdf::JointSharedPtr joint, AZ::EntityId followColliderEntityId, AZ::EntityId leadColliderEntityId) const
+        const sdf::Joint* joint, AZ::EntityId followColliderEntityId, AZ::EntityId leadColliderEntityId) const
     {
         AZ::Entity* followColliderEntity = AzToolsFramework::GetEntityById(followColliderEntityId);
         PhysX::EditorJointComponent* jointComponent = nullptr;
@@ -30,10 +29,16 @@ namespace ROS2
         // with Euler angles can be applied to configure the desirable direction of the joint. A quaternion that transforms a unit vector X
         // {1,0,0} to a vector given by the URDF joint need to be found. Heavily suboptimal element in this conversion is needed of
         // converting the unit quaternion to Euler vector.
-        const AZ::Vector3 o3de_joint_dir{ 1.0, 0.0, 0.0 };
-        const AZ::Vector3 joint_axis = URDF::TypeConversions::ConvertVector3(joint->axis);
-        const auto quaternion =
-            joint_axis.IsZero() ? AZ::Quaternion::CreateIdentity() : AZ::Quaternion::CreateShortestArc(o3de_joint_dir, joint_axis);
+        const AZ::Vector3 o3deJointDir{ 1.0, 0.0, 0.0 };
+        const sdf::JointAxis* jointAxis = joint->Axis();
+        AZ::Vector3 jointCoordinateAxis = AZ::Vector3::CreateZero();
+        auto quaternion = AZ::Quaternion::CreateIdentity();
+        if (jointAxis != nullptr)
+        {
+            jointCoordinateAxis = URDF::TypeConversions::ConvertVector3(jointAxis->Xyz());
+            quaternion =
+                jointCoordinateAxis.IsZero() ? AZ::Quaternion::CreateIdentity() : AZ::Quaternion::CreateShortestArc(o3deJointDir, jointCoordinateAxis);
+        }
 
         AZ_Printf(
             "JointsMaker",
@@ -44,14 +49,14 @@ namespace ROS2
             quaternion.GetW());
         const AZ::Vector3 rotation = quaternion.GetEulerDegrees();
 
-        switch (joint->type)
+        switch (joint->Type())
         {
-        case urdf::Joint::FIXED:
+        case sdf::JointType::FIXED:
             {
                 jointComponent = followColliderEntity->CreateComponent<PhysX::EditorFixedJointComponent>();
             }
             break;
-        case urdf::Joint::PRISMATIC:
+        case sdf::JointType::PRISMATIC:
             {
                 jointComponent = followColliderEntity->CreateComponent<PhysX::EditorPrismaticJointComponent>();
                 followColliderEntity->Activate();
@@ -62,16 +67,18 @@ namespace ROS2
                     PhysX::JointsComponentModeCommon::ParameterNames::Rotation,
                     rotation);
 
-                PhysX::EditorJointRequestBus::Event(
-                    AZ::EntityComponentIdPair(followColliderEntityId, jointComponent->GetId()),
-                    &PhysX::EditorJointRequests::SetLinearValuePair,
-                    PhysX::JointsComponentModeCommon::ParameterNames::LinearLimits,
-                    PhysX::AngleLimitsFloatPair(joint->limits->upper, joint->limits->lower));
-
+                if (jointAxis != nullptr)
+                {
+                    PhysX::EditorJointRequestBus::Event(
+                        AZ::EntityComponentIdPair(followColliderEntityId, jointComponent->GetId()),
+                        &PhysX::EditorJointRequests::SetLinearValuePair,
+                        PhysX::JointsComponentModeCommon::ParameterNames::LinearLimits,
+                        PhysX::AngleLimitsFloatPair(jointAxis->Upper(), jointAxis->Lower()));
+                }
                 followColliderEntity->Deactivate();
             }
             break;
-        case urdf::Joint::CONTINUOUS:
+        case sdf::JointType::CONTINUOUS:
             { // Implemented as Hinge with angular limit disabled
                 jointComponent = followColliderEntity->CreateComponent<PhysX::EditorHingeJointComponent>();
                 followColliderEntity->Activate();
@@ -89,35 +96,48 @@ namespace ROS2
                 followColliderEntity->Deactivate();
             }
             break;
-        case urdf::Joint::REVOLUTE:
+        case sdf::JointType::REVOLUTE:
             { // Hinge
                 jointComponent = followColliderEntity->CreateComponent<PhysX::EditorHingeJointComponent>();
                 followColliderEntity->Activate();
 
-                const double limitUpper = AZ::RadToDeg(joint->limits->upper);
-                const double limitLower = AZ::RadToDeg(joint->limits->lower);
-                AZ_Printf(
-                    "JointsMaker",
-                    "Setting limits : upper: %.1f lower: %.1f (URDF:%f,%f)",
-                    limitUpper,
-                    limitLower,
-                    joint->limits->upper,
-                    joint->limits->lower);
-                PhysX::EditorJointRequestBus::Event(
-                    AZ::EntityComponentIdPair(followColliderEntityId, jointComponent->GetId()),
-                    [&rotation, &limitLower, &limitUpper](PhysX::EditorJointRequests* editorJointRequest)
-                    {
-                        editorJointRequest->SetVector3Value(PhysX::JointsComponentModeCommon::ParameterNames::Rotation, rotation);
-                        editorJointRequest->SetLinearValuePair(
-                            PhysX::JointsComponentModeCommon::ParameterNames::TwistLimits,
-                            PhysX::AngleLimitsFloatPair(limitUpper, limitLower));
-                    });
+                if (jointAxis != nullptr)
+                {
+                    using LimitType = decltype(jointAxis->Upper());
+                    const bool enableJointLimits = jointAxis->Upper() != AZStd::numeric_limits<LimitType>::infinity()
+                        || jointAxis->Lower() != -AZStd::numeric_limits<LimitType>::infinity();
+                    const double limitUpper = jointAxis->Upper() != AZStd::numeric_limits<LimitType>::infinity()
+                        ? AZ::RadToDeg(jointAxis->Upper())
+                        : AZ::RadToDeg(AZ::Constants::TwoPi);
+                    const double limitLower = jointAxis->Lower() != -AZStd::numeric_limits<LimitType>::infinity()
+                        ? AZ::RadToDeg(jointAxis->Lower())
+                        : -AZ::RadToDeg(AZ::Constants::TwoPi);
+                    AZ_Printf(
+                        "JointsMaker",
+                        "Setting limits : upper: %.1f lower: %.1f (URDF:%f,%f)",
+                        limitUpper,
+                        limitLower,
+                        jointAxis->Upper(),
+                        jointAxis->Lower());
+                    PhysX::EditorJointRequestBus::Event(
+                        AZ::EntityComponentIdPair(followColliderEntityId, jointComponent->GetId()),
+                        [&rotation, enableJointLimits, &limitLower, &limitUpper](PhysX::EditorJointRequests* editorJointRequest)
+                        {
+                            editorJointRequest->SetVector3Value(PhysX::JointsComponentModeCommon::ParameterNames::Rotation, rotation);
+                            editorJointRequest->SetLinearValuePair(
+                                PhysX::JointsComponentModeCommon::ParameterNames::TwistLimits,
+                                PhysX::AngleLimitsFloatPair(limitUpper, limitLower));
+                            editorJointRequest->SetBoolValue(
+                                PhysX::JointsComponentModeCommon::ParameterNames::EnableLimits, enableJointLimits);
+                        });
+                }
                 followColliderEntity->Deactivate();
             }
             break;
         default:
-            AZ_Warning("AddJointComponent", false, "Unknown or unsupported joint type %d for joint %s", joint->type, joint->name.c_str());
-            return AZ::Failure(AZStd::string::format("unsupported joint type : %d", joint->type));
+            AZ_Warning("AddJointComponent", false, "Unknown or unsupported joint type %d for joint %s",
+                static_cast<int>(joint->Type()), joint->Name().c_str());
+            return AZ::Failure(AZStd::string::format("unsupported joint type : %d", static_cast<int>(joint->Type())));
         }
 
         followColliderEntity->Activate();

+ 1 - 1
Gems/ROS2/Code/Source/RobotImporter/URDF/JointsMaker.h

@@ -28,6 +28,6 @@ namespace ROS2
         //! @param leadColliderEntityId An entity higher in hierarchy which is connected through the joint with the child entity.
         //! @returns created components Id or string with fail
         JointsMakerResult AddJointComponent(
-            urdf::JointSharedPtr joint, AZ::EntityId followColliderEntityId, AZ::EntityId leadColliderEntityId) const;
+            const sdf::Joint* joint, AZ::EntityId followColliderEntityId, AZ::EntityId leadColliderEntityId) const;
     };
 } // namespace ROS2

+ 3 - 3
Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.cpp

@@ -45,10 +45,10 @@ namespace ROS2::PrefabMakerUtils
         return assetPath;
     }
 
-    void SetEntityTransformLocal(const urdf::Pose& origin, AZ::EntityId entityId)
+    void SetEntityTransformLocal(const gz::math::Pose3d& origin, AZ::EntityId entityId)
     {
-        urdf::Vector3 urdfPosition = origin.position;
-        urdf::Rotation urdfRotation = origin.rotation;
+        gz::math::Vector3 urdfPosition = origin.Pos();
+        gz::math::Quaternion urdfRotation = origin.Rot();
         AZ::Quaternion azRotation = URDF::TypeConversions::ConvertQuaternion(urdfRotation);
         AZ::Vector3 azPosition = URDF::TypeConversions::ConvertVector3(urdfPosition);
         AZ::Transform tf(azPosition, azRotation, 1.0f);

+ 1 - 1
Gems/ROS2/Code/Source/RobotImporter/URDF/PrefabMakerUtils.h

@@ -32,7 +32,7 @@ namespace ROS2::PrefabMakerUtils
     //! Set the transform for an entity.
     //! @param origin pose for the entity to set.
     //! @param entityId entity which will be modified.
-    void SetEntityTransformLocal(const urdf::Pose& origin, AZ::EntityId entityId);
+    void SetEntityTransformLocal(const gz::math::Pose3d& origin, AZ::EntityId entityId);
 
     //! Create a prefab entity in a hierarchy. The new entity will not yet be active.
     //! @param parentEntityId id of parent entity for this new entity.

+ 197 - 70
Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp

@@ -32,13 +32,13 @@ namespace ROS2
 {
     URDFPrefabMaker::URDFPrefabMaker(
         const AZStd::string& modelFilePath,
-        urdf::ModelInterfaceSharedPtr model,
+        const sdf::Root* root,
         AZStd::string prefabPath,
         const AZStd::shared_ptr<Utils::UrdfAssetMap> urdfAssetsMapping,
         bool useArticulations,
         const AZStd::optional<AZ::Transform> spawnPosition)
-        : m_model(model)
-        , m_visualsMaker(model->materials_, urdfAssetsMapping)
+        : m_root(root)
+        , m_visualsMaker{}
         , m_collidersMaker(urdfAssetsMapping)
         , m_prefabPath(std::move(prefabPath))
         , m_urdfAssetsMapping(urdfAssetsMapping)
@@ -46,16 +46,74 @@ namespace ROS2
         , m_useArticulations(useArticulations)
     {
         AZ_Assert(!m_prefabPath.empty(), "Prefab path is empty");
-        AZ_Assert(m_model, "Model is nullptr");
+        AZ_Assert(m_root, "SDF Root is nullptr");
+        if (m_root != nullptr)
+        {
+            AZ_Assert(GetFirstModel(), "SDF Model is nullptr");
+
+            VisualsMaker::MaterialNameMap materialMap;
+            auto GetVisualsFromModel = [&materialMap](const sdf::Model& model)
+            {
+                for (uint64_t linkIndex{}; linkIndex < model.LinkCount(); ++linkIndex)
+                {
+                    if (const sdf::Link* link = model.LinkByIndex(linkIndex); link != nullptr)
+                    {
+                        for (uint64_t visualIndex{}; visualIndex < link->VisualCount(); ++visualIndex)
+                        {
+                            if (const sdf::Visual* visual = link->VisualByIndex(visualIndex); visual != nullptr)
+                            {
+                                if (const sdf::Material* material = visual->Material(); material != nullptr)
+                                {
+                                    const std::string visualName = visual->Name();
+                                    materialMap.emplace(AZStd::string{ visualName.c_str(), visualName.size() }, material);
+                                }
+                            }
+                        }
+                    }
+                }
+            };
+
+            // Iterate over all visuals to get their materials
+            for (uint64_t worldIndex{}; worldIndex < m_root->WorldCount(); ++worldIndex)
+            {
+                if (const sdf::World* world = m_root->WorldByIndex(worldIndex); world != nullptr)
+                {
+                    for (uint64_t modelIndex{}; modelIndex < world->ModelCount(); ++modelIndex)
+                    {
+                        if (const sdf::Model* model = world->ModelByIndex(modelIndex); model != nullptr)
+                        {
+                            GetVisualsFromModel(*model);
+                        }
+                    }
+                }
+            }
+            // If there is a model tag at the root, iterate over it now
+            if (const sdf::Model* model = m_root->Model(); model != nullptr)
+            {
+                GetVisualsFromModel(*model);
+            }
+            m_visualsMaker = VisualsMaker(AZStd::move(materialMap), urdfAssetsMapping);
+        }
     }
 
-    void URDFPrefabMaker::BuildAssetsForLink(urdf::LinkSharedPtr link)
+    void URDFPrefabMaker::BuildAssetsForLink(const sdf::Link* link)
     {
         m_collidersMaker.BuildColliders(link);
-        for (auto childLink : link->child_links)
+        // Find the links which are childen in a joint where this link
+        // is a parent
+        auto BuildAssetsFromJointChildLinks = [this](const sdf::Joint& joint)
         {
-            BuildAssetsForLink(childLink);
-        }
+            const sdf::Model& model = *GetFirstModel();
+            if (const sdf::Link* childLink = model.LinkByName(joint.ChildName());
+                childLink != nullptr)
+            {
+                BuildAssetsForLink(childLink);
+            }
+
+            return true;
+        };
+        constexpr bool visitNestedModelLinks = true;
+        Utils::VisitJoints(*GetFirstModel(), BuildAssetsFromJointChildLinks, visitNestedModelLinks);
     }
 
     URDFPrefabMaker::CreatePrefabTemplateResult URDFPrefabMaker::CreatePrefabTemplateFromURDF()
@@ -65,28 +123,19 @@ namespace ROS2
             m_status.clear();
         }
 
-        if (!m_model)
+        if (GetFirstModel() == nullptr)
         {
             return AZ::Failure(AZStd::string("Null model."));
         }
 
         // Build up a list of all entities created as a part of processing the file.
         AZStd::vector<AZ::EntityId> createdEntities;
-
         AZStd::unordered_map<AZStd::string, AzToolsFramework::Prefab::PrefabEntityResult> createdLinks;
-        AzToolsFramework::Prefab::PrefabEntityResult createEntityRoot = AddEntitiesForLink(m_model->root_link_, AZ::EntityId(), createdEntities);
-        AZStd::string rootName(m_model->root_link_->name.c_str(), m_model->root_link_->name.size());
-        createdLinks[rootName] = createEntityRoot;
-        if (!createEntityRoot.IsSuccess())
-        {
-            return AZ::Failure(AZStd::string(createEntityRoot.GetError()));
-        }
-
-        auto links = Utils::GetAllLinks(m_model->root_link_->child_links);
+        auto links = Utils::GetAllLinks(*GetFirstModel(), true);
 
         for (const auto& [name, linkPtr] : links)
         {
-            createdLinks[name] = AddEntitiesForLink(linkPtr, createEntityRoot.GetValue(), createdEntities);
+            createdLinks[name] = AddEntitiesForLink(linkPtr, AZ::EntityId{}, createdEntities);
         }
 
         for (const auto& [name, result] : createdLinks)
@@ -110,11 +159,11 @@ namespace ROS2
         // Set the transforms of links
         for (const auto& [name, linkPtr] : links)
         {
-            const auto this_entry = createdLinks.at(name);
-            if (this_entry.IsSuccess())
+            if (const auto thisEntry = createdLinks.at(name);
+                thisEntry.IsSuccess())
             {
                 AZ::Transform tf = Utils::GetWorldTransformURDF(linkPtr);
-                auto* entity = AzToolsFramework::GetEntityById(this_entry.GetValue());
+                auto* entity = AzToolsFramework::GetEntityById(thisEntry.GetValue());
                 if (entity)
                 {
                     auto* transformInterface = entity->FindComponent<AzToolsFramework::Components::TransformComponent>();
@@ -124,7 +173,7 @@ namespace ROS2
                             "CreatePrefabFromURDF",
                             "Setting transform %s %s to [%f %f %f] [%f %f %f %f]\n",
                             name.c_str(),
-                            this_entry.GetValue().ToString().c_str(),
+                            thisEntry.GetValue().ToString().c_str(),
                             tf.GetTranslation().GetX(),
                             tf.GetTranslation().GetY(),
                             tf.GetTranslation().GetZ(),
@@ -143,54 +192,97 @@ namespace ROS2
         }
 
         // Set the hierarchy
-        for (const auto& [name, linkPtr] : links)
+        AZStd::vector<AZ::EntityId> linkEntityIdsWithoutParent;
+        for (const auto& [linkName, linkPtr] : links)
         {
-            const auto thisEntry = createdLinks.at(name);
-            if (!thisEntry.IsSuccess())
+            const auto linkPrefabResult = createdLinks.at(linkName);
+            if (!linkPrefabResult.IsSuccess())
             {
-                AZ_Trace("CreatePrefabFromURDF", "Link %s creation failed\n", name.c_str());
+                AZ_Trace("CreatePrefabFromURDF", "Link %s creation failed\n", linkName.c_str());
                 continue;
             }
-            auto parentPtr = linkPtr->getParent();
-            if (!parentPtr)
+
+            AZStd::vector<const sdf::Joint*> jointsWhereLinkIsChild = Utils::GetJointsForChildLink(*GetFirstModel(),
+                linkName, true);
+            if (jointsWhereLinkIsChild.empty())
             {
-                AZ_Trace("CreatePrefabFromURDF", "Link %s has no parents\n", name.c_str());
+                // emplace unique entry to the container of links that don't have a parent link associated with it
+                if (auto existingLinkIt =
+                        AZStd::find(linkEntityIdsWithoutParent.begin(), linkEntityIdsWithoutParent.end(), linkPrefabResult.GetValue());
+                    existingLinkIt == linkEntityIdsWithoutParent.end())
+                {
+                    linkEntityIdsWithoutParent.emplace_back(linkPrefabResult.GetValue());
+                }
+                AZ_Trace("CreatePrefabFromURDF", "Link %s has no parents\n", linkName.c_str());
                 continue;
             }
-            AZStd::string parentName(parentPtr->name.c_str(), parentPtr->name.size());
+
+            // For URDF, a link can only be child in a single joint
+            // a link can't be a child of two other links as URDF models a tree structure and not a graph
+            /*
+                Here is a snippet from the Pose Frame Semantics documentation for SDFormat that explains the differences
+                between URDF and SDF coordinate frame
+                http://sdformat.org/tutorials?tut=pose_frame_semantics&ver=1.5#parent-frames-in-urdf
+                > The most significant difference between URDF and SDFormat coordinate frames is related to links and joints.
+                > While SDFormat allows kinematic loops with the topology of a directed graph,
+                > URDF kinematics must have the topology of a directed tree, with each link being the child of at most one joint.
+                > URDF coordinate frames are defined recursively based on this tree structure, with each joint's <origin/> tag
+                > defining the coordinate transformation from the parent link frame to the child link frame.
+            */
+
+            AZStd::string parentName(jointsWhereLinkIsChild.front()->ParentName().c_str(),
+                jointsWhereLinkIsChild.front()->ParentName().size());
             const auto parentEntry = createdLinks.find(parentName);
             if (parentEntry == createdLinks.end())
             {
-                AZ_Trace("CreatePrefabFromURDF", "Link %s has invalid parent name %s\n", name.c_str(), parentName.c_str());
+                AZ_Trace("CreatePrefabFromURDF", "Link %s has invalid parent name %s\n", linkName.c_str(), parentName.c_str());
                 continue;
             }
             if (!parentEntry->second.IsSuccess())
             {
-                AZ_Trace("CreatePrefabFromURDF", "Link %s has parent %s which has failed to create\n", name.c_str(), parentName.c_str());
+                AZ_Trace("CreatePrefabFromURDF", "Link %s has parent %s which has failed to create\n", linkName.c_str(), parentName.c_str());
                 continue;
             }
             AZ_Trace(
                 "CreatePrefabFromURDF",
                 "Link %s setting parent to %s\n",
-                thisEntry.GetValue().ToString().c_str(),
+                linkPrefabResult.GetValue().ToString().c_str(),
                 parentEntry->second.GetValue().ToString().c_str());
-            AZ_Trace("CreatePrefabFromURDF", "Link %s setting parent to %s\n", name.c_str(), parentName.c_str());
-            PrefabMakerUtils::SetEntityParent(thisEntry.GetValue(), parentEntry->second.GetValue());
+            AZ_Trace("CreatePrefabFromURDF", "Link %s setting parent to %s\n", linkName.c_str(), parentName.c_str());
+            PrefabMakerUtils::SetEntityParent(linkPrefabResult.GetValue(), parentEntry->second.GetValue());
         }
 
-        auto joints = Utils::GetAllJoints(m_model->root_link_->child_links);
-        for (const auto& [name, jointPtr] : joints)
+        auto JointVisitor = [this, &createdLinks](const sdf::Joint& joint)
         {
-            AZ_Assert(jointPtr, "joint %s is null", name.c_str());
+            auto jointPtr = &joint;
+
+            const std::string& jointName = jointPtr->Name();
+            AZStd::string azJointName(jointName.c_str(), jointName.size());
+            AZStd::string parentLinkName(jointPtr->ParentName().c_str(), jointPtr->ParentName().size());
+            AZStd::string childLinkName(jointPtr->ChildName().c_str(), jointPtr->ChildName().size());
+
+            auto parentLinkIter = createdLinks.find(parentLinkName);
+            if (parentLinkIter == createdLinks.end())
+            {
+                AZ_Warning("CreatePrefabFromURDF", false, "Joint %s has no parent link %s. Cannot create", azJointName.c_str(), parentLinkName.c_str());
+                return true;
+            }
+            auto leadEntity = parentLinkIter->second;
+
+            auto childLinkIter = createdLinks.find(childLinkName);
+            if (childLinkIter == createdLinks.end())
+            {
+                AZ_Warning("CreatePrefabFromURDF", false, "Joint %s has no child link %s. Cannot create", azJointName.c_str(), childLinkName.c_str());
+                return true;
+            }
+            auto childEntity = childLinkIter->second;
+
             AZ_Trace(
                 "CreatePrefabFromURDF",
                 "Creating joint %s : %s -> %s\n",
-                name.c_str(),
-                jointPtr->parent_link_name.c_str(),
-                jointPtr->child_link_name.c_str());
-
-            auto leadEntity = createdLinks.at(jointPtr->parent_link_name.c_str());
-            auto childEntity = createdLinks.at(jointPtr->child_link_name.c_str());
+                azJointName.c_str(),
+                parentLinkName.c_str(),
+                childLinkName.c_str());
 
 
             AZ::Entity* childEntityPtr = AzToolsFramework::GetEntityById(childEntity.GetValue());
@@ -199,7 +291,7 @@ namespace ROS2
                 auto* component = Utils::GetGameOrEditorComponent<ROS2FrameComponent>(childEntityPtr);
                 if (component)
                 {
-                    component->SetJointName(AZStd::string(name.c_str(), name.length()));
+                    component->SetJointName(azJointName);
                 }
             }
             // check if both has RigidBody and we are not creating articulation
@@ -208,18 +300,29 @@ namespace ROS2
                 AZStd::lock_guard<AZStd::mutex> lck(m_statusLock);
                 auto result = m_jointsMaker.AddJointComponent(jointPtr, childEntity.GetValue(), leadEntity.GetValue());
                 m_status.emplace(
-                    name, AZStd::string::format(" %s %llu", result.IsSuccess() ? "created as" : "Failed", result.GetValue()));
+                    azJointName, AZStd::string::format(" %s %llu", result.IsSuccess() ? "created as" : "Failed", result.GetValue()));
             }
             else
             {
-                AZ_Warning("CreatePrefabFromURDF", false, "cannot create joint %s", name.c_str());
+                AZ_Warning("CreatePrefabFromURDF", false, "cannot create joint %s", azJointName.c_str());
             }
-        }
 
-        MoveEntityToDefaultSpawnPoint(createEntityRoot.GetValue(), m_spawnPosition);
+            return true;
+        };
 
-        auto contentEntityId = createEntityRoot.GetValue();
-        AddRobotControl(contentEntityId);
+        // The JointVisitor is instead of iterating over the sdf::Model::JointCount
+        // as it will skip joints that exist that doesn't have an attached parent link or child link
+        // such as the root link when its name is "world"
+        constexpr bool visitNestedJoints = true;
+        Utils::VisitJoints(*GetFirstModel(), JointVisitor, visitNestedJoints);
+
+        // Use the first entity based on a link that is not parented to any other link
+        if (!linkEntityIdsWithoutParent.empty() && linkEntityIdsWithoutParent.front().IsValid())
+        {
+            AZ::EntityId contentEntityId = linkEntityIdsWithoutParent.front();
+            MoveEntityToDefaultSpawnPoint(contentEntityId, m_spawnPosition);
+            AddRobotControl(contentEntityId);
+        }
 
         // Create prefab, save it to disk immediately
         // Remove prefab, if it was already created.
@@ -244,7 +347,7 @@ namespace ROS2
         AzToolsFramework::Prefab::PrefabSystemScriptingBus::BroadcastResult(
             prefabTemplateId,
             &AzToolsFramework::Prefab::PrefabSystemScriptingBus::Events::CreatePrefabTemplate,
-            createdEntities, relativePath.String());        
+            createdEntities, relativePath.String());
 
         if (prefabTemplateId == AzToolsFramework::Prefab::InvalidTemplateId)
         {
@@ -290,7 +393,7 @@ namespace ROS2
         {
             // If the template saved successfully, also instantiate it into the level.
             auto prefabInterface = AZ::Interface<AzToolsFramework::Prefab::PrefabPublicInterface>::Get();
-            [[maybe_unused]] auto createPrefabOutcome = 
+            [[maybe_unused]] auto createPrefabOutcome =
                 prefabInterface->InstantiatePrefab(relativePath.String(), AZ::EntityId(), AZ::Vector3::CreateZero());
         }
         else
@@ -309,15 +412,14 @@ namespace ROS2
         return result;
     }
 
-
-    AzToolsFramework::Prefab::PrefabEntityResult URDFPrefabMaker::AddEntitiesForLink(urdf::LinkSharedPtr link, AZ::EntityId parentEntityId, AZStd::vector<AZ::EntityId>& createdEntities)
+    AzToolsFramework::Prefab::PrefabEntityResult URDFPrefabMaker::AddEntitiesForLink(const sdf::Link* link, AZ::EntityId parentEntityId, AZStd::vector<AZ::EntityId>& createdEntities)
     {
         if (!link)
         {
             AZ::Failure(AZStd::string("Failed to create prefab entity - link is null"));
         }
 
-        auto createEntityResult = PrefabMakerUtils::CreateEntity(parentEntityId, link->name.c_str());
+        auto createEntityResult = PrefabMakerUtils::CreateEntity(parentEntityId, link->Name().c_str());
         if (!createEntityResult.IsSuccess())
         {
             return createEntityResult;
@@ -332,21 +434,21 @@ namespace ROS2
         {
             auto* component = Utils::GetGameOrEditorComponent<ROS2FrameComponent>(entity);
             AZ_Assert(component, "ROS2 Frame Component does not exist for %s", entityId.ToString().c_str());
-            component->SetFrameID(AZStd::string(link->name.c_str(), link->name.size()));
+            component->SetFrameID(AZStd::string(link->Name().c_str(), link->Name().size()));
         }
         auto createdVisualEntities = m_visualsMaker.AddVisuals(link, entityId);
         createdEntities.insert(createdEntities.end(), createdVisualEntities.begin(), createdVisualEntities.end());
 
         if (!m_useArticulations)
         {
-            m_inertialsMaker.AddInertial(link->inertial, entityId);
+            m_inertialsMaker.AddInertial(link->Inertial(), entityId);
         }
         else
         {
-            m_articulationsMaker.AddArticulationLink(link, entityId);
+            m_articulationsMaker.AddArticulationLink(*GetFirstModel(), link, entityId);
         }
 
-        m_collidersMaker.AddColliders(link, entityId);
+        m_collidersMaker.AddColliders(*GetFirstModel(), link, entityId);
         return AZ::Success(entityId);
     }
 
@@ -379,16 +481,19 @@ namespace ROS2
             return;
         }
 
-        auto entity_ = AzToolsFramework::GetEntityById(rootEntityId);
-        auto* transformInterface_ = entity_->FindComponent<AzToolsFramework::Components::TransformComponent>();
-
-        if (transformInterface_ == nullptr)
+        if (auto entity_ = AzToolsFramework::GetEntityById(rootEntityId);
+            entity_ != nullptr)
         {
-            AZ_Trace("URDF Importer", "TransformComponent not found in created entity\n") return;
-        }
+            auto* transformInterface_ = entity_->FindComponent<AzToolsFramework::Components::TransformComponent>();
 
-        transformInterface_->SetWorldTM(*spawnPosition);
-        AZ_Trace("URDF Importer", "Successfully set spawn position\n")
+            if (transformInterface_ == nullptr)
+            {
+                AZ_Trace("URDF Importer", "TransformComponent not found in created entity\n") return;
+            }
+
+            transformInterface_->SetWorldTM(*spawnPosition);
+            AZ_Trace("URDF Importer", "Successfully set spawn position\n")
+        }
     }
 
     AZStd::string URDFPrefabMaker::GetStatus()
@@ -401,4 +506,26 @@ namespace ROS2
         }
         return str;
     }
+
+    const sdf::Model* URDFPrefabMaker::GetFirstModel() const
+    {
+        // First look for the model at the root of the SDF
+        if (const sdf::Model* model = m_root->Model();
+            model != nullptr)
+        {
+            return model;
+        }
+
+        // Next check if there is a world at the root of the sdf
+        if (m_root->WorldCount() > 0)
+        {
+            if (const sdf::World* world = m_root->WorldByIndex(0);
+                world != nullptr && world->ModelCount() > 0)
+            {
+                return world->ModelByIndex(0);
+            }
+        }
+
+        return nullptr;
+    }
 } // namespace ROS2

+ 8 - 5
Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.h

@@ -33,13 +33,13 @@ namespace ROS2
     public:
         //! Construct URDFPrefabMaker from arguments.
         //! @param modelFilePath path to the source URDF model.
-        //! @param model parsed model.
+        //! @param root parsed SDF root object.
         //! @param prefabPath path to the prefab which will be created as a result of import.
         //! @param urdfAssetsMapping prepared mapping of URDF meshes to Assets.
         //! @param useArticulations allows urdfImporter to create PhysXArticulations instead of multiple rigid bodies and joints.
         URDFPrefabMaker(
             const AZStd::string& modelFilePath,
-            urdf::ModelInterfaceSharedPtr model,
+            const sdf::Root* root,
             AZStd::string prefabPath,
             const AZStd::shared_ptr<Utils::UrdfAssetMap> urdfAssetsMapping,
             bool useArticulations = false,
@@ -69,12 +69,15 @@ namespace ROS2
         AZStd::string GetStatus();
 
     private:
-        AzToolsFramework::Prefab::PrefabEntityResult AddEntitiesForLink(urdf::LinkSharedPtr link, AZ::EntityId parentEntityId, AZStd::vector<AZ::EntityId>& createdEntities);
-        void BuildAssetsForLink(urdf::LinkSharedPtr link);
+        AzToolsFramework::Prefab::PrefabEntityResult AddEntitiesForLink(const sdf::Link* link, AZ::EntityId parentEntityId, AZStd::vector<AZ::EntityId>& createdEntities);
+        void BuildAssetsForLink(const sdf::Link* link);
         void AddRobotControl(AZ::EntityId rootEntityId);
         static void MoveEntityToDefaultSpawnPoint(const AZ::EntityId& rootEntityId, AZStd::optional<AZ::Transform> spawnPosition);
 
-        urdf::ModelInterfaceSharedPtr m_model;
+        // Returns the <model> at the root of the SDF or the first <world><model> if it exist
+        const sdf::Model* GetFirstModel() const;
+
+        const sdf::Root* m_root;
         AZStd::string m_prefabPath;
         VisualsMaker m_visualsMaker;
         CollidersMaker m_collidersMaker;

+ 26 - 72
Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.cpp

@@ -8,91 +8,45 @@
 
 #include "UrdfParser.h"
 
-#include <fstream>
-
 #include <AzCore/Debug/Trace.h>
 #include <AzCore/std/string/string.h>
-#include <console_bridge/console.h>
-#include <urdf_model/model.h>
 
-namespace ROS2
+namespace ROS2::UrdfParser
 {
-    namespace UrdfParser::Internal
+    RootObjectOutcome Parse(AZStd::string_view xmlString, const sdf::ParserConfig& parserConfig)
     {
-        void CheckIfCurrentLocaleHasDotAsADecimalSeparator()
-        {
-            // Due to the fact that URDF parser takes into account the locale information, incompatibility between URDF file locale and
-            // system locale might lead to incorrect URDF parsing. Mainly it affects floating point numbers, and the decimal separator. When
-            // locales are set to system with comma as decimal separator and URDF file is created with dot as decimal separator, URDF parser
-            // will trim the floating point number after comma. For example, if parsing 0.1, URDF parser will parse it as 0.
-            // This might lead to incorrect URDF loading. If the current locale is not a dot (as per standard ROS locale), we warn the user.
-            std::locale currentLocale("");
-            if (std::use_facet<std::numpunct<char>>(currentLocale).decimal_point() != '.')
-            {
-                AZ_Warning(
-                    "UrdfParser", false, "Locale %s might be incompatible with the URDF file content.\n", currentLocale.name().c_str());
-            }
-        }
-
-        class CustomConsoleHandler : public console_bridge::OutputHandler
-        {
-        private:
-            std::stringstream console_ss;
-
-        public:
-            void log(const std::string& text, console_bridge::LogLevel level, const char* filename, int line) override final;
-
-            //! Clears accumulated log
-            void Clear();
-
-            //! Read accumulated log to a string
-            AZStd::string GetLog();
-        };
-
-        void CustomConsoleHandler::log(const std::string& text, console_bridge::LogLevel level, const char* filename, int line)
-        {
-            AZ_Printf("UrdfParser", "%s\n", text.c_str());
-            console_ss << text << "\n";
-        }
-
-        void CustomConsoleHandler::Clear()
-        {
-            console_ss = std::stringstream();
-        }
-
-        AZStd::string CustomConsoleHandler::GetLog()
-        {
-            return AZStd::string(console_ss.str().c_str(), console_ss.str().size());
-        }
-
-        CustomConsoleHandler customConsoleHandler;
-    } // namespace UrdfParser::Internal
-
-    urdf::ModelInterfaceSharedPtr UrdfParser::Parse(const AZStd::string& xmlString)
+        return Parse(std::string(xmlString.data(), xmlString.size()), parserConfig);
+    }
+    RootObjectOutcome Parse(const std::string& xmlString, const sdf::ParserConfig& parserConfig)
     {
-        console_bridge::useOutputHandler(&Internal::customConsoleHandler);
-        Internal::CheckIfCurrentLocaleHasDotAsADecimalSeparator();
-        const auto ret = urdf::parseURDF(xmlString.c_str());
-        console_bridge::restorePreviousOutputHandler();
-        return ret;
+        sdf::Root root;
+        auto parseRootErrors = root.LoadSdfString(xmlString, parserConfig);
+
+        // if there are no parse errors return the sdf Root object otherwise return the errors
+        return parseRootErrors.empty() ? RootObjectOutcome(root) : RootObjectOutcome(AZStd::unexpect, parseRootErrors);
     }
 
-    urdf::ModelInterfaceSharedPtr UrdfParser::ParseFromFile(const AZStd::string& filePath)
+    RootObjectOutcome ParseFromFile(AZ::IO::PathView filePath, const sdf::ParserConfig& parserConfig)
     {
-        std::ifstream istream(filePath.c_str());
+        // Store path in a AZ::IO::FixedMaxPath which is stack based structure that provides memory
+        // for the path string and is null terminated.
+        // It is backed by an AZStd::fixed_string<1024> which is a char buffer of 1025 internally
+        AZ::IO::FixedMaxPath urdfFilePath = filePath.FixedMaxPathString();
+        std::ifstream istream(urdfFilePath.c_str());
         if (!istream)
         {
-            AZ_Error("UrdfParser", false, "File %s does not exist", filePath.c_str());
-            return nullptr;
+            auto fileNotFoundMessage = AZStd::fixed_string<1024>::format("File %.*s does not exist", AZ_PATH_ARG(urdfFilePath));
+            RootObjectOutcome fileNotExistOutcome;
+            fileNotExistOutcome = AZStd::unexpected<sdf::Errors>(AZStd::in_place,
+                {
+                    sdf::Error{ sdf::ErrorCode::FILE_READ,
+                        std::string{ fileNotFoundMessage.c_str(), fileNotFoundMessage.size() },
+                        std::string{ urdfFilePath.c_str(), urdfFilePath.Native().size() } }
+                });
+            return fileNotExistOutcome;
         }
 
         std::string xmlStr((std::istreambuf_iterator<char>(istream)), std::istreambuf_iterator<char>());
-        return Parse(xmlStr.c_str());
-    }
-
-    AZStd::string UrdfParser::GetUrdfParsingLog()
-    {
-        return Internal::customConsoleHandler.GetLog();
+        return Parse(xmlStr, parserConfig);
     }
-
 } // namespace ROS2

+ 31 - 12
Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.h

@@ -8,27 +8,46 @@
 
 #pragma once
 
-#include <AzCore/std/string/string.h>
-#include <urdf_parser/urdf_parser.h>
+#include <AzCore/IO/Path/Path.h>
+#include <AzCore/std/utility/expected.h>
+#include <sdf/Root.hh>
+#include <sdf/Link.hh>
+#include <sdf/Mesh.hh>
+#include <sdf/Material.hh>
+#include <sdf/Model.hh>
+#include <sdf/Joint.hh>
+#include <sdf/JointAxis.hh>
+#include <sdf/Visual.hh>
+#include <sdf/Geometry.hh>
+#include <sdf/Collision.hh>
 
 namespace ROS2
 {
     //! Class for parsing URDF data.
     namespace UrdfParser
     {
+        //! Stores the result of parsing URDF data
+        //! On success the sdf::Root object is returned
+        //! On failure the sdf::Errors vector is returned
+        using RootObjectOutcome = AZStd::expected<sdf::Root, sdf::Errors>;
+
         //! Parse string with URDF data and generate model.
         //! @param xmlString a string that contains URDF data (XML format).
-        //! @return model represented as a tree of parsed links.
-        urdf::ModelInterfaceSharedPtr Parse(const AZStd::string& xmlString);
+        //! @param parserConfig structure that contains configuration options for the SDFormatter parser
+        //!        The relevant ParserConfig functions for URDF importing are
+        //!        URDFPreserveFixedJoint() function to prevent merging of robot links bound by fixed joint
+        //!        AddURIPath() function to provide a mapping of package:// and model:// references to the local filesystem
+        //! @return SDF root object containing parsed <world> or <model> tags
+        RootObjectOutcome Parse(AZStd::string_view xmlString, const sdf::ParserConfig& parserConfig);
+        RootObjectOutcome Parse(const std::string& xmlString, const sdf::ParserConfig& parserConfig);
 
         //! Parse file with URDF data and generate model.
         //! @param filePath is a path to file with URDF data that will be loaded and parsed.
-        //! @return model represented as a tree of parsed links.
-        urdf::ModelInterfaceSharedPtr ParseFromFile(const AZStd::string& filePath);
-
-        //! Retrieve console log from URDF parsing
-        //! @return a log with output from urdf_parser
-        AZStd::string GetUrdfParsingLog();
-
+        //! @param parserConfig structure that contains configuration options for the SDFormater parser
+        //!        The relevant ParserConfig functions for URDF importing are
+        //!        URDFPreserveFixedJoint() function to prevent merging of robot links bound by fixed joint
+        //!        AddURIPath() function to provide a mapping of package:// and model:// references to the local filesystem
+        //! @return SDF root object containing parsed <world> or <model> tags
+        RootObjectOutcome ParseFromFile(AZ::IO::PathView filePath, const sdf::ParserConfig& parserConfig);
     }; // namespace UrdfParser
-} // namespace ROS2
+} // namespace ROS2

+ 44 - 45
Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.cpp

@@ -25,40 +25,36 @@
 
 namespace ROS2
 {
+    VisualsMaker::VisualsMaker() = default;
     VisualsMaker::VisualsMaker(
-        const std::map<std::string, urdf::MaterialSharedPtr>& materials, const AZStd::shared_ptr<Utils::UrdfAssetMap>& urdfAssetsMapping)
-        : m_urdfAssetsMapping(urdfAssetsMapping)
+        MaterialNameMap materials, const AZStd::shared_ptr<Utils::UrdfAssetMap>& urdfAssetsMapping)
+        : m_materials(AZStd::move(materials))
+        , m_urdfAssetsMapping(urdfAssetsMapping)
     {
-        AZStd::ranges::for_each(
-            materials,
-            [&](const auto& p)
-            {
-                m_materials[AZStd::string(p.first.c_str(), p.first.size())] = p.second;
-            });
     }
 
-    AZStd::vector<AZ::EntityId> VisualsMaker::AddVisuals(urdf::LinkSharedPtr link, AZ::EntityId entityId) const
+    AZStd::vector<AZ::EntityId> VisualsMaker::AddVisuals(const sdf::Link* link, AZ::EntityId entityId) const
     {
         AZStd::vector<AZ::EntityId> createdEntities;
 
         const AZStd::string typeString = "visual";
-        if (link->visual_array.size() < 1)
-        { 
+        if (link->VisualCount() < 1)
+        {
             // For zero visuals, element is used
-            auto createdEntity = AddVisual(link->visual, entityId, PrefabMakerUtils::MakeEntityName(link->name.c_str(), typeString));
+            auto createdEntity = AddVisual(nullptr, entityId, PrefabMakerUtils::MakeEntityName(link->Name().c_str(), typeString));
             if (createdEntity.IsValid())
             {
                 createdEntities.emplace_back(createdEntity);
             }
         }
         else
-        { 
+        {
             // For one or more visuals, an array is used
             size_t nameSuffixIndex = 0; // For disambiguation when multiple unnamed visuals are present. The order does not matter here
 
-            for (auto visual : link->visual_array)
+            for (uint64_t index = 0; index < link->VisualCount(); index++)
             {
-                auto createdEntity = AddVisual(visual, entityId, PrefabMakerUtils::MakeEntityName(link->name.c_str(), typeString, nameSuffixIndex));
+                auto createdEntity = AddVisual(link->VisualByIndex(index), entityId, PrefabMakerUtils::MakeEntityName(link->Name().c_str(), typeString, nameSuffixIndex));
                 if (createdEntity.IsValid())
                 {
                     createdEntities.emplace_back(createdEntity);
@@ -69,14 +65,14 @@ namespace ROS2
         return createdEntities;
     }
 
-    AZ::EntityId VisualsMaker::AddVisual(urdf::VisualSharedPtr visual, AZ::EntityId entityId, const AZStd::string& generatedName) const
+    AZ::EntityId VisualsMaker::AddVisual(const sdf::Visual* visual, AZ::EntityId entityId, const AZStd::string& generatedName) const
     {
         if (!visual)
         { // It is ok not to have a visual in a link
             return AZ::EntityId();
         }
 
-        if (!visual->geometry)
+        if (!visual->Geom())
         { // Non-empty visual should have a geometry. Warn if no geometry present
             AZ_Warning("AddVisual", false, "No Geometry for a visual");
             return AZ::EntityId();
@@ -85,12 +81,12 @@ namespace ROS2
         AZ_Trace("AddVisual", "Processing visual for entity id:%s\n", entityId.ToString().c_str());
 
         // Use a name generated from the link unless specific name is defined for this visual
-        const char* subEntityName = visual->name.empty() ? generatedName.c_str() : visual->name.c_str();
+        AZStd::string subEntityName = visual->Name().empty() ? generatedName.c_str() : visual->Name().c_str();
         // Since O3DE does not allow origin for visuals, we need to create a sub-entity and store visual there
-        auto createEntityResult = PrefabMakerUtils::CreateEntity(entityId, subEntityName);
+        auto createEntityResult = PrefabMakerUtils::CreateEntity(entityId, subEntityName.c_str());
         if (!createEntityResult.IsSuccess())
         {
-            AZ_Error("AddVisual", false, "Unable to create a sub-entity for visual element %s\n", subEntityName);
+            AZ_Error("AddVisual", false, "Unable to create a sub-entity for visual element %s\n", subEntityName.c_str());
             return AZ::EntityId();
         }
         auto visualEntityId = createEntityResult.GetValue();
@@ -99,49 +95,49 @@ namespace ROS2
         return visualEntityId;
     }
 
-    void VisualsMaker::AddVisualToEntity(urdf::VisualSharedPtr visual, AZ::EntityId entityId) const
+    void VisualsMaker::AddVisualToEntity(const sdf::Visual* visual, AZ::EntityId entityId) const
     {
         // Apply transform as per origin
-        PrefabMakerUtils::SetEntityTransformLocal(visual->origin, entityId);
+        PrefabMakerUtils::SetEntityTransformLocal(visual->RawPose(), entityId);
 
         AZ::Entity* entity = AzToolsFramework::GetEntityById(entityId);
-        auto geometry = visual->geometry;
-        switch (geometry->type)
+        auto geometry = visual->Geom();
+        switch (geometry->Type())
         {
-        case urdf::Geometry::SPHERE:
+        case sdf::GeometryType::SPHERE:
             {
-                auto sphereGeometry = std::dynamic_pointer_cast<urdf::Sphere>(geometry);
+                auto sphereGeometry = geometry->SphereShape();
                 AZ_Assert(sphereGeometry, "geometry is not Sphere");
                 entity->CreateComponent(LmbrCentral::EditorSphereShapeComponentTypeId);
                 entity->Activate();
                 LmbrCentral::SphereShapeComponentRequestsBus::Event(
-                    entityId, &LmbrCentral::SphereShapeComponentRequests::SetRadius, sphereGeometry->radius);
+                    entityId, &LmbrCentral::SphereShapeComponentRequests::SetRadius, sphereGeometry->Radius());
                 LmbrCentral::EditorShapeComponentRequestsBus::Event(
                     entityId, &LmbrCentral::EditorShapeComponentRequests::SetVisibleInGame, true);
                 entity->Deactivate();
             }
             break;
-        case urdf::Geometry::CYLINDER:
+        case sdf::GeometryType::CYLINDER:
             {
-                auto cylinderGeometry = std::dynamic_pointer_cast<urdf::Cylinder>(geometry);
+                auto cylinderGeometry = geometry->CylinderShape();
                 AZ_Assert(cylinderGeometry, "geometry is not Cylinder");
                 entity->CreateComponent(LmbrCentral::EditorCylinderShapeComponentTypeId);
                 entity->Activate();
                 LmbrCentral::CylinderShapeComponentRequestsBus::Event(
-                    entityId, &LmbrCentral::CylinderShapeComponentRequests::SetRadius, cylinderGeometry->radius);
+                    entityId, &LmbrCentral::CylinderShapeComponentRequests::SetRadius, cylinderGeometry->Radius());
                 LmbrCentral::CylinderShapeComponentRequestsBus::Event(
-                    entityId, &LmbrCentral::CylinderShapeComponentRequests::SetHeight, cylinderGeometry->length);
+                    entityId, &LmbrCentral::CylinderShapeComponentRequests::SetHeight, cylinderGeometry->Length());
                 LmbrCentral::EditorShapeComponentRequestsBus::Event(
                     entityId, &LmbrCentral::EditorShapeComponentRequests::SetVisibleInGame, true);
                 entity->Deactivate();
             }
             break;
-        case urdf::Geometry::BOX:
+        case sdf::GeometryType::BOX:
             {
-                auto boxGeometry = std::dynamic_pointer_cast<urdf::Box>(geometry);
+                auto boxGeometry = geometry->BoxShape();
                 AZ_Assert(boxGeometry, "geometry is not Box");
                 entity->CreateComponent(LmbrCentral::EditorBoxShapeComponentTypeId);
-                AZ::Vector3 boxDimensions = URDF::TypeConversions::ConvertVector3(boxGeometry->dim);
+                AZ::Vector3 boxDimensions = URDF::TypeConversions::ConvertVector3(boxGeometry->Size());
                 entity->Activate();
                 LmbrCentral::BoxShapeComponentRequestsBus::Event(
                     entityId, &LmbrCentral::BoxShapeComponentRequests::SetBoxDimensions, boxDimensions);
@@ -150,19 +146,19 @@ namespace ROS2
                 entity->Deactivate();
             }
             break;
-        case urdf::Geometry::MESH:
+        case sdf::GeometryType::MESH:
             {
-                auto meshGeometry = std::dynamic_pointer_cast<urdf::Mesh>(geometry);
+                auto meshGeometry = geometry->MeshShape();
                 AZ_Assert(meshGeometry, "geometry is not Mesh");
 
-                const auto asset = PrefabMakerUtils::GetAssetFromPath(*m_urdfAssetsMapping, meshGeometry->filename);
+                const auto asset = PrefabMakerUtils::GetAssetFromPath(*m_urdfAssetsMapping, AZStd::string(meshGeometry->Uri().c_str()));
 
                 if (asset)
                 {
                     auto editorMeshComponent = entity->CreateComponent(AZ::Render::EditorMeshComponentTypeId);
 
                     // Prepare scale
-                    AZ::Vector3 scaleVector = URDF::TypeConversions::ConvertVector3(meshGeometry->scale);
+                    AZ::Vector3 scaleVector = URDF::TypeConversions::ConvertVector3(meshGeometry->Scale());
                     bool isUniformScale =
                         AZ::IsClose(scaleVector.GetMaxElement(), scaleVector.GetMinElement(), AZ::Constants::FloatEpsilon);
                     if (!isUniformScale)
@@ -200,15 +196,15 @@ namespace ROS2
             }
             break;
         default:
-            AZ_Warning("AddVisual", false, "Unsupported visual geometry type, %d", geometry->type);
+            AZ_Warning("AddVisual", false, "Unsupported visual geometry type, %d", (int)geometry->Type());
             return;
         }
     }
 
-    void VisualsMaker::AddMaterialForVisual(urdf::VisualSharedPtr visual, AZ::EntityId entityId) const
+    void VisualsMaker::AddMaterialForVisual(const sdf::Visual* visual, AZ::EntityId entityId) const
     {
         // URDF does not include information from <gazebo> tags with specific materials, diffuse, specular and emissive params
-        if (!visual->material || !visual->geometry)
+        if (!visual->Material() || !visual->Geom())
         {
             // Material is optional, and it requires geometry
             return;
@@ -216,13 +212,16 @@ namespace ROS2
 
         AZ::Entity* entity = AzToolsFramework::GetEntityById(entityId);
 
-        const AZStd::string material_name{ visual->material->name.c_str() };
+        // As a Material doesn't have a name and there can only be 1 material per <visual> tag,
+        // the Visual Name is used for the material
+        const std::string materialName{ visual->Name() };
+        const AZStd::string azMaterialName{ materialName.c_str(), materialName.size() };
 
         // If present in map, take map color definition as priority, otherwise apply local node definition
-        const auto materialColorUrdf = m_materials.contains(material_name) ? m_materials.at(material_name)->color : visual->material->color;
+        const auto materialColorUrdf = m_materials.contains(azMaterialName) ? m_materials.at(azMaterialName)->Diffuse() : visual->Material()->Diffuse();
 
         const AZ::Color materialColor = URDF::TypeConversions::ConvertColor(materialColorUrdf);
-        bool isPrimitive = visual->geometry->type != urdf::Geometry::MESH;
+        bool isPrimitive = visual->Geom()->Type() != sdf::GeometryType::MESH;
         if (isPrimitive)
         { // For primitives, set the color in the shape component
             entity->Activate();
@@ -233,7 +232,7 @@ namespace ROS2
         }
 
         entity->CreateComponent(AZ::Render::EditorMaterialComponentTypeId);
-        AZ_Printf("AddVisual", "Setting color for material %s\n", visual->material->name.c_str());
+        AZ_Printf("AddVisual", "Setting color for material %s\n", azMaterialName.c_str());
         entity->Activate();
         AZ::Render::MaterialComponentRequestBus::Event(
             entityId,

+ 9 - 6
Gems/ROS2/Code/Source/RobotImporter/URDF/VisualsMaker.h

@@ -21,8 +21,11 @@ namespace ROS2
     class VisualsMaker
     {
     public:
+        using MaterialNameMap = AZStd::unordered_map<AZStd::string, const sdf::Material*>;
+
+        VisualsMaker();
         VisualsMaker(
-            const std::map<std::string, urdf::MaterialSharedPtr>& materials,
+            MaterialNameMap materials,
             const AZStd::shared_ptr<Utils::UrdfAssetMap>& urdfAssetsMapping);
 
         //! Add zero, one or many visual elements to a given entity (depending on link content).
@@ -30,14 +33,14 @@ namespace ROS2
         //! @param link A parsed URDF tree link node which could hold information about visuals.
         //! @param entityId A non-active entity which will be affected.
         //! @return List containing any entities created.
-        AZStd::vector<AZ::EntityId> AddVisuals(urdf::LinkSharedPtr link, AZ::EntityId entityId) const;
+        AZStd::vector<AZ::EntityId> AddVisuals(const sdf::Link* link, AZ::EntityId entityId) const;
 
     private:
-        AZ::EntityId AddVisual(urdf::VisualSharedPtr visual, AZ::EntityId entityId, const AZStd::string& generatedName) const;
-        void AddVisualToEntity(urdf::VisualSharedPtr visual, AZ::EntityId entityId) const;
-        void AddMaterialForVisual(urdf::VisualSharedPtr visual, AZ::EntityId entityId) const;
+        AZ::EntityId AddVisual(const sdf::Visual* visual, AZ::EntityId entityId, const AZStd::string& generatedName) const;
+        void AddVisualToEntity(const sdf::Visual* visual, AZ::EntityId entityId) const;
+        void AddMaterialForVisual(const sdf::Visual* visual, AZ::EntityId entityId) const;
 
-        AZStd::unordered_map<AZStd::string, urdf::MaterialSharedPtr> m_materials;
+        MaterialNameMap m_materials;
         AZStd::shared_ptr<Utils::UrdfAssetMap> m_urdfAssetsMapping;
     };
 } // namespace ROS2

+ 36 - 0
Gems/ROS2/Code/Source/RobotImporter/Utils/ErrorUtils.cpp

@@ -0,0 +1,36 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+
+#include "ErrorUtils.h"
+
+#include <sdf/sdf.hh>
+
+namespace ROS2::Utils
+{
+    AZStd::string JoinSdfErrorsToString(AZStd::span<const sdf::Error> sdfErrors)
+    {
+        AZStd::string aggregateErrorMessages;
+        AppendSdfErrorsToString(aggregateErrorMessages, sdfErrors);
+        return aggregateErrorMessages;
+    }
+
+    void AppendSdfErrorsToString(AZStd::string& outputResult, AZStd::span<const sdf::Error> sdfErrors)
+    {
+        for (const sdf::Error& sdfError : sdfErrors)
+        {
+            AZStd::string errorMessage = AZStd::string::format("ErrorCode=%d", static_cast<int32_t>(sdfError.Code()));
+            errorMessage += AZStd::string::format(", Message=%s", sdfError.Message().c_str());
+            if (sdfError.LineNumber().has_value())
+            {
+                errorMessage += AZStd::string::format(", Line=%d", sdfError.LineNumber().value());
+            }
+            outputResult += errorMessage;
+            outputResult += '\n';
+        }
+    }
+} // namespace ROS2::Utils

+ 70 - 0
Gems/ROS2/Code/Source/RobotImporter/Utils/ErrorUtils.h

@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+#pragma once
+
+#include <AzCore/Component/ComponentBus.h>
+#include <AzCore/IO/SystemFile.h>
+#include <AzCore/Math/Transform.h>
+#include <AzCore/std/containers/unordered_map.h>
+#include <AzCore/std/containers/unordered_set.h>
+#include <AzCore/std/containers/vector.h>
+#include <AzCore/std/function/function_template.h>
+#include <AzCore/std/string/string.h>
+#include <RobotImporter/URDF/UrdfParser.h>
+
+#include <sdf/sdf.hh>
+
+namespace sdf
+{
+    inline namespace v13
+    {
+        class Error;
+    } // namespace v13
+} // namespace sdf
+
+
+namespace AZStd
+{
+    // Allow std::vector<sdf::Error> to meet the requirements of contiguous iterator in C++17
+    // This allows constructing an AZStd::span from a std::vector
+    template<>
+    struct iterator_traits<typename std::vector<sdf::v13::Error>::iterator>
+    {
+        // Use the standard library iterator traits for all traits except for the iterator_concept
+        using difference_type = typename std::iterator_traits<typename std::vector<sdf::v13::Error>::iterator>::difference_type;
+        using value_type = typename std::iterator_traits<typename std::vector<sdf::v13::Error>::iterator>::value_type;
+        using pointer = typename std::iterator_traits<typename std::vector<sdf::v13::Error>::iterator>::pointer;
+        using reference = typename std::iterator_traits<typename std::vector<sdf::v13::Error>::iterator>::reference;
+        using iterator_category = typename std::iterator_traits<typename std::vector<sdf::v13::Error>::iterator>::iterator_category;
+        using iterator_concept = contiguous_iterator_tag;
+    };
+
+    template<>
+    struct iterator_traits<typename std::vector<sdf::v13::Error>::const_iterator>
+    {
+        // Use the standard library iterator traits for all traits except for the iterator_concept
+        using difference_type = typename std::iterator_traits<typename std::vector<sdf::v13::Error>::const_iterator>::difference_type;
+        using value_type = typename std::iterator_traits<typename std::vector<sdf::v13::Error>::const_iterator>::value_type;
+        using pointer = typename std::iterator_traits<typename std::vector<sdf::v13::Error>::const_iterator>::pointer;
+        using reference = typename std::iterator_traits<typename std::vector<sdf::v13::Error>::const_iterator>::reference;
+        using iterator_category = typename std::iterator_traits<typename std::vector<sdf::v13::Error>::const_iterator>::iterator_category;
+        using iterator_concept = contiguous_iterator_tag;
+    };
+}
+
+namespace ROS2::Utils
+{
+    //! Converts each sdf::Error from an sdf::Errors vector into an AZStd::string
+    //! @param sdfErrors A span of sdf::Error objects
+    //! @return AZStd::string which contains each error formatted for human readable output
+    AZStd::string JoinSdfErrorsToString(AZStd::span<const sdf::Error> sdfErrors);
+    //! Converts each sdf::Error from an sdf::Errors vector into an AZStd::string
+    //! @param outputResult Appends to output string each sdf error
+    //! @param sdfErrors A span of sdf::Error objects
+    void AppendSdfErrorsToString(AZStd::string& outputResult, AZStd::span<const sdf::Error> sdfErrors);
+} // namespace ROS2::Utils

+ 6 - 1
Gems/ROS2/Code/Source/RobotImporter/Utils/FilePath.cpp

@@ -18,7 +18,7 @@ namespace ROS2
         {
             if (!filename.HasExtension())
             {
-                return "";
+                return AZStd::string{};
             }
             AZStd::string extension{ filename.Extension().Native() };
             AZStd::to_upper(extension.begin(), extension.end());
@@ -34,5 +34,10 @@ namespace ROS2
         {
             return GetCapitalizedExtension(filename) == ".URDF";
         }
+
+        bool IsFileSDF(const AZ::IO::Path& filename)
+        {
+            return GetCapitalizedExtension(filename) == ".SDF" || GetCapitalizedExtension(filename) == ".WORLD";
+        }
     } // namespace Utils
 } // namespace ROS2

+ 434 - 189
Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp

@@ -7,6 +7,7 @@
  */
 
 #include "RobotImporterUtils.h"
+#include <RobotImporter/Utils/ErrorUtils.h>
 #include "TypeConversions.h"
 #include <AzCore/Asset/AssetManager.h>
 #include <AzCore/Asset/AssetManagerBus.h>
@@ -17,10 +18,18 @@
 #include <AzToolsFramework/API/EditorAssetSystemAPI.h>
 #include <string.h>
 
-namespace ROS2
+namespace ROS2::Utils
 {
+    inline namespace Internal
+    {
+        bool FileExistsCall(const AZ::IO::PathView& filePath)
+        {
+            AZ::IO::FixedMaxPath pathStorage(filePath);
+            return AZ::IO::SystemFile::Exists(pathStorage.c_str());
+        };
+    }
 
-    bool Utils::WaitForAssetsToProcess(const AZStd::unordered_map<AZStd::string, AZ::IO::Path>& sourceAssetsPaths)
+    bool WaitForAssetsToProcess(const AZStd::unordered_map<AZStd::string, AZ::IO::Path>& sourceAssetsPaths)
     {
         bool allAssetProcessed = false;
         bool assetProcessorFailed = false;
@@ -90,289 +99,525 @@ namespace ROS2
         return false;
     }
 
-    bool Utils::IsWheelURDFHeuristics(const urdf::LinkConstSharedPtr& link)
+    bool IsWheelURDFHeuristics(const sdf::Model& model, const sdf::Link* link)
     {
-        const AZStd::regex wheel_regex("wheel[_]||[_]wheel");
-        const AZStd::regex joint_regex("(?i)joint");
-        const AZStd::string link_name(link->name.c_str(), link->name.size());
-        AZStd::smatch match;
-        // Check if name is catchy for wheel
-        if (!AZStd::regex_search(link_name, match, wheel_regex))
+        auto wheelMatcher = [](AZStd::string_view name)
         {
-            return false;
-        }
-        // The name should contain a joint word
-        if (AZStd::regex_search(link_name, match, joint_regex))
+            // StringFunc matches are case-insensitive by default
+            return AZ::StringFunc::StartsWith(name, "wheel_") ||
+                AZ::StringFunc::EndsWith(name, "_wheel");
+        };
+
+        const AZStd::string linkName(link->Name().c_str(), link->Name().size());
+        // Check if link name is catchy for wheel
+        if (!wheelMatcher(linkName))
         {
             return false;
         }
+
         // Wheels need to have collision and visuals
-        if (!(link->collision && link->visual))
+        if ((link->CollisionCount() == 0) || (link->VisualCount() == 0))
         {
             return false;
         }
-        // Parent joint needs to be CONTINOUS
-        if (link->parent_joint && link->parent_joint->type == urdf::Joint::CONTINUOUS)
+
+        // When this link is a child, the parent link joint needs to be CONTINUOUS
+        AZStd::vector<const sdf::Joint*> joints = GetJointsForChildLink(model,
+            linkName, true);
+
+        // URDFs only have a single parent
+        // This is explained in the Pose frame semantics tutorial for sdformat
+        // http://sdformat.org/tutorials?tut=pose_frame_semantics&ver=1.5#parent-frames-in-urdf
+
+        // The SDF URDF parser converts continuous joints to revolute joints with a limit
+        // of -infinity to +infinity
+        // https://github.com/gazebosim/sdformat/blob/sdf13/src/parser_urdf.cc#L3009-L3039
+        bool isWheel{};
+        if (!joints.empty())
         {
-            return true;
+            const sdf::Joint* potentialWheelJoint = joints.front();
+            if (const sdf::JointAxis* jointAxis = potentialWheelJoint->Axis(); jointAxis != nullptr)
+            {
+                using LimitType = decltype(jointAxis->Lower());
+                // There should only be 1 element for URDF, however that will not be verified
+                // in case this function is called on link from an SDF file
+                isWheel = potentialWheelJoint->Type() == sdf::JointType::CONTINUOUS;
+                isWheel = isWheel || (potentialWheelJoint->Type() == sdf::JointType::REVOLUTE
+                    && jointAxis->Lower() == -AZStd::numeric_limits<LimitType>::infinity()
+                    && jointAxis->Upper() == AZStd::numeric_limits<LimitType>::infinity());
+            }
         }
-        return false;
+
+        return isWheel;
     }
 
-    AZ::Transform Utils::GetWorldTransformURDF(const urdf::LinkSharedPtr& link, AZ::Transform t)
+    AZ::Transform GetWorldTransformURDF(const sdf::Link* link, AZ::Transform t)
     {
-        if (link->getParent() != nullptr)
+        // Determine if the pose is relative to another link
+        // See doxygen at http://osrf-distributions.s3.amazonaws.com/sdformat/api/13.2.0/classsdf_1_1SDF__VERSION__NAMESPACE_1_1Link.html#a011d84b31f584938d89ac6b8c8a09eb3
+
+        sdf::SemanticPose linkSemanticPos = link->SemanticPose();
+        gz::math::Pose3d resolvedPose;
+
+        if (sdf::Errors poseResolveErrors = linkSemanticPos.Resolve(resolvedPose);
+            !poseResolveErrors.empty())
         {
-            t = URDF::TypeConversions::ConvertPose(link->parent_joint->parent_to_joint_origin_transform) * t;
-            return GetWorldTransformURDF(link->getParent(), t);
+            AZStd::string poseErrorMessages = Utils::JoinSdfErrorsToString(poseResolveErrors);
+
+            AZ_Error("RobotImporter", false, R"(Failed to get world transform for link %s. Errors: "%s")",
+                link->Name().c_str(), poseErrorMessages.c_str());
+            return {};
         }
-        return t;
+
+        const AZ::Transform linkTransform = URDF::TypeConversions::ConvertPose(resolvedPose);
+        const AZ::Transform resolvedTransform = linkTransform * t;
+        return resolvedTransform;
+    }
+
+    void VisitLinks(const sdf::Model& sdfModel, const LinkVisitorCallback& linkVisitorCB,
+        bool visitNestedModelLinks)
+    {
+        // Function object which can visit all links of a model
+        // Optionally it supports recursing nested models to visit their links as well
+        struct VisitLinksForNestedModels_fn
+        {
+            void operator()(const sdf::Model& model)
+            {
+                if (VisitLinksForModel(model) && m_recurseModels)
+                {
+                    // Nested model link are only visited if the joint visitor returns true
+                    for (uint64_t modelIndex{}; modelIndex < model.ModelCount(); ++modelIndex)
+                    {
+                        const sdf::Model* nestedModel = model.ModelByIndex(modelIndex);
+                        if (nestedModel != nullptr)
+                        {
+                            if (!VisitLinksForModel(*nestedModel))
+                            {
+                                // Sibling nested model links are only visited
+                                // if the joint visitor returns true
+                                break;
+                            }
+                        }
+                    }
+                }
+            }
+
+        private:
+            //! Returns success by default
+            //! But an invoked visitor can return false to halt further iteration
+            bool VisitLinksForModel(const sdf::Model& currentModel)
+            {
+                for (uint64_t linkIndex{}; linkIndex < currentModel.LinkCount(); ++linkIndex)
+                {
+                    const sdf::Link* link = currentModel.LinkByIndex(linkIndex);
+                    if (link != nullptr)
+                    {
+                        if (!m_linkVisitorCB(*link))
+                        {
+                            return false;
+                        }
+                    }
+                }
+
+                return true;
+            }
+
+        public:
+            LinkVisitorCallback m_linkVisitorCB;
+            bool m_recurseModels{};
+        };
+
+        VisitLinksForNestedModels_fn VisitLinksForNestedModels{};
+        VisitLinksForNestedModels.m_linkVisitorCB = linkVisitorCB;
+        VisitLinksForNestedModels.m_recurseModels = visitNestedModelLinks;
+        VisitLinksForNestedModels(sdfModel);
     }
 
-    AZStd::unordered_map<AZStd::string, urdf::LinkSharedPtr> Utils::GetAllLinks(const std::vector<urdf::LinkSharedPtr>& childLinks)
+    void VisitJoints(const sdf::Model& sdfModel, const JointVisitorCallback& jointVisitorCB,
+        bool visitNestedModelJoints)
     {
-        AZStd::unordered_map<AZStd::string, urdf::LinkSharedPtr> pointers;
-        AZStd::function<void(const std::vector<urdf::LinkSharedPtr>&)> link_visitor =
-            [&](const std::vector<urdf::LinkSharedPtr>& child_links) -> void
+        // Function object which can visit all joints of a model
+        // Optionally it supports recursing nested models to visit their joints as well
+        struct VisitJointsForNestedModels_fn
         {
-            for (const urdf::LinkSharedPtr& child_link : child_links)
+            void operator()(const sdf::Model& model)
             {
-                AZStd::string link_name(child_link->name.c_str(), child_link->name.size());
-                pointers[link_name] = child_link;
-                link_visitor(child_link->child_links);
+                if (VisitJointsForModel(model) && m_recurseModels)
+                {
+                    // Nested model joints are only visited if the joint visitor returns true
+                    for (uint64_t modelIndex{}; modelIndex < model.ModelCount(); ++modelIndex)
+                    {
+                        const sdf::Model* nestedModel =  model.ModelByIndex(modelIndex);
+                        if (nestedModel != nullptr)
+                        {
+                            if (!VisitJointsForModel(*nestedModel))
+                            {
+                                // Sibling nested model joints are only visited
+                                // if the joint visitor returns true
+                                break;
+                            }
+                        }
+                    }
+                }
+            }
+
+        private:
+            bool VisitJointsForModel(const sdf::Model& currentModel)
+            {
+                for (uint64_t jointIndex{}; jointIndex < currentModel.JointCount(); ++jointIndex)
+                {
+                    const sdf::Joint* joint = currentModel.JointByIndex(jointIndex);
+                    // Skip any joints whose parent and child link references
+                    // don't have an actual sdf::link in the parsed model
+                    if (joint != nullptr && currentModel.LinkNameExists(joint->ParentName())
+                        && currentModel.LinkByName(joint->ChildName()))
+                    {
+                        if (!m_jointVisitorCB(*joint))
+                        {
+                            return false;
+                        }
+                    }
+                }
+
+                return true;
             }
+
+        public:
+            JointVisitorCallback m_jointVisitorCB;
+            bool m_recurseModels{};
+        };
+
+        VisitJointsForNestedModels_fn VisitJointsForNestedModels{};
+        VisitJointsForNestedModels.m_jointVisitorCB = jointVisitorCB;
+        VisitJointsForNestedModels.m_recurseModels = visitNestedModelJoints;
+        VisitJointsForNestedModels(sdfModel);
+    }
+
+    AZStd::unordered_map<AZStd::string, const sdf::Link*> GetAllLinks(const sdf::Model& sdfModel,
+        bool gatherNestedModelLinks)
+    {
+        using LinkMap = AZStd::unordered_map<AZStd::string, const sdf::Link*>;
+        LinkMap links;
+        auto GatherLinks = [&links](const sdf::Link& link)
+        {
+            AZStd::string linkName(link.Name().c_str(), link.Name().size());
+            links.insert_or_assign(AZStd::move(linkName), &link);
+            return true;
+        };
+
+        VisitLinks(sdfModel, GatherLinks, gatherNestedModelLinks);
+        return links;
+    }
+
+    AZStd::unordered_map<AZStd::string, const sdf::Joint*> GetAllJoints(const sdf::Model& sdfModel,
+        bool gatherNestedModelJoints)
+    {
+        using JointMap = AZStd::unordered_map<AZStd::string, const sdf::Joint*>;
+        JointMap joints;
+        auto GatherJoints = [&joints](const sdf::Joint& joint)
+        {
+            AZStd::string jointName(joint.Name().c_str(), joint.Name().size());
+            joints.insert_or_assign(AZStd::move(jointName), &joint);
+            return true;
         };
-        link_visitor(childLinks);
-        return pointers;
+
+        VisitJoints(sdfModel, GatherJoints, gatherNestedModelJoints);
+        return joints;
     }
 
-    AZStd::unordered_map<AZStd::string, urdf::JointSharedPtr> Utils::GetAllJoints(const std::vector<urdf::LinkSharedPtr>& childLinks)
+    AZStd::vector<const sdf::Joint*> GetJointsForChildLink(const sdf::Model& sdfModel, AZStd::string_view linkName,
+        bool gatherNestedModelJoints)
     {
-        AZStd::unordered_map<AZStd::string, urdf::JointSharedPtr> joints;
-        AZStd::function<void(const std::vector<urdf::LinkSharedPtr>&)> link_visitor =
-            [&](const std::vector<urdf::LinkSharedPtr>& child_links) -> void
+        using JointVector = AZStd::vector<const sdf::Joint*>;
+        JointVector joints;
+        auto GatherJointsWhereLinkIsChild = [&joints, linkName](const sdf::Joint& joint)
         {
-            for (auto child_link : child_links)
+            if (AZStd::string_view jointChildName{ joint.ChildName().c_str(), joint.ChildName().size() };
+                jointChildName == linkName)
             {
-                const auto& joint = child_link->parent_joint;
-                AZStd::string joint_name(joint->name.c_str(), joint->name.size());
-                joints[joint_name] = joint;
-                link_visitor(child_link->child_links);
+                joints.emplace_back(&joint);
             }
+
+            return true;
         };
-        link_visitor(childLinks);
+
+        VisitJoints(sdfModel, GatherJointsWhereLinkIsChild, gatherNestedModelJoints);
         return joints;
     }
 
-    AZStd::unordered_set<AZStd::string> Utils::GetMeshesFilenames(const urdf::LinkConstSharedPtr& rootLink, bool visual, bool colliders)
+    AZStd::vector<const sdf::Joint*> GetJointsForParentLink(const sdf::Model& sdfModel, AZStd::string_view linkName,
+        bool gatherNestedModelJoints)
     {
+        using JointVector = AZStd::vector<const sdf::Joint*>;
+        JointVector joints;
+        auto GatherJointsWhereLinkIsParent = [&joints, linkName](const sdf::Joint& joint)
+        {
+            if (AZStd::string_view jointParentName{ joint.ParentName().c_str(), joint.ParentName().size() };
+                jointParentName == linkName)
+            {
+                joints.emplace_back(&joint);
+            }
+
+            return true;
+        };
+
+        VisitJoints(sdfModel, GatherJointsWhereLinkIsParent, gatherNestedModelJoints);
+        return joints;
+    }
+
+    AZStd::unordered_set<AZStd::string> GetMeshesFilenames(const sdf::Root* root, bool visual, bool colliders)
+    {
+        const sdf::Model* model = root != nullptr ? root->Model() : nullptr;
+        if (model == nullptr)
+        {
+            return {};
+        }
+
         AZStd::unordered_set<AZStd::string> filenames;
-        const auto addFilenameFromGeometry = [&filenames](const urdf::GeometrySharedPtr& geometry)
+        const auto addFilenameFromGeometry = [&filenames](const sdf::Geometry* geometry)
         {
-            if (geometry->type == urdf::Geometry::MESH)
+            if (geometry->Type() == sdf::GeometryType::MESH)
             {
-                auto pMesh = std::dynamic_pointer_cast<urdf::Mesh>(geometry);
+                auto pMesh = geometry->MeshShape();
                 if (pMesh)
                 {
-                    filenames.insert(AZStd::string(pMesh->filename.c_str(), pMesh->filename.size()));
+                    filenames.insert(AZStd::string(pMesh->Uri().c_str(), pMesh->Uri().size()));
                 }
             }
         };
 
-        const auto processLink = [&addFilenameFromGeometry, visual, colliders](const urdf::Link& link)
+        const auto processLink = [&addFilenameFromGeometry, visual, colliders](const sdf::Link* link)
         {
             if (visual)
             {
-                for (auto& p : link.visual_array)
+                for (uint64_t index = 0; index < link->VisualCount(); index++)
                 {
-                    addFilenameFromGeometry(p->geometry);
+                    addFilenameFromGeometry(link->VisualByIndex(index)->Geom());
                 }
             }
             if (colliders)
             {
-                for (auto& p : link.collision_array)
+                for (uint64_t index = 0; index < link->CollisionCount(); index++)
                 {
-                    addFilenameFromGeometry(p->geometry);
+                    addFilenameFromGeometry(link->CollisionByIndex(index)->Geom());
                 }
             }
         };
 
-        AZStd::function<void(const std::vector<urdf::LinkConstSharedPtr>&)> linkVisitor =
-            [&](const std::vector<urdf::LinkConstSharedPtr>& child_links) -> void
+        for (uint64_t index = 0; index < model->LinkCount(); index++)
         {
-            for (auto link : child_links)
-            {
-                processLink(*link);
-                std::vector<urdf::LinkConstSharedPtr> childVector(link->child_links.size());
-                std::transform(
-                    link->child_links.begin(),
-                    link->child_links.end(),
-                    childVector.begin(),
-                    [](const urdf::LinkSharedPtr& p)
-                    {
-                        return urdf::const_pointer_cast<urdf::Link>(p);
-                    });
-                linkVisitor(childVector);
-            }
-        };
-        linkVisitor({ rootLink });
+            processLink(model->LinkByIndex(index));
+        }
+
         return filenames;
     }
 
-    AZStd::optional<AZ::IO::Path> GetResolvedPath(
-        const AZ::IO::Path& packagePath, const AZ::IO::Path& unresolvedPath, const AZStd::function<bool(const AZStd::string&)>& fileExists)
+    /// Finds global path from URDF path
+    AZ::IO::Path ResolveURDFPath(
+        AZ::IO::Path unresolvedPath,
+        const AZ::IO::PathView& urdfFilePath,
+        const AZ::IO::PathView& amentPrefixPath,
+        const FileExistsCB& fileExists)
     {
-        AZ::IO::Path packageXmlCandite = packagePath / "package.xml";
-        if (fileExists(packageXmlCandite.String()))
-        {
-            AZ::IO::Path resolvedPath = packagePath / unresolvedPath;
-            if (fileExists(resolvedPath.String()))
-            {
-                return AZStd::optional<AZ::IO::Path>{ resolvedPath };
-            }
-        }
-        return AZStd::optional<AZ::IO::Path>{};
-    }
+        AZ_Printf("ResolveURDFPath", "ResolveURDFPath with %s\n", unresolvedPath.c_str());
 
-    AZ::IO::Path GetPathFromSubPath(const AZ::IO::Path::const_iterator& begin, const AZ::IO::Path::const_iterator& end)
-    {
-        AZ::IO::Path subpath;
-        if (begin == end)
+        // TODO: Query URDF prefix map from Settings Registry
+        AZStd::vector<AZ::IO::Path> amentPrefixPaths;
+
+        // Split the AMENT_PREFIX_PATH into multiple paths
+        auto AmentPrefixPathVisitor = [&amentPrefixPaths](
+            AZStd::string_view prefixPath)
         {
-            return subpath;
-        }
-        for (AZ::IO::Path::iterator pathIt = begin; pathIt != end; pathIt++)
+            amentPrefixPaths.push_back(prefixPath);
+        };
+        // Note this code only works on Unix platforms
+        // For Windows this will not work as the drive letter has a colon in it (C:\)
+        AZ::StringFunc::TokenizeVisitor(amentPrefixPath.Native(), AmentPrefixPathVisitor, ':');
+
+        // Append the urdf file ancestor directories to the candidate replacement paths
+        AZStd::vector<AZ::IO::Path> urdfAncestorPaths;
+        if (!urdfFilePath.empty())
         {
-            subpath /= *pathIt;
+            AZ::IO::Path urdfFileAncestorPath = urdfFilePath;
+            bool rootPathVisited = false;
+            do
+            {
+                AZ::IO::PathView parentPath = urdfFileAncestorPath.ParentPath();
+                rootPathVisited = (urdfFileAncestorPath == parentPath);
+                urdfAncestorPaths.emplace_back(parentPath);
+                urdfFileAncestorPath = parentPath;
+            } while (!rootPathVisited);
         }
-        return subpath;
-    }
 
-    /// Finds global path from URDF path
-    AZStd::string Utils::ResolveURDFPath(
-        AZStd::string unresolvedPath,
-        const AZStd::string& urdfFilePath,
-        const AZStd::string& amentPrefixPath,
-        const AZStd::function<bool(const AZStd::string&)>& fileExists)
-    {
-        AZ_Printf("ResolveURDFPath", "ResolveURDFPath with %s\n", unresolvedPath.c_str());
-        if (unresolvedPath.starts_with("package://"))
+        // Structure which accepts a callback that can convert an unresolved URI path(package://, model://, file://, etc...)
+        // to a filesystem path
+        struct UriPrefix
         {
-            AZ::StringFunc::Replace(unresolvedPath, "package://", "", true, true);
+            using SchemeResolver = AZStd::function<AZStd::optional<AZ::IO::Path>(AZ::IO::PathView)>;
+            SchemeResolver m_schemeResolver;
+        };
 
-            const AZ::IO::Path unresolvedProperPath(unresolvedPath);
-            if (!unresolvedProperPath.empty())
+        auto GetReplacementSchemeResolver = [](
+            AZStd::string_view schemePrefix, AZStd::span<const AZ::IO::Path> amentPrefixPaths,
+            AZStd::span<const AZ::IO::Path> urdfAncestorPaths, const FileExistsCB& fileExistsCB)
+        {
+            return [schemePrefix, amentPrefixPaths, urdfAncestorPaths, &fileExistsCB](
+                AZ::IO::PathView uriPath) -> AZStd::optional<AZ::IO::Path>
             {
-                const AZStd::string packageNameCandidate = unresolvedProperPath.begin()->String();
-                AZStd::vector<AZStd::string> amentPathTokenized;
-                AZ::StringFunc::Tokenize(amentPrefixPath, amentPathTokenized, ':');
-                for (const auto& package : amentPathTokenized)
+                // Note this is a case-sensitive check to match the exact URI scheme
+                // If that is not desired, then this code should be updated to read
+                // a value from the Setting Registry indicating whether the uriPrefix matching
+                // should be case sensitive
+                bool uriPrefixMatchCaseSensitive = true;
+                // Check if the path starts with the URI scheme prefix
+                if (AZ::StringFunc::StartsWith(uriPath.Native(), schemePrefix, uriPrefixMatchCaseSensitive))
                 {
-                    if (package.ends_with(packageNameCandidate))
+                    // Strip the number of characters from the Uri scheme from beginning of the path
+                    AZ::IO::PathView strippedUriPath = uriPath.Native().substr(schemePrefix.size());
+                    if (strippedUriPath.empty())
+                    {
+                        // The stripped URI path is empty, so there is nothing to resolve
+                        return AZStd::nullopt;
+                    }
+
+                    // Check to see if the relative part of the URI path refers to a location
+                    // within each <ament prefix path>/share directory
+                    for (const AZ::IO::Path& amentPrefixPath : amentPrefixPaths)
                     {
-                        auto pathIt = unresolvedProperPath.begin();
-                        AZStd::advance(pathIt, 1);
-                        if (pathIt != unresolvedProperPath.end())
+                        auto pathIter = strippedUriPath.begin();
+                        AZ::IO::PathView packageName = *pathIter;
+                        const AZ::IO::Path amentSharePath = amentPrefixPath / "share";
+                        const AZ::IO::Path packageManifestPath = amentSharePath / packageName / "package.xml";
+
+                        if (const AZ::IO::Path candidateResolvedPath = amentSharePath / strippedUriPath;
+                            fileExistsCB(packageManifestPath) && fileExistsCB(candidateResolvedPath))
                         {
-                            AZ::IO::Path unresolvedPathStripped = GetPathFromSubPath(pathIt, unresolvedProperPath.end());
+                            return candidateResolvedPath;
+                        }
+                    }
 
-                            const AZ::IO::Path packagePath = AZ::IO::Path{ package } / "share";
-                            auto resolvedPath =
-                                GetResolvedPath(packagePath / AZ::IO::Path{ packageNameCandidate }, unresolvedPathStripped, fileExists);
-                            if (resolvedPath.has_value())
-                            {
-                                AZ_Printf("ResolveURDFPath", "Resolved to using Ament to : %s\n", resolvedPath->String().c_str());
-                                return resolvedPath->String();
-                            }
+                    // The URI path cannot be resolved within the any ament prefix path,
+                    // so try the directory containing the URDF file as well as any of its parent directories
+                    for (const AZ::IO::Path& urdfAncestorPath : urdfAncestorPaths)
+                    {
+                        if (const AZ::IO::Path candidateResolvedPath = urdfAncestorPath / strippedUriPath;
+                            fileExistsCB(candidateResolvedPath))
+                        {
+                            return candidateResolvedPath;
                         }
                     }
                 }
+
+                return AZStd::nullopt;
+            };
+        };
+
+        constexpr AZStd::string_view PackageSchemePrefix = "package://";
+        UriPrefix packageUriPrefix;
+        packageUriPrefix.m_schemeResolver = GetReplacementSchemeResolver(PackageSchemePrefix,
+            amentPrefixPaths, urdfAncestorPaths, fileExists);
+
+        constexpr AZStd::string_view ModelSchemePrefix = "model://";
+        UriPrefix modelUriPrefix;
+        modelUriPrefix.m_schemeResolver = GetReplacementSchemeResolver(ModelSchemePrefix,
+            amentPrefixPaths, urdfAncestorPaths, fileExists);
+
+        // For a local file path convert the file URI to a local path
+        UriPrefix fileUriPrefix;
+        fileUriPrefix.m_schemeResolver = [](AZ::IO::PathView uriPath) -> AZStd::optional<AZ::IO::Path>
+        {
+            constexpr AZStd::string_view FileSchemePrefix = "file://";
+            // Paths that start with 'file:///' are absolute paths, so only 'file://' needs to be stripped
+            bool uriPrefixMatchCaseSensitive = true;
+            if (AZ::StringFunc::StartsWith(uriPath.Native(), FileSchemePrefix, uriPrefixMatchCaseSensitive))
+            {
+                AZStd::string_view strippedUriPath = uriPath.Native().substr(FileSchemePrefix.size());
+                return AZ::IO::Path(strippedUriPath);
             }
 
-            const AZ::IO::Path urdfProperPath(urdfFilePath);
-            if (!urdfProperPath.empty())
+            return AZStd::nullopt;
+        };
+
+        // Step 1: Attempt to resolved URI scheme paths
+        // libsdformat seems to convert package:// references to model:// references
+        // So the model:// URI prefix resolver is run first
+        const auto uriPrefixes = AZStd::to_array<UriPrefix>({
+            AZStd::move(modelUriPrefix),
+            AZStd::move(fileUriPrefix),
+            AZStd::move(packageUriPrefix)});
+        for (const UriPrefix& uriPrefix : uriPrefixes)
+        {
+            if (auto resolvedPath = uriPrefix.m_schemeResolver(unresolvedPath);
+                resolvedPath.has_value())
             {
-                auto it = --urdfProperPath.end();
-                for (; it != urdfProperPath.begin(); it--)
-                {
-                    const auto packagePath = GetPathFromSubPath(urdfProperPath.begin(), it);
-                    std::cout << "packagePath : " << packagePath.String().c_str() << std::endl;
-                    const auto resolvedPath = GetResolvedPath(packagePath, unresolvedPath, fileExists);
-                    if (resolvedPath.has_value())
-                    {
-                        AZ_Printf("ResolveURDFPath", "ResolveURDFPath with relative path to : %s\n", resolvedPath->String().c_str());
-                        return resolvedPath->String();
-                    }
-                }
+                AZ_Printf("ResolveURDFPath", R"(Resolved Path using URI Prefix "%.*s" -> "%.*s")" "\n", AZ_PATH_ARG(unresolvedPath),
+                    AZ_PATH_ARG(resolvedPath.value()));
+                return resolvedPath.value();
             }
-            // No path available
-            return "";
         }
-        if (unresolvedPath.starts_with("file:///"))
+
+        // At this point, the path has no URI scheme
+        if (unresolvedPath.IsAbsolute())
         {
-            // Paths that start with 'file:///' are absolute paths
-            AZ::StringFunc::Replace(unresolvedPath, "file://", "", true, true);
-            AZ_Printf("ResolveURDFPath", "ResolveURDFPath with absolute path to : %s\n", unresolvedPath.c_str());
+            AZ_Printf("ResolveURDFPath", "Input Path is an absolute local filesystem path to : %s\n", unresolvedPath.c_str());
             return unresolvedPath;
         }
-        // seems to be relative path
-        AZ::IO::Path relativePath(unresolvedPath);
-        AZ::IO::Path urdfProperPath(urdfFilePath);
-        AZ::IO::Path urdfParentPath = urdfProperPath.ParentPath();
-        const AZ::IO::Path resolvedPath = urdfParentPath / relativePath;
-        AZ_Printf("ResolveURDFPath", "ResolveURDFPath with relative path to : %s\n", unresolvedPath.c_str());
-        return resolvedPath.String();
+
+        // The path is relative path, so append to the directory containing the .urdf file
+        const AZ::IO::Path resolvedPath = AZ::IO::Path(urdfFilePath.ParentPath()) / unresolvedPath;
+        AZ_Printf("ResolveURDFPath", "Input Path %s is being returned as is\n", unresolvedPath.c_str());
+        return resolvedPath;
     }
 
-    namespace Utils::SDFormat
+} // namespace ROS2::Utils
+
+namespace ROS2::Utils::SDFormat
+{
+    AZStd::string GetPluginFilename(const sdf::Plugin& plugin)
     {
-        AZStd::string GetPluginFilename(const sdf::Plugin& plugin)
-        {
-            const AZ::IO::Path path{ plugin.Filename().c_str() };
-            return path.Filename().String();
-        }
+        const AZ::IO::Path path{ plugin.Filename().c_str() };
+        return path.Filename().String();
+    }
+
+    AZStd::vector<AZStd::string> GetUnsupportedParams(
+        const sdf::ElementPtr& rootElement, const AZStd::unordered_set<AZStd::string>& supportedParams)
+    {
+        AZStd::vector<AZStd::string> unsupportedParams;
 
-        AZStd::vector<AZStd::string> GetUnsupportedParams(
-            const sdf::ElementPtr& rootElement, const AZStd::unordered_set<AZStd::string>& supportedParams)
+        AZStd::function<void(const sdf::ElementPtr& elementPtr, const std::string& prefix)> elementVisitor =
+            [&](const sdf::ElementPtr& elementPtr, const std::string& prefix) -> void
         {
-            AZStd::vector<AZStd::string> unsupportedParams;
+            auto childPtr = elementPtr->GetFirstElement();
 
-            AZStd::function<void(const sdf::ElementPtr& elementPtr, const std::string& prefix)> elementVisitor =
-                [&](const sdf::ElementPtr& elementPtr, const std::string& prefix) -> void
+            AZStd::string prefixAz(prefix.c_str(), prefix.size());
+            if (!childPtr && !prefixAz.empty() && !supportedParams.contains(prefixAz))
             {
-                auto childPtr = elementPtr->GetFirstElement();
+                unsupportedParams.push_back(prefixAz);
+            }
 
-                AZStd::string prefixAz(prefix.c_str(), prefix.size());
-                if (!childPtr && !prefixAz.empty() && !supportedParams.contains(prefixAz))
+            while (childPtr)
+            {
+                if (childPtr->GetName() == "plugin")
                 {
-                    unsupportedParams.push_back(prefixAz);
+                    break;
                 }
 
-                while (childPtr)
-                {
-                    if (childPtr->GetName() == "plugin")
-                    {
-                        break;
-                    }
-
-                    std::string currentName = prefix;
-                    currentName.append(">");
-                    currentName.append(childPtr->GetName());
+                std::string currentName = prefix;
+                currentName.append(">");
+                currentName.append(childPtr->GetName());
 
-                    elementVisitor(childPtr, currentName);
-                    childPtr = childPtr->GetNextElement();
-                }
-            };
-
-            elementVisitor(rootElement, "");
+                elementVisitor(childPtr, currentName);
+                childPtr = childPtr->GetNextElement();
+            }
+        };
 
-            return unsupportedParams;
-        }
+        elementVisitor(rootElement, "");
 
-        bool IsPluginSupported(const sdf::Plugin& plugin, const AZStd::unordered_set<AZStd::string>& supportedPlugins)
-        {
-            return supportedPlugins.contains(GetPluginFilename(plugin));
-        }
-    } // namespace Utils::SDFormat
+        return unsupportedParams;
+    }
 
-} // namespace ROS2
+    bool IsPluginSupported(const sdf::Plugin& plugin, const AZStd::unordered_set<AZStd::string>& supportedPlugins)
+    {
+        return supportedPlugins.contains(GetPluginFilename(plugin));
+    }
+} // namespace ROS2::Utils::SDFormat

+ 126 - 83
Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h

@@ -19,89 +19,132 @@
 
 #include <sdf/sdf.hh>
 
-namespace ROS2
+namespace ROS2::Utils
 {
-    namespace
+    inline namespace Internal
     {
-        static inline bool FileExistsCall(const AZStd::string& filename)
-        {
-            return AZ::IO::SystemFile::Exists(filename.c_str());
-        };
-    } // namespace
+        bool FileExistsCall(const AZ::IO::PathView& filePath);
+    } // namespace Internal
+} // namespace ROS2::Utils
 
-    namespace Utils
-    {
-        //! Determine whether a given link is likely a wheel link.
-        //! This can be useful to provide a good default behavior - for example, to add Vehicle Dynamics components to this link's entity.
-        //! @param link the link that will be subjected to the heuristic.
-        //! @return true if the link is likely a wheel link.
-        bool IsWheelURDFHeuristics(const urdf::LinkConstSharedPtr& link);
-
-        //! The recursive function for the given link goes through URDF and finds world-to-entity transformation for us.
-        //! @param link pointer to URDF link that root of robot description
-        //! @param t initial transform, should be identity for non-recursive call.
-        //! @returns root to entity transform
-        AZ::Transform GetWorldTransformURDF(const urdf::LinkSharedPtr& link, AZ::Transform t = AZ::Transform::Identity());
-
-        //! Retrieve all links in URDF as a map, where a key is link's name and a value is a pointer to link.
-        //! Allows to retrieve a pointer to a link given it name.
-        //! @param childLinks list of links in a query
-        //! @returns mapping from link name to link pointer
-        AZStd::unordered_map<AZStd::string, urdf::LinkSharedPtr> GetAllLinks(const std::vector<urdf::LinkSharedPtr>& childLinks);
-
-        //! Retrieve all joints in URDF.
-        //! @param childLinks list of links in a query
-        //! @returns mapping from joint name to joint pointer
-        AZStd::unordered_map<AZStd::string, urdf::JointSharedPtr> GetAllJoints(const std::vector<urdf::LinkSharedPtr>& childLinks);
-
-        //! Retrieve all meshes referenced in URDF as unresolved URDF patches.
-        //! Note that returned filenames are unresolved URDF patches.
-        //! @param visual - search for visual meshes.
-        //! @param colliders - search for collider meshes.
-        //! @param rootLink - pointer to URDF link that is a root of robot description
-        //! @returns set of meshes' filenames.
-        AZStd::unordered_set<AZStd::string> GetMeshesFilenames(const urdf::LinkConstSharedPtr& rootLink, bool visual, bool colliders);
-
-        //! Resolves path from unresolved URDF path.
-        //! @param unresolvedPath - unresolved URDF path, example : `package://meshes/foo.dae`.
-        //! @param urdfFilePath - the absolute path of URDF file which contains the path that is to be resolved.
-        //! @param amentPrefixPath - the string that contains available packages' path, separated by ':' signs.
-        //! @param fileExists - functor to check if the given file exists. Exposed for unit test, default one should be used.
-        //! @returns resolved path to the mesh
-        AZStd::string ResolveURDFPath(
-            AZStd::string unresolvedPath,
-            const AZStd::string& urdfFilePath,
-            const AZStd::string& amentPrefixPath,
-            const AZStd::function<bool(const AZStd::string&)>& fileExists = FileExistsCall);
-
-        //! Waits for asset processor to process provided assets.
-        //! This function will timeout after the time specified in /O3DE/ROS2/RobotImporter/AssetProcessorTimeoutInSeconds
-        //! settings registry.
-        //! @param sourceAssetsPaths - set of all non relative paths to assets for which we want to wait for processing
-        //! @returns false if a timeout or error occurs, otherwise true
-        bool WaitForAssetsToProcess(const AZStd::unordered_map<AZStd::string, AZ::IO::Path>& sourceAssetsPaths);
-
-        namespace SDFormat
-        {
-            //! Retrieve plugin's filename. The filepath is converted into the filename if necessary.
-            //! @param plugin plugin in the parsed SDFormat data
-            //! @returns filename (including extension) without path
-            AZStd::string GetPluginFilename(const sdf::Plugin& plugin);
-
-            //! Retrieve all parameters that were defined for an element in XML data that are not supported in O3DE.
-            //! Allows to store the list of unsupported parameters in metadata and logs. It is typically used with sensors and plugins.
-            //! @param rootElement pointer to a root Element in parsed XML data that will be a subject to heuristics
-            //! @param supportedParams set of predefined parameters that are supported
-            //! @returns list of unsupported parameters defined for given element
-            AZStd::vector<AZStd::string> GetUnsupportedParams(
-                const sdf::ElementPtr& rootElement, const AZStd::unordered_set<AZStd::string>& supportedParams);
-
-            //! Check if plugin is supported by using it's filename. The filepath is converted into the filename if necessary.
-            //! @param plugin plugin in the parsed SDFormat data
-            //! @param supportedPlugins set of predefined plugins that are supported
-            //! @returns true if plugin is supported
-            bool IsPluginSupported(const sdf::Plugin& plugin, const AZStd::unordered_set<AZStd::string>& supportedPlugins);
-        } // namespace SDFormat
-
-    } // namespace Utils
-} // namespace ROS2
+namespace ROS2::Utils
+{
+    //! Determine whether a given link is likely a wheel link.
+    //! This can be useful to provide a good default behavior - for example, to add Vehicle Dynamics components to this link's entity.
+    //! @param sdfModel Model object which is used to query the joints from SDF format data
+    //! @param link the link that will be subjected to the heuristic.
+    //! @return true if the link is likely a wheel link.
+    bool IsWheelURDFHeuristics(const sdf::Model& model, const sdf::Link* link);
+
+    //! The recursive function for the given link goes through URDF and finds world-to-entity transformation for us.
+    //! @param link pointer to URDF link that root of robot description
+    //! @param t initial transform, should be identity for non-recursive call.
+    //! @returns root to entity transform
+    AZ::Transform GetWorldTransformURDF(const sdf::Link* link, AZ::Transform t = AZ::Transform::Identity());
+
+    //! Callback which is invoke for each link within a model
+    //! @return Return true to continue visiting links or false to halt
+    using LinkVisitorCallback = AZStd::function<bool(const sdf::Link&)>;
+    //! Visit links from URDF or SDF
+    //! @param sdfModel Model object of SDF document corresponding to the <model> tag. It used to query link
+    //! @param visitNestedModelLinks When true recurses to any nested <model> tags of the Model object and invoke visitor on their links as well
+    //! @returns void
+    void VisitLinks(const sdf::Model& sdfModel, const LinkVisitorCallback& linkVisitorCB,
+        bool visitNestedModelLinks = false);
+
+    //! Retrieve all links in URDF as a map, where a key is link's name and a value is a pointer to link.
+    //! Allows to retrieve a pointer to a link given it name.
+    //! @param sdfModel object of SDF document corresponding to the <model> tag. It used to query links
+    //! @param gatherNestedModelLinks When true recurses to any nested <model> tags of the Model object and also gathers their links as well
+    //! @returns mapping from link name to link pointer
+    AZStd::unordered_map<AZStd::string, const sdf::Link*> GetAllLinks(const sdf::Model& sdfModel,
+        bool gatherNestedModelLinks = false);
+
+    //! Callback which is invoke for each valid joint for a given model
+    //! @return Return true to continue visiting joint or false to halt
+    using JointVisitorCallback = AZStd::function<bool(const sdf::Joint&)>;
+    //! Visit joints from URDF
+    //! @param sdfModel Model object of SDF document corresponding to the <model> tag. It used to query joints
+    //! @param visitNestedModelJoints When true recurses to any nested <model> tags of the Model object and invoke visitor on their joints as well
+    //! @returns void
+    void VisitJoints(const sdf::Model& sdfModel, const JointVisitorCallback& jointVisitorCB,
+        bool visitNestedModelJoints = false);
+
+    //! Retrieve all joints in URDF.
+    //! @param sdfModel Model object of SDF document corresponding to the <model> tag. It used to query joints
+    //! @param gatherNestedModelJoints When true recurses to any nested <model> tags of the Model object and also gathers their joints as well
+    //! @returns mapping from joint name to joint pointer
+    AZStd::unordered_map<AZStd::string, const sdf::Joint*> GetAllJoints(const sdf::Model& sdfModel,
+        bool gatherNestedModelJoints = false);
+
+    //! Retrieve all joints from URDF in which the specified link is a child in a sdf::Joint.
+    //! @param sdfModel Model object of SDF document corresponding to the <model> tag. It used to query joints
+    //! @param linkName Name of link which to query in joint objects ChildName()
+    //! @param gatherNestedModelJoints When true recurses to any nested <model> tags of the Model object and also gathers their joints as well
+    //! @returns vector of joints where link is a child
+    AZStd::vector<const sdf::Joint*> GetJointsForChildLink(const sdf::Model& sdfModel, AZStd::string_view linkName,
+        bool gatherNestedModelJoints = false);
+
+    //! Retrieve all joints from URDF in which the specified link is a parent in a sdf::Joint.
+    //! @param sdfModel Model object of SDF document corresponding to the <model> tag. It used to query joints
+    //! @param linkName Name of link which to query in joint objects ParentName()
+    //! @param gatherNestedModelJoints When true recurses to any nested <model> tags of the Model object and also gathers their joints as well
+    //! @returns vector of joints where link is a parent
+    AZStd::vector<const sdf::Joint*> GetJointsForParentLink(const sdf::Model& sdfModel, AZStd::string_view linkName,
+        bool gatherNestedModelJoints = false);
+
+    //! Retrieve all meshes referenced in URDF as unresolved URDF patches.
+    //! Note that returned filenames are unresolved URDF patches.
+    //! @param visual - search for visual meshes.
+    //! @param colliders - search for collider meshes.
+    //! @param rootLink - pointer to sdf Rootobject that corresponds to to root of robot description after it hasbeen converted from URDF to SDF
+    //! @returns set of meshes' filenames.
+    AZStd::unordered_set<AZStd::string> GetMeshesFilenames(const sdf::Root* root, bool visual, bool colliders);
+
+    //! Callback used to check for file exist of a path referenced within a URDF/SDF file
+    //! @param path Candidate local filesystem path to check for existence
+    //! @return true should be returned if the file exist otherwise false
+    using FileExistsCB = AZStd::function<bool(const AZ::IO::PathView&)>;
+
+    //! Resolves path from unresolved URDF path.
+    //! @param unresolvedPath - unresolved URDF path, example : `package://meshes/foo.dae`.
+    //! @param urdfFilePath - the absolute path of URDF file which contains the path that is to be resolved.
+    //! @param amentPrefixPath - the string that contains available packages' path, separated by ':' signs.
+    //! @param fileExists - functor to check if the given file exists. Exposed for unit test, default one should be used.
+    //! @returns resolved path to the referenced file within the URDF
+    AZ::IO::Path ResolveURDFPath(
+        AZ::IO::Path unresolvedPath,
+        const AZ::IO::PathView& urdfFilePath,
+        const AZ::IO::PathView& amentPrefixPath,
+        const FileExistsCB& fileExists = &Internal::FileExistsCall);
+
+    //! Waits for asset processor to process provided assets.
+    //! This function will timeout after the time specified in /O3DE/ROS2/RobotImporter/AssetProcessorTimeoutInSeconds
+    //! settings registry.
+    //! @param sourceAssetsPaths - set of all non relative paths to assets for which we want to wait for processing
+    //! @returns false if a timeout or error occurs, otherwise true
+    bool WaitForAssetsToProcess(const AZStd::unordered_map<AZStd::string, AZ::IO::Path>& sourceAssetsPaths);
+
+} // namespace ROS2::Utils
+
+namespace ROS2::Utils::SDFormat
+{
+    //! Retrieve plugin's filename. The filepath is converted into the filename if necessary.
+    //! @param plugin plugin in the parsed SDFormat data
+    //! @returns filename (including extension) without path
+    AZStd::string GetPluginFilename(const sdf::Plugin& plugin);
+
+    //! Retrieve all parameters that were defined for an element in XML data that are not supported in O3DE.
+    //! Allows to store the list of unsupported parameters in metadata and logs. It is typically used with sensors and plugins.
+    //! @param rootElement pointer to a root Element in parsed XML data that will be a subject to heuristics
+    //! @param supportedParams set of predefined parameters that are supported
+    //! @returns list of unsupported parameters defined for given element
+    AZStd::vector<AZStd::string> GetUnsupportedParams(
+        const sdf::ElementPtr& rootElement, const AZStd::unordered_set<AZStd::string>& supportedParams);
+
+    //! Check if plugin is supported by using it's filename. The filepath is converted into the filename if necessary.
+    //! @param plugin plugin in the parsed SDFormat data
+    //! @param supportedPlugins set of predefined plugins that are supported
+    //! @returns true if plugin is supported
+    bool IsPluginSupported(const sdf::Plugin& plugin, const AZStd::unordered_set<AZStd::string>& supportedPlugins);
+} // namespace ROS2::Utils::SDFormat

+ 22 - 13
Gems/ROS2/Code/Source/RobotImporter/Utils/SourceAssetsStorage.cpp

@@ -71,7 +71,7 @@ namespace ROS2::Utils
     }
 
     /// Function computes CRC32 on first kilobyte of file.
-    AZ::Crc32 GetFileCRC(const AZStd::string& filename)
+    AZ::Crc32 GetFileCRC(const AZ::IO::Path& filename)
     {
         auto fileSize = AZ::IO::SystemFile::Length(filename.c_str());
         fileSize = AZStd::min(fileSize, 1024ull); // limit crc computation to first kilobyte
@@ -291,7 +291,7 @@ namespace ROS2::Utils
         AZ::Crc32 urdfFileCrc;
         urdfFileCrc.Add(urdfFilename);
         const AZ::IO::Path urdfPath(urdfFilename);
-      
+
         const auto directoryNameDst = AZ::IO::FixedMaxPathString::format(
             "%u_%.*s%.*s", AZ::u32(urdfFileCrc), AZ_PATH_ARG(urdfPath.Stem()), AZ_STRING_ARG(outputDirSuffix));
 
@@ -309,7 +309,8 @@ namespace ROS2::Utils
 
         for (const auto& unresolvedUrfFileName : meshesFilenames)
         {
-            auto resolved = Utils::ResolveURDFPath(unresolvedUrfFileName, urdfFilename, amentPrefixPath);
+            auto resolved = Utils::ResolveURDFPath(unresolvedUrfFileName, AZ::IO::PathView(urdfFilename),
+                AZ::IO::PathView(amentPrefixPath));
             if (resolved.empty())
             {
                 AZ_Warning("CopyAssetForURDF", false, "There is not resolved path for %s", unresolvedUrfFileName.c_str());
@@ -347,7 +348,8 @@ namespace ROS2::Utils
 
             Utils::UrdfAsset asset;
             asset.m_urdfPath = urdfFilename;
-            asset.m_resolvedUrdfPath = Utils::ResolveURDFPath(unresolvedUrfFileName, urdfFilename, amentPrefixPath);
+            asset.m_resolvedUrdfPath = Utils::ResolveURDFPath(unresolvedUrfFileName, AZ::IO::PathView(urdfFilename),
+                AZ::IO::PathView(amentPrefixPath));
             asset.m_urdfFileCRC = AZ::Crc32();
             urdfAssetMap.emplace(unresolvedUrfFileName, AZStd::move(asset));
         }
@@ -380,16 +382,23 @@ namespace ROS2::Utils
 
     UrdfAssetMap FindAssetsForUrdf(const AZStd::unordered_set<AZStd::string>& meshesFilenames, const AZStd::string& urdfFilename)
     {
-        auto enviromentalVariable = std::getenv("AMENT_PREFIX_PATH");
-        AZ_Error("UrdfAssetMap", enviromentalVariable, "AMENT_PREFIX_PATH is not found.");
-        AZStd::string amentPrefixPath{ enviromentalVariable };
+        // Support reading the AMENT_PREFIX_PATH environment variable on Unix/Windows platforms
+        auto StoreAmentPrefixPath = [](char* buffer, size_t size) -> size_t
+        {
+            auto getEnvOutcome = AZ::Utils::GetEnv(AZStd::span(buffer, size), "AMENT_PREFIX_PATH");
+            return getEnvOutcome ? getEnvOutcome.GetValue().size() : 0;
+        };
+        AZStd::fixed_string<4096> amentPrefixPath;
+        amentPrefixPath.resize_and_overwrite(amentPrefixPath.capacity(), StoreAmentPrefixPath);
+        AZ_Error("UrdfAssetMap", !amentPrefixPath.empty(), "AMENT_PREFIX_PATH is not found.");
 
         UrdfAssetMap urdfToAsset;
         for (const auto& t : meshesFilenames)
         {
             Utils::UrdfAsset asset;
             asset.m_urdfPath = t;
-            asset.m_resolvedUrdfPath = Utils::ResolveURDFPath(asset.m_urdfPath, urdfFilename, amentPrefixPath);
+            asset.m_resolvedUrdfPath = Utils::ResolveURDFPath(asset.m_urdfPath, AZ::IO::PathView(urdfFilename),
+                AZ::IO::PathView(amentPrefixPath));
             asset.m_urdfFileCRC = Utils::GetFileCRC(asset.m_resolvedUrdfPath);
             urdfToAsset.emplace(t, AZStd::move(asset));
         }
@@ -413,9 +422,9 @@ namespace ROS2::Utils
         return urdfToAsset;
     }
 
-    bool CreateSceneManifest(const AZStd::string& sourceAssetPath, const AZStd::string& assetInfoFile, bool collider, bool visual)
+    bool CreateSceneManifest(const AZ::IO::Path& sourceAssetPath, const AZ::IO::Path& assetInfoFile, bool collider, bool visual)
     {
-        const AZStd::string azMeshPath = sourceAssetPath;
+        const auto& azMeshPath = sourceAssetPath;
         AZ_Printf("CreateSceneManifest", "Creating manifest for asset %s at : %s ", sourceAssetPath.c_str(), assetInfoFile.c_str());
         AZStd::shared_ptr<AZ::SceneAPI::Containers::Scene> scene;
         AZ::SceneAPI::Events::SceneSerializationBus::BroadcastResult(
@@ -489,16 +498,16 @@ namespace ROS2::Utils
             return false;
         }
 
-        scene->GetManifest().SaveToFile(assetInfoFile.c_str());
+        scene->GetManifest().SaveToFile(assetInfoFile.Native());
         AZ_Printf("CreateSceneManifest", "Saving scene manifest to %s\n", assetInfoFile.c_str());
 
         return true;
     }
 
-    bool CreateSceneManifest(const AZStd::string& sourceAssetPath, bool collider, bool visual)
+    bool CreateSceneManifest(const AZ::IO::Path& sourceAssetPath, bool collider, bool visual)
     {
         auto assetInfoFilePath = AZ::IO::Path{ sourceAssetPath };
         assetInfoFilePath.Native() += ".assetinfo";
-        return CreateSceneManifest(sourceAssetPath, assetInfoFilePath.String(), collider, visual);
+        return CreateSceneManifest(sourceAssetPath, sourceAssetPath.Native() + ".assetinfo", collider, visual);
     }
 } // namespace ROS2::Utils

+ 10 - 9
Gems/ROS2/Code/Source/RobotImporter/Utils/SourceAssetsStorage.h

@@ -13,6 +13,7 @@
 #include <AzCore/Asset/AssetManager.h>
 #include <AzCore/Asset/AssetManagerBus.h>
 #include <AzCore/IO/FileIO.h>
+#include <AzCore/IO/Path/Path.h>
 #include <AzCore/Math/Crc.h>
 #include <AzCore/std/containers/unordered_map.h>
 #include <AzCore/std/containers/unordered_set.h>
@@ -25,13 +26,13 @@ namespace ROS2::Utils
     struct AvailableAsset
     {
         //! Relative path to source asset eg `Assets/foo_robot/meshes/bar_link.dae`.
-        AZStd::string m_sourceAssetRelativePath;
+        AZ::IO::Path m_sourceAssetRelativePath;
 
         //! Relative path to source asset eg `/home/user/project/Assets/foo_robot/meshes/bar_link.dae`.
-        AZStd::string m_sourceAssetGlobalPath;
+        AZ::IO::Path m_sourceAssetGlobalPath;
 
         //! Relative path to source asset eg `foo_robot/meshes/bar_link.azmodel`.
-        AZStd::string m_productAssetRelativePath;
+        AZ::IO::Path m_productAssetRelativePath;
 
         //! Source GUID of source asset
         AZ::Uuid m_sourceGuid = AZ::Uuid::CreateNull();
@@ -41,10 +42,10 @@ namespace ROS2::Utils
     struct UrdfAsset
     {
         //! Unresolved URDF path to mesh, eg `package://meshes/bar_link.dae`.
-        AZStd::string m_urdfPath;
+        AZ::IO::Path m_urdfPath;
 
         //! Resolved URDF path, points to the valid mesh in the filestystem, eg `/home/user/ros_ws/src/foo_robot/meshes/bar_link.dae'
-        AZStd::string m_resolvedUrdfPath;
+        AZ::IO::Path m_resolvedUrdfPath;
 
         //! Checksum of the file located pointed by `m_resolvedUrdfPath`.
         AZ::Crc32 m_urdfFileCRC;
@@ -54,10 +55,10 @@ namespace ROS2::Utils
     };
 
     /// Type that hold result of mapping from URDF path to asset info
-    using UrdfAssetMap = AZStd::unordered_map<AZStd::string, Utils::UrdfAsset>;
+    using UrdfAssetMap = AZStd::unordered_map<AZ::IO::Path, Utils::UrdfAsset>;
 
     //! Function computes CRC32 on first kilobyte of file.
-    AZ::Crc32 GetFileCRC(const AZStd::string& filename);
+    AZ::Crc32 GetFileCRC(const AZ::IO::Path& filename);
 
     //! Compute CRC for every source mesh from the assets catalog.
     //! @returns map where key is crc of source file and value is AvailableAsset.
@@ -113,7 +114,7 @@ namespace ROS2::Utils
     //! @param collider - create assetinfo section for collider product asset
     //! @param visual - create assetinfo section for visual mesh
     //! @returns true if succeed
-    bool CreateSceneManifest(const AZStd::string& sourceAssetPath, bool collider, bool visual);
+    bool CreateSceneManifest(const AZ::IO::Path& sourceAssetPath, bool collider, bool visual);
 
     //! Creates side-car file (.assetinfo) that configures the imported scene (eg DAE file).
     //! @param sourceAssetPath - global path to source asset
@@ -121,7 +122,7 @@ namespace ROS2::Utils
     //! @param collider - create assetinfo section for collider product asset
     //! @param visual - create assetinfo section for visual mesh
     //! @returns true if succeed
-    bool CreateSceneManifest(const AZStd::string& sourceAssetPath, const AZStd::string& assetInfoFile, bool collider, bool visual);
+    bool CreateSceneManifest(const AZ::IO::Path& sourceAssetPath, const AZ::IO::Path& assetInfoFile, bool collider, bool visual);
 
     //! A function that copies and prepares meshes that are referenced in URDF.
     //! It resolves every mesh, creates a directory in Project's Asset directory, copies files, and prepares assets info.

+ 9 - 9
Gems/ROS2/Code/Source/RobotImporter/Utils/TypeConversions.cpp

@@ -10,25 +10,25 @@
 
 namespace ROS2::URDF
 {
-    AZ::Vector3 TypeConversions::ConvertVector3(const urdf::Vector3& urdfVector)
+    AZ::Vector3 TypeConversions::ConvertVector3(const gz::math::Vector3d& urdfVector)
     {
-        return AZ::Vector3(urdfVector.x, urdfVector.y, urdfVector.z);
+        return AZ::Vector3(urdfVector.X(), urdfVector.Y(), urdfVector.Z());
     }
 
-    AZ::Quaternion TypeConversions::ConvertQuaternion(const urdf::Rotation& urdfQuaternion)
+    AZ::Quaternion TypeConversions::ConvertQuaternion(const gz::math::Quaterniond& urdfQuaternion)
     {
-        return AZ::Quaternion(urdfQuaternion.x, urdfQuaternion.y, urdfQuaternion.z, urdfQuaternion.w);
+        return AZ::Quaternion(urdfQuaternion.X(), urdfQuaternion.Y(), urdfQuaternion.Z(), urdfQuaternion.W());
     }
 
-    AZ::Color TypeConversions::ConvertColor(const urdf::Color& color)
+    AZ::Color TypeConversions::ConvertColor(const gz::math::Color& color)
     {
-        return AZ::Color(color.r, color.g, color.b, color.a);
+        return AZ::Color(color.R(), color.G(), color.B(), color.A());
     }
 
-    AZ::Transform TypeConversions::ConvertPose(const urdf::Pose& pose)
+    AZ::Transform TypeConversions::ConvertPose(const gz::math::Pose3d& pose)
     {
-        AZ::Quaternion azRotation = URDF::TypeConversions::ConvertQuaternion(pose.rotation);
-        AZ::Vector3 azPosition = URDF::TypeConversions::ConvertVector3(pose.position);
+        AZ::Quaternion azRotation = URDF::TypeConversions::ConvertQuaternion(pose.Rot());
+        AZ::Vector3 azPosition = URDF::TypeConversions::ConvertVector3(pose.Pos());
         return AZ::Transform(azPosition, azRotation, 1.0f);
     }
 

+ 4 - 4
Gems/ROS2/Code/Source/RobotImporter/Utils/TypeConversions.h

@@ -19,9 +19,9 @@ namespace ROS2::URDF
     //! Common types conversion between urdf and AZ formats
     namespace TypeConversions
     {
-        AZ::Vector3 ConvertVector3(const urdf::Vector3& urdfVector);
-        AZ::Quaternion ConvertQuaternion(const urdf::Rotation& urdfQuaternion);
-        AZ::Color ConvertColor(const urdf::Color& color);
-        AZ::Transform ConvertPose(const urdf::Pose& pose);
+        AZ::Vector3 ConvertVector3(const gz::math::Vector3d& urdfVector);
+        AZ::Quaternion ConvertQuaternion(const gz::math::Quaterniond& urdfQuaternion);
+        AZ::Color ConvertColor(const gz::math::Color& color);
+        AZ::Transform ConvertPose(const gz::math::Pose3d& pose);
     }; // namespace TypeConversions
 } // namespace ROS2::URDF

+ 40 - 9
Gems/ROS2/Code/Source/RobotImporter/xacro/XacroUtils.cpp

@@ -14,14 +14,15 @@
 #include <AzFramework/Process/ProcessWatcher.h>
 #include <QString>
 
+#include <SdfAssetBuilder/SdfAssetBuilderSettings.h>
+
 namespace ROS2::Utils::xacro
 {
-
     ExecutionOutcome ParseXacro(const AZStd::string& filename, const Params& params)
     {
         ExecutionOutcome outcome;
         // test if xacro exists
-        AZStd::string xacroPath = "xacro";
+        AZ::IO::Path xacroPath = "xacro";
         auto settingsRegistry = AZ::SettingsRegistry::Get();
         AZStd::string xacroExecutablePath;
         if (settingsRegistry && settingsRegistry->Get(xacroExecutablePath, "/O3DE/ROS2/xacro_executable_path"))
@@ -35,16 +36,40 @@ namespace ROS2::Utils::xacro
             }
         }
 
-        AZ_Warning("ParseXacro", !xacroPath.empty(), "There is xacro executable in your path");
-        if (xacroPath.empty())
+        AzFramework::ProcessLauncher::ProcessLaunchInfo processLaunchInfo;
+        processLaunchInfo.m_processExecutableString = xacroPath.Native();
+        processLaunchInfo.m_commandlineParameters.emplace<AZStd::vector<AZStd::string>>({ AZStd::string("--help") });
+        if (AZStd::unique_ptr<AzFramework::ProcessWatcher> xacroHelpProcess(AzFramework::ProcessWatcher::LaunchProcess(processLaunchInfo,
+            AzFramework::ProcessCommunicationType::COMMUNICATOR_TYPE_STDINOUT));
+            xacroHelpProcess != nullptr)
         {
-            return ExecutionOutcome{};
+            // 60 seconds is used as a cap on how long it should take to run the `xacro --help` command
+            // In reality this should take less than a second, but there has been a couple of occurrences
+            // where it takes over a second to run. 60 seconds is a fail fase value
+            constexpr AZStd::chrono::seconds helpCommandWaitTime{ 60 };
+
+            AZ::u32 exitCode{ AZStd::numeric_limits<AZ::u32>::max() };
+            xacroHelpProcess->WaitForProcessToExit(static_cast<AZ::u32>(helpCommandWaitTime.count()), &exitCode);
+            if (exitCode != 0)
+            {
+                const auto fullProcessCommand = AZStd::string::format("%s %s",
+                    processLaunchInfo.m_processExecutableString.c_str(),
+                    processLaunchInfo.GetCommandLineParametersAsString().c_str());
+                AZ_Warning(
+                    "ParseXacro",
+                    false,
+                    "Attempted to run validate xacro existence with command: %s",
+                    fullProcessCommand.c_str());
+                outcome.m_called = fullProcessCommand;
+                return outcome;
+            }
         }
+
         AZ_Printf("ParseXacro", "xacro executable : %s \n", xacroPath.c_str());
         AZ_Printf("ParseXacro", "Convert xacro file : %s \n", filename.c_str());
-        const QString program = QString::fromUtf8(xacroPath.data(), xacroPath.size());
-        AzFramework::ProcessLauncher::ProcessLaunchInfo processLaunchInfo;
-        processLaunchInfo.m_processExecutableString = xacroPath;
+        // Clear out the processLanunchInfo structure
+        processLaunchInfo = {};
+        processLaunchInfo.m_processExecutableString = xacroPath.Native();
         processLaunchInfo.m_commandlineParameters.emplace<AZStd::vector<AZStd::string>>({ filename });
         for (const auto& param : params)
         {
@@ -65,7 +90,13 @@ namespace ROS2::Utils::xacro
         {
             AZ_Printf("ParseXacro", "xacro finished with success \n");
             const auto& output = process_output.outputResult;
-            outcome.m_urdfHandle = UrdfParser::Parse(output.data());
+            // Read the SDF Settings from the Settings Registry into a local struct
+            SdfAssetBuilderSettings sdfBuilderSettings;
+            sdfBuilderSettings.LoadSettings();
+            // Set the parser config settings for URDF content
+            sdf::ParserConfig parserConfig;
+            parserConfig.URDFSetPreserveFixedJoint(sdfBuilderSettings.m_urdfPreserveFixedJoints);
+            outcome.m_urdfHandle = UrdfParser::Parse(output, parserConfig);
             outcome.m_succeed = true;
         }
         else

+ 2 - 2
Gems/ROS2/Code/Source/RobotImporter/xacro/XacroUtils.h

@@ -21,7 +21,7 @@ namespace ROS2::Utils::xacro
     struct ExecutionOutcome
     {
         //! Parsed URDF from successful xacro's output
-        urdf::ModelInterfaceSharedPtr m_urdfHandle;
+        UrdfParser::RootObjectOutcome m_urdfHandle;
         //! Return code of 'xacro' program
         bool m_succeed{ false };
         //! Called program name
@@ -34,7 +34,7 @@ namespace ROS2::Utils::xacro
         //! Gets if execution was a success
         explicit operator bool() const
         {
-            return m_succeed && m_urdfHandle != nullptr;
+            return m_succeed && m_urdfHandle;
         }
     };
 

+ 48 - 30
Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilder.cpp

@@ -24,15 +24,16 @@
 
 #include <RobotImporter/URDF/URDFPrefabMaker.h>
 #include <RobotImporter/URDF/UrdfParser.h>
+#include <SdfAssetBuilder/SdfAssetBuilderSettings.h>
+#include <RobotImporter/Utils/ErrorUtils.h>
 #include <Utils/RobotImporterUtils.h>
 
 namespace ROS2
 {
-        namespace
-        {
-            [[maybe_unused]] constexpr const char* SdfAssetBuilderName = "SdfAssetBuilder";
-            constexpr const char* SdfAssetBuilderJobKey = "Sdf Asset Builder";
-        }
+    inline namespace SDFAssetBuilderInternal
+    {
+        constexpr const char* SdfAssetBuilderJobKey = "Sdf Asset Builder";
+    }
 
     SdfAssetBuilder::SdfAssetBuilder()
     {
@@ -44,7 +45,7 @@ namespace ROS2
         // The fingerprint should only use the global builder settings, not the per-file settings.
         // The analysis fingerprint is set at the builder level, outside of any individual files,
         // so it should only include data that's invariant across files.
-        // Per-file settings changes will cause rebuilds of individual files through a separate 
+        // Per-file settings changes will cause rebuilds of individual files through a separate
         // mechanism in the Asset Processor that detects when an associated metadata settings file changes.
         m_fingerprint = GetFingerprint();
 
@@ -56,14 +57,14 @@ namespace ROS2
         sdfAssetBuilderDescriptor.m_patterns = m_globalSettings.m_builderPatterns;
         sdfAssetBuilderDescriptor.m_analysisFingerprint = m_fingerprint; // set the fingerprint to the global settings
 
-        sdfAssetBuilderDescriptor.m_createJobFunction = [this](auto && request, auto && response) 
-            { 
-                return CreateJobs(request, response); 
+        sdfAssetBuilderDescriptor.m_createJobFunction = [this](auto && request, auto && response)
+            {
+                return CreateJobs(request, response);
             };
 
-        sdfAssetBuilderDescriptor.m_processJobFunction = [this](auto && request, auto && response) 
-            { 
-                return ProcessJob(request, response); 
+        sdfAssetBuilderDescriptor.m_processJobFunction = [this](auto && request, auto && response)
+            {
+                return ProcessJob(request, response);
             };
 
         // Listen for asset builder notifications requesting jobs for any of the sdf source file types.
@@ -82,10 +83,10 @@ namespace ROS2
         // The AssetBuilderSDK doesn't support deregistration, so there's nothing more to do here.
     }
 
-    Utils::UrdfAssetMap SdfAssetBuilder::FindAssets(const urdf::LinkConstSharedPtr& rootLink, const AZStd::string& sourceFilename) const
+    Utils::UrdfAssetMap SdfAssetBuilder::FindAssets(const sdf::Root& root, const AZStd::string& sourceFilename) const
     {
         AZ_Info(SdfAssetBuilderName, "Parsing mesh and collider names");
-        auto assetNames = Utils::GetMeshesFilenames(rootLink, true, true);
+        auto assetNames = Utils::GetMeshesFilenames(&root, true, true);
 
         Utils::UrdfAssetMap assetMap;
 
@@ -94,7 +95,7 @@ namespace ROS2
         // Unlike the RobotImporter, the SDF Asset Builder does not use the AMENT_PREFIX_PATH
         // to resolve file locations. There wouldn't be a way to guarantee identical results across
         // machines or to detect the need to rebuild assets if the environment variable changes.
-        const AZStd::string emptyAmentPrefixPath;
+        constexpr AZ::IO::PathView emptyAmentPrefixPath;
 
         for (const auto& uri : assetNames)
         {
@@ -102,7 +103,8 @@ namespace ROS2
             asset.m_urdfPath = uri;
 
             // Attempt to find the absolute path for the raw uri reference, which might look something like "model://meshes/model.dae"
-            asset.m_resolvedUrdfPath = Utils::ResolveURDFPath(asset.m_urdfPath, sourceFilename, emptyAmentPrefixPath);
+            asset.m_resolvedUrdfPath = Utils::ResolveURDFPath(asset.m_urdfPath, AZ::IO::PathView(sourceFilename),
+                emptyAmentPrefixPath);
             if (asset.m_resolvedUrdfPath.empty())
             {
                 AZ_Warning(SdfAssetBuilderName, false, "Failed to resolve file reference '%s' to an absolute path, skipping.", uri.c_str());
@@ -119,7 +121,7 @@ namespace ROS2
             AZ::Data::AssetInfo assetInfo;
             AZStd::string watchFolder;
             AssetSysReqBus::BroadcastResult(
-                sourceAssetFound, &AssetSysReqBus::Events::GetSourceInfoBySourcePath, 
+                sourceAssetFound, &AssetSysReqBus::Events::GetSourceInfoBySourcePath,
                 asset.m_resolvedUrdfPath.c_str(), assetInfo, watchFolder);
 
             if (!sourceAssetFound)
@@ -165,7 +167,7 @@ namespace ROS2
 
         [[maybe_unused]] AZ::Outcome<void, AZStd::string> saveObjectResult =
             AZ::JsonSerializationUtils::SaveObjectToStream(&m_globalSettings, stream, {}, &jsonSettings);
-        AZ_Assert(saveObjectResult.IsSuccess(), "Failed to save settings to fingerprint string: %s", 
+        AZ_Assert(saveObjectResult.IsSuccess(), "Failed to save settings to fingerprint string: %s",
             saveObjectResult.GetError().c_str());
 
         return settingsString;
@@ -176,7 +178,7 @@ namespace ROS2
         AssetBuilderSDK::CreateJobsResponse& response) const
     {
         // To be able to successfully process the SDF job, we need job dependencies on every asset
-        // referenced by the SDF file. Otherwise we won't be able to connect the references to the 
+        // referenced by the SDF file. Otherwise we won't be able to connect the references to the
         // correct product assets. Unfortunately, this means that we need to redundantly parse the
         // source file once here to set up the job dependencies, and then we'll parse it a second
         // time when actually running ProcessJob().
@@ -187,16 +189,24 @@ namespace ROS2
 
         const auto fullSourcePath = AZ::IO::Path(request.m_watchFolder) / AZ::IO::Path(request.m_sourceFile);
 
+        // Set the parser config settings for parsing URDF content through the libsdformat parser
+        sdf::ParserConfig parserConfig;
+        parserConfig.URDFSetPreserveFixedJoint(m_globalSettings.m_urdfPreserveFixedJoints);
+
         AZ_Info(SdfAssetBuilderName, "Parsing source file: %s", fullSourcePath.c_str());
-        auto parsedSourceFile = UrdfParser::ParseFromFile(fullSourcePath.String());
-        if (!parsedSourceFile)
+        auto parsedSdfRootOutcome = UrdfParser::ParseFromFile(fullSourcePath, parserConfig);
+        if (!parsedSdfRootOutcome)
         {
-            AZ_Error(SdfAssetBuilderName, false, "Failed to parse source file '%s'.", fullSourcePath.c_str());
+            const AZStd::string sdfParseErrors = Utils::JoinSdfErrorsToString(parsedSdfRootOutcome.error());
+            AZ_Error(SdfAssetBuilderName, false, R"(Failed to parse source file "%s". Errors: "%s")",
+                fullSourcePath.c_str(), sdfParseErrors.c_str());
             return;
         }
 
+        const sdf::Root& sdfRoot = parsedSdfRootOutcome.value();
+
         AZ_Info(SdfAssetBuilderName, "Finding asset IDs for all mesh and collider assets.");
-        auto sourceAssetMap = AZStd::make_shared<Utils::UrdfAssetMap>(FindAssets(parsedSourceFile->getRoot(), fullSourcePath.String()));
+        auto sourceAssetMap = AZStd::make_shared<Utils::UrdfAssetMap>(FindAssets(sdfRoot, fullSourcePath.String()));
 
         // Create an output job for each platform
         for (const AssetBuilderSDK::PlatformInfo& platformInfo : request.m_enabledPlatforms)
@@ -241,24 +251,32 @@ namespace ROS2
         auto tempAssetOutputPath = AZ::IO::Path(request.m_tempDirPath) / request.m_sourceFile;
         tempAssetOutputPath.ReplaceExtension("procprefab");
 
+        // Set the parser config settings for parsing URDF content through the libsdformat parser
+        sdf::ParserConfig parserConfig;
+        parserConfig.URDFSetPreserveFixedJoint(m_globalSettings.m_urdfPreserveFixedJoints);
+
         // Read in and parse the source SDF file.
         AZ_Info(SdfAssetBuilderName, "Parsing source file: %s", request.m_fullPath.c_str());
-        auto parsedSourceFile = UrdfParser::ParseFromFile(request.m_fullPath);
-        if (!parsedSourceFile)
+        auto parsedSdfRootOutcome = UrdfParser::ParseFromFile(AZ::IO::PathView(request.m_fullPath), parserConfig);
+        if (!parsedSdfRootOutcome)
         {
-            AZ_Error(SdfAssetBuilderName, false, "Failed to parse source file '%s'.", request.m_fullPath.c_str());
+            const AZStd::string sdfParseErrors = Utils::JoinSdfErrorsToString(parsedSdfRootOutcome.error());
+            AZ_Error(SdfAssetBuilderName, false, R"(Failed to parse source file "%s". Errors: "%s")",
+                request.m_fullPath.c_str(), sdfParseErrors.c_str());
             response.m_resultCode = AssetBuilderSDK::ProcessJobResult_Failed;
             return;
         }
 
+        const sdf::Root& sdfRoot = parsedSdfRootOutcome.value();
+
         // Resolve all the URI references into source asset GUIDs.
         AZ_Info(SdfAssetBuilderName, "Finding asset IDs for all mesh and collider assets.");
-        auto assetMap = AZStd::make_shared<Utils::UrdfAssetMap>(FindAssets(parsedSourceFile->getRoot(), request.m_fullPath));
+        auto assetMap = AZStd::make_shared<Utils::UrdfAssetMap>(FindAssets(sdfRoot, request.m_fullPath));
 
         // Given the parsed source file and asset mappings, generate an in-memory prefab.
         AZ_Info(SdfAssetBuilderName, "Creating prefab from source file.");
         auto prefabMaker = AZStd::make_unique<URDFPrefabMaker>(
-            request.m_fullPath, parsedSourceFile, tempAssetOutputPath.String(), assetMap, useArticulation);
+            request.m_fullPath, &sdfRoot, tempAssetOutputPath.String(), assetMap, useArticulation);
         auto prefabResult = prefabMaker->CreatePrefabTemplateFromURDF();
         if (!prefabResult.IsSuccess())
         {
@@ -278,14 +296,14 @@ namespace ROS2
 
         if (!saveResult)
         {
-            AZ_Error(SdfAssetBuilderName, false, "Failed to write out temp asset file '%s'.", 
+            AZ_Error(SdfAssetBuilderName, false, "Failed to write out temp asset file '%s'.",
                 tempAssetOutputPath.c_str());
             response.m_resultCode = AssetBuilderSDK::ProcessJobResult_Failed;
             return;
         }
 
         AZ_Info(SdfAssetBuilderName, "Prefab creation completed successfully.");
-        
+
         // Mark the resulting prefab as a product asset with the "procedural prefab" asset type.
         AssetBuilderSDK::JobProduct sdfJobProduct;
         sdfJobProduct.m_productFileName = tempAssetOutputPath.String();

+ 3 - 1
Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilder.h

@@ -17,6 +17,8 @@
 
 namespace ROS2
 {
+    [[maybe_unused]] constexpr const char* SdfAssetBuilderName = "SdfAssetBuilder";
+
     //! Builder to convert the following file types into procedural prefab assets:
     //! * sdf (Simulation Description Format: http://sdformat.org/ )
     //! * urdf (Unified Robotics Description Format: http://wiki.ros.org/urdf )
@@ -41,7 +43,7 @@ namespace ROS2
         AZStd::string GetFingerprint() const;
 
         //! Create a mapping of all the asset references in the source file.
-        Utils::UrdfAssetMap FindAssets(const urdf::LinkConstSharedPtr& rootLink, const AZStd::string& sourceFilename) const;
+        Utils::UrdfAssetMap FindAssets(const sdf::Root& root, const AZStd::string& sourceFilename) const;
 
         SdfAssetBuilderSettings m_globalSettings;
         AZStd::string m_fingerprint;

+ 42 - 5
Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilderSettings.cpp

@@ -7,17 +7,50 @@
  */
 
 #include <SdfAssetBuilder/SdfAssetBuilderSettings.h>
+#include <SdfAssetBuilder/SdfAssetBuilder.h>
 
 #include <AzCore/Serialization/Json/JsonUtils.h>
 #include <AzCore/Settings/SettingsRegistryVisitorUtils.h>
 
 namespace ROS2
 {
-        namespace
+    namespace
+    {
+        struct SDFSettingsRootKeyType
         {
-            constexpr const char* SdfAssetBuilderSupportedFileExtensionsRegistryKey = "/O3DE/ROS2/SdfAssetBuilder/SupportedFileTypeExtensions";
-            constexpr const char* SdfAssetBuilderUseArticulationsRegistryKey = "/O3DE/ROS2/SdfAssetBuilder/UseArticulations";
-        }
+            using StringType = AZStd::fixed_string<256>;
+
+            constexpr StringType operator()(AZStd::string_view name) const
+            {
+                constexpr size_t MaxTotalKeySize = StringType{}.max_size();
+                // The +1 is for the '/' separator
+                [[maybe_unused]] const size_t maxNameSize = MaxTotalKeySize - (SettingsPrefix.size() + 1);
+
+                AZ_Assert(name.size() <= maxNameSize,
+                    R"(The size of the event logger name "%.*s" is too long. It must be <= %zu characters)",
+                    AZ_STRING_ARG(name), maxNameSize);
+                StringType settingsKey(SettingsPrefix);
+                settingsKey += '/';
+                settingsKey += name;
+
+                return settingsKey;
+            }
+
+            constexpr operator AZStd::string_view() const
+            {
+                return SettingsPrefix;
+            }
+
+        private:
+            AZStd::string_view SettingsPrefix = "/O3DE/ROS2/SdfAssetBuilder";
+        };
+
+        constexpr SDFSettingsRootKeyType SDFSettingsRootKey;
+
+        constexpr auto SdfAssetBuilderSupportedFileExtensionsRegistryKey = SDFSettingsRootKey("SupportedFileTypeExtensions");
+        constexpr auto SdfAssetBuilderUseArticulationsRegistryKey = SDFSettingsRootKey("UseArticulations");
+        constexpr auto SdfAssetBuilderURDFPreserveFixedJointRegistryKey = SDFSettingsRootKey("URDFPreserveFixedJoint");
+    }
 
     void SdfAssetBuilderSettings::Reflect(AZ::ReflectContext* context)
     {
@@ -26,11 +59,12 @@ namespace ROS2
             serializeContext->Class<SdfAssetBuilderSettings>()
                 ->Version(0)
                 ->Field("UseArticulations", &SdfAssetBuilderSettings::m_useArticulations)
+                ->Field("URDFPreserveFixedJoint", &SdfAssetBuilderSettings::m_urdfPreserveFixedJoints)
 
                 // m_builderPatterns aren't serialized because we only use the serialization
                 // to detect when global settings changes cause us to rebuild our assets.
                 // A change to the builder patterns will cause the Asset Processor to add or
-                // remove the affected product assets, so we don't need to trigger any 
+                // remove the affected product assets, so we don't need to trigger any
                 // additional rebuilds beyond that.
              ;
         }
@@ -48,6 +82,9 @@ namespace ROS2
         // Default to using articulations.
         settingsRegistry->Get(m_useArticulations, SdfAssetBuilderUseArticulationsRegistryKey);
 
+        // Query the option to preserve child links of fixed joints when parsing a URDF using libsdformat
+        settingsRegistry->Get(m_urdfPreserveFixedJoints, SdfAssetBuilderURDFPreserveFixedJointRegistryKey);
+
         // Visit each supported file type extension and create an asset builder wildcard pattern for it.
         auto VisitFileTypeExtensions = [&settingsRegistry, this]
             (const AZ::SettingsRegistryInterface::VisitArgs& visitArgs)

+ 3 - 1
Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilderSettings.h

@@ -32,5 +32,7 @@ namespace ROS2
 
         AZStd::vector<AssetBuilderSDK::AssetBuilderPattern> m_builderPatterns;
         bool m_useArticulations = true;
+        // By default, fixed joint in URDF files that are processed by libsdformat are preserved
+        bool m_urdfPreserveFixedJoints = true;
     };
-} // ROS2
+} // namespace ROS2

+ 578 - 124
Gems/ROS2/Code/Tests/UrdfParserTest.cpp

@@ -7,9 +7,11 @@
  */
 
 #include <AzCore/UnitTest/TestTypes.h>
+#include <AzCore/std/ranges/ranges_algorithm.h>
 #include <AzCore/std/string/string.h>
 #include <AzTest/AzTest.h>
 #include <RobotImporter/URDF/UrdfParser.h>
+#include <RobotImporter/Utils/ErrorUtils.h>
 #include <RobotImporter/Utils/RobotImporterUtils.h>
 #include <RobotImporter/xacro/XacroUtils.h>
 
@@ -51,9 +53,9 @@ namespace UnitTest
                    "</robot>";
         }
 
-        AZStd::string GetUrdfWithTwoLinksAndJoint()
+        AZStd::string GetUrdfWithTwoLinksAndJoint(AZStd::string_view jointType = "fixed")
         {
-            return "<robot name=\"test_two_links_one_joint\">  "
+            return AZStd::string::format("<robot name=\"test_two_links_one_joint\">  "
                    "  <material name=\"some_material\">\n"
                    "    <color rgba=\"0 0 0 1\"/>\n"
                    "  </material>"
@@ -81,15 +83,96 @@ namespace UnitTest
                    "      <material name=\"some_material\"/>"
                    "    </visual>"
                    "  </link>"
-                   "  <joint name=\"joint12\" type=\"fixed\">"
+                   R"(  <joint name="joint12" type="%.*s">)"
                    "    <parent link=\"link1\"/>"
                    "    <child link=\"link2\"/>"
                    "    <origin rpy=\"0 0 0\" xyz=\"1.0 0.5 0.0\"/>"
                    "    <dynamics damping=\"10.0\" friction=\"5.0\"/>"
                    "    <limit lower=\"10.0\" upper=\"20.0\" effort=\"90.0\" velocity=\"10.0\"/>"
                    "  </joint>"
-                   "</robot>";
+                   "</robot>", AZ_STRING_ARG(jointType));
+        }
+
+        AZStd::string GetURDFWithTwoLinksAndBaseLinkNoInertia()
+        {
+            return R"(<?xml version="1.0" ?>
+                <robot name="always_ignored">
+                  <link name="base_link">
+                    <visual>
+                      <geometry>
+                        <box size="1.0 1.0 1.0"/>
+                      </geometry>
+                    </visual>
+                  </link>
+                  <link name="child_link">
+                    <inertial>
+                      <mass value="1.0"/>
+                      <inertia ixx="1.0" iyy="1.0" izz="1.0" ixy="0" ixz="0" iyz="0"/>
+                    </inertial>
+                    <visual>
+                      <geometry>
+                        <sphere radius="1.0"/>
+                      </geometry>
+                    </visual>
+                  </link>
+                  <joint name="joint12" type="fixed">
+                    <parent link="base_link"/>
+                    <child link="child_link"/>
+                  </joint>
+                </robot>)";
+        }
+
+        AZStd::string GetURDFWithFourLinksAndRootLinkNoInertia(AZStd::string_view rootLinkName)
+        {
+            return AZStd::string::format(R"(<?xml version="1.0" ?>
+                <robot name="FooRobot">
+                  <link name="%.*s"/>
+                  <link name="base_link"/>
+                  <link name="base_link_inertia">
+                    <inertial>
+                      <mass value="1.0"/>
+                      <inertia ixx="1.0" iyy="1.0" izz="1.0" ixy="0" ixz="0" iyz="0"/>
+                    </inertial>
+                    <visual>
+                      <geometry>
+                        <box size="1.0 1.0 1.0"/>
+                      </geometry>
+                    </visual>
+                  </link>
+                  <link name="child_link">
+                    <inertial>
+                      <mass value="2.0"/>
+                      <inertia ixx="1.0" iyy="1.0" izz="1.0" ixy="0" ixz="0" iyz="0"/>
+                    </inertial>
+                    <visual>
+                      <geometry>
+                        <sphere radius="1.0"/>
+                      </geometry>
+                    </visual>
+                  </link>
+                  <joint name="world_base_joint" type="fixed">
+                    <parent link="%.*s"/>
+                    <child link="base_link"/>
+                  </joint>
+                  <joint name="base_inertia_joint" type="fixed">
+                    <parent link="base_link"/>
+                    <child link="base_link_inertia"/>
+                  </joint>
+                  <joint name="base_inertia_child_joint" type="revolute">
+                    <parent link="base_link_inertia"/>
+                    <child link="child_link"/>
+                    <axis xyz="0 0 1" />
+                    <origin rpy="0 0 0" xyz="1.0 0.5 0.0"/>
+                    <dynamics damping="10.0" friction="5.0"/>
+                    <limit lower="10.0" upper="20.0" effort="90.0" velocity="10.0"/>
+                  </joint>
+                </robot>)", AZ_STRING_ARG(rootLinkName), AZ_STRING_ARG(rootLinkName));
         }
+
+        // A URDF <model> can only represent structure which is configurable though the <joint> tags
+        // Therefore link can only appear as a child of a single joint.
+        // It cannot be a child of multiple joints
+        // https://wiki.ros.org/urdf/XML/model
         AZStd::string GetURDFWithTranforms()
         {
             return "<?xml version=\"1.0\"?>\n"
@@ -171,12 +254,6 @@ namespace UnitTest
                    "        <axis xyz=\"0. 0. 1.\"/>\n"
                    "        <origin rpy=\"0.000000 0.000000 2.094395\" xyz=\"-1.200000286102295 2.078460931777954 0.\"/>\n"
                    "    </joint> \n"
-                   "    <joint name=\"joint2\" type=\"continuous\">\n"
-                   "        <parent link=\"link1\"/>\n"
-                   "        <child link=\"link3\"/>\n"
-                   "        <axis xyz=\"0. 0. 1.\"/>\n"
-                   "        <origin rpy=\"0.000000 0.000000 -2.094395160675049\" xyz=\"-2.4000000953674316 0.0 0.0\"/>\n"
-                   "    </joint>\n"
                    "</robot>";
         }
 
@@ -227,161 +304,474 @@ namespace UnitTest
 
     TEST_F(UrdfParserTest, ParseUrdfWithOneLink)
     {
-
         const auto xmlStr = GetUrdfWithOneLink();
-        const auto urdf = ROS2::UrdfParser::Parse(xmlStr);
-
-        EXPECT_EQ(urdf->getName(), "test_one_link");
-
-        std::vector<urdf::LinkSharedPtr> links;
-        urdf->getLinks(links);
-        EXPECT_EQ(links.size(), 1U);
-
-        const auto link1 = urdf->getLink("link1");
-
-        ASSERT_TRUE(link1);
-        EXPECT_EQ(link1->inertial->mass, 1.0);
-        EXPECT_EQ(link1->inertial->ixx, 1.0);
-        EXPECT_EQ(link1->inertial->ixy, 0.0);
-        EXPECT_EQ(link1->inertial->ixz, 0.0);
-        EXPECT_EQ(link1->inertial->iyy, 1.0);
-        EXPECT_EQ(link1->inertial->iyz, 0.0);
-        EXPECT_EQ(link1->inertial->izz, 1.0);
-
-        EXPECT_EQ(link1->visual->geometry->type, 1);
-        const auto visualBox = std::dynamic_pointer_cast<urdf::Box>(link1->visual->geometry);
-        EXPECT_EQ(visualBox->dim.x, 1.0);
-        EXPECT_EQ(visualBox->dim.y, 2.0);
-        EXPECT_EQ(visualBox->dim.z, 1.0);
-
-        EXPECT_EQ(link1->collision->geometry->type, 1);
-        const auto collisionBox = std::dynamic_pointer_cast<urdf::Box>(link1->visual->geometry);
-        EXPECT_EQ(collisionBox->dim.x, 1.0);
-        EXPECT_EQ(collisionBox->dim.y, 2.0);
-        EXPECT_EQ(collisionBox->dim.z, 1.0);
+        sdf::ParserConfig parserConfig;
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+
+        const sdf::Model* model = sdfRoot.Model();
+        EXPECT_EQ("test_one_link", model->Name());
+        ASSERT_NE(nullptr, model);
+
+        uint64_t linkCount = model->LinkCount();
+        EXPECT_EQ(1U, linkCount);
+
+        const sdf::Link* link1 = model->LinkByName("link1");
+
+        ASSERT_NE(nullptr, link1);
+        EXPECT_EQ(1.0, link1->Inertial().MassMatrix().Mass());
+        EXPECT_EQ(1.0, link1->Inertial().MassMatrix().Ixx());
+        EXPECT_EQ(0.0, link1->Inertial().MassMatrix().Ixy());
+        EXPECT_EQ(0.0, link1->Inertial().MassMatrix().Ixz());
+        EXPECT_EQ(1.0, link1->Inertial().MassMatrix().Iyy());
+        EXPECT_EQ(0.0, link1->Inertial().MassMatrix().Iyz());
+        EXPECT_EQ(1.0, link1->Inertial().MassMatrix().Izz());
+
+        ASSERT_EQ(1, link1->VisualCount());
+        const sdf::Visual* visual = link1->VisualByIndex(0);
+        ASSERT_NE(nullptr, visual);
+        const sdf::Geometry* geometryVis = visual->Geom();
+        ASSERT_NE(nullptr, geometryVis);
+        EXPECT_EQ(sdf::GeometryType::BOX, geometryVis->Type());
+        const sdf::Box* visualBox = geometryVis->BoxShape();
+        ASSERT_NE(nullptr, visualBox);
+        EXPECT_EQ(1.0, visualBox->Size().X());
+        EXPECT_EQ(2.0, visualBox->Size().Y());
+        EXPECT_EQ(1.0, visualBox->Size().Z());
+
+        ASSERT_EQ(1, link1->CollisionCount());
+        const sdf::Collision* collision = link1->CollisionByIndex(0);
+        ASSERT_NE(nullptr, collision);
+        const sdf::Geometry* geometryCol = visual->Geom();
+        ASSERT_NE(nullptr, geometryCol);
+        EXPECT_EQ(sdf::GeometryType::BOX, geometryCol->Type());
+        const sdf::Box* collisionBox = geometryCol->BoxShape();
+        EXPECT_EQ(1.0, collisionBox->Size().X());
+        EXPECT_EQ(2.0, collisionBox->Size().Y());
+        EXPECT_EQ(1.0, collisionBox->Size().Z());
     }
 
-    TEST_F(UrdfParserTest, ParseUrdfWithTwoLinksAndJoint)
+    TEST_F(UrdfParserTest, ParseUrdfWithTwoLinksAndFixedJoint_WithPreserveFixedJoint_False)
     {
+        const auto xmlStr = GetUrdfWithTwoLinksAndJoint();
+        sdf::ParserConfig parserConfig;
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+
+        const sdf::Model* model = sdfRoot.Model();
+        EXPECT_EQ("test_two_links_one_joint", model->Name());
+        ASSERT_NE(nullptr, model);
+
+        // The SDFormat URDF parser combines links in joints that are fixed
+        // together
+        // https://github.com/gazebosim/sdformat/pull/1149
+        // So for a URDF with 2 links that are combined with a single fixed joint
+        // The resulted SDF has one 1 link and no joints
+        //
+        // The SDFormat <gazebo> extension tag can be used to preserve fixed joint by adding
+        // a <gazebo><preserveFixedJoint>true</preserveFixedJoint></gazebo> XML element
+        // http://sdformat.org/tutorials?tut=sdformat_urdf_extensions&cat=specification&#gazebo-elements-for-joints
+        ASSERT_EQ(1, model->LinkCount());
+
+        EXPECT_TRUE(model->FrameNameExists("link2"));
+        EXPECT_TRUE(model->FrameNameExists("joint12"));
+
+        const sdf::Link* link1 = model->LinkByName("link1");
+        ASSERT_NE(nullptr, link1);
+    }
 
+    TEST_F(UrdfParserTest, ParseUrdfWithTwoLinksAndFixedJoint_WithPreserveFixedJoint_True)
+    {
         const auto xmlStr = GetUrdfWithTwoLinksAndJoint();
-        const auto urdf = ROS2::UrdfParser::Parse(xmlStr);
+        sdf::ParserConfig parserConfig;
+        parserConfig.URDFSetPreserveFixedJoint(true);
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+
+        const sdf::Model* model = sdfRoot.Model();
+        EXPECT_EQ("test_two_links_one_joint", model->Name());
+        ASSERT_NE(nullptr, model);
+
+        // As the <preserveFixedJoint> option has been set
+        // in this case the child link and the joint in the SDF should be remain
+        ASSERT_EQ(2, model->LinkCount());
+        ASSERT_EQ(1, model->JointCount());
+
+        // No Frames are made for perserved joints
+        EXPECT_FALSE(model->FrameNameExists("link2"));
+        EXPECT_FALSE(model->FrameNameExists("joint12"));
+
+        const sdf::Link* link1 = model->LinkByName("link1");
+        ASSERT_NE(nullptr, link1);
 
-        EXPECT_EQ(urdf->getName(), "test_two_links_one_joint");
+        const sdf::Link* link2 = model->LinkByName("link2");
+        ASSERT_NE(nullptr, link2);
 
-        std::vector<urdf::LinkSharedPtr> links;
-        urdf->getLinks(links);
-        EXPECT_EQ(links.size(), 2U);
+        const sdf::Joint* joint12 = model->JointByName("joint12");
+        ASSERT_NE(nullptr, joint12);
 
-        const auto link1 = urdf->getLink("link1");
-        ASSERT_TRUE(link1);
+        EXPECT_EQ("link1", joint12->ParentName());
+        EXPECT_EQ("link2", joint12->ChildName());
 
-        const auto link2 = urdf->getLink("link2");
-        ASSERT_TRUE(link2);
+        gz::math::Pose3d jointPose = joint12->RawPose();
+        EXPECT_DOUBLE_EQ(1.0, jointPose.X());
+        EXPECT_DOUBLE_EQ(0.5, jointPose.Y());
+        EXPECT_DOUBLE_EQ(0.0, jointPose.Z());
 
-        const auto joint12 = urdf->getJoint("joint12");
-        ASSERT_TRUE(joint12);
+        double roll, pitch, yaw;
+        const gz::math::Quaternion rot = jointPose.Rot();
+        roll = rot.Roll();
+        pitch = rot.Pitch();
+        yaw = rot.Yaw();
+        EXPECT_DOUBLE_EQ(roll, 0.0);
+        EXPECT_DOUBLE_EQ(pitch, 0.0);
+        EXPECT_DOUBLE_EQ(yaw, 0.0);
+
+        EXPECT_EQ(sdf::JointType::FIXED, joint12->Type());
+    }
+
+    TEST_F(UrdfParserTest, ParseUrdfWithTwoLinksAndNonFixedJoint)
+    {
+        const auto xmlStr = GetUrdfWithTwoLinksAndJoint("continuous");
+        sdf::ParserConfig parserConfig;
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+
+        const sdf::Model* model = sdfRoot.Model();
+        EXPECT_EQ("test_two_links_one_joint", model->Name());
+        ASSERT_NE(nullptr, model);
+
+        ASSERT_EQ(2, model->LinkCount());
 
-        EXPECT_EQ(joint12->parent_link_name, "link1");
-        EXPECT_EQ(joint12->child_link_name, "link2");
+        const sdf::Link* link1 = model->LinkByName("link1");
+        ASSERT_NE(nullptr, link1);
 
-        EXPECT_EQ(joint12->parent_to_joint_origin_transform.position.x, 1.0);
-        EXPECT_EQ(joint12->parent_to_joint_origin_transform.position.y, 0.5);
-        EXPECT_EQ(joint12->parent_to_joint_origin_transform.position.z, 0.0);
+        const sdf::Link* link2 = model->LinkByName("link2");
+        ASSERT_NE(nullptr, link2);
+
+        const sdf::Joint* joint12 = model->JointByName("joint12");
+        ASSERT_NE(nullptr, joint12);
+
+        EXPECT_EQ("link1", joint12->ParentName());
+        EXPECT_EQ("link2", joint12->ChildName());
+
+        gz::math::Pose3d jointPose = joint12->RawPose();
+        EXPECT_DOUBLE_EQ(1.0, jointPose.X());
+        EXPECT_DOUBLE_EQ(0.5, jointPose.Y());
+        EXPECT_DOUBLE_EQ(0.0, jointPose.Z());
 
         double roll, pitch, yaw;
-        joint12->parent_to_joint_origin_transform.rotation.getRPY(roll, pitch, yaw);
+        const gz::math::Quaternion rot = jointPose.Rot();
+        roll = rot.Roll();
+        pitch = rot.Pitch();
+        yaw = rot.Yaw();
         EXPECT_DOUBLE_EQ(roll, 0.0);
         EXPECT_DOUBLE_EQ(pitch, 0.0);
         EXPECT_DOUBLE_EQ(yaw, 0.0);
 
-        EXPECT_EQ(joint12->dynamics->damping, 10.0);
-        EXPECT_EQ(joint12->dynamics->friction, 5.0);
+        const sdf::JointAxis* joint12Axis = joint12->Axis();
+        ASSERT_NE(nullptr, joint12Axis);
+
+        EXPECT_DOUBLE_EQ(10.0, joint12Axis->Damping());
+        EXPECT_DOUBLE_EQ(5.0, joint12Axis->Friction());
+
+        EXPECT_DOUBLE_EQ(-AZStd::numeric_limits<double>::infinity(), joint12Axis->Lower());
+        EXPECT_DOUBLE_EQ(AZStd::numeric_limits<double>::infinity(), joint12Axis->Upper());
+        EXPECT_DOUBLE_EQ(90.0, joint12Axis->Effort());
+        EXPECT_DOUBLE_EQ(10.0, joint12Axis->MaxVelocity());
+    }
 
-        EXPECT_EQ(joint12->limits->lower, 10.0);
-        EXPECT_EQ(joint12->limits->upper, 20.0);
-        EXPECT_EQ(joint12->limits->effort, 90.0);
-        EXPECT_EQ(joint12->limits->velocity, 10.0);
+        TEST_F(UrdfParserTest, ParseURDF_WithTwoLinks_AndBaseLinkWithNoInertia_WithUrdfFixedJointPreservationOn_Fails)
+    {
+        const auto xmlStr = GetURDFWithTwoLinksAndBaseLinkNoInertia();
+        sdf::ParserConfig parserConfig;
+        parserConfig.URDFSetPreserveFixedJoint(true);
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
+        ASSERT_FALSE(sdfRootOutcome);
+        AZStd::string errorString = ROS2::Utils::JoinSdfErrorsToString(sdfRootOutcome.error());
+        printf("URDF with single link and no inertia: %s\n", errorString.c_str());
+    }
+
+    TEST_F(UrdfParserTest, ParseURDF_WithTwoLinks_AndBaseLinkWithNoInertia_WithUrdfFixedJointPreservationOff_Succeeds)
+    {
+        const auto xmlStr = GetURDFWithTwoLinksAndBaseLinkNoInertia();
+        sdf::ParserConfig parserConfig;
+        parserConfig.URDFSetPreserveFixedJoint(false);
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+
+        const sdf::Model* model = sdfRoot.Model();
+        EXPECT_EQ("always_ignored", model->Name());
+        ASSERT_NE(nullptr, model);
+
+        ASSERT_EQ(1, model->LinkCount());
+
+        EXPECT_TRUE(model->FrameNameExists("child_link"));
+        EXPECT_TRUE(model->FrameNameExists("joint12"));
+
+        const sdf::Link* link1 = model->LinkByName("base_link");
+        ASSERT_NE(nullptr, link1);
+    }
+
+    MATCHER(UnorderedMapKeyMatcher, "") {
+        *result_listener << "Expected Key" << AZStd::get<1>(arg)
+            << "Actual Key" << AZStd::get<0>(arg).first.c_str();
+        return AZStd::get<0>(arg).first == AZStd::get<1>(arg);
+    }
+
+    TEST_F(UrdfParserTest, ParseUrdf_WithRootLink_WithName_world_DoesNotContain_world_Link)
+    {
+        // The libsdformat URDF parser skips converting the root link if its
+        // name is "world"
+        // https://github.com/gazebosim/sdformat/blob/a1027c3ed96f2f663760df10f13b06f47f922c55/src/parser_urdf.cc#L3385-L3399
+        // Therefore it will not be part of joint reduction
+        constexpr const char* RootLinkName = "world";
+        const auto xmlStr = GetURDFWithFourLinksAndRootLinkNoInertia(RootLinkName);
+        sdf::ParserConfig parserConfig;
+        // Make sure joint reduction occurs
+        parserConfig.URDFSetPreserveFixedJoint(false);
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+
+        const sdf::Model* model = sdfRoot.Model();
+        EXPECT_EQ("FooRobot", model->Name());
+        ASSERT_NE(nullptr, model);
+
+        // Due to joint reduction there should be 2 links and 2 frames
+        ASSERT_EQ(2, model->LinkCount());
+        ASSERT_EQ(2, model->FrameCount());
+
+        // The base_inertia_joint was elimimated when the base_link parent
+        // merged with the base_link_inertia child
+        // NOTE: The "world_base_joint" is not merged because the root link
+        // is has a name of "world"
+        EXPECT_TRUE(model->FrameNameExists("base_inertia_joint"));
+        EXPECT_TRUE(model->FrameNameExists("base_link_inertia"));
+
+        const sdf::Link* link1 = model->LinkByName("base_link");
+        ASSERT_NE(nullptr, link1);
+
+        const sdf::Link* link2 = model->LinkByName("child_link");
+        ASSERT_NE(nullptr, link2);
+
+        // Check the ROS2 visitor logic to make sure the joint with "world" parent link isn't visited
+        auto joints = ROS2::Utils::GetAllJoints(*model);
+        EXPECT_EQ(1, joints.size());
+        EXPECT_THAT(joints, ::testing::UnorderedPointwise(UnorderedMapKeyMatcher(), { "base_inertia_child_joint" }));
+
+        // The libsdformat sdf::Model::JointCount function however returns 2
+        // as the "world_base_joint" is skipped over by joint reduction
+        // because the it's parent link is "world"
+        EXPECT_EQ(2, model->JointCount());
+    }
+
+    TEST_F(UrdfParserTest, ParseUrdf_WithRootLink_WithoutName_world_DoesContainThatLink)
+    {
+        // Because the name of the root link in the URDF is not "world"
+        // It should get reduced as part of joint reduction
+        constexpr const char* RootLinkName = "not_world";
+        const auto xmlStr = GetURDFWithFourLinksAndRootLinkNoInertia(RootLinkName);
+        sdf::ParserConfig parserConfig;
+        // Make sure joint reduction occurs
+        parserConfig.URDFSetPreserveFixedJoint(false);
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+
+        const sdf::Model* model = sdfRoot.Model();
+        EXPECT_EQ("FooRobot", model->Name());
+        ASSERT_NE(nullptr, model);
+
+        // Due to joint reduction there should be 2 links and 43 frames
+        // This is different from the previous test, as the root link
+        // can now partipate in joint reduction because it has a name
+        // that isn't "world"
+        ASSERT_EQ(2, model->LinkCount());
+        ASSERT_EQ(4, model->FrameCount());
+
+        // The base_inertia_joint and world_base_joint should be merged
+        // together into the top level link of "not_world"
+        EXPECT_TRUE(model->FrameNameExists("base_inertia_joint"));
+        EXPECT_TRUE(model->FrameNameExists("base_link_inertia"));
+        EXPECT_TRUE(model->FrameNameExists("world_base_joint"));
+        EXPECT_TRUE(model->FrameNameExists("base_link"));
+
+        const sdf::Link* link1 = model->LinkByName(RootLinkName);
+        ASSERT_NE(nullptr, link1);
+
+        const sdf::Link* link2 = model->LinkByName("child_link");
+        ASSERT_NE(nullptr, link2);
+
+        // The ROS2 Visitor logic should visit all reduced joints which
+        // there should only be a single one of the revolute joint
+        auto joints = ROS2::Utils::GetAllJoints(*model);
+        EXPECT_EQ(1, joints.size());
+        EXPECT_THAT(joints, ::testing::UnorderedPointwise(UnorderedMapKeyMatcher(), { "base_inertia_child_joint" }));
+
+        // The libsdformat sdf::Model::JointCount function should match
+        // the ROS2 Visitor logic as the root link of "not_world" is part of joint reduction
+        EXPECT_EQ(1, model->JointCount());
     }
 
     TEST_F(UrdfParserTest, WheelHeuristicNameValid)
     {
-        const AZStd::string wheel_name("wheel_left_link");
-        const auto xmlStr = GetURDFWithWheel(wheel_name, "continuous");
-        const auto urdf = ROS2::UrdfParser::Parse(xmlStr);
-        auto wheel_candidate = urdf->getLink(wheel_name.c_str());
-        ASSERT_TRUE(wheel_candidate);
-        EXPECT_EQ(ROS2::Utils::IsWheelURDFHeuristics(wheel_candidate), true);
+        sdf::ParserConfig parserConfig;
+        const AZStd::string_view wheelName("wheel_left_link");
+        const auto xmlStr = GetURDFWithWheel(wheelName, "continuous");
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Model* model = sdfRoot.Model();
+        ASSERT_NE(nullptr, model);
+        auto wheelCandidate = model->LinkByName(std::string(wheelName.data(), wheelName.size()));
+        ASSERT_NE(nullptr, wheelCandidate);
+        EXPECT_TRUE(ROS2::Utils::IsWheelURDFHeuristics(*model, wheelCandidate));
+
+        const AZStd::string_view wheelNameSuffix("left_link_wheel");
+        const auto xmlStr2 = GetURDFWithWheel(wheelNameSuffix, "continuous");
+        const auto sdfRootOutcome2 = ROS2::UrdfParser::Parse(xmlStr2, parserConfig);
+        ASSERT_TRUE(sdfRootOutcome2);
+        const sdf::Root& sdfRoot2 = sdfRootOutcome2.value();
+        const sdf::Model* model2 = sdfRoot2.Model();
+        ASSERT_NE(nullptr, model2);
+        auto wheelCandidate2 = model2->LinkByName(std::string(wheelNameSuffix.data(), wheelNameSuffix.size()));
+        ASSERT_NE(nullptr, wheelCandidate2);
+        EXPECT_TRUE(ROS2::Utils::IsWheelURDFHeuristics(*model2, wheelCandidate2));
     }
 
     TEST_F(UrdfParserTest, WheelHeuristicNameNotValid1)
     {
-        const AZStd::string wheel_name("wheel_left_joint");
-        const auto xmlStr = GetURDFWithWheel(wheel_name, "continuous");
-        const auto urdf = ROS2::UrdfParser::Parse(xmlStr);
-        auto wheel_candidate = urdf->getLink(wheel_name.c_str());
-        ASSERT_TRUE(wheel_candidate);
-        EXPECT_EQ(ROS2::Utils::IsWheelURDFHeuristics(wheel_candidate), false);
+        const AZStd::string wheelName("whe3l_left_link");
+        const auto xmlStr = GetURDFWithWheel(wheelName, "continuous");
+        sdf::ParserConfig parserConfig;
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Model* model = sdfRoot.Model();
+        ASSERT_NE(nullptr, model);
+        auto wheelCandidate = model->LinkByName(std::string(wheelName.data(), wheelName.size()));
+        ASSERT_NE(nullptr, wheelCandidate);
+        EXPECT_FALSE(ROS2::Utils::IsWheelURDFHeuristics(*model, wheelCandidate));
     }
 
     TEST_F(UrdfParserTest, WheelHeuristicJointNotValid)
     {
-        const AZStd::string wheel_name("wheel_left_link");
-        const auto xmlStr = GetURDFWithWheel(wheel_name, "fixed");
-        const auto urdf = ROS2::UrdfParser::Parse(xmlStr);
-        auto wheel_candidate = urdf->getLink(wheel_name.c_str());
-        ASSERT_TRUE(wheel_candidate);
-        EXPECT_EQ(ROS2::Utils::IsWheelURDFHeuristics(wheel_candidate), false);
+        const AZStd::string wheelName("wheel_left_link");
+        const auto xmlStr = GetURDFWithWheel(wheelName, "fixed");
+        sdf::ParserConfig parserConfig;
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Model* model = sdfRoot.Model();
+        ASSERT_NE(nullptr, model);
+        // SDFormat converts combines the links of a joint with a fixed type
+        // into a single link
+        // It does however create a Frame with the name of the child link and joint that was combined
+        EXPECT_EQ(1, model->LinkCount());
+
+        auto wheelCandidate = model->LinkByName("base_link");
+        ASSERT_NE(nullptr, wheelCandidate);
+
+        EXPECT_TRUE(model->FrameNameExists(std::string{ wheelName.c_str(), wheelName.size() }));
+        EXPECT_TRUE(model->FrameNameExists("joint0"));
+        EXPECT_FALSE(ROS2::Utils::IsWheelURDFHeuristics(*model, wheelCandidate));
     }
 
     TEST_F(UrdfParserTest, WheelHeuristicJointVisualNotValid)
     {
-        const AZStd::string wheel_name("wheel_left_link");
-        const auto xmlStr = GetURDFWithWheel(wheel_name, "continuous", false, true);
-        const auto urdf = ROS2::UrdfParser::Parse(xmlStr);
-        auto wheel_candidate = urdf->getLink(wheel_name.c_str());
-        ASSERT_TRUE(wheel_candidate);
-        EXPECT_EQ(ROS2::Utils::IsWheelURDFHeuristics(wheel_candidate), false);
+        const AZStd::string wheelName("wheel_left_link");
+        const auto xmlStr = GetURDFWithWheel(wheelName, "continuous", false, true);
+        sdf::ParserConfig parserConfig;
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Model* model = sdfRoot.Model();
+        ASSERT_NE(nullptr, model);
+        auto wheelCandidate = model->LinkByName(std::string(wheelName.c_str(), wheelName.size()));
+        ASSERT_NE(nullptr, wheelCandidate);
+        EXPECT_FALSE(ROS2::Utils::IsWheelURDFHeuristics(*model, wheelCandidate));
     }
 
     TEST_F(UrdfParserTest, WheelHeuristicJointColliderNotValid)
     {
-        const AZStd::string wheel_name("wheel_left_link");
-        const auto xmlStr = GetURDFWithWheel(wheel_name, "continuous", true, false);
-        const auto urdf = ROS2::UrdfParser::Parse(xmlStr);
-        auto wheel_candidate = urdf->getLink(wheel_name.c_str());
-        ASSERT_TRUE(wheel_candidate);
-        EXPECT_EQ(ROS2::Utils::IsWheelURDFHeuristics(wheel_candidate), false);
+        const AZStd::string wheelName("wheel_left_link");
+        const auto xmlStr = GetURDFWithWheel(wheelName, "continuous", true, false);
+        sdf::ParserConfig parserConfig;
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Model* model = sdfRoot.Model();
+        ASSERT_NE(nullptr, model);
+        auto wheelCandidate = model->LinkByName(std::string(wheelName.c_str(), wheelName.size()));
+        ASSERT_NE(nullptr, wheelCandidate);
+        EXPECT_FALSE(ROS2::Utils::IsWheelURDFHeuristics(*model, wheelCandidate));
     }
 
     TEST_F(UrdfParserTest, TestLinkListing)
     {
         const auto xmlStr = GetURDFWithTranforms();
-        const auto urdf = ROS2::UrdfParser::Parse(xmlStr);
-        auto links = ROS2::Utils::GetAllLinks(urdf->getRoot()->child_links);
+        sdf::ParserConfig parserConfig;
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Model* model = sdfRoot.Model();
+        ASSERT_NE(nullptr, model);
+        auto links = ROS2::Utils::GetAllLinks(*model);
+        // As the "joint_bs" is a fixed joint, it and it's child link are combined
+        // Therefore the "link1" child link and "joint_bs" fixed joint are combined
+        // into the base_link of the SDF
+        // However there are Frames for the combined links and joints
         EXPECT_EQ(links.size(), 3);
-        ASSERT_TRUE(links.contains("link1"));
+        ASSERT_TRUE(links.contains("base_link"));
         ASSERT_TRUE(links.contains("link2"));
         ASSERT_TRUE(links.contains("link3"));
-        EXPECT_EQ(links.at("link1")->name, "link1");
-        EXPECT_EQ(links.at("link2")->name, "link2");
-        EXPECT_EQ(links.at("link3")->name, "link3");
+        EXPECT_EQ("base_link", links.at("base_link")->Name());
+        EXPECT_EQ("link2", links.at("link2")->Name());
+        EXPECT_EQ("link3", links.at("link3")->Name());
+
+        // Check that the frame names exist on the model
+        EXPECT_TRUE(model->FrameNameExists("joint_bs"));
+        EXPECT_TRUE(model->FrameNameExists("link1"));
     }
 
     TEST_F(UrdfParserTest, TestJointLink)
     {
         const auto xmlStr = GetURDFWithTranforms();
-        const auto urdf = ROS2::UrdfParser::Parse(xmlStr);
-        auto joints = ROS2::Utils::GetAllJoints(urdf->getRoot()->child_links);
-        EXPECT_EQ(joints.size(), 3);
+        sdf::ParserConfig parserConfig;
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Model* model = sdfRoot.Model();
+        ASSERT_NE(nullptr, model);
+        auto joints = ROS2::Utils::GetAllJoints(*model);
+        EXPECT_EQ(2, joints.size());
+        ASSERT_TRUE(joints.contains("joint0"));
+        ASSERT_TRUE(joints.contains("joint1"));
     }
 
     TEST_F(UrdfParserTest, TestTransforms)
     {
         const auto xmlStr = GetURDFWithTranforms();
-        const auto urdf = ROS2::UrdfParser::Parse(xmlStr);
-        const auto links = ROS2::Utils::GetAllLinks(urdf->getRoot()->child_links);
-        const auto link1_ptr = links.at("link1");
+        sdf::ParserConfig parserConfig;
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Model* model = sdfRoot.Model();
+        ASSERT_NE(nullptr, model);
+        const auto links = ROS2::Utils::GetAllLinks(*model);
+        // The "link1" is combined with the base_link through
+        // joint reduction in the URDF->SDF parser logic
+        // https://github.com/gazebosim/sdformat/issues/1110
+        ASSERT_TRUE(links.contains("base_link"));
+        ASSERT_TRUE(links.contains("link2"));
+        ASSERT_TRUE(links.contains("link3"));
+        const auto base_link_ptr = links.at("base_link");
         const auto link2_ptr = links.at("link2");
         const auto link3_ptr = links.at("link3");
 
@@ -390,7 +780,7 @@ namespace UnitTest
         const AZ::Vector3 expected_translation_link2{ -1.2000000476837158, 2.0784599781036377, 0.0 };
         const AZ::Vector3 expected_translation_link3{ -2.4000000953674316, 0.0, 0.0 };
 
-        const AZ::Transform transform_from_urdf_link1 = ROS2::Utils::GetWorldTransformURDF(link1_ptr);
+        const AZ::Transform transform_from_urdf_link1 = ROS2::Utils::GetWorldTransformURDF(base_link_ptr);
         EXPECT_NEAR(expected_translation_link1.GetX(), transform_from_urdf_link1.GetTranslation().GetX(), 1e-5);
         EXPECT_NEAR(expected_translation_link1.GetY(), transform_from_urdf_link1.GetTranslation().GetY(), 1e-5);
         EXPECT_NEAR(expected_translation_link1.GetZ(), transform_from_urdf_link1.GetTranslation().GetZ(), 1e-5);
@@ -406,14 +796,64 @@ namespace UnitTest
         EXPECT_NEAR(expected_translation_link3.GetZ(), transform_from_urdf_link3.GetTranslation().GetZ(), 1e-5);
     }
 
+    TEST_F(UrdfParserTest, TestQueryJointsForParentLink_Succeeds)
+    {
+        const auto xmlStr = GetURDFWithTranforms();
+        sdf::ParserConfig parserConfig;
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Model* model = sdfRoot.Model();
+        ASSERT_NE(nullptr, model);
+        auto joints = ROS2::Utils::GetJointsForParentLink(*model, "base_link");
+        EXPECT_EQ(1, joints.size());
+
+        auto jointToNameProjection = [](const sdf::Joint* joint)
+        {
+            return AZStd::string_view(joint->Name().c_str(), joint->Name().size());
+        };
+        ASSERT_TRUE(AZStd::ranges::contains(joints, "joint0", jointToNameProjection));
+
+        // Now check the middle link of "link2"
+        joints = ROS2::Utils::GetJointsForParentLink(*model, "link2");
+        EXPECT_EQ(1, joints.size());
+
+        ASSERT_TRUE(AZStd::ranges::contains(joints, "joint1", jointToNameProjection));
+    }
+
+    TEST_F(UrdfParserTest, TestQueryJointsForChildLink_Succeeds)
+    {
+        const auto xmlStr = GetURDFWithTranforms();
+        sdf::ParserConfig parserConfig;
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Model* model = sdfRoot.Model();
+        ASSERT_NE(nullptr, model);
+        auto joints = ROS2::Utils::GetJointsForChildLink(*model, "link2");
+        EXPECT_EQ(1, joints.size());
+
+        auto jointToNameProjection = [](const sdf::Joint* joint)
+        {
+            return AZStd::string_view(joint->Name().c_str(), joint->Name().size());
+        };
+        ASSERT_TRUE(AZStd::ranges::contains(joints, "joint0", jointToNameProjection));
+
+        // Now check the final link of "link3"
+        joints = ROS2::Utils::GetJointsForChildLink(*model, "link3");
+        EXPECT_EQ(1, joints.size());
+
+        ASSERT_TRUE(AZStd::ranges::contains(joints, "joint1", jointToNameProjection));
+    }
+
     TEST_F(UrdfParserTest, TestPathResolvementGlobal)
     {
-        AZStd::string dae = "file:///home/foo/ros_ws/install/foo_robot/meshes/bar.dae";
-        AZStd::string urdf = "/home/foo/ros_ws/install/foo_robot/foo_robot.urdf";
+        constexpr AZ::IO::PathView dae = "file:///home/foo/ros_ws/install/foo_robot/meshes/bar.dae";
+        constexpr AZ::IO::PathView urdf = "/home/foo/ros_ws/install/foo_robot/foo_robot.urdf";
         auto result = ROS2::Utils::ResolveURDFPath(
             dae,
             urdf, "",
-            [](const AZStd::string& p) -> bool
+            [](const AZ::IO::PathView&) -> bool
             {
                 return false;
             });
@@ -422,12 +862,12 @@ namespace UnitTest
 
     TEST_F(UrdfParserTest, TestPathResolvementRelative)
     {
-        AZStd::string dae = "meshes/bar.dae";
-        AZStd::string urdf = "/home/foo/ros_ws/install/foo_robot/foo_robot.urdf";
+        constexpr AZ::IO::PathView dae = "meshes/bar.dae";
+        constexpr AZ::IO::PathView urdf = "/home/foo/ros_ws/install/foo_robot/foo_robot.urdf";
         auto result = ROS2::Utils::ResolveURDFPath(
             dae,
             urdf, "",
-            [](const AZStd::string& p) -> bool
+            [](const AZ::IO::PathView&) -> bool
             {
                 return false;
             });
@@ -436,11 +876,11 @@ namespace UnitTest
 
     TEST_F(UrdfParserTest, TestPathResolvementRelativePackage)
     {
-        AZStd::string dae = "package://meshes/bar.dae";
-        AZStd::string urdf = "/home/foo/ros_ws/install/foo_robot/description/foo_robot.urdf";
-        AZStd::string xml = "/home/foo/ros_ws/install/foo_robot/package.xml";
-        AZStd::string resolvedDae = "/home/foo/ros_ws/install/foo_robot/meshes/bar.dae";
-        auto mockFileSystem = [&](const AZStd::string& p) -> bool
+        constexpr AZ::IO::PathView dae = "package://meshes/bar.dae";
+        constexpr AZ::IO::PathView urdf = "/home/foo/ros_ws/install/foo_robot/description/foo_robot.urdf";
+        constexpr AZ::IO::PathView xml = "/home/foo/ros_ws/install/foo_robot/package.xml";
+        constexpr AZStd::string_view resolvedDae = "/home/foo/ros_ws/install/foo_robot/meshes/bar.dae";
+        auto mockFileSystem = [&](const AZ::IO::PathView& p) -> bool
         {
             return (p == xml || p == resolvedDae);
         };
@@ -450,11 +890,25 @@ namespace UnitTest
 
     TEST_F(UrdfParserTest, TestPathResolvementExplicitPackageName)
     {
-        AZStd::string dae = "package://foo_robot/meshes/bar.dae";
-        AZStd::string urdf = "/home/foo/ros_ws/install/foo_robot/share/foo_robot/description/foo_robot.urdf";
-        AZStd::string xml = "/home/foo/ros_ws/install/foo_robot/share/foo_robot/package.xml";
-        AZStd::string resolvedDae = "/home/foo/ros_ws/install/foo_robot/share/foo_robot/meshes/bar.dae";
-        auto mockFileSystem = [&](const AZStd::string& p) -> bool
+        constexpr AZ::IO::PathView dae = "package://foo_robot/meshes/bar.dae";
+        constexpr AZ::IO::PathView urdf = "/home/foo/ros_ws/install/foo_robot/share/foo_robot/description/foo_robot.urdf";
+        constexpr AZ::IO::PathView xml = "/home/foo/ros_ws/install/foo_robot/share/foo_robot/package.xml";
+        constexpr AZStd::string_view resolvedDae = "/home/foo/ros_ws/install/foo_robot/share/foo_robot/meshes/bar.dae";
+        auto mockFileSystem = [&](const AZ::IO::PathView& p) -> bool
+        {
+            return (p == xml || p == resolvedDae);
+        };
+        auto result = ROS2::Utils::ResolveURDFPath(dae, urdf, "/home/foo/ros_ws/install/foo_robot", mockFileSystem);
+        EXPECT_EQ(result, resolvedDae);
+    }
+
+    TEST_F(UrdfParserTest, ResolvePath_UsingModelUriScheme_Succeeds)
+    {
+        constexpr AZ::IO::PathView dae = "model://foo_robot/meshes/bar.dae";
+        constexpr AZ::IO::PathView urdf = "/home/foo/ros_ws/install/foo_robot/share/foo_robot/description/foo_robot.urdf";
+        constexpr AZ::IO::PathView xml = "/home/foo/ros_ws/install/foo_robot/share/foo_robot/package.xml";
+        constexpr AZStd::string_view resolvedDae = "/home/foo/ros_ws/install/foo_robot/share/foo_robot/meshes/bar.dae";
+        auto mockFileSystem = [&](const AZ::IO::PathView& p) -> bool
         {
             return (p == xml || p == resolvedDae);
         };

+ 2 - 0
Gems/ROS2/Code/ros2_editor_files.cmake

@@ -49,6 +49,8 @@ set(FILES
     Source/RobotImporter/xacro/XacroUtils.cpp
     Source/RobotImporter/xacro/XacroUtils.h
     Source/RobotImporter/Utils/DefaultSolverConfiguration.h
+    Source/RobotImporter/Utils/ErrorUtils.cpp
+    Source/RobotImporter/Utils/ErrorUtils.h
     Source/RobotImporter/Utils/FilePath.cpp
     Source/RobotImporter/Utils/FilePath.h
     Source/RobotImporter/Utils/RobotImporterUtils.cpp

+ 2 - 1
Gems/ROS2/Registry/sdfassetbuilder_settings.setreg

@@ -12,7 +12,8 @@
                     "world",
                     "xacro"
                 ],
-                "UseArticulations": true
+                "UseArticulations": true,
+                "URDFPreserveFixedJoint": true
             }
         }
     }