diff --git a/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.cpp b/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.cpp index 42c81488d8..645cb5e4c8 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.cpp @@ -1,10 +1,10 @@ /* -* 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 -* -*/ + * 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 + * + */ #include "FixURDF.h" #include @@ -25,9 +25,8 @@ namespace ROS2::Utils } } - for (auto * link : links) + for (auto* link : links) { - xml_node<>* inertial = urdf->document()->allocate_node(node_element, "inertial"); xml_node<>* mass = urdf->document()->allocate_node(node_element, "mass"); @@ -59,7 +58,6 @@ namespace ROS2::Utils modifiedLinks.push_back(name->value()); } link->append_node(inertial); - } return modifiedLinks; @@ -90,17 +88,18 @@ namespace ROS2::Utils linkAndJointsName.insert(newName); modifiedLinks.push_back(newName); } - else{ + else + { linkAndJointsName.insert(name->value()); } - } } return modifiedLinks; } - std::string ModifyURDFInMemory(const std::string& data) + AZStd::pair> ModifyURDFInMemory(const std::string& data) { + AZStd::vector modifiedElements; using namespace AZ::rapidxml; xml_document<> doc; doc.parse<0>(const_cast(data.c_str())); @@ -112,7 +111,8 @@ namespace ROS2::Utils AZ_Warning("ROS2", false, "Added missing inertia to links: "); for (auto& link : links) { - AZ_Warning("ROS2", false, " -> %s", link.c_str()); + modifiedElements.push_back(link); + AZ_Warning("ROS2", false, " -> %s", link.c_str()); } } @@ -122,13 +122,14 @@ namespace ROS2::Utils AZ_Warning("ROS2", false, "Renamed duplicated links and joints: "); for (auto& dup : renames) { - AZ_Warning("ROS2", false, " -> %s", dup.c_str()); + modifiedElements.push_back(dup); + AZ_Warning("ROS2", false, " -> %s", dup.c_str()); } } std::string xmlDocString; AZ::rapidxml::print(std::back_inserter(xmlDocString), *urdf, 0); - return xmlDocString; + return { xmlDocString, modifiedElements }; } } // namespace ROS2::Utils diff --git a/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.h b/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.h index 95395deae0..eb5762b032 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.h +++ b/Gems/ROS2/Code/Source/RobotImporter/FixURDF/FixURDF.h @@ -1,33 +1,35 @@ /* -* 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 -* -*/ + * 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 #include - +#include 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); + 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 names of links and joints to be unique, circle navigate SDF error 2 - //! @param urdf - the in memory URDF to modify - //! @returns a list of links that were modified - AZStd::vector ChangeDuplications(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 + //! @returns a list of links that were modified + AZStd::vector ChangeDuplications(AZ::rapidxml::xml_node<>* urdf); - std::string ModifyURDFInMemory(const std::string& data); + //! Modifies in memory URDF to add missing elements + //! @param urdf - the in memory 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 Utils } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckUrdfPage.cpp b/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckUrdfPage.cpp index 2c68a830e3..16a2a014ab 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckUrdfPage.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckUrdfPage.cpp @@ -14,6 +14,7 @@ namespace ROS2 CheckUrdfPage::CheckUrdfPage(QWizard* parent) : QWizardPage(parent) , m_success(false) + , m_warning(false) { m_log = new QTextEdit(this); setTitle(tr("URDF opening results:")); @@ -24,10 +25,11 @@ namespace ROS2 setLayout(layout); } - void CheckUrdfPage::ReportURDFResult(const QString& status, bool isSuccess) + void CheckUrdfPage::ReportURDFResult(const QString& status, bool isSuccess, bool isWarning) { m_log->setMarkdown(status); m_success = isSuccess; + m_warning = isWarning; emit completeChanged(); } @@ -35,4 +37,9 @@ namespace ROS2 { return m_success; } + + bool CheckUrdfPage::isWarning() const + { + return m_warning; + } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckUrdfPage.h b/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckUrdfPage.h index 7877f4af36..293ffc09ea 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckUrdfPage.h +++ b/Gems/ROS2/Code/Source/RobotImporter/Pages/CheckUrdfPage.h @@ -22,12 +22,14 @@ namespace ROS2 Q_OBJECT public: explicit CheckUrdfPage(QWizard* parent); - void ReportURDFResult(const QString& result, bool isSuccess); + void ReportURDFResult(const QString& result, bool isSuccess, bool isWarning = false); bool isComplete() const override; + bool isWarning() const; private: QTextEdit* m_log; QString m_fileName; bool m_success; + bool m_warning; }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp b/Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp index 65d6bf5669..61506ee28b 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp @@ -106,7 +106,7 @@ namespace ROS2 sdf::ParserConfig parserConfig; parserConfig.URDFSetPreserveFixedJoint(sdfBuilderSettings.m_urdfPreserveFixedJoints); - auto parsedUrdfOutcome = UrdfParser::ParseFromFile(filePath, parserConfig); + auto parsedUrdfOutcome = UrdfParser::ParseFromFile(filePath, parserConfig, sdfBuilderSettings); if (!parsedUrdfOutcome) { const AZStd::string log = Utils::JoinSdfErrorsToString(parsedUrdfOutcome.GetSdfErrors()); diff --git a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp index 9555925c59..ca1d5726fc 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp @@ -12,15 +12,15 @@ #include #include "RobotImporterWidget.h" +#include +#include +#include #include #include #include +#include #include #include -#include -#include -#include -#include namespace ROS2 { @@ -119,8 +119,7 @@ namespace ROS2 report += "```\n"; if (outcome.m_logErrorOutput.size()) { - report += - QString::fromUtf8(outcome.m_logErrorOutput.data(), static_cast(outcome.m_logErrorOutput.size())); + report += QString::fromUtf8(outcome.m_logErrorOutput.data(), static_cast(outcome.m_logErrorOutput.size())); } else { @@ -131,8 +130,8 @@ namespace ROS2 report += "```\n"; if (outcome.m_logStandardOutput.size()) { - report += QString::fromUtf8( - outcome.m_logStandardOutput.data(), static_cast(outcome.m_logStandardOutput.size())); + report += + QString::fromUtf8(outcome.m_logStandardOutput.data(), static_cast(outcome.m_logStandardOutput.size())); } else { @@ -147,7 +146,7 @@ namespace ROS2 else if (Utils::IsFileUrdf(m_urdfPath)) { // standard URDF - parsedUrdfOutcome = UrdfParser::ParseFromFile(m_urdfPath, parserConfig); + parsedUrdfOutcome = UrdfParser::ParseFromFile(m_urdfPath, parserConfig, sdfBuilderSettings); } else { @@ -155,13 +154,28 @@ namespace ROS2 } AZStd::string log; bool urdfParsedSuccess{ parsedUrdfOutcome }; + bool urdfParsedWithWarnings{ parsedUrdfOutcome.m_modifiedURDFContent.size() > 0}; if (urdfParsedSuccess) { + if (urdfParsedWithWarnings) + { + report += "# " + tr("The URDF was parsed, but it was modified to be compatible") + "\n"; + report += tr("Modified tags in URDF:") + "\n"; + for (const auto& modifiedTag : parsedUrdfOutcome.m_modifiedURDFTags) + { + report += " - " + QString::fromUtf8(modifiedTag.data(), static_cast(modifiedTag.size())) + "\n"; + } + report += "\n# "+tr("The modified URDF code:") + "\n"; + report += "```\n" + QString::fromStdString(parsedUrdfOutcome.m_modifiedURDFContent) + "```\n"; + } + else + { + report += "# " + tr("The URDF was parsed and opened successfully") + "\n"; + AZ_Printf("Wizard", "Wizard skips m_checkUrdfPage since there is no errors in URDF\n"); + } m_parsedUrdf = AZStd::move(parsedUrdfOutcome.GetRoot()); - report += "# " + tr("The URDF was parsed and opened successfully") + "\n"; m_prefabMaker.reset(); // Report the status of skipping this page - AZ_Printf("Wizard", "Wizard skips m_checkUrdfPage since there is no errors in URDF\n"); m_meshNames = Utils::GetMeshesFilenames(&m_parsedUrdf, true, true); m_assetPage->ClearAssetsList(); } @@ -177,7 +191,7 @@ namespace ROS2 report += QString::fromUtf8(log.data(), int(log.size())); report += "`"; } - m_checkUrdfPage->ReportURDFResult(report, urdfParsedSuccess); + m_checkUrdfPage->ReportURDFResult(report, urdfParsedSuccess, urdfParsedWithWarnings); } } @@ -319,8 +333,7 @@ namespace ROS2 if (currentPage() == m_xacroParamsPage) { m_params = m_xacroParamsPage->GetXacroParameters(); - if (const bool isFileUrdfOrXacro = Utils::IsFileXacro(m_urdfPath) || Utils::IsFileUrdf(m_urdfPath); - isFileUrdfOrXacro) + if (const bool isFileUrdfOrXacro = Utils::IsFileXacro(m_urdfPath) || Utils::IsFileUrdf(m_urdfPath); isFileUrdfOrXacro) { OpenUrdf(); } @@ -350,6 +363,10 @@ namespace ROS2 { if ((currentPage() == m_fileSelectPage && m_params.empty()) || currentPage() == m_xacroParamsPage) { + if (m_parsedUrdf.Model() != nullptr && m_checkUrdfPage->isWarning()) + { + return m_xacroParamsPage->nextId(); + } if (m_parsedUrdf.Model() != nullptr && m_checkUrdfPage->isComplete()) { if (m_meshNames.size() == 0) diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.cpp index 8386567e86..15097666ec 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.cpp @@ -14,6 +14,7 @@ #include #include #include +#include namespace ROS2::UrdfParser { @@ -26,8 +27,7 @@ namespace ROS2::UrdfParser // @param consoleStream Reference to sdf::Console::ConsoleStream whose // output will be redirected // @param redirectStream reference to stream where console stream output is redirected to - RedirectSDFOutputStream(sdf::Console::ConsoleStream& consoleStream, - std::ostream& redirectStream) + RedirectSDFOutputStream(sdf::Console::ConsoleStream& consoleStream, std::ostream& redirectStream) : m_consoleStreamRef(consoleStream) , m_origConsoleStream(consoleStream) { @@ -51,7 +51,7 @@ namespace ROS2::UrdfParser { return m_root; } - const sdf::Root& ParseResult::GetRoot() const & + const sdf::Root& ParseResult::GetRoot() const& { return m_root; } @@ -106,7 +106,8 @@ namespace ROS2::UrdfParser return parseResult; } - RootObjectOutcome ParseFromFile(AZ::IO::PathView filePath, const sdf::ParserConfig& parserConfig) + RootObjectOutcome ParseFromFile( + AZ::IO::PathView filePath, const sdf::ParserConfig& parserConfig, const SdfAssetBuilderSettings& settings) { // Store path in a AZ::IO::FixedMaxPath which is stack based structure that provides memory // for the path string and is null terminated. @@ -117,15 +118,24 @@ namespace ROS2::UrdfParser { auto fileNotFoundMessage = AZStd::fixed_string<1024>::format("File %.*s does not exist", AZ_PATH_ARG(urdfFilePath)); ParseResult fileNotFoundResult; - fileNotFoundResult.m_sdfErrors.emplace_back(sdf::ErrorCode::FILE_READ, - std::string{ fileNotFoundMessage.c_str(), fileNotFoundMessage.size() }, - std::string{ urdfFilePath.c_str(), urdfFilePath.Native().size() }); + fileNotFoundResult.m_sdfErrors.emplace_back( + sdf::ErrorCode::FILE_READ, + std::string{ fileNotFoundMessage.c_str(), fileNotFoundMessage.size() }, + std::string{ urdfFilePath.c_str(), urdfFilePath.Native().size() }); return fileNotFoundResult; } std::string xmlStr((std::istreambuf_iterator(istream)), std::istreambuf_iterator()); // modify in memory - std::string xmlStrChanged = (ROS2::Utils::ModifyURDFInMemory(xmlStr)); - return Parse(xmlStrChanged, parserConfig); + if (Utils::IsFileUrdf(filePath) && settings.m_fixURDF) + { + const auto& [modifiedXmlStr, modifiedElements] = (ROS2::Utils::ModifyURDFInMemory(xmlStr)); + + auto result = Parse(modifiedXmlStr, parserConfig); + result.m_modifiedURDFTags = modifiedElements; + result.m_modifiedURDFContent = modifiedXmlStr; + return result; + } + return Parse(xmlStr, parserConfig); } -} // namespace ROS2 +} // namespace ROS2::UrdfParser diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.h b/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.h index 376f120cdf..e1633ee68e 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.h +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.h @@ -10,16 +10,17 @@ #include #include -#include +#include +#include +#include +#include +#include #include -#include #include +#include #include -#include -#include +#include #include -#include -#include namespace ROS2 { @@ -64,6 +65,11 @@ namespace ROS2 sdf::Root m_root; AZStd::string m_parseMessages; 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; }; using RootObjectOutcome = ParseResult; @@ -83,7 +89,8 @@ namespace ROS2 //! The relevant ParserConfig functions for URDF importing are //! URDFPreserveFixedJoint() function to prevent merging of robot links bound by fixed joint //! 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); + 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/SdfAssetBuilder.cpp b/Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilder.cpp index c47336d213..e70461ccb0 100644 --- a/Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilder.cpp +++ b/Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilder.cpp @@ -194,7 +194,7 @@ namespace ROS2 parserConfig.URDFSetPreserveFixedJoint(m_globalSettings.m_urdfPreserveFixedJoints); AZ_Info(SdfAssetBuilderName, "Parsing source file: %s", fullSourcePath.c_str()); - auto parsedSdfRootOutcome = UrdfParser::ParseFromFile(fullSourcePath, parserConfig); + auto parsedSdfRootOutcome = UrdfParser::ParseFromFile(fullSourcePath, parserConfig, m_globalSettings); if (!parsedSdfRootOutcome) { const AZStd::string sdfParseErrors = Utils::JoinSdfErrorsToString(parsedSdfRootOutcome.GetSdfErrors()); @@ -257,7 +257,7 @@ namespace ROS2 // Read in and parse the source SDF file. AZ_Info(SdfAssetBuilderName, "Parsing source file: %s", request.m_fullPath.c_str()); - auto parsedSdfRootOutcome = UrdfParser::ParseFromFile(AZ::IO::PathView(request.m_fullPath), parserConfig); + auto parsedSdfRootOutcome = UrdfParser::ParseFromFile(AZ::IO::PathView(request.m_fullPath), parserConfig, m_globalSettings); if (!parsedSdfRootOutcome) { const AZStd::string sdfParseErrors = Utils::JoinSdfErrorsToString(parsedSdfRootOutcome.GetSdfErrors()); diff --git a/Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilderSettings.cpp b/Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilderSettings.cpp index 2484566d7e..056e4dce0a 100644 --- a/Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilderSettings.cpp +++ b/Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilderSettings.cpp @@ -51,6 +51,7 @@ namespace ROS2 constexpr auto SdfAssetBuilderUseArticulationsRegistryKey = SDFSettingsRootKey("UseArticulations"); constexpr auto SdfAssetBuilderURDFPreserveFixedJointRegistryKey = SDFSettingsRootKey("URDFPreserveFixedJoint"); constexpr auto SdfAssetBuilderImportMeshesJointRegistryKey = SDFSettingsRootKey("ImportMeshes"); + constexpr auto SdfAssetBuilderFixURDFRegistryKey = SDFSettingsRootKey("FixURDF"); } void SdfAssetBuilderSettings::Reflect(AZ::ReflectContext* context) @@ -62,6 +63,7 @@ namespace ROS2 ->Field("UseArticulations", &SdfAssetBuilderSettings::m_useArticulations) ->Field("URDFPreserveFixedJoint", &SdfAssetBuilderSettings::m_urdfPreserveFixedJoints) ->Field("ImportReferencedMeshFiles", &SdfAssetBuilderSettings::m_importReferencedMeshFiles) + ->Field("FixURDF", &SdfAssetBuilderSettings::m_fixURDF) // m_builderPatterns aren't serialized because we only use the serialization // to detect when global settings changes cause us to rebuild our assets. @@ -90,7 +92,13 @@ namespace ROS2 AZ::Edit::UIHandlers::Default, &SdfAssetBuilderSettings::m_importReferencedMeshFiles, "Import meshes", - "Allows importing of referenced mesh content files such as .dae or .stl files when importing the URDF/SDF."); + "Allows importing of referenced mesh content files such as .dae or .stl files when importing the URDF/SDF.") + ->DataElement( + 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." + ); } } } @@ -113,6 +121,9 @@ namespace ROS2 // Query the import references meshes option from the Settings Registry to determine if mesh source assets are copied settingsRegistry->Get(m_importReferencedMeshFiles, SdfAssetBuilderImportMeshesJointRegistryKey); + // Query the fix URDF option from the Settings Registry to determine if the URDF file should be fixed before importing + settingsRegistry->Get(m_fixURDF, SdfAssetBuilderFixURDFRegistryKey); + // Visit each supported file type extension and create an asset builder wildcard pattern for it. auto VisitFileTypeExtensions = [&settingsRegistry, this] (const AZ::SettingsRegistryInterface::VisitArgs& visitArgs) diff --git a/Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilderSettings.h b/Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilderSettings.h index 04f02dbb81..2e50b2a6ce 100644 --- a/Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilderSettings.h +++ b/Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilderSettings.h @@ -36,5 +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 + bool m_fixURDF = true; }; } // namespace ROS2 diff --git a/Gems/ROS2/Registry/sdfassetbuilder_settings.setreg b/Gems/ROS2/Registry/sdfassetbuilder_settings.setreg index c1f2e8c923..7f47181327 100644 --- a/Gems/ROS2/Registry/sdfassetbuilder_settings.setreg +++ b/Gems/ROS2/Registry/sdfassetbuilder_settings.setreg @@ -13,7 +13,7 @@ "xacro" ], "UseArticulations": true, - "URDFPreserveFixedJoint": false + "URDFPreserveFixedJoint": true } } }