[RFC 96] EBus and ROS2 support for sensor configurations

Gems/ROS2/Code/Source/SystemComponents/ROS2SystemComponent.cpp

@@ -76,7 +76,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)

Gems/ROS2Sensors/Code/CMakeLists.txt

@@ -265,6 +265,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
             )
 

Gems/ROS2Sensors/Code/Include/ROS2Sensors/Configuration/ConfigurationBus.h

@@ -0,0 +1,35 @@
+/*
+ * 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/ComponentBus.h>
+#include <AzCore/Component/EntityId.h>
+
+namespace ROS2 {
+    template<typename Configuration>
+    class ConfigurationRequests : public AZ::ComponentBus
+    {
+    public:
+        using BusIdType = AZ::EntityId;
+        static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById;
+        static constexpr AZ::EBusHandlerPolicy HandlerPolicy = AZ::EBusHandlerPolicy::Multiple;
+
+        //! Returns the current configuration of the component.
+        virtual const Configuration GetConfiguration() const = 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 Configuration configuration) = 0;
+    };
+
+    template<typename Configuration>
+    using ConfigurationBus = AZ::EBus<ConfigurationRequests<Configuration>>;
+}

Gems/ROS2Sensors/Code/Source/Lidar/LidarSensorConfiguration.h → Gems/ROS2Sensors/Code/Include/ROS2Sensors/Lidar/LidarSensorConfiguration.h

@@ -11,9 +11,8 @@
 #include <AzCore/RTTI/RTTI.h>
 #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/LidarTemplate.h>
+#include <ROS2Sensors/Lidar/LidarTemplateUtils.h>
 #include <ROS2Sensors/Lidar/LidarRegistrarBus.h>
 
 namespace ROS2
@@ -59,7 +58,7 @@ namespace ROS2
         //! Get all available lidar systems.
         AZStd::vector<AZStd::string> FetchLidarSystemList();
 
-        const AZStd::vector<LidarTemplate::LidarModel> m_availableModels;
+        AZStd::vector<LidarTemplate::LidarModel> m_availableModels;
         AZStd::string m_lidarModelName = "CustomLidar2D";
 
         void UpdateShowNoise();

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 ROS2
 {

Gems/ROS2Sensors/Code/Include/ROS2Sensors/Odometry/ROS2WheelOdometryConfiguration.h

@@ -0,0 +1,30 @@
+/*
+ * 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>
+
+namespace ROS2 {
+
+    //! A structure capturing configuration of a wheel odometry sensor.
+    struct ROS2WheelOdometryConfiguration
+    {
+        AZ_RTTI(ROS2WheelOdometryConfiguration, "{9dc58d89-e674-4d7f-9ea9-afe3ae7fd2eb}");
+
+        ROS2WheelOdometryConfiguration() = default;
+        virtual ~ROS2WheelOdometryConfiguration() = default;
+
+        static void Reflect(AZ::ReflectContext* context);
+
+        ROS2OdometryCovariance m_poseCovariance;
+        ROS2OdometryCovariance m_twistCovariance;
+    };
+}

Gems/ROS2Sensors/Code/Include/ROS2Sensors/ROS2SensorsTypeIds.h

@@ -21,6 +21,9 @@ namespace ROS2Sensors
     // so they use the Same TypeId
     inline constexpr const char* ROS2SensorsEditorModuleTypeId = ROS2SensorsModuleTypeId;
 
+    // Sensor Components Base TypeIds
+    inline constexpr const char* ROS2SensorComponentBaseTypeId = "{A0C2D1B4-8E5F-4E7C-8A3D-6F9B5F1A0E7D}";
+
     // System Sensor Components TypeIds
     inline constexpr const char* ROS2SystemCameraComponentTypeId = "{B4665D39-78FD-40DE-8518-2F6BD345A831}";
     inline constexpr const char* ROS2EditorCameraSystemComponentTypeId = "{407F51C0-92C9-11EE-B9D1-0242AC120002}";

Gems/ROS2Sensors/Code/Include/ROS2Sensors/Sensor/ROS2SensorComponentBase.h

@@ -13,6 +13,7 @@
 #include <ROS2/Frame/ROS2FrameComponent.h>
 #include <ROS2/ROS2GemUtilities.h>
 #include <ROS2/Sensor/Events/EventSourceAdapter.h>
+#include <ROS2Sensors/Configuration/ConfigurationBus.h>
 #include <ROS2Sensors/ROS2SensorsTypeIds.h>
 #include <ROS2Sensors/Sensor/SensorConfiguration.h>
 #include <ROS2Sensors/Sensor/SensorConfigurationRequestBus.h>
@@ -30,29 +31,32 @@ namespace ROS2
     //! chosen event source implementation).
     //! @see ROS2::TickBasedSource
     //! @see ROS2::PhysicsBasedSource
-    template<class EventSourceT>
+    template<class EventSourceT, class ComponentConfigurationT>
     class ROS2SensorComponentBase
         : public AZ::Component
         , public SensorConfigurationRequestBus::Handler
+        , public ConfigurationBus<ComponentConfigurationT>::Handler
     {
     public:
-        using SensorBaseType = ROS2SensorComponentBase<EventSourceT>;
+        using SensorBaseType = ROS2SensorComponentBase<EventSourceT, ComponentConfigurationT>;
 
-        AZ_COMPONENT_DECL((ROS2SensorComponentBase, AZ_CLASS));
+        AZ_COMPONENT_DECL((ROS2SensorComponentBase, AZ_CLASS, AZ_CLASS));
 
         static void Reflect(AZ::ReflectContext* context)
         {
             if (auto* serializeContext = azrtti_cast<AZ::SerializeContext*>(context))
             {
-                serializeContext->Class<ROS2SensorComponentBase<EventSourceT>, AZ::Component>()->Version(1)->Field(
-                    "SensorConfiguration", &ROS2SensorComponentBase<EventSourceT>::m_sensorConfiguration);
+                serializeContext->Class<ROS2SensorComponentBase<EventSourceT, ComponentConfigurationT>, AZ::Component>()->Version(1)->Field(
+                    "SensorConfiguration", &ROS2SensorComponentBase<EventSourceT, ComponentConfigurationT>::m_sensorConfiguration);
 
                 if (auto* editContext = serializeContext->GetEditContext())
                 {
-                    editContext->Class<ROS2SensorComponentBase<EventSourceT>>("ROS2 Sensor Component Base", "Base component for sensors")
+                    editContext
+                        ->Class<ROS2SensorComponentBase<EventSourceT, ComponentConfigurationT>>(
+                            "ROS2 Sensor Component Base", "Base component for sensors")
                         ->DataElement(
                             AZ::Edit::UIHandlers::Default,
-                            &ROS2SensorComponentBase<EventSourceT>::m_sensorConfiguration,
+                            &ROS2SensorComponentBase<EventSourceT, ComponentConfigurationT>::m_sensorConfiguration,
                             "Sensor configuration",
                             "Sensor configuration");
                 }
@@ -108,13 +112,20 @@ namespace ROS2
         {
             AZ::EntityComponentIdPair entityComponentIdPair(GetEntityId(), GetId());
             SensorConfigurationRequestBus::Handler::BusConnect(entityComponentIdPair);
+            ConfigurationBus<ComponentConfigurationT>::Handler::BusConnect(GetEntityId());
         }
 
         void Deactivate() override
         {
+            ConfigurationBus<ComponentConfigurationT>::Handler::BusDisconnect();
             SensorConfigurationRequestBus::Handler::BusDisconnect();
         }
 
+        AZ::TypeId GetUnderlyingComponentType() const override
+        {
+            return AZ::TypeId(ROS2Sensors::ROS2SensorComponentBaseTypeId);
+        }
+
     protected:
         //! Starts sensor with passed frequency and adapted event callback. Optionally, user can pass source event callback, that will be
         //! called with event source frequency.
@@ -142,7 +153,7 @@ namespace ROS2
             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();
@@ -176,5 +187,5 @@ namespace ROS2
     };
 
     AZ_COMPONENT_IMPL_INLINE(
-        (ROS2SensorComponentBase, AZ_CLASS), "ROS2SensorComponentBase", "{2DF9A652-DF5D-43B1-932F-B6A838E36E97}", AZ::Component)
+        (ROS2SensorComponentBase, AZ_CLASS, AZ_CLASS), "ROS2SensorComponentBase", ROS2Sensors::ROS2SensorComponentBaseTypeId, AZ::Component)
 } // namespace ROS2

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 ROS2
 {

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>

Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSensorComponent.cpp

@@ -22,6 +22,7 @@ namespace ROS2
     void ROS2CameraSensorComponent::Reflect(AZ::ReflectContext* context)
     {
         CameraSensorConfiguration::Reflect(context);
+        ROS2SensorComponentBase<TickBasedSource, CameraSensorConfiguration>::Reflect(context);
 
         auto* serialize = azrtti_cast<AZ::SerializeContext*>(context);
         if (serialize)
@@ -34,18 +35,8 @@ namespace ROS2
     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<ROS2FrameComponent>();
         AZ_Assert(component, "Entity has no ROS2FrameComponent");
@@ -136,4 +127,39 @@ namespace ROS2
         }
         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() const
+    {
+        return m_cameraConfiguration;
+    }
+
 } // namespace ROS2

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/CameraSensorConfiguration.h>
 #include <ROS2Sensors/ROS2SensorsTypeIds.h>
 #include <ROS2Sensors/Sensor/ROS2SensorComponentBase.h>
 
@@ -32,7 +32,7 @@ namespace ROS2
     //!   - camera vertical field of view in degrees
     //! Camera frustum is facing negative Z axis; image plane is parallel to X,Y plane: X - right, Y - up
     class ROS2CameraSensorComponent
-        : public ROS2SensorComponentBase<TickBasedSource>
+        : public ROS2SensorComponentBase<TickBasedSource, CameraSensorConfiguration>
         , public CameraCalibrationRequestBus::Handler
     {
     public:
@@ -73,6 +73,13 @@ namespace ROS2
         ///! Requests message publication from camera sensor.
         void FrequencyTick();
 
+        //! Sets the camera sensor and image source.
+        void SetCameraSensorConfiguration();
+
+        // ConfigurationBus overrides
+        void SetConfiguration(const CameraSensorConfiguration configuration) override;
+        const CameraSensorConfiguration GetConfiguration() const override;
+
         CameraSensorConfiguration m_cameraConfiguration;
         AZStd::string m_frameName;
         AZStd::shared_ptr<CameraSensor> m_cameraSensor;

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/CameraSensorConfiguration.h>
 #include <ROS2Sensors/Sensor/SensorConfiguration.h>
 
 namespace ROS2

Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSystemComponent.cpp

@@ -41,7 +41,11 @@ namespace ROS2
    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()

Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSystemComponent.h

@@ -24,6 +24,7 @@ namespace ROS2
         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();
 

Gems/ROS2Sensors/Code/Source/Camera/ROS2EditorCameraSystemComponent.cpp

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

Gems/ROS2Sensors/Code/Source/Camera/ROS2EditorCameraSystemComponent.h

@@ -29,6 +29,7 @@ namespace ROS2
         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