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

@@ -10,12 +10,16 @@
 
 #include <AzCore/Component/Component.h>
 #include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/Json/JsonSerializationResult.h>
 #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>
+#include <rapidjson/writer.h>
+#include <std_msgs/msg/string.hpp>
 
 namespace ROS2
 {
@@ -30,29 +34,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 +115,82 @@ namespace ROS2
         {
             AZ::EntityComponentIdPair entityComponentIdPair(GetEntityId(), GetId());
             SensorConfigurationRequestBus::Handler::BusConnect(entityComponentIdPair);
+            ConfigurationBus<ComponentConfigurationT>::Handler::BusConnect(GetEntityId());
+
+            if (m_sensorConfiguration.m_configurableFromROS2)
+            {
+                AZStd::string ns = GetNamespace();
+                AZStd::string configurationName = ComponentConfigurationT::TYPEINFO_Name();
+                AZStd::string getTopic = ns + "/" + "Get" + configurationName;
+                AZStd::string setTopic = ns + "/" + "Set" + configurationName;
+                m_configurationPublisher = ROS2Interface::Get()->GetNode()->create_publisher<std_msgs::msg::String>(
+                    getTopic.data(), rclcpp::QoS(1).transient_local());
+                m_configurationSubscriber = ROS2Interface::Get()->GetNode()->create_subscription<std_msgs::msg::String>(
+                    setTopic.data(),
+                    rclcpp::QoS(1).reliable(),
+                    [this](const std_msgs::msg::String::SharedPtr msg)
+                    {
+                        ProcessConfigurationMsg(AZStd::string(msg->data.c_str()));
+                    });
+
+                PublishConfiguration(GetConfiguration());
+            }
         }
 
         void Deactivate() override
         {
+            m_configurationPublisher.reset();
+            m_configurationSubscriber.reset();
+
+            ConfigurationBus<ComponentConfigurationT>::Handler::BusDisconnect();
             SensorConfigurationRequestBus::Handler::BusDisconnect();
         }
 
+        AZ::TypeId GetUnderlyingComponentType() const override
+        {
+            return AZ::TypeId(ROS2Sensors::ROS2SensorComponentBaseTypeId);
+        }
+
+        const ComponentConfigurationT GetConfiguration() const override
+        {
+            return GetComponentConfiguration();
+        }
+
+        void ProcessConfigurationMsg(AZStd::string msg)
+        {
+            rapidjson::Document document;
+            document.Parse(msg.c_str());
+            if (document.HasParseError())
+            {
+                AZ_Error("ROS2Sensors", false, "Failed to parse configuration message");
+                return;
+            }
+
+            AZ::JsonDeserializerSettings deserializerSettings;
+
+            ComponentConfigurationT configuration = GetConfiguration();
+            auto result = AZ::JsonSerialization::Load(configuration, document, deserializerSettings);
+            auto processing = result.GetProcessing();
+            if (processing == AZ::JsonSerializationResult::Processing::Completed)
+            {
+                SetConfiguration(configuration);
+            }
+            else
+            {
+                AZ_Error("ROS2Sensors", false, "Failed to deserialize configuration message: %s", result.ToString("").c_str());
+            }
+        }
+
+        void SetConfiguration(const ComponentConfigurationT configuration) override
+        {
+            SetComponentConfiguration(configuration);
+
+            PublishConfiguration(configuration);
+        }
+
+        virtual const ComponentConfigurationT GetComponentConfiguration() const = 0;
+        virtual void SetComponentConfiguration(const ComponentConfigurationT configuration) = 0;
+
     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 +218,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();
@@ -173,8 +249,29 @@ namespace ROS2
 
         //! Handler for adapted event. Requires manual assignment and connecting to adapted event in derived class.
         typename EventSourceT::AdaptedEventHandlerType m_adaptedEventHandler;
+
+        rclcpp::Publisher<std_msgs::msg::String>::SharedPtr m_configurationPublisher; ///< Publisher for the sensor configuration.
+        rclcpp::Subscription<std_msgs::msg::String>::SharedPtr m_configurationSubscriber; ///< Subscriber for the sensor configuration.
+
+    private:
+        void PublishConfiguration(const ComponentConfigurationT& configuration)
+        {
+            if (m_configurationPublisher != nullptr)
+            {
+                rapidjson::Document document(rapidjson::kObjectType);
+                AZ::JsonSerializerSettings serializerSettings;
+                serializerSettings.m_keepDefaults = true;
+                AZ::JsonSerialization::Store(document, document.GetAllocator(), configuration, serializerSettings);
+                rapidjson::StringBuffer buffer;
+                rapidjson::Writer<rapidjson::StringBuffer> writer(buffer);
+                document.Accept(writer);
+                std_msgs::msg::String msg;
+                msg.data = buffer.GetString();
+                m_configurationPublisher->publish(msg);
+            }
+        }
     };
 
     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/Include/ROS2Sensors/Sensor/SensorConfiguration.h

@@ -35,6 +35,9 @@ namespace ROS2
 
         bool m_publishingEnabled = true; //!< Determines whether the sensor is publishing (sending data to ROS 2 ecosystem).
         bool m_visualize = true; //!< Determines whether the sensor is visualized in O3DE (for example, point cloud is drawn for LIDAR).
+
+        bool m_configurableFromROS2 =
+            false; //!< Determines whether the sensor can be configured from ROS 2 (for example, camera resolution).
     private:
         // Frequency limit is once per day.
         static constexpr float m_minFrequency = AZStd::numeric_limits<float>::epsilon();

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>