From 3821f0f3e3242a9a9f48cf4be3c2388f0a17f02c Mon Sep 17 00:00:00 2001 From: "Signed-off-by: Kacper Lewandowski" Date: Mon, 2 Oct 2023 11:49:44 +0200 Subject: [PATCH] display explanations of modifications made to URDF (#518) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * 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 Co-authored-by: Jan Hanca <134940295+jhanca-robotecai@users.noreply.github.com> Signed-off-by: Michał Pełka --- .../Source/RobotImporter/FixURDF/FixURDF.cpp | 72 ++++++++++++------- .../Source/RobotImporter/FixURDF/FixURDF.h | 5 +- .../RobotImporter/FixURDF/URDFModifications.h | 41 +++++++++++ .../RobotImporter/RobotImporterWidget.cpp | 61 +++++++++++++--- .../RobotImporter/RobotImporterWidget.h | 3 + .../Source/RobotImporter/URDF/UrdfParser.cpp | 6 +- .../Source/RobotImporter/URDF/UrdfParser.h | 5 +- .../Source/RobotImporter/xacro/XacroUtils.cpp | 2 +- 8 files changed, 153 insertions(+), 42 deletions(-) create mode 100644 Gems/ROS2/Code/Source/RobotImporter/FixURDF/URDFModifications.h diff --git a/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.cpp b/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.cpp index 64660897b..749dae5db 100644 --- 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 #include #include @@ -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 AddMissingInertiaToLinks(AZ::rapidxml::xml_node<>* urdf) + AZStd::pair, AZStd::vector> AddMissingInertiaToLinks(AZ::rapidxml::xml_node<>* urdf) { - AZStd::vector modifiedLinks; + AZStd::vector missingInertias; + AZStd::vector 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,15 +89,16 @@ 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) @@ -90,10 +106,10 @@ namespace ROS2::Utils //! 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 RenameDuplicatedJoints(AZ::rapidxml::xml_node<>* urdf) + AZStd::vector RenameDuplicatedJoints(AZ::rapidxml::xml_node<>* urdf) { using namespace AZ::rapidxml; - AZStd::vector modifiedLinks; + AZStd::vector duplicatedJoints; AZStd::unordered_map 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> ModifyURDFInMemory(const AZStd::string& data) + AZStd::pair ModifyURDFInMemory(const AZStd::string& data) { - AZStd::vector modifiedElements; + UrdfModifications modifiedElements; using namespace AZ::rapidxml; xml_document<> doc; doc.parse<0>(const_cast(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> ModifyURDFInMemory(const std::string& data) + AZStd::pair 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 index ec61ee3fc..d33f062cc 100644 --- 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 #include @@ -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> ModifyURDFInMemory(const std::string& data); - AZStd::pair> ModifyURDFInMemory(const AZStd::string& data); + AZStd::pair ModifyURDFInMemory(const std::string& data); + AZStd::pair 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 new file mode 100644 index 000000000..579cd70f8 --- /dev/null +++ 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 +#include + +namespace ROS2::Utils +{ + struct MissingInertia + { + AZStd::string linkName; + + ~MissingInertia() = default; + }; + + struct IncompleteInertia + { + AZStd::string linkName; + AZStd::vector missingTags; + }; + + struct DuplicatedJoint + { + AZStd::string oldName; + AZStd::string newName; + }; + + struct UrdfModifications + { + AZStd::vector missingInertias; + AZStd::vector incompleteInertias; + AZStd::vector duplicatedJoints; + }; + +} // namespace ROS2::Utils diff --git a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp index 417238769..548710a78 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp @@ -11,6 +11,7 @@ #include #include +#include "FixURDF/URDFModifications.h" #include "RobotImporterWidget.h" #include #include @@ -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(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(modifiedTag.linkName.size())) + ": "; + + for (const auto& tag : modifiedTag.missingTags) + { + report += QString::fromUtf8(tag.data(), static_cast(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(modifiedTag.oldName.size())) + " -> " + + QString::fromUtf8(modifiedTag.newName.data(), static_cast(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(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 index 1b611b5f7..57f9c49e8 100644 --- 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 #include +#include #include #include @@ -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 index 6075aa51c..00f399232 100644 --- 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 index 139a85d4e..7231c6169 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.h +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.h @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -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 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 index a9adf3314..5418cf6de 100644 --- 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