Exposing publishing switch api and adding post processing api for GNSS (#639)

* Added SensorConfiguration
* Added GNSS postprocessing API
* Expose sensor publishing state

---------

Signed-off-by: Adam Krawczyk <[email protected]>
Signed-off-by: Khasreto <[email protected]>
Co-authored-by: Adam Dąbrowski <[email protected]>

Gems/ROS2/Code/Include/ROS2/GNSS/GNSSPostProcessingRequestBus.h

@@ -0,0 +1,63 @@
+/*
+ * 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/EBusSharedDispatchTraits.h>
+#include <AzCore/Interface/Interface.h>
+#include <AzCore/std/string/string.h>
+#include <sensor_msgs/msg/nav_sat_fix.hpp>
+
+namespace ROS2
+{
+    //! Interface class that allows to add post-processing to the pipeline
+    //!
+    //! Each function call can be processed without blocking Bus for other dispatches.
+    //! Do not use connects / disconnects to this bus during event dispatch, as they are not allowed for this concurrency model.
+    //! Those constraints allow for processing multiple GNSS frames at the same time.
+    //! Bus implementations should allow for asynchronous execution of provided functions.
+    class GNSSPostProcessingRequests : public AZ::EBusSharedDispatchTraits<GNSSPostProcessingRequests>
+    {
+    public:
+        using BusIdType = AZ::EntityId;
+        static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById;
+
+        //! Multiple post-processing functions can be registered to the bus.
+        //! They will be executed in the order
+        static constexpr AZ::EBusHandlerPolicy HandlerPolicy = AZ::EBusHandlerPolicy::MultipleAndOrdered;
+
+        //! Priority of the post-processing bus.
+        //! @note higher priority buses will be processed first.
+        static constexpr AZ::u8 MIN_PRIORITY = 0;
+        static constexpr AZ::u8 MAX_PRIORITY = 255;
+        static constexpr AZ::u8 DEFAULT_PRIORITY = 127;
+
+        //! Apply post-processing function to GNSS data.
+        //! @param gnss standard GNSS message passed as a reference. It will be changed through post-processing.
+        virtual void ApplyPostProcessing(sensor_msgs::msg::NavSatFix& gnss) = 0;
+
+        //! Get priority of the post-processing bus.
+        //! @return priority of the bus.
+        //! @note higher priority buses will be processed first.
+        virtual AZ::u8 GetPriority() const = 0;
+
+        //! Compare two post-processing buses.
+        //! @param other bus to compare to.
+        //! @return true if this bus should be processed before the other.
+        inline bool Compare(const GNSSPostProcessingRequests* other) const
+        {
+            return GetPriority() > other->GetPriority();
+        }
+
+    protected:
+        ~GNSSPostProcessingRequests() = default;
+    };
+
+    using GNSSPostProcessingRequestBus = AZ::EBus<GNSSPostProcessingRequests>;
+} // namespace ROS2

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

@@ -14,6 +14,7 @@
 #include <ROS2/ROS2GemUtilities.h>
 #include <ROS2/Sensor/Events/EventSourceAdapter.h>
 #include <ROS2/Sensor/SensorConfiguration.h>
+#include <ROS2/Sensor/SensorConfigurationRequestBus.h>
 
 namespace ROS2
 {
@@ -29,7 +30,9 @@ namespace ROS2
     //! @see ROS2::TickBasedSource
     //! @see ROS2::PhysicsBasedSource
     template<class EventSourceT>
-    class ROS2SensorComponentBase : public AZ::Component
+    class ROS2SensorComponentBase
+        : public AZ::Component
+        , public SensorConfigurationRequestBus::Handler
     {
     public:
         using SensorBaseType = ROS2SensorComponentBase<EventSourceT>;
@@ -40,9 +43,8 @@ namespace ROS2
         {
             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>, AZ::Component>()->Version(1)->Field(
+                    "SensorConfiguration", &ROS2SensorComponentBase<EventSourceT>::m_sensorConfiguration);
 
                 if (auto* editContext = serializeContext->GetEditContext())
                 {
@@ -61,6 +63,16 @@ namespace ROS2
             required.push_back(AZ_CRC_CE("ROS2Frame"));
         }
 
+        SensorConfiguration GetSensorConfiguration() const override
+        {
+            return m_sensorConfiguration;
+        }
+
+        void EnablePublishing(bool publishingEnabled) override
+        {
+            m_sensorConfiguration.m_publishingEnabled = publishingEnabled;
+        }
+
         virtual ~ROS2SensorComponentBase() = default;
 
         void Activate() override

Gems/ROS2/Code/Include/ROS2/Sensor/SensorConfigurationRequestBus.h

@@ -0,0 +1,29 @@
+/*
+ * 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 <ROS2/Sensor/SensorConfiguration.h>
+
+namespace ROS2
+{
+    //! Interface that allows to get sensor configuration and switch publish state.
+    class SensorConfigurationRequest : public AZ::EBusTraits
+    {
+    public:
+        using BusIdType = AZ::EntityId;
+        static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById;
+
+        virtual SensorConfiguration GetSensorConfiguration() const = 0;
+        virtual void EnablePublishing(bool publishingEnabled) = 0;
+    };
+
+    using SensorConfigurationRequestBus = AZ::EBus<SensorConfigurationRequest>;
+} // namespace ROS2

Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp

@@ -13,6 +13,7 @@
 #include <ROS2/Utilities/ROS2Names.h>
 
 #include "Georeference/GNSSFormatConversions.h"
+#include <ROS2/GNSS/GNSSPostProcessingRequestBus.h>
 #include <ROS2/Georeference/GeoreferenceBus.h>
 
 namespace ROS2
@@ -78,6 +79,11 @@ namespace ROS2
             });
     }
 
+    void ROS2GNSSSensorComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided)
+    {
+        provided.push_back(AZ_CRC_CE("ROS2GNSSSensor"));
+    }
+
     void ROS2GNSSSensorComponent::Deactivate()
     {
         StopSensor();
@@ -86,7 +92,6 @@ namespace ROS2
 
     void ROS2GNSSSensorComponent::FrequencyTick()
     {
-
         AZ::Vector3 currentPosition{ 0.0f };
         AZ::TransformBus::EventResult(currentPosition, GetEntityId(), &AZ::TransformBus::Events::GetWorldTranslation);
 
@@ -100,9 +105,9 @@ namespace ROS2
 
         m_gnssMsg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_SBAS_FIX;
         m_gnssMsg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS;
+        GNSSPostProcessingRequestBus::Event(GetEntityId(), &GNSSPostProcessingRequests::ApplyPostProcessing, m_gnssMsg);
 
         m_gnssPublisher->publish(m_gnssMsg);
     }
 
-
 } // namespace ROS2

Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h

@@ -28,6 +28,8 @@ namespace ROS2
         ROS2GNSSSensorComponent(const SensorConfiguration& sensorConfiguration);
         ~ROS2GNSSSensorComponent() = default;
         static void Reflect(AZ::ReflectContext* context);
+
+        static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided);
         //////////////////////////////////////////////////////////////////////////
         // Component overrides
         void Activate() override;

Gems/ROS2/Code/ros2_header_files.cmake

@@ -17,6 +17,7 @@ set(FILES
         Include/ROS2/Georeference/GeoreferenceBus.h
         Include/ROS2/Georeference/GeoreferenceStructures.h
         Include/ROS2/Georeference/GeoreferenceStructures.h
+        Include/ROS2/GNSS/GNSSPostProcessingRequestBus.h
         Include/ROS2/Gripper/GripperRequestBus.h
         Include/ROS2/Manipulation/Controllers/JointsPositionControllerRequests.h
         Include/ROS2/Manipulation/JointInfo.h
@@ -42,6 +43,7 @@ set(FILES
         Include/ROS2/Sensor/Events/TickBasedSource.h
         Include/ROS2/Sensor/ROS2SensorComponentBase.h
         Include/ROS2/Sensor/SensorConfiguration.h
+        Include/ROS2/Sensor/SensorConfigurationRequestBus.h
         Include/ROS2/Spawner/SpawnerBus.h
         Include/ROS2/Utilities/Controllers/PidConfiguration.h
         Include/ROS2/Utilities/ROS2Conversions.h