Added UnitTest for URDF links without inertia

URDFs with links without inertia that are not merged with other links that do have inertia via joint reduction causes the URDF to fail to parse using SDF.

When the `UrdfPreserveFixedJoint` setting is turned off, a URDF that doesn't have an inertial tag can be merged with a link that does have an inertial tag and allow the import to succeed.

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

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

@@ -8,11 +8,8 @@
 
 #include "UrdfParser.h"
 
-#include <fstream>
-
 #include <AzCore/Debug/Trace.h>
 #include <AzCore/std/string/string.h>
-#include <console_bridge/console.h>
 
 namespace ROS2::UrdfParser
 {

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

@@ -43,6 +43,18 @@ namespace AZStd
         using iterator_category = typename std::iterator_traits<typename std::vector<sdf::v13::Error>::iterator>::iterator_category;
         using iterator_concept = contiguous_iterator_tag;
     };
+
+    template<>
+    struct iterator_traits<typename std::vector<sdf::v13::Error>::const_iterator>
+    {
+        // Use the standard library iterator traits for all traits except for the iterator_concept
+        using difference_type = typename std::iterator_traits<typename std::vector<sdf::v13::Error>::const_iterator>::difference_type;
+        using value_type = typename std::iterator_traits<typename std::vector<sdf::v13::Error>::const_iterator>::value_type;
+        using pointer = typename std::iterator_traits<typename std::vector<sdf::v13::Error>::const_iterator>::pointer;
+        using reference = typename std::iterator_traits<typename std::vector<sdf::v13::Error>::const_iterator>::reference;
+        using iterator_category = typename std::iterator_traits<typename std::vector<sdf::v13::Error>::const_iterator>::iterator_category;
+        using iterator_concept = contiguous_iterator_tag;
+    };
 }
 
 namespace ROS2::Utils

Gems/ROS2/Code/Tests/UrdfParserTest.cpp

@@ -11,6 +11,7 @@
 #include <AzCore/std/string/string.h>
 #include <AzTest/AzTest.h>
 #include <RobotImporter/URDF/UrdfParser.h>
+#include <RobotImporter/Utils/ErrorUtils.h>
 #include <RobotImporter/Utils/RobotImporterUtils.h>
 #include <RobotImporter/xacro/XacroUtils.h>
 
@@ -92,6 +93,35 @@ namespace UnitTest
                    "</robot>", AZ_STRING_ARG(jointType));
         }
 
+        AZStd::string GetURDFWithTwoLinksAndBaseLinkNoInertia()
+        {
+            return R"(<?xml version="1.0" ?>
+                <robot name="always_ignored">
+                  <link name="base_link">
+                    <visual>
+                      <geometry>
+                        <box size="1.0 1.0 1.0"/>
+                      </geometry>
+                    </visual>
+                  </link>
+                  <link name="child_link">
+                    <inertial>
+                      <mass value="1.0"/>
+                      <inertia ixx="1.0" iyy="1.0" izz="1.0" ixy="0" ixz="0" iyz="0"/>
+                    </inertial>
+                    <visual>
+                      <geometry>
+                        <sphere radius="1.0"/>
+                      </geometry>
+                    </visual>
+                  </link>
+                  <joint name="joint12" type="fixed">
+                    <parent link="base_link"/>
+                    <child link="child_link"/>
+                  </joint>
+                </robot>)";
+        }
+
         // 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
@@ -408,6 +438,39 @@ namespace UnitTest
         EXPECT_DOUBLE_EQ(10.0, joint12Axis->MaxVelocity());
     }
 
+        TEST_F(UrdfParserTest, ParseURDF_WithTwoLinks_AndBaseLinkWithNoInertia_WithUrdfFixedJointPreservationOn_Fails)
+    {
+        const auto xmlStr = GetURDFWithTwoLinksAndBaseLinkNoInertia();
+        sdf::ParserConfig parserConfig;
+        parserConfig.URDFSetPreserveFixedJoint(true);
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
+        ASSERT_FALSE(sdfRootOutcome);
+        AZStd::string errorString = ROS2::Utils::JoinSdfErrorsToString(sdfRootOutcome.error());
+        printf("URDF with single link and no inertia: %s\n", errorString.c_str());
+    }
+
+    TEST_F(UrdfParserTest, ParseURDF_WithTwoLinks_AndBaseLinkWithNoInertia_WithUrdfFixedJointPreservationOff_Succeeds)
+    {
+        const auto xmlStr = GetURDFWithTwoLinksAndBaseLinkNoInertia();
+        sdf::ParserConfig parserConfig;
+        parserConfig.URDFSetPreserveFixedJoint(false);
+        const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
+        ASSERT_TRUE(sdfRootOutcome);
+        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+
+        const sdf::Model* model = sdfRoot.Model();
+        EXPECT_EQ("always_ignored", model->Name());
+        ASSERT_NE(nullptr, model);
+
+        ASSERT_EQ(1, model->LinkCount());
+
+        EXPECT_TRUE(model->FrameNameExists("child_link"));
+        EXPECT_TRUE(model->FrameNameExists("joint12"));
+
+        const sdf::Link* link1 = model->LinkByName("base_link");
+        ASSERT_NE(nullptr, link1);
+    }
+
     TEST_F(UrdfParserTest, WheelHeuristicNameValid)
     {
         sdf::ParserConfig parserConfig;