Addressed feedback for URDF Import using SDFormat PR

Added a "/O3DE/ROS2/SdfAssetBuilder/URDFPreserveFixedJoint" setting which is used to preserve fixed joints when parsing URDFs

In order to use the URDFPreserveFixedJoint setting, the UrdfParser `Parse*` functions have been updated to accept an sdf::ParserConfig which can be used to set the preserve fixed joint setting.

Refactored the RobotImporter.cpp ResolveURDFPath function to support "model://" URI prefix when importing through the Widget.

Fixed the GetAllLinks function population of the SDF LinkMap to not use a dangling reference to the Link Name.

Fixed building of ROS2 Gem when Unity builds are turned off.
The SdfAssetBuilderName was being used in the SdfAssetBuilderSettings.cpp but it was only declared in the SdfAssetBuilder.cpp file

Moved the function that converts a sdf::Error vector into an AZStd::string to a Common ErrorUtils.cpp/h file.

Signed-off-by: lumberyard-employee-dm <[email protected]>

Gems/ROS2/Code/Source/RobotImporter/Pages/FileSelectionPage.cpp

@@ -19,7 +19,7 @@ namespace ROS2
     {
         m_fileDialog = new QFileDialog(this);
         m_fileDialog->setDirectory(QString::fromUtf8(AZ::Utils::GetProjectPath().data()));
-        m_fileDialog->setNameFilter("URDF, XACRO, SDF (*.urdf *.xacro *.sdf)");
+        m_fileDialog->setNameFilter("URDF, XACRO (*.urdf *.xacro)");
         m_button = new QPushButton("...", this);
         m_textEdit = new QLineEdit("", this);
         m_copyFiles = new QCheckBox(tr("Import meshes during URDF load"), this);

Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp

@@ -7,9 +7,11 @@
  */
 
 #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>
@@ -17,9 +19,8 @@
 #include <AzCore/StringFunc/StringFunc.h>
 #include <AzToolsFramework/API/ViewPaneOptions.h>
 
-#include <RobotImporter/URDF/UrdfParser.h>
 
-#include <sdf/World.hh>
+#include <sdf/sdf.hh>
 
 #if !defined(Q_MOC_RUN)
 #include <QWindow>
@@ -89,33 +90,29 @@ 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;
         }
 
-        auto parsedUrdfOutcome = UrdfParser::ParseFromFile(filePath);
+        // 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();
-            AZStd::string aggregateErrorMessages;
-            for (const sdf::Error& sdfError : parsedUrdfOutcome.error())
-            {
-                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());
-                }
-                aggregateErrorMessages += errorMessage;
-                aggregateErrorMessages += '\n';
-            }
-            AZ_Warning("ROS2EditorSystemComponent", false, "URDF parsing failed with errors: %s\nRefer to %s",
-                aggregateErrorMessages.c_str(), 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;
         }
 
@@ -148,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;
             }
 
@@ -158,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;
                 }
 
@@ -179,34 +176,45 @@ 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");
             }
         };
 
-        uint64_t urdfWorldCount = parsedUrdfRoot.WorldCount();
-        if (urdfWorldCount == 0)
+        // 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("ROS2EditorSystemComponent", false, "URDF file converted to SDF %s contains no worlds."
-                " O3DE Prefab cannot be created", filePath.data());
+            AZ_Error("ROS2RobotImporterEditorSystemComponent", false, "URDF file converted to SDF %.*s contains no worlds."
+                " O3DE Prefab cannot be created", AZ_STRING_ARG(filePath));
             return false;
         }
 
-        //! NOTE: Only support a single world case as no other tools support MultiWorld SDFs at the moment.
-        //! The first <world> tag is the only that will be processed
-        const sdf::World* parsedUrdfWorld = parsedUrdfRoot.WorldByIndex(0);
-        AZStd::string prefabName = AZStd::string(parsedUrdfWorld->Name().c_str(), parsedUrdfWorld->Name().size()) + ".prefab";
 
         const AZ::IO::Path prefabPathRelative(AZ::IO::Path("Assets") / "Importer" / prefabName);
         const AZ::IO::Path prefabPath(AZ::IO::Path(AZ::Utils::GetProjectPath()) / prefabPathRelative);
@@ -216,7 +224,7 @@ namespace ROS2
 
         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;
         }
 

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>
@@ -130,13 +132,19 @@ namespace ROS2
             else if (Utils::IsFileUrdf(m_urdfPath))
             {
                 // standard URDF
-                parsedUrdfOutcome = UrdfParser::ParseFromFile(m_urdfPath);
+                // 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();
+            AZStd::string log;
             bool urdfParsedSuccess = parsedUrdfOutcome.has_value();
             if (urdfParsedSuccess)
             {
@@ -150,6 +158,7 @@ namespace ROS2
             }
             else
             {
+                log = Utils::JoinSdfErrorsToString(parsedUrdfOutcome.error());
                 report += "# " + tr("The URDF was not opened") + "\n";
                 report += tr("URDF parser returned following errors:") + "\n\n";
             }
@@ -228,7 +237,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);
@@ -249,8 +258,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());
                     }
@@ -278,6 +287,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();
@@ -289,7 +300,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();
             }
@@ -298,7 +310,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)
         {

Gems/ROS2/Code/Source/RobotImporter/URDF/ArticulationsMaker.cpp

@@ -102,9 +102,8 @@ namespace ROS2
 
         articulationLinkConfiguration = AddToArticulationConfig(articulationLinkConfiguration, link->Inertial());
 
-        // TODO: Figure out parent/child relationships
         constexpr bool getNestedModelJoints = true;
-        AZStd::string_view linkName(link->Name().c_str(), link->Name().size());
+        AZStd::string linkName(link->Name().c_str(), link->Name().size());
         for (const sdf::Joint* joint : Utils::GetJointsForChildLink(model, linkName, getNestedModelJoints))
         {
             articulationLinkConfiguration = AddToArticulationConfig(articulationLinkConfiguration, joint);

Gems/ROS2/Code/Source/RobotImporter/URDF/CollidersMaker.cpp

@@ -98,7 +98,7 @@ namespace ROS2
             {
                 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(

Gems/ROS2/Code/Source/RobotImporter/URDF/JointsMaker.cpp

@@ -120,8 +120,9 @@ namespace ROS2
             }
             break;
         default:
-            AZ_Warning("AddJointComponent", false, "Unknown or unsupported joint type %d for joint %s", (int)joint->Type(), joint->Name().c_str());
-            return AZ::Failure(AZStd::string::format("unsupported joint type : %d", (int)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();

Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp

@@ -50,26 +50,28 @@ namespace ROS2
         AZ_Assert(m_root, "SDF Root is nullptr");
         if (m_root != nullptr)
         {
-            AZ_Assert(m_root->Model(), "SDF Model is nullptr");
+            AZ_Assert(GetFirstModel(), "SDF Model is nullptr");
         }
     }
 
     void URDFPrefabMaker::BuildAssetsForLink(const sdf::Link* link)
     {
         m_collidersMaker.BuildColliders(link);
-        // Find the links which are childs in a joint where the this link
+        // Find the links which are childen in a joint where this link
         // is a parent
         auto BuildAssetsFromJointChildLinks = [this](const sdf::Joint& joint)
         {
-            const sdf::Model& model = *m_root->Model();
+            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(*m_root->Model(), BuildAssetsFromJointChildLinks, visitNestedModelLinks);
+        Utils::VisitJoints(*GetFirstModel(), BuildAssetsFromJointChildLinks, visitNestedModelLinks);
     }
 
     URDFPrefabMaker::CreatePrefabTemplateResult URDFPrefabMaker::CreatePrefabTemplateFromURDF()
@@ -79,7 +81,7 @@ namespace ROS2
             m_status.clear();
         }
 
-        if (m_root->Model() == nullptr)
+        if (GetFirstModel() == nullptr)
         {
             return AZ::Failure(AZStd::string("Null model."));
         }
@@ -88,16 +90,16 @@ namespace ROS2
         AZStd::vector<AZ::EntityId> createdEntities;
 
         AZStd::unordered_map<AZStd::string, AzToolsFramework::Prefab::PrefabEntityResult> createdLinks;
-        const sdf::Link* rootLink = m_root->Model()->LinkByIndex(0);
+        const sdf::Link* rootLink = GetFirstModel()->LinkByIndex(0);
         AzToolsFramework::Prefab::PrefabEntityResult createEntityRoot = AddEntitiesForLink(rootLink, AZ::EntityId(), createdEntities);
-        AZStd::string rootName(m_root->Model()->Name().c_str(), m_root->Model()->Name().size());
+        AZStd::string rootName(GetFirstModel()->Name().c_str(), GetFirstModel()->Name().size());
         createdLinks[rootName] = createEntityRoot;
         if (!createEntityRoot.IsSuccess())
         {
             return AZ::Failure(AZStd::string(createEntityRoot.GetError()));
         }
 
-        auto links = Utils::GetAllLinks(*m_root->Model(), true);
+        auto links = Utils::GetAllLinks(*GetFirstModel(), true);
 
         for (const auto& [name, linkPtr] : links)
         {
@@ -167,7 +169,7 @@ namespace ROS2
                 continue;
             }
 
-            AZStd::vector<const sdf::Joint*> jointsWhereLinkIsChild = Utils::GetJointsForChildLink(*m_root->Model(),
+            AZStd::vector<const sdf::Joint*> jointsWhereLinkIsChild = Utils::GetJointsForChildLink(*GetFirstModel(),
                 linkName, true);
             if (jointsWhereLinkIsChild.empty())
             {
@@ -175,12 +177,17 @@ namespace ROS2
                 continue;
             }
 
-            // For URDF there is only a link can only be child with a single joint
+            // 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.
+                > 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.
             */
 
             jointsWhereLinkIsChild.front()->ParentName();
@@ -206,9 +213,9 @@ namespace ROS2
             PrefabMakerUtils::SetEntityParent(thisEntry.GetValue(), parentEntry->second.GetValue());
         }
 
-        for (uint64_t jointIndex{}; jointIndex < m_root->Model()->JointCount(); ++jointIndex)
+        for (uint64_t jointIndex{}; jointIndex < GetFirstModel()->JointCount(); ++jointIndex)
         {
-            auto jointPtr = m_root->Model()->JointByIndex(jointIndex);
+            auto jointPtr = GetFirstModel()->JointByIndex(jointIndex);
             AZ_Assert(jointPtr, "joint at index %" PRIu64 " is null", jointIndex);
             if (jointPtr == nullptr)
             {
@@ -377,10 +384,10 @@ namespace ROS2
         }
         else
         {
-            m_articulationsMaker.AddArticulationLink(*m_root->Model(), link, entityId);
+            m_articulationsMaker.AddArticulationLink(*GetFirstModel(), link, entityId);
         }
 
-        m_collidersMaker.AddColliders(*m_root->Model(), link, entityId);
+        m_collidersMaker.AddColliders(*GetFirstModel(), link, entityId);
         return AZ::Success(entityId);
     }
 
@@ -435,4 +442,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

Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.h

@@ -74,6 +74,9 @@ namespace ROS2
         void AddRobotControl(AZ::EntityId rootEntityId);
         static void MoveEntityToDefaultSpawnPoint(const AZ::EntityId& rootEntityId, AZStd::optional<AZ::Transform> spawnPosition);
 
+        // 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;

Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.cpp

@@ -14,77 +14,22 @@
 #include <AzCore/std/string/string.h>
 #include <console_bridge/console.h>
 
-namespace ROS2::UrdfParser::Internal
-{
-    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 ROS2::UrdfParser::Internal
-
 namespace ROS2::UrdfParser
 {
-    RootObjectOutcome Parse(AZStd::string_view xmlString)
+    RootObjectOutcome Parse(AZStd::string_view xmlString, const sdf::ParserConfig& parserConfig)
     {
-        return Parse(std::string(xmlString.data(), xmlString.size()));
+        return Parse(std::string(xmlString.data(), xmlString.size()), parserConfig);
     }
-    RootObjectOutcome Parse(const std::string& xmlString)
+    RootObjectOutcome Parse(const std::string& xmlString, const sdf::ParserConfig& parserConfig)
     {
-        // TODO: Figure out how to route the output handler
-        //console_bridge::useOutputHandler(&Internal::customConsoleHandler);
-        Internal::CheckIfCurrentLocaleHasDotAsADecimalSeparator();
         sdf::Root root;
-        auto parseRootErrors = root.LoadSdfString(xmlString);
-        //console_bridge::restorePreviousOutputHandler();
+        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);
     }
 
-    RootObjectOutcome ParseFromFile(AZ::IO::PathView filePath)
+    RootObjectOutcome ParseFromFile(AZ::IO::PathView filePath, const sdf::ParserConfig& parserConfig)
     {
         // Store path in a AZ::IO::FixedMaxPath which is stack based structure that provides memory
         // for the path string and is null terminated.
@@ -105,12 +50,6 @@ namespace ROS2::UrdfParser
         }
 
         std::string xmlStr((std::istreambuf_iterator<char>(istream)), std::istreambuf_iterator<char>());
-        return Parse(xmlStr);
-    }
-
-    AZStd::string GetUrdfParsingLog()
-    {
-        return Internal::customConsoleHandler.GetLog();
+        return Parse(xmlStr, parserConfig);
     }
-
 } // namespace ROS2

Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.h

@@ -33,18 +33,21 @@ namespace ROS2
 
         //! 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.
-        RootObjectOutcome Parse(AZStd::string_view xmlString);
-        RootObjectOutcome Parse(const std::string& xmlString);
+        //! @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 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.
-        RootObjectOutcome ParseFromFile(AZ::IO::PathView 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

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

Gems/ROS2/Code/Source/RobotImporter/Utils/ErrorUtils.h

@@ -0,0 +1,58 @@
+/*
+ * 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;
+    };
+}
+
+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

Gems/ROS2/Code/Source/RobotImporter/Utils/FilePath.cpp

@@ -34,5 +34,10 @@ namespace ROS2
         {
             return GetCapitalizedExtension(filename) == ".URDF";
         }
+
+        bool IsFileSDF(const AZ::IO::Path& filename)
+        {
+            return GetCapitalizedExtension(filename) == ".SDF";
+        }
     } // namespace Utils
 } // namespace ROS2

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>
@@ -19,6 +20,15 @@
 
 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 WaitForAssetsToProcess(const AZStd::unordered_map<AZStd::string, AZ::IO::Path>& sourceAssetsPaths)
     {
         bool allAssetProcessed = false;
@@ -91,20 +101,20 @@ namespace ROS2::Utils
 
     bool IsWheelURDFHeuristics(const sdf::Model& model, const sdf::Link* link)
     {
-        const AZStd::regex wheelRegex("wheel[_]||[_]wheel");
-        const AZStd::regex jointRegex("(?i)joint");
-        const AZStd::string linkName(link->Name().c_str(), link->Name().size());
-        AZStd::smatch match;
-        // Check if name is catchy for wheel
-        if (!AZStd::regex_search(linkName, match, wheelRegex))
+        auto wheelMatcher = [](AZStd::string_view name)
         {
-            return false;
-        }
-        // The name should contain a joint word
-        if (AZStd::regex_search(linkName, match, jointRegex))
+            // 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->CollisionCount() == 0) || (link->VisualCount() == 0))
         {
@@ -149,19 +159,7 @@ namespace ROS2::Utils
         if (sdf::Errors poseResolveErrors = linkSemanticPos.Resolve(resolvedPose);
             !poseResolveErrors.empty())
         {
-            AZStd::string poseErrorMessages;
-            for (const sdf::Error& error : poseResolveErrors)
-            {
-                AZStd::string errorMessage = AZStd::string::format("ErrorCode=%d", static_cast<int32_t>(error.Code()));
-                errorMessage += AZStd::string::format(", Message=%s", error.Message().c_str());
-                if (error.LineNumber().has_value())
-                {
-                    errorMessage += AZStd::string::format(", Line=%d", error.LineNumber().value());
-                }
-
-                poseErrorMessages += errorMessage;
-                poseErrorMessages += '\n';
-            }
+            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());
@@ -182,31 +180,43 @@ namespace ROS2::Utils
         {
             void operator()(const sdf::Model& model)
             {
-                VisitLinksForModel(model);
-                if (m_recurseModels)
+                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);
+                        const sdf::Model* nestedModel = model.ModelByIndex(modelIndex);
                         if (nestedModel != nullptr)
                         {
-                            VisitLinksForModel(*nestedModel);
+                            if (!VisitLinksForModel(*nestedModel))
+                            {
+                                // Sibling nested model links are only visited
+                                // if the joint visitor returns true
+                                break;
+                            }
                         }
                     }
                 }
             }
 
         private:
-            void VisitLinksForModel(const sdf::Model& currentModel)
+            //! 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)
                     {
-                        m_linkVisitorCB(*link);
+                        if (!m_linkVisitorCB(*link))
+                        {
+                            return false;
+                        }
                     }
                 }
+
+                return true;
             }
 
         public:
@@ -229,31 +239,41 @@ namespace ROS2::Utils
         {
             void operator()(const sdf::Model& model)
             {
-                VisitJointsForModel(model);
-                if (m_recurseModels)
+                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)
                         {
-                            VisitJointsForModel(*nestedModel);
+                            if (!VisitJointsForModel(*nestedModel))
+                            {
+                                // Sibling nested model joints are only visited
+                                // if the joint visitor returns true
+                                break;
+                            }
                         }
                     }
                 }
             }
 
         private:
-            void VisitJointsForModel(const sdf::Model& currentModel)
+            bool VisitJointsForModel(const sdf::Model& currentModel)
             {
                 for (uint64_t jointIndex{}; jointIndex < currentModel.JointCount(); ++jointIndex)
                 {
                     const sdf::Joint* joint = currentModel.JointByIndex(jointIndex);
                     if (joint != nullptr)
                     {
-                        m_jointVisitorCB(*joint);
+                        if (!m_jointVisitorCB(*joint))
+                        {
+                            return false;
+                        }
                     }
                 }
+
+                return true;
             }
 
         public:
@@ -274,8 +294,9 @@ namespace ROS2::Utils
         LinkMap links;
         auto GatherLinks = [&links](const sdf::Link& link)
         {
-            AZStd::string_view linkName(link.Name().c_str(), link.Name().size());
-            links.insert_or_assign(linkName, &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);
@@ -289,8 +310,9 @@ namespace ROS2::Utils
         JointMap joints;
         auto GatherJoints = [&joints](const sdf::Joint& joint)
         {
-            AZStd::string_view jointName(joint.Name().c_str(), joint.Name().size());
-            joints.insert_or_assign(jointName, &joint);
+            AZStd::string jointName(joint.Name().c_str(), joint.Name().size());
+            joints.insert_or_assign(AZStd::move(jointName), &joint);
+            return true;
         };
 
         VisitJoints(sdfModel, GatherJoints, gatherNestedModelJoints);
@@ -304,11 +326,13 @@ namespace ROS2::Utils
         JointVector joints;
         auto GatherJointsWhereLinkIsChild = [&joints, linkName](const sdf::Joint& joint)
         {
-            AZStd::string_view jointChildName{ joint.ChildName().c_str(), joint.ChildName().size() };
-            if (jointChildName == linkName)
+            if (AZStd::string_view jointChildName{ joint.ChildName().c_str(), joint.ChildName().size() };
+                jointChildName == linkName)
             {
                 joints.emplace_back(&joint);
             }
+
+            return true;
         };
 
         VisitJoints(sdfModel, GatherJointsWhereLinkIsChild, gatherNestedModelJoints);
@@ -322,11 +346,13 @@ namespace ROS2::Utils
         JointVector joints;
         auto GatherJointsWhereLinkIsParent = [&joints, linkName](const sdf::Joint& joint)
         {
-            AZStd::string_view jointParentName{ joint.ParentName().c_str(), joint.ParentName().size() };
-            if (jointParentName == linkName)
+            if (AZStd::string_view jointParentName{ joint.ParentName().c_str(), joint.ParentName().size() };
+                jointParentName == linkName)
             {
                 joints.emplace_back(&joint);
             }
+
+            return true;
         };
 
         VisitJoints(sdfModel, GatherJointsWhereLinkIsParent, gatherNestedModelJoints);
@@ -380,109 +406,161 @@ namespace ROS2::Utils
         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 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 ROS2::Utils

Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h

@@ -19,16 +19,13 @@
 
 #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 ROS2::Utils
 {
@@ -46,7 +43,8 @@ namespace ROS2::Utils
     AZ::Transform GetWorldTransformURDF(const sdf::Link* link, AZ::Transform t = AZ::Transform::Identity());
 
     //! Callback which is invoke for each link within a model
-    using LinkVisitorCallback = AZStd::function<void(const sdf::Link&)>;
+    //! @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
@@ -63,7 +61,8 @@ namespace ROS2::Utils
         bool gatherNestedModelLinks = false);
 
     //! Callback which is invoke for each valid joint for a given model
-    using JointVisitorCallback = AZStd::function<void(const sdf::Joint&)>;
+    //! @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
@@ -102,17 +101,22 @@ namespace ROS2::Utils
     //! @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 mesh
-    AZStd::string ResolveURDFPath(
-        AZStd::string unresolvedPath,
-        const AZStd::string& urdfFilePath,
-        const AZStd::string& amentPrefixPath,
-        const AZStd::function<bool(const AZStd::string&)>& fileExists = FileExistsCall);
+    //! @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

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

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.

Gems/ROS2/Code/Source/RobotImporter/xacro/XacroUtils.cpp

@@ -14,6 +14,8 @@
 #include <AzFramework/Process/ProcessWatcher.h>
 #include <QString>
 
+#include <SdfAssetBuilder/SdfAssetBuilderSettings.h>
+
 namespace ROS2::Utils::xacro
 {
 
@@ -65,7 +67,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);
+            // 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

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()
     {
@@ -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());
@@ -187,25 +189,17 @@ 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 parsedSdfRootOutcome = UrdfParser::ParseFromFile(fullSourcePath);
+        auto parsedSdfRootOutcome = UrdfParser::ParseFromFile(fullSourcePath, parserConfig);
         if (!parsedSdfRootOutcome)
         {
-            AZStd::string sdfImportErrors;
-            for (const sdf::Error& sdfError : parsedSdfRootOutcome.error())
-            {
-                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());
-                }
-                sdfImportErrors += errorMessage;
-                sdfImportErrors += '\n';
-            }
-
+            const AZStd::string sdfParseErrors = Utils::JoinSdfErrorsToString(parsedSdfRootOutcome.error());
             AZ_Error(SdfAssetBuilderName, false, R"(Failed to parse source file "%s". Errors: "%s")",
-                fullSourcePath.c_str(), sdfImportErrors.c_str());
+                fullSourcePath.c_str(), sdfParseErrors.c_str());
             return;
         }
 
@@ -257,23 +251,16 @@ 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 parsedSdfRootOutcome = UrdfParser::ParseFromFile(AZ::IO::PathView(request.m_fullPath));
+        auto parsedSdfRootOutcome = UrdfParser::ParseFromFile(AZ::IO::PathView(request.m_fullPath), parserConfig);
         if (!parsedSdfRootOutcome)
         {
-            AZStd::string sdfParseErrors;
-            for (const sdf::Error& sdfError : parsedSdfRootOutcome.error())
-            {
-                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());
-                }
-                sdfParseErrors += errorMessage;
-                sdfParseErrors += '\n';
-            }
+            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;

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 )

Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilderSettings.cpp

@@ -7,6 +7,7 @@
  */
 
 #include <SdfAssetBuilder/SdfAssetBuilderSettings.h>
+#include <SdfAssetBuilder/SdfAssetBuilder.h>
 
 #include <AzCore/Serialization/Json/JsonUtils.h>
 #include <AzCore/Settings/SettingsRegistryVisitorUtils.h>
@@ -15,8 +16,40 @@ namespace ROS2
 {
     namespace
     {
-        constexpr const char* SdfAssetBuilderSupportedFileExtensionsRegistryKey = "/O3DE/ROS2/SdfAssetBuilder/SupportedFileTypeExtensions";
-        constexpr const char* SdfAssetBuilderUseArticulationsRegistryKey = "/O3DE/ROS2/SdfAssetBuilder/UseArticulations";
+        struct SDFSettingsRootKeyType
+        {
+            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,6 +59,7 @@ 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.
@@ -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)

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

Gems/ROS2/Code/Tests/UrdfParserTest.cpp

@@ -228,7 +228,8 @@ namespace UnitTest
     TEST_F(UrdfParserTest, ParseUrdfWithOneLink)
     {
         const auto xmlStr = GetUrdfWithOneLink();
-        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr);
+        sdf::ParserConfig parserConfig;
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
         ASSERT_TRUE(sdfRootOutcome);
         const sdf::Root& sdfRoot = sdfRootOutcome.value();
 
@@ -274,10 +275,11 @@ namespace UnitTest
         EXPECT_EQ(1.0, collisionBox->Size().Z());
     }
 
-    TEST_F(UrdfParserTest, ParseUrdfWithTwoLinksAndFixedJoint)
+    TEST_F(UrdfParserTest, ParseUrdfWithTwoLinksAndFixedJoint_WithPreserveFixedJoint_False)
     {
         const auto xmlStr = GetUrdfWithTwoLinksAndJoint();
-        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr);
+        sdf::ParserConfig parserConfig;
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
         ASSERT_TRUE(sdfRootOutcome);
         const sdf::Root& sdfRoot = sdfRootOutcome.value();
 
@@ -303,10 +305,62 @@ namespace UnitTest
         ASSERT_NE(nullptr, link1);
     }
 
+    TEST_F(UrdfParserTest, ParseUrdfWithTwoLinksAndFixedJoint_WithPreserveFixedJoint_True)
+    {
+        const auto xmlStr = GetUrdfWithTwoLinksAndJoint();
+        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);
+
+        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;
+        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");
-        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr);
+        sdf::ParserConfig parserConfig;
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
         ASSERT_TRUE(sdfRootOutcome);
         const sdf::Root& sdfRoot = sdfRootOutcome.value();
 
@@ -328,12 +382,10 @@ namespace UnitTest
         EXPECT_EQ("link1", joint12->ParentName());
         EXPECT_EQ("link2", joint12->ChildName());
 
-        gz::math::Pose3d jointPose;
-        sdf::Errors poseResolveErrors = joint12->SemanticPose().Resolve(jointPose);
-        EXPECT_TRUE(poseResolveErrors.empty());
-        EXPECT_EQ(0.0, jointPose.X());
-        EXPECT_EQ(0.0, jointPose.Y());
-        EXPECT_EQ(0.0, jointPose.Z());
+        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;
         const gz::math::Quaternion rot = jointPose.Rot();
@@ -347,20 +399,21 @@ namespace UnitTest
         const sdf::JointAxis* joint12Axis = joint12->Axis();
         ASSERT_NE(nullptr, joint12Axis);
 
-        EXPECT_EQ(10.0, joint12Axis->Damping());
-        EXPECT_EQ(5.0, joint12Axis->Friction());
+        EXPECT_DOUBLE_EQ(10.0, joint12Axis->Damping());
+        EXPECT_DOUBLE_EQ(5.0, joint12Axis->Friction());
 
-        EXPECT_EQ(-AZStd::numeric_limits<double>::infinity(), joint12Axis->Lower());
-        EXPECT_EQ(AZStd::numeric_limits<double>::infinity(), joint12Axis->Upper());
-        EXPECT_EQ(90.0, joint12Axis->Effort());
-        EXPECT_EQ(10.0, joint12Axis->MaxVelocity());
+        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());
     }
 
     TEST_F(UrdfParserTest, WheelHeuristicNameValid)
     {
+        sdf::ParserConfig parserConfig;
         const AZStd::string_view wheelName("wheel_left_link");
         const auto xmlStr = GetURDFWithWheel(wheelName, "continuous");
-        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr);
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
         ASSERT_TRUE(sdfRootOutcome);
         const sdf::Root& sdfRoot = sdfRootOutcome.value();
         const sdf::Model* model = sdfRoot.Model();
@@ -368,13 +421,25 @@ namespace UnitTest
         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 wheelName("wheel_left_joint");
+        const AZStd::string wheelName("whe3l_left_link");
         const auto xmlStr = GetURDFWithWheel(wheelName, "continuous");
-        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr);
+        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();
@@ -388,7 +453,8 @@ namespace UnitTest
     {
         const AZStd::string wheelName("wheel_left_link");
         const auto xmlStr = GetURDFWithWheel(wheelName, "fixed");
-        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr);
+        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();
@@ -410,7 +476,8 @@ namespace UnitTest
     {
         const AZStd::string wheelName("wheel_left_link");
         const auto xmlStr = GetURDFWithWheel(wheelName, "continuous", false, true);
-        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr);
+        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();
@@ -424,7 +491,8 @@ namespace UnitTest
     {
         const AZStd::string wheelName("wheel_left_link");
         const auto xmlStr = GetURDFWithWheel(wheelName, "continuous", true, false);
-        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr);
+        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();
@@ -437,7 +505,8 @@ namespace UnitTest
     TEST_F(UrdfParserTest, TestLinkListing)
     {
         const auto xmlStr = GetURDFWithTranforms();
-        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr);
+        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();
@@ -463,7 +532,8 @@ namespace UnitTest
     TEST_F(UrdfParserTest, TestJointLink)
     {
         const auto xmlStr = GetURDFWithTranforms();
-        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr);
+        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();
@@ -477,7 +547,8 @@ namespace UnitTest
     TEST_F(UrdfParserTest, TestTransforms)
     {
         const auto xmlStr = GetURDFWithTranforms();
-        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr);
+        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();
@@ -517,7 +588,8 @@ namespace UnitTest
     TEST_F(UrdfParserTest, TestQueryJointsForParentLink_Succeeds)
     {
         const auto xmlStr = GetURDFWithTranforms();
-        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr);
+        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();
@@ -541,7 +613,8 @@ namespace UnitTest
     TEST_F(UrdfParserTest, TestQueryJointsForChildLink_Succeeds)
     {
         const auto xmlStr = GetURDFWithTranforms();
-        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr);
+        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();
@@ -564,12 +637,12 @@ namespace UnitTest
 
     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;
             });
@@ -578,12 +651,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;
             });
@@ -592,11 +665,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);
         };
@@ -606,11 +679,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);
         };

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

Gems/ROS2/Registry/sdfassetbuilder_settings.setreg

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