Ver Fonte

Adding SDFAssetBuilderSettings Reflected Property Editor to the Robot Importer

An `SDFAssetBuilderSettings` object is being exposed via the File Select page in the Robot Importer to allow users to alter the preserve URDF fixed joints object.

Refactored the `UrdfParser::Parse` functions to support capturing the sdf::Console messages logged during URDF/SDF parsing.

Signed-off-by: lumberyard-employee-dm <[email protected]>
lumberyard-employee-dm há 1 ano atrás
pai
commit
e39fd6d9b1

+ 27 - 1
Gems/ROS2/Code/Source/RobotImporter/Pages/FileSelectionPage.cpp

@@ -8,6 +8,9 @@
 
 #include "FileSelectionPage.h"
 #include <AzCore/Utils/Utils.h>
+#include <AzToolsFramework/UI/PropertyEditor/ReflectedPropertyEditor.hxx>
+#include <SdfAssetBuilder/SdfAssetBuilderSettings.h>
+
 #include <QFileInfo>
 #include <QHBoxLayout>
 #include <QVBoxLayout>
@@ -16,6 +19,7 @@ namespace ROS2
 {
     FileSelectionPage::FileSelectionPage(QWizard* parent)
         : QWizardPage(parent)
+        , m_sdfAssetBuilderSettings(AZStd::make_unique<SdfAssetBuilderSettings>())
     {
         m_fileDialog = new QFileDialog(this);
         m_fileDialog->setDirectory(QString::fromUtf8(AZ::Utils::GetProjectPath().data()));
@@ -33,7 +37,22 @@ namespace ROS2
         layout_in->addWidget(m_textEdit);
         layout->addLayout(layout_in);
         layout->addWidget(m_copyFiles);
-        layout->addStretch();
+
+        // Seed the SDF Asset Builder Settings with the values from the Settings Registry
+        m_sdfAssetBuilderSettings->LoadSettings();
+        // Create a reflected property editor that can modify the SDF AssetBuilder Settings
+        m_sdfAssetBuilderSettingsEditor = new AzToolsFramework::ReflectedPropertyEditor(this);
+        AZ::ComponentApplicationRequests* componentApplicationRequests = AZ::Interface<AZ::ComponentApplicationRequests>::Get();
+        AZ::SerializeContext* serializeContext{ componentApplicationRequests ? componentApplicationRequests->GetSerializeContext() : nullptr };
+        AZ_Assert(serializeContext != nullptr, "Serialize Context is missing. It is required for the SDF Asset Builder Settings Editor");
+        constexpr bool enableScrollBars = true;
+        m_sdfAssetBuilderSettingsEditor->Setup(serializeContext, nullptr, enableScrollBars);
+        m_sdfAssetBuilderSettingsEditor->AddInstance(m_sdfAssetBuilderSettings.get());
+        m_sdfAssetBuilderSettingsEditor->InvalidateAll();
+        // Make sure the SDF Asset Builder settings are expanded by default
+        m_sdfAssetBuilderSettingsEditor->ExpandAll();
+        layout->addWidget(m_sdfAssetBuilderSettingsEditor);
+
         this->setLayout(layout);
         connect(m_button, &QPushButton::pressed, this, &FileSelectionPage::onLoadButtonPressed);
         connect(m_fileDialog, &QFileDialog::fileSelected, this, &FileSelectionPage::onFileSelected);
@@ -41,6 +60,8 @@ namespace ROS2
         FileSelectionPage::onEditingFinished();
     }
 
+    FileSelectionPage::~FileSelectionPage() = default;
+
     void FileSelectionPage::onLoadButtonPressed()
     {
         m_fileDialog->show();
@@ -70,4 +91,9 @@ namespace ROS2
     {
         return m_copyFiles->isChecked();
     }
+
+    const SdfAssetBuilderSettings& FileSelectionPage::GetSdfAssetBuilderSettings() const
+    {
+        return *m_sdfAssetBuilderSettings;
+    }
 } // namespace ROS2

+ 18 - 0
Gems/ROS2/Code/Source/RobotImporter/Pages/FileSelectionPage.h

@@ -18,14 +18,27 @@
 #include <QWizardPage>
 #endif
 
+namespace AzToolsFramework
+{
+    class ReflectedPropertyEditor;
+}
+
 namespace ROS2
 {
+    struct SdfAssetBuilderSettings;
+}
 
+
+namespace ROS2
+{
     class FileSelectionPage : public QWizardPage
     {
         Q_OBJECT
     public:
         explicit FileSelectionPage(QWizard* parent);
+        // The destructor is defaulted in the cpp file to allow the unique_ptr<SdfAssetBuilderSettings>
+        // to not need a complete type in the header
+        ~FileSelectionPage();
 
         bool isComplete() const override;
 
@@ -39,11 +52,16 @@ namespace ROS2
         }
         bool getIfCopyAssetsDuringUrdfImport() const;
 
+        const SdfAssetBuilderSettings& GetSdfAssetBuilderSettings() const;
+
     private:
         QFileDialog* m_fileDialog;
         QPushButton* m_button;
         QLineEdit* m_textEdit;
         QCheckBox* m_copyFiles;
+
+        AZStd::unique_ptr<SdfAssetBuilderSettings> m_sdfAssetBuilderSettings;
+        AzToolsFramework::ReflectedPropertyEditor* m_sdfAssetBuilderSettingsEditor{};
         void onLoadButtonPressed();
 
         void onFileSelected(const QString& file);

+ 4 - 9
Gems/ROS2/Code/Source/RobotImporter/Pages/PrefabMakerPage.cpp

@@ -13,6 +13,7 @@
 #include <AzCore/Math/Transform.h>
 #include <AzCore/std/smart_ptr/make_shared.h>
 #include <AzCore/std/string/string.h>
+
 #include <ROS2/Spawner/SpawnerBus.h>
 #include <ROS2/Spawner/SpawnerInfo.h>
 #include <RobotImporter/RobotImporterWidget.h>
@@ -22,11 +23,10 @@
 
 namespace ROS2
 {
-
     PrefabMakerPage::PrefabMakerPage(RobotImporterWidget* parent)
         : QWizardPage(parent)
-        , m_parentImporterWidget(parent)
         , m_success(false)
+        , m_parentImporterWidget(parent)
     {
         AZ::EBusAggregateResults<AZStd::unordered_map<AZStd::string, SpawnPointInfo>> allActiveSpawnPoints;
         SpawnerRequestsBus::BroadcastResult(allActiveSpawnPoints, &SpawnerRequestsBus::Events::GetAllSpawnPointInfos);
@@ -45,14 +45,13 @@ namespace ROS2
         m_prefabName = new QLineEdit(this);
         m_createButton = new QPushButton(tr("Create Prefab"), this);
         m_log = new QTextEdit(this);
-        m_useArticulation = new QCheckBox(tr("Use articulation for joints and rigid bodies"), this);
+
         setTitle(tr("Prefab creation"));
         QVBoxLayout* layout = new QVBoxLayout;
         QHBoxLayout* layoutInner = new QHBoxLayout;
         layoutInner->addWidget(m_prefabName);
         layoutInner->addWidget(m_createButton);
         layout->addLayout(layoutInner);
-        layout->addWidget(m_useArticulation);
         QLabel* spawnPointListLabel;
         if (allActiveSpawnPoints.values.size() == 0)
         {
@@ -69,6 +68,7 @@ namespace ROS2
         connect(m_createButton, &QPushButton::pressed, this, &PrefabMakerPage::onCreateButtonPressed);
     }
 
+
     void PrefabMakerPage::setProposedPrefabName(const AZStd::string prefabName)
     {
         m_prefabName->setText(QString::fromUtf8(prefabName.data(), int(prefabName.size())));
@@ -92,10 +92,6 @@ namespace ROS2
     {
         return m_success;
     }
-    bool PrefabMakerPage::IsUseArticulations() const
-    {
-        return m_useArticulation->isChecked();
-    }
     AZStd::optional<AZ::Transform> PrefabMakerPage::getSelectedSpawnPoint() const
     {
         if (!m_spawnPointsInfos.empty())
@@ -111,5 +107,4 @@ namespace ROS2
         }
         return AZStd::nullopt;
     }
-
 } // namespace ROS2

+ 0 - 2
Gems/ROS2/Code/Source/RobotImporter/Pages/PrefabMakerPage.h

@@ -36,7 +36,6 @@ namespace ROS2
         void reportProgress(const AZStd::string& progressForUser);
         void setSuccess(bool success);
         bool isComplete() const override;
-        bool IsUseArticulations() const;
         AZStd::optional<AZ::Transform> getSelectedSpawnPoint() const;
     Q_SIGNALS:
         void onCreateButtonPressed();
@@ -46,7 +45,6 @@ namespace ROS2
         QLineEdit* m_prefabName;
         QPushButton* m_createButton;
         QTextEdit* m_log;
-        QCheckBox* m_useArticulation;
         QComboBox* m_spawnPointsComboBox;
         AZStd::vector<SpawnPointInfoMap> m_spawnPointsInfos;
         RobotImporterWidget* m_parentImporterWidget;

+ 2 - 2
Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp

@@ -109,7 +109,7 @@ namespace ROS2
         auto parsedUrdfOutcome = UrdfParser::ParseFromFile(filePath, parserConfig);
         if (!parsedUrdfOutcome)
         {
-            const AZStd::string log = Utils::JoinSdfErrorsToString(parsedUrdfOutcome.error());
+            const AZStd::string log = Utils::JoinSdfErrorsToString(parsedUrdfOutcome.GetSdfErrors());
 
             AZ_Warning("ROS2RobotImporterEditorSystemComponent", false, "URDF parsing failed with errors:\nRefer to %s",
                 log.c_str());
@@ -117,7 +117,7 @@ namespace ROS2
         }
 
         // Urdf Root has been parsed successfully retrieve it from the Outcome
-        const sdf::Root& parsedUrdfRoot = parsedUrdfOutcome.value();
+        const sdf::Root& parsedUrdfRoot = parsedUrdfOutcome.GetRoot();
 
         auto collidersNames = Utils::GetMeshesFilenames(&parsedUrdfRoot, false, true);
         auto visualNames = Utils::GetMeshesFilenames(&parsedUrdfRoot, true, false);

+ 16 - 13
Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp

@@ -83,15 +83,22 @@ namespace ROS2
 
     void RobotImporterWidget::OpenUrdf()
     {
-        UrdfParser::RootObjectOutcome parsedUrdfOutcome(AZStd::unexpect);
+        UrdfParser::RootObjectOutcome parsedUrdfOutcome;
         QString report;
         if (!m_urdfPath.empty())
         {
+            // Read the SDF Settings from PrefabMakerPage
+            const SdfAssetBuilderSettings& sdfBuilderSettings = m_fileSelectPage->GetSdfAssetBuilderSettings();
+
+            // Set the parser config settings for URDF content
+            sdf::ParserConfig parserConfig;
+            parserConfig.URDFSetPreserveFixedJoint(sdfBuilderSettings.m_urdfPreserveFixedJoints);
+
             if (Utils::IsFileXacro(m_urdfPath))
             {
-                Utils::xacro::ExecutionOutcome outcome = Utils::xacro::ParseXacro(m_urdfPath.String(), m_params);
+                Utils::xacro::ExecutionOutcome outcome = Utils::xacro::ParseXacro(m_urdfPath.String(), m_params, parserConfig);
                 // Store off the URDF parsing outcome which will be output later in this function
-                parsedUrdfOutcome = outcome.m_urdfHandle;
+                parsedUrdfOutcome = AZStd::move(outcome.m_urdfHandle);
                 if (outcome)
                 {
                     report += "# " + tr("XACRO execution succeeded") + "\n";
@@ -140,12 +147,6 @@ namespace ROS2
             else if (Utils::IsFileUrdf(m_urdfPath))
             {
                 // standard URDF
-                // Read the SDF Settings from the Settings Registry into a local struct
-                SdfAssetBuilderSettings sdfBuilderSettings;
-                sdfBuilderSettings.LoadSettings();
-                // Set the parser config settings for URDF content
-                sdf::ParserConfig parserConfig;
-                parserConfig.URDFSetPreserveFixedJoint(sdfBuilderSettings.m_urdfPreserveFixedJoints);
                 parsedUrdfOutcome = UrdfParser::ParseFromFile(m_urdfPath, parserConfig);
             }
             else
@@ -153,10 +154,10 @@ namespace ROS2
                 AZ_Assert(false, "Unknown file extension : %s \n", m_urdfPath.c_str());
             }
             AZStd::string log;
-            bool urdfParsedSuccess = parsedUrdfOutcome.has_value();
+            bool urdfParsedSuccess{ parsedUrdfOutcome };
             if (urdfParsedSuccess)
             {
-                m_parsedUrdf = AZStd::move(parsedUrdfOutcome.value());
+                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
@@ -166,7 +167,7 @@ namespace ROS2
             }
             else
             {
-                log = Utils::JoinSdfErrorsToString(parsedUrdfOutcome.error());
+                log = Utils::JoinSdfErrorsToString(parsedUrdfOutcome.GetSdfErrors());
                 report += "# " + tr("The URDF was not opened") + "\n";
                 report += tr("URDF parser returned following errors:") + "\n\n";
             }
@@ -395,7 +396,9 @@ namespace ROS2
                 return;
             }
         }
-        const bool useArticulation = m_prefabMakerPage->IsUseArticulations();
+
+        const auto& sdfAssetBuilderSettings = m_fileSelectPage->GetSdfAssetBuilderSettings();
+        const bool useArticulation = sdfAssetBuilderSettings.m_useArticulations;
         m_prefabMaker = AZStd::make_unique<URDFPrefabMaker>(
             m_urdfPath.String(),
             &m_parsedUrdf,

+ 86 - 10
Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.cpp

@@ -8,22 +8,101 @@
 
 #include "UrdfParser.h"
 
+#include <sstream>
+
 #include <AzCore/Debug/Trace.h>
 #include <AzCore/std/string/string.h>
+#include <RobotImporter/Utils/ErrorUtils.h>
 
 namespace ROS2::UrdfParser
 {
+    class RedirectSDFOutputStream
+    {
+    public:
+        // Copy the original console stream to restore in the destructor via RAII
+        // the console stream itself is referenced here and redirected to a local
+        // os stream by default
+        // @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)
+            : m_consoleStreamRef(consoleStream)
+            , m_origConsoleStream(consoleStream)
+        {
+            // Redirect the Output stream to the supplied
+            m_consoleStreamRef = sdf::Console::ConsoleStream(&redirectStream);
+        }
+
+        ~RedirectSDFOutputStream()
+        {
+            // Restore the original console stream
+            m_consoleStreamRef = AZStd::move(m_origConsoleStream);
+        }
+
+    private:
+        sdf::Console::ConsoleStream& m_consoleStreamRef;
+        sdf::Console::ConsoleStream m_origConsoleStream;
+    };
+
+    // Parser result member functions
+    sdf::Root& ParseResult::GetRoot() &
+    {
+        return m_root;
+    }
+    const sdf::Root& ParseResult::GetRoot() const &
+    {
+        return m_root;
+    }
+    sdf::Root&& ParseResult::GetRoot() &&
+    {
+        return AZStd::move(m_root);
+    }
+
+    const AZStd::string& ParseResult::GetParseMessages() const&
+    {
+        return m_parseMessages;
+    }
+
+    AZStd::string&& ParseResult::GetParseMessages() &&
+    {
+        return AZStd::move(m_parseMessages);
+    }
+
+    const sdf::Errors& ParseResult::GetSdfErrors() const&
+    {
+        return m_sdfErrors;
+    }
+
+    sdf::Errors&& ParseResult::GetSdfErrors() &&
+    {
+        return AZStd::move(m_sdfErrors);
+    }
+
+    ParseResult::operator bool() const
+    {
+        return m_sdfErrors.empty();
+    }
+
     RootObjectOutcome Parse(AZStd::string_view xmlString, const sdf::ParserConfig& parserConfig)
     {
         return Parse(std::string(xmlString.data(), xmlString.size()), parserConfig);
     }
     RootObjectOutcome Parse(const std::string& xmlString, const sdf::ParserConfig& parserConfig)
     {
-        sdf::Root root;
-        auto parseRootErrors = root.LoadSdfString(xmlString, parserConfig);
+        ParseResult parseResult;
+        std::ostringstream parseStringStream;
+        {
+            RedirectSDFOutputStream redirectConsoleStream(sdf::Console::Instance()->GetMsgStream(), parseStringStream);
+            parseResult.m_sdfErrors = parseResult.m_root.LoadSdfString(xmlString, parserConfig);
+            // Get any captured sdf::Error messages
+        }
+
+        std::string parseMessages = parseStringStream.str();
+        parseResult.m_parseMessages = AZStd::string(parseMessages.c_str(), parseMessages.size());
 
         // if there are no parse errors return the sdf Root object otherwise return the errors
-        return parseRootErrors.empty() ? RootObjectOutcome(root) : RootObjectOutcome(AZStd::unexpect, parseRootErrors);
+        return parseResult;
     }
 
     RootObjectOutcome ParseFromFile(AZ::IO::PathView filePath, const sdf::ParserConfig& parserConfig)
@@ -36,14 +115,11 @@ namespace ROS2::UrdfParser
         if (!istream)
         {
             auto fileNotFoundMessage = AZStd::fixed_string<1024>::format("File %.*s does not exist", AZ_PATH_ARG(urdfFilePath));
-            RootObjectOutcome fileNotExistOutcome;
-            fileNotExistOutcome = AZStd::unexpected<sdf::Errors>(AZStd::in_place,
-                {
-                    sdf::Error{ sdf::ErrorCode::FILE_READ,
+            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() } }
-                });
-            return fileNotExistOutcome;
+                        std::string{ urdfFilePath.c_str(), urdfFilePath.Native().size() });
+            return fileNotFoundResult;
         }
 
         std::string xmlStr((std::istreambuf_iterator<char>(istream)), std::istreambuf_iterator<char>());

+ 39 - 3
Gems/ROS2/Code/Source/RobotImporter/URDF/UrdfParser.h

@@ -27,9 +27,45 @@ namespace ROS2
     namespace UrdfParser
     {
         //! Stores the result of parsing URDF data
-        //! On success the sdf::Root object is returned
-        //! On failure the sdf::Errors vector is returned
-        using RootObjectOutcome = AZStd::expected<sdf::Root, sdf::Errors>;
+        //! The operator bool returns true if the sdf::Errors vector is empty
+        struct ParseResult
+        {
+        private:
+            // Provides custom sdf::ErrorCode values when parsing is done through O3DE
+            inline static constexpr auto O3DESdfErrorCodeStart = static_cast<sdf::ErrorCode>(1000);
+        public:
+           inline static constexpr auto O3DESdfErrorParseNotStarted = static_cast<sdf::ErrorCode>(static_cast<int>(O3DESdfErrorCodeStart) + 1);
+
+            //! Ref qualifer overloads for retrieving sdf::Root
+            //! it supports a non-const lvalue overload to allow
+            //! modification of the sdf::Root objec directly
+            sdf::Root& GetRoot() &;
+            const sdf::Root& GetRoot() const&;
+            sdf::Root&& GetRoot() &&;
+
+            //! Property getters for retrieving parsing messages
+            //! logged during libsdformat parsing of the URDF content
+            //! This does not support a non-const lvalue overload
+            //! As modification the result structure parser messages
+            //! have no need to be modified inplace
+            const AZStd::string& GetParseMessages() const&;
+            AZStd::string&& GetParseMessages() &&;
+
+            //! Returns the sdf::Error vector containing any parser errors
+            //! This does not support a non-const lvalue overload
+            //! As modification the result structure sdf Errors
+            //! have no need to be modified inplace
+            const sdf::Errors& GetSdfErrors() const&;
+            sdf::Errors&& GetSdfErrors() &&;
+
+            //! Returns if the parsing of the SDF file has succeeded
+            explicit operator bool() const;
+
+            sdf::Root m_root;
+            AZStd::string m_parseMessages;
+            sdf::Errors m_sdfErrors{ sdf::Error{ O3DESdfErrorParseNotStarted, std::string{"No Parsing has occured yet"}} };
+        };
+        using RootObjectOutcome = ParseResult;
 
         //! Parse string with URDF data and generate model.
         //! @param xmlString a string that contains URDF data (XML format).

+ 1 - 7
Gems/ROS2/Code/Source/RobotImporter/xacro/XacroUtils.cpp

@@ -18,7 +18,7 @@
 
 namespace ROS2::Utils::xacro
 {
-    ExecutionOutcome ParseXacro(const AZStd::string& filename, const Params& params)
+    ExecutionOutcome ParseXacro(const AZStd::string& filename, const Params& params, const sdf::ParserConfig& parserConfig)
     {
         ExecutionOutcome outcome;
         // test if xacro exists
@@ -90,12 +90,6 @@ namespace ROS2::Utils::xacro
         {
             AZ_Printf("ParseXacro", "xacro finished with success \n");
             const auto& output = process_output.outputResult;
-            // Read the SDF Settings from the Settings Registry into a local struct
-            SdfAssetBuilderSettings sdfBuilderSettings;
-            sdfBuilderSettings.LoadSettings();
-            // Set the parser config settings for URDF content
-            sdf::ParserConfig parserConfig;
-            parserConfig.URDFSetPreserveFixedJoint(sdfBuilderSettings.m_urdfPreserveFixedJoints);
             outcome.m_urdfHandle = UrdfParser::Parse(output, parserConfig);
             outcome.m_succeed = true;
         }

+ 1 - 1
Gems/ROS2/Code/Source/RobotImporter/xacro/XacroUtils.h

@@ -41,7 +41,7 @@ namespace ROS2::Utils::xacro
     AZStd::unordered_map<AZStd::string, AZStd::string> GetParameterFromXacroData(const AZStd::string& data);
     AZStd::unordered_map<AZStd::string, AZStd::string> GetParameterFromXacroFile(const AZStd::string& filename);
 
-    ExecutionOutcome ParseXacro(const AZStd::string& filename, const Params& params);
+    ExecutionOutcome ParseXacro(const AZStd::string& filename, const Params& params, const sdf::ParserConfig& parserConfig);
 
 
 } // namespace ROS2::Utils::xacro

+ 4 - 4
Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilder.cpp

@@ -197,13 +197,13 @@ namespace ROS2
         auto parsedSdfRootOutcome = UrdfParser::ParseFromFile(fullSourcePath, parserConfig);
         if (!parsedSdfRootOutcome)
         {
-            const AZStd::string sdfParseErrors = Utils::JoinSdfErrorsToString(parsedSdfRootOutcome.error());
+            const AZStd::string sdfParseErrors = Utils::JoinSdfErrorsToString(parsedSdfRootOutcome.GetSdfErrors());
             AZ_Error(SdfAssetBuilderName, false, R"(Failed to parse source file "%s". Errors: "%s")",
                 fullSourcePath.c_str(), sdfParseErrors.c_str());
             return;
         }
 
-        const sdf::Root& sdfRoot = parsedSdfRootOutcome.value();
+        const sdf::Root& sdfRoot = parsedSdfRootOutcome.GetRoot();
 
         AZ_Info(SdfAssetBuilderName, "Finding asset IDs for all mesh and collider assets.");
         auto sourceAssetMap = AZStd::make_shared<Utils::UrdfAssetMap>(FindAssets(sdfRoot, fullSourcePath.String()));
@@ -260,14 +260,14 @@ namespace ROS2
         auto parsedSdfRootOutcome = UrdfParser::ParseFromFile(AZ::IO::PathView(request.m_fullPath), parserConfig);
         if (!parsedSdfRootOutcome)
         {
-            const AZStd::string sdfParseErrors = Utils::JoinSdfErrorsToString(parsedSdfRootOutcome.error());
+            const AZStd::string sdfParseErrors = Utils::JoinSdfErrorsToString(parsedSdfRootOutcome.GetSdfErrors());
             AZ_Error(SdfAssetBuilderName, false, R"(Failed to parse source file "%s". Errors: "%s")",
                 request.m_fullPath.c_str(), sdfParseErrors.c_str());
             response.m_resultCode = AssetBuilderSDK::ProcessJobResult_Failed;
             return;
         }
 
-        const sdf::Root& sdfRoot = parsedSdfRootOutcome.value();
+        const sdf::Root& sdfRoot = parsedSdfRootOutcome.GetRoot();
 
         // Resolve all the URI references into source asset GUIDs.
         AZ_Info(SdfAssetBuilderName, "Finding asset IDs for all mesh and collider assets.");

+ 18 - 0
Gems/ROS2/Code/Source/SdfAssetBuilder/SdfAssetBuilderSettings.cpp

@@ -67,6 +67,24 @@ namespace ROS2
                 // remove the affected product assets, so we don't need to trigger any
                 // additional rebuilds beyond that.
              ;
+
+            if (auto editContext = serializeContext->GetEditContext(); editContext != nullptr)
+            {
+                editContext
+                    ->Class<SdfAssetBuilderSettings>(
+                        "SDF Asset Import Settings", "Exposes settings which alters importing of URDF/XACRO/SDF files")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &SdfAssetBuilderSettings::m_useArticulations,
+                        "Use Articulations",
+                        "Determines whether PhysX articulation components should be used for joints and rigid bodies")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &SdfAssetBuilderSettings::m_urdfPreserveFixedJoints,
+                        "Preserve URDF fixed joints",
+                        "When set, preserves any fixed joints found when importing a URDF/XACRO file."
+                        " This prevents the joint reduction logic in libsdformat from merging links of those joints.");
+            }
         }
     }
 

+ 20 - 20
Gems/ROS2/Code/Tests/UrdfParserTest.cpp

@@ -308,7 +308,7 @@ namespace UnitTest
         sdf::ParserConfig parserConfig;
         const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
         ASSERT_TRUE(sdfRootOutcome);
-        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Root& sdfRoot = sdfRootOutcome.GetRoot();
 
         const sdf::Model* model = sdfRoot.Model();
         EXPECT_EQ("test_one_link", model->Name());
@@ -358,7 +358,7 @@ namespace UnitTest
         sdf::ParserConfig parserConfig;
         const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
         ASSERT_TRUE(sdfRootOutcome);
-        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Root& sdfRoot = sdfRootOutcome.GetRoot();
 
         const sdf::Model* model = sdfRoot.Model();
         EXPECT_EQ("test_two_links_one_joint", model->Name());
@@ -389,7 +389,7 @@ namespace UnitTest
         parserConfig.URDFSetPreserveFixedJoint(true);
         const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
         ASSERT_TRUE(sdfRootOutcome);
-        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Root& sdfRoot = sdfRootOutcome.GetRoot();
 
         const sdf::Model* model = sdfRoot.Model();
         EXPECT_EQ("test_two_links_one_joint", model->Name());
@@ -439,7 +439,7 @@ namespace UnitTest
         sdf::ParserConfig parserConfig;
         const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
         ASSERT_TRUE(sdfRootOutcome);
-        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Root& sdfRoot = sdfRootOutcome.GetRoot();
 
         const sdf::Model* model = sdfRoot.Model();
         EXPECT_EQ("test_two_links_one_joint", model->Name());
@@ -485,14 +485,14 @@ namespace UnitTest
         EXPECT_DOUBLE_EQ(10.0, joint12Axis->MaxVelocity());
     }
 
-        TEST_F(UrdfParserTest, ParseURDF_WithTwoLinks_AndBaseLinkWithNoInertia_WithUrdfFixedJointPreservationOn_Fails)
+    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());
+        AZStd::string errorString = ROS2::Utils::JoinSdfErrorsToString(sdfRootOutcome.GetSdfErrors());
         printf("URDF with single link and no inertia: %s\n", errorString.c_str());
     }
 
@@ -503,7 +503,7 @@ namespace UnitTest
         parserConfig.URDFSetPreserveFixedJoint(false);
         const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
         ASSERT_TRUE(sdfRootOutcome);
-        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Root& sdfRoot = sdfRootOutcome.GetRoot();
 
         const sdf::Model* model = sdfRoot.Model();
         EXPECT_EQ("always_ignored", model->Name());
@@ -537,7 +537,7 @@ namespace UnitTest
         parserConfig.URDFSetPreserveFixedJoint(false);
         const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
         ASSERT_TRUE(sdfRootOutcome);
-        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Root& sdfRoot = sdfRootOutcome.GetRoot();
 
         const sdf::Model* model = sdfRoot.Model();
         EXPECT_EQ("FooRobot", model->Name());
@@ -582,7 +582,7 @@ namespace UnitTest
         parserConfig.URDFSetPreserveFixedJoint(false);
         const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
         ASSERT_TRUE(sdfRootOutcome);
-        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Root& sdfRoot = sdfRootOutcome.GetRoot();
 
         const sdf::Model* model = sdfRoot.Model();
         EXPECT_EQ("FooRobot", model->Name());
@@ -626,7 +626,7 @@ namespace UnitTest
         const auto xmlStr = GetURDFWithWheel(wheelName, "continuous");
         const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
         ASSERT_TRUE(sdfRootOutcome);
-        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Root& sdfRoot = sdfRootOutcome.GetRoot();
         const sdf::Model* model = sdfRoot.Model();
         ASSERT_NE(nullptr, model);
         auto wheelCandidate = model->LinkByName(std::string(wheelName.data(), wheelName.size()));
@@ -637,7 +637,7 @@ namespace UnitTest
         const auto xmlStr2 = GetURDFWithWheel(wheelNameSuffix, "continuous");
         const auto sdfRootOutcome2 = ROS2::UrdfParser::Parse(xmlStr2, parserConfig);
         ASSERT_TRUE(sdfRootOutcome2);
-        const sdf::Root& sdfRoot2 = sdfRootOutcome2.value();
+        const sdf::Root& sdfRoot2 = sdfRootOutcome2.GetRoot();
         const sdf::Model* model2 = sdfRoot2.Model();
         ASSERT_NE(nullptr, model2);
         auto wheelCandidate2 = model2->LinkByName(std::string(wheelNameSuffix.data(), wheelNameSuffix.size()));
@@ -652,7 +652,7 @@ namespace UnitTest
         sdf::ParserConfig parserConfig;
         const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
         ASSERT_TRUE(sdfRootOutcome);
-        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Root& sdfRoot = sdfRootOutcome.GetRoot();
         const sdf::Model* model = sdfRoot.Model();
         ASSERT_NE(nullptr, model);
         auto wheelCandidate = model->LinkByName(std::string(wheelName.data(), wheelName.size()));
@@ -667,7 +667,7 @@ namespace UnitTest
         sdf::ParserConfig parserConfig;
         const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
         ASSERT_TRUE(sdfRootOutcome);
-        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Root& sdfRoot = sdfRootOutcome.GetRoot();
         const sdf::Model* model = sdfRoot.Model();
         ASSERT_NE(nullptr, model);
         // SDFormat converts combines the links of a joint with a fixed type
@@ -690,7 +690,7 @@ namespace UnitTest
         sdf::ParserConfig parserConfig;
         const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
         ASSERT_TRUE(sdfRootOutcome);
-        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Root& sdfRoot = sdfRootOutcome.GetRoot();
         const sdf::Model* model = sdfRoot.Model();
         ASSERT_NE(nullptr, model);
         auto wheelCandidate = model->LinkByName(std::string(wheelName.c_str(), wheelName.size()));
@@ -705,7 +705,7 @@ namespace UnitTest
         sdf::ParserConfig parserConfig;
         const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
         ASSERT_TRUE(sdfRootOutcome);
-        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Root& sdfRoot = sdfRootOutcome.GetRoot();
         const sdf::Model* model = sdfRoot.Model();
         ASSERT_NE(nullptr, model);
         auto wheelCandidate = model->LinkByName(std::string(wheelName.c_str(), wheelName.size()));
@@ -719,7 +719,7 @@ namespace UnitTest
         sdf::ParserConfig parserConfig;
         const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
         ASSERT_TRUE(sdfRootOutcome);
-        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Root& sdfRoot = sdfRootOutcome.GetRoot();
         const sdf::Model* model = sdfRoot.Model();
         ASSERT_NE(nullptr, model);
         auto links = ROS2::Utils::GetAllLinks(*model);
@@ -746,7 +746,7 @@ namespace UnitTest
         sdf::ParserConfig parserConfig;
         const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
         ASSERT_TRUE(sdfRootOutcome);
-        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Root& sdfRoot = sdfRootOutcome.GetRoot();
         const sdf::Model* model = sdfRoot.Model();
         ASSERT_NE(nullptr, model);
         auto joints = ROS2::Utils::GetAllJoints(*model);
@@ -761,7 +761,7 @@ namespace UnitTest
         sdf::ParserConfig parserConfig;
         const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
         ASSERT_TRUE(sdfRootOutcome);
-        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Root& sdfRoot = sdfRootOutcome.GetRoot();
         const sdf::Model* model = sdfRoot.Model();
         ASSERT_NE(nullptr, model);
         const auto links = ROS2::Utils::GetAllLinks(*model);
@@ -802,7 +802,7 @@ namespace UnitTest
         sdf::ParserConfig parserConfig;
         const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
         ASSERT_TRUE(sdfRootOutcome);
-        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Root& sdfRoot = sdfRootOutcome.GetRoot();
         const sdf::Model* model = sdfRoot.Model();
         ASSERT_NE(nullptr, model);
         auto joints = ROS2::Utils::GetJointsForParentLink(*model, "base_link");
@@ -827,7 +827,7 @@ namespace UnitTest
         sdf::ParserConfig parserConfig;
         const auto sdfRootOutcome = ROS2::UrdfParser::Parse(xmlStr, parserConfig);
         ASSERT_TRUE(sdfRootOutcome);
-        const sdf::Root& sdfRoot = sdfRootOutcome.value();
+        const sdf::Root& sdfRoot = sdfRootOutcome.GetRoot();
         const sdf::Model* model = sdfRoot.Model();
         ASSERT_NE(nullptr, model);
         auto joints = ROS2::Utils::GetJointsForChildLink(*model, "link2");