display explanations of modifications made to URDF (#518)

* Messages about each kind of modification in separate sections Apply suggestions from code review * more visible descriptions of modifications * Added license; removed unused variable * moved generation of modification description to separate function * not printing headers of unused messages --------- Signed-off-by: Kacper Lewandowski <[email protected]> Co-authored-by: Jan Hanca <[email protected]> Signed-off-by: Michał Pełka <[email protected]>
o3de · Oct 2, 2023 · 3821f0f · 3821f0f
1 parent 6702361
commit 3821f0f
Show file tree

Hide file tree

Showing 8 changed files with 153 additions and 42 deletions.
diff --git a/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.cpp b/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.cpp
@@ -7,6 +7,7 @@
  */
 
 #include "FixURDF.h"
+#include "URDFModifications.h"
 #include <AzCore/XML/rapidxml.h>
 #include <AzCore/XML/rapidxml_print.h>
 #include <AzCore/std/containers/unordered_map.h>
@@ -17,15 +18,16 @@ namespace ROS2::Utils
     //! Modifies a parsed URDF in memory to add missing inertia to links, which prevents SDF error 19.
     //! @param urdf URDF to modify.
     //! @returns a list of names of links that were modified.
-    AZStd::vector<AZStd::string> AddMissingInertiaToLinks(AZ::rapidxml::xml_node<>* urdf)
+    AZStd::pair<AZStd::vector<MissingInertia>, AZStd::vector<IncompleteInertia>> AddMissingInertiaToLinks(AZ::rapidxml::xml_node<>* urdf)
     {
-        AZStd::vector<AZStd::string> modifiedLinks;
+        AZStd::vector<MissingInertia> missingInertias;
+        AZStd::vector<IncompleteInertia> incompleteInertias;
         using namespace AZ::rapidxml;
 
         for (xml_node<>* link = urdf->first_node("link"); link; link = link->next_sibling("link"))
         {
             bool modified = false;
-            bool inertiaPresent = true;
+            bool inertialPresent = true;
 
             auto name_xml = link->first_attribute("name");
             AZStd::string name = "unknown_link";
@@ -41,23 +43,36 @@ namespace ROS2::Utils
                 inertial = urdf->document()->allocate_node(node_element, "inertial");
                 link->append_node(inertial);
                 modified = true;
-                inertiaPresent = false;
+                inertialPresent = false;
+
+                MissingInertia missingInertia;
+                missingInertia.linkName = name;
+                missingInertias.push_back(missingInertia);
             }
 
+            IncompleteInertia incompleteInertia;
+            incompleteInertia.linkName = name;
+
             auto mass = inertial->first_node("mass");
             if (!mass)
             {
-                AZ_Warning("URDF", !inertiaPresent, "Missing mass tag inside the inertial tag link %s, applying default mass tag.", name.c_str());
+                AZ_Warning(
+                    "URDF", !inertialPresent, "Missing mass tag inside the inertial tag link %s, applying default mass tag.", name.c_str());
                 mass = urdf->document()->allocate_node(node_element, "mass");
                 mass->append_attribute(urdf->document()->allocate_attribute("value", "1."));
                 inertial->append_node(mass);
                 modified = true;
+                incompleteInertia.missingTags.push_back("mass");
             }
 
             auto inertia = inertial->first_node("inertia");
             if (!inertia)
             {
-                AZ_Warning("URDF", !inertiaPresent, "Missing inertia tag inside the inertial tag link %s, applying default inertia tag.", name.c_str());
+                AZ_Warning(
+                    "URDF",
+                    !inertialPresent,
+                    "Missing inertia tag inside the inertial tag link %s, applying default inertia tag.",
+                    name.c_str());
                 inertia = urdf->document()->allocate_node(node_element, "inertia");
 
                 inertia->append_attribute(urdf->document()->allocate_attribute("ixx", "1."));
@@ -74,26 +89,27 @@ namespace ROS2::Utils
 
                 inertial->append_node(inertia);
                 modified = true;
+                incompleteInertia.missingTags.push_back("inertia");
             }
 
-            if (modified)
+            if (modified && inertialPresent)
             {
-                modifiedLinks.push_back(name);
+                incompleteInertias.push_back(incompleteInertia);
             }
         }
 
-        return modifiedLinks;
+        return { missingInertias, incompleteInertias };
     }
 
     //! Handles a case of multiple joints and the link sharing a common names which causes SDF error2 (but is fine in URDF)
     //! Function will add a suffix "_dup" to the name of the joint if it is also the name of a link.
     //! If there are name collisions in links, this will not be able to fix it, the SDF parser will throw an error.
     //! @param urdf URDF to modify.
     //! @returns a list of links that were modified
-    AZStd::vector<AZStd::string> RenameDuplicatedJoints(AZ::rapidxml::xml_node<>* urdf)
+    AZStd::vector<DuplicatedJoint> RenameDuplicatedJoints(AZ::rapidxml::xml_node<>* urdf)
     {
         using namespace AZ::rapidxml;
-        AZStd::vector<AZStd::string> modifiedLinks;
+        AZStd::vector<DuplicatedJoint> duplicatedJoints;
         AZStd::unordered_map<AZStd::string, unsigned int> linkAndJointsName;
         for (xml_node<>* link = urdf->first_node("link"); link; link = link->next_sibling("link"))
         {
@@ -108,26 +124,34 @@ namespace ROS2::Utils
             auto* name = joint->first_attribute("name");
             if (name)
             {
-                if (linkAndJointsName.contains(name->value()))
+                AZStd::string name_value = name->value();
+
+                if (linkAndJointsName.contains(name_value))
                 {
+                    AZStd::string name_value = name->value();
+
                     unsigned int& count = linkAndJointsName[name->value()];
-                    auto newName = AZStd::string::format("%s_dup%u", name->value(), count);
+                    auto newName = AZStd::string::format("%s_dup%u", name_value.c_str(), count);
                     name->value(urdf->document()->allocate_string(newName.c_str()));
                     count++;
-                    modifiedLinks.push_back(AZStd::move(newName));
+
+                    DuplicatedJoint duplication;
+                    duplication.newName = newName;
+                    duplication.oldName = name_value;
+                    duplicatedJoints.push_back(duplication);
                 }
                 else
                 {
-                    linkAndJointsName.insert(AZStd::make_pair(name->value(), 0));
+                    linkAndJointsName.insert(AZStd::make_pair(name_value, 0));
                 }
             }
         }
-        return modifiedLinks;
+        return duplicatedJoints;
     }
 
-    AZStd::pair<std::string, AZStd::vector<AZStd::string>> ModifyURDFInMemory(const AZStd::string& data)
+    AZStd::pair<std::string, UrdfModifications> ModifyURDFInMemory(const AZStd::string& data)
     {
-        AZStd::vector<AZStd::string> modifiedElements;
+        UrdfModifications modifiedElements;
         using namespace AZ::rapidxml;
         xml_document<> doc;
         doc.parse<0>(const_cast<char*>(data.c_str()));
@@ -137,19 +161,19 @@ namespace ROS2::Utils
             AZ_Warning("URDF", false, "No robot tag in URDF");
             return { data.c_str(), modifiedElements };
         }
-        auto links = AddMissingInertiaToLinks(urdf);
-        modifiedElements.insert(modifiedElements.end(), AZStd::make_move_iterator(links.begin()), AZStd::make_move_iterator(links.end()));
+        auto [missing, incomplete] = AddMissingInertiaToLinks(urdf);
+
+        modifiedElements.missingInertias = missing;
+        modifiedElements.incompleteInertias = incomplete;
 
-        auto renames = RenameDuplicatedJoints(urdf);
-        modifiedElements.insert(
-            modifiedElements.end(), AZStd::make_move_iterator(renames.begin()), AZStd::make_move_iterator(renames.end()));
+        modifiedElements.duplicatedJoints = RenameDuplicatedJoints(urdf);
 
         std::string xmlDocString;
         AZ::rapidxml::print(std::back_inserter(xmlDocString), *urdf, 0);
         return { xmlDocString, modifiedElements };
     }
 
-    AZStd::pair<std::string, AZStd::vector<AZStd::string>> ModifyURDFInMemory(const std::string& data)
+    AZStd::pair<std::string, UrdfModifications> ModifyURDFInMemory(const std::string& data)
     {
         return ModifyURDFInMemory(AZStd::string(data.c_str()));
     }

diff --git a/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.h b/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.h
@@ -8,6 +8,7 @@
 
 #pragma once
 
+#include "URDFModifications.h"
 #include <AzCore/std/containers/vector.h>
 #include <AzCore/std/string/string.h>
 
@@ -19,7 +20,7 @@ namespace ROS2::Utils
     //! - Renames joints that have the same name as a link.
     //! @param urdf URDF to modify.
     //! @returns a modified URDF and a list of XML element that were modified
-    AZStd::pair<std::string, AZStd::vector<AZStd::string>> ModifyURDFInMemory(const std::string& data);
-    AZStd::pair<std::string, AZStd::vector<AZStd::string>> ModifyURDFInMemory(const AZStd::string& data);
+    AZStd::pair<std::string, UrdfModifications> ModifyURDFInMemory(const std::string& data);
+    AZStd::pair<std::string, UrdfModifications> ModifyURDFInMemory(const AZStd::string& data);
 
 } // namespace ROS2::Utils
diff --git a/Gems/ROS2/Code/Source/RobotImporter/FixURDF/URDFModifications.h b/Gems/ROS2/Code/Source/RobotImporter/FixURDF/URDFModifications.h
@@ -0,0 +1,41 @@
+/*
+ * 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/std/containers/vector.h>
+#include <AzCore/std/string/string.h>
+
+namespace ROS2::Utils
+{
+    struct MissingInertia
+    {
+        AZStd::string linkName;
+
+        ~MissingInertia() = default;
+    };
+
+    struct IncompleteInertia
+    {
+        AZStd::string linkName;
+        AZStd::vector<AZStd::string> missingTags;
+    };
+
+    struct DuplicatedJoint
+    {
+        AZStd::string oldName;
+        AZStd::string newName;
+    };
+
+    struct UrdfModifications
+    {
+        AZStd::vector<MissingInertia> missingInertias;
+        AZStd::vector<IncompleteInertia> incompleteInertias;
+        AZStd::vector<DuplicatedJoint> duplicatedJoints;
+    };
+
+} // namespace ROS2::Utils
diff --git a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp
@@ -11,6 +11,7 @@
 #include <AzCore/Math/Uuid.h>
 #include <AzCore/Utils/Utils.h>
 
+#include "FixURDF/URDFModifications.h"
 #include "RobotImporterWidget.h"
 #include <QApplication>
 #include <QScreen>
@@ -80,6 +81,53 @@ namespace ROS2
         }
     }
 
+    void RobotImporterWidget::AddModificationWarningsToReportString(QString& report, const UrdfParser::RootObjectOutcome& parsedSdfOutcome)
+    {
+        // This is a URDF only path, and therefore the report text does not mention SDF
+        report += "# " + tr("The URDF was parsed, though results were modified to be compatible with SDFormat") + "\n";
+
+        if (!parsedSdfOutcome.m_urdfModifications.missingInertias.empty())
+        {
+            report += "## " + tr("Inertial information in the following links is missing, reset to default: ") + "\n";
+            for (const auto& modifiedTag : parsedSdfOutcome.m_urdfModifications.missingInertias)
+            {
+                report += " - " + QString::fromUtf8(modifiedTag.linkName.data(), static_cast<int>(modifiedTag.linkName.size())) + "\n";
+            }
+            report += "\n";
+        }
+
+        if (!parsedSdfOutcome.m_urdfModifications.incompleteInertias.empty())
+        {
+            report +=
+                "## " + tr("Inertial information in the following links is incomplete, set default values for listed subtags: ") + "\n";
+            for (const auto& modifiedTag : parsedSdfOutcome.m_urdfModifications.incompleteInertias)
+            {
+                report += " - " + QString::fromUtf8(modifiedTag.linkName.data(), static_cast<int>(modifiedTag.linkName.size())) + ": ";
+
+                for (const auto& tag : modifiedTag.missingTags)
+                {
+                    report += QString::fromUtf8(tag.data(), static_cast<int>(tag.size())) + ", ";
+                }
+
+                report += "\n";
+            }
+            report += "\n";
+        }
+
+        if (!parsedSdfOutcome.m_urdfModifications.duplicatedJoints.empty())
+        {
+            report += "## " + tr("The following joints were renamed to avoid duplication") + "\n";
+            for (const auto& modifiedTag : parsedSdfOutcome.m_urdfModifications.duplicatedJoints)
+            {
+                report += " - " + QString::fromUtf8(modifiedTag.oldName.data(), static_cast<int>(modifiedTag.oldName.size())) + " -> " +
+                    QString::fromUtf8(modifiedTag.newName.data(), static_cast<int>(modifiedTag.newName.size())) + "\n";
+            }
+        }
+
+        report += "\n# " + tr("The modified URDF code:") + "\n";
+        report += "```\n" + QString::fromStdString(parsedSdfOutcome.m_modifiedURDFContent) + "```\n";
+    }
+
     void RobotImporterWidget::OpenUrdf()
     {
         UrdfParser::RootObjectOutcome parsedSdfOutcome;
@@ -94,7 +142,8 @@ namespace ROS2
 
             if (Utils::IsFileXacro(m_urdfPath))
             {
-                Utils::xacro::ExecutionOutcome outcome = Utils::xacro::ParseXacro(m_urdfPath.String(), m_params, parserConfig, sdfBuilderSettings);
+                Utils::xacro::ExecutionOutcome outcome =
+                    Utils::xacro::ParseXacro(m_urdfPath.String(), m_params, parserConfig, sdfBuilderSettings);
                 // Store off the URDF parsing outcome which will be output later in this function
                 parsedSdfOutcome = AZStd::move(outcome.m_urdfHandle);
                 if (outcome)
@@ -158,15 +207,7 @@ namespace ROS2
             {
                 if (urdfParsedWithWarnings)
                 {
-                    // This is a URDF only path, and therefore the report text does not mention SDF
-                    report += "# " + tr("The URDF was parsed, though results were modified to be compatible with SDFormat") + "\n";
-                    report += tr("Modified tags in URDF:") + "\n";
-                    for (const auto& modifiedTag : parsedSdfOutcome.m_modifiedURDFTags)
-                    {
-                        report += " - " + QString::fromUtf8(modifiedTag.data(), static_cast<int>(modifiedTag.size())) + "\n";
-                    }
-                    report += "\n# " + tr("The modified URDF code:") + "\n";
-                    report += "```\n" + QString::fromStdString(parsedSdfOutcome.m_modifiedURDFContent) + "```\n";
+                    AddModificationWarningsToReportString(report, parsedSdfOutcome);
                 }
                 else
                 {

diff --git a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.h b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.h
@@ -20,6 +20,7 @@
 #include "URDF/UrdfParser.h"
 #include <AzCore/Asset/AssetCommon.h>
 #include <AzCore/std/containers/unordered_map.h>
+#include <RobotImporter/FixURDF/URDFModifications.h>
 #include <RobotImporter/Utils/RobotImporterUtils.h>
 #include <RobotImporter/xacro/XacroUtils.h>
 
@@ -96,6 +97,8 @@ namespace ROS2
         //! @param errorMessage error message to display to the user
         void ReportError(const QString& errorMessage);
 
+        void AddModificationWarningsToReportString(QString& report, const UrdfParser::RootObjectOutcome& parsedSdfOutcome);
+
         static constexpr QWizard::WizardButton PrefabCreationButtonId{ QWizard::CustomButton1 };
         static constexpr QWizard::WizardOption HavePrefabCreationButton{ QWizard::HaveCustomButton1 };
     };

diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.cpp
@@ -88,7 +88,8 @@ namespace ROS2::UrdfParser
 
     bool ParseResult::UrdfParsedWithModifiedContent() const
     {
-        return m_modifiedURDFTags.size() > 0;
+        return m_urdfModifications.duplicatedJoints.size() > 0 || m_urdfModifications.missingInertias.size() > 0 ||
+            m_urdfModifications.incompleteInertias.size() > 0;
     }
 
     RootObjectOutcome Parse(AZStd::string_view xmlString, const sdf::ParserConfig& parserConfig)
@@ -141,12 +142,11 @@ namespace ROS2::UrdfParser
             auto [modifiedXmlStr, modifiedElements] = (ROS2::Utils::ModifyURDFInMemory(xmlStr));
 
             auto result = Parse(modifiedXmlStr, parserConfig);
-            result.m_modifiedURDFTags = AZStd::move(modifiedElements);
+            result.m_urdfModifications = AZStd::move(modifiedElements);
             result.m_modifiedURDFContent = AZStd::move(modifiedXmlStr);
             return result;
         }
         return Parse(xmlStr, parserConfig);
     }
 
-
 } // namespace ROS2::UrdfParser
diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.h b/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.h
@@ -11,6 +11,7 @@
 #include <AzCore/IO/Path/Path.h>
 #include <AzCore/std/containers/vector.h>
 #include <AzCore/std/utility/expected.h>
+#include <RobotImporter/FixURDF/URDFModifications.h>
 #include <SdfAssetBuilder/SdfAssetBuilderSettings.h>
 #include <sdf/Collision.hh>
 #include <sdf/Geometry.hh>
@@ -80,8 +81,8 @@ namespace ROS2::UrdfParser
         //! Stores the modified URDF content after parsing, empty if no modification occurred
         std::string m_modifiedURDFContent;
 
-        //! Stores the modified URDF tags after parsing, empty if no modification occurred
-        AZStd::vector<AZStd::string> m_modifiedURDFTags;
+        //! Stores description of URDF modifications, empty if no modification occurred
+        Utils::UrdfModifications m_urdfModifications;
     };
     using RootObjectOutcome = ParseResult;
 

diff --git a/Gems/ROS2/Code/Source/RobotImporter/xacro/XacroUtils.cpp b/Gems/ROS2/Code/Source/RobotImporter/xacro/XacroUtils.cpp
@@ -98,7 +98,7 @@ namespace ROS2::Utils::xacro
                 auto [modifiedXmlStr, modifiedElements] = (ROS2::Utils::ModifyURDFInMemory(output));
                 outcome.m_urdfHandle = UrdfParser::Parse(modifiedXmlStr, parserConfig);
                 outcome.m_urdfHandle.m_modifiedURDFContent = AZStd::move(modifiedXmlStr);
-                outcome.m_urdfHandle.m_modifiedURDFTags = AZStd::move(modifiedElements);
+                outcome.m_urdfHandle.m_urdfModifications = AZStd::move(modifiedElements);
                 outcome.m_succeed = true;
             }
             else