Updated the Link query logic in the RobotImporter

Joints in which a link is parent can be queried using the new `GetJointsForParentLink` function.

Joints in which a link is child can be queried using the new `GetJointsForChildLink` function.

A VisitJointsForModel and VisitLinksForModel function has been added to allow visting each joint or link on model.
It supports visiting joints or links on nested models as well

Fixed the URDF Parser Test to account for link and joints changes when using libsdformat for parsing a URDF.

SDFormat URDF parser uses joint reduction to combine two links joined together by fixed joint into the parent link itself.

That operation results in the removal of the child link and the joint.
SDFormat creates a Frame for the removed child link and joint using their "name" fields, so it is possible to still discover information about the child link.

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

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

@@ -112,6 +112,7 @@ namespace ROS2
                     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());

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

@@ -7,9 +7,10 @@
  */
 
 #include "ArticulationsMaker.h"
-#include "RobotImporter/Utils/DefaultSolverConfiguration.h"
 #include <AzCore/Component/EntityId.h>
 #include <AzToolsFramework/Entity/EditorEntityHelpers.h>
+#include <RobotImporter/Utils/DefaultSolverConfiguration.h>
+#include <RobotImporter/Utils/RobotImporterUtils.h>
 #include <RobotImporter/Utils/TypeConversions.h>
 #include <Source/EditorArticulationLinkComponent.h>
 
@@ -38,7 +39,7 @@ namespace ROS2
             "ArticulationsMaker",
             supportedArticulationType != SupportedJointTypes.end(),
             "Articulations do not support type %d for URDF joint %s.",
-            (int)joint->Type(),
+            static_cast<int>(joint->Type()),
             joint->Name().c_str());
         if (supportedArticulationType != SupportedJointTypes.end())
         {
@@ -91,7 +92,7 @@ namespace ROS2
         return articulationLinkConfiguration;
     }
 
-    void ArticulationsMaker::AddArticulationLink(const sdf::Link* link, AZ::EntityId entityId) const
+    void ArticulationsMaker::AddArticulationLink(const sdf::Model& model, const sdf::Link* link, AZ::EntityId entityId) const
     {
         AZ::Entity* entity = AzToolsFramework::GetEntityById(entityId);
         AZ_Assert(entity, "No entity for id %s", entityId.ToString().c_str());
@@ -102,7 +103,12 @@ namespace ROS2
         articulationLinkConfiguration = AddToArticulationConfig(articulationLinkConfiguration, link->Inertial());
 
         // TODO: Figure out parent/child relationships
-        //articulationLinkConfiguration = AddToArticulationConfig(articulationLinkConfiguration, link->parent_joint);
+        constexpr bool getNestedModelJoints = true;
+        AZStd::string_view linkName(link->Name().c_str(), link->Name().size());
+        for (const sdf::Joint* joint : Utils::GetJointsForChildLink(model, linkName, getNestedModelJoints))
+        {
+            articulationLinkConfiguration = AddToArticulationConfig(articulationLinkConfiguration, joint);
+        }
 
         entity->CreateComponent<PhysX::EditorArticulationLinkComponent>(articulationLinkConfiguration);
     }

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

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

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

@@ -205,10 +205,10 @@ namespace ROS2
         }
     }
 
-    void CollidersMaker::AddColliders(const sdf::Link* link, AZ::EntityId entityId)
+    void CollidersMaker::AddColliders(const sdf::Model& model, const sdf::Link* link, AZ::EntityId entityId)
     {
         AZStd::string typeString = "collider";
-        const bool isWheelEntity = Utils::IsWheelURDFHeuristics(link);
+        const bool isWheelEntity = Utils::IsWheelURDFHeuristics(model, link);
         if (isWheelEntity)
         {
             AZ_Printf(Internal::CollidersMakerLoggingTag, "Due to its name, %s is considered a wheel entity\n", link->Name().c_str());

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

@@ -41,9 +41,10 @@ namespace ROS2
         //! @param link A parsed URDF tree link node which could hold information about colliders.
         void BuildColliders(const sdf::Link* link);
         //! Add zero, one or many collider elements (depending on link content).
+        //! @param model An SDF model object provided by libsdformat from a parsed URDF
         //! @param link A parsed URDF tree link node which could hold information about colliders.
         //! @param entityId A non-active entity which will be affected.
-        void AddColliders(const sdf::Link* link, AZ::EntityId entityId);
+        void AddColliders(const sdf::Model& model, const sdf::Link* link, AZ::EntityId entityId);
         //! Sends meshes required for colliders to asset processor.
         //! @param buildReadyCb Function to call when the processing finishes.
         void ProcessMeshes(BuildReadyCallback notifyBuildReadyCb);

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

@@ -93,8 +93,13 @@ namespace ROS2
                 jointComponent = followColliderEntity->CreateComponent<PhysX::EditorHingeJointComponent>();
                 followColliderEntity->Activate();
 
-                const double limitUpper = AZ::RadToDeg(joint->Axis()->Upper());
-                const double limitLower = AZ::RadToDeg(joint->Axis()->Lower());
+                using LimitType = decltype(joint->Axis()->Upper());
+                const double limitUpper = joint->Axis()->Upper() != AZStd::numeric_limits<LimitType>::infinity()
+                    ? AZ::RadToDeg(joint->Axis()->Upper())
+                    : AZ::RadToDeg(AZ::Constants::TwoPi);
+                const double limitLower = joint->Axis()->Lower() != -AZStd::numeric_limits<LimitType>::infinity()
+                    ? AZ::RadToDeg(joint->Axis()->Lower())
+                    : -AZ::RadToDeg(AZ::Constants::TwoPi);
                 AZ_Printf(
                     "JointsMaker",
                     "Setting limits : upper: %.1f lower: %.1f (URDF:%f,%f)",

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

@@ -57,13 +57,19 @@ namespace ROS2
     void URDFPrefabMaker::BuildAssetsForLink(const sdf::Link* link)
     {
         m_collidersMaker.BuildColliders(link);
-        // TODO: Figure out what child links are now
-        /*
-        for (auto childLink : link->child_links)
+        // Find the links which are childs in a joint where the this link
+        // is a parent
+        auto BuildAssetsFromJointChildLinks = [this](const sdf::Joint& joint)
         {
-            BuildAssetsForLink(childLink);
-        }
-        */
+            const sdf::Model& model = *m_root->Model();
+            if (const sdf::Link* childLink = model.LinkByName(joint.ChildName());
+                childLink != nullptr)
+            {
+                BuildAssetsForLink(childLink);
+            }
+        };
+        constexpr bool visitNestedModelLinks = true;
+        Utils::VisitJoints(*m_root->Model(), BuildAssetsFromJointChildLinks, visitNestedModelLinks);
     }
 
     URDFPrefabMaker::CreatePrefabTemplateResult URDFPrefabMaker::CreatePrefabTemplateFromURDF()
@@ -91,13 +97,7 @@ namespace ROS2
             return AZ::Failure(AZStd::string(createEntityRoot.GetError()));
         }
 
-        std::vector<const sdf::Link*> modelLinks;
-        for (uint64_t index = 0; index < m_root->Model()->LinkCount(); index++)
-        {
-            modelLinks.push_back(m_root->Model()->LinkByIndex(index));
-        }
-
-        auto links = Utils::GetAllLinks(modelLinks);
+        auto links = Utils::GetAllLinks(*m_root->Model(), true);
 
         for (const auto& [name, linkPtr] : links)
         {
@@ -125,11 +125,11 @@ namespace ROS2
         // Set the transforms of links
         for (const auto& [name, linkPtr] : links)
         {
-            const auto this_entry = createdLinks.at(name);
-            if (this_entry.IsSuccess())
+            if (const auto thisEntry = createdLinks.at(name);
+                thisEntry.IsSuccess())
             {
                 AZ::Transform tf = Utils::GetWorldTransformURDF(linkPtr);
-                auto* entity = AzToolsFramework::GetEntityById(this_entry.GetValue());
+                auto* entity = AzToolsFramework::GetEntityById(thisEntry.GetValue());
                 if (entity)
                 {
                     auto* transformInterface = entity->FindComponent<AzToolsFramework::Components::TransformComponent>();
@@ -139,7 +139,7 @@ namespace ROS2
                             "CreatePrefabFromURDF",
                             "Setting transform %s %s to [%f %f %f] [%f %f %f %f]\n",
                             name.c_str(),
-                            this_entry.GetValue().ToString().c_str(),
+                            thisEntry.GetValue().ToString().c_str(),
                             tf.GetTranslation().GetX(),
                             tf.GetTranslation().GetY(),
                             tf.GetTranslation().GetZ(),
@@ -158,32 +158,43 @@ namespace ROS2
         }
 
         // Set the hierarchy
-        /*
-        for (const auto& [name, linkPtr] : links)
+        for (const auto& [linkName, linkPtr] : links)
         {
-            const auto thisEntry = createdLinks.at(name);
+            const auto thisEntry = createdLinks.at(linkName);
             if (!thisEntry.IsSuccess())
             {
-                AZ_Trace("CreatePrefabFromURDF", "Link %s creation failed\n", name.c_str());
+                AZ_Trace("CreatePrefabFromURDF", "Link %s creation failed\n", linkName.c_str());
                 continue;
             }
 
-            auto parentPtr = linkPtr->getParent();
-            if (!parentPtr)
+            AZStd::vector<const sdf::Joint*> jointsWhereLinkIsChild = Utils::GetJointsForChildLink(*m_root->Model(),
+                linkName, true);
+            if (jointsWhereLinkIsChild.empty())
             {
-                AZ_Trace("CreatePrefabFromURDF", "Link %s has no parents\n", name.c_str());
+                AZ_Trace("CreatePrefabFromURDF", "Link %s has no parents\n", linkName.c_str());
                 continue;
             }
-            AZStd::string parentName(parentPtr->name.c_str(), parentPtr->name.size());
+
+            // For URDF there is only a link can only be child with a single joint
+            /*
+                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.
+            */
+
+            jointsWhereLinkIsChild.front()->ParentName();
+            AZStd::string parentName(jointsWhereLinkIsChild.front()->ParentName().c_str(),
+                jointsWhereLinkIsChild.front()->ParentName().size());
             const auto parentEntry = createdLinks.find(parentName);
             if (parentEntry == createdLinks.end())
             {
-                AZ_Trace("CreatePrefabFromURDF", "Link %s has invalid parent name %s\n", name.c_str(), parentName.c_str());
+                AZ_Trace("CreatePrefabFromURDF", "Link %s has invalid parent name %s\n", linkName.c_str(), parentName.c_str());
                 continue;
             }
             if (!parentEntry->second.IsSuccess())
             {
-                AZ_Trace("CreatePrefabFromURDF", "Link %s has parent %s which has failed to create\n", name.c_str(), parentName.c_str());
+                AZ_Trace("CreatePrefabFromURDF", "Link %s has parent %s which has failed to create\n", linkName.c_str(), parentName.c_str());
                 continue;
             }
             AZ_Trace(
@@ -191,10 +202,9 @@ namespace ROS2
                 "Link %s setting parent to %s\n",
                 thisEntry.GetValue().ToString().c_str(),
                 parentEntry->second.GetValue().ToString().c_str());
-            AZ_Trace("CreatePrefabFromURDF", "Link %s setting parent to %s\n", name.c_str(), parentName.c_str());
+            AZ_Trace("CreatePrefabFromURDF", "Link %s setting parent to %s\n", linkName.c_str(), parentName.c_str());
             PrefabMakerUtils::SetEntityParent(thisEntry.GetValue(), parentEntry->second.GetValue());
         }
-        */
 
         for (uint64_t jointIndex{}; jointIndex < m_root->Model()->JointCount(); ++jointIndex)
         {
@@ -367,10 +377,10 @@ namespace ROS2
         }
         else
         {
-            m_articulationsMaker.AddArticulationLink(link, entityId);
+            m_articulationsMaker.AddArticulationLink(*m_root->Model(), link, entityId);
         }
 
-        m_collidersMaker.AddColliders(link, entityId);
+        m_collidersMaker.AddColliders(*m_root->Model(), link, entityId);
         return AZ::Success(entityId);
     }
 

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

@@ -67,6 +67,10 @@ namespace ROS2::UrdfParser::Internal
 
 namespace ROS2::UrdfParser
 {
+    RootObjectOutcome Parse(AZStd::string_view xmlString)
+    {
+        return Parse(std::string(xmlString.data(), xmlString.size()));
+    }
     RootObjectOutcome Parse(const std::string& xmlString)
     {
         // TODO: Figure out how to route the output handler

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

@@ -34,6 +34,7 @@ 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);
 
         //! Parse file with URDF data and generate model.

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

@@ -17,10 +17,9 @@
 #include <AzToolsFramework/API/EditorAssetSystemAPI.h>
 #include <string.h>
 
-namespace ROS2
+namespace ROS2::Utils
 {
-
-    bool Utils::WaitForAssetsToProcess(const AZStd::unordered_map<AZStd::string, AZ::IO::Path>& sourceAssetsPaths)
+    bool WaitForAssetsToProcess(const AZStd::unordered_map<AZStd::string, AZ::IO::Path>& sourceAssetsPaths)
     {
         bool allAssetProcessed = false;
         bool assetProcessorFailed = false;
@@ -90,19 +89,19 @@ namespace ROS2
         return false;
     }
 
-    bool Utils::IsWheelURDFHeuristics(const sdf::Link* link)
+    bool IsWheelURDFHeuristics(const sdf::Model& model, const sdf::Link* link)
     {
-        const AZStd::regex wheel_regex("wheel[_]||[_]wheel");
-        const AZStd::regex joint_regex("(?i)joint");
-        const AZStd::string link_name(link->Name().c_str(), link->Name().size());
+        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(link_name, match, wheel_regex))
+        if (!AZStd::regex_search(linkName, match, wheelRegex))
         {
             return false;
         }
         // The name should contain a joint word
-        if (AZStd::regex_search(link_name, match, joint_regex))
+        if (AZStd::regex_search(linkName, match, jointRegex))
         {
             return false;
         }
@@ -111,71 +110,237 @@ namespace ROS2
         {
             return false;
         }
-        // Parent joint needs to be CONTINOUS
-        // TODO: Figure out parent/child joints
-        /*
-        if (link->parent_joint && link->parent_joint->type == urdf::Joint::CONTINUOUS)
+
+        // When this link is a child, the parent link joint needs to be CONTINUOUS
+        AZStd::vector<const sdf::Joint*> joints = GetJointsForChildLink(model,
+            linkName, true);
+
+        // URDFs only have a single parent
+        // This is explained in the Pose frame semantics tutorial for sdformat
+        // http://sdformat.org/tutorials?tut=pose_frame_semantics&ver=1.5#parent-frames-in-urdf
+
+        // The SDF URDF parser converts continuous joints to revolute joints with a limit
+        // of -infinity to +infinity
+        // https://github.com/gazebosim/sdformat/blob/sdf13/src/parser_urdf.cc#L3009-L3039
+        bool isWheel{};
+        if (!joints.empty())
         {
-            return true;
+            const sdf::Joint* potentialWheelJoint = joints.front();
+            using LimitType = decltype(potentialWheelJoint->Axis()->Lower());
+            // There should only be 1 element for URDF, however that will not be verified
+            // in case this function is called on link from an SDF file
+            isWheel = potentialWheelJoint->Type() == sdf::JointType::CONTINUOUS;
+            isWheel = isWheel || (potentialWheelJoint->Type() == sdf::JointType::REVOLUTE
+                && potentialWheelJoint->Axis()->Lower() == -AZStd::numeric_limits<LimitType>::infinity()
+                && potentialWheelJoint->Axis()->Upper() == AZStd::numeric_limits<LimitType>::infinity());
         }
-        */
-        return false;
+
+        return isWheel;
     }
 
-    AZ::Transform Utils::GetWorldTransformURDF(const sdf::Link* link, AZ::Transform t)
+    AZ::Transform GetWorldTransformURDF(const sdf::Link* link, AZ::Transform t)
     {
-        // TODO: Figure out parent/child links
-        /*
-        if (link->getParent() != nullptr)
+        // Determine if the pose is relative to another link
+        // See doxygen at http://osrf-distributions.s3.amazonaws.com/sdformat/api/13.2.0/classsdf_1_1SDF__VERSION__NAMESPACE_1_1Link.html#a011d84b31f584938d89ac6b8c8a09eb3
+
+        sdf::SemanticPose linkSemanticPos = link->SemanticPose();
+        gz::math::Pose3d resolvedPose;
+
+        if (sdf::Errors poseResolveErrors = linkSemanticPos.Resolve(resolvedPose);
+            !poseResolveErrors.empty())
         {
-            t = URDF::TypeConversions::ConvertPose(link->parent_joint->parent_to_joint_origin_transform) * t;
-            return GetWorldTransformURDF(link->getParent(), t);
+            AZStd::string poseErrorMessages;
+            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';
+            }
+
+            AZ_Error("RobotImporter", false, R"(Failed to get world transform for link %s. Errors: "%s")",
+                link->Name().c_str(), poseErrorMessages.c_str());
+            return {};
         }
-        */
-        return t;
+
+        const AZ::Transform linkTransform = URDF::TypeConversions::ConvertPose(resolvedPose);
+        const AZ::Transform resolvedTransform = linkTransform * t;
+        return resolvedTransform;
+    }
+
+    void VisitLinks(const sdf::Model& sdfModel, const LinkVisitorCallback& linkVisitorCB,
+        bool visitNestedModelLinks)
+    {
+        // Function object which can visit all links of a model
+        // Optionally it supports recursing nested models to visit their links as well
+        struct VisitLinksForNestedModels_fn
+        {
+            void operator()(const sdf::Model& model)
+            {
+                VisitLinksForModel(model);
+                if (m_recurseModels)
+                {
+                    for (uint64_t modelIndex{}; modelIndex < model.ModelCount(); ++modelIndex)
+                    {
+                        const sdf::Model* nestedModel =  model.ModelByIndex(modelIndex);
+                        if (nestedModel != nullptr)
+                        {
+                            VisitLinksForModel(*nestedModel);
+                        }
+                    }
+                }
+            }
+
+        private:
+            void 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);
+                    }
+                }
+            }
+
+        public:
+            LinkVisitorCallback m_linkVisitorCB;
+            bool m_recurseModels{};
+        };
+
+        VisitLinksForNestedModels_fn VisitLinksForNestedModels{};
+        VisitLinksForNestedModels.m_linkVisitorCB = linkVisitorCB;
+        VisitLinksForNestedModels.m_recurseModels = visitNestedModelLinks;
+        VisitLinksForNestedModels(sdfModel);
+    }
+
+    void VisitJoints(const sdf::Model& sdfModel, const JointVisitorCallback& jointVisitorCB,
+        bool visitNestedModelJoints)
+    {
+        // Function object which can visit all joints of a model
+        // Optionally it supports recursing nested models to visit their joints as well
+        struct VisitJointsForNestedModels_fn
+        {
+            void operator()(const sdf::Model& model)
+            {
+                VisitJointsForModel(model);
+                if (m_recurseModels)
+                {
+                    for (uint64_t modelIndex{}; modelIndex < model.ModelCount(); ++modelIndex)
+                    {
+                        const sdf::Model* nestedModel =  model.ModelByIndex(modelIndex);
+                        if (nestedModel != nullptr)
+                        {
+                            VisitJointsForModel(*nestedModel);
+                        }
+                    }
+                }
+            }
+
+        private:
+            void 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);
+                    }
+                }
+            }
+
+        public:
+            JointVisitorCallback m_jointVisitorCB;
+            bool m_recurseModels{};
+        };
+
+        VisitJointsForNestedModels_fn VisitJointsForNestedModels{};
+        VisitJointsForNestedModels.m_jointVisitorCB = jointVisitorCB;
+        VisitJointsForNestedModels.m_recurseModels = visitNestedModelJoints;
+        VisitJointsForNestedModels(sdfModel);
+    }
+
+    AZStd::unordered_map<AZStd::string, const sdf::Link*> GetAllLinks(const sdf::Model& sdfModel,
+        bool gatherNestedModelLinks)
+    {
+        using LinkMap = AZStd::unordered_map<AZStd::string, const sdf::Link*>;
+        LinkMap links;
+        auto GatherLinks = [&links](const sdf::Link& link)
+        {
+            AZStd::string_view linkName(link.Name().c_str(), link.Name().size());
+            links.insert_or_assign(linkName, &link);
+        };
+
+        VisitLinks(sdfModel, GatherLinks, gatherNestedModelLinks);
+        return links;
     }
 
-    AZStd::unordered_map<AZStd::string, const sdf::Link*> Utils::GetAllLinks(const std::vector<const sdf::Link*>& childLinks)
+    AZStd::unordered_map<AZStd::string, const sdf::Joint*> GetAllJoints(const sdf::Model& sdfModel,
+        bool gatherNestedModelJoints)
     {
-        AZStd::unordered_map<AZStd::string, const sdf::Link*> pointers;
-        AZStd::function<void(const std::vector<const sdf::Link*>&)> link_visitor =
-            [&](const std::vector<const sdf::Link*>& child_links) -> void
+        using JointMap = AZStd::unordered_map<AZStd::string, const sdf::Joint*>;
+        JointMap joints;
+        auto GatherJoints = [&joints](const sdf::Joint& joint)
         {
-            for (const sdf::Link* child_link : child_links)
+            AZStd::string_view jointName(joint.Name().c_str(), joint.Name().size());
+            joints.insert_or_assign(jointName, &joint);
+        };
+
+        VisitJoints(sdfModel, GatherJoints, gatherNestedModelJoints);
+        return joints;
+    }
+
+    AZStd::vector<const sdf::Joint*> GetJointsForChildLink(const sdf::Model& sdfModel, AZStd::string_view linkName,
+        bool gatherNestedModelJoints)
+    {
+        using JointVector = AZStd::vector<const sdf::Joint*>;
+        JointVector joints;
+        auto GatherJointsWhereLinkIsChild = [&joints, linkName](const sdf::Joint& joint)
+        {
+            AZStd::string_view jointChildName{ joint.ChildName().c_str(), joint.ChildName().size() };
+            if (jointChildName == linkName)
             {
-                AZStd::string link_name(child_link->Name().c_str(), child_link->Name().size());
-                pointers[link_name] = child_link;
-                // TODO: Figure out parent/child links
-                //link_visitor(child_link->child_links);
+                joints.emplace_back(&joint);
             }
         };
-        link_visitor(childLinks);
-        return pointers;
+
+        VisitJoints(sdfModel, GatherJointsWhereLinkIsChild, gatherNestedModelJoints);
+        return joints;
     }
 
-    AZStd::unordered_map<AZStd::string, const sdf::Joint*> Utils::GetAllJoints(const std::vector<const sdf::Link*>& childLinks)
+    AZStd::vector<const sdf::Joint*> GetJointsForParentLink(const sdf::Model& sdfModel, AZStd::string_view linkName,
+        bool gatherNestedModelJoints)
     {
-        AZStd::unordered_map<AZStd::string, const sdf::Joint*> joints;
-        AZStd::function<void(const std::vector<const sdf::Link*>&)> link_visitor =
-            [&](const std::vector<const sdf::Link*>& child_links) -> void
+        using JointVector = AZStd::vector<const sdf::Joint*>;
+        JointVector joints;
+        auto GatherJointsWhereLinkIsParent = [&joints, linkName](const sdf::Joint& joint)
         {
-            // TODO: Figure out parent/child links
-            /*
-            for (auto child_link : child_links)
+            AZStd::string_view jointParentName{ joint.ParentName().c_str(), joint.ParentName().size() };
+            if (jointParentName == linkName)
             {
-                const auto& joint = child_link->parent_joint;
-                AZStd::string joint_name(joint->Name().c_str(), joint->N()ame.size());
-                joints[joint_name] = joint;
-                //link_visitor(child_link->child_links);
+                joints.emplace_back(&joint);
             }
-            */
         };
-        link_visitor(childLinks);
+
+        VisitJoints(sdfModel, GatherJointsWhereLinkIsParent, gatherNestedModelJoints);
         return joints;
     }
 
-    AZStd::unordered_set<AZStd::string> Utils::GetMeshesFilenames(const sdf::Root* rootLink, bool visual, bool colliders)
+    AZStd::unordered_set<AZStd::string> GetMeshesFilenames(const sdf::Root* root, bool visual, bool colliders)
     {
+        const sdf::Model* model = root != nullptr ? root->Model() : nullptr;
+        if (model == nullptr)
+        {
+            return {};
+        }
+
         AZStd::unordered_set<AZStd::string> filenames;
         const auto addFilenameFromGeometry = [&filenames](const sdf::Geometry* geometry)
         {
@@ -207,31 +372,11 @@ namespace ROS2
             }
         };
 
-        AZStd::function<void(const std::vector<const sdf::Link*>&)> linkVisitor =
-            [&](const std::vector<const sdf::Link*>& child_links) -> void
-        {
-            for (auto link : child_links)
-            {
-                processLink(link);
-                // TODO: Figure out parent/child
-                /*
-                std::vector<const sdf::Link*> childVector(link->child_links.size());
-                std::transform(
-                    link->child_links.begin(),
-                    link->child_links.end(),
-                    childVector.begin(),
-                    [](const sdf::Link* p)
-                    {
-                        return p;
-                    });
-                linkVisitor(childVector);
-                */
-            }
-        };
-        for (uint64_t index = 0; index < rootLink->Model()->LinkCount(); index++)
+        for (uint64_t index = 0; index < model->LinkCount(); index++)
         {
-            linkVisitor({ rootLink->Model()->LinkByIndex(index) });
+            processLink(model->LinkByIndex(index));
         }
+
         return filenames;
     }
 
@@ -265,7 +410,7 @@ namespace ROS2
     }
 
     /// Finds global path from URDF path
-    AZStd::string Utils::ResolveURDFPath(
+    AZStd::string ResolveURDFPath(
         AZStd::string unresolvedPath,
         const AZStd::string& urdfFilePath,
         const AZStd::string& amentPrefixPath,
@@ -340,55 +485,55 @@ namespace ROS2
         return resolvedPath.String();
     }
 
-    namespace Utils::SDFormat
+} // namespace ROS2::Utils
+
+namespace ROS2::Utils::SDFormat
+{
+    AZStd::string GetPluginFilename(const sdf::Plugin& plugin)
     {
-        AZStd::string GetPluginFilename(const sdf::Plugin& plugin)
-        {
-            const AZ::IO::Path path{ plugin.Filename().c_str() };
-            return path.Filename().String();
-        }
+        const AZ::IO::Path path{ plugin.Filename().c_str() };
+        return path.Filename().String();
+    }
+
+    AZStd::vector<AZStd::string> GetUnsupportedParams(
+        const sdf::ElementPtr& rootElement, const AZStd::unordered_set<AZStd::string>& supportedParams)
+    {
+        AZStd::vector<AZStd::string> unsupportedParams;
 
-        AZStd::vector<AZStd::string> GetUnsupportedParams(
-            const sdf::ElementPtr& rootElement, const AZStd::unordered_set<AZStd::string>& supportedParams)
+        AZStd::function<void(const sdf::ElementPtr& elementPtr, const std::string& prefix)> elementVisitor =
+            [&](const sdf::ElementPtr& elementPtr, const std::string& prefix) -> void
         {
-            AZStd::vector<AZStd::string> unsupportedParams;
+            auto childPtr = elementPtr->GetFirstElement();
 
-            AZStd::function<void(const sdf::ElementPtr& elementPtr, const std::string& prefix)> elementVisitor =
-                [&](const sdf::ElementPtr& elementPtr, const std::string& prefix) -> void
+            AZStd::string prefixAz(prefix.c_str(), prefix.size());
+            if (!childPtr && !prefixAz.empty() && !supportedParams.contains(prefixAz))
             {
-                auto childPtr = elementPtr->GetFirstElement();
+                unsupportedParams.push_back(prefixAz);
+            }
 
-                AZStd::string prefixAz(prefix.c_str(), prefix.size());
-                if (!childPtr && !prefixAz.empty() && !supportedParams.contains(prefixAz))
+            while (childPtr)
+            {
+                if (childPtr->GetName() == "plugin")
                 {
-                    unsupportedParams.push_back(prefixAz);
+                    break;
                 }
 
-                while (childPtr)
-                {
-                    if (childPtr->GetName() == "plugin")
-                    {
-                        break;
-                    }
+                std::string currentName = prefix;
+                currentName.append(">");
+                currentName.append(childPtr->GetName());
 
-                    std::string currentName = prefix;
-                    currentName.append(">");
-                    currentName.append(childPtr->GetName());
-
-                    elementVisitor(childPtr, currentName);
-                    childPtr = childPtr->GetNextElement();
-                }
-            };
+                elementVisitor(childPtr, currentName);
+                childPtr = childPtr->GetNextElement();
+            }
+        };
 
-            elementVisitor(rootElement, "");
+        elementVisitor(rootElement, "");
 
-            return unsupportedParams;
-        }
-
-        bool IsPluginSupported(const sdf::Plugin& plugin, const AZStd::unordered_set<AZStd::string>& supportedPlugins)
-        {
-            return supportedPlugins.contains(GetPluginFilename(plugin));
-        }
-    } // namespace Utils::SDFormat
+        return unsupportedParams;
+    }
 
-} // namespace ROS2
+    bool IsPluginSupported(const sdf::Plugin& plugin, const AZStd::unordered_set<AZStd::string>& supportedPlugins)
+    {
+        return supportedPlugins.contains(GetPluginFilename(plugin));
+    }
+} // namespace ROS2::Utils::SDFormat

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

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

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

@@ -65,7 +65,7 @@ namespace ROS2::Utils::xacro
         {
             AZ_Printf("ParseXacro", "xacro finished with success \n");
             const auto& output = process_output.outputResult;
-            outcome.m_urdfHandle = UrdfParser::Parse({ output.c_str(), output.size() });
+            outcome.m_urdfHandle = UrdfParser::Parse(output);
             outcome.m_succeed = true;
         }
         else

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

@@ -44,7 +44,7 @@ namespace ROS2
         // The fingerprint should only use the global builder settings, not the per-file settings.
         // The analysis fingerprint is set at the builder level, outside of any individual files,
         // so it should only include data that's invariant across files.
-        // Per-file settings changes will cause rebuilds of individual files through a separate 
+        // Per-file settings changes will cause rebuilds of individual files through a separate
         // mechanism in the Asset Processor that detects when an associated metadata settings file changes.
         m_fingerprint = GetFingerprint();
 
@@ -82,50 +82,6 @@ namespace ROS2
         // The AssetBuilderSDK doesn't support deregistration, so there's nothing more to do here.
     }
 
-    AZStd::vector<AssetBuilderSDK::AssetBuilderPattern> SdfAssetBuilder::GetSupportedBuilderPatterns()
-    {
-        AZStd::vector<AssetBuilderSDK::AssetBuilderPattern> patterns;
-
-        auto settingsRegistry = AZ::SettingsRegistry::Get();
-        if (settingsRegistry == nullptr)
-        {
-            AZ_Error(SdfAssetBuilderName, false, "Settings Registry not found, no sdf file type extensions enabled.");
-            return {};
-        }
-
-        // Visit each supported file type extension and create an asset builder wildcard pattern for it.
-        auto VisitFileTypeExtensions = [&settingsRegistry, &patterns]
-            (const AZ::SettingsRegistryInterface::VisitArgs& visitArgs)
-            {
-                if (AZ::SettingsRegistryInterface::FixedValueString value;
-                    settingsRegistry->Get(value, visitArgs.m_jsonKeyPath))
-                {
-                    // Ignore any entries that are either completely empty or *only* contain a '.'.
-                    // These will produce excessive (and presumably incorrect) wildcard matches.
-                    if (value.empty() ||
-                        ((value.size() == 1) && value.starts_with('.')))
-                    {
-                        return AZ::SettingsRegistryInterface::VisitResponse::Continue;
-                    }
-
-                    // Support both 'sdf' and '.sdf' style entries in the setreg file for robustness.
-                    // Either one will get turned into a '*.sdf' pattern.
-                    AZStd::string wildcardPattern = value.starts_with('.')
-                        ? AZStd::string::format("*%s", value.c_str())
-                        : AZStd::string::format("*.%s", value.c_str());
-
-                    patterns.push_back(
-                            AssetBuilderSDK::AssetBuilderPattern(
-                                wildcardPattern, AssetBuilderSDK::AssetBuilderPattern::PatternType::Wildcard));
-                }
-                return AZ::SettingsRegistryInterface::VisitResponse::Continue;
-            };
-        AZ::SettingsRegistryVisitorUtils::VisitArray(*settingsRegistry, VisitFileTypeExtensions, SdfAssetBuilderSupportedFileExtensionsRegistryKey);
-
-        AZ_Warning(SdfAssetBuilderName, !patterns.empty(), "SdfAssetBuilder disabled, no supported file type extensions found.");
-        return patterns;
-    }
-
     Utils::UrdfAssetMap SdfAssetBuilder::FindAssets(const sdf::Root& root, const AZStd::string& sourceFilename) const
     {
         AZ_Info(SdfAssetBuilderName, "Parsing mesh and collider names");
@@ -209,7 +165,7 @@ namespace ROS2
 
         [[maybe_unused]] AZ::Outcome<void, AZStd::string> saveObjectResult =
             AZ::JsonSerializationUtils::SaveObjectToStream(&m_globalSettings, stream, {}, &jsonSettings);
-        AZ_Assert(saveObjectResult.IsSuccess(), "Failed to save settings to fingerprint string: %s", 
+        AZ_Assert(saveObjectResult.IsSuccess(), "Failed to save settings to fingerprint string: %s",
             saveObjectResult.GetError().c_str());
 
         return settingsString;
@@ -245,6 +201,7 @@ namespace ROS2
                     errorMessage += AZStd::string::format(", Line=%d", sdfError.LineNumber().value());
                 }
                 sdfImportErrors += errorMessage;
+                sdfImportErrors += '\n';
             }
 
             AZ_Error(SdfAssetBuilderName, false, R"(Failed to parse source file "%s". Errors: "%s")",
@@ -315,6 +272,7 @@ namespace ROS2
                     errorMessage += AZStd::string::format(", Line=%d", sdfError.LineNumber().value());
                 }
                 sdfParseErrors += errorMessage;
+                sdfParseErrors += '\n';
             }
             AZ_Error(SdfAssetBuilderName, false, R"(Failed to parse source file "%s". Errors: "%s")",
                 request.m_fullPath.c_str(), sdfParseErrors.c_str());

Gems/ROS2/Code/Tests/UrdfParserTest.cpp

@@ -7,6 +7,7 @@
  */
 
 #include <AzCore/UnitTest/TestTypes.h>
+#include <AzCore/std/ranges/ranges_algorithm.h>
 #include <AzCore/std/string/string.h>
 #include <AzTest/AzTest.h>
 #include <RobotImporter/URDF/UrdfParser.h>
@@ -51,9 +52,9 @@ namespace UnitTest
                    "</robot>";
         }
 
-        AZStd::string GetUrdfWithTwoLinksAndJoint()
+        AZStd::string GetUrdfWithTwoLinksAndJoint(AZStd::string_view jointType = "fixed")
         {
-            return "<robot name=\"test_two_links_one_joint\">  "
+            return AZStd::string::format("<robot name=\"test_two_links_one_joint\">  "
                    "  <material name=\"some_material\">\n"
                    "    <color rgba=\"0 0 0 1\"/>\n"
                    "  </material>"
@@ -81,15 +82,20 @@ namespace UnitTest
                    "      <material name=\"some_material\"/>"
                    "    </visual>"
                    "  </link>"
-                   "  <joint name=\"joint12\" type=\"fixed\">"
+                   R"(  <joint name="joint12" type="%.*s">)"
                    "    <parent link=\"link1\"/>"
                    "    <child link=\"link2\"/>"
                    "    <origin rpy=\"0 0 0\" xyz=\"1.0 0.5 0.0\"/>"
                    "    <dynamics damping=\"10.0\" friction=\"5.0\"/>"
                    "    <limit lower=\"10.0\" upper=\"20.0\" effort=\"90.0\" velocity=\"10.0\"/>"
                    "  </joint>"
-                   "</robot>";
+                   "</robot>", AZ_STRING_ARG(jointType));
         }
+
+        // A URDF <model> can only represent structure which is configurable though the <joint> tags
+        // Therefore link can only appear as a child of a single joint.
+        // It cannot be a child of multiple joints
+        // https://wiki.ros.org/urdf/XML/model
         AZStd::string GetURDFWithTranforms()
         {
             return "<?xml version=\"1.0\"?>\n"
@@ -171,12 +177,6 @@ namespace UnitTest
                    "        <axis xyz=\"0. 0. 1.\"/>\n"
                    "        <origin rpy=\"0.000000 0.000000 2.094395\" xyz=\"-1.200000286102295 2.078460931777954 0.\"/>\n"
                    "    </joint> \n"
-                   "    <joint name=\"joint2\" type=\"continuous\">\n"
-                   "        <parent link=\"link1\"/>\n"
-                   "        <child link=\"link3\"/>\n"
-                   "        <axis xyz=\"0. 0. 1.\"/>\n"
-                   "        <origin rpy=\"0.000000 0.000000 -2.094395160675049\" xyz=\"-2.4000000953674316 0.0 0.0\"/>\n"
-                   "    </joint>\n"
                    "</robot>";
         }
 
@@ -227,161 +227,269 @@ namespace UnitTest
 
     TEST_F(UrdfParserTest, ParseUrdfWithOneLink)
     {
-
         const auto xmlStr = GetUrdfWithOneLink();
-        const auto urdf = ROS2::UrdfParser::Parse(xmlStr);
-
-        EXPECT_EQ(urdf->getName(), "test_one_link");
-
-        std::vector<urdf::LinkSharedPtr> links;
-        urdf->getLinks(links);
-        EXPECT_EQ(links.size(), 1U);
-
-        const auto link1 = urdf->getLink("link1");
-
-        ASSERT_TRUE(link1);
-        EXPECT_EQ(link1->inertial->mass, 1.0);
-        EXPECT_EQ(link1->inertial->ixx, 1.0);
-        EXPECT_EQ(link1->inertial->ixy, 0.0);
-        EXPECT_EQ(link1->inertial->ixz, 0.0);
-        EXPECT_EQ(link1->inertial->iyy, 1.0);
-        EXPECT_EQ(link1->inertial->iyz, 0.0);
-        EXPECT_EQ(link1->inertial->izz, 1.0);
-
-        EXPECT_EQ(link1->visual->geometry->type, 1);
-        const auto visualBox = std::dynamic_pointer_cast<urdf::Box>(link1->visual->geometry);
-        EXPECT_EQ(visualBox->dim.x, 1.0);
-        EXPECT_EQ(visualBox->dim.y, 2.0);
-        EXPECT_EQ(visualBox->dim.z, 1.0);
-
-        EXPECT_EQ(link1->collision->geometry->type, 1);
-        const auto collisionBox = std::dynamic_pointer_cast<urdf::Box>(link1->visual->geometry);
-        EXPECT_EQ(collisionBox->dim.x, 1.0);
-        EXPECT_EQ(collisionBox->dim.y, 2.0);
-        EXPECT_EQ(collisionBox->dim.z, 1.0);
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+
+        const sdf::Model* model = sdfRoot.Model();
+        EXPECT_EQ("test_one_link", model->Name());
+        ASSERT_NE(nullptr, model);
+
+        uint64_t linkCount = model->LinkCount();
+        EXPECT_EQ(1U, linkCount);
+
+        const sdf::Link* link1 = model->LinkByName("link1");
+
+        ASSERT_NE(nullptr, link1);
+        EXPECT_EQ(1.0, link1->Inertial().MassMatrix().Mass());
+        EXPECT_EQ(1.0, link1->Inertial().MassMatrix().Ixx());
+        EXPECT_EQ(0.0, link1->Inertial().MassMatrix().Ixy());
+        EXPECT_EQ(0.0, link1->Inertial().MassMatrix().Ixz());
+        EXPECT_EQ(1.0, link1->Inertial().MassMatrix().Iyy());
+        EXPECT_EQ(0.0, link1->Inertial().MassMatrix().Iyz());
+        EXPECT_EQ(1.0, link1->Inertial().MassMatrix().Izz());
+
+        ASSERT_EQ(1, link1->VisualCount());
+        const sdf::Visual* visual = link1->VisualByIndex(0);
+        ASSERT_NE(nullptr, visual);
+        const sdf::Geometry* geometryVis = visual->Geom();
+        ASSERT_NE(nullptr, geometryVis);
+        EXPECT_EQ(sdf::GeometryType::BOX, geometryVis->Type());
+        const sdf::Box* visualBox = geometryVis->BoxShape();
+        ASSERT_NE(nullptr, visualBox);
+        EXPECT_EQ(1.0, visualBox->Size().X());
+        EXPECT_EQ(2.0, visualBox->Size().Y());
+        EXPECT_EQ(1.0, visualBox->Size().Z());
+
+        ASSERT_EQ(1, link1->CollisionCount());
+        const sdf::Collision* collision = link1->CollisionByIndex(0);
+        ASSERT_NE(nullptr, collision);
+        const sdf::Geometry* geometryCol = visual->Geom();
+        ASSERT_NE(nullptr, geometryCol);
+        EXPECT_EQ(sdf::GeometryType::BOX, geometryCol->Type());
+        const sdf::Box* collisionBox = geometryCol->BoxShape();
+        EXPECT_EQ(1.0, collisionBox->Size().X());
+        EXPECT_EQ(2.0, collisionBox->Size().Y());
+        EXPECT_EQ(1.0, collisionBox->Size().Z());
     }
 
-    TEST_F(UrdfParserTest, ParseUrdfWithTwoLinksAndJoint)
+    TEST_F(UrdfParserTest, ParseUrdfWithTwoLinksAndFixedJoint)
     {
-
         const auto xmlStr = GetUrdfWithTwoLinksAndJoint();
-        const auto urdf = ROS2::UrdfParser::Parse(xmlStr);
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+
+        const sdf::Model* model = sdfRoot.Model();
+        EXPECT_EQ("test_two_links_one_joint", model->Name());
+        ASSERT_NE(nullptr, model);
+
+        // The SDFormat URDF parser combines links in joints that are fixed
+        // together
+        // https://github.com/gazebosim/sdformat/pull/1149
+        // So for a URDF with 2 links that are combined with a single fixed joint
+        // The resulted SDF has one 1 link and no joints
+        //
+        // The SDFormat <gazebo> extension tag can be used to preserve fixed joint by adding
+        // a <gazebo><preserveFixedJoint>true</preserveFixedJoint></gazebo> XML element
+        // http://sdformat.org/tutorials?tut=sdformat_urdf_extensions&cat=specification&#gazebo-elements-for-joints
+        ASSERT_EQ(1, model->LinkCount());
+
+        EXPECT_TRUE(model->FrameNameExists("link2"));
+        EXPECT_TRUE(model->FrameNameExists("joint12"));
+
+        const sdf::Link* link1 = model->LinkByName("link1");
+        ASSERT_NE(nullptr, link1);
+    }
+
+    TEST_F(UrdfParserTest, ParseUrdfWithTwoLinksAndNonFixedJoint)
+    {
+        const auto xmlStr = GetUrdfWithTwoLinksAndJoint("continuous");
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
 
-        EXPECT_EQ(urdf->getName(), "test_two_links_one_joint");
+        const sdf::Model* model = sdfRoot.Model();
+        EXPECT_EQ("test_two_links_one_joint", model->Name());
+        ASSERT_NE(nullptr, model);
 
-        std::vector<urdf::LinkSharedPtr> links;
-        urdf->getLinks(links);
-        EXPECT_EQ(links.size(), 2U);
+        ASSERT_EQ(2, model->LinkCount());
 
-        const auto link1 = urdf->getLink("link1");
-        ASSERT_TRUE(link1);
+        const sdf::Link* link1 = model->LinkByName("link1");
+        ASSERT_NE(nullptr, link1);
 
-        const auto link2 = urdf->getLink("link2");
-        ASSERT_TRUE(link2);
+        const sdf::Link* link2 = model->LinkByName("link2");
+        ASSERT_NE(nullptr, link2);
 
-        const auto joint12 = urdf->getJoint("joint12");
-        ASSERT_TRUE(joint12);
+        const sdf::Joint* joint12 = model->JointByName("joint12");
+        ASSERT_NE(nullptr, joint12);
 
-        EXPECT_EQ(joint12->parent_link_name, "link1");
-        EXPECT_EQ(joint12->child_link_name, "link2");
+        EXPECT_EQ("link1", joint12->ParentName());
+        EXPECT_EQ("link2", joint12->ChildName());
 
-        EXPECT_EQ(joint12->parent_to_joint_origin_transform.position.x, 1.0);
-        EXPECT_EQ(joint12->parent_to_joint_origin_transform.position.y, 0.5);
-        EXPECT_EQ(joint12->parent_to_joint_origin_transform.position.z, 0.0);
+        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());
 
         double roll, pitch, yaw;
-        joint12->parent_to_joint_origin_transform.rotation.getRPY(roll, pitch, yaw);
+        const gz::math::Quaternion rot = jointPose.Rot();
+        roll = rot.Roll();
+        pitch = rot.Pitch();
+        yaw = rot.Yaw();
         EXPECT_DOUBLE_EQ(roll, 0.0);
         EXPECT_DOUBLE_EQ(pitch, 0.0);
         EXPECT_DOUBLE_EQ(yaw, 0.0);
 
-        EXPECT_EQ(joint12->dynamics->damping, 10.0);
-        EXPECT_EQ(joint12->dynamics->friction, 5.0);
+        const sdf::JointAxis* joint12Axis = joint12->Axis();
+        ASSERT_NE(nullptr, joint12Axis);
+
+        EXPECT_EQ(10.0, joint12Axis->Damping());
+        EXPECT_EQ(5.0, joint12Axis->Friction());
 
-        EXPECT_EQ(joint12->limits->lower, 10.0);
-        EXPECT_EQ(joint12->limits->upper, 20.0);
-        EXPECT_EQ(joint12->limits->effort, 90.0);
-        EXPECT_EQ(joint12->limits->velocity, 10.0);
+        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());
     }
 
     TEST_F(UrdfParserTest, WheelHeuristicNameValid)
     {
-        const AZStd::string wheel_name("wheel_left_link");
-        const auto xmlStr = GetURDFWithWheel(wheel_name, "continuous");
-        const auto urdf = ROS2::UrdfParser::Parse(xmlStr);
-        auto wheel_candidate = urdf->getLink(wheel_name.c_str());
-        ASSERT_TRUE(wheel_candidate);
-        EXPECT_EQ(ROS2::Utils::IsWheelURDFHeuristics(wheel_candidate), true);
+        const AZStd::string_view wheelName("wheel_left_link");
+        const auto xmlStr = GetURDFWithWheel(wheelName, "continuous");
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Model* model = sdfRoot.Model();
+        ASSERT_NE(nullptr, model);
+        auto wheelCandidate = model->LinkByName(std::string(wheelName.data(), wheelName.size()));
+        ASSERT_NE(nullptr, wheelCandidate);
+        EXPECT_TRUE(ROS2::Utils::IsWheelURDFHeuristics(*model, wheelCandidate));
     }
 
     TEST_F(UrdfParserTest, WheelHeuristicNameNotValid1)
     {
-        const AZStd::string wheel_name("wheel_left_joint");
-        const auto xmlStr = GetURDFWithWheel(wheel_name, "continuous");
-        const auto urdf = ROS2::UrdfParser::Parse(xmlStr);
-        auto wheel_candidate = urdf->getLink(wheel_name.c_str());
-        ASSERT_TRUE(wheel_candidate);
-        EXPECT_EQ(ROS2::Utils::IsWheelURDFHeuristics(wheel_candidate), false);
+        const AZStd::string wheelName("wheel_left_joint");
+        const auto xmlStr = GetURDFWithWheel(wheelName, "continuous");
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Model* model = sdfRoot.Model();
+        ASSERT_NE(nullptr, model);
+        auto wheelCandidate = model->LinkByName(std::string(wheelName.data(), wheelName.size()));
+        ASSERT_NE(nullptr, wheelCandidate);
+        EXPECT_FALSE(ROS2::Utils::IsWheelURDFHeuristics(*model, wheelCandidate));
     }
 
     TEST_F(UrdfParserTest, WheelHeuristicJointNotValid)
     {
-        const AZStd::string wheel_name("wheel_left_link");
-        const auto xmlStr = GetURDFWithWheel(wheel_name, "fixed");
-        const auto urdf = ROS2::UrdfParser::Parse(xmlStr);
-        auto wheel_candidate = urdf->getLink(wheel_name.c_str());
-        ASSERT_TRUE(wheel_candidate);
-        EXPECT_EQ(ROS2::Utils::IsWheelURDFHeuristics(wheel_candidate), false);
+        const AZStd::string wheelName("wheel_left_link");
+        const auto xmlStr = GetURDFWithWheel(wheelName, "fixed");
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Model* model = sdfRoot.Model();
+        ASSERT_NE(nullptr, model);
+        // SDFormat converts combines the links of a joint with a fixed type
+        // into a single link
+        // It does however create a Frame with the name of the child link and joint that was combined
+        EXPECT_EQ(1, model->LinkCount());
+
+        auto wheelCandidate = model->LinkByName("base_link");
+        ASSERT_NE(nullptr, wheelCandidate);
+
+        EXPECT_TRUE(model->FrameNameExists(std::string{ wheelName.c_str(), wheelName.size() }));
+        EXPECT_TRUE(model->FrameNameExists("joint0"));
+        EXPECT_FALSE(ROS2::Utils::IsWheelURDFHeuristics(*model, wheelCandidate));
     }
 
     TEST_F(UrdfParserTest, WheelHeuristicJointVisualNotValid)
     {
-        const AZStd::string wheel_name("wheel_left_link");
-        const auto xmlStr = GetURDFWithWheel(wheel_name, "continuous", false, true);
-        const auto urdf = ROS2::UrdfParser::Parse(xmlStr);
-        auto wheel_candidate = urdf->getLink(wheel_name.c_str());
-        ASSERT_TRUE(wheel_candidate);
-        EXPECT_EQ(ROS2::Utils::IsWheelURDFHeuristics(wheel_candidate), false);
+        const AZStd::string wheelName("wheel_left_link");
+        const auto xmlStr = GetURDFWithWheel(wheelName, "continuous", false, true);
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Model* model = sdfRoot.Model();
+        ASSERT_NE(nullptr, model);
+        auto wheelCandidate = model->LinkByName(std::string(wheelName.c_str(), wheelName.size()));
+        ASSERT_NE(nullptr, wheelCandidate);
+        EXPECT_FALSE(ROS2::Utils::IsWheelURDFHeuristics(*model, wheelCandidate));
     }
 
     TEST_F(UrdfParserTest, WheelHeuristicJointColliderNotValid)
     {
-        const AZStd::string wheel_name("wheel_left_link");
-        const auto xmlStr = GetURDFWithWheel(wheel_name, "continuous", true, false);
-        const auto urdf = ROS2::UrdfParser::Parse(xmlStr);
-        auto wheel_candidate = urdf->getLink(wheel_name.c_str());
-        ASSERT_TRUE(wheel_candidate);
-        EXPECT_EQ(ROS2::Utils::IsWheelURDFHeuristics(wheel_candidate), false);
+        const AZStd::string wheelName("wheel_left_link");
+        const auto xmlStr = GetURDFWithWheel(wheelName, "continuous", true, false);
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Model* model = sdfRoot.Model();
+        ASSERT_NE(nullptr, model);
+        auto wheelCandidate = model->LinkByName(std::string(wheelName.c_str(), wheelName.size()));
+        ASSERT_NE(nullptr, wheelCandidate);
+        EXPECT_FALSE(ROS2::Utils::IsWheelURDFHeuristics(*model, wheelCandidate));
     }
 
     TEST_F(UrdfParserTest, TestLinkListing)
     {
         const auto xmlStr = GetURDFWithTranforms();
-        const auto urdf = ROS2::UrdfParser::Parse(xmlStr);
-        auto links = ROS2::Utils::GetAllLinks(urdf->getRoot()->child_links);
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Model* model = sdfRoot.Model();
+        ASSERT_NE(nullptr, model);
+        auto links = ROS2::Utils::GetAllLinks(*model);
+        // As the "joint_bs" is a fixed joint, it and it's child link are combined
+        // Therefore the "link1" child link and "joint_bs" fixed joint are combined
+        // into the base_link of the SDF
+        // However there are Frames for the combined links and joints
         EXPECT_EQ(links.size(), 3);
-        ASSERT_TRUE(links.contains("link1"));
+        ASSERT_TRUE(links.contains("base_link"));
         ASSERT_TRUE(links.contains("link2"));
         ASSERT_TRUE(links.contains("link3"));
-        EXPECT_EQ(links.at("link1")->name, "link1");
-        EXPECT_EQ(links.at("link2")->name, "link2");
-        EXPECT_EQ(links.at("link3")->name, "link3");
+        EXPECT_EQ("base_link", links.at("base_link")->Name());
+        EXPECT_EQ("link2", links.at("link2")->Name());
+        EXPECT_EQ("link3", links.at("link3")->Name());
+
+        // Check that the frame names exist on the model
+        EXPECT_TRUE(model->FrameNameExists("joint_bs"));
+        EXPECT_TRUE(model->FrameNameExists("link1"));
     }
 
     TEST_F(UrdfParserTest, TestJointLink)
     {
         const auto xmlStr = GetURDFWithTranforms();
-        const auto urdf = ROS2::UrdfParser::Parse(xmlStr);
-        auto joints = ROS2::Utils::GetAllJoints(urdf->getRoot()->child_links);
-        EXPECT_EQ(joints.size(), 3);
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Model* model = sdfRoot.Model();
+        ASSERT_NE(nullptr, model);
+        auto joints = ROS2::Utils::GetAllJoints(*model);
+        EXPECT_EQ(2, joints.size());
+        ASSERT_TRUE(joints.contains("joint0"));
+        ASSERT_TRUE(joints.contains("joint1"));
     }
 
     TEST_F(UrdfParserTest, TestTransforms)
     {
         const auto xmlStr = GetURDFWithTranforms();
-        const auto urdf = ROS2::UrdfParser::Parse(xmlStr);
-        const auto links = ROS2::Utils::GetAllLinks(urdf->getRoot()->child_links);
-        const auto link1_ptr = links.at("link1");
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Model* model = sdfRoot.Model();
+        ASSERT_NE(nullptr, model);
+        const auto links = ROS2::Utils::GetAllLinks(*model);
+        // The "link1" is combined with the base_link through
+        // joint reduction in the URDF->SDF parser logic
+        // https://github.com/gazebosim/sdformat/issues/1110
+        ASSERT_TRUE(links.contains("base_link"));
+        ASSERT_TRUE(links.contains("link2"));
+        ASSERT_TRUE(links.contains("link3"));
+        const auto base_link_ptr = links.at("base_link");
         const auto link2_ptr = links.at("link2");
         const auto link3_ptr = links.at("link3");
 
@@ -390,7 +498,7 @@ namespace UnitTest
         const AZ::Vector3 expected_translation_link2{ -1.2000000476837158, 2.0784599781036377, 0.0 };
         const AZ::Vector3 expected_translation_link3{ -2.4000000953674316, 0.0, 0.0 };
 
-        const AZ::Transform transform_from_urdf_link1 = ROS2::Utils::GetWorldTransformURDF(link1_ptr);
+        const AZ::Transform transform_from_urdf_link1 = ROS2::Utils::GetWorldTransformURDF(base_link_ptr);
         EXPECT_NEAR(expected_translation_link1.GetX(), transform_from_urdf_link1.GetTranslation().GetX(), 1e-5);
         EXPECT_NEAR(expected_translation_link1.GetY(), transform_from_urdf_link1.GetTranslation().GetY(), 1e-5);
         EXPECT_NEAR(expected_translation_link1.GetZ(), transform_from_urdf_link1.GetTranslation().GetZ(), 1e-5);
@@ -406,6 +514,54 @@ namespace UnitTest
         EXPECT_NEAR(expected_translation_link3.GetZ(), transform_from_urdf_link3.GetTranslation().GetZ(), 1e-5);
     }
 
+    TEST_F(UrdfParserTest, TestQueryJointsForParentLink_Succeeds)
+    {
+        const auto xmlStr = GetURDFWithTranforms();
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Model* model = sdfRoot.Model();
+        ASSERT_NE(nullptr, model);
+        auto joints = ROS2::Utils::GetJointsForParentLink(*model, "base_link");
+        EXPECT_EQ(1, joints.size());
+
+        auto jointToNameProjection = [](const sdf::Joint* joint)
+        {
+            return AZStd::string_view(joint->Name().c_str(), joint->Name().size());
+        };
+        ASSERT_TRUE(AZStd::ranges::contains(joints, "joint0", jointToNameProjection));
+
+        // Now check the middle link of "link2"
+        joints = ROS2::Utils::GetJointsForParentLink(*model, "link2");
+        EXPECT_EQ(1, joints.size());
+
+        ASSERT_TRUE(AZStd::ranges::contains(joints, "joint1", jointToNameProjection));
+    }
+
+    TEST_F(UrdfParserTest, TestQueryJointsForChildLink_Succeeds)
+    {
+        const auto xmlStr = GetURDFWithTranforms();
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Model* model = sdfRoot.Model();
+        ASSERT_NE(nullptr, model);
+        auto joints = ROS2::Utils::GetJointsForChildLink(*model, "link2");
+        EXPECT_EQ(1, joints.size());
+
+        auto jointToNameProjection = [](const sdf::Joint* joint)
+        {
+            return AZStd::string_view(joint->Name().c_str(), joint->Name().size());
+        };
+        ASSERT_TRUE(AZStd::ranges::contains(joints, "joint0", jointToNameProjection));
+
+        // Now check the final link of "link3"
+        joints = ROS2::Utils::GetJointsForChildLink(*model, "link3");
+        EXPECT_EQ(1, joints.size());
+
+        ASSERT_TRUE(AZStd::ranges::contains(joints, "joint1", jointToNameProjection));
+    }
+
     TEST_F(UrdfParserTest, TestPathResolvementGlobal)
     {
         AZStd::string dae = "file:///home/foo/ros_ws/install/foo_robot/meshes/bar.dae";