Add SDFormat importer hooks (#453)

Add sensors SDF importer hooks

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

Gems/ROS2/Code/Source/Lidar/LidarTemplateUtils.cpp

@@ -148,43 +148,43 @@ namespace ROS2
                 {
                     /*.m_model = */ Model::Custom2DLidar,
                     /*.m_name = */ "CustomLidar2D",
-                  /*.m_minHAngle = */ -180.0f,
-                  /*.m_maxHAngle = */ 180.0f,
-                  /*.m_minVAngle = */ 0.f,
-                  /*.m_maxVAngle = */ 0.f,
-                  /*.m_layers = */ 1,
-                  /*.m_numberOfIncrements = */ 924,
-                  /*.m_minRange = */ 0.0f,
-                  /*.m_maxRange = */ 100.0f,
-                  /*.m_noiseParameters = */
-                  {
-                      /*.m_angularNoiseStdDev = */ 0.0f,
+                    /*.m_minHAngle = */ -180.0f,
+                    /*.m_maxHAngle = */ 180.0f,
+                    /*.m_minVAngle = */ 0.f,
+                    /*.m_maxVAngle = */ 0.f,
+                    /*.m_layers = */ 1,
+                    /*.m_numberOfIncrements = */ 924,
+                    /*.m_minRange = */ 0.0f,
+                    /*.m_maxRange = */ 100.0f,
+                    /*.m_noiseParameters = */
+                    {
+                        /*.m_angularNoiseStdDev = */ 0.0f,
                         /*.m_distanceNoiseStdDevBase = */ 0.02f,
                         /*.m_distanceNoiseStdDevRisePerMeter = */ 0.001f,
                     },
-                }
+                },
             },
             {
                 Model::Slamtec_RPLIDAR_S1,
                 {
                     /*.m_model = */ Model::Slamtec_RPLIDAR_S1,
                     /*.m_name = */ "Slamtec RPLIDAR S1",
-                  /*.m_minHAngle = */ -180.0f,
-                  /*.m_maxHAngle = */ 180.0f,
-                  /*.m_minVAngle = */ 0.f,
-                  /*.m_maxVAngle = */ 0.f,
-                  /*.m_layers = */ 1,
-                  /*.m_numberOfIncrements = */ 921,
-                  /*.m_minRange = */ 0.1f,
-                  /*.m_maxRange = */ 40.0f,
-                  /*.m_noiseParameters = */
-                  {
-                      /*.m_angularNoiseStdDev = */ 0.0f,
+                    /*.m_minHAngle = */ -180.0f,
+                    /*.m_maxHAngle = */ 180.0f,
+                    /*.m_minVAngle = */ 0.f,
+                    /*.m_maxVAngle = */ 0.f,
+                    /*.m_layers = */ 1,
+                    /*.m_numberOfIncrements = */ 921,
+                    /*.m_minRange = */ 0.1f,
+                    /*.m_maxRange = */ 40.0f,
+                    /*.m_noiseParameters = */
+                    {
+                        /*.m_angularNoiseStdDev = */ 0.0f,
                         /*.m_distanceNoiseStdDevBase = */ 0.02f,
                         /*.m_distanceNoiseStdDevRisePerMeter = */ 0.001f,
                     },
-                }
-            },
+                },
+            }
         };
 
         auto it = templates.find(model);

Gems/ROS2/Code/Source/RobotControl/ROS2RobotControlComponent.cpp

@@ -89,7 +89,7 @@ namespace ROS2
         return m_controlConfiguration;
     }
 
-    const TopicConfiguration& ROS2RobotControlComponent::GetSubscriberConfigration() const
+    const TopicConfiguration& ROS2RobotControlComponent::GetSubscriberConfiguration() const
     {
         return m_subscriberConfiguration;
     }

Gems/ROS2/Code/Source/RobotControl/ROS2RobotControlComponent.h

@@ -32,7 +32,7 @@ namespace ROS2
 
         void SetControlConfiguration(const ControlConfiguration& controlConfiguration);
 
-        const TopicConfiguration& GetSubscriberConfigration() const;
+        const TopicConfiguration& GetSubscriberConfiguration() const;
 
         void SetSubscriberConfiguration(const TopicConfiguration& subscriberConfiguration);
 

Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp → Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp

@@ -6,33 +6,17 @@
  *
  */
 
-#include "ROS2SensorHooks.h"
-
 #include <Camera/CameraConstants.h>
 #include <Camera/ROS2CameraSensorEditorComponent.h>
-#include <GNSS/ROS2GNSSSensorComponent.h>
-#include <RobotImporter/Utils/RobotImporterUtils.h>
+#include <ROS2/Frame/ROS2FrameComponent.h>
+#include <RobotImporter/SDFormat/ROS2SensorHooks.h>
+#include <RobotImporter/SDFormat/ROS2SensorHooksUtils.h>
 
 #include <sdf/Camera.hh>
-#include <sdf/NavSat.hh>
+#include <sdf/Sensor.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;
@@ -49,14 +33,22 @@ namespace ROS2::SDFormat
                                                          const sdf::Sensor& sdfSensor) -> SensorImporterHook::ConvertSensorOutcome
         {
             auto* cameraSensor = sdfSensor.CameraSensor();
+            if (!cameraSensor)
+            {
+                return AZ::Failure(AZStd::string("Failed to read parsed SDFormat data of %s camera sensor", sdfSensor.Name().c_str()));
+            }
 
             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 (cameraConfiguration.m_width != 0)
+            {
+                double aspectRatio = static_cast<double>(cameraConfiguration.m_height) / cameraConfiguration.m_width;
+                cameraConfiguration.m_verticalFieldOfViewDeg =
+                    2.0 * AZStd::atan(AZStd::tan(cameraSensor->HorizontalFov().Radian() / 2.0) * aspectRatio);
+            }
             if (sdfSensor.Type() != sdf::SensorType::DEPTH_CAMERA)
             {
                 cameraConfiguration.m_nearClipDistance = static_cast<float>(cameraSensor->NearClip());
@@ -71,21 +63,25 @@ namespace ROS2::SDFormat
             SensorConfiguration sensorConfiguration;
             sensorConfiguration.m_frequency = sdfSensor.UpdateRate();
             if (sdfSensor.Type() != sdf::SensorType::DEPTH_CAMERA)
-            {
-                Internal::AddTopicConfiguration(
+            { // COLOR_CAMERA and RGBD_CAMERA
+                Utils::AddTopicConfiguration(
                     sensorConfiguration, "camera_image_color", CameraConstants::ImageMessageType, CameraConstants::ColorImageConfig);
-                Internal::AddTopicConfiguration(
+                Utils::AddTopicConfiguration(
                     sensorConfiguration, "color_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::ColorInfoConfig);
             }
             if (sdfSensor.Type() != sdf::SensorType::CAMERA)
-            {
-                Internal::AddTopicConfiguration(
+            { // DEPTH_CAMERA and RGBD_CAMERA
+                Utils::AddTopicConfiguration(
                     sensorConfiguration, "camera_image_depth", CameraConstants::ImageMessageType, CameraConstants::DepthImageConfig);
-                Internal::AddTopicConfiguration(
+                Utils::AddTopicConfiguration(
                     sensorConfiguration, "depth_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::DepthInfoConfig);
             }
 
-            if (entity.CreateComponent<ROS2CameraSensorEditorComponent>(sensorConfiguration, cameraConfiguration))
+            // Create required components
+            Utils::CreateComponent<ROS2FrameComponent>(entity);
+
+            // Create Camera component
+            if (Utils::CreateComponent<ROS2CameraSensorEditorComponent>(entity, sensorConfiguration, cameraConfiguration))
             {
                 return AZ::Success();
             }
@@ -97,5 +93,4 @@ namespace ROS2::SDFormat
 
         return importerHook;
     }
-
 } // namespace ROS2::SDFormat

Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp

@@ -0,0 +1,50 @@
+/*
+ * 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 <GNSS/ROS2GNSSSensorComponent.h>
+#include <RobotImporter/SDFormat/ROS2SensorHooks.h>
+#include <RobotImporter/SDFormat/ROS2SensorHooksUtils.h>
+
+#include <sdf/NavSat.hh>
+#include <sdf/Sensor.hh>
+
+namespace ROS2::SDFormat
+{
+    SensorImporterHook ROS2SensorHooks::ROS2GNSSSensor()
+    {
+        SensorImporterHook importerHook;
+        importerHook.m_sensorTypes = AZStd::unordered_set<sdf::SensorType>{ sdf::SensorType::NAVSAT };
+        importerHook.m_supportedSensorParams = AZStd::unordered_set<AZStd::string>{ ">update_rate" };
+        importerHook.m_pluginNames = AZStd::unordered_set<AZStd::string>{ "libgazebo_ros_gps_sensor.so" };
+        importerHook.m_supportedPluginParams = AZStd::unordered_set<AZStd::string>{};
+        importerHook.m_sdfSensorToComponentCallback = [](AZ::Entity& entity,
+                                                         const sdf::Sensor& sdfSensor) -> SensorImporterHook::ConvertSensorOutcome
+        {
+            if (!sdfSensor.NavSatSensor())
+            {
+                return AZ::Failure(AZStd::string("Failed to read parsed SDFormat data of %s NavSat sensor", sdfSensor.Name().c_str()));
+            }
+
+            SensorConfiguration sensorConfiguration;
+            sensorConfiguration.m_frequency = sdfSensor.UpdateRate();
+            const AZStd::string messageType = "sensor_msgs::msg::NavSatFix";
+            Utils::AddTopicConfiguration(sensorConfiguration, "gnss", messageType, messageType);
+
+            if (Utils::CreateComponent<ROS2GNSSSensorComponent>(entity, sensorConfiguration, GNSSSensorConfiguration()))
+            {
+                return AZ::Success();
+            }
+            else
+            {
+                return AZ::Failure(AZStd::string("Failed to create ROS2 GNSS Sensor component"));
+            }
+        };
+
+        return importerHook;
+    }
+} // namespace ROS2::SDFormat

Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.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 <Imu/ROS2ImuSensorComponent.h>
+#include <ROS2/Frame/ROS2FrameComponent.h>
+#include <RobotImporter/SDFormat/ROS2SensorHooks.h>
+#include <RobotImporter/SDFormat/ROS2SensorHooksUtils.h>
+#include <Source/EditorStaticRigidBodyComponent.h>
+
+#include <sdf/Imu.hh>
+#include <sdf/Sensor.hh>
+
+namespace ROS2::SDFormat
+{
+    SensorImporterHook ROS2SensorHooks::ROS2ImuSensor()
+    {
+        SensorImporterHook importerHook;
+        importerHook.m_sensorTypes = AZStd::unordered_set<sdf::SensorType>{ sdf::SensorType::IMU };
+        importerHook.m_supportedSensorParams = AZStd::unordered_set<AZStd::string>{ ">update_rate",
+                                                                                    ">imu>angular_velocity>x>noise>mean",
+                                                                                    ">imu>angular_velocity>x>noise>stddev",
+                                                                                    ">imu>angular_velocity>y>noise>mean",
+                                                                                    ">imu>angular_velocity>y>noise>stddev",
+                                                                                    ">imu>angular_velocity>z>noise>mean",
+                                                                                    ">imu>angular_velocity>z>noise>stddev",
+                                                                                    ">imu>linear_acceleration>x>noise>mean",
+                                                                                    ">imu>linear_acceleration>x>noise>stddev",
+                                                                                    ">imu>linear_acceleration>y>noise>mean",
+                                                                                    ">imu>linear_acceleration>y>noise>stddev",
+                                                                                    ">imu>linear_acceleration>z>noise>mean",
+                                                                                    ">imu>linear_acceleration>z>noise>stddev" };
+        importerHook.m_pluginNames = AZStd::unordered_set<AZStd::string>{ "libgazebo_ros_imu_sensor.so" };
+        importerHook.m_supportedPluginParams = AZStd::unordered_set<AZStd::string>{};
+        importerHook.m_sdfSensorToComponentCallback = [](AZ::Entity& entity,
+                                                         const sdf::Sensor& sdfSensor) -> SensorImporterHook::ConvertSensorOutcome
+        {
+            auto* imuSensor = sdfSensor.ImuSensor();
+            if (!imuSensor)
+            {
+                return AZ::Failure(AZStd::string("Failed to read parsed SDFormat data of %s imu sensor", sdfSensor.Name().c_str()));
+            }
+
+            ImuSensorConfiguration imuConfiguration;
+            const auto& angVelXNoise = imuSensor->AngularVelocityXNoise();
+            const auto& angVelYNoise = imuSensor->AngularVelocityYNoise();
+            const auto& angVelZNoise = imuSensor->AngularVelocityZNoise();
+            if (angVelXNoise.Type() == sdf::NoiseType::GAUSSIAN && angVelYNoise.Type() == sdf::NoiseType::GAUSSIAN &&
+                angVelZNoise.Type() == sdf::NoiseType::GAUSSIAN)
+            {
+                if (angVelXNoise.Mean() == 0.0 && angVelYNoise.Mean() == 0.0 && angVelZNoise.Mean() == 0.0)
+                {
+                    imuConfiguration.m_angularVelocityVariance = AZ::Vector3(
+                        static_cast<float>(angVelXNoise.StdDev() * angVelXNoise.StdDev()),
+                        static_cast<float>(angVelYNoise.StdDev() * angVelYNoise.StdDev()),
+                        static_cast<float>(angVelZNoise.StdDev() * angVelZNoise.StdDev()));
+                }
+            }
+
+            const auto& linAccXNoise = imuSensor->LinearAccelerationXNoise();
+            const auto& linAccYNoise = imuSensor->LinearAccelerationYNoise();
+            const auto& linAccZNoise = imuSensor->LinearAccelerationZNoise();
+            if (linAccXNoise.Type() == sdf::NoiseType::GAUSSIAN && linAccYNoise.Type() == sdf::NoiseType::GAUSSIAN &&
+                linAccZNoise.Type() == sdf::NoiseType::GAUSSIAN)
+            {
+                if (linAccXNoise.Mean() == 0.0 && linAccYNoise.Mean() == 0.0 && linAccZNoise.Mean() == 0.0)
+                {
+                    imuConfiguration.m_linearAccelerationVariance = AZ::Vector3(
+                        static_cast<float>(linAccXNoise.StdDev() * linAccXNoise.StdDev()),
+                        static_cast<float>(linAccYNoise.StdDev() * linAccYNoise.StdDev()),
+                        static_cast<float>(linAccZNoise.StdDev() * linAccZNoise.StdDev()));
+                }
+            }
+
+            SensorConfiguration sensorConfiguration;
+            sensorConfiguration.m_frequency = sdfSensor.UpdateRate();
+            const AZStd::string messageType = "sensor_msgs::msg::Imu";
+            Utils::AddTopicConfiguration(sensorConfiguration, "imu", messageType, messageType);
+
+            // Create required components
+            Utils::CreateComponent<ROS2FrameComponent>(entity);
+            Utils::CreateComponent<PhysX::EditorStaticRigidBodyComponent>(entity);
+
+            // Create Imu component
+            if (Utils::CreateComponent<ROS2ImuSensorComponent>(entity, sensorConfiguration, imuConfiguration))
+            {
+                return AZ::Success();
+            }
+            else
+            {
+                return AZ::Failure(AZStd::string("Failed to create ROS2 Imu Sensor component"));
+            }
+        };
+
+        return importerHook;
+    }
+} // namespace ROS2::SDFormat

Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp

@@ -0,0 +1,87 @@
+/*
+ * 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 <Lidar/ROS2LidarSensorComponent.h>
+#include <ROS2/Frame/ROS2FrameComponent.h>
+#include <RobotImporter/SDFormat/ROS2SensorHooks.h>
+#include <RobotImporter/SDFormat/ROS2SensorHooksUtils.h>
+
+#include <sdf/Lidar.hh>
+#include <sdf/Sensor.hh>
+
+namespace ROS2::SDFormat
+{
+    SensorImporterHook ROS2SensorHooks::ROS2LidarSensor()
+    {
+        SensorImporterHook importerHook;
+        importerHook.m_sensorTypes = AZStd::unordered_set<sdf::SensorType>{ sdf::SensorType::LIDAR };
+        importerHook.m_supportedSensorParams = AZStd::unordered_set<AZStd::string>{
+            ">update_rate",
+            ">lidar>scan>horizontal>samples",
+            ">lidar>scan>horizontal>min_angle",
+            ">lidar>scan>horizontal>max_angle",
+            ">lidar>scan>vertical>samples",
+            ">lidar>scan>vertical>min_angle",
+            ">lidar>scan>vertical>max_angle",
+            ">lidar>range>min",
+            ">lidar>range>max",
+            // Gazebo-classic
+            ">ray>scan>horizontal>samples",
+            ">ray>scan>horizontal>min_angle",
+            ">ray>scan>horizontal>max_angle",
+            ">ray>scan>vertical>samples",
+            ">ray>scan>vertical>min_angle",
+            ">ray>scan>vertical>max_angle",
+            ">ray>range>min",
+            ">ray>range>max",
+        };
+        importerHook.m_pluginNames = AZStd::unordered_set<AZStd::string>{ "libgazebo_ros_ray_sensor.so", "libgazebo_ros_laser.so" };
+        importerHook.m_supportedPluginParams = AZStd::unordered_set<AZStd::string>{};
+        importerHook.m_sdfSensorToComponentCallback = [](AZ::Entity& entity,
+                                                         const sdf::Sensor& sdfSensor) -> SensorImporterHook::ConvertSensorOutcome
+        {
+            auto* lidarSensor = sdfSensor.LidarSensor();
+            if (!lidarSensor)
+            {
+                return AZ::Failure(AZStd::string("Failed to read parsed SDFormat data of %s Lidar sensor", sdfSensor.Name().c_str()));
+            }
+
+            SensorConfiguration sensorConfiguration;
+            sensorConfiguration.m_frequency = sdfSensor.UpdateRate();
+            const AZStd::string messageType = "sensor_msgs::msg::PointCloud2";
+            Utils::AddTopicConfiguration(sensorConfiguration, "pc", messageType, messageType);
+
+            LidarSensorConfiguration lidarConfiguration;
+            lidarConfiguration.m_lidarParameters.m_model = LidarTemplate::LidarModel::Custom3DLidar;
+            lidarConfiguration.m_lidarParameters.m_name = AZStd::string(sdfSensor.Name().c_str());
+            lidarConfiguration.m_lidarParameters.m_minHAngle = lidarSensor->HorizontalScanMinAngle().Degree();
+            lidarConfiguration.m_lidarParameters.m_maxHAngle = lidarSensor->HorizontalScanMaxAngle().Degree();
+            lidarConfiguration.m_lidarParameters.m_minVAngle = lidarSensor->VerticalScanMinAngle().Degree();
+            lidarConfiguration.m_lidarParameters.m_maxVAngle = lidarSensor->VerticalScanMaxAngle().Degree();
+            lidarConfiguration.m_lidarParameters.m_layers = lidarSensor->HorizontalScanSamples();
+            lidarConfiguration.m_lidarParameters.m_numberOfIncrements = lidarSensor->VerticalScanSamples();
+            lidarConfiguration.m_lidarParameters.m_minRange = lidarSensor->RangeMin();
+            lidarConfiguration.m_lidarParameters.m_maxRange = lidarSensor->RangeMax();
+
+            // Create required components
+            Utils::CreateComponent<ROS2FrameComponent>(entity);
+
+            // Create Lidar component
+            if (Utils::CreateComponent<ROS2LidarSensorComponent>(entity, sensorConfiguration, lidarConfiguration))
+            {
+                return AZ::Success();
+            }
+            else
+            {
+                return AZ::Failure(AZStd::string("Failed to create ROS2 Lidar Sensor component"));
+            }
+        };
+
+        return importerHook;
+    }
+} // namespace ROS2::SDFormat

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

@@ -14,6 +14,17 @@ namespace ROS2::SDFormat
 {
     namespace ROS2SensorHooks
     {
+        //! Retrieve a sensor importer hook which is used to map SDFormat sensor of type camera, depth or rgbd into O3DE
+        //! ROS2CameraSensorComponent
         SensorImporterHook ROS2CameraSensor();
+
+        //! Retrieve a sensor importer hook which is used to map SDFormat sensor of type navsat into O3DE ROS2GNSSSensorComponent
+        SensorImporterHook ROS2GNSSSensor();
+
+        //! Retrieve a sensor importer hook which is used to map SDFormat sensor of type imu into O3DE ROS2ImuSensorComponent
+        SensorImporterHook ROS2ImuSensor();
+
+        //! Retrieve a sensor importer hook which is used to map SDFormat sensor of type lidar into O3DE ROS2LidarSensorComponent
+        SensorImporterHook ROS2LidarSensor();
     } // namespace ROS2SensorHooks
 } // namespace ROS2::SDFormat

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

@@ -0,0 +1,28 @@
+/*
+ * 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 "ROS2SensorHooksUtils.h"
+#include <ROS2/Communication/TopicConfiguration.h>
+
+namespace ROS2::SDFormat
+{
+    namespace ROS2SensorHooks
+    {
+        void Utils::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 ROS2SensorHooks
+} // namespace ROS2::SDFormat

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

@@ -0,0 +1,78 @@
+/*
+ * 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/Component.h>
+#include <AzCore/Component/Entity.h>
+#include <AzCore/Memory/SystemAllocator.h>
+#include <AzCore/RTTI/RTTI.h>
+#include <AzCore/std/string/string.h>
+#include <AzToolsFramework/ToolsComponents/EditorComponentBase.h>
+#include <AzToolsFramework/ToolsComponents/GenericComponentWrapper.h>
+#include <ROS2/Sensor/SensorConfiguration.h>
+
+namespace ROS2::SDFormat
+{
+    namespace ROS2SensorHooks
+    {
+        namespace Utils
+        {
+            //! Add a ROS2 topic configuration to sensor parameters.
+            //! @param sensorConfig sensor's configuration which hosts multiple topic configurations
+            //! @param topic ROS2 topic name
+            //! @param messageType ROS2 message type
+            //! @param configName name under which topic configuration is stored in sensor's configuration
+            void AddTopicConfiguration(
+                SensorConfiguration& sensorConfig,
+                const AZStd::string& topic,
+                const AZStd::string& messageType,
+                const AZStd::string& configName);
+
+            //! Create a component and attach the component to the entity.
+            //! This method ensures that game components are wrapped into GenericComponentWrapper.
+            //! @param entity entity to which the new component is added
+            //! @param args constructor arguments used to create the new component
+            //! @return A pointer to the component. Returns a null pointer if the component could not be created.
+            template<class ComponentType, typename... Args>
+            AZ::Component* CreateComponent(AZ::Entity& entity, Args&&... args)
+            {
+                // Do not create a component if the same type is already added.
+                if (entity.FindComponent<ComponentType>())
+                {
+                    return nullptr;
+                }
+
+                // Create component.
+                // If it's not an "editor component" then wrap it in a GenericComponentWrapper.
+                AZ::Component* component = nullptr;
+                if (AZ::GetRttiHelper<ComponentType>() &&
+                    AZ::GetRttiHelper<ComponentType>()->IsTypeOf(AzToolsFramework::Components::EditorComponentBase::RTTI_Type()))
+                {
+                    component = aznew ComponentType(AZStd::forward<Args>(args)...);
+                }
+                else
+                {
+                    AZ::Component* gameComponent = aznew ComponentType(AZStd::forward<Args>(args)...);
+                    component = aznew AzToolsFramework::Components::GenericComponentWrapper(gameComponent);
+                }
+                AZ_Assert(component, "Failed to create component: %s", AZ::AzTypeInfo<ComponentType>::Name());
+
+                if (component)
+                {
+                    if (!entity.IsComponentReadyToAdd(component) || !entity.AddComponent(component))
+                    {
+                        delete component;
+                        component = nullptr;
+                    }
+                }
+                return component;
+            }
+        } // namespace Utils
+    } // namespace ROS2SensorHooks
+} // namespace ROS2::SDFormat

Gems/ROS2/Code/Tests/SdfParserTest.cpp

@@ -88,56 +88,165 @@ namespace UnitTest
                         </model>
                       </sdf>)";
         }
+
+        std::string GetSdfWithImuSensor()
+        {
+            return R"(<?xml version="1.0"?>
+                      <sdf version="1.6">
+                        <model name="test_imu_sensor">
+                          <link name="link1">
+                            <sensor name="link1_imu" type="imu">
+                              <always_on>true</always_on>
+                              <update_rate>200</update_rate>
+                              <imu>
+                                <angular_velocity>
+                                  <x>
+                                    <noise type="gaussian">
+                                      <mean>0.0</mean>
+                                      <stddev>2e-4</stddev>
+                                    </noise>
+                                  </x>
+                                  <y>
+                                    <noise type="gaussian">
+                                      <mean>0.0</mean>
+                                      <stddev>3e-4</stddev>
+                                    </noise>
+                                  </y>
+                                  <z>
+                                    <noise type="gaussian">
+                                      <mean>0.0</mean>
+                                      <stddev>4e-4</stddev>
+                                    </noise>
+                                  </z>
+                                </angular_velocity>
+                                <linear_acceleration>
+                                  <x>
+                                    <noise type="gaussian">
+                                      <mean>0.0</mean>
+                                      <stddev>1.7e-2</stddev>
+                                    </noise>
+                                  </x>
+                                  <y>
+                                    <noise type="gaussian">
+                                      <mean>0.0</mean>
+                                      <stddev>1.8e-2</stddev>
+                                    </noise>
+                                  </y>
+                                  <z>
+                                    <noise type="gaussian">
+                                      <mean>0.0</mean>
+                                      <stddev>1.9e-2</stddev>
+                                    </noise>
+                                  </z>
+                                </linear_acceleration>
+                              </imu>
+                              <plugin name="imu_plugin" filename="libgazebo_ros_imu_sensor.so">
+                                <ros>
+                                  <argument>~/out:=imu</argument>
+                                </ros>
+                              </plugin>
+                            </sensor>
+                          </link>
+                        </model>
+                      </sdf>)";
+        }
     };
 
     TEST_F(SdfParserTest, CheckModelCorrectness)
     {
-        const auto xmlStr = GetSdfWithTwoSensors();
-        const auto sdfRoot = Parse(xmlStr);
-        ASSERT_TRUE(sdfRoot);
-        const auto* sdfModel = sdfRoot->Model();
-        ASSERT_TRUE(sdfModel);
-
-        EXPECT_EQ(sdfModel->Name(), "test_two_sensors");
-        EXPECT_EQ(sdfModel->LinkCount(), 2U);
-
-        const auto* link1 = sdfModel->LinkByName("link1");
-        ASSERT_TRUE(link1);
-        EXPECT_EQ(link1->SensorCount(), 1U);
-        const auto* sensor1 = link1->SensorByIndex(0U);
-        ASSERT_TRUE(sensor1);
-        EXPECT_EQ(sensor1->Type(), sdf::SensorType::CAMERA);
-        EXPECT_EQ(sensor1->UpdateRate(), 10);
-        auto* cameraSensor = sensor1->CameraSensor();
-        ASSERT_TRUE(cameraSensor);
-        EXPECT_EQ(cameraSensor->ImageWidth(), 640);
-        EXPECT_EQ(cameraSensor->ImageHeight(), 480);
-        EXPECT_NEAR(cameraSensor->HorizontalFov().Radian(), 2.0, 1e-5);
-        EXPECT_NEAR(cameraSensor->NearClip(), 0.01, 1e-5);
-        EXPECT_NEAR(cameraSensor->FarClip(), 1000, 1e-5);
-        EXPECT_EQ(sensor1->Plugins().size(), 1U);
-        EXPECT_EQ(sensor1->Plugins().at(0).Name(), "camera_plug");
-        EXPECT_EQ(sensor1->Plugins().at(0).Filename(), "libgazebo_ros_camera.so");
-
-        const auto* link2 = sdfModel->LinkByName("link2");
-        ASSERT_TRUE(link2);
-        EXPECT_EQ(link2->SensorCount(), 1U);
-        const auto* sensor2 = link2->SensorByIndex(0U);
-        ASSERT_TRUE(sensor2);
-        EXPECT_EQ(sensor2->Type(), sdf::SensorType::LIDAR);
-        EXPECT_EQ(sensor2->UpdateRate(), 20);
-        auto* lidarSensor = sensor2->LidarSensor();
-        ASSERT_TRUE(lidarSensor);
-        EXPECT_EQ(lidarSensor->HorizontalScanSamples(), 640);
-        EXPECT_NEAR(lidarSensor->HorizontalScanResolution(), 1.0, 1e-5);
-        EXPECT_NEAR(lidarSensor->HorizontalScanMinAngle().Radian(), -2.0, 1e-5);
-        EXPECT_NEAR(lidarSensor->HorizontalScanMaxAngle().Radian(), 2.5, 1e-5);
-        EXPECT_NEAR(lidarSensor->RangeResolution(), 0.01, 1e-5);
-        EXPECT_NEAR(lidarSensor->RangeMin(), 0.02, 1e-5);
-        EXPECT_NEAR(lidarSensor->RangeMax(), 10.0, 1e-5);
-        EXPECT_EQ(sensor2->Plugins().size(), 1U);
-        EXPECT_EQ(sensor2->Plugins().at(0).Name(), "laser_plug");
-        EXPECT_EQ(sensor2->Plugins().at(0).Filename(), "librayplugin.so");
+        {
+            const auto xmlStr = GetSdfWithTwoSensors();
+            const auto sdfRoot = Parse(xmlStr);
+            ASSERT_TRUE(sdfRoot);
+            const auto* sdfModel = sdfRoot->Model();
+            ASSERT_TRUE(sdfModel);
+
+            EXPECT_EQ(sdfModel->Name(), "test_two_sensors");
+            EXPECT_EQ(sdfModel->LinkCount(), 2U);
+
+            const auto* link1 = sdfModel->LinkByName("link1");
+            ASSERT_TRUE(link1);
+            EXPECT_EQ(link1->SensorCount(), 1U);
+            const auto* sensor1 = link1->SensorByIndex(0U);
+            ASSERT_TRUE(sensor1);
+            EXPECT_EQ(sensor1->Type(), sdf::SensorType::CAMERA);
+            EXPECT_EQ(sensor1->UpdateRate(), 10);
+            auto* cameraSensor = sensor1->CameraSensor();
+            ASSERT_TRUE(cameraSensor);
+            EXPECT_EQ(cameraSensor->ImageWidth(), 640);
+            EXPECT_EQ(cameraSensor->ImageHeight(), 480);
+            EXPECT_NEAR(cameraSensor->HorizontalFov().Radian(), 2.0, 1e-5);
+            EXPECT_NEAR(cameraSensor->NearClip(), 0.01, 1e-5);
+            EXPECT_NEAR(cameraSensor->FarClip(), 1000, 1e-5);
+            EXPECT_EQ(sensor1->Plugins().size(), 1U);
+            EXPECT_EQ(sensor1->Plugins().at(0).Name(), "camera_plug");
+            EXPECT_EQ(sensor1->Plugins().at(0).Filename(), "libgazebo_ros_camera.so");
+
+            const auto* link2 = sdfModel->LinkByName("link2");
+            ASSERT_TRUE(link2);
+            EXPECT_EQ(link2->SensorCount(), 1U);
+            const auto* sensor2 = link2->SensorByIndex(0U);
+            ASSERT_TRUE(sensor2);
+            EXPECT_EQ(sensor2->Type(), sdf::SensorType::LIDAR);
+            EXPECT_EQ(sensor2->UpdateRate(), 20);
+            auto* lidarSensor = sensor2->LidarSensor();
+            ASSERT_TRUE(lidarSensor);
+            EXPECT_EQ(lidarSensor->HorizontalScanSamples(), 640);
+            EXPECT_NEAR(lidarSensor->HorizontalScanResolution(), 1.0, 1e-5);
+            EXPECT_NEAR(lidarSensor->HorizontalScanMinAngle().Radian(), -2.0, 1e-5);
+            EXPECT_NEAR(lidarSensor->HorizontalScanMaxAngle().Radian(), 2.5, 1e-5);
+            EXPECT_NEAR(lidarSensor->RangeResolution(), 0.01, 1e-5);
+            EXPECT_NEAR(lidarSensor->RangeMin(), 0.02, 1e-5);
+            EXPECT_NEAR(lidarSensor->RangeMax(), 10.0, 1e-5);
+            EXPECT_EQ(sensor2->Plugins().size(), 1U);
+            EXPECT_EQ(sensor2->Plugins().at(0).Name(), "laser_plug");
+            EXPECT_EQ(sensor2->Plugins().at(0).Filename(), "librayplugin.so");
+        }
+
+        {
+            const auto xmlStr = GetSdfWithImuSensor();
+            const auto sdfRoot = Parse(xmlStr);
+            ASSERT_TRUE(sdfRoot);
+            const auto* sdfModel = sdfRoot->Model();
+            ASSERT_TRUE(sdfModel);
+
+            EXPECT_EQ(sdfModel->Name(), "test_imu_sensor");
+            EXPECT_EQ(sdfModel->LinkCount(), 1U);
+
+            const auto* link1 = sdfModel->LinkByName("link1");
+            ASSERT_TRUE(link1);
+            EXPECT_EQ(link1->SensorCount(), 1U);
+            const auto* sensor1 = link1->SensorByIndex(0U);
+            ASSERT_TRUE(sensor1);
+            EXPECT_EQ(sensor1->Type(), sdf::SensorType::IMU);
+            EXPECT_EQ(sensor1->Name(), "link1_imu");
+            EXPECT_EQ(sensor1->UpdateRate(), 200);
+
+            auto* imuSensor = sensor1->ImuSensor();
+            ASSERT_TRUE(imuSensor);
+            EXPECT_EQ(imuSensor->AngularVelocityXNoise().Type(), sdf::NoiseType::GAUSSIAN);
+            EXPECT_EQ(imuSensor->AngularVelocityYNoise().Type(), sdf::NoiseType::GAUSSIAN);
+            EXPECT_EQ(imuSensor->AngularVelocityZNoise().Type(), sdf::NoiseType::GAUSSIAN);
+            EXPECT_EQ(imuSensor->AngularVelocityXNoise().Mean(), 0.0);
+            EXPECT_EQ(imuSensor->AngularVelocityYNoise().Mean(), 0.0);
+            EXPECT_EQ(imuSensor->AngularVelocityZNoise().Mean(), 0.0);
+            EXPECT_NEAR(imuSensor->AngularVelocityXNoise().StdDev(), 2e-4, 1e-5);
+            EXPECT_NEAR(imuSensor->AngularVelocityYNoise().StdDev(), 3e-4, 1e-5);
+            EXPECT_NEAR(imuSensor->AngularVelocityZNoise().StdDev(), 4e-4, 1e-5);
+            EXPECT_EQ(imuSensor->LinearAccelerationXNoise().Type(), sdf::NoiseType::GAUSSIAN);
+            EXPECT_EQ(imuSensor->LinearAccelerationYNoise().Type(), sdf::NoiseType::GAUSSIAN);
+            EXPECT_EQ(imuSensor->LinearAccelerationZNoise().Type(), sdf::NoiseType::GAUSSIAN);
+            EXPECT_EQ(imuSensor->LinearAccelerationXNoise().Mean(), 0.0);
+            EXPECT_EQ(imuSensor->LinearAccelerationYNoise().Mean(), 0.0);
+            EXPECT_EQ(imuSensor->LinearAccelerationZNoise().Mean(), 0.0);
+            EXPECT_NEAR(imuSensor->LinearAccelerationXNoise().StdDev(), 1.7e-2, 1e-5);
+            EXPECT_NEAR(imuSensor->LinearAccelerationYNoise().StdDev(), 1.8e-2, 1e-5);
+            EXPECT_NEAR(imuSensor->LinearAccelerationZNoise().StdDev(), 1.9e-2, 1e-5);
+
+            EXPECT_EQ(sensor1->Plugins().size(), 1U);
+            EXPECT_EQ(sensor1->Plugins().at(0).Name(), "imu_plugin");
+            EXPECT_EQ(sensor1->Plugins().at(0).Filename(), "libgazebo_ros_imu_sensor.so");
+        }
     }
 
     TEST_F(SdfParserTest, RobotImporterUtils)
@@ -212,30 +321,55 @@ namespace UnitTest
 
     TEST_F(SdfParserTest, SensorPluginImporterHookCheck)
     {
-        const auto xmlStr = GetSdfWithTwoSensors();
-        const auto sdfRoot = Parse(xmlStr);
-        const auto* sdfModel = sdfRoot->Model();
-        const sdf::ElementPtr cameraElement = sdfModel->LinkByName("link1")->SensorByIndex(0U)->Element();
-        const auto& importerHook = ROS2::SDFormat::ROS2SensorHooks::ROS2CameraSensor();
+        {
+            const auto xmlStr = GetSdfWithTwoSensors();
+            const auto sdfRoot = Parse(xmlStr);
+            const auto* sdfModel = sdfRoot->Model();
+            const sdf::ElementPtr cameraElement = sdfModel->LinkByName("link1")->SensorByIndex(0U)->Element();
+            const auto& cameraImporterHook = ROS2::SDFormat::ROS2SensorHooks::ROS2CameraSensor();
 
-        const auto& unsupportedCameraParams =
-            ROS2::Utils::SDFormat::GetUnsupportedParams(cameraElement, importerHook.m_supportedSensorParams);
-        EXPECT_EQ(unsupportedCameraParams.size(), 1U);
-        EXPECT_EQ(unsupportedCameraParams[0U], ">pose");
+            const auto& unsupportedCameraParams =
+                ROS2::Utils::SDFormat::GetUnsupportedParams(cameraElement, cameraImporterHook.m_supportedSensorParams);
+            EXPECT_EQ(unsupportedCameraParams.size(), 1U);
+            EXPECT_EQ(unsupportedCameraParams[0U], ">pose");
 
-        sdf::Plugin plug;
-        plug.SetName("test_camera");
-        plug.SetFilename("libgazebo_ros_camera.so");
-        EXPECT_TRUE(ROS2::Utils::SDFormat::IsPluginSupported(plug, importerHook.m_pluginNames));
-        plug.SetFilename("/usr/lib/libgazebo_ros_openni_kinect.so");
-        EXPECT_TRUE(ROS2::Utils::SDFormat::IsPluginSupported(plug, importerHook.m_pluginNames));
-        plug.SetFilename("~/dev/libgazebo_ros_imu.so");
-        EXPECT_FALSE(ROS2::Utils::SDFormat::IsPluginSupported(plug, importerHook.m_pluginNames));
-        plug.SetFilename("libgazebo_ros_camera");
-        EXPECT_FALSE(ROS2::Utils::SDFormat::IsPluginSupported(plug, importerHook.m_pluginNames));
-
-        EXPECT_TRUE(importerHook.m_sensorTypes.contains(sdf::SensorType::CAMERA));
-        EXPECT_TRUE(importerHook.m_sensorTypes.contains(sdf::SensorType::DEPTH_CAMERA));
-        EXPECT_FALSE(importerHook.m_sensorTypes.contains(sdf::SensorType::GPS));
+            sdf::Plugin plug;
+            plug.SetName("test_camera");
+            plug.SetFilename("libgazebo_ros_camera.so");
+            EXPECT_TRUE(ROS2::Utils::SDFormat::IsPluginSupported(plug, cameraImporterHook.m_pluginNames));
+            plug.SetFilename("/usr/lib/libgazebo_ros_openni_kinect.so");
+            EXPECT_TRUE(ROS2::Utils::SDFormat::IsPluginSupported(plug, cameraImporterHook.m_pluginNames));
+            plug.SetFilename("~/dev/libgazebo_ros_imu.so");
+            EXPECT_FALSE(ROS2::Utils::SDFormat::IsPluginSupported(plug, cameraImporterHook.m_pluginNames));
+            plug.SetFilename("libgazebo_ros_camera");
+            EXPECT_FALSE(ROS2::Utils::SDFormat::IsPluginSupported(plug, cameraImporterHook.m_pluginNames));
+
+            EXPECT_TRUE(cameraImporterHook.m_sensorTypes.contains(sdf::SensorType::CAMERA));
+            EXPECT_TRUE(cameraImporterHook.m_sensorTypes.contains(sdf::SensorType::DEPTH_CAMERA));
+            EXPECT_FALSE(cameraImporterHook.m_sensorTypes.contains(sdf::SensorType::GPS));
+
+            const sdf::ElementPtr lidarElement = sdfModel->LinkByName("link2")->SensorByIndex(0U)->Element();
+            const auto& lidarImporterHook = ROS2::SDFormat::ROS2SensorHooks::ROS2LidarSensor();
+            const auto& unsupportedLidarParams =
+                ROS2::Utils::SDFormat::GetUnsupportedParams(lidarElement, lidarImporterHook.m_supportedSensorParams);
+            EXPECT_EQ(unsupportedLidarParams.size(), 5U);
+            EXPECT_EQ(unsupportedLidarParams[0U], ">always_on");
+            EXPECT_EQ(unsupportedLidarParams[1U], ">visualize");
+            EXPECT_EQ(unsupportedLidarParams[2U], ">pose");
+            EXPECT_EQ(unsupportedLidarParams[3U], ">ray>scan>horizontal>resolution");
+            EXPECT_EQ(unsupportedLidarParams[4U], ">ray>range>resolution");
+        }
+        {
+            const auto xmlStr = GetSdfWithImuSensor();
+            const auto sdfRoot = Parse(xmlStr);
+            const auto* sdfModel = sdfRoot->Model();
+            const sdf::ElementPtr imuElement = sdfModel->LinkByName("link1")->SensorByIndex(0U)->Element();
+            const auto& importerHook = ROS2::SDFormat::ROS2SensorHooks::ROS2ImuSensor();
+
+            const auto& unsupportedImuParams =
+                ROS2::Utils::SDFormat::GetUnsupportedParams(imuElement, importerHook.m_supportedSensorParams);
+            EXPECT_EQ(unsupportedImuParams.size(), 1U);
+            EXPECT_EQ(unsupportedImuParams[0U], ">always_on");
+        }
     }
 } // namespace UnitTest

Gems/ROS2/Code/ros2_editor_files.cmake

@@ -28,7 +28,12 @@ set(FILES
     Source/RobotImporter/RobotImporterWidget.h
     Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp
     Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.h
-    Source/RobotImporter/SDFormat/ROS2SensorHooks.cpp
+    Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp
+    Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp
+    Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp
+    Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp
+    Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.cpp
+    Source/RobotImporter/SDFormat/ROS2SensorHooksUtils.h
     Source/RobotImporter/SDFormat/ROS2SensorHooks.h
     Source/RobotImporter/URDF/ArticulationsMaker.cpp
     Source/RobotImporter/URDF/ArticulationsMaker.h