Просмотр исходного кода

Add sensor configuration API (#914)

* Remove SensorHelper from API
* Move interface TypeIDs to a separate header
* Add ImuSensorConfigurationRequestBus
* Add WheelOdometryConfigurationRequestBus
* Add CameraConfigurationRequestBus
* Add LidarConfigurationRequestBus
* Fix camera and imu configuration (reconfigure correctly)
* Add WheelOdometryConfiguration and serialization (test fails) as in (#894)
* Extend LidarConfigurationRequestBus (reconfigure params correctly)
* Fix tests: reshuffle files in ROS2 cmake files
* Port changes from (#894)

Signed-off-by: Jan Hanca <[email protected]>
Jan Hanca 1 месяц назад
Родитель
Сommit
0194b6e092
74 измененных файлов с 1951 добавлено и 534 удалено
  1. 4 4
      Gems/ROS2/Assets/seedList.seed
  2. 0 1
      Gems/ROS2/Code/Source/Clients/ROS2Module.cpp
  3. 0 1
      Gems/ROS2/Code/Source/Clients/ROS2SystemComponent.cpp
  4. 0 3
      Gems/ROS2/Code/Source/ROS2ModuleInterface.h
  5. 0 1
      Gems/ROS2/Code/Source/Tools/ROS2EditorSystemComponent.h
  6. 0 1
      Gems/ROS2/Code/ros2_api_files.cmake
  7. 1 0
      Gems/ROS2/Code/ros2_editor_api_files.cmake
  8. 5 5
      Gems/ROS2/Code/ros2_editor_private_files.cmake
  9. 2 0
      Gems/ROS2/Code/ros2_editor_shared_files.cmake
  10. 0 2
      Gems/ROS2/Code/ros2_private_files.cmake
  11. 2 3
      Gems/ROS2/Code/ros2_shared_api_files.cmake
  12. 1 0
      Gems/ROS2/Code/ros2_shared_files.cmake
  13. 2 2
      Gems/ROS2Controllers/Code/CMakeLists.txt
  14. 1 1
      Gems/ROS2RobotImporter/Code/CMakeLists.txt
  15. 4 1
      Gems/ROS2Sensors/Code/CMakeLists.txt
  16. 0 44
      Gems/ROS2Sensors/Code/Include/ROS2Sensors/Camera/CameraCalibrationRequestBus.h
  17. 81 0
      Gems/ROS2Sensors/Code/Include/ROS2Sensors/Camera/CameraConfigurationRequestBus.h
  18. 4 3
      Gems/ROS2Sensors/Code/Include/ROS2Sensors/Camera/CameraSensorConfiguration.h
  19. 72 0
      Gems/ROS2Sensors/Code/Include/ROS2Sensors/Imu/ImuConfigurationRequestBus.h
  20. 5 5
      Gems/ROS2Sensors/Code/Include/ROS2Sensors/Imu/ImuSensorConfiguration.h
  21. 2 1
      Gems/ROS2Sensors/Code/Include/ROS2Sensors/Lidar/ClassSegmentationBus.h
  22. 128 0
      Gems/ROS2Sensors/Code/Include/ROS2Sensors/Lidar/LidarConfigurationRequestBus.h
  23. 2 1
      Gems/ROS2Sensors/Code/Include/ROS2Sensors/Lidar/LidarRaycasterBus.h
  24. 2 1
      Gems/ROS2Sensors/Code/Include/ROS2Sensors/Lidar/LidarRegistrarBus.h
  25. 9 8
      Gems/ROS2Sensors/Code/Include/ROS2Sensors/Lidar/LidarSensorConfiguration.h
  26. 2 1
      Gems/ROS2Sensors/Code/Include/ROS2Sensors/Lidar/LidarSystemBus.h
  27. 3 2
      Gems/ROS2Sensors/Code/Include/ROS2Sensors/Lidar/LidarTemplate.h
  28. 1 1
      Gems/ROS2Sensors/Code/Include/ROS2Sensors/Lidar/LidarTemplateUtils.h
  29. 3 1
      Gems/ROS2Sensors/Code/Include/ROS2Sensors/Lidar/SegmentationClassConfiguration.h
  30. 2 1
      Gems/ROS2Sensors/Code/Include/ROS2Sensors/Odometry/ROS2OdometryCovariance.h
  31. 48 0
      Gems/ROS2Sensors/Code/Include/ROS2Sensors/Odometry/WheelOdometryConfigurationRequestBus.h
  32. 32 0
      Gems/ROS2Sensors/Code/Include/ROS2Sensors/Odometry/WheelOdometrySensorConfiguration.h
  33. 26 4
      Gems/ROS2Sensors/Code/Include/ROS2Sensors/ROS2SensorsTypeIds.h
  34. 2 3
      Gems/ROS2Sensors/Code/Include/ROS2Sensors/Sensor/ROS2SensorComponentBase.h
  35. 2 1
      Gems/ROS2Sensors/Code/Include/ROS2Sensors/Sensor/SensorConfiguration.h
  36. 1 2
      Gems/ROS2Sensors/Code/Include/ROS2Sensors/Sensor/SensorConfigurationRequestBus.h
  37. 0 30
      Gems/ROS2Sensors/Code/Include/ROS2Sensors/Sensor/SensorHelper.h
  38. 1 1
      Gems/ROS2Sensors/Code/Platform/Linux/PAL_linux.cmake
  39. 1 1
      Gems/ROS2Sensors/Code/Source/Camera/CameraSensorConfiguration.cpp
  40. 1 1
      Gems/ROS2Sensors/Code/Source/Camera/CameraSensorDescription.h
  41. 160 35
      Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSensorComponent.cpp
  42. 25 9
      Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSensorComponent.h
  43. 122 27
      Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp
  44. 23 10
      Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSensorEditorComponent.h
  45. 5 1
      Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSystemComponent.cpp
  46. 1 0
      Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSystemComponent.h
  47. 5 0
      Gems/ROS2Sensors/Code/Source/Camera/ROS2EditorCameraSystemComponent.cpp
  48. 1 0
      Gems/ROS2Sensors/Code/Source/Camera/ROS2EditorCameraSystemComponent.h
  49. 1 1
      Gems/ROS2Sensors/Code/Source/Imu/ImuSensorConfiguration.cpp
  50. 99 3
      Gems/ROS2Sensors/Code/Source/Imu/ROS2ImuSensorComponent.cpp
  51. 27 6
      Gems/ROS2Sensors/Code/Source/Imu/ROS2ImuSensorComponent.h
  52. 3 3
      Gems/ROS2Sensors/Code/Source/Lidar/LidarCore.h
  53. 1 1
      Gems/ROS2Sensors/Code/Source/Lidar/LidarRaycaster.cpp
  54. 1 1
      Gems/ROS2Sensors/Code/Source/Lidar/LidarRegistrarSystemComponent.cpp
  55. 11 3
      Gems/ROS2Sensors/Code/Source/Lidar/LidarSensorConfiguration.cpp
  56. 1 1
      Gems/ROS2Sensors/Code/Source/Lidar/LidarTemplate.cpp
  57. 1 1
      Gems/ROS2Sensors/Code/Source/Lidar/LidarTemplateUtils.cpp
  58. 164 0
      Gems/ROS2Sensors/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp
  59. 30 4
      Gems/ROS2Sensors/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h
  60. 211 0
      Gems/ROS2Sensors/Code/Source/Lidar/ROS2LidarSensorComponent.cpp
  61. 37 7
      Gems/ROS2Sensors/Code/Source/Lidar/ROS2LidarSensorComponent.h
  62. 1 2
      Gems/ROS2Sensors/Code/Source/Odometry/ROS2OdometryCovariance.cpp
  63. 0 140
      Gems/ROS2Sensors/Code/Source/Odometry/ROS2WheelOdometry.cpp
  64. 0 49
      Gems/ROS2Sensors/Code/Source/Odometry/ROS2WheelOdometry.h
  65. 251 0
      Gems/ROS2Sensors/Code/Source/Odometry/ROS2WheelOdometrySensorComponent.cpp
  66. 76 0
      Gems/ROS2Sensors/Code/Source/Odometry/ROS2WheelOdometrySensorComponent.h
  67. 43 0
      Gems/ROS2Sensors/Code/Source/Odometry/WheelOdometrySensorConfiguration.cpp
  68. 1 5
      Gems/ROS2Sensors/Code/Source/ROS2SensorsModuleInterface.cpp
  69. 0 75
      Gems/ROS2Sensors/Code/Source/Sensor/SensorHelpers.cpp
  70. 138 0
      Gems/ROS2Sensors/Code/Tests/Odometry/WheelOdometrySensorConfigurationSerializationTest.cpp
  71. 45 1
      Gems/ROS2Sensors/Code/Tests/Tools/ROS2SensorsEditorTest.cpp
  72. 11 3
      Gems/ROS2Sensors/Code/ros2sensors_api_files.cmake
  73. 1 0
      Gems/ROS2Sensors/Code/ros2sensors_editor_tests_files.cmake
  74. 3 9
      Gems/ROS2Sensors/Code/ros2sensors_private_files.cmake

+ 4 - 4
Gems/ROS2/Assets/seedList.seed

@@ -18,6 +18,7 @@ include(${pal_dir}/PAL_${PAL_PLATFORM_NAME_LOWERCASE}.cmake)
 
 # Check to see if building the Gem Modules are supported for the current platform
 if(NOT PAL_TRAIT_ROS2_SUPPORTED)
+    message(FATAL_ERROR "The ROS2 Gem is not supported on ${PAL_PLATFORM_NAME}")
     return()
 endif()
 
@@ -87,7 +88,7 @@ ly_add_target(
             AZ::AzCore
 )
 
-target_interface_depends_on_ros2_packages(${gem_name}.API rclcpp builtin_interfaces geometry_msgs)
+target_interface_depends_on_ros2_packages(${gem_name}.API rclcpp builtin_interfaces control_msgs geometry_msgs)
 
 # The ${gem_name}.Private.Object target is an internal target
 # It should not be used outside of this Gems CMakeLists.txt
@@ -128,8 +129,8 @@ ly_add_target(
     BUILD_DEPENDENCIES
         PUBLIC
             Gem::${gem_name}.API
-            Gem::${gem_name}.Static
         PRIVATE
+            Gem::${gem_name}.Static
             Gem::${gem_name}.Private.Object
 )
 
@@ -199,8 +200,7 @@ if(PAL_TRAIT_BUILD_HOST_TOOLS)
         BUILD_DEPENDENCIES
             PUBLIC
                 AZ::AzToolsFramework
-                ${gem_name}.Private.Object
-                ${gem_name}.Static
+                Gem::${gem_name}.Static
     )
 
     ly_add_target(

+ 0 - 1
Gems/ROS2/Code/Source/Clients/ROS2Module.cpp

@@ -6,7 +6,6 @@
  *
  */
 
-#include "ROS2SystemComponent.h"
 #include <ROS2/ROS2TypeIds.h>
 #include <ROS2ModuleInterface.h>
 

+ 0 - 1
Gems/ROS2/Code/Source/Clients/ROS2SystemComponent.cpp

@@ -73,7 +73,6 @@ namespace ROS2
     void ROS2SystemComponent::GetRequiredServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required)
     {
         required.push_back(AZ_CRC("AssetDatabaseService", 0x3abf5601));
-        required.push_back(AZ_CRC("RPISystem", 0xf2add773));
     }
 
     void ROS2SystemComponent::GetDependentServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& dependent)

+ 0 - 3
Gems/ROS2/Code/Source/ROS2ModuleInterface.h

@@ -23,9 +23,6 @@ namespace ROS2
 
         ROS2ModuleInterface();
 
-        /**
-         * Add required SystemComponents to the SystemEntity.
-         */
         AZ::ComponentTypeList GetRequiredSystemComponents() const override;
     };
 } // namespace ROS2

+ 0 - 1
Gems/ROS2/Code/Source/Tools/ROS2EditorSystemComponent.h

@@ -9,7 +9,6 @@
 
 #include <AzToolsFramework/API/ToolsApplicationAPI.h>
 #include <AzToolsFramework/Entity/EditorEntityContextBus.h>
-
 #include <Clients/ROS2SystemComponent.h>
 
 namespace ROS2

+ 0 - 1
Gems/ROS2/Code/ros2_api_files.cmake

@@ -5,7 +5,6 @@
 
 set(FILES
     Include/ROS2/ROS2Bus.h
-    Include/ROS2/ROS2GemUtilities.h
     Include/ROS2/ROS2TypeIds.h
     Include/ROS2/Clock/ROS2Clock.h
     Include/ROS2/Clock/ITimeSource.h

+ 1 - 0
Gems/ROS2/Code/ros2_editor_api_files.cmake

@@ -5,4 +5,5 @@
 
 set(FILES
     Include/ROS2/Frame/ROS2FrameEditorComponent.h
+    Include/ROS2/ROS2GemUtilities.h
 )

+ 5 - 5
Gems/ROS2/Code/ros2_editor_private_files.cmake

@@ -4,13 +4,13 @@
 # SPDX-License-Identifier: Apache-2.0 OR MIT
 
 set(FILES
-    Source/Tools/ROS2EditorSystemComponent.cpp
-    Source/Tools/ROS2EditorSystemComponent.h
+    Source/Frame/ROS2FrameSystemComponent.cpp
+    Source/Frame/ROS2FrameSystemComponent.h
+    Source/Frame/ROS2FrameSystemBus.h
     Source/Spawner/ROS2SpawnerEditorComponent.cpp
     Source/Spawner/ROS2SpawnerEditorComponent.h
     Source/Spawner/ROS2SpawnPointEditorComponent.cpp
     Source/Spawner/ROS2SpawnPointEditorComponent.h
-    Source/Frame/ROS2FrameSystemComponent.cpp
-    Source/Frame/ROS2FrameSystemComponent.h
-    Source/Frame/ROS2FrameSystemBus.h
+    Source/Tools/ROS2EditorSystemComponent.cpp
+    Source/Tools/ROS2EditorSystemComponent.h
 )

+ 2 - 0
Gems/ROS2/Code/ros2_editor_shared_files.cmake

@@ -4,5 +4,7 @@
 # SPDX-License-Identifier: Apache-2.0 OR MIT
 
 set(FILES
+    Source/Frame/ROS2FrameEditorComponent.cpp
     Source/Tools/ROS2EditorModule.cpp
+    Source/ROS2GemUtilities.cpp
 )

+ 0 - 2
Gems/ROS2/Code/ros2_private_files.cmake

@@ -4,8 +4,6 @@
 # SPDX-License-Identifier: Apache-2.0 OR MIT
 
 set(FILES
-    Source/ROS2ModuleInterface.cpp
-    Source/ROS2ModuleInterface.h
     ../Assets/Passes/PipelineRenderToTextureROSColor.pass
     ../Assets/Passes/PipelineRenderToTextureROSDepth.pass
     ../Assets/Passes/PipelineROSColor.pass

+ 2 - 3
Gems/ROS2/Code/ros2_shared_api_files.cmake

@@ -4,7 +4,6 @@
 # SPDX-License-Identifier: Apache-2.0 OR MIT
 
 set(FILES
-    Source/Clients/ROS2Module.cpp
     Source/Clients/ROS2SystemComponent.cpp
     Source/Clients/ROS2SystemComponent.h
     Source/Clock/ROS2Clock.cpp
@@ -17,11 +16,11 @@ set(FILES
     Source/Frame/NamespaceConfiguration.cpp
     Source/Frame/ROS2FrameComponent.cpp
     Source/Frame/ROS2FrameConfiguration.cpp
-    Source/Frame/ROS2FrameEditorComponent.cpp
     Source/Frame/ROS2Transform.cpp
+    Source/ROS2ModuleInterface.cpp
+    Source/ROS2ModuleInterface.h
     Source/Sensor/Events/PhysicsBasedSource.cpp
     Source/Sensor/Events/TickBasedSource.cpp
     Source/Utilities/ROS2Conversions.cpp
     Source/Utilities/ROS2Names.cpp
-    Source/ROS2GemUtilities.cpp
 )

+ 1 - 0
Gems/ROS2/Code/ros2_shared_files.cmake

@@ -4,4 +4,5 @@
 # SPDX-License-Identifier: Apache-2.0 OR MIT
 
 set(FILES
+    Source/Clients/ROS2Module.cpp
 )

+ 2 - 2
Gems/ROS2Controllers/Code/CMakeLists.txt

@@ -36,7 +36,7 @@ ly_add_target(
             Include
     BUILD_DEPENDENCIES
         INTERFACE
-           AZ::AzCore
+            AZ::AzCore
 )
 
 # The ${gem_name}.Private.Object target is an internal target
@@ -60,7 +60,7 @@ ly_add_target(
             Gem::ImGui.Static
             Gem::LmbrCentral.API
             Gem::PhysX5.Static
-            Gem::ROS2
+            Gem::ROS2.Static
             Gem::StartingPointInput
 )
 

+ 1 - 1
Gems/ROS2RobotImporter/Code/CMakeLists.txt

@@ -57,7 +57,7 @@ ly_add_target(
         PUBLIC
             AZ::AzCore
             AZ::AzFramework
-            Gem::ROS2
+            Gem::ROS2.Static
 )
 
 # Here add ${gem_name} target, it depends on the Private Object library and Public API interface

+ 4 - 1
Gems/ROS2Sensors/Code/CMakeLists.txt

@@ -79,7 +79,7 @@ ly_add_target(
             AZ::AzFramework
             Gem::Atom_Feature_Common.Public # Camera Sensor
             Gem::PhysX5.Static # Imu Sensor
-            Gem::ROS2
+            Gem::ROS2.Static
             Gem::LevelGeoreferencing.API
             ${gem_name}.Lidar.Static
 )
@@ -267,6 +267,9 @@ if(PAL_TRAIT_BUILD_TESTS_SUPPORTED)
                 BUILD_DEPENDENCIES
                     PRIVATE
                         AZ::AzTest
+                        AZ::AzTestShared
+                        AZ::AzToolsFramework
+                        AZ::AzManipulatorTestFramework.Static
                         Gem::${gem_name}.Editor.Private.Object
             )
 

+ 0 - 44
Gems/ROS2Sensors/Code/Include/ROS2Sensors/Camera/CameraCalibrationRequestBus.h

@@ -1,44 +0,0 @@
-/*
- * 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/EntityId.h>
-#include <AzCore/EBus/EBus.h>
-#include <AzCore/Interface/Interface.h>
-#include <AzCore/Math/Matrix4x4.h>
-
-namespace ROS2Sensors
-{
-    //! Interface allows to obtain intrinsic parameters of the camera. To obtain extrinsic parameters use TransformProviderRequestBus.
-    class CameraCalibrationRequest : public AZ::EBusTraits
-    {
-    public:
-        using BusIdType = AZ::EntityId;
-        static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById;
-
-        //! Returns the intrinsic calibration matrix of the camera as:
-        //!  [fx  0 cx]
-        //!  [ 0 fy cy]
-        //!  [ 0  0  1]
-        //! where:
-        //!  - fx, fy : the focal lengths in meters
-        //!  - cx, cy : principal point in pixels.
-        virtual AZ::Matrix3x3 GetCameraMatrix() const = 0;
-
-        //! Returns the width of the camera sensor in pixels
-        virtual int GetWidth() const = 0;
-
-        //! Returns the height of the camera sensor in pixels
-        virtual int GetHeight() const = 0;
-
-        //! Returns the vertical field of view of the camera in degrees
-        virtual float GetVerticalFOV() const = 0;
-    };
-
-    using CameraCalibrationRequestBus = AZ::EBus<CameraCalibrationRequest>;
-} // namespace ROS2Sensors

+ 81 - 0
Gems/ROS2Sensors/Code/Include/ROS2Sensors/Camera/CameraConfigurationRequestBus.h

@@ -0,0 +1,81 @@
+/*
+ * 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/EBus/EBus.h>
+#include <AzCore/Math/Matrix3x3.h>
+#include <AzCore/Math/Vector3.h>
+#include <AzCore/RTTI/RTTI.h>
+#include <ROS2Sensors/Camera/CameraSensorConfiguration.h>
+
+namespace ROS2Sensors
+{
+    //! Interface that allows to get and set Camera sensor's configuration.
+    class CameraConfigurationRequest : public AZ::EBusTraits
+    {
+    public:
+        using BusIdType = AZ::EntityId;
+        static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById;
+
+        //! Returns the current configuration of the component.
+        virtual const CameraSensorConfiguration GetConfiguration() = 0;
+
+        //! Sets the configuration of the component.
+        //! Each component should handle the configuration change without fully reinitializing the ROS2 publisher.
+        //! This will allow to change the configuration of the component at runtime.
+        //! Note: this method does not verify the configuration, so it is up to the caller to ensure that the configuration is valid.
+        //! @param configuration The new configuration to set.
+        virtual void SetConfiguration(const CameraSensorConfiguration& configuration) = 0;
+
+        //! Returns the intrinsic calibration matrix of the camera as:
+        //!  [fx  0 cx]
+        //!  [ 0 fy cy]
+        //!  [ 0  0  1]
+        //! where:
+        //!  - fx, fy : the focal lengths in meters
+        //!  - cx, cy : principal point in pixels.
+        virtual AZ::Matrix3x3 GetCameraMatrix() = 0;
+
+        //! Gets the vertical field of view in degrees.
+        virtual float GetVerticalFOV() = 0;
+        //! Sets the vertical field of view in degrees.
+        virtual void SetVerticalFOV(float value) = 0;
+
+        //! Gets the camera image width in pixels.
+        virtual int GetWidth() = 0;
+        //! Sets the camera image width in pixels.
+        virtual void SetWidth(int value) = 0;
+
+        //! Gets the camera image height in pixels.
+        virtual int GetHeight() = 0;
+        //! Sets the camera image height in pixels.
+        virtual void SetHeight(int value) = 0;
+
+        //! Checks if the color camera is enabled.
+        virtual bool IsColorCamera() = 0;
+        //! Enables or disables the color camera.
+        virtual void SetColorCamera(bool value) = 0;
+
+        //! Checks if the depth camera is enabled.
+        virtual bool IsDepthCamera() = 0;
+        //! Enables or disables the depth camera.
+        virtual void SetDepthCamera(bool value) = 0;
+
+        //! Gets the near clip distance of the camera.
+        virtual float GetNearClipDistance() = 0;
+        //! Sets the near clip distance of the camera.
+        virtual void SetNearClipDistance(float value) = 0;
+
+        //! Gets the far clip distance of the camera.
+        virtual float GetFarClipDistance() = 0;
+        //! Sets the far clip distance of the camera.
+        virtual void SetFarClipDistance(float value) = 0;
+    };
+
+    using CameraConfigurationRequestBus = AZ::EBus<CameraConfigurationRequest>;
+} // namespace ROS2Sensors

+ 4 - 3
Gems/ROS2Sensors/Code/Source/Camera/CameraSensorConfiguration.h → Gems/ROS2Sensors/Code/Include/ROS2Sensors/Camera/CameraSensorConfiguration.h

@@ -10,21 +10,22 @@
 #include <AzCore/RTTI/RTTI.h>
 #include <AzCore/Serialization/SerializeContext.h>
 #include <AzCore/std/string/string.h>
+#include <ROS2Sensors/ROS2SensorsTypeIds.h>
 
 namespace ROS2Sensors
 {
     //! A structure capturing configuration of a single camera sensor with up to two image sources (color and depth).
     struct CameraSensorConfiguration
     {
-        AZ_TYPE_INFO(CameraSensorConfiguration, "{386A2640-442B-473D-BC2A-665D049D7EF5}");
+        AZ_TYPE_INFO(CameraSensorConfiguration, CameraSensorConfigurationTypeId);
         static void Reflect(AZ::ReflectContext* context);
 
         static constexpr int m_minWidth = 1;
         static constexpr int m_minHeight = 1;
+        static constexpr float m_minVerticalFieldOfViewDeg = 0.0f;
+        static constexpr float m_maxVerticalFieldOfViewDeg = 360.0f;
 
         float m_verticalFieldOfViewDeg = 90.0f; //!< Vertical field of view of camera sensor.
-        float m_minVerticalFieldOfViewDeg = 0.0f;
-        float m_maxVerticalFieldOfViewDeg = 360.0f;
         int m_width = 640; //!< Camera image width in pixels.
         int m_height = 480; //!< Camera image height in pixels.
         bool m_colorCamera = true; //!< Use color camera?

+ 72 - 0
Gems/ROS2Sensors/Code/Include/ROS2Sensors/Imu/ImuConfigurationRequestBus.h

@@ -0,0 +1,72 @@
+/*
+ * 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/EBus/EBus.h>
+#include <AzCore/Math/Vector3.h>
+#include <AzCore/RTTI/RTTI.h>
+#include <ROS2Sensors/Imu/ImuSensorConfiguration.h>
+
+namespace ROS2Sensors
+{
+    //! Interface that allows to get and set Imu sensor's configuration.
+    class ImuConfigurationRequest : public AZ::EBusTraits
+    {
+    public:
+        using BusIdType = AZ::EntityId;
+        static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById;
+
+        //! Returns the current configuration of the component.
+        virtual const ImuSensorConfiguration GetConfiguration() = 0;
+
+        //! Sets the configuration of the component.
+        //! Each component should handle the configuration change without fully reinitializing the ROS2 publisher.
+        //! This will allow to change the configuration of the component at runtime.
+        //! Note: this method does not verify the configuration, so it is up to the caller to ensure that the configuration is valid.
+        //! @param configuration The new configuration to set.
+        virtual void SetConfiguration(const ImuSensorConfiguration& configuration) = 0;
+
+        //! Gets the current filter size used for IMU data processing.
+        virtual int GetFilterSize() = 0;
+
+        //! Sets the filter size for IMU data processing.
+        virtual void SetFilterSize(int filterSize) = 0;
+
+        //! Checks if gravity is included in the IMU data.
+        virtual bool GetIncludeGravity() = 0;
+
+        //! Sets whether gravity should be included in the IMU data.
+        virtual void SetIncludeGravity(bool includeGravity) = 0;
+
+        //! Checks if absolute rotation is enabled for the IMU.
+        virtual bool GetAbsoluteRotation() = 0;
+
+        //! Sets whether absolute rotation should be enabled for the IMU.
+        virtual void SetAbsoluteRotation(bool absoluteRotation) = 0;
+
+        //! Gets the orientation variance for the IMU.
+        virtual AZ::Vector3 GetOrientationVariance() = 0;
+
+        //! Sets the orientation variance for the IMU.
+        virtual void SetOrientationVariance(const AZ::Vector3& orientationVariance) = 0;
+
+        //! Gets the angular velocity variance for the IMU.
+        virtual AZ::Vector3 GetAngularVelocityVariance() = 0;
+
+        //! Sets the angular velocity variance for the IMU.
+        virtual void SetAngularVelocityVariance(const AZ::Vector3& angularVelocityVariance) = 0;
+
+        //! Gets the linear acceleration variance for the IMU.
+        virtual AZ::Vector3 GetLinearAccelerationVariance() = 0;
+
+        //! Sets the linear acceleration variance for the IMU.
+        virtual void SetLinearAccelerationVariance(const AZ::Vector3& linearAccelerationVariance) = 0;
+    };
+
+    using ImuConfigurationRequestBus = AZ::EBus<ImuConfigurationRequest>;
+} // namespace ROS2Sensors

+ 5 - 5
Gems/ROS2Sensors/Code/Source/Imu/ImuSensorConfiguration.h → Gems/ROS2Sensors/Code/Include/ROS2Sensors/Imu/ImuSensorConfiguration.h

@@ -7,23 +7,23 @@
  */
 #pragma once
 
-#include <AzCore/Math/Matrix3x3.h>
+#include <AzCore/Math/Vector3.h>
 #include <AzCore/RTTI/RTTI.h>
 #include <AzCore/Serialization/SerializeContext.h>
-#include <AzCore/std/string/string.h>
+#include <ROS2Sensors/ROS2SensorsTypeIds.h>
 
 namespace ROS2Sensors
 {
     //! A structure capturing configuration of a IMU sensor.
     struct ImuSensorConfiguration
     {
-        AZ_TYPE_INFO(ImuSensorConfiguration, "{6788e84f-b985-4413-8e2b-46fbfb667c95}");
+        AZ_TYPE_INFO(ImuSensorConfiguration, ImuSensorConfigurationTypeId);
         static void Reflect(AZ::ReflectContext* context);
 
         //! Length of filter that removes numerical noise
+        static constexpr int m_minFilterSize = 1;
+        static constexpr int m_maxFilterSize = 200;
         int m_filterSize = 10;
-        int m_minFilterSize = 1;
-        int m_maxFilterSize = 200;
 
         //! Include gravity acceleration
         bool m_includeGravity = true;

+ 2 - 1
Gems/ROS2Sensors/Code/Include/ROS2Sensors/Lidar/ClassSegmentationBus.h

@@ -13,6 +13,7 @@
 #include <AzCore/Math/Color.h>
 #include <LmbrCentral/Scripting/TagComponentBus.h>
 #include <ROS2Sensors/Lidar/SegmentationClassConfiguration.h>
+#include <ROS2Sensors/ROS2SensorsTypeIds.h>
 
 namespace ROS2Sensors
 {
@@ -24,7 +25,7 @@ namespace ROS2Sensors
     class ClassSegmentationRequests
     {
     public:
-        AZ_RTTI(ClassSegmentationRequests, "{69b4109e-25ff-482f-b92e-f19cdf06bce2}");
+        AZ_RTTI(ClassSegmentationRequests, ClassSegmentationBusTypeId);
 
         //! Returns the color of segmentation class with the provided class ID.
         //! If no segmentation class is found with provided class ID, returns AZ::Colors::White.

+ 128 - 0
Gems/ROS2Sensors/Code/Include/ROS2Sensors/Lidar/LidarConfigurationRequestBus.h

@@ -0,0 +1,128 @@
+/*
+ * 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/EBus/EBus.h>
+#include <AzCore/Math/Vector3.h>
+#include <AzCore/RTTI/RTTI.h>
+#include <AzCore/std/string/string.h>
+#include <ROS2Sensors/Lidar/LidarSensorConfiguration.h>
+#include <ROS2Sensors/Lidar/LidarTemplate.h>
+
+namespace ROS2Sensors
+{
+    //! Interface that allows to get and set Lidar sensor's configuration.
+    class LidarConfigurationRequest : public AZ::EBusTraits
+    {
+    public:
+        using BusIdType = AZ::EntityId;
+        static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById;
+
+        //! Returns the current configuration of the component.
+        virtual const LidarSensorConfiguration GetConfiguration() = 0;
+
+        //! Sets the configuration of the component.
+        //! Each component should handle the configuration change without fully reinitializing the ROS2 publisher.
+        //! This will allow to change the configuration of the component at runtime.
+        //! Note: this method does not verify the configuration, so it is up to the caller to ensure that the configuration is valid.
+        //! @param configuration The new configuration to set.
+        virtual void SetConfiguration(const LidarSensorConfiguration& configuration) = 0;
+
+        //! Get the name of the lidar model.
+        virtual AZStd::string GetModelName() = 0;
+        //! Set the name of the lidar model. Unknown name will set a custom lidar model.
+        virtual void SetModelName(const AZStd::string& name) = 0;
+
+        //! Check if segmentation is enabled in lidar.
+        virtual bool IsSegmentationEnabled() = 0;
+        //! Set if segmentation is enabled in lidar.
+        virtual void SetSegmentationEnabled(bool enabled) = 0;
+
+        //! Check if the lidar output should include points at maximum range.
+        virtual bool IsAddPointsAtMaxEnabled() = 0;
+        //! Set if the lidar output should include points at maximum range.
+        virtual void SetAddPointsAtMaxEnabled(bool addPoints) = 0;
+
+        //! Check if the lidar is 2D.
+        virtual bool Is2D() = 0;
+
+        //! Get the minimum horizontal angle (altitude of the ray), in degrees.
+        virtual float GetMinHAngle() = 0;
+        //! Set the minimum horizontal angle (altitude of the ray), in degrees.
+        virtual void SetMinHAngle(float angle) = 0;
+
+        //! Get the maximum horizontal angle (altitude of the ray), in degrees.
+        virtual float GetMaxHAngle() = 0;
+        //! Set the maximum horizontal angle (altitude of the ray), in degrees.
+        virtual void SetMaxHAngle(float angle) = 0;
+
+        //! Get the minimum vertical angle (azimuth of the ray), in degrees.
+        //! Override for 3D Lidars; use default implementation for 2D Lidars.
+        virtual float GetMinVAngle()
+        {
+            AZ_Warning("LidarConfigurationRequestBus", false, "GetMinVAngle() is not implemented.");
+            return 0.0f;
+        }
+        //! Set the minimum vertical angle (azimuth of the ray), in degrees.
+        //! Override for 3D Lidars; use default implementation for 2D Lidars.
+        virtual void SetMinVAngle(float angle)
+        {
+            AZ_Warning("LidarConfigurationRequestBus", false, "SetMinVAngle() is not implemented.");
+        }
+
+        //! Get the maximum vertical angle (azimuth of the ray), in degrees.
+        //! Override for 3D Lidars; use default implementation for 2D Lidars.
+        virtual float GetMaxVAngle()
+        {
+            AZ_Warning("LidarConfigurationRequestBus", false, "GetMaxVAngle() is not implemented.");
+            return 0.0f;
+        }
+        //! Set the maximum vertical angle (azimuth of the ray), in degrees.
+        //! Override for 3D Lidars; use default implementation for 2D Lidars.
+        virtual void SetMaxVAngle(float angle)
+        {
+            AZ_Warning("LidarConfigurationRequestBus", false, "SetMaxVAngle() is not implemented.");
+        }
+
+        //! Get the number of laser layers (resolution in horizontal direction).
+        //! Override for 3D Lidars; use default implementation for 2D Lidars.
+        virtual unsigned int GetLayers()
+        {
+            AZ_Warning("LidarConfigurationRequestBus", false, "GetLayers() is not implemented.");
+            return 0;
+        }
+        //! Set the number of laser layers (resolution in horizontal direction).
+        //! Override for 3D Lidars; use default implementation for 2D Lidars.
+        virtual void SetLayers(unsigned int layers)
+        {
+            AZ_Warning("LidarConfigurationRequestBus", false, "SetLayers() is not implemented.");
+        }
+
+        //! Get the resolution in vertical direction.
+        virtual unsigned int GetNumberOfIncrements() = 0;
+        //! Set the resolution in vertical direction.
+        virtual void SetNumberOfIncrements(unsigned int increments) = 0;
+
+        //! Get the minimum range of the simulated Lidar.
+        virtual float GetMinRange() = 0;
+        //! Set the minimum range of the simulated Lidar.
+        virtual void SetMinRange(float range) = 0;
+
+        //! Get the maximum range of the simulated Lidar.
+        virtual float GetMaxRange() = 0;
+        //! Set the maximum range of the simulated Lidar.
+        virtual void SetMaxRange(float range) = 0;
+
+        //! Get the noise parameters.
+        virtual const LidarTemplate::NoiseParameters& GetNoiseParameters() = 0;
+        //! Set the noise parameters.
+        virtual void SetNoiseParameters(const LidarTemplate::NoiseParameters& params) = 0;
+    };
+
+    using LidarConfigurationRequestBus = AZ::EBus<LidarConfigurationRequest>;
+} // namespace ROS2Sensors

+ 2 - 1
Gems/ROS2Sensors/Code/Include/ROS2Sensors/Lidar/LidarRaycasterBus.h

@@ -14,6 +14,7 @@
 #include <AzCore/Outcome/Outcome.h>
 #include <ROS2/Communication/QoS.h>
 #include <ROS2Sensors/Lidar/RaycastResults.h>
+#include <ROS2Sensors/ROS2SensorsTypeIds.h>
 
 namespace ROS2Sensors
 {
@@ -98,7 +99,7 @@ namespace ROS2Sensors
     class LidarRaycasterRequests
     {
     public:
-        AZ_RTTI(LidarRaycasterRequests, "{253a02c8-b6cb-493c-b16f-012ccf9db226}");
+        AZ_RTTI(LidarRaycasterRequests, LidarRaycasterBusTypeId);
 
         //! Configures ray orientations.
         //! @param orientations Vector of orientations as Euler angles in radians. Each ray direction is computed by transforming a unit

+ 2 - 1
Gems/ROS2Sensors/Code/Include/ROS2Sensors/Lidar/LidarRegistrarBus.h

@@ -10,6 +10,7 @@
 #include <AzCore/EBus/EBus.h>
 #include <AzCore/Interface/Interface.h>
 #include <AzCore/std/string/string.h>
+#include <ROS2Sensors/ROS2SensorsTypeIds.h>
 
 namespace ROS2Sensors
 {
@@ -41,7 +42,7 @@ namespace ROS2Sensors
     class LidarRegistrarRequests
     {
     public:
-        AZ_RTTI(LidarRegistrarRequests, "{22030dc7-a1db-43bd-b748-0fb9ec43ce2e}");
+        AZ_RTTI(LidarRegistrarRequests, LidarRegistrarBusTypeId);
 
         //! Registers a new lidar system under the provided name.
         //! To obtain the busId of a lidarSystem use the AZ_CRC macro as follows.

+ 9 - 8
Gems/ROS2Sensors/Code/Source/Lidar/LidarSensorConfiguration.h → Gems/ROS2Sensors/Code/Include/ROS2Sensors/Lidar/LidarSensorConfiguration.h

@@ -12,9 +12,9 @@
 #include <AzCore/Serialization/SerializeContext.h>
 #include <AzCore/std/string/string.h>
 #include <Lidar/LidarRegistrarSystemComponent.h>
-#include <Lidar/LidarTemplate.h>
-#include <Lidar/LidarTemplateUtils.h>
-#include <ROS2Sensors/Lidar/LidarRegistrarBus.h>
+#include <ROS2Sensors/Lidar/LidarTemplate.h>
+#include <ROS2Sensors/Lidar/LidarTemplateUtils.h>
+#include <ROS2Sensors/ROS2SensorsTypeIds.h>
 
 namespace ROS2Sensors
 {
@@ -22,7 +22,7 @@ namespace ROS2Sensors
     class LidarSensorConfiguration
     {
     public:
-        AZ_TYPE_INFO(LidarSensorConfiguration, "{e46e75f4-1e0e-48ca-a22f-43afc8f25101}");
+        AZ_TYPE_INFO(LidarSensorConfiguration, LidarSensorConfigurationTypeId);
         static void Reflect(AZ::ReflectContext* context);
 
         LidarSensorConfiguration(AZStd::vector<LidarTemplate::LidarModel> availableModels = {});
@@ -30,11 +30,15 @@ namespace ROS2Sensors
         //! Update the lidar system features based on the current lidar system selected.
         void FetchLidarImplementationFeatures();
 
+        //! Update the lidar configuration based on the current lidar model selected.
+        void FetchLidarModelConfiguration();
+
         LidarSystemFeatures m_lidarSystemFeatures;
 
         AZStd::string m_lidarSystem;
         LidarTemplate::LidarModel m_lidarModel = LidarTemplate::LidarModel::Custom2DLidar;
         LidarTemplate m_lidarParameters = LidarTemplateUtils::GetTemplate(LidarTemplate::LidarModel::Custom2DLidar);
+        AZStd::string m_lidarModelName = "CustomLidar2D";
 
         AZStd::unordered_set<AZ::u32> m_ignoredCollisionLayers;
         AZStd::vector<AZ::EntityId> m_excludedEntities;
@@ -48,8 +52,6 @@ namespace ROS2Sensors
         bool IsEntityExclusionVisible() const;
         bool IsMaxPointsConfigurationVisible() const;
         bool IsSegmentationConfigurationVisible() const;
-        //! Update the lidar configuration based on the current lidar model selected.
-        void FetchLidarModelConfiguration();
 
         AZ::Crc32 OnLidarModelSelected();
         AZ::Crc32 OnLidarImplementationSelected();
@@ -59,8 +61,7 @@ namespace ROS2Sensors
         //! Get all available lidar systems.
         AZStd::vector<AZStd::string> FetchLidarSystemList();
 
-        const AZStd::vector<LidarTemplate::LidarModel> m_availableModels;
-        AZStd::string m_lidarModelName = "CustomLidar2D";
+        AZStd::vector<LidarTemplate::LidarModel> m_availableModels;
 
         void UpdateShowNoise();
     };

+ 2 - 1
Gems/ROS2Sensors/Code/Include/ROS2Sensors/Lidar/LidarSystemBus.h

@@ -10,6 +10,7 @@
 #include <AzCore/Component/EntityId.h>
 #include <AzCore/EBus/EBus.h>
 #include <ROS2Sensors/Lidar/LidarRaycasterBus.h>
+#include <ROS2Sensors/ROS2SensorsTypeIds.h>
 
 namespace ROS2Sensors
 {
@@ -17,7 +18,7 @@ namespace ROS2Sensors
     class LidarSystemRequests
     {
     public:
-        AZ_RTTI(LidarSystemRequests, "{007871d1-2783-4382-977b-558f436c54a5}");
+        AZ_RTTI(LidarSystemRequests, LidarSystemBusTypeId);
 
         //! Creates a new Lidar.
         //! @param lidarEntityId EntityId holding the ROS2LidarSensorComponent.

+ 3 - 2
Gems/ROS2Sensors/Code/Source/Lidar/LidarTemplate.h → Gems/ROS2Sensors/Code/Include/ROS2Sensors/Lidar/LidarTemplate.h

@@ -10,6 +10,7 @@
 #include <AzCore/RTTI/RTTI.h>
 #include <AzCore/Serialization/SerializeContext.h>
 #include <AzCore/std/string/string.h>
+#include <ROS2Sensors/ROS2SensorsTypeIds.h>
 
 namespace ROS2Sensors
 {
@@ -20,7 +21,7 @@ namespace ROS2Sensors
     struct LidarTemplate
     {
     public:
-        AZ_TYPE_INFO(LidarTemplate, "{9E9EF583-733D-4450-BBA0-ADD4D1BEFBF2}");
+        AZ_TYPE_INFO(LidarTemplate, LidarTemplateTypeId);
         static void Reflect(AZ::ReflectContext* context);
 
         enum class LidarModel
@@ -39,7 +40,7 @@ namespace ROS2Sensors
         struct NoiseParameters
         {
         public:
-            AZ_TYPE_INFO(NoiseParameters, "{58c007ad-320f-49df-bc20-6419159ee176}");
+            AZ_TYPE_INFO(NoiseParameters, LidarTemplateNoiseParametersTypeId);
             static void Reflect(AZ::ReflectContext* context);
 
             //! Angular noise standard deviation, in degrees

+ 1 - 1
Gems/ROS2Sensors/Code/Source/Lidar/LidarTemplateUtils.h → Gems/ROS2Sensors/Code/Include/ROS2Sensors/Lidar/LidarTemplateUtils.h

@@ -9,7 +9,7 @@
 
 #include <AzCore/Math/Vector3.h>
 #include <AzCore/std/containers/vector.h>
-#include <Lidar/LidarTemplate.h>
+#include <ROS2Sensors/Lidar/LidarTemplate.h>
 
 namespace ROS2Sensors
 {

+ 3 - 1
Gems/ROS2Sensors/Code/Include/ROS2Sensors/Lidar/SegmentationClassConfiguration.h

@@ -8,9 +8,11 @@
 #pragma once
 
 #include <AzCore/Component/EntityId.h>
+#include <AzCore/Math/Color.h>
 #include <AzCore/RTTI/RTTI.h>
 #include <AzCore/Serialization/SerializeContext.h>
 #include <AzCore/std/string/string.h>
+#include <ROS2Sensors/ROS2SensorsTypeIds.h>
 
 namespace ROS2Sensors
 {
@@ -18,7 +20,7 @@ namespace ROS2Sensors
     class SegmentationClassConfiguration
     {
     public:
-        AZ_TYPE_INFO(SegmentationClassConfiguration, "{e46e75f4-1e0e-48ca-a22f-43afc8f25133}");
+        AZ_TYPE_INFO(SegmentationClassConfiguration, SegmentationClassConfigurationTypeId);
         static void Reflect(AZ::ReflectContext* context);
 
         static const SegmentationClassConfiguration UnknownClass;

+ 2 - 1
Gems/ROS2Sensors/Code/Source/Odometry/ROS2OdometryCovariance.h → Gems/ROS2Sensors/Code/Include/ROS2Sensors/Odometry/ROS2OdometryCovariance.h

@@ -11,13 +11,14 @@
 #include <AzCore/RTTI/RTTI.h>
 #include <AzCore/RTTI/TypeInfoSimple.h>
 #include <AzCore/Serialization/SerializeContext.h>
+#include <ROS2Sensors/ROS2SensorsTypeIds.h>
 #include <array>
 
 namespace ROS2Sensors
 {
     struct ROS2OdometryCovariance
     {
-        AZ_TYPE_INFO(ROS2OdometryCovariance, "{db015832-f621-11ed-b67e-0242ac120002}");
+        AZ_TYPE_INFO(ROS2OdometryCovariance, ROS2OdometryCovarianceTypeId);
 
         static void Reflect(AZ::ReflectContext* context);
 

+ 48 - 0
Gems/ROS2Sensors/Code/Include/ROS2Sensors/Odometry/WheelOdometryConfigurationRequestBus.h

@@ -0,0 +1,48 @@
+/*
+ * 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/EBus/EBus.h>
+#include <AzCore/Math/Vector3.h>
+#include <AzCore/RTTI/RTTI.h>
+#include <ROS2Sensors/Odometry/ROS2OdometryCovariance.h>
+#include <ROS2Sensors/Odometry/WheelOdometrySensorConfiguration.h>
+
+namespace ROS2Sensors
+{
+    //! Interface that allows to get and set WheelOdometry sensor's configuration.
+    class WheelOdometryConfigurationRequest : public AZ::EBusTraits
+    {
+    public:
+        using BusIdType = AZ::EntityId;
+        static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById;
+
+        //! Returns the current configuration of the component.
+        virtual const WheelOdometrySensorConfiguration GetConfiguration() = 0;
+
+        //! Sets the configuration of the component.
+        //! Each component should handle the configuration change without fully reinitializing the ROS2 publisher.
+        //! This will allow to change the configuration of the component at runtime.
+        //! @param configuration The new configuration to set.
+        virtual void SetConfiguration(const WheelOdometrySensorConfiguration& configuration) = 0;
+
+        //! Get the pose covariance.
+        virtual ROS2OdometryCovariance GetPoseCovariance() = 0;
+
+        //! Set the pose covariance.
+        virtual void SetPoseCovariance(const ROS2OdometryCovariance& covariance) = 0;
+
+        //! Get the twist covariance.
+        virtual ROS2OdometryCovariance GetTwistCovariance() = 0;
+
+        //! Set the twist covariance.
+        virtual void SetTwistCovariance(const ROS2OdometryCovariance& covariance) = 0;
+    };
+
+    using WheelOdometryConfigurationRequestBus = AZ::EBus<WheelOdometryConfigurationRequest>;
+} // namespace ROS2Sensors

+ 32 - 0
Gems/ROS2Sensors/Code/Include/ROS2Sensors/Odometry/WheelOdometrySensorConfiguration.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 <AzCore/RTTI/RTTI.h>
+#include <AzCore/Serialization/SerializeContext.h>
+#include <ROS2Sensors/Odometry/ROS2OdometryCovariance.h>
+#include <ROS2Sensors/ROS2SensorsTypeIds.h>
+
+namespace ROS2Sensors
+{
+
+    //! A structure capturing configuration of a wheel odometry sensor.
+    struct WheelOdometrySensorConfiguration
+    {
+        AZ_RTTI(WheelOdometrySensorConfiguration, WheelOdometrySensorConfigurationTypeId);
+
+        WheelOdometrySensorConfiguration() = default;
+        virtual ~WheelOdometrySensorConfiguration() = default;
+
+        static void Reflect(AZ::ReflectContext* context);
+
+        ROS2OdometryCovariance m_poseCovariance;
+        ROS2OdometryCovariance m_twistCovariance;
+    };
+} // namespace ROS2Sensors

+ 26 - 4
Gems/ROS2Sensors/Code/Include/ROS2Sensors/ROS2SensorsTypeIds.h

@@ -21,24 +21,46 @@ namespace ROS2Sensors
     // so they use the Same TypeId
     inline constexpr const char* ROS2SensorsEditorModuleTypeId = ROS2SensorsModuleTypeId;
 
+    // ROS2Sensors Interface TypeIds
+    inline constexpr const char* ImuConfigurationRequestTypeId = "{8D426B46-DF12-419A-96FB-4E2834567ECD}";
+    inline constexpr const char* SensorConfigurationTypeId = "{4755363D-0B5A-42D7-BBEF-152D87BA10D7}";
+    inline constexpr const char* SensorConfigurationRequestTypeId = "{01904EAB-FA33-7487-B634-E3F8361EB5FB}";
+    inline constexpr const char* ROS2SensorComponentBaseTypeId = "{2DF9A652-DF5D-43B1-932F-B6A838E36E97}";
+
     // System Sensor Components TypeIds
     inline constexpr const char* ROS2SystemCameraComponentTypeId = "{B4665D39-78FD-40DE-8518-2F6BD345A831}";
     inline constexpr const char* ROS2EditorCameraSystemComponentTypeId = "{407F51C0-92C9-11EE-B9D1-0242AC120002}";
     inline constexpr const char* LidarRegistrarSystemComponentTypeId = "{78CBA3F1-DB2C-46DE-9C3D-C40DD72F2F1E}";
     inline constexpr const char* LidarRegistrarEditorSystemComponentTypeId = "{7F11B599-5ACE-4498-A9A4-AD280C92BACC}";
 
+    // Lidar API TypeIds
+    inline constexpr const char* ClassSegmentationBusTypeId = "{69B4109E-25FF-482F-B92E-F19CDF06BCE2}";
+    inline constexpr const char* SegmentationClassConfigurationTypeId = "{E46E75F4-1E0E-48CA-A22F-43AFC8F25133}";
+    inline constexpr const char* LidarRaycasterBusTypeId = "{253A02C8-B6CB-493C-B16F-012CCF9DB226}";
+    inline constexpr const char* LidarRegistrarBusTypeId = "{22030DC7-A1DB-43BD-B748-0FB9EC43CE2E}";
+    inline constexpr const char* LidarSystemBusTypeId = "{007871D1-2783-4382-977B-558F436C54A5}";
+
+    // Sensor Configurations TypeIds
+    inline constexpr const char* CameraSensorConfigurationTypeId = "{386A2640-442B-473D-BC2A-665D049D7EF5}";
+    inline constexpr const char* ImuSensorConfigurationTypeId = "{6788E84F-B985-4413-8E2B-46FBFB667C95}";
+    inline constexpr const char* LidarSensorConfigurationTypeId = "{E46E75F4-1E0E-48CA-A22F-43AFD8F25101}";
+    inline constexpr const char* LidarTemplateTypeId = "{9E9EF583-733D-4450-BBA0-ADD4D1BEFBF2}";
+    inline constexpr const char* LidarTemplateNoiseParametersTypeId = "{58C007AD-320F-49DF-BC20-6419159EE176}";
+    inline constexpr const char* ROS2OdometryCovarianceTypeId = "{DB015832-F621-11ED-B67E-0242AC120002}";
+    inline constexpr const char* WheelOdometrySensorConfigurationTypeId = "{9DC58D89-E674-4D7F-9EA9-AFE3AE7FD2EB}";
+
     // Sensor Components TypeIds
     inline constexpr const char* ROS2CameraSensorComponentTypeId = "{3C6B8AE6-9721-4639-B8F9-D8D28FD7A071}";
-    inline constexpr const char* ROS2ContactSensorComponentTypeId = "{91272e66-c9f1-4aa2-a9d5-98eaa4ef4e9a}";
+    inline constexpr const char* ROS2ContactSensorComponentTypeId = "{91272E66-C9F1-4AA2-A9D5-98EAA4EF4E9A}";
     inline constexpr const char* ROS2GNSSSensorComponentTypeId = "{55B4A299-7FA3-496A-88F0-764C75B0E9A7}";
     inline constexpr const char* ROS2ImuSensorComponentTypeId = "{502A955E-7742-4E23-AD77-5E4063739DCA}";
     inline constexpr const char* ROS2LidarSensorComponentTypeId = "{502A955F-7742-4E23-AD77-5E4063739DCA}";
     inline constexpr const char* ROS2Lidar2DSensorComponentTypeId = "{F4C2D970-1D69-40F2-9D4D-B52DCFDD2704}";
-    inline constexpr const char* ROS2WheelOdometryComponentTypeId = "{9bdb8c23-ac76-4c25-8d35-37aaa9f43fac}";
+    inline constexpr const char* ROS2WheelOdometryComponentTypeId = "{9BDB8C23-AC76-4C25-8D35-37AAA9F43FAC}";
     inline constexpr const char* ROS2OdometrySensorComponent = "{61387448-63AA-4563-AF87-60C72B05B863}";
 
     // Sensors Components Tools TypeIds
-    inline constexpr const char* ClassSegmentationConfigurationComponentTypeId = "{BAB1EA0C-7456-40EA-BC1E-71697137c27c}";
-    inline constexpr const char* ROS2ImageEncodingConversionComponentTypeId = "{12449810-D179-44F1-8F72-22D8d3fa4460}";
+    inline constexpr const char* ClassSegmentationConfigurationComponentTypeId = "{BAB1EA0C-7456-40EA-BC1E-71697137C27C}";
+    inline constexpr const char* ROS2ImageEncodingConversionComponentTypeId = "{12449810-D179-44F1-8F72-22D8D3FA4460}";
     inline constexpr const char* EncodingConversionTypeId = "{DB361ADC-B339-4A4E-A10B-C6BF6791EDA6}";
 } // namespace ROS2Sensors

+ 2 - 3
Gems/ROS2Sensors/Code/Include/ROS2Sensors/Sensor/ROS2SensorComponentBase.h

@@ -142,7 +142,7 @@ namespace ROS2Sensors
             m_eventSourceAdapter.Start();
         }
 
-        //! Stops sensor and disconnects event callbacks passed through RSO2::ROS2SensorComponentBase::StartSensor.
+        //! Stops sensor and disconnects event callbacks passed through ROS2::ROS2SensorComponentBase::StartSensor.
         void StopSensor()
         {
             m_eventSourceAdapter.Stop();
@@ -175,6 +175,5 @@ namespace ROS2Sensors
         typename EventSourceT::AdaptedEventHandlerType m_adaptedEventHandler;
     };
 
-    AZ_COMPONENT_IMPL_INLINE(
-        (ROS2SensorComponentBase, AZ_CLASS), "ROS2SensorComponentBase", "{2DF9A652-DF5D-43B1-932F-B6A838E36E97}", AZ::Component)
+    AZ_COMPONENT_IMPL_INLINE((ROS2SensorComponentBase, AZ_CLASS), "ROS2SensorComponentBase", ROS2SensorComponentBaseTypeId, AZ::Component)
 } // namespace ROS2Sensors

+ 2 - 1
Gems/ROS2Sensors/Code/Include/ROS2Sensors/Sensor/SensorConfiguration.h

@@ -12,6 +12,7 @@
 #include <AzCore/std/containers/map.h>
 #include <AzCore/std/string/string.h>
 #include <ROS2/Communication/TopicConfiguration.h>
+#include <ROS2Sensors/ROS2SensorsTypeIds.h>
 
 namespace ROS2Sensors
 {
@@ -21,7 +22,7 @@ namespace ROS2Sensors
     struct SensorConfiguration
     {
     public:
-        AZ_TYPE_INFO(SensorConfiguration, "{4755363D-0B5A-42D7-BBEF-152D87BA10D7}");
+        AZ_TYPE_INFO(SensorConfiguration, SensorConfigurationTypeId);
         static void Reflect(AZ::ReflectContext* context);
 
         //! ROS2 Publishers of this sensor.

+ 1 - 2
Gems/ROS2Sensors/Code/Include/ROS2Sensors/Sensor/SensorConfigurationRequestBus.h

@@ -10,7 +10,6 @@
 #include <AzCore/Component/ComponentBus.h>
 #include <AzCore/Component/EntityId.h>
 #include <AzCore/EBus/EBus.h>
-#include <AzCore/Interface/Interface.h>
 #include <ROS2Sensors/Sensor/SensorConfiguration.h>
 namespace ROS2Sensors
 {
@@ -18,7 +17,7 @@ namespace ROS2Sensors
     class SensorConfigurationRequest : public AZ::EBusTraits
     {
     public:
-        AZ_RTTI(SensorConfigurationRequest, "{01904eab-fa33-7487-b634-e3f8361eb5fb}");
+        AZ_RTTI(SensorConfigurationRequest, SensorConfigurationRequestTypeId);
         using BusIdType = AZ::EntityComponentIdPair;
         static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById;
 

+ 0 - 30
Gems/ROS2Sensors/Code/Include/ROS2Sensors/Sensor/SensorHelper.h

@@ -1,30 +0,0 @@
-/*
- * 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>
-
-namespace ROS2Sensors
-{
-    //! Function to check if the component is a sensor component.
-    //! @param component Component to check
-    //! @return True if the component is a sensor component
-    bool IsComponentROS2Sensor(const AZ::Component* component);
-
-    //! Function to get Ids of all sensors attached to the entity.
-    //! @param entityId Id of the entity
-    //! @return Vector of EntityComponentIdPair of all sensors attached to the entity
-    AZStd::vector<AZ::EntityComponentIdPair> GetSensorsForEntity(const AZ::EntityId& entityId);
-
-    //! Function to get Ids of all sensors attached to the entity
-    //! @param entityId Id of the entity
-    //! @param sensorType Type of the sensor, see @file ROS2SensorTypesIds.h
-    //! @return Vector of EntityComponentIdPair of all sensors attached to the entity
-    AZStd::vector<AZ::EntityComponentIdPair> GetSensorsOfType(const AZ::EntityId& entityId, const AZ::Uuid& sensorType);
-
-} // namespace ROS2Sensors

+ 1 - 1
Gems/ROS2Sensors/Code/Platform/Linux/PAL_linux.cmake

@@ -8,4 +8,4 @@
 
 set(PAL_TRAIT_ROS2SENSORS_SUPPORTED TRUE)
 set(PAL_TRAIT_ROS2SENSORS_TEST_SUPPORTED FALSE)
-set(PAL_TRAIT_ROS2SENSORS_EDITOR_TEST_SUPPORTED FALSE)
+set(PAL_TRAIT_ROS2SENSORS_EDITOR_TEST_SUPPORTED TRUE)

+ 1 - 1
Gems/ROS2Sensors/Code/Source/Camera/CameraSensorConfiguration.cpp

@@ -6,9 +6,9 @@
  *
  */
 
-#include "CameraSensorConfiguration.h"
 #include <AzCore/Serialization/EditContext.h>
 #include <AzCore/Serialization/EditContextConstants.inl>
+#include <ROS2Sensors/Camera/CameraSensorConfiguration.h>
 
 namespace ROS2Sensors
 {

+ 1 - 1
Gems/ROS2Sensors/Code/Source/Camera/CameraSensorDescription.h

@@ -7,7 +7,7 @@
  */
 #pragma once
 
-#include "CameraSensorConfiguration.h"
+#include <ROS2Sensors/Camera/CameraSensorConfiguration.h>
 #include <ROS2Sensors/Sensor/SensorConfiguration.h>
 
 #include <AzCore/Math/Matrix4x4.h>

+ 160 - 35
Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSensorComponent.cpp

@@ -34,23 +34,13 @@ namespace ROS2Sensors
     void ROS2CameraSensorComponent::Activate()
     {
         ROS2SensorComponentBase::Activate();
-        if (m_cameraConfiguration.m_colorCamera && m_cameraConfiguration.m_depthCamera)
-        {
-            SetImageSource<CameraRGBDSensor>();
-        }
-        else if (m_cameraConfiguration.m_colorCamera)
-        {
-            SetImageSource<CameraColorSensor>();
-        }
-        else if (m_cameraConfiguration.m_depthCamera)
-        {
-            SetImageSource<CameraDepthSensor>();
-        }
+
+        SetCameraSensorConfiguration();
 
         const auto* component = GetEntity()->FindComponent<ROS2::ROS2FrameComponent>();
         AZ_Assert(component, "Entity has no ROS2FrameComponent");
         m_frameName = component->GetFrameID();
-        CameraCalibrationRequestBus::Handler::BusConnect(GetEntityId());
+        CameraConfigurationRequestBus::Handler::BusConnect(GetEntityId());
 
         StartSensor(
             m_sensorConfiguration.m_frequency,
@@ -68,31 +58,10 @@ namespace ROS2Sensors
     {
         StopSensor();
         m_cameraSensor.reset();
-        CameraCalibrationRequestBus::Handler::BusDisconnect(GetEntityId());
+        CameraConfigurationRequestBus::Handler::BusDisconnect(GetEntityId());
         ROS2SensorComponentBase::Deactivate();
     }
 
-    AZ::Matrix3x3 ROS2CameraSensorComponent::GetCameraMatrix() const
-    {
-        return CameraUtils::MakeCameraIntrinsics(
-            m_cameraConfiguration.m_width, m_cameraConfiguration.m_height, m_cameraConfiguration.m_verticalFieldOfViewDeg);
-    }
-
-    int ROS2CameraSensorComponent::GetWidth() const
-    {
-        return m_cameraConfiguration.m_width;
-    }
-
-    int ROS2CameraSensorComponent::GetHeight() const
-    {
-        return m_cameraConfiguration.m_height;
-    }
-
-    float ROS2CameraSensorComponent::GetVerticalFOV() const
-    {
-        return m_cameraConfiguration.m_verticalFieldOfViewDeg;
-    }
-
     void ROS2CameraSensorComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
     {
         required.push_back(AZ_CRC("ROS2Frame"));
@@ -136,4 +105,160 @@ namespace ROS2Sensors
         }
         return AZStd::string{};
     }
+
+    void ROS2CameraSensorComponent::SetCameraSensorConfiguration()
+    {
+        if (m_cameraConfiguration.m_colorCamera && m_cameraConfiguration.m_depthCamera)
+        {
+            SetImageSource<CameraRGBDSensor>();
+        }
+        else if (m_cameraConfiguration.m_colorCamera)
+        {
+            SetImageSource<CameraColorSensor>();
+        }
+        else if (m_cameraConfiguration.m_depthCamera)
+        {
+            SetImageSource<CameraDepthSensor>();
+        }
+    }
+
+    void ROS2CameraSensorComponent::SetConfiguration(const CameraSensorConfiguration& configuration)
+    {
+        m_cameraConfiguration = configuration;
+        m_cameraSensor.reset();
+        // SetCameraSensorConfiguration() is called in the next tick to ensure that the camera sensor is
+        // reset.
+        AZ::SystemTickBus::QueueFunction(
+            [this]()
+            {
+                SetCameraSensorConfiguration();
+            });
+    }
+
+    const CameraSensorConfiguration ROS2CameraSensorComponent::GetConfiguration()
+    {
+        return m_cameraConfiguration;
+    }
+
+    AZ::Matrix3x3 ROS2CameraSensorComponent::GetCameraMatrix()
+    {
+        return CameraUtils::MakeCameraIntrinsics(
+            m_cameraConfiguration.m_width, m_cameraConfiguration.m_height, m_cameraConfiguration.m_verticalFieldOfViewDeg);
+    }
+
+    float ROS2CameraSensorComponent::GetVerticalFOV()
+    {
+        return m_cameraConfiguration.m_verticalFieldOfViewDeg;
+    }
+
+    void ROS2CameraSensorComponent::SetVerticalFOV(float value)
+    {
+        if (value < CameraSensorConfiguration::m_minVerticalFieldOfViewDeg ||
+            value > CameraSensorConfiguration::m_maxVerticalFieldOfViewDeg)
+        {
+            AZ_Warning(
+                "ROS2CameraSensorComponent",
+                false,
+                "Vertical field of view value %f is out of bounds [%f, %f].",
+                value,
+                CameraSensorConfiguration::m_minVerticalFieldOfViewDeg,
+                CameraSensorConfiguration::m_maxVerticalFieldOfViewDeg);
+            return;
+        }
+        auto cameraConfiguration = m_cameraConfiguration;
+        cameraConfiguration.m_verticalFieldOfViewDeg = value;
+        SetConfiguration(cameraConfiguration);
+    }
+
+    int ROS2CameraSensorComponent::GetWidth()
+    {
+        return m_cameraConfiguration.m_width;
+    }
+
+    void ROS2CameraSensorComponent::SetWidth(int value)
+    {
+        if (value < CameraSensorConfiguration::m_minWidth)
+        {
+            AZ_Warning(
+                "ROS2CameraSensorComponent",
+                false,
+                "Width value %d is less than the minimum allowed width %d.",
+                value,
+                CameraSensorConfiguration::m_minWidth);
+            return;
+        }
+        auto cameraConfiguration = m_cameraConfiguration;
+        cameraConfiguration.m_width = value;
+        SetConfiguration(cameraConfiguration);
+    }
+
+    int ROS2CameraSensorComponent::GetHeight()
+    {
+        return m_cameraConfiguration.m_height;
+    }
+
+    void ROS2CameraSensorComponent::SetHeight(int value)
+    {
+        if (value < CameraSensorConfiguration::m_minHeight)
+        {
+            AZ_Warning(
+                "ROS2CameraSensorComponent",
+                false,
+                "Height value %d is less than the minimum allowed height %d.",
+                value,
+                CameraSensorConfiguration::m_minHeight);
+            return;
+        }
+        auto cameraConfiguration = m_cameraConfiguration;
+        cameraConfiguration.m_height = value;
+        SetConfiguration(cameraConfiguration);
+    }
+
+    bool ROS2CameraSensorComponent::IsColorCamera()
+    {
+        return m_cameraConfiguration.m_colorCamera;
+    }
+
+    void ROS2CameraSensorComponent::SetColorCamera(bool value)
+    {
+        auto cameraConfiguration = m_cameraConfiguration;
+        m_cameraConfiguration.m_colorCamera = value;
+        SetConfiguration(cameraConfiguration);
+    }
+
+    bool ROS2CameraSensorComponent::IsDepthCamera()
+    {
+        return m_cameraConfiguration.m_depthCamera;
+    }
+
+    void ROS2CameraSensorComponent::SetDepthCamera(bool value)
+    {
+        auto cameraConfiguration = m_cameraConfiguration;
+        cameraConfiguration.m_depthCamera = value;
+        SetConfiguration(cameraConfiguration);
+    }
+
+    float ROS2CameraSensorComponent::GetNearClipDistance()
+    {
+        return m_cameraConfiguration.m_nearClipDistance;
+    }
+
+    void ROS2CameraSensorComponent::SetNearClipDistance(float value)
+    {
+        auto cameraConfiguration = m_cameraConfiguration;
+        cameraConfiguration.m_nearClipDistance = value;
+        SetConfiguration(cameraConfiguration);
+    }
+
+    float ROS2CameraSensorComponent::GetFarClipDistance()
+    {
+        return m_cameraConfiguration.m_farClipDistance;
+    }
+
+    void ROS2CameraSensorComponent::SetFarClipDistance(float value)
+    {
+        auto cameraConfiguration = m_cameraConfiguration;
+        cameraConfiguration.m_farClipDistance = value;
+        SetConfiguration(cameraConfiguration);
+    }
 } // namespace ROS2Sensors

+ 25 - 9
Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSensorComponent.h

@@ -15,10 +15,10 @@
 #include <AzCore/std/containers/vector.h>
 
 #include "CameraSensor.h"
-#include "CameraSensorConfiguration.h"
 #include <ROS2/ROS2Bus.h>
 #include <ROS2/Sensor/Events/TickBasedSource.h>
-#include <ROS2Sensors/Camera/CameraCalibrationRequestBus.h>
+#include <ROS2Sensors/Camera/CameraConfigurationRequestBus.h>
+#include <ROS2Sensors/Camera/CameraSensorConfiguration.h>
 #include <ROS2Sensors/ROS2SensorsTypeIds.h>
 #include <ROS2Sensors/Sensor/ROS2SensorComponentBase.h>
 
@@ -33,7 +33,7 @@ namespace ROS2Sensors
     //! Camera frustum is facing negative Z axis; image plane is parallel to X,Y plane: X - right, Y - up
     class ROS2CameraSensorComponent
         : public ROS2SensorComponentBase<ROS2::TickBasedSource>
-        , public CameraCalibrationRequestBus::Handler
+        , protected CameraConfigurationRequestBus::Handler
     {
     public:
         ROS2CameraSensorComponent() = default;
@@ -50,13 +50,26 @@ namespace ROS2Sensors
         void Activate() override;
         void Deactivate() override;
 
-        // CameraCalibrationRequestBus::Handler overrides ...
-        AZ::Matrix3x3 GetCameraMatrix() const override;
-        int GetWidth() const override;
-        int GetHeight() const override;
-        float GetVerticalFOV() const override;
-
     private:
+        // CameraConfigurationRequestBus::Handler overrides ..
+        void SetConfiguration(const CameraSensorConfiguration& configuration) override;
+        const CameraSensorConfiguration GetConfiguration() override;
+        AZ::Matrix3x3 GetCameraMatrix() override;
+        float GetVerticalFOV() override;
+        void SetVerticalFOV(float value) override;
+        int GetWidth() override;
+        void SetWidth(int value) override;
+        int GetHeight() override;
+        void SetHeight(int value) override;
+        bool IsColorCamera() override;
+        void SetColorCamera(bool value) override;
+        bool IsDepthCamera() override;
+        void SetDepthCamera(bool value) override;
+        float GetNearClipDistance() override;
+        void SetNearClipDistance(float value) override;
+        float GetFarClipDistance() override;
+        void SetFarClipDistance(float value) override;
+
         //! Helper that adds an image source.
         //! @tparam CameraType type of camera sensor (eg 'CameraColorSensor')
         template<typename CameraType>
@@ -73,6 +86,9 @@ namespace ROS2Sensors
         ///! Requests message publication from camera sensor.
         void FrequencyTick();
 
+        //! Sets the camera sensor and image source.
+        void SetCameraSensorConfiguration();
+
         CameraSensorConfiguration m_cameraConfiguration;
         AZStd::string m_frameName;
         AZStd::shared_ptr<CameraSensor> m_cameraSensor;

+ 122 - 27
Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp

@@ -31,7 +31,7 @@ namespace ROS2Sensors
     ROS2CameraSensorEditorComponent::ROS2CameraSensorEditorComponent(
         const SensorConfiguration& sensorConfiguration, const CameraSensorConfiguration& cameraConfiguration)
         : m_sensorConfiguration(sensorConfiguration)
-        , m_cameraSensorConfiguration(cameraConfiguration)
+        , m_cameraConfiguration(cameraConfiguration)
     {
     }
 
@@ -41,7 +41,7 @@ namespace ROS2Sensors
         {
             serialize->Class<ROS2CameraSensorEditorComponent, AzToolsFramework::Components::EditorComponentBase>()
                 ->Version(4)
-                ->Field("CameraSensorConfig", &ROS2CameraSensorEditorComponent::m_cameraSensorConfiguration)
+                ->Field("CameraSensorConfig", &ROS2CameraSensorEditorComponent::m_cameraConfiguration)
                 ->Field("SensorConfig", &ROS2CameraSensorEditorComponent::m_sensorConfiguration);
 
             if (AZ::EditContext* editContext = serialize->GetEditContext())
@@ -59,7 +59,7 @@ namespace ROS2Sensors
                         "Sensor configuration")
                     ->DataElement(
                         AZ::Edit::UIHandlers::Default,
-                        &ROS2CameraSensorEditorComponent::m_cameraSensorConfiguration,
+                        &ROS2CameraSensorEditorComponent::m_cameraConfiguration,
                         "Camera configuration",
                         "Camera configuration.");
             }
@@ -70,14 +70,14 @@ namespace ROS2Sensors
     {
         AzFramework::EntityDebugDisplayEventBus::Handler::BusConnect(this->GetEntityId());
         AzToolsFramework::Components::EditorComponentBase::Activate();
-        CameraCalibrationRequestBus::Handler::BusConnect(GetEntityId());
+        CameraConfigurationRequestBus::Handler::BusConnect(GetEntityId());
     }
 
     void ROS2CameraSensorEditorComponent::Deactivate()
     {
+        CameraConfigurationRequestBus::Handler::BusDisconnect(GetEntityId());
         AzToolsFramework::Components::EditorComponentBase::Deactivate();
         AzFramework::EntityDebugDisplayEventBus::Handler::BusDisconnect();
-        CameraCalibrationRequestBus::Handler::BusDisconnect(GetEntityId());
     }
 
     void ROS2CameraSensorEditorComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
@@ -97,31 +97,126 @@ namespace ROS2Sensors
 
     void ROS2CameraSensorEditorComponent::BuildGameEntity(AZ::Entity* gameEntity)
     {
-        gameEntity->CreateComponent<ROS2CameraSensorComponent>(m_sensorConfiguration, m_cameraSensorConfiguration);
+        gameEntity->CreateComponent<ROS2CameraSensorComponent>(m_sensorConfiguration, m_cameraConfiguration);
     }
 
-    AZ::Matrix3x3 ROS2CameraSensorEditorComponent::GetCameraMatrix() const
+    void ROS2CameraSensorEditorComponent::SetConfiguration(const CameraSensorConfiguration& configuration)
+    {
+        m_cameraConfiguration = configuration;
+    }
+
+    const CameraSensorConfiguration ROS2CameraSensorEditorComponent::GetConfiguration()
+    {
+        return m_cameraConfiguration;
+    }
+
+    AZ::Matrix3x3 ROS2CameraSensorEditorComponent::GetCameraMatrix()
     {
         return CameraUtils::MakeCameraIntrinsics(
-            m_cameraSensorConfiguration.m_width,
-            m_cameraSensorConfiguration.m_height,
-            m_cameraSensorConfiguration.m_verticalFieldOfViewDeg);
-    };
+            m_cameraConfiguration.m_width, m_cameraConfiguration.m_height, m_cameraConfiguration.m_verticalFieldOfViewDeg);
+    }
+
+    float ROS2CameraSensorEditorComponent::GetVerticalFOV()
+    {
+        return m_cameraConfiguration.m_verticalFieldOfViewDeg;
+    }
+
+    void ROS2CameraSensorEditorComponent::SetVerticalFOV(float value)
+    {
+        if (value < CameraSensorConfiguration::m_minVerticalFieldOfViewDeg ||
+            value > CameraSensorConfiguration::m_maxVerticalFieldOfViewDeg)
+        {
+            AZ_Warning(
+                "ROS2CameraSensorEditorComponent",
+                false,
+                "Vertical field of view value %f is out of bounds [%f, %f].",
+                value,
+                CameraSensorConfiguration::m_minVerticalFieldOfViewDeg,
+                CameraSensorConfiguration::m_maxVerticalFieldOfViewDeg);
+            return;
+        }
+        m_cameraConfiguration.m_verticalFieldOfViewDeg = value;
+    }
+
+    int ROS2CameraSensorEditorComponent::GetWidth()
+    {
+        return m_cameraConfiguration.m_width;
+    }
+
+    void ROS2CameraSensorEditorComponent::SetWidth(int value)
+    {
+        if (value < CameraSensorConfiguration::m_minWidth)
+        {
+            AZ_Warning(
+                "ROS2CameraSensorEditorComponent",
+                false,
+                "Width value %d is less than the minimum allowed width %d.",
+                value,
+                CameraSensorConfiguration::m_minWidth);
+            return;
+        }
+        m_cameraConfiguration.m_width = value;
+    }
+
+    int ROS2CameraSensorEditorComponent::GetHeight()
+    {
+        return m_cameraConfiguration.m_height;
+    }
+
+    void ROS2CameraSensorEditorComponent::SetHeight(int value)
+    {
+        if (value < CameraSensorConfiguration::m_minHeight)
+        {
+            AZ_Warning(
+                "ROS2CameraSensorEditorComponent",
+                false,
+                "Height value %d is less than the minimum allowed height %d.",
+                value,
+                CameraSensorConfiguration::m_minHeight);
+            return;
+        }
+        m_cameraConfiguration.m_height = value;
+    }
+
+    bool ROS2CameraSensorEditorComponent::IsColorCamera()
+    {
+        return m_cameraConfiguration.m_colorCamera;
+    }
+
+    void ROS2CameraSensorEditorComponent::SetColorCamera(bool value)
+    {
+        m_cameraConfiguration.m_colorCamera = value;
+    }
 
-    int ROS2CameraSensorEditorComponent::GetWidth() const
+    bool ROS2CameraSensorEditorComponent::IsDepthCamera()
     {
-        return m_cameraSensorConfiguration.m_width;
-    };
+        return m_cameraConfiguration.m_depthCamera;
+    }
 
-    int ROS2CameraSensorEditorComponent::GetHeight() const
+    void ROS2CameraSensorEditorComponent::SetDepthCamera(bool value)
     {
-        return m_cameraSensorConfiguration.m_height;
-    };
+        m_cameraConfiguration.m_depthCamera = value;
+    }
 
-    float ROS2CameraSensorEditorComponent::GetVerticalFOV() const
+    float ROS2CameraSensorEditorComponent::GetNearClipDistance()
     {
-        return m_cameraSensorConfiguration.m_verticalFieldOfViewDeg;
-    };
+        return m_cameraConfiguration.m_nearClipDistance;
+    }
+
+    void ROS2CameraSensorEditorComponent::SetNearClipDistance(float value)
+    {
+        m_cameraConfiguration.m_nearClipDistance = value;
+    }
+
+    float ROS2CameraSensorEditorComponent::GetFarClipDistance()
+    {
+        return m_cameraConfiguration.m_farClipDistance;
+    }
+
+    void ROS2CameraSensorEditorComponent::SetFarClipDistance(float value)
+    {
+        m_cameraConfiguration.m_farClipDistance = value;
+    }
 
     void ROS2CameraSensorEditorComponent::DisplayEntityViewport(
         [[maybe_unused]] const AzFramework::ViewportInfo& viewportInfo, AzFramework::DebugDisplayRequests& debugDisplay)
@@ -133,11 +228,11 @@ namespace ROS2Sensors
         const AZ::u32 stateBefore = debugDisplay.GetState();
         AZ::Transform transform = GetEntity()->GetTransform()->GetWorldTM();
 
-        const float distance = m_cameraSensorConfiguration.m_farClipDistance * 0.1f;
-        const float tangent = static_cast<float>(tan(0.5f * AZ::DegToRad(m_cameraSensorConfiguration.m_verticalFieldOfViewDeg)));
+        const float distance = m_cameraConfiguration.m_farClipDistance * 0.1f;
+        const float tangent = static_cast<float>(tan(0.5f * AZ::DegToRad(m_cameraConfiguration.m_verticalFieldOfViewDeg)));
 
         float height = distance * tangent;
-        float width = height * m_cameraSensorConfiguration.m_width / m_cameraSensorConfiguration.m_height;
+        float width = height * m_cameraConfiguration.m_width / m_cameraConfiguration.m_height;
 
         AZ::Vector3 farPoints[4];
         farPoints[0] = AZ::Vector3(width, height, distance);
@@ -146,10 +241,10 @@ namespace ROS2Sensors
         farPoints[3] = AZ::Vector3(width, -height, distance);
 
         AZ::Vector3 nearPoints[4];
-        nearPoints[0] = farPoints[0].GetNormalizedSafe() * m_cameraSensorConfiguration.m_nearClipDistance;
-        nearPoints[1] = farPoints[1].GetNormalizedSafe() * m_cameraSensorConfiguration.m_nearClipDistance;
-        nearPoints[2] = farPoints[2].GetNormalizedSafe() * m_cameraSensorConfiguration.m_nearClipDistance;
-        nearPoints[3] = farPoints[3].GetNormalizedSafe() * m_cameraSensorConfiguration.m_nearClipDistance;
+        nearPoints[0] = farPoints[0].GetNormalizedSafe() * m_cameraConfiguration.m_nearClipDistance;
+        nearPoints[1] = farPoints[1].GetNormalizedSafe() * m_cameraConfiguration.m_nearClipDistance;
+        nearPoints[2] = farPoints[2].GetNormalizedSafe() * m_cameraConfiguration.m_nearClipDistance;
+        nearPoints[3] = farPoints[3].GetNormalizedSafe() * m_cameraConfiguration.m_nearClipDistance;
 
         // dimension of drawing
         const float arrowRise = 0.11f;

+ 23 - 10
Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSensorEditorComponent.h

@@ -12,10 +12,10 @@
 #include <AzToolsFramework/API/ComponentEntitySelectionBus.h>
 #include <AzToolsFramework/ToolsComponents/EditorComponentBase.h>
 
-#include "CameraSensorConfiguration.h"
 #include <ROS2/Frame/NamespaceConfiguration.h>
 #include <ROS2/Frame/ROS2Transform.h>
-#include <ROS2Sensors/Camera/CameraCalibrationRequestBus.h>
+#include <ROS2Sensors/Camera/CameraConfigurationRequestBus.h>
+#include <ROS2Sensors/Camera/CameraSensorConfiguration.h>
 #include <ROS2Sensors/Sensor/SensorConfiguration.h>
 
 namespace ROS2Sensors
@@ -25,7 +25,7 @@ namespace ROS2Sensors
     //! Component draws camera frustum in the Editor
     class ROS2CameraSensorEditorComponent
         : public AzToolsFramework::Components::EditorComponentBase
-        , public CameraCalibrationRequestBus::Handler
+        , public CameraConfigurationRequestBus::Handler
         , protected AzFramework::EntityDebugDisplayEventBus::Handler
     {
     public:
@@ -45,13 +45,26 @@ namespace ROS2Sensors
         // AzToolsFramework::Components::EditorComponentBase overrides
         void BuildGameEntity(AZ::Entity* gameEntity) override;
 
-        // CameraCalibrationRequestBus::Handler overrides
-        AZ::Matrix3x3 GetCameraMatrix() const override;
-        int GetWidth() const override;
-        int GetHeight() const override;
-        float GetVerticalFOV() const override;
-
     private:
+        // CameraConfigurationRequestBus::Handler overrides ..
+        void SetConfiguration(const CameraSensorConfiguration& configuration) override;
+        const CameraSensorConfiguration GetConfiguration() override;
+        AZ::Matrix3x3 GetCameraMatrix() override;
+        float GetVerticalFOV() override;
+        void SetVerticalFOV(float value) override;
+        int GetWidth() override;
+        void SetWidth(int value) override;
+        int GetHeight() override;
+        void SetHeight(int value) override;
+        bool IsColorCamera() override;
+        void SetColorCamera(bool value) override;
+        bool IsDepthCamera() override;
+        void SetDepthCamera(bool value) override;
+        float GetNearClipDistance() override;
+        void SetNearClipDistance(float value) override;
+        float GetFarClipDistance() override;
+        void SetFarClipDistance(float value) override;
+
         // EntityDebugDisplayEventBus::Handler overrides
         void DisplayEntityViewport(const AzFramework::ViewportInfo& viewportInfo, AzFramework::DebugDisplayRequests& debugDisplay) override;
 
@@ -59,6 +72,6 @@ namespace ROS2Sensors
             const AZStd::string& topic, const AZStd::string& messageType, const AZStd::string& configName) const;
 
         SensorConfiguration m_sensorConfiguration;
-        CameraSensorConfiguration m_cameraSensorConfiguration;
+        CameraSensorConfiguration m_cameraConfiguration;
     };
 } // namespace ROS2Sensors

+ 5 - 1
Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSystemComponent.cpp

@@ -43,7 +43,11 @@ namespace ROS2Sensors
     void ROS2SystemCameraComponent::GetRequiredServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required)
     {
         required.push_back(AZ_CRC_CE("ROS2Service"));
-        required.push_back(AZ_CRC_CE("RPISystem"));
+    }
+
+    void ROS2SystemCameraComponent::GetDependentServices(AZ::ComponentDescriptor::DependencyArrayType& dependent)
+    {
+        dependent.push_back(AZ_CRC_CE("RPISystem"));
     }
 
     void ROS2SystemCameraComponent::InitPassTemplateMappingsHandler()

+ 1 - 0
Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSystemComponent.h

@@ -24,6 +24,7 @@ namespace ROS2Sensors
         static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided);
         static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible);
         static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required);
+        static void GetDependentServices(AZ::ComponentDescriptor::DependencyArrayType& dependent);
 
         void InitPassTemplateMappingsHandler();
 

+ 5 - 0
Gems/ROS2Sensors/Code/Source/Camera/ROS2EditorCameraSystemComponent.cpp

@@ -36,6 +36,11 @@ namespace ROS2Sensors
         BaseSystemComponent::GetRequiredServices(required);
     }
 
+    void ROS2EditorCameraSystemComponent::GetDependentServices(AZ::ComponentDescriptor::DependencyArrayType& dependent)
+    {
+        BaseSystemComponent::GetDependentServices(dependent);
+    }
+
     void ROS2EditorCameraSystemComponent::Activate()
     {
         AzToolsFramework::EditorEntityContextNotificationBus::Handler::BusConnect();

+ 1 - 0
Gems/ROS2Sensors/Code/Source/Camera/ROS2EditorCameraSystemComponent.h

@@ -29,6 +29,7 @@ namespace ROS2Sensors
         static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided);
         static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible);
         static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required);
+        static void GetDependentServices(AZ::ComponentDescriptor::DependencyArrayType& dependent);
 
         //////////////////////////////////////////////////////////////////////////
         // Component overrides

+ 1 - 1
Gems/ROS2Sensors/Code/Source/Imu/ImuSensorConfiguration.cpp

@@ -6,9 +6,9 @@
  *
  */
 
-#include "ImuSensorConfiguration.h"
 #include <AzCore/Serialization/EditContext.h>
 #include <AzCore/Serialization/EditContextConstants.inl>
+#include <ROS2Sensors/Imu/ImuSensorConfiguration.h>
 
 namespace ROS2Sensors
 {

+ 99 - 3
Gems/ROS2Sensors/Code/Source/Imu/ROS2ImuSensorComponent.cpp

@@ -82,6 +82,7 @@ namespace ROS2Sensors
     void ROS2ImuSensorComponent::Activate()
     {
         ROS2SensorComponentBase::Activate();
+
         auto ros2Node = ROS2::ROS2Interface::Get()->GetNode();
         AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for IMU sensor");
         m_imuMsg.header.frame_id = GetFrameID().c_str();
@@ -89,9 +90,7 @@ namespace ROS2Sensors
         const auto fullTopic = ROS2::ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic);
         m_imuPublisher = ros2Node->create_publisher<sensor_msgs::msg::Imu>(fullTopic.data(), publisherConfig.GetQoS());
 
-        m_linearAccelerationCovariance = ToDiagonalCovarianceMatrix(m_imuConfiguration.m_linearAccelerationVariance);
-        m_angularVelocityCovariance = ToDiagonalCovarianceMatrix(m_imuConfiguration.m_angularVelocityVariance);
-        m_orientationCovariance = ToDiagonalCovarianceMatrix(m_imuConfiguration.m_orientationVariance);
+        ConfigureSensor();
 
         StartSensor(
             m_sensorConfiguration.m_frequency,
@@ -107,13 +106,18 @@ namespace ROS2Sensors
             {
                 OnPhysicsEvent(sceneHandle);
             });
+
+        ImuConfigurationRequestBus::Handler::BusConnect(GetEntityId());
     }
 
     void ROS2ImuSensorComponent::Deactivate()
     {
+        ImuConfigurationRequestBus::Handler::BusDisconnect(GetEntityId());
+
         StopSensor();
         m_bodyHandle = AzPhysics::InvalidSimulatedBodyHandle;
         m_imuPublisher.reset();
+
         ROS2SensorComponentBase::Deactivate();
     }
 
@@ -205,4 +209,96 @@ namespace ROS2Sensors
         return covarianceMatrix;
     }
 
+    void ROS2ImuSensorComponent::ConfigureSensor()
+    {
+        m_linearAccelerationCovariance = ToDiagonalCovarianceMatrix(m_imuConfiguration.m_linearAccelerationVariance);
+        m_angularVelocityCovariance = ToDiagonalCovarianceMatrix(m_imuConfiguration.m_angularVelocityVariance);
+        m_orientationCovariance = ToDiagonalCovarianceMatrix(m_imuConfiguration.m_orientationVariance);
+    }
+
+    const ImuSensorConfiguration ROS2ImuSensorComponent::GetConfiguration()
+    {
+        return m_imuConfiguration;
+    }
+
+    void ROS2ImuSensorComponent::SetConfiguration(const ImuSensorConfiguration& configuration)
+    {
+        m_imuConfiguration = configuration;
+        ConfigureSensor();
+    }
+
+    int ROS2ImuSensorComponent::GetFilterSize()
+    {
+        return m_imuConfiguration.m_filterSize;
+    }
+
+    void ROS2ImuSensorComponent::SetFilterSize(int filterSize)
+    {
+        if (filterSize < m_imuConfiguration.m_minFilterSize || filterSize > m_imuConfiguration.m_maxFilterSize)
+        {
+            AZ_Error(
+                "ROS2ImuSensorComponent",
+                false,
+                "Filter size %d is out of bounds [%d, %d].",
+                filterSize,
+                m_imuConfiguration.m_minFilterSize,
+                m_imuConfiguration.m_maxFilterSize);
+            return;
+        }
+        m_imuConfiguration.m_filterSize = filterSize;
+    }
+
+    bool ROS2ImuSensorComponent::GetIncludeGravity()
+    {
+        return m_imuConfiguration.m_includeGravity;
+    }
+
+    void ROS2ImuSensorComponent::SetIncludeGravity(bool includeGravity)
+    {
+        m_imuConfiguration.m_includeGravity = includeGravity;
+    }
+
+    bool ROS2ImuSensorComponent::GetAbsoluteRotation()
+    {
+        return m_imuConfiguration.m_absoluteRotation;
+    }
+
+    void ROS2ImuSensorComponent::SetAbsoluteRotation(bool absoluteRotation)
+    {
+        m_imuConfiguration.m_absoluteRotation = absoluteRotation;
+    }
+
+    AZ::Vector3 ROS2ImuSensorComponent::GetOrientationVariance()
+    {
+        return m_imuConfiguration.m_orientationVariance;
+    }
+
+    void ROS2ImuSensorComponent::SetOrientationVariance(const AZ::Vector3& orientationVariance)
+    {
+        m_imuConfiguration.m_orientationVariance = orientationVariance;
+        ConfigureSensor();
+    }
+
+    AZ::Vector3 ROS2ImuSensorComponent::GetAngularVelocityVariance()
+    {
+        return m_imuConfiguration.m_angularVelocityVariance;
+    }
+
+    void ROS2ImuSensorComponent::SetAngularVelocityVariance(const AZ::Vector3& angularVelocityVariance)
+    {
+        m_imuConfiguration.m_angularVelocityVariance = angularVelocityVariance;
+        ConfigureSensor();
+    }
+
+    AZ::Vector3 ROS2ImuSensorComponent::GetLinearAccelerationVariance()
+    {
+        return m_imuConfiguration.m_linearAccelerationVariance;
+    }
+
+    void ROS2ImuSensorComponent::SetLinearAccelerationVariance(const AZ::Vector3& linearAccelerationVariance)
+    {
+        m_imuConfiguration.m_linearAccelerationVariance = linearAccelerationVariance;
+        ConfigureSensor();
+    }
+
 } // namespace ROS2Sensors

+ 27 - 6
Gems/ROS2Sensors/Code/Source/Imu/ROS2ImuSensorComponent.h

@@ -14,18 +14,20 @@
 #include <AzFramework/Physics/Common/PhysicsEvents.h>
 #include <AzFramework/Physics/PhysicsSystem.h>
 #include <ROS2/Sensor/Events/PhysicsBasedSource.h>
+#include <ROS2Sensors/Imu/ImuConfigurationRequestBus.h>
+#include <ROS2Sensors/Imu/ImuSensorConfiguration.h>
 #include <ROS2Sensors/Sensor/ROS2SensorComponentBase.h>
 #include <rclcpp/publisher.hpp>
 #include <sensor_msgs/msg/imu.hpp>
 
-#include "ImuSensorConfiguration.h"
-
 namespace ROS2Sensors
 {
     //! An IMU (Inertial Measurement Unit) sensor Component.
     //! IMUs typically include gyroscopes, accelerometers and magnetometers. This component encapsulates data
     //! acquisition and its publishing to ROS2 ecosystem. IMU Component requires ROS2FrameComponent.
-    class ROS2ImuSensorComponent : public ROS2SensorComponentBase<ROS2::PhysicsBasedSource>
+    class ROS2ImuSensorComponent
+        : public ROS2SensorComponentBase<ROS2::PhysicsBasedSource>
+        , protected ImuConfigurationRequestBus::Handler
     {
     public:
         AZ_COMPONENT(ROS2ImuSensorComponent, ROS2Sensors::ROS2ImuSensorComponentTypeId, SensorBaseType);
@@ -41,6 +43,28 @@ namespace ROS2Sensors
         //////////////////////////////////////////////////////////////////////////
 
     private:
+        //////////////////////////////////////////////////////////////////////////
+        // ImuConfigurationRequestBus::Handler overrides
+        void SetConfiguration(const ImuSensorConfiguration& configuration) override;
+        const ImuSensorConfiguration GetConfiguration() override;
+        int GetFilterSize() override;
+        void SetFilterSize(int filterSize) override;
+        bool GetIncludeGravity() override;
+        void SetIncludeGravity(bool includeGravity) override;
+        bool GetAbsoluteRotation() override;
+        void SetAbsoluteRotation(bool absoluteRotation) override;
+        AZ::Vector3 GetOrientationVariance() override;
+        void SetOrientationVariance(const AZ::Vector3& orientationVariance) override;
+        AZ::Vector3 GetAngularVelocityVariance() override;
+        void SetAngularVelocityVariance(const AZ::Vector3& angularVelocityVariance) override;
+        AZ::Vector3 GetLinearAccelerationVariance() override;
+        void SetLinearAccelerationVariance(const AZ::Vector3& linearAccelerationVariance) override;
+        //////////////////////////////////////////////////////////////////////////
+
+        void ConfigureSensor();
+
+        ImuSensorConfiguration m_imuConfiguration;
+
         std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::Imu>> m_imuPublisher;
         sensor_msgs::msg::Imu m_imuMsg;
         AZ::Vector3 m_previousLinearVelocity = AZ::Vector3::CreateZero();
@@ -49,13 +73,10 @@ namespace ROS2Sensors
         AZStd::deque<AZ::Vector3> m_filterAcceleration;
         AZStd::deque<AZ::Vector3> m_filterAngularVelocity;
 
-        ImuSensorConfiguration m_imuConfiguration;
-
         AZ::Matrix3x3 m_orientationCovariance = AZ::Matrix3x3::CreateZero();
         AZ::Matrix3x3 m_angularVelocityCovariance = AZ::Matrix3x3::CreateZero();
         AZ::Matrix3x3 m_linearAccelerationCovariance = AZ::Matrix3x3::CreateZero();
 
-    private:
         void OnPhysicsEvent(AzPhysics::SceneHandle sceneHandle);
 
         void OnImuEvent(float imuDeltaTime, AzPhysics::SceneHandle sceneHandle, float physicsDeltaTime);

+ 3 - 3
Gems/ROS2Sensors/Code/Source/Lidar/LidarCore.h

@@ -7,15 +7,14 @@
  */
 #pragma once
 
+#include "LidarRaycaster.h"
 #include <Atom/RPI.Public/AuxGeom/AuxGeomDraw.h>
 #include <AzCore/Component/EntityId.h>
 #include <AzCore/Serialization/SerializeContext.h>
 #include <ROS2Sensors/Lidar/LidarRegistrarBus.h>
+#include <ROS2Sensors/Lidar/LidarSensorConfiguration.h>
 #include <ROS2Sensors/Lidar/LidarSystemBus.h>
 
-#include "LidarRaycaster.h"
-#include "LidarSensorConfiguration.h"
-
 namespace ROS2Sensors
 {
     //! A class for executing lidar operations, such as data acquisition and visualization.
@@ -50,6 +49,7 @@ namespace ROS2Sensors
         RaycastResultFlags GetResultFlags() const;
 
         //! Configuration according to which the lidar performs its raycasts.
+        //! Note: the configuration can be changed only when the lidar is not active.
         LidarSensorConfiguration m_lidarConfiguration;
 
     private:

+ 1 - 1
Gems/ROS2Sensors/Code/Source/Lidar/LidarRaycaster.cpp

@@ -13,7 +13,7 @@
 #include <AzFramework/Physics/PhysicsSystem.h>
 #include <AzFramework/Physics/Shape.h>
 #include <Lidar/LidarRaycaster.h>
-#include <Lidar/LidarTemplateUtils.h>
+#include <ROS2Sensors/Lidar/LidarTemplateUtils.h>
 #include <ROS2Sensors/Lidar/SegmentationUtils.h>
 
 namespace ROS2Sensors

+ 1 - 1
Gems/ROS2Sensors/Code/Source/Lidar/LidarRegistrarSystemComponent.cpp

@@ -9,7 +9,7 @@
 #include <AzCore/Serialization/EditContext.h>
 #include <AzCore/Serialization/SerializeContext.h>
 #include <Lidar/LidarRegistrarSystemComponent.h>
-#include <Lidar/LidarTemplate.h>
+#include <ROS2Sensors/Lidar/LidarTemplate.h>
 
 namespace ROS2Sensors
 {

+ 11 - 3
Gems/ROS2Sensors/Code/Source/Lidar/LidarSensorConfiguration.cpp

@@ -6,9 +6,10 @@
  *
  */
 
-#include "LidarSensorConfiguration.h"
+#include "Lidar/LidarRegistrarSystemComponent.h"
 #include <AzCore/Serialization/EditContext.h>
 #include <AzCore/Serialization/EditContextConstants.inl>
+#include <ROS2Sensors/Lidar/LidarSensorConfiguration.h>
 
 namespace ROS2Sensors
 {
@@ -106,7 +107,7 @@ namespace ROS2Sensors
     AZStd::vector<AZStd::string> LidarSensorConfiguration::GetAvailableModels() const
     {
         AZStd::vector<AZStd::string> result;
-        for (const auto model : m_availableModels)
+        for (const auto& model : m_availableModels)
         {
             auto templ = LidarTemplateUtils::GetTemplate(model);
             result.push_back({ templ.m_name });
@@ -116,7 +117,14 @@ namespace ROS2Sensors
 
     void LidarSensorConfiguration::FetchLidarModelConfiguration()
     {
-        for (const auto model : m_availableModels)
+        if (m_availableModels.empty())
+        {
+            AZ_Warning("LidarSensorConfiguration", false, "Lidar configuration created with an empty models list");
+            return;
+        }
+
+        m_lidarModel = m_availableModels.front();
+        for (const auto& model : m_availableModels)
         {
             auto templ = LidarTemplateUtils::GetTemplate(model);
             if (m_lidarModelName == templ.m_name)

+ 1 - 1
Gems/ROS2Sensors/Code/Source/Lidar/LidarTemplate.cpp

@@ -7,7 +7,7 @@
  */
 
 #include <AzCore/Serialization/EditContext.h>
-#include <Lidar/LidarTemplate.h>
+#include <ROS2Sensors/Lidar/LidarTemplate.h>
 
 namespace ROS2Sensors
 {

+ 1 - 1
Gems/ROS2Sensors/Code/Source/Lidar/LidarTemplateUtils.cpp

@@ -8,7 +8,7 @@
 
 #include <AzCore/Math/Quaternion.h>
 #include <AzCore/Math/Transform.h>
-#include <Lidar/LidarTemplateUtils.h>
+#include <ROS2Sensors/Lidar/LidarTemplateUtils.h>
 
 namespace ROS2Sensors
 {

+ 164 - 0
Gems/ROS2Sensors/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp

@@ -89,10 +89,13 @@ namespace ROS2Sensors
                 }
                 m_lidarCore.VisualizeResults();
             });
+
+        LidarConfigurationRequestBus::Handler::BusConnect(GetEntityId());
     }
 
     void ROS2Lidar2DSensorComponent::Deactivate()
     {
+        LidarConfigurationRequestBus::Handler::BusDisconnect(GetEntityId());
         StopSensor();
         m_laserScanPublisher.reset();
         m_lidarCore.Deinit();
@@ -144,4 +147,165 @@ namespace ROS2Sensors
 
         m_laserScanPublisher->publish(message);
     }
+
+    void ROS2Lidar2DSensorComponent::SetConfiguration(const LidarSensorConfiguration& configuration)
+    {
+        m_lidarCore.Deinit();
+        m_lidarCore.m_lidarConfiguration = configuration;
+        m_lidarCore.Init(GetEntityId());
+    }
+
+    const LidarSensorConfiguration ROS2Lidar2DSensorComponent::GetConfiguration()
+    {
+        return m_lidarCore.m_lidarConfiguration;
+    }
+
+    AZStd::string ROS2Lidar2DSensorComponent::GetModelName()
+    {
+        return m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_name;
+    }
+
+    void ROS2Lidar2DSensorComponent::SetModelName(const AZStd::string& name)
+    {
+        m_lidarCore.Deinit();
+        m_lidarCore.m_lidarConfiguration.m_lidarModelName = name;
+        m_lidarCore.m_lidarConfiguration.FetchLidarModelConfiguration();
+        // Unknown lidar models are set to custom lidar model with a given name
+        m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_name = name;
+        m_lidarCore.Init(GetEntityId());
+    }
+
+    bool ROS2Lidar2DSensorComponent::IsSegmentationEnabled()
+    {
+        return m_lidarCore.m_lidarConfiguration.m_isSegmentationEnabled;
+    }
+
+    void ROS2Lidar2DSensorComponent::SetSegmentationEnabled(bool enabled)
+    {
+        m_lidarCore.Deinit();
+        m_lidarCore.m_lidarConfiguration.m_isSegmentationEnabled = enabled;
+        m_lidarCore.Init(GetEntityId());
+    }
+
+    bool ROS2Lidar2DSensorComponent::IsAddPointsAtMaxEnabled()
+    {
+        return m_lidarCore.m_lidarConfiguration.m_addPointsAtMax;
+    }
+
+    void ROS2Lidar2DSensorComponent::SetAddPointsAtMaxEnabled(bool addPoints)
+    {
+        m_lidarCore.Deinit();
+        m_lidarCore.m_lidarConfiguration.m_addPointsAtMax = addPoints;
+        m_lidarCore.Init(GetEntityId());
+    }
+
+    bool ROS2Lidar2DSensorComponent::Is2D()
+    {
+        return m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_is2D;
+    }
+
+    float ROS2Lidar2DSensorComponent::GetMinHAngle()
+    {
+        return m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_minHAngle;
+    }
+
+    void ROS2Lidar2DSensorComponent::SetMinHAngle(float angle)
+    {
+        if (angle < -180.0f || angle > 180.0f)
+        {
+            AZ_Error("ROS2Lidar2DSensorComponent", false, "Min horizontal angle must be between -180 and 180 degrees.");
+            return;
+        }
+
+        m_lidarCore.Deinit();
+        m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_minHAngle = angle;
+        m_lidarCore.Init(GetEntityId());
+    }
+
+    float ROS2Lidar2DSensorComponent::GetMaxHAngle()
+    {
+        return m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_maxHAngle;
+    }
+
+    void ROS2Lidar2DSensorComponent::SetMaxHAngle(float angle)
+    {
+        if (angle < -180.0f || angle > 180.0f)
+        {
+            AZ_Error("ROS2Lidar2DSensorComponent", false, "Max horizontal angle must be between -180 and 180 degrees.");
+            return;
+        }
+
+        m_lidarCore.Deinit();
+        m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_maxHAngle = angle;
+        m_lidarCore.Init(GetEntityId());
+    }
+
+    unsigned int ROS2Lidar2DSensorComponent::GetNumberOfIncrements()
+    {
+        return m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_numberOfIncrements;
+    }
+
+    void ROS2Lidar2DSensorComponent::SetNumberOfIncrements(unsigned int increments)
+    {
+        m_lidarCore.Deinit();
+        m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_numberOfIncrements = increments;
+        m_lidarCore.Init(GetEntityId());
+    }
+
+    float ROS2Lidar2DSensorComponent::GetMinRange()
+    {
+        return m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_minRange;
+    }
+
+    void ROS2Lidar2DSensorComponent::SetMinRange(float range)
+    {
+        const float maxRange = m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_maxRange;
+        if (range < 0.0f || range > maxRange)
+        {
+            AZ_Error("ROS2Lidar2DSensorComponent", false, "Min range cannot be less than 0 or greater than max range (%f).", maxRange);
+            return;
+        }
+
+        m_lidarCore.Deinit();
+        m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_minRange = range;
+        m_lidarCore.Init(GetEntityId());
+    }
+
+    float ROS2Lidar2DSensorComponent::GetMaxRange()
+    {
+        return m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_maxRange;
+    }
+
+    void ROS2Lidar2DSensorComponent::SetMaxRange(float range)
+    {
+        const float minRange = m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_minRange;
+        if (range < minRange)
+        {
+            AZ_Error("ROS2Lidar2DSensorComponent", false, "Max range cannot be less than min range (%f).", minRange);
+            return;
+        }
+
+        m_lidarCore.Deinit();
+        m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_maxRange = range;
+        m_lidarCore.Init(GetEntityId());
+    }
+
+    const LidarTemplate::NoiseParameters& ROS2Lidar2DSensorComponent::GetNoiseParameters()
+    {
+        return m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_noiseParameters;
+    }
+
+    void ROS2Lidar2DSensorComponent::SetNoiseParameters(const LidarTemplate::NoiseParameters& params)
+    {
+        if (params.m_angularNoiseStdDev < 0.0f || params.m_angularNoiseStdDev > 180.0f || params.m_distanceNoiseStdDevBase < 0.0f ||
+            params.m_distanceNoiseStdDevRisePerMeter < 0.0f)
+        {
+            AZ_Error("ROS2Lidar2DSensorComponent", false, "Invalid noise parameters provided.");
+            return;
+        }
+
+        m_lidarCore.Deinit();
+        m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_noiseParameters = params;
+        m_lidarCore.Init(GetEntityId());
+    }
 } // namespace ROS2Sensors

+ 30 - 4
Gems/ROS2Sensors/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h

@@ -7,25 +7,27 @@
  */
 #pragma once
 
+#include "LidarCore.h"
+#include "LidarRaycaster.h"
 #include <Atom/RPI.Public/AuxGeom/AuxGeomDraw.h>
 #include <AzCore/Serialization/SerializeContext.h>
 #include <ROS2/Sensor/Events/TickBasedSource.h>
+#include <ROS2Sensors/Lidar/LidarConfigurationRequestBus.h>
 #include <ROS2Sensors/Lidar/LidarRegistrarBus.h>
 #include <ROS2Sensors/Lidar/LidarSystemBus.h>
 #include <ROS2Sensors/Sensor/ROS2SensorComponentBase.h>
 #include <rclcpp/publisher.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
 
-#include "LidarCore.h"
-#include "LidarRaycaster.h"
-
 namespace ROS2Sensors
 {
     //! Lidar 2D sensor Component.
     //! Lidars (Light Detection and Ranging) emit laser light and measure it after reflection.
     //! Lidar Component allows customization of lidar type and behavior and encapsulates both simulation
     //! and data publishing. It requires ROS2FrameComponent.
-    class ROS2Lidar2DSensorComponent : public ROS2SensorComponentBase<ROS2::TickBasedSource>
+    class ROS2Lidar2DSensorComponent
+        : public ROS2SensorComponentBase<ROS2::TickBasedSource>
+        , protected LidarConfigurationRequestBus::Handler
     {
     public:
         AZ_COMPONENT(ROS2Lidar2DSensorComponent, ROS2Sensors::ROS2Lidar2DSensorComponentTypeId, SensorBaseType);
@@ -42,6 +44,30 @@ namespace ROS2Sensors
 
     private:
         //////////////////////////////////////////////////////////////////////////
+        // LidarConfigurationRequestBus::Handler overrides
+        const LidarSensorConfiguration GetConfiguration() override;
+        void SetConfiguration(const LidarSensorConfiguration& configuration) override;
+        AZStd::string GetModelName() override;
+        void SetModelName(const AZStd::string& name) override;
+        bool IsSegmentationEnabled() override;
+        void SetSegmentationEnabled(bool enabled) override;
+        bool IsAddPointsAtMaxEnabled() override;
+        void SetAddPointsAtMaxEnabled(bool addPoints) override;
+        bool Is2D() override;
+        float GetMinHAngle() override;
+        void SetMinHAngle(float angle) override;
+        float GetMaxHAngle() override;
+        void SetMaxHAngle(float angle) override;
+        unsigned int GetNumberOfIncrements() override;
+        void SetNumberOfIncrements(unsigned int increments) override;
+        float GetMinRange() override;
+        void SetMinRange(float range) override;
+        float GetMaxRange() override;
+        void SetMaxRange(float range) override;
+        const LidarTemplate::NoiseParameters& GetNoiseParameters() override;
+        void SetNoiseParameters(const LidarTemplate::NoiseParameters& params) override;
+        //////////////////////////////////////////////////////////////////////////
+
         void FrequencyTick();
 
         void PublishRaycastResults(const RaycastResults& results);

+ 211 - 0
Gems/ROS2Sensors/Code/Source/Lidar/ROS2LidarSensorComponent.cpp

@@ -127,10 +127,13 @@ namespace ROS2Sensors
                 }
                 m_lidarCore.VisualizeResults();
             });
+
+        LidarConfigurationRequestBus::Handler::BusConnect(GetEntityId());
     }
 
     void ROS2LidarSensorComponent::Deactivate()
     {
+        LidarConfigurationRequestBus::Handler::BusDisconnect(GetEntityId());
         StopSensor();
         m_pointCloudPublisher.reset();
         m_lidarCore.Deinit();
@@ -285,4 +288,212 @@ namespace ROS2Sensors
             m_segmentationClassesPublisher->publish(segmentationClasses);
         }
     }
+
+    void ROS2LidarSensorComponent::SetConfiguration(const LidarSensorConfiguration& configuration)
+    {
+        m_lidarCore.Deinit();
+        m_lidarCore.m_lidarConfiguration = configuration;
+        m_lidarCore.Init(GetEntityId());
+    }
+
+    const LidarSensorConfiguration ROS2LidarSensorComponent::GetConfiguration()
+    {
+        return m_lidarCore.m_lidarConfiguration;
+    }
+
+    AZStd::string ROS2LidarSensorComponent::GetModelName()
+    {
+        return m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_name;
+    }
+
+    void ROS2LidarSensorComponent::SetModelName(const AZStd::string& name)
+    {
+        m_lidarCore.Deinit();
+        m_lidarCore.m_lidarConfiguration.m_lidarModelName = name;
+        m_lidarCore.m_lidarConfiguration.FetchLidarModelConfiguration();
+        // Unknown lidar models are set to custom lidar model with a given name
+        m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_name = name;
+        m_lidarCore.Init(GetEntityId());
+    }
+
+    bool ROS2LidarSensorComponent::IsSegmentationEnabled()
+    {
+        return m_lidarCore.m_lidarConfiguration.m_isSegmentationEnabled;
+    }
+
+    void ROS2LidarSensorComponent::SetSegmentationEnabled(bool enabled)
+    {
+        m_lidarCore.Deinit();
+        m_lidarCore.m_lidarConfiguration.m_isSegmentationEnabled = enabled;
+        m_lidarCore.Init(GetEntityId());
+    }
+
+    bool ROS2LidarSensorComponent::IsAddPointsAtMaxEnabled()
+    {
+        return m_lidarCore.m_lidarConfiguration.m_addPointsAtMax;
+    }
+
+    void ROS2LidarSensorComponent::SetAddPointsAtMaxEnabled(bool addPoints)
+    {
+        m_lidarCore.Deinit();
+        m_lidarCore.m_lidarConfiguration.m_addPointsAtMax = addPoints;
+        m_lidarCore.Init(GetEntityId());
+    }
+
+    bool ROS2LidarSensorComponent::Is2D()
+    {
+        return m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_is2D;
+    }
+
+    float ROS2LidarSensorComponent::GetMinHAngle()
+    {
+        return m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_minHAngle;
+    }
+
+    void ROS2LidarSensorComponent::SetMinHAngle(float angle)
+    {
+        if (angle < -180.0f || angle > 180.0f)
+        {
+            AZ_Error("ROS2LidarSensorComponent", false, "Min horizontal angle must be between -180 and 180 degrees.");
+            return;
+        }
+
+        m_lidarCore.Deinit();
+        m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_minHAngle = angle;
+        m_lidarCore.Init(GetEntityId());
+    }
+
+    float ROS2LidarSensorComponent::GetMaxHAngle()
+    {
+        return m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_maxHAngle;
+    }
+
+    void ROS2LidarSensorComponent::SetMaxHAngle(float angle)
+    {
+        if (angle < -180.0f || angle > 180.0f)
+        {
+            AZ_Error("ROS2LidarSensorComponent", false, "Max horizontal angle must be between -180 and 180 degrees.");
+            return;
+        }
+
+        m_lidarCore.Deinit();
+        m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_maxHAngle = angle;
+        m_lidarCore.Init(GetEntityId());
+    }
+
+    float ROS2LidarSensorComponent::GetMinVAngle()
+    {
+        return m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_minVAngle;
+    }
+
+    void ROS2LidarSensorComponent::SetMinVAngle(float angle)
+    {
+        if (angle < -90.0f || angle > 90.0f)
+        {
+            AZ_Error("ROS2LidarSensorComponent", false, "Min vertical angle must be between -90 and 90 degrees.");
+            return;
+        }
+        m_lidarCore.Deinit();
+        m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_minVAngle = angle;
+        m_lidarCore.Init(GetEntityId());
+    }
+
+    float ROS2LidarSensorComponent::GetMaxVAngle()
+    {
+        return m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_maxVAngle;
+    }
+
+    void ROS2LidarSensorComponent::SetMaxVAngle(float angle)
+    {
+        if (angle < -90.0f || angle > 90.0f)
+        {
+            AZ_Error("ROS2LidarSensorComponent", false, "Max vertical angle must be between -90 and 90 degrees.");
+            return;
+        }
+
+        m_lidarCore.Deinit();
+        m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_maxVAngle = angle;
+        m_lidarCore.Init(GetEntityId());
+    }
+
+    unsigned int ROS2LidarSensorComponent::GetLayers()
+    {
+        return m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_layers;
+    }
+
+    void ROS2LidarSensorComponent::SetLayers(unsigned int layers)
+    {
+        m_lidarCore.Deinit();
+        m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_layers = layers;
+        m_lidarCore.Init(GetEntityId());
+    }
+
+    unsigned int ROS2LidarSensorComponent::GetNumberOfIncrements()
+    {
+        return m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_numberOfIncrements;
+    }
+
+    void ROS2LidarSensorComponent::SetNumberOfIncrements(unsigned int increments)
+    {
+        m_lidarCore.Deinit();
+        m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_numberOfIncrements = increments;
+        m_lidarCore.Init(GetEntityId());
+    }
+
+    float ROS2LidarSensorComponent::GetMinRange()
+    {
+        return m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_minRange;
+    }
+
+    void ROS2LidarSensorComponent::SetMinRange(float range)
+    {
+        const float maxRange = m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_maxRange;
+        if (range < 0.0f || range > maxRange)
+        {
+            AZ_Error("ROS2LidarSensorComponent", false, "Min range cannot be less than 0 or greater than max range (%f).", maxRange);
+            return;
+        }
+
+        m_lidarCore.Deinit();
+        m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_minRange = range;
+        m_lidarCore.Init(GetEntityId());
+    }
+
+    float ROS2LidarSensorComponent::GetMaxRange()
+    {
+        return m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_maxRange;
+    }
+
+    void ROS2LidarSensorComponent::SetMaxRange(float range)
+    {
+        const float minRange = m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_minRange;
+        if (range < minRange)
+        {
+            AZ_Error("ROS2LidarSensorComponent", false, "Max range cannot be less than min range (%f).", minRange);
+            return;
+        }
+
+        m_lidarCore.Deinit();
+        m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_maxRange = range;
+        m_lidarCore.Init(GetEntityId());
+    }
+
+    const LidarTemplate::NoiseParameters& ROS2LidarSensorComponent::GetNoiseParameters()
+    {
+        return m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_noiseParameters;
+    }
+
+    void ROS2LidarSensorComponent::SetNoiseParameters(const LidarTemplate::NoiseParameters& params)
+    {
+        if (params.m_angularNoiseStdDev < 0.0f || params.m_angularNoiseStdDev > 180.0f || params.m_distanceNoiseStdDevBase < 0.0f ||
+            params.m_distanceNoiseStdDevRisePerMeter < 0.0f)
+        {
+            AZ_Error("ROS2LidarSensorComponent", false, "Invalid noise parameters provided.");
+            return;
+        }
+
+        m_lidarCore.Deinit();
+        m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_noiseParameters = params;
+        m_lidarCore.Init(GetEntityId());
+    }
 } // namespace ROS2Sensors

+ 37 - 7
Gems/ROS2Sensors/Code/Source/Lidar/ROS2LidarSensorComponent.h

@@ -7,27 +7,27 @@
  */
 #pragma once
 
+#include "LidarCore.h"
+#include "LidarRaycaster.h"
 #include <Atom/RPI.Public/AuxGeom/AuxGeomDraw.h>
 #include <AzCore/Serialization/SerializeContext.h>
 #include <ROS2/Sensor/Events/TickBasedSource.h>
-#include <ROS2Sensors/Lidar/LidarRegistrarBus.h>
-#include <ROS2Sensors/Lidar/LidarSystemBus.h>
+#include <ROS2Sensors/Lidar/LidarConfigurationRequestBus.h>
+#include <ROS2Sensors/Lidar/LidarSensorConfiguration.h>
 #include <ROS2Sensors/Sensor/ROS2SensorComponentBase.h>
 #include <rclcpp/publisher.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <vision_msgs/msg/label_info.hpp>
 
-#include "LidarCore.h"
-#include "LidarRaycaster.h"
-#include "LidarSensorConfiguration.h"
-
 namespace ROS2Sensors
 {
     //! Lidar sensor Component.
     //! Lidars (Light Detection and Ranging) emit laser light and measure it after reflection.
     //! Lidar Component allows customization of lidar type and behavior and encapsulates both simulation
     //! and data publishing. It requires ROS2FrameComponent.
-    class ROS2LidarSensorComponent : public ROS2SensorComponentBase<ROS2::TickBasedSource>
+    class ROS2LidarSensorComponent
+        : public ROS2SensorComponentBase<ROS2::TickBasedSource>
+        , protected LidarConfigurationRequestBus::Handler
     {
     public:
         AZ_COMPONENT(ROS2LidarSensorComponent, ROS2Sensors::ROS2LidarSensorComponentTypeId, SensorBaseType);
@@ -44,6 +44,36 @@ namespace ROS2Sensors
 
     private:
         //////////////////////////////////////////////////////////////////////////
+        // LidarConfigurationRequestBus::Handler overrides
+        const LidarSensorConfiguration GetConfiguration() override;
+        void SetConfiguration(const LidarSensorConfiguration& configuration) override;
+        AZStd::string GetModelName() override;
+        void SetModelName(const AZStd::string& name) override;
+        bool IsSegmentationEnabled() override;
+        void SetSegmentationEnabled(bool enabled) override;
+        bool IsAddPointsAtMaxEnabled() override;
+        void SetAddPointsAtMaxEnabled(bool addPoints) override;
+        bool Is2D() override;
+        float GetMinHAngle() override;
+        void SetMinHAngle(float angle) override;
+        float GetMaxHAngle() override;
+        void SetMaxHAngle(float angle) override;
+        float GetMinVAngle() override;
+        void SetMinVAngle(float angle) override;
+        float GetMaxVAngle() override;
+        void SetMaxVAngle(float angle) override;
+        unsigned int GetLayers() override;
+        void SetLayers(unsigned int layers) override;
+        unsigned int GetNumberOfIncrements() override;
+        void SetNumberOfIncrements(unsigned int increments) override;
+        float GetMinRange() override;
+        void SetMinRange(float range) override;
+        float GetMaxRange() override;
+        void SetMaxRange(float range) override;
+        const LidarTemplate::NoiseParameters& GetNoiseParameters() override;
+        void SetNoiseParameters(const LidarTemplate::NoiseParameters& params) override;
+        //////////////////////////////////////////////////////////////////////////
+
         void FrequencyTick();
         void PublishRaycastResults(const RaycastResults& results);
 

+ 1 - 2
Gems/ROS2Sensors/Code/Source/Odometry/ROS2OdometryCovariance.cpp

@@ -6,12 +6,11 @@
  *
  */
 
-#include "ROS2OdometryCovariance.h"
 #include <AzCore/RTTI/RTTIMacros.h>
 #include <AzCore/Serialization/EditContext.h>
 #include <AzCore/Serialization/EditContextConstants.inl>
 #include <AzCore/base.h>
-#include <array>
+#include <ROS2Sensors/Odometry/ROS2OdometryCovariance.h>
 
 namespace ROS2Sensors
 {

+ 0 - 140
Gems/ROS2Sensors/Code/Source/Odometry/ROS2WheelOdometry.cpp

@@ -1,140 +0,0 @@
-/*
- * 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 "ROS2WheelOdometry.h"
-#include "Odometry/ROS2OdometryCovariance.h"
-#include <ROS2/Utilities/ROS2Conversions.h>
-#include <ROS2/Utilities/ROS2Names.h>
-
-namespace ROS2Sensors
-{
-    namespace
-    {
-        const char* WheelOdometryMsgType = "nav_msgs::msg::Odometry";
-    }
-
-    void ROS2WheelOdometryComponent::Reflect(AZ::ReflectContext* context)
-    {
-        ROS2OdometryCovariance::Reflect(context);
-
-        if (auto* serialize = azrtti_cast<AZ::SerializeContext*>(context))
-        {
-            serialize->Class<ROS2WheelOdometryComponent, SensorBaseType>()
-                ->Version(2)
-                ->Field("Twist covariance", &ROS2WheelOdometryComponent::m_twistCovariance)
-                ->Field("Pose covariance", &ROS2WheelOdometryComponent::m_poseCovariance);
-
-            if (auto* editContext = serialize->GetEditContext())
-            {
-                editContext->Class<ROS2WheelOdometryComponent>("ROS2 Wheel Odometry Sensor", "Odometry sensor component")
-                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
-                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
-                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game"))
-                    ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/ROS2WheelOdometrySensor.svg")
-                    ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/ROS2WheelOdometrySensor.svg")
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &ROS2WheelOdometryComponent::m_twistCovariance,
-                        "Twist covariance",
-                        "Set ROS twist covariance")
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &ROS2WheelOdometryComponent::m_poseCovariance,
-                        "Pose covariance",
-                        "Set ROS pose covariance");
-            }
-        }
-    }
-
-    ROS2WheelOdometryComponent::ROS2WheelOdometryComponent()
-    {
-        ROS2::TopicConfiguration tc;
-        const AZStd::string type = WheelOdometryMsgType;
-        tc.m_type = type;
-        tc.m_topic = "odom";
-        m_sensorConfiguration.m_frequency = 10;
-        m_sensorConfiguration.m_publishersConfigurations.insert(AZStd::make_pair(type, tc));
-    }
-
-    void ROS2WheelOdometryComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
-    {
-        required.push_back(AZ_CRC_CE("ROS2Frame"));
-        required.push_back(AZ_CRC_CE("SkidSteeringModelService"));
-    }
-
-    void ROS2WheelOdometryComponent::OnOdometryEvent()
-    {
-        m_odometryMsg.pose.pose.position = ROS2::ROS2Conversions::ToROS2Point(m_robotPose);
-        m_odometryMsg.pose.pose.orientation = ROS2::ROS2Conversions::ToROS2Quaternion(m_robotRotation);
-        m_odometryMsg.pose.covariance = m_poseCovariance.GetRosCovariance();
-
-        m_odometryPublisher->publish(m_odometryMsg);
-    }
-
-    void ROS2WheelOdometryComponent::OnPhysicsEvent(float physicsDeltaTime)
-    {
-        AZStd::pair<AZ::Vector3, AZ::Vector3> vt;
-
-        // Temporarily disabled until the ROS2Controllers gem is available
-        // VehicleDynamics::VehicleInputControlRequestBus::EventResult(
-        //     vt, GetEntityId(), &VehicleDynamics::VehicleInputControlRequests::GetWheelsOdometry);
-
-        m_odometryMsg.header.stamp = ROS2::ROS2Interface::Get()->GetROSTimestamp();
-        m_odometryMsg.twist.twist.linear = ROS2::ROS2Conversions::ToROS2Vector3(vt.first);
-        m_odometryMsg.twist.twist.angular = ROS2::ROS2Conversions::ToROS2Vector3(vt.second);
-        m_odometryMsg.twist.covariance = m_twistCovariance.GetRosCovariance();
-
-        if (m_sensorConfiguration.m_frequency > 0)
-        {
-            const auto updatePos = physicsDeltaTime * vt.first; // in meters
-            const auto updateRot = physicsDeltaTime * vt.second; // in radians
-            m_robotPose += m_robotRotation.TransformVector(updatePos);
-            m_robotRotation *= AZ::Quaternion::CreateFromScaledAxisAngle(updateRot);
-        }
-    }
-
-    void ROS2WheelOdometryComponent::Activate()
-    {
-        ROS2SensorComponentBase::Activate();
-        m_robotPose = AZ::Vector3{ 0 };
-        m_robotRotation = AZ::Quaternion{ 0, 0, 0, 1 };
-
-        // "odom" is globally fixed frame for all robots, no matter the namespace
-        m_odometryMsg.header.frame_id = ROS2::ROS2Names::GetNamespacedName(GetNamespace(), "odom").c_str();
-        m_odometryMsg.child_frame_id = GetFrameID().c_str();
-
-        auto ros2Node = ROS2::ROS2Interface::Get()->GetNode();
-        AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for Odometry sensor");
-
-        const auto& publisherConfig = m_sensorConfiguration.m_publishersConfigurations[WheelOdometryMsgType];
-        const auto fullTopic = ROS2::ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic);
-        m_odometryPublisher = ros2Node->create_publisher<nav_msgs::msg::Odometry>(fullTopic.data(), publisherConfig.GetQoS());
-
-        StartSensor(
-            m_sensorConfiguration.m_frequency,
-            [this]([[maybe_unused]] auto&&... args)
-            {
-                if (!m_sensorConfiguration.m_publishingEnabled)
-                {
-                    return;
-                }
-                OnOdometryEvent();
-            },
-            [this]([[maybe_unused]] AzPhysics::SceneHandle sceneHandle, float physicsDeltaTime)
-            {
-                OnPhysicsEvent(physicsDeltaTime);
-            });
-    }
-
-    void ROS2WheelOdometryComponent::Deactivate()
-    {
-        StopSensor();
-        m_odometryPublisher.reset();
-        ROS2SensorComponentBase::Deactivate();
-    }
-} // namespace ROS2Sensors

+ 0 - 49
Gems/ROS2Sensors/Code/Source/Odometry/ROS2WheelOdometry.h

@@ -1,49 +0,0 @@
-/*
- * 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/Math/Transform.h>
-#include <ROS2/Sensor/Events/PhysicsBasedSource.h>
-#include <ROS2Sensors/Sensor/ROS2SensorComponentBase.h>
-#include <nav_msgs/msg/odometry.hpp>
-#include <rclcpp/publisher.hpp>
-
-#include "ROS2OdometryCovariance.h"
-
-namespace ROS2Sensors
-{
-    //! Wheel odometry sensor component.
-    //! It constructs and publishes an odometry message, which contains information about the vehicle's velocity and position in space.
-    //! This is a physical sensor that takes a vehicle's configuration and computes updates from the wheels' rotations.
-    //! @see <a href="https://index.ros.org/p/nav_msgs/">nav_msgs package</a>.
-    class ROS2WheelOdometryComponent : public ROS2SensorComponentBase<ROS2::PhysicsBasedSource>
-    {
-    public:
-        AZ_COMPONENT(ROS2WheelOdometryComponent, ROS2Sensors::ROS2WheelOdometryComponentTypeId, SensorBaseType);
-        ROS2WheelOdometryComponent();
-        ~ROS2WheelOdometryComponent() = default;
-        static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required);
-        static void Reflect(AZ::ReflectContext* context);
-        //////////////////////////////////////////////////////////////////////////
-        // Component overrides
-        void Activate() override;
-        void Deactivate() override;
-        //////////////////////////////////////////////////////////////////////////
-
-    private:
-        std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> m_odometryPublisher;
-        nav_msgs::msg::Odometry m_odometryMsg;
-        AZ::Vector3 m_robotPose{ 0 };
-        AZ::Quaternion m_robotRotation{ 0, 0, 0, 1 };
-        ROS2OdometryCovariance m_poseCovariance;
-        ROS2OdometryCovariance m_twistCovariance;
-
-        void OnOdometryEvent();
-        void OnPhysicsEvent(float physicsDeltaTime);
-    };
-} // namespace ROS2Sensors

+ 251 - 0
Gems/ROS2Sensors/Code/Source/Odometry/ROS2WheelOdometrySensorComponent.cpp

@@ -0,0 +1,251 @@
+/*
+ * 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 "ROS2WheelOdometrySensorComponent.h"
+#include <AzCore/Serialization/Json/JsonSerialization.h>
+#include <AzCore/Serialization/Json/JsonSerializationResult.h>
+#include <AzCore/Serialization/Json/RegistrationContext.h>
+#include <ROS2/Utilities/ROS2Conversions.h>
+#include <ROS2/Utilities/ROS2Names.h>
+#include <ROS2Sensors/Odometry/ROS2OdometryCovariance.h>
+
+namespace ROS2Sensors
+{
+    namespace
+    {
+        const char* WheelOdometryMsgType = "nav_msgs::msg::Odometry";
+    }
+
+    // Manual conversion between version without a configuration struct and with a configuration struct.
+    // This is done to maintain backwards compatibility with older versions of the component.
+    // This function checks if in the prefab there exits a member called "Twist covariance" or "Pose covariance".
+    // If it does, it will load the values into the new configuration struct.
+    // If it doesn't, it will treat the loaded json as it would load the new version of the component.
+    // Checking old members is used instead of checking if there is a member called "Odometry configuration"
+    // because is default values are used O3DE does not create an empty member and initializes the component with default values.
+    // Meaning if there does not exist a member called "Odometry configuration" this component could use old values or default ones.
+    AZ::JsonSerializationResult::Result JsonROS2WheelOdometryComponentConfigSerializer::Load(
+        void* outputValue, const AZ::Uuid& outputValueTypeId, const rapidjson::Value& inputValue, AZ::JsonDeserializerContext& context)
+    {
+        namespace JSR = AZ::JsonSerializationResult;
+
+        auto configInstance = reinterpret_cast<ROS2WheelOdometryComponent*>(outputValue);
+        AZ_Assert(configInstance, "Output value for JsonROS2WheelOdometryComponentConfigSerializer can't be null.");
+
+        JSR::ResultCode result(JSR::Tasks::ReadField);
+
+        const bool hasOldMembers = inputValue.HasMember("Twist covariance") || inputValue.HasMember("Pose covariance");
+        if (hasOldMembers)
+        {
+            {
+                JSR::ResultCode componentIdLoadResult = ContinueLoadingFromJsonObjectField(
+                    &configInstance->m_odometryConfiguration.m_poseCovariance,
+                    azrtti_typeid<decltype(configInstance->m_odometryConfiguration.m_poseCovariance)>(),
+                    inputValue,
+                    "Pose covariance",
+                    context);
+
+                result.Combine(componentIdLoadResult);
+            }
+
+            {
+                JSR::ResultCode componentIdLoadResult = ContinueLoadingFromJsonObjectField(
+                    &configInstance->m_odometryConfiguration.m_twistCovariance,
+                    azrtti_typeid<decltype(configInstance->m_odometryConfiguration.m_twistCovariance)>(),
+                    inputValue,
+                    "Twist covariance",
+                    context);
+
+                result.Combine(componentIdLoadResult);
+            }
+        }
+        else
+        {
+            {
+                JSR::ResultCode componentIdLoadResult = ContinueLoadingFromJsonObjectField(
+                    &configInstance->m_odometryConfiguration,
+                    azrtti_typeid<decltype(configInstance->m_odometryConfiguration)>(),
+                    inputValue,
+                    "Odometry configuration",
+                    context);
+
+                result.Combine(componentIdLoadResult);
+            }
+        }
+        {
+            JSR::ResultCode componentIdLoadResult = ContinueLoadingFromJsonObjectField(
+                &configInstance->m_sensorConfiguration,
+                azrtti_typeid<decltype(configInstance->m_sensorConfiguration)>(),
+                inputValue,
+                "Sensor configuration",
+                context);
+
+            result.Combine(componentIdLoadResult);
+        }
+
+        return context.Report(
+            result,
+            result.GetProcessing() != JSR::Processing::Halted ? "Successfully loaded ROS2FrameComponent information."
+                                                              : "Failed to load ROS2FrameComponent information.");
+    }
+
+    AZ_CLASS_ALLOCATOR_IMPL(JsonROS2WheelOdometryComponentConfigSerializer, AZ::SystemAllocator);
+
+    void ROS2WheelOdometryComponent::Reflect(AZ::ReflectContext* context)
+    {
+        if (auto jsonContext = azrtti_cast<AZ::JsonRegistrationContext*>(context))
+        {
+            jsonContext->Serializer<JsonROS2WheelOdometryComponentConfigSerializer>()->HandlesType<ROS2WheelOdometryComponent>();
+        }
+
+        WheelOdometrySensorConfiguration::Reflect(context);
+
+        if (auto* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<ROS2WheelOdometryComponent, SensorBaseType>()->Version(3)->Field(
+                "Odometry configuration", &ROS2WheelOdometryComponent::m_odometryConfiguration);
+
+            if (auto* editContext = serialize->GetEditContext())
+            {
+                editContext->Class<ROS2WheelOdometryComponent>("ROS2 Wheel Odometry Sensor", "Odometry sensor component")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
+                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game"))
+                    ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/ROS2WheelOdometrySensor.svg")
+                    ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/ROS2WheelOdometrySensor.svg")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &ROS2WheelOdometryComponent::m_odometryConfiguration,
+                        "Odometry configuration",
+                        "Odometry sensor configuration")
+                    ->Attribute(AZ::Edit::Attributes::Visibility, AZ::Edit::PropertyVisibility::ShowChildrenOnly);
+            }
+        }
+    }
+
+    ROS2WheelOdometryComponent::ROS2WheelOdometryComponent()
+    {
+        ROS2::TopicConfiguration tc;
+        const AZStd::string type = WheelOdometryMsgType;
+        tc.m_type = type;
+        tc.m_topic = "odom";
+        m_sensorConfiguration.m_frequency = 10;
+        m_sensorConfiguration.m_publishersConfigurations.insert(AZStd::make_pair(type, tc));
+    }
+
+    void ROS2WheelOdometryComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
+    {
+        required.push_back(AZ_CRC_CE("ROS2Frame"));
+        required.push_back(AZ_CRC_CE("SkidSteeringModelService"));
+    }
+
+    void ROS2WheelOdometryComponent::OnOdometryEvent()
+    {
+        m_odometryMsg.pose.pose.position = ROS2::ROS2Conversions::ToROS2Point(m_robotPose);
+        m_odometryMsg.pose.pose.orientation = ROS2::ROS2Conversions::ToROS2Quaternion(m_robotRotation);
+        m_odometryMsg.pose.covariance = m_odometryConfiguration.m_poseCovariance.GetRosCovariance();
+
+        m_odometryPublisher->publish(m_odometryMsg);
+    }
+
+    void ROS2WheelOdometryComponent::OnPhysicsEvent(float physicsDeltaTime)
+    {
+        AZStd::pair<AZ::Vector3, AZ::Vector3> vt;
+
+        // Temporarily disabled until the ROS2Controllers gem is available
+        // VehicleDynamics::VehicleInputControlRequestBus::EventResult(
+        //     vt, GetEntityId(), &VehicleDynamics::VehicleInputControlRequests::GetWheelsOdometry);
+
+        m_odometryMsg.header.stamp = ROS2::ROS2Interface::Get()->GetROSTimestamp();
+        m_odometryMsg.twist.twist.linear = ROS2::ROS2Conversions::ToROS2Vector3(vt.first);
+        m_odometryMsg.twist.twist.angular = ROS2::ROS2Conversions::ToROS2Vector3(vt.second);
+        m_odometryMsg.twist.covariance = m_odometryConfiguration.m_twistCovariance.GetRosCovariance();
+
+        if (m_sensorConfiguration.m_frequency > 0)
+        {
+            const auto updatePos = physicsDeltaTime * vt.first; // in meters
+            const auto updateRot = physicsDeltaTime * vt.second; // in radians
+            m_robotPose += m_robotRotation.TransformVector(updatePos);
+            m_robotRotation *= AZ::Quaternion::CreateFromScaledAxisAngle(updateRot);
+        }
+    }
+
+    void ROS2WheelOdometryComponent::Activate()
+    {
+        ROS2SensorComponentBase::Activate();
+        m_robotPose = AZ::Vector3{ 0 };
+        m_robotRotation = AZ::Quaternion{ 0, 0, 0, 1 };
+
+        // "odom" is globally fixed frame for all robots, no matter the namespace
+        m_odometryMsg.header.frame_id = ROS2::ROS2Names::GetNamespacedName(GetNamespace(), "odom").c_str();
+        m_odometryMsg.child_frame_id = GetFrameID().c_str();
+
+        auto ros2Node = ROS2::ROS2Interface::Get()->GetNode();
+        AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for Odometry sensor");
+
+        const auto& publisherConfig = m_sensorConfiguration.m_publishersConfigurations[WheelOdometryMsgType];
+        const auto fullTopic = ROS2::ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic);
+        m_odometryPublisher = ros2Node->create_publisher<nav_msgs::msg::Odometry>(fullTopic.data(), publisherConfig.GetQoS());
+
+        StartSensor(
+            m_sensorConfiguration.m_frequency,
+            [this]([[maybe_unused]] auto&&... args)
+            {
+                if (!m_sensorConfiguration.m_publishingEnabled)
+                {
+                    return;
+                }
+                OnOdometryEvent();
+            },
+            [this]([[maybe_unused]] AzPhysics::SceneHandle sceneHandle, float physicsDeltaTime)
+            {
+                OnPhysicsEvent(physicsDeltaTime);
+            });
+
+        WheelOdometryConfigurationRequestBus::Handler::BusConnect(GetEntityId());
+    }
+
+    void ROS2WheelOdometryComponent::Deactivate()
+    {
+        WheelOdometryConfigurationRequestBus::Handler::BusDisconnect(GetEntityId());
+        StopSensor();
+        m_odometryPublisher.reset();
+        ROS2SensorComponentBase::Deactivate();
+    }
+
+    const WheelOdometrySensorConfiguration ROS2WheelOdometryComponent::GetConfiguration()
+    {
+        return m_odometryConfiguration;
+    }
+
+    void ROS2WheelOdometryComponent::SetConfiguration(const WheelOdometrySensorConfiguration& configuration)
+    {
+        m_odometryConfiguration = configuration;
+    }
+
+    ROS2OdometryCovariance ROS2WheelOdometryComponent::GetPoseCovariance()
+    {
+        return m_odometryConfiguration.m_poseCovariance;
+    }
+
+    void ROS2WheelOdometryComponent::SetPoseCovariance(const ROS2OdometryCovariance& covariance)
+    {
+        m_odometryConfiguration.m_poseCovariance = covariance;
+    }
+
+    ROS2OdometryCovariance ROS2WheelOdometryComponent::GetTwistCovariance()
+    {
+        return m_odometryConfiguration.m_twistCovariance;
+    }
+
+    void ROS2WheelOdometryComponent::SetTwistCovariance(const ROS2OdometryCovariance& covariance)
+    {
+        m_odometryConfiguration.m_twistCovariance = covariance;
+    }
+
+} // namespace ROS2Sensors

+ 76 - 0
Gems/ROS2Sensors/Code/Source/Odometry/ROS2WheelOdometrySensorComponent.h

@@ -0,0 +1,76 @@
+/*
+ * 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/Math/Transform.h>
+#include <ROS2/Sensor/Events/PhysicsBasedSource.h>
+#include <ROS2Sensors/Odometry/ROS2OdometryCovariance.h>
+#include <ROS2Sensors/Odometry/WheelOdometryConfigurationRequestBus.h>
+#include <ROS2Sensors/Sensor/ROS2SensorComponentBase.h>
+#include <nav_msgs/msg/odometry.hpp>
+#include <rclcpp/publisher.hpp>
+
+namespace ROS2Sensors
+{
+    class JsonROS2WheelOdometryComponentConfigSerializer : public AZ::BaseJsonSerializer
+    {
+    public:
+        AZ_RTTI(JsonROS2WheelOdometryComponentConfigSerializer, "{3f7f9be6-d964-4a55-b856-c03cc5754df0}", AZ::BaseJsonSerializer);
+        AZ_CLASS_ALLOCATOR_DECL;
+
+        AZ::JsonSerializationResult::Result Load(
+            void* outputValue,
+            const AZ::Uuid& outputValueTypeId,
+            const rapidjson::Value& inputValue,
+            AZ::JsonDeserializerContext& context) override;
+    };
+
+    //! Wheel odometry sensor component.
+    //! It constructs and publishes an odometry message, which contains information about the vehicle's velocity and position in space.
+    //! This is a physical sensor that takes a vehicle's configuration and computes updates from the wheels' rotations.
+    //! @see <a href="https://index.ros.org/p/nav_msgs/">nav_msgs package</a>.
+    class ROS2WheelOdometryComponent
+        : public ROS2SensorComponentBase<ROS2::PhysicsBasedSource>
+        , protected WheelOdometryConfigurationRequestBus::Handler
+    {
+        friend class JsonROS2WheelOdometryComponentConfigSerializer;
+
+    public:
+        AZ_COMPONENT(ROS2WheelOdometryComponent, ROS2Sensors::ROS2WheelOdometryComponentTypeId, SensorBaseType);
+        ROS2WheelOdometryComponent();
+        ~ROS2WheelOdometryComponent() = default;
+        static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required);
+        static void Reflect(AZ::ReflectContext* context);
+        //////////////////////////////////////////////////////////////////////////
+        // Component overrides
+        void Activate() override;
+        void Deactivate() override;
+        //////////////////////////////////////////////////////////////////////////
+
+    private:
+        //////////////////////////////////////////////////////////////////////////
+        // WheelOdometryConfigurationRequestBus::Handler overrides
+        const WheelOdometrySensorConfiguration GetConfiguration() override;
+        void SetConfiguration(const WheelOdometrySensorConfiguration& configuration) override;
+        ROS2OdometryCovariance GetPoseCovariance() override;
+        void SetPoseCovariance(const ROS2OdometryCovariance& covariance) override;
+        ROS2OdometryCovariance GetTwistCovariance() override;
+        void SetTwistCovariance(const ROS2OdometryCovariance& covariance) override;
+
+        //////////////////////////////////////////////////////////////////////////
+
+        std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> m_odometryPublisher;
+        nav_msgs::msg::Odometry m_odometryMsg;
+        AZ::Vector3 m_robotPose = AZ::Vector3::CreateZero();
+        AZ::Quaternion m_robotRotation = AZ::Quaternion::CreateIdentity();
+        WheelOdometrySensorConfiguration m_odometryConfiguration;
+
+        void OnOdometryEvent();
+        void OnPhysicsEvent(float physicsDeltaTime);
+    };
+} // namespace ROS2Sensors

+ 43 - 0
Gems/ROS2Sensors/Code/Source/Odometry/WheelOdometrySensorConfiguration.cpp

@@ -0,0 +1,43 @@
+/*
+ * 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 <AzCore/Serialization/EditContext.h>
+#include <ROS2Sensors/Odometry/ROS2OdometryCovariance.h>
+#include <ROS2Sensors/Odometry/WheelOdometrySensorConfiguration.h>
+
+namespace ROS2Sensors
+{
+    void WheelOdometrySensorConfiguration::Reflect(AZ::ReflectContext* context)
+    {
+        ROS2OdometryCovariance::Reflect(context);
+
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<WheelOdometrySensorConfiguration>()
+                ->Version(1)
+                ->Field("Pose covariance", &WheelOdometrySensorConfiguration::m_poseCovariance)
+                ->Field("Twist covariance", &WheelOdometrySensorConfiguration::m_twistCovariance);
+
+            if (AZ::EditContext* editContext = serialize->GetEditContext())
+            {
+                editContext
+                    ->Class<WheelOdometrySensorConfiguration>("ROS2 Wheel Odometry configuration", "Wheel odometry sensor configuration")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &WheelOdometrySensorConfiguration::m_poseCovariance,
+                        "Pose covariance",
+                        "Set ROS pose covariance")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &WheelOdometrySensorConfiguration::m_twistCovariance,
+                        "Twist covariance",
+                        "Set ROS twist covariance");
+            }
+        }
+    }
+} // namespace ROS2Sensors

+ 1 - 5
Gems/ROS2Sensors/Code/Source/ROS2SensorsModuleInterface.cpp

@@ -27,7 +27,7 @@
 #include <Lidar/ROS2Lidar2DSensorComponent.h>
 #include <Lidar/ROS2LidarSensorComponent.h>
 #include <Odometry/ROS2OdometrySensorComponent.h>
-#include <Odometry/ROS2WheelOdometry.h>
+#include <Odometry/ROS2WheelOdometrySensorComponent.h>
 
 namespace ROS2Sensors
 {
@@ -37,10 +37,6 @@ namespace ROS2Sensors
 
     ROS2SensorsModuleInterface::ROS2SensorsModuleInterface()
     {
-        // Push results of [MyComponent]::CreateDescriptor() into m_descriptors here.
-        // Add ALL components descriptors associated with this gem to m_descriptors.
-        // This will associate the AzTypeInfo information for the components with the the SerializeContext, BehaviorContext and EditContext.
-        // This happens through the [MyComponent]::Reflect() function.
         m_descriptors.insert(
             m_descriptors.end(),
             {

+ 0 - 75
Gems/ROS2Sensors/Code/Source/Sensor/SensorHelpers.cpp

@@ -1,75 +0,0 @@
-/*
- * 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 <ROS2/Sensor/Events/PhysicsBasedSource.h>
-#include <ROS2/Sensor/Events/TickBasedSource.h>
-#include <ROS2Sensors/Sensor/ROS2SensorComponentBase.h>
-#include <ROS2Sensors/Sensor/SensorHelper.h>
-
-namespace ROS2Sensors
-{
-
-    AZStd::vector<AZ::EntityComponentIdPair> GetSensorsForEntity(const AZ::EntityId& entityId)
-    {
-        AZStd::vector<AZ::EntityComponentIdPair> sensors;
-        if (AZ::Entity* entity = AZ::Interface<AZ::ComponentApplicationRequests>::Get()->FindEntity(entityId))
-        {
-            auto components = entity->GetComponents();
-            for (const auto* component : components)
-            {
-                AZ_Assert(component, "Component not found");
-                if (IsComponentROS2Sensor(component))
-                {
-                    sensors.push_back(AZ::EntityComponentIdPair(entityId, component->GetId()));
-                }
-            }
-        }
-        else
-        {
-            AZ_Warning("SensorHelpers", false, "Entity not found");
-        }
-        return sensors;
-    }
-
-    AZStd::vector<AZ::EntityComponentIdPair> GetSensorsOfType(const AZ::EntityId& entityId, const AZ::Uuid& sensorType)
-    {
-        AZStd::vector<AZ::EntityComponentIdPair> sensors;
-        if (AZ::Entity* entity = AZ::Interface<AZ::ComponentApplicationRequests>::Get()->FindEntity(entityId))
-        {
-            auto components = entity->GetComponents();
-            for (const auto* component : components)
-            {
-                AZ_Assert(component, "Component not found");
-                if (component->RTTI_IsTypeOf(sensorType))
-                {
-                    sensors.push_back(AZ::EntityComponentIdPair(entityId, component->GetId()));
-                }
-            }
-        }
-        else
-        {
-            AZ_Warning("SensorHelpers", false, "Entity not found");
-        }
-        return sensors;
-    }
-
-    bool IsComponentROS2Sensor(const AZ::Component* component)
-    {
-        // In ROS2Sensors gem we have at this moment two types of base classes for sensors, we need to check if the component is derived
-        // from one of them. If we add more base classes for sensors in the future, we need to update this function.
-        if (azrtti_cast<const ROS2SensorComponentBase<ROS2::TickBasedSource>*>(component))
-        {
-            return true;
-        }
-        if (azrtti_cast<const ROS2SensorComponentBase<ROS2::PhysicsBasedSource>*>(component))
-        {
-            return true;
-        }
-        return false;
-    }
-}; // namespace ROS2Sensors

+ 138 - 0
Gems/ROS2Sensors/Code/Tests/Odometry/WheelOdometrySensorConfigurationSerializationTest.cpp

@@ -0,0 +1,138 @@
+/*
+ * 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 <AzCore/UnitTest/TestTypes.h>
+#include <AzTest/AzTest.h>
+#include <Odometry/ROS2WheelOdometrySensorComponent.h>
+#include <rapidjson/stringbuffer.h>
+#include <rapidjson/writer.h>
+
+namespace UnitTest
+{
+    class ROS2SensorsTestFixture : public ::testing::Test
+    {
+    };
+
+    // Store test for the old schema, where there is no configuration class.
+    TEST_F(ROS2SensorsTestFixture, Deserialize_WhenOldSchemaProvided_StoresSuccessfully)
+    {
+        ROS2Sensors::ROS2WheelOdometryComponent wheelOdometryComponent;
+
+        rapidjson::Document jsonDocument(rapidjson::kObjectType);
+        jsonDocument.Parse(R"({
+            "Twist covariance": {
+                "Linear covariance": [
+                    123.0,
+                    456.0,
+                    789.0
+                ],
+                "Angular covariance": [
+                    987.0,
+                    654.0,
+                    321.0
+                ]
+            },
+            "Pose covariance": {
+                "Linear covariance": [
+                    321.0,
+                    654.0,
+                    987.0
+                ],
+                "Angular covariance": [
+                    789.0,
+                    456.0,
+                    123.0
+                ]
+            }
+        })");
+
+        AZ::JsonDeserializerSettings settings;
+        auto storeResult = AZ::JsonSerialization::Load(wheelOdometryComponent, jsonDocument, settings);
+
+        EXPECT_EQ(storeResult.GetOutcome(), AZ::JsonSerializationResult::Outcomes::PartialDefaults);
+
+        rapidjson::Document document(rapidjson::kObjectType);
+        document.Parse("{}");
+
+        AZ::JsonSerializerSettings serializerSettings;
+        serializerSettings.m_keepDefaults = true;
+
+        AZ::JsonSerializationResult::ResultCode resultCode =
+            AZ::JsonSerialization::Store(document, document.GetAllocator(), wheelOdometryComponent, serializerSettings);
+
+        EXPECT_EQ(resultCode.GetOutcome(), AZ::JsonSerializationResult::Outcomes::Success);
+
+        rapidjson::StringBuffer buffer;
+        rapidjson::Writer<rapidjson::StringBuffer> writer(buffer);
+        document.Accept(writer);
+
+        const char* expectedOutput =
+            R"~({"Id":0,"SensorConfiguration":{"Visualize":true,"Publishing Enabled":true,"Frequency (HZ)":10.0,"Publishers":{"nav_msgs::msg::Odometry":{"Type":"nav_msgs::msg::Odometry","Topic":"odom","QoS":{"Reliability":2,"Durability":2,"Depth":5}}}},"Odometry configuration":{"Pose covariance":{"Linear covariance":[321.0,654.0,987.0],"Angular covariance":[789.0,456.0,123.0]},"Twist covariance":{"Linear covariance":[123.0,456.0,789.0],"Angular covariance":[987.0,654.0,321.0]}}})~";
+        EXPECT_STREQ(buffer.GetString(), expectedOutput);
+    }
+
+    // Store test for the new schema, where there is a configuration class.
+    TEST_F(ROS2SensorsTestFixture, Deserialize_WhenNewSchemaProvided_StoresSuccessfully)
+    {
+        ROS2Sensors::ROS2WheelOdometryComponent wheelOdometryComponent;
+
+        rapidjson::Document jsonDocument(rapidjson::kObjectType);
+        jsonDocument.Parse(R"({
+            "Odometry configuration": {
+            "Twist covariance": {
+                "Linear covariance": [
+                123.0,
+                456.0,
+                789.0
+                ],
+                "Angular covariance": [
+                987.0,
+                654.0,
+                321.0
+                ]
+            },
+            "Pose covariance": {
+                "Linear covariance": [
+                321.0,
+                654.0,
+                987.0
+                ],
+                "Angular covariance": [
+                789.0,
+                456.0,
+                123.0
+                ]
+            }
+            }
+        })");
+
+        AZ::JsonDeserializerSettings settings;
+        auto storeResult = AZ::JsonSerialization::Load(wheelOdometryComponent, jsonDocument, settings);
+
+        EXPECT_EQ(storeResult.GetOutcome(), AZ::JsonSerializationResult::Outcomes::PartialDefaults);
+
+        rapidjson::Document document(rapidjson::kObjectType);
+        document.Parse("{}");
+
+        AZ::JsonSerializerSettings serializerSettings;
+        serializerSettings.m_keepDefaults = true;
+
+        AZ::JsonSerializationResult::ResultCode resultCode =
+            AZ::JsonSerialization::Store(document, document.GetAllocator(), wheelOdometryComponent, serializerSettings);
+
+        EXPECT_EQ(resultCode.GetOutcome(), AZ::JsonSerializationResult::Outcomes::Success);
+
+        rapidjson::StringBuffer buffer;
+        rapidjson::Writer<rapidjson::StringBuffer> writer(buffer);
+        document.Accept(writer);
+
+        const char* expectedOutput =
+            R"~({"Id":0,"SensorConfiguration":{"Visualize":true,"Publishing Enabled":true,"Frequency (HZ)":10.0,"Publishers":{"nav_msgs::msg::Odometry":{"Type":"nav_msgs::msg::Odometry","Topic":"odom","QoS":{"Reliability":2,"Durability":2,"Depth":5}}}},"Odometry configuration":{"Pose covariance":{"Linear covariance":[321.0,654.0,987.0],"Angular covariance":[789.0,456.0,123.0]},"Twist covariance":{"Linear covariance":[123.0,456.0,789.0],"Angular covariance":[987.0,654.0,321.0]}}})~";
+        EXPECT_STREQ(buffer.GetString(), expectedOutput);
+    }
+} // namespace UnitTest

+ 45 - 1
Gems/ROS2Sensors/Code/Tests/Tools/ROS2SensorsEditorTest.cpp

@@ -6,6 +6,50 @@
  *
  */
 
+#include <AzCore/UserSettings/UserSettingsComponent.h>
 #include <AzTest/AzTest.h>
+#include <AzTest/GemTestEnvironment.h>
+#include <AzToolsFramework/UnitTest/ToolsTestApplication.h>
 
-AZ_UNIT_TEST_HOOK(DEFAULT_UNIT_TEST_ENV);
+namespace UnitTest
+{
+    class ROS2SensorsTestEnvironment : public AZ::Test::GemTestEnvironment
+    {
+        // AZ::Test::GemTestEnvironment overrides ...
+        void AddGemsAndComponents() override;
+        AZ::ComponentApplication* CreateApplicationInstance() override;
+        void PostSystemEntityActivate() override;
+
+    public:
+        ROS2SensorsTestEnvironment() = default;
+        ~ROS2SensorsTestEnvironment() override = default;
+    };
+
+    void ROS2SensorsTestEnvironment::AddGemsAndComponents()
+    {
+        AddActiveGems(AZStd::to_array<AZStd::string_view>({ "ROS2", "ROS2Sensors" }));
+        AddDynamicModulePaths({ "ROS2", "ROS2Sensors" });
+    }
+
+    AZ::ComponentApplication* ROS2SensorsTestEnvironment::CreateApplicationInstance()
+    {
+        // Using ToolsTestApplication to have AzFramework and AzToolsFramework components.
+        return aznew UnitTest::ToolsTestApplication("ROS2SensorsTestEnvironment");
+    }
+
+    void ROS2SensorsTestEnvironment::PostSystemEntityActivate()
+    {
+        AZ::UserSettingsComponentRequestBus::Broadcast(&AZ::UserSettingsComponentRequests::DisableSaveOnFinalize);
+    }
+} // namespace UnitTest
+
+AZTEST_EXPORT int AZ_UNIT_TEST_HOOK_NAME(int argc, char** argv)
+{
+    ::testing::InitGoogleMock(&argc, argv);
+    AZ::Test::printUnusedParametersWarning(argc, argv);
+    AZ::Test::addTestEnvironments({ new UnitTest::ROS2SensorsTestEnvironment() });
+    int result = RUN_ALL_TESTS();
+    return result;
+}
+
+IMPLEMENT_TEST_EXECUTABLE_MAIN();

+ 11 - 3
Gems/ROS2Sensors/Code/ros2sensors_api_files.cmake

@@ -11,12 +11,20 @@ set(FILES
     Include/ROS2Sensors/Sensor/SensorConfiguration.h
     Include/ROS2Sensors/Sensor/SensorConfigurationRequestBus.h
     Include/ROS2Sensors/Sensor/ROS2SensorComponentBase.h
-    Include/ROS2Sensors/Sensor/SensorHelper.h
-    Include/ROS2Sensors/Camera/CameraCalibrationRequestBus.h
     Include/ROS2Sensors/Camera/CameraPostProcessingRequestBus.h
+    Include/ROS2Sensors/Camera/CameraSensorConfiguration.h
     Include/ROS2Sensors/GNSS/GNSSPostProcessingRequestBus.h
+    Include/ROS2Sensors/Imu/ImuConfigurationRequestBus.h
+    Include/ROS2Sensors/Imu/ImuSensorConfiguration.h
     Include/ROS2Sensors/Lidar/ClassSegmentationBus.h
+    Include/ROS2Sensors/Lidar/LidarConfigurationRequestBus.h
+    Include/ROS2Sensors/Lidar/LidarSensorConfiguration.h
     Include/ROS2Sensors/Lidar/LidarRaycasterBus.h
-    Include/ROS2Sensors/Lidar/LidarSystemBus.h
     Include/ROS2Sensors/Lidar/LidarRegistrarBus.h
+    Include/ROS2Sensors/Lidar/LidarSystemBus.h
+    Include/ROS2Sensors/Lidar/LidarTemplate.h
+    Include/ROS2Sensors/Lidar/LidarTemplateUtils.h
+    Include/ROS2Sensors/Odometry/ROS2OdometryCovariance.h
+    Include/ROS2Sensors/Odometry/WheelOdometrySensorConfiguration.h
+    Include/ROS2Sensors/Odometry/WheelOdometryConfigurationRequestBus.h
 )

+ 1 - 0
Gems/ROS2Sensors/Code/ros2sensors_editor_tests_files.cmake

@@ -8,4 +8,5 @@
 
 set(FILES
     Tests/Tools/ROS2SensorsEditorTest.cpp
+    Tests/Odometry/WheelOdometrySensorConfigurationSerializationTest.cpp
 )

+ 3 - 9
Gems/ROS2Sensors/Code/ros2sensors_private_files.cmake

@@ -12,7 +12,6 @@ set(FILES
     Source/Clients/ROS2SensorsSystemComponent.cpp
     Source/Clients/ROS2SensorsSystemComponent.h
     Source/Sensor/SensorConfiguration.cpp
-    Source/Sensor/SensorHelpers.cpp
     Source/Camera/PostProcessing/ROS2ImageEncodingConversionComponent.cpp
     Source/Camera/PostProcessing/ROS2ImageEncodingConversionComponent.h
     Source/Camera/CameraConstants.h
@@ -23,7 +22,6 @@ set(FILES
     Source/Camera/CameraSensorDescription.cpp
     Source/Camera/CameraSensorDescription.h
     Source/Camera/CameraSensorConfiguration.cpp
-    Source/Camera/CameraSensorConfiguration.h
     Source/Camera/CameraUtilities.cpp
     Source/Camera/CameraUtilities.h
     Source/Camera/ROS2CameraSensorComponent.cpp
@@ -35,7 +33,6 @@ set(FILES
     Source/GNSS/ROS2GNSSSensorComponent.cpp
     Source/GNSS/ROS2GNSSSensorComponent.h
     Source/Imu/ImuSensorConfiguration.cpp
-    Source/Imu/ImuSensorConfiguration.h
     Source/Imu/ROS2ImuSensorComponent.cpp
     Source/Imu/ROS2ImuSensorComponent.h
     Source/Lidar/ClassSegmentationConfigurationComponent.cpp
@@ -47,13 +44,10 @@ set(FILES
     Source/Lidar/LidarRegistrarSystemComponent.cpp
     Source/Lidar/LidarRegistrarSystemComponent.h
     Source/Lidar/LidarSensorConfiguration.cpp
-    Source/Lidar/LidarSensorConfiguration.h
     Source/Lidar/LidarSystem.cpp
     Source/Lidar/LidarSystem.h
     Source/Lidar/LidarTemplate.cpp
-    Source/Lidar/LidarTemplate.h
     Source/Lidar/LidarTemplateUtils.cpp
-    Source/Lidar/LidarTemplateUtils.h
     Source/Lidar/PointCloudMessageBuilder.cpp
     Source/Lidar/PointCloudMessageBuilder.h
     Source/Lidar/ROS2Lidar2DSensorComponent.cpp
@@ -62,8 +56,8 @@ set(FILES
     Source/Lidar/ROS2LidarSensorComponent.h
     Source/Odometry/ROS2OdometrySensorComponent.cpp
     Source/Odometry/ROS2OdometrySensorComponent.h
-    Source/Odometry/ROS2WheelOdometry.cpp
-    Source/Odometry/ROS2WheelOdometry.h
+    Source/Odometry/ROS2WheelOdometrySensorComponent.cpp
+    Source/Odometry/ROS2WheelOdometrySensorComponent.h
+    Source/Odometry/WheelOdometrySensorConfiguration.cpp
     Source/Odometry/ROS2OdometryCovariance.cpp
-    Source/Odometry/ROS2OdometryCovariance.h
 )