diff --git a/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.cpp b/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.cpp index 645cb5e4c8..8fd6e4ada2 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.cpp @@ -8,11 +8,12 @@ #include "FixURDF.h" #include -#include +#include #include + namespace ROS2::Utils { - AZStd::vector AddMissingInertiaToLink(AZ::rapidxml::xml_node<>* urdf) + AZStd::vector AddMissingInertiaToLinks(AZ::rapidxml::xml_node<>* urdf) { AZStd::vector modifiedLinks; using namespace AZ::rapidxml; @@ -67,7 +68,7 @@ namespace ROS2::Utils { using namespace AZ::rapidxml; AZStd::vector modifiedLinks; - AZStd::set linkAndJointsName; + AZStd::unordered_set linkAndJointsName; for (xml_node<>* link = urdf->first_node("link"); link; link = link->next_sibling("link")) { auto* name = link->first_attribute("name"); @@ -86,7 +87,7 @@ namespace ROS2::Utils auto newName = AZStd::string(name->value()) + "_joint_dup"; name->value(urdf->document()->allocate_string(newName.c_str())); linkAndJointsName.insert(newName); - modifiedLinks.push_back(newName); + modifiedLinks.push_back(AZStd::move(newName)); } else { @@ -104,7 +105,7 @@ namespace ROS2::Utils xml_document<> doc; doc.parse<0>(const_cast(data.c_str())); xml_node<>* urdf = doc.first_node("robot"); - auto links = AddMissingInertiaToLink(urdf); + auto links = AddMissingInertiaToLinks(urdf); if (links.size()) { diff --git a/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.h b/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.h index eb5762b032..1dbbb0b047 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.h +++ b/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.h @@ -16,20 +16,20 @@ namespace ROS2 { namespace Utils { - //! Modifies in memory URDF to add missing inertia to links, circle navigate SDF error 19 - //! @param urdf - the in memory URDF to modify - //! @returns a list of links that were modified - AZStd::vector AddMissingInertiaToLink(AZ::rapidxml::xml_node<>* urdf); + //! 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); - //! Modifies names of links and joints to be unique, circle navigate SDF error 2 - //! @param urdf - the in memory URDF to modify + //! Modifies names of links and joints to be unique, which means that a set of both contains no duplicates. This prevents SDF + //! error 2. + //! @param urdf URDF to modify. //! @returns a list of links that were modified AZStd::vector ChangeDuplications(AZ::rapidxml::xml_node<>* urdf); //! Modifies in memory URDF to add missing elements - //! @param urdf - the in memory URDF to modify + //! @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); - } // namespace Utils } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp index ca1d5726fc..8e69a57c89 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp @@ -153,13 +153,13 @@ namespace ROS2 AZ_Assert(false, "Unknown file extension : %s \n", m_urdfPath.c_str()); } AZStd::string log; - bool urdfParsedSuccess{ parsedUrdfOutcome }; - bool urdfParsedWithWarnings{ parsedUrdfOutcome.m_modifiedURDFContent.size() > 0}; + const bool urdfParsedSuccess{ parsedUrdfOutcome }; + const bool urdfParsedWithWarnings{ parsedUrdfOutcome.UrdfParsedWithModifiedContent() }; if (urdfParsedSuccess) { if (urdfParsedWithWarnings) { - report += "# " + tr("The URDF was parsed, but it was modified to be compatible") + "\n"; + 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 : parsedUrdfOutcome.m_modifiedURDFTags) { diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.cpp index 15097666ec..a004ad6cf3 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.cpp @@ -14,7 +14,7 @@ #include #include #include -#include +#include namespace ROS2::UrdfParser { @@ -85,6 +85,11 @@ namespace ROS2::UrdfParser return m_sdfErrors.empty(); } + bool ParseResult::UrdfParsedWithModifiedContent() const + { + return m_modifiedURDFTags.size() > 0; + } + RootObjectOutcome Parse(AZStd::string_view xmlString, const sdf::ParserConfig& parserConfig) { return Parse(std::string(xmlString.data(), xmlString.size()), parserConfig); @@ -126,16 +131,18 @@ namespace ROS2::UrdfParser } std::string xmlStr((std::istreambuf_iterator(istream)), std::istreambuf_iterator()); - // modify in memory if (Utils::IsFileUrdf(filePath) && settings.m_fixURDF) { - const auto& [modifiedXmlStr, modifiedElements] = (ROS2::Utils::ModifyURDFInMemory(xmlStr)); + // modify in memory + auto [modifiedXmlStr, modifiedElements] = (ROS2::Utils::ModifyURDFInMemory(xmlStr)); auto result = Parse(modifiedXmlStr, parserConfig); - result.m_modifiedURDFTags = modifiedElements; - result.m_modifiedURDFContent = modifiedXmlStr; + result.m_modifiedURDFTags = 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 e1633ee68e..81e07e4632 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.h +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.h @@ -9,6 +9,7 @@ #pragma once #include +#include #include #include #include @@ -34,8 +35,10 @@ namespace ROS2 private: // Provides custom sdf::ErrorCode values when parsing is done through O3DE inline static constexpr auto O3DESdfErrorCodeStart = static_cast(1000); + public: - inline static constexpr auto O3DESdfErrorParseNotStarted = static_cast(static_cast(O3DESdfErrorCodeStart) + 1); + inline static constexpr auto O3DESdfErrorParseNotStarted = + static_cast(static_cast(O3DESdfErrorCodeStart) + 1); //! Ref qualifier overloads for retrieving sdf::Root //! it supports a non-const lvalue overload to allow @@ -62,12 +65,16 @@ namespace ROS2 //! Returns if the parsing of the SDF file has succeeded explicit operator bool() const; + //! Returns if the URDF content was modified during parsing + bool UrdfParsedWithModifiedContent() const; + sdf::Root m_root; AZStd::string m_parseMessages; - sdf::Errors m_sdfErrors{ sdf::Error{ O3DESdfErrorParseNotStarted, std::string{"No Parsing has occurred yet"}} }; + sdf::Errors m_sdfErrors{ sdf::Error{ O3DESdfErrorParseNotStarted, std::string{ "No Parsing has occurred yet" } } }; //! 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; }; @@ -91,6 +98,7 @@ namespace ROS2 //! AddURIPath() function to provide a mapping of package:// and model:// references to the local filesystem //! @paragraph settings structure that contains configuration options for the SDFAssetBuilder //! @return SDF root object containing parsed or tags - RootObjectOutcome ParseFromFile(AZ::IO::PathView filePath, const sdf::ParserConfig& parserConfig, const SdfAssetBuilderSettings& settings); + RootObjectOutcome ParseFromFile( + AZ::IO::PathView filePath, const sdf::ParserConfig& parserConfig, const SdfAssetBuilderSettings& settings); }; // namespace UrdfParser } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilderSettings.cpp b/Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilderSettings.cpp index 056e4dce0a..a5021ca538 100644 --- a/Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilderSettings.cpp +++ b/Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilderSettings.cpp @@ -97,7 +97,7 @@ namespace ROS2 AZ::Edit::UIHandlers::Default, &SdfAssetBuilderSettings::m_fixURDF, "Fix URDF to be compatible with libsdformat", - "When set, fixes the URDF file before importing it. This is useful for fixing URDF files that have missing inertials." + "When set, fixes the URDF file before importing it. This is useful for fixing URDF files that have missing inertials or duplicate names within links and joints." ); } } diff --git a/Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilderSettings.h b/Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilderSettings.h index 2e50b2a6ce..f5858dafd7 100644 --- a/Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilderSettings.h +++ b/Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilderSettings.h @@ -36,7 +36,7 @@ namespace ROS2 bool m_urdfPreserveFixedJoints = true; // When true, .dae/.stl mesh files are imported into the project folder to allow the AP to process them bool m_importReferencedMeshFiles = true; - // When true URDF will be fixed to be compatible with SDF + // When true URDF will be fixed to be compatible with SDFormat. bool m_fixURDF = true; }; } // namespace ROS2