Robot Importer: add sensor maker

Gems/ROS2/Code/Include/ROS2/RobotImporter/RobotImporterBus.h

@@ -7,9 +7,10 @@
  */
 #pragma once
 
+#include <AzCore/EBus/EBus.h>
 #include <AzCore/EBus/Policies.h>
 #include <AzCore/std/string/string.h>
-#include <AzCore/EBus/EBus.h>
+#include <ROS2/RobotImporter/SDFormatSensorImporterHook.h>
 
 namespace ROS2
 {
@@ -24,6 +25,9 @@ namespace ROS2
         //! @param importAssetWithUrdf If true, the assets referenced in the urdf file will be imported
         //! @param useArticulation If true, the prefab will be generated with articulation
         virtual bool GeneratePrefabFromFile(const AZStd::string_view filePath, bool importAssetWithUrdf, bool useArticulation) = 0;
+
+        //! Return the reference to the list of sensor importer hooks
+        virtual const SDFormat::SensorImporterHooksStorage& GetSensorHooks() const = 0;
     };
 
     using RobotImporterRequestBus = AZ::EBus<RobotImporterRequest>;

Gems/ROS2/Code/Include/ROS2/RobotImporter/SDFormatSensorImporterHook.h

@@ -56,4 +56,7 @@ namespace ROS2::SDFormat
         using ConvertSDFSensorCallback = AZStd::function<ConvertSensorOutcome(AZ::Entity&, const sdf::Sensor&)>;
         ConvertSDFSensorCallback m_sdfSensorToComponentCallback;
     };
+
+    //! Buffer for SensorImporterHook data
+    using SensorImporterHooksStorage = AZStd::vector<ROS2::SDFormat::SensorImporterHook>;
 } // namespace ROS2::SDFormat

Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp

@@ -13,10 +13,13 @@
 #include <AzCore/Utils/Utils.h>
 #include <AzCore/std/chrono/chrono.h>
 #include <AzCore/std/string/string.h>
+#include <AzCore/std/utility/move.h>
 #include <AzToolsFramework/API/ViewPaneOptions.h>
+#include <AzToolsFramework/Entity/EditorEntityHelpers.h>
 #include <RobotImporter/URDF/UrdfParser.h>
 #include <RobotImporter/Utils/ErrorUtils.h>
 #include <RobotImporter/Utils/FilePath.h>
+#include <SDFormat/ROS2SensorHooks.h>
 #include <SdfAssetBuilder/SdfAssetBuilderSettings.h>
 
 #include <sdf/sdf.hh>
@@ -31,7 +34,16 @@ namespace ROS2
     {
         if (auto serializeContext = azrtti_cast<AZ::SerializeContext*>(context))
         {
-            serializeContext->Class<ROS2RobotImporterEditorSystemComponent, ROS2RobotImporterSystemComponent>()->Version(0);
+            const auto& importerHookCamera = ROS2::SDFormat::ROS2SensorHooks::ROS2CameraSensor();
+            const auto& importerHookGNSS = ROS2::SDFormat::ROS2SensorHooks::ROS2GNSSSensor();
+            const auto& importerHookImu = ROS2::SDFormat::ROS2SensorHooks::ROS2ImuSensor();
+            const auto& importerHookLidar = ROS2::SDFormat::ROS2SensorHooks::ROS2LidarSensor();
+            serializeContext->Class<ROS2RobotImporterEditorSystemComponent, ROS2RobotImporterSystemComponent>()->Version(0)->Attribute(
+                "SensorImporterHooks",
+                SDFormat::SensorImporterHooksStorage{ AZStd::move(importerHookCamera),
+                                                      AZStd::move(importerHookGNSS),
+                                                      AZStd::move(importerHookImu),
+                                                      AZStd::move(importerHookLidar) });
         }
 
         if (AZ::BehaviorContext* behaviorContext = azrtti_cast<AZ::BehaviorContext*>(context))
@@ -61,6 +73,26 @@ namespace ROS2
         ROS2RobotImporterSystemComponent::Activate();
         AzToolsFramework::EditorEvents::Bus::Handler::BusConnect();
         RobotImporterRequestBus::Handler::BusConnect();
+
+        auto serializeContext = AZ::Interface<AZ::ComponentApplicationRequests>::Get()->GetSerializeContext();
+        serializeContext->EnumerateAll(
+            [&](const AZ::SerializeContext::ClassData* classData, const AZ::Uuid& typeId) -> bool
+            {
+                auto* attribute = AZ::FindAttribute(AZ::Crc32("SensorImporterHooks"), classData->m_attributes);
+                if (attribute == nullptr)
+                {
+                    return true;
+                }
+
+                AZ::AttributeReader reader(nullptr, attribute);
+                SDFormat::SensorImporterHooksStorage sensorHooks;
+                if (reader.Read<SDFormat::SensorImporterHooksStorage>(sensorHooks))
+                {
+                    m_sensorHooks.insert(m_sensorHooks.end(), sensorHooks.begin(), sensorHooks.end());
+                }
+
+                return false;
+            });
     }
 
     void ROS2RobotImporterEditorSystemComponent::Deactivate()
@@ -223,4 +255,9 @@ namespace ROS2
         return true;
     }
 
+    const SDFormat::SensorImporterHooksStorage& ROS2RobotImporterEditorSystemComponent::GetSensorHooks() const
+    {
+        return m_sensorHooks;
+    }
+
 } // namespace ROS2

Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.h

@@ -12,6 +12,7 @@
 #include <AzCore/std/string/string.h>
 #include <AzToolsFramework/Entity/EditorEntityContextBus.h>
 #include <ROS2/RobotImporter/RobotImporterBus.h>
+#include <ROS2/RobotImporter/SDFormatSensorImporterHook.h>
 #include <RobotImporter/Utils/SourceAssetsStorage.h>
 namespace ROS2
 {
@@ -42,8 +43,12 @@ namespace ROS2
 
         // RobotImporterRequestsBus::Handler overrides ..
         bool GeneratePrefabFromFile(const AZStd::string_view filePath, bool importAssetWithUrdf, bool useArticulation) override;
+        const SDFormat::SensorImporterHooksStorage& GetSensorHooks() const override;
 
         // Timeout for loop waiting for assets to be built
         static constexpr AZStd::chrono::seconds assetLoopTimeout = AZStd::chrono::seconds(30);
+
+        // Cache for storing sensor importer hooks (read only once)
+        SDFormat::SensorImporterHooksStorage m_sensorHooks;
     };
 } // namespace ROS2

Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.cpp

@@ -244,12 +244,13 @@ namespace ROS2
 
             if (m_importAssetWithUrdf)
             {
-                m_urdfAssetsMapping = AZStd::make_shared<Utils::UrdfAssetMap>(
-                    Utils::CopyAssetForURDFAndCreateAssetMap(m_meshNames, m_urdfPath.String(), collidersNames, visualNames, sdfBuilderSettings, dirSuffix));
+                m_urdfAssetsMapping = AZStd::make_shared<Utils::UrdfAssetMap>(Utils::CopyAssetForURDFAndCreateAssetMap(
+                    m_meshNames, m_urdfPath.String(), collidersNames, visualNames, sdfBuilderSettings, dirSuffix));
             }
             else
             {
-                m_urdfAssetsMapping = AZStd::make_shared<Utils::UrdfAssetMap>(Utils::FindAssetsForUrdf(m_meshNames, m_urdfPath.String(), sdfBuilderSettings));
+                m_urdfAssetsMapping =
+                    AZStd::make_shared<Utils::UrdfAssetMap>(Utils::FindAssetsForUrdf(m_meshNames, m_urdfPath.String(), sdfBuilderSettings));
                 for (const AZStd::string& meshPath : m_meshNames)
                 {
                     if (m_urdfAssetsMapping->contains(meshPath))
@@ -313,7 +314,7 @@ namespace ROS2
     void RobotImporterWidget::FillPrefabMakerPage()
     {
         // Use the URDF/SDF file name stem the prefab name
-        AZStd::string robotName = AZStd::string(m_urdfPath.Stem().Native());
+        AZStd::string robotName = AZStd::string(m_urdfPath.Stem().Native()) + ".prefab";
         m_prefabMakerPage->setProposedPrefabName(robotName);
         QWizard::button(PrefabCreationButtonId)->setText(tr("Create Prefab"));
         QWizard::setOption(HavePrefabCreationButton, true);
@@ -400,11 +401,11 @@ namespace ROS2
 
     void RobotImporterWidget::CreatePrefab(AZStd::string prefabName)
     {
-        const AZ::IO::Path prefabPathRealative(AZ::IO::Path("Assets") / "Importer" / prefabName);
-        const AZ::IO::Path prefabPath(AZ::IO::Path(AZ::Utils::GetProjectPath()) / prefabPathRealative);
+        const AZ::IO::Path prefabPathRelative(AZ::IO::Path("Assets") / "Importer" / prefabName);
+        const AZ::IO::Path prefabPath(AZ::IO::Path(AZ::Utils::GetProjectPath()) / prefabPathRelative);
         bool fileExists = AZ::IO::FileIOBase::GetInstance()->Exists(prefabPath.c_str());
 
-        if (CheckCyclicalDependency(prefabPathRealative))
+        if (CheckCyclicalDependency(prefabPathRelative))
         {
             m_prefabMakerPage->setSuccess(false);
             return;

Gems/ROS2/Code/Source/RobotImporter/RobotImporterWidget.h

@@ -88,7 +88,7 @@ namespace ROS2
 
         //! Checks if the importedPrefabFilename is the same as focused prefab name.
         //! @param importedPrefabFilename name of imported prefab
-        //! @return True if names of prefabs are identical or an erorr occured during validation
+        //! @return True if names of prefabs are identical or an error occurred during validation
         bool CheckCyclicalDependency(AZ::IO::Path importedPrefabFilename);
 
         //! Report an error to the user.

Gems/ROS2/Code/Source/RobotImporter/URDF/SensorsMaker.cpp

@@ -0,0 +1,47 @@
+/*
+ * 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 "SensorsMaker.h"
+
+#include <AzCore/Component/EntityId.h>
+#include <AzToolsFramework/Entity/EditorEntityHelpers.h>
+#include <ROS2/RobotImporter/RobotImporterBus.h>
+#include <ROS2/RobotImporter/SDFormatSensorImporterHook.h>
+
+#include <sdf/Link.hh>
+#include <sdf/Sensor.hh>
+
+namespace ROS2
+{
+    bool AddSensor(AZ::EntityId entityId, const sdf::Sensor* sensor)
+    {
+        SDFormat::SensorImporterHooksStorage sensorHooks;
+        ROS2::RobotImporterRequestBus::BroadcastResult(sensorHooks, &ROS2::RobotImporterRequest::GetSensorHooks);
+        for (const auto& hook : sensorHooks)
+        {
+            if (hook.m_sensorTypes.contains(sensor->Type()))
+            {
+                AZ::Entity* entity = AzToolsFramework::GetEntityById(entityId);
+                hook.m_sdfSensorToComponentCallback(*entity, *sensor);
+                return true;
+            }
+        }
+
+        return false;
+    }
+
+    void SensorsMaker::AddSensors(const sdf::Model& model, const sdf::Link* link, AZ::EntityId entityId) const
+    {
+        for (size_t si = 0; si < link->SensorCount(); ++si)
+        {
+            const auto* sensor = link->SensorByIndex(si);
+            const bool success = AddSensor(entityId, sensor);
+            AZ_Warning("SensorMaker", success, "Cannot find a sensor hook for sensor %d", sensor->Type());
+        }
+    }
+} // namespace ROS2

Gems/ROS2/Code/Source/RobotImporter/URDF/SensorsMaker.h

@@ -0,0 +1,32 @@
+/*
+ * 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 "UrdfParser.h"
+#include <AzCore/Component/ComponentBus.h>
+#include <AzCore/Component/EntityId.h>
+#include <AzCore/Outcome/Outcome.h>
+
+#include <sdf/sdf.hh>
+
+namespace ROS2
+{
+    //! Populates a given entity with all the contents of the <sensor> tag.
+    //! Sensors are specified as children of link or joint in SDFormat.
+    class SensorsMaker
+    {
+    public:
+        //! Adds a sensor to an entity and sets it accordingly based on SDFormat description.
+        //! @param model A parsed SDF model which could hold information about sensor to be made.
+        //! @param link A parsed SDF tree link node used to identify link being currently processed.
+        //! @param entityId A non-active entity which will be affected.
+        //! @returns created components Id or string with fail
+        void AddSensors(const sdf::Model& model, const sdf::Link* link, AZ::EntityId entityId) const;
+    };
+} // namespace ROS2

Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp

@@ -16,8 +16,8 @@
 #include <AzCore/Serialization/Json/JsonUtils.h>
 #include <AzToolsFramework/Entity/EditorEntityHelpers.h>
 #include <AzToolsFramework/Prefab/PrefabLoaderInterface.h>
-#include <AzToolsFramework/Prefab/PrefabSystemComponentInterface.h>
 #include <AzToolsFramework/Prefab/PrefabLoaderScriptingBus.h>
+#include <AzToolsFramework/Prefab/PrefabSystemComponentInterface.h>
 #include <AzToolsFramework/Prefab/PrefabSystemScriptingBus.h>
 #include <AzToolsFramework/Prefab/Procedural/ProceduralPrefabAsset.h>
 #include <AzToolsFramework/ToolsComponents/GenericComponentWrapper.h>
@@ -149,7 +149,6 @@ namespace ROS2
         constexpr bool visitNestedModels = true;
         Utils::VisitModels(*m_root, GetAllLinksFromModel, visitNestedModels);
 
-
         for (const auto& [name, linkPtr] : links)
         {
             createdLinks[name] = AddEntitiesForLink(linkPtr, AZ::EntityId{}, createdEntities);
@@ -176,8 +175,7 @@ namespace ROS2
         // Set the transforms of links
         for (const auto& [name, linkPtr] : links)
         {
-            if (const auto thisEntry = createdLinks.at(name);
-                thisEntry.IsSuccess())
+            if (const auto thisEntry = createdLinks.at(name); thisEntry.IsSuccess())
             {
                 AZ::Transform tf = Utils::GetWorldTransformURDF(linkPtr);
                 auto* entity = AzToolsFramework::GetEntityById(thisEntry.GetValue());
@@ -202,7 +200,8 @@ namespace ROS2
                     }
                     else
                     {
-                        AZ_Trace("CreatePrefabFromUrdfOrSdf", "Setting transform failed: %s does not have transform interface\n", name.c_str());
+                        AZ_Trace(
+                            "CreatePrefabFromUrdfOrSdf", "Setting transform failed: %s does not have transform interface\n", name.c_str());
                     }
                 }
             }
@@ -252,8 +251,8 @@ namespace ROS2
                 > defining the coordinate transformation from the parent link frame to the child link frame.
             */
 
-            AZStd::string parentName(jointsWhereLinkIsChild.front()->ParentName().c_str(),
-                jointsWhereLinkIsChild.front()->ParentName().size());
+            AZStd::string parentName(
+                jointsWhereLinkIsChild.front()->ParentName().c_str(), jointsWhereLinkIsChild.front()->ParentName().size());
             const auto parentEntry = createdLinks.find(parentName);
             if (parentEntry == createdLinks.end())
             {
@@ -262,7 +261,11 @@ namespace ROS2
             }
             if (!parentEntry->second.IsSuccess())
             {
-                AZ_Trace("CreatePrefabFromUrdfOrSdf", "Link %s has parent %s which has failed to create\n", linkName.c_str(), parentName.c_str());
+                AZ_Trace(
+                    "CreatePrefabFromUrdfOrSdf",
+                    "Link %s has parent %s which has failed to create\n",
+                    linkName.c_str(),
+                    parentName.c_str());
                 continue;
             }
             AZ_Trace(
@@ -286,15 +289,25 @@ namespace ROS2
             auto parentLinkIter = createdLinks.find(parentLinkName);
             if (parentLinkIter == createdLinks.end())
             {
-                AZ_Warning("CreatePrefabFromUrdfOrSdf", false, "Joint %s has no parent link %s. Cannot create", azJointName.c_str(), parentLinkName.c_str());
+                AZ_Warning(
+                    "CreatePrefabFromUrdfOrSdf",
+                    false,
+                    "Joint %s has no parent link %s. Cannot create",
+                    azJointName.c_str(),
+                    parentLinkName.c_str());
                 return true;
             }
             auto leadEntity = parentLinkIter->second;
 
             auto childLinkIter = createdLinks.find(childLinkName);
             if (childLinkIter == createdLinks.end())
             {
-                AZ_Warning("CreatePrefabFromUrdfOrSdf", false, "Joint %s has no child link %s. Cannot create", azJointName.c_str(), childLinkName.c_str());
+                AZ_Warning(
+                    "CreatePrefabFromUrdfOrSdf",
+                    false,
+                    "Joint %s has no child link %s. Cannot create",
+                    azJointName.c_str(),
+                    childLinkName.c_str());
                 return true;
             }
             auto childEntity = childLinkIter->second;
@@ -306,7 +319,6 @@ namespace ROS2
                 parentLinkName.c_str(),
                 childLinkName.c_str());
 
-
             AZ::Entity* childEntityPtr = AzToolsFramework::GetEntityById(childEntity.GetValue());
             if (childEntityPtr)
             {
@@ -381,7 +393,8 @@ namespace ROS2
         AzToolsFramework::Prefab::PrefabSystemScriptingBus::BroadcastResult(
             prefabTemplateId,
             &AzToolsFramework::Prefab::PrefabSystemScriptingBus::Events::CreatePrefabTemplate,
-            createdEntities, relativePath.String());
+            createdEntities,
+            relativePath.String());
 
         if (prefabTemplateId == AzToolsFramework::Prefab::InvalidTemplateId)
         {
@@ -432,8 +445,7 @@ namespace ROS2
         }
         else
         {
-            result = AZ::Failure(AZStd::string::format("Could not save the newly created prefab to '%s'",
-                m_prefabPath.c_str()));
+            result = AZ::Failure(AZStd::string::format("Could not save the newly created prefab to '%s'", m_prefabPath.c_str()));
         }
 
         // End undo batch labeled "Robot Importer prefab creation"
@@ -446,7 +458,8 @@ namespace ROS2
         return result;
     }
 
-    AzToolsFramework::Prefab::PrefabEntityResult URDFPrefabMaker::AddEntitiesForLink(const sdf::Link* link, AZ::EntityId parentEntityId, AZStd::vector<AZ::EntityId>& createdEntities)
+    AzToolsFramework::Prefab::PrefabEntityResult URDFPrefabMaker::AddEntitiesForLink(
+        const sdf::Link* link, AZ::EntityId parentEntityId, AZStd::vector<AZ::EntityId>& createdEntities)
     {
         if (!link)
         {
@@ -491,6 +504,7 @@ namespace ROS2
         if (modelContainingLink != nullptr)
         {
             m_collidersMaker.AddColliders(*modelContainingLink, link, entityId);
+            m_sensorsMaker.AddSensors(*modelContainingLink, link, entityId);
         }
         return AZ::Success(entityId);
     }
@@ -524,8 +538,7 @@ namespace ROS2
             return;
         }
 
-        if (auto entity_ = AzToolsFramework::GetEntityById(rootEntityId);
-            entity_ != nullptr)
+        if (auto entity_ = AzToolsFramework::GetEntityById(rootEntityId); entity_ != nullptr)
         {
             auto* transformInterface_ = entity_->FindComponent<AzToolsFramework::Components::TransformComponent>();