add SDFormat sensor importer hooks (#446)

add SDFormat importer hooks

Signed-off-by: Jan Hanca <[email protected]>

Gems/ROS2/Code/CMakeLists.txt

@@ -165,8 +165,8 @@ if(PAL_TRAIT_BUILD_HOST_TOOLS)
     )
 
     # By default, we will specify that the above target ROS2 would be used by
-    # Tool and Builder type targets when this gem is enabled.  If you don't want it
-    # active in Tools or Builders by default, delete one of both of the following lines:
+    # Tool and Builder type targets when this gem is enabled. If you don't want it
+    # active in Tools or Builders by default, delete one or both of the following lines:
     ly_create_alias(NAME ${gem_name}.Tools    NAMESPACE Gem TARGETS Gem::${gem_name}.Editor)
     ly_create_alias(NAME ${gem_name}.Builders NAMESPACE Gem TARGETS Gem::${gem_name}.Editor)
 endif()
@@ -218,6 +218,7 @@ if(PAL_TRAIT_BUILD_TESTS_SUPPORTED)
                     PRIVATE
                         AZ::AzTest
                         Gem::${gem_name}.Editor
+                        3rdParty::sdformat
             )
 
             # Add ROS2.Editor.Tests to googletest

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

@@ -0,0 +1,59 @@
+/*
+ * 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 <AzCore/Component/Entity.h>
+#include <AzCore/Outcome/Outcome.h>
+#include <AzCore/std/containers/unordered_set.h>
+#include <AzCore/std/function/function_template.h>
+#include <AzCore/std/string/string.h>
+
+namespace sdf
+{
+    inline namespace v13
+    {
+        enum class SensorType;
+        class Sensor;
+    } // namespace v13
+} // namespace sdf
+
+namespace ROS2::SDFormat
+{
+    //! Hook used in ROS2 Gem Robot Importer to create ROS2 sensor components based on SDFormat model description.
+    //! It implements the parameters mapping between the SDFormat sensor and the particular O3DE component.
+    //! It should be registered as a serialization attribute in the component's reflection to make the it visible in the SDFormat model
+    //! parser. See the sensor element at http://sdformat.org/spec?ver=1.10&elem=sensor for more details on SDFormat.
+    struct SensorImporterHook
+    {
+        AZ_TYPE_INFO(SensorImporterHook, "{23f189e3-8da4-4498-9b89-1ef6c900940a}");
+
+        //! Types of sensors in SDFormat description that can be mapped to the particular O3DE component.
+        //! Multiple SDFormat sensors can map to one O3DE component.
+        AZStd::unordered_set<sdf::SensorType> m_sensorTypes;
+
+        //! Names of the sensor's parameters in SDFormat description that are supported by the particular O3DE component.
+        AZStd::unordered_set<AZStd::string> m_supportedSensorParams;
+
+        //! Names of plugins associated with the sensor in SDFormat description that are supported by the particular O3DE component.
+        //! Multiple SDFormat plugins can map to one O3DE component.
+        AZStd::unordered_set<AZStd::string> m_pluginNames;
+
+        //! Names of the plugin's parameters associated with the sensor in SDFormat description that are supported
+        //! by the particular O3DE component.
+        AZStd::unordered_set<AZStd::string> m_supportedPluginParams;
+
+        //! Registered function that is invoked when a sensor of the specified type is processed by the SDFormat Importer.
+        //! @param AZ::Entity& a reference to the Entity in which one or more O3DE component is created by the importer
+        //! @param sdf::Sensor& a reference to the sensor data in SDFormat, which is used to configure O3DE component
+        using ErrorString = AZStd::string;
+        using ConvertSensorOutcome = AZ::Outcome<void, AZStd::string>;
+        using ConvertSDFSensorCallback = AZStd::function<ConvertSensorOutcome(AZ::Entity&, const sdf::Sensor&)>;
+        ConvertSDFSensorCallback m_sdfSensorToComponentCallback;
+    };
+} // namespace ROS2::SDFormat

Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp

@@ -28,6 +28,13 @@ namespace ROS2
             MakeTopicConfigurationPair("depth_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::DepthInfoConfig));
     }
 
+    ROS2CameraSensorEditorComponent::ROS2CameraSensorEditorComponent(
+        const SensorConfiguration& sensorConfiguration, const CameraSensorConfiguration& cameraConfiguration)
+        : m_sensorConfiguration(sensorConfiguration)
+        , m_cameraSensorConfiguration(cameraConfiguration)
+    {
+    }
+
     void ROS2CameraSensorEditorComponent::Reflect(AZ::ReflectContext* context)
     {
         CameraSensorConfiguration::Reflect(context);

Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h

@@ -22,14 +22,16 @@ namespace ROS2
 {
     //! ROS2 Camera Editor sensor component class
     //! Allows turning an entity into a camera sensor in Editor
-    //! Component draws camera frustrum in the Editor
+    //! Component draws camera frustum in the Editor
     class ROS2CameraSensorEditorComponent
         : public AzToolsFramework::Components::EditorComponentBase
         , public CameraCalibrationRequestBus::Handler
         , protected AzFramework::EntityDebugDisplayEventBus::Handler
     {
     public:
         ROS2CameraSensorEditorComponent();
+        ROS2CameraSensorEditorComponent(
+            const SensorConfiguration& sensorConfiguration, const CameraSensorConfiguration& cameraConfiguration);
         ~ROS2CameraSensorEditorComponent() override = default;
         AZ_EDITOR_COMPONENT(ROS2CameraSensorEditorComponent, "{3C2A86B2-AD58-4BF1-A5EF-71E0F94A2B42}");
         static void Reflect(AZ::ReflectContext* context);
@@ -58,4 +60,4 @@ namespace ROS2
         SensorConfiguration m_sensorConfiguration;
         CameraSensorConfiguration m_cameraSensorConfiguration;
     };
-} // namespace ROS2
+} // namespace ROS2

Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp

@@ -0,0 +1,101 @@
+/*
+ * 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 "ROS2SensorHooks.h"
+
+#include <Camera/CameraConstants.h>
+#include <Camera/ROS2CameraSensorEditorComponent.h>
+#include <GNSS/ROS2GNSSSensorComponent.h>
+#include <RobotImporter/Utils/RobotImporterUtils.h>
+
+#include <sdf/Camera.hh>
+#include <sdf/NavSat.hh>
+
+namespace ROS2::SDFormat
+{
+    namespace Internal
+    {
+        void AddTopicConfiguration(
+            SensorConfiguration& sensorConfig,
+            const AZStd::string& topic,
+            const AZStd::string& messageType,
+            const AZStd::string& configName)
+        {
+            TopicConfiguration config;
+            config.m_topic = topic;
+            config.m_type = messageType;
+            sensorConfig.m_publishersConfigurations.insert(AZStd::make_pair(configName, config));
+        }
+    } // namespace Internal
+
+    SensorImporterHook ROS2SensorHooks::ROS2CameraSensor()
+    {
+        SensorImporterHook importerHook;
+        importerHook.m_sensorTypes =
+            AZStd::unordered_set<sdf::SensorType>{ sdf::SensorType::CAMERA, sdf::SensorType::DEPTH_CAMERA, sdf::SensorType::RGBD_CAMERA };
+        importerHook.m_supportedSensorParams =
+            AZStd::unordered_set<AZStd::string>{ ">update_rate",         ">camera>horizontal_fov", ">camera>image>width",
+                                                 ">camera>image>height", ">camera>clip>near",      ">camera>clip>far" };
+        importerHook.m_pluginNames = AZStd::unordered_set<AZStd::string>{ "libgazebo_ros_camera.so",
+                                                                          "libgazebo_ros_depth_camera.so",
+                                                                          "libgazebo_ros_openni_kinect.so" };
+        importerHook.m_supportedPluginParams = AZStd::unordered_set<AZStd::string>{};
+        importerHook.m_sdfSensorToComponentCallback = [](AZ::Entity& entity,
+                                                         const sdf::Sensor& sdfSensor) -> SensorImporterHook::ConvertSensorOutcome
+        {
+            auto* cameraSensor = sdfSensor.CameraSensor();
+
+            CameraSensorConfiguration cameraConfiguration;
+            cameraConfiguration.m_depthCamera = cameraSensor->HasDepthCamera();
+            cameraConfiguration.m_colorCamera = (sdfSensor.Type() != sdf::SensorType::DEPTH_CAMERA) ? true : false;
+            cameraConfiguration.m_width = cameraSensor->ImageWidth();
+            cameraConfiguration.m_height = cameraSensor->ImageHeight();
+            cameraConfiguration.m_verticalFieldOfViewDeg =
+                cameraSensor->HorizontalFov().Degree() * (cameraConfiguration.m_height / cameraConfiguration.m_width);
+            if (sdfSensor.Type() != sdf::SensorType::DEPTH_CAMERA)
+            {
+                cameraConfiguration.m_nearClipDistance = static_cast<float>(cameraSensor->NearClip());
+                cameraConfiguration.m_farClipDistance = static_cast<float>(cameraSensor->FarClip());
+            }
+            else
+            {
+                cameraConfiguration.m_nearClipDistance = static_cast<float>(cameraSensor->DepthNearClip());
+                cameraConfiguration.m_farClipDistance = static_cast<float>(cameraSensor->DepthFarClip());
+            }
+
+            SensorConfiguration sensorConfiguration;
+            sensorConfiguration.m_frequency = sdfSensor.UpdateRate();
+            if (sdfSensor.Type() != sdf::SensorType::DEPTH_CAMERA)
+            {
+                Internal::AddTopicConfiguration(
+                    sensorConfiguration, "camera_image_color", CameraConstants::ImageMessageType, CameraConstants::ColorImageConfig);
+                Internal::AddTopicConfiguration(
+                    sensorConfiguration, "color_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::ColorInfoConfig);
+            }
+            if (sdfSensor.Type() != sdf::SensorType::CAMERA)
+            {
+                Internal::AddTopicConfiguration(
+                    sensorConfiguration, "camera_image_depth", CameraConstants::ImageMessageType, CameraConstants::DepthImageConfig);
+                Internal::AddTopicConfiguration(
+                    sensorConfiguration, "depth_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::DepthInfoConfig);
+            }
+
+            if (entity.CreateComponent<ROS2CameraSensorEditorComponent>(sensorConfiguration, cameraConfiguration))
+            {
+                return AZ::Success();
+            }
+            else
+            {
+                return AZ::Failure(AZStd::string("Failed to create ROS2 Camera Sensor component"));
+            }
+        };
+
+        return importerHook;
+    }
+
+} // namespace ROS2::SDFormat

Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.h

@@ -0,0 +1,19 @@
+/*
+ * 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 <ROS2/RobotImporter/SDFormatSensorImporterHook.h>
+
+namespace ROS2::SDFormat
+{
+    namespace ROS2SensorHooks
+    {
+        SensorImporterHook ROS2CameraSensor();
+    } // namespace ROS2SensorHooks
+} // namespace ROS2::SDFormat

Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp

@@ -15,6 +15,7 @@
 #include <AzCore/StringFunc/StringFunc.h>
 #include <AzCore/std/string/regex.h>
 #include <AzToolsFramework/API/EditorAssetSystemAPI.h>
+#include <string.h>
 
 namespace ROS2
 {
@@ -218,17 +219,16 @@ namespace ROS2
         return filenames;
     }
 
-    AZStd::optional<AZ::IO::Path> GetResolvedPath(const AZ::IO::Path &packagePath,
-                                                 const AZ::IO::Path &unresolvedPath,
-                                                 const AZStd::function<bool(const AZStd::string&)>& fileExists)
+    AZStd::optional<AZ::IO::Path> GetResolvedPath(
+        const AZ::IO::Path& packagePath, const AZ::IO::Path& unresolvedPath, const AZStd::function<bool(const AZStd::string&)>& fileExists)
     {
         AZ::IO::Path packageXmlCandite = packagePath / "package.xml";
         if (fileExists(packageXmlCandite.String()))
         {
             AZ::IO::Path resolvedPath = packagePath / unresolvedPath;
             if (fileExists(resolvedPath.String()))
             {
-                return AZStd::optional<AZ::IO::Path>{resolvedPath};
+                return AZStd::optional<AZ::IO::Path>{ resolvedPath };
             }
         }
         return AZStd::optional<AZ::IO::Path>{};
@@ -241,7 +241,7 @@ namespace ROS2
         {
             return subpath;
         }
-        for (AZ::IO::Path::iterator pathIt = begin;pathIt!=end;pathIt++)
+        for (AZ::IO::Path::iterator pathIt = begin; pathIt != end; pathIt++)
         {
             subpath /= *pathIt;
         }
@@ -271,7 +271,7 @@ namespace ROS2
                     if (package.ends_with(packageNameCandidate))
                     {
                         auto pathIt = unresolvedProperPath.begin();
-                        AZStd::advance(pathIt,1);
+                        AZStd::advance(pathIt, 1);
                         if (pathIt != unresolvedProperPath.end())
                         {
                             AZ::IO::Path unresolvedPathStripped = GetPathFromSubPath(pathIt, unresolvedProperPath.end());
@@ -295,7 +295,7 @@ namespace ROS2
                 auto it = --urdfProperPath.end();
                 for (; it != urdfProperPath.begin(); it--)
                 {
-                    const auto packagePath = GetPathFromSubPath(urdfProperPath.begin(),it);
+                    const auto packagePath = GetPathFromSubPath(urdfProperPath.begin(), it);
                     std::cout << "packagePath : " << packagePath.String().c_str() << std::endl;
                     const auto resolvedPath = GetResolvedPath(packagePath, unresolvedPath, fileExists);
                     if (resolvedPath.has_value())
@@ -324,4 +324,55 @@ namespace ROS2
         return resolvedPath.String();
     }
 
+    namespace Utils::SDFormat
+    {
+        AZStd::string GetPluginFilename(const sdf::Plugin& plugin)
+        {
+            const AZ::IO::Path path{ plugin.Filename().c_str() };
+            return path.Filename().String();
+        }
+
+        AZStd::vector<AZStd::string> GetUnsupportedParams(
+            const sdf::ElementPtr& rootElement, const AZStd::unordered_set<AZStd::string>& supportedParams)
+        {
+            AZStd::vector<AZStd::string> unsupportedParams;
+
+            AZStd::function<void(const sdf::ElementPtr& elementPtr, const std::string& prefix)> elementVisitor =
+                [&](const sdf::ElementPtr& elementPtr, const std::string& prefix) -> void
+            {
+                auto childPtr = elementPtr->GetFirstElement();
+
+                AZStd::string prefixAz(prefix.c_str(), prefix.size());
+                if (!childPtr && !prefixAz.empty() && !supportedParams.contains(prefixAz))
+                {
+                    unsupportedParams.push_back(prefixAz);
+                }
+
+                while (childPtr)
+                {
+                    if (childPtr->GetName() == "plugin")
+                    {
+                        break;
+                    }
+
+                    std::string currentName = prefix;
+                    currentName.append(">");
+                    currentName.append(childPtr->GetName());
+
+                    elementVisitor(childPtr, currentName);
+                    childPtr = childPtr->GetNextElement();
+                }
+            };
+
+            elementVisitor(rootElement, "");
+
+            return unsupportedParams;
+        }
+
+        bool IsPluginSupported(const sdf::Plugin& plugin, const AZStd::unordered_set<AZStd::string>& supportedPlugins)
+        {
+            return supportedPlugins.contains(GetPluginFilename(plugin));
+        }
+    } // namespace Utils::SDFormat
+
 } // namespace ROS2

Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h

@@ -11,10 +11,14 @@
 #include <AzCore/IO/SystemFile.h>
 #include <AzCore/Math/Transform.h>
 #include <AzCore/std/containers/unordered_map.h>
+#include <AzCore/std/containers/unordered_set.h>
+#include <AzCore/std/containers/vector.h>
 #include <AzCore/std/function/function_template.h>
 #include <AzCore/std/string/string.h>
 #include <RobotImporter/URDF/UrdfParser.h>
 
+#include <sdf/sdf.hh>
+
 namespace ROS2
 {
     namespace
@@ -77,5 +81,27 @@ namespace ROS2
         //! @returns false if a timeout or error occurs, otherwise true
         bool WaitForAssetsToProcess(const AZStd::unordered_map<AZStd::string, AZ::IO::Path>& sourceAssetsPaths);
 
+        namespace SDFormat
+        {
+            //! Retrieve plugin's filename. The filepath is converted into the filename if necessary.
+            //! @param plugin plugin in the parsed SDFormat data
+            //! @returns filename (including extension) without path
+            AZStd::string GetPluginFilename(const sdf::Plugin& plugin);
+
+            //! Retrieve all parameters that were defined for an element in XML data that are not supported in O3DE.
+            //! Allows to store the list of unsupported parameters in metadata and logs. It is typically used with sensors and plugins.
+            //! @param rootElement pointer to a root Element in parsed XML data that will be a subject to heuristics
+            //! @param supportedParams set of predefined parameters that are supported
+            //! @returns list of unsupported parameters defined for given element
+            AZStd::vector<AZStd::string> GetUnsupportedParams(
+                const sdf::ElementPtr& rootElement, const AZStd::unordered_set<AZStd::string>& supportedParams);
+
+            //! Check if plugin is supported by using it's filename. The filepath is converted into the filename if necessary.
+            //! @param plugin plugin in the parsed SDFormat data
+            //! @param supportedPlugins set of predefined plugins that are supported
+            //! @returns true if plugin is supported
+            bool IsPluginSupported(const sdf::Plugin& plugin, const AZStd::unordered_set<AZStd::string>& supportedPlugins);
+        } // namespace SDFormat
+
     } // namespace Utils
 } // namespace ROS2