Use ROS 2 clock for sensor triggering (#807)

* Use ROS 2 clock for sensor triggering, reintroduce median filtering to FPS calculation
* Update Gems/ROS2/Code/Include/ROS2/Sensor/Events/EventSourceAdapter.h

---------

Signed-off-by: Michał Pełka <[email protected]>
Co-authored-by: Jan Hanca <[email protected]>

Gems/ROS2/Code/Include/ROS2/Clock/ROS2Clock.h

@@ -21,7 +21,6 @@ namespace ROS2
     //! the /use_sim_time parameter set to true.
     class ROS2Clock
     {
-        static constexpr size_t FramesNumberForStats = 60;
 
     public:
         ROS2Clock();

Gems/ROS2/Code/Include/ROS2/ROS2Bus.h

@@ -64,6 +64,9 @@ namespace ROS2
         //! Obtains a simulation clock that is used across simulation.
         //! @returns constant reference to currently running clock.
         virtual const ROS2Clock& GetSimulationClock() const = 0;
+
+        //! Returns an expected loop time of simulation. It is an estimation from past frames.
+        virtual float GetExpectedSimulationLoopTime() const = 0;
     };
 
     class ROS2BusTraits : public AZ::EBusTraits

Gems/ROS2/Code/Include/ROS2/Sensor/Events/PhysicsBasedSource.h

@@ -11,6 +11,7 @@
 #include <AzCore/EBus/OrderedEvent.h>
 #include <AzFramework/Physics/Common/PhysicsEvents.h>
 #include <AzFramework/Physics/Common/PhysicsTypes.h>
+#include <ROS2/ROS2Bus.h>
 #include <ROS2/Sensor/Events/SensorEventSource.h>
 
 namespace ROS2
@@ -32,5 +33,6 @@ namespace ROS2
 
     private:
         AzPhysics::SceneEvents::OnSceneSimulationFinishHandler m_onSceneSimulationEventHandler; ///< Handler for physics callback.
+        builtin_interfaces::msg::Time m_lastSimulationTime; ///< Last simulation time.
     };
 } // namespace ROS2

Gems/ROS2/Code/Include/ROS2/Sensor/Events/TickBasedSource.h

@@ -11,14 +11,14 @@
 #include <AzCore/Component/TickBus.h>
 #include <AzCore/EBus/Event.h>
 #include <ROS2/Sensor/Events/SensorEventSource.h>
-
+#include <ROS2/ROS2Bus.h>
 namespace ROS2
 {
     //! Class implementing system TickBus (draw calls) as sensor event source. Source event (ROS2::SensorEventSource) is signalled based on
     //! system ticks.
     //! @see ROS2::SensorEventSource
     class TickBasedSource final
-        : public SensorEventSource<AZ::Event, AZ::EventHandler, float, AZ::ScriptTimePoint>
+        : public SensorEventSource<AZ::Event, AZ::EventHandler, float>
         , protected AZ::TickBus::Handler
     {
     public:
@@ -28,10 +28,14 @@ namespace ROS2
         // Overrides of ROS2::SensorEventSource.
         void Start() override;
         void Stop() override;
-        [[nodiscard]] float GetDeltaTime(float deltaTime, AZ::ScriptTimePoint time) const override;
+        [[nodiscard]] float GetDeltaTime(float deltaTime) const override;
 
     private:
         // Override of AZ::TickBus::Handler.
         void OnTick(float deltaTime, AZ::ScriptTimePoint time) override;
+
+        builtin_interfaces::msg::Time m_lastTickTimestamp;
+        AZStd::deque<float> m_deltaTimes;
+        constexpr static size_t m_maxDeltaTimes = 100;
     };
 } // namespace ROS2

Gems/ROS2/Code/Source/Sensor/Events/PhysicsBasedSource.cpp

@@ -7,6 +7,8 @@
  */
 
 #include <AzFramework/Physics/PhysicsSystem.h>
+
+#include <ROS2/Utilities/ROS2Conversions.h>
 #include <ROS2/Sensor/Events/PhysicsBasedSource.h>
 #include <ROS2/Sensor/SensorConfiguration.h>
 
@@ -23,10 +25,16 @@ namespace ROS2
     void PhysicsBasedSource::Start()
     {
         m_onSceneSimulationEventHandler.Disconnect();
+        const auto* ros2Interface = ROS2Interface::Get();
+        AZ_Assert(ros2Interface, "ROS2 interface is not initialized.");
+
         m_onSceneSimulationEventHandler = AzPhysics::SceneEvents::OnSceneSimulationFinishHandler(
-            [this](AzPhysics::SceneHandle sceneHandle, float deltaTime)
+            [this, ros2Interface](AzPhysics::SceneHandle sceneHandle, float deltaTime)
             {
-                m_sourceEvent.Signal(sceneHandle, deltaTime);
+                const auto simulationTime = ros2Interface->GetROSTimestamp();
+                const float deltaSimulationTime = ROS2Conversions::GetTimeDifference(m_lastSimulationTime, simulationTime);
+                m_sourceEvent.Signal(sceneHandle, deltaSimulationTime);
+                m_lastSimulationTime = simulationTime;
             });
 
         auto* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();

Gems/ROS2/Code/Source/Sensor/Events/TickBasedSource.cpp

@@ -8,7 +8,9 @@
 
 #include <ROS2/Sensor/Events/TickBasedSource.h>
 #include <ROS2/Sensor/SensorConfiguration.h>
-
+#include <ROS2/ROS2Bus.h>
+#include <ROS2/Utilities/ROS2Conversions.h>
+#include <AzCore/std/numeric.h>
 namespace ROS2
 {
     void TickBasedSource::Reflect(AZ::ReflectContext* context)
@@ -29,13 +31,17 @@ namespace ROS2
         AZ::TickBus::Handler::BusDisconnect();
     }
 
-    float TickBasedSource::GetDeltaTime(float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time) const
+    float TickBasedSource::GetDeltaTime(float deltaTime) const
     {
         return deltaTime;
     }
 
     void TickBasedSource::OnTick(float deltaTime, AZ::ScriptTimePoint time)
     {
-        m_sourceEvent.Signal(deltaTime, time);
+        AZ_UNUSED(time);
+        AZ_UNUSED(deltaTime);
+        const auto expectedSimulationLoopTime = ROS2Interface::Get()->GetExpectedSimulationLoopTime();
+        m_sourceEvent.Signal(expectedSimulationLoopTime);
+
     }
 } // namespace ROS2

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

@@ -9,8 +9,8 @@
 
 #include "ROS2SystemComponent.h"
 #include <Lidar/LidarCore.h>
-#include <ROS2/Clock/RealTimeSource.h>
 #include <ROS2/Clock/ROS2TimeSource.h>
+#include <ROS2/Clock/RealTimeSource.h>
 #include <ROS2/Clock/SimulationTimeSource.h>
 #include <ROS2/Communication/PublisherConfiguration.h>
 #include <ROS2/Communication/QoS.h>
@@ -27,14 +27,17 @@
 #include <AzCore/Settings/SettingsRegistry.h>
 #include <AzCore/Time/ITime.h>
 #include <AzCore/std/smart_ptr/make_shared.h>
+#include <AzCore/std/sort.h>
 #include <AzCore/std/string/string_view.h>
 #include <AzFramework/API/ApplicationAPI.h>
 #include <ROS2/Sensor/SensorConfigurationRequestBus.h>
+#include <ROS2/Utilities/ROS2Conversions.h>
 
 namespace ROS2
 {
     constexpr AZStd::string_view ClockTypeConfigurationKey = "/O3DE/ROS2/ClockType";
     constexpr AZStd::string_view PublishClockConfigurationKey = "/O3DE/ROS2/PublishClock";
+    constexpr size_t FramesNumberForStats = 60;
 
     void ROS2SystemComponent::Reflect(AZ::ReflectContext* context)
     {
@@ -105,8 +108,8 @@ namespace ROS2
     void ROS2SystemComponent::Init()
     {
         rclcpp::init(0, 0);
-        
-        // handle signals, e.g. via `Ctrl+C` hotkey or `kill` command 
+
+        // handle signals, e.g. via `Ctrl+C` hotkey or `kill` command
         auto handler = [](int sig){
             rclcpp::shutdown(); // shutdown rclcpp
             std::raise(sig); // shutdown o3de
@@ -230,6 +233,25 @@ namespace ROS2
             m_simulationClock->Tick();
             m_executor->spin_some();
         }
+        // Calculate simulation loop time
+        const auto simulationTimestamp = m_simulationClock->GetROSTimestamp();
+        const float deltaSimTime = ROS2Conversions::GetTimeDifference(m_lastSimulationTime, simulationTimestamp);
+
+        // Filter out the outliers
+        m_simulationLoopTimes.push_back(deltaSimTime);
+        if (m_simulationLoopTimes.size() > FramesNumberForStats)
+        {
+            m_simulationLoopTimes.pop_front();
+        }
+        AZStd::vector<float> frameTimeSorted{ m_simulationLoopTimes.begin(), m_simulationLoopTimes.end() };
+        AZStd::sort(frameTimeSorted.begin(), frameTimeSorted.end());
+        m_simulationLoopTimeMedian = frameTimeSorted[frameTimeSorted.size() / 2];
+
+        m_lastSimulationTime = simulationTimestamp;
     }
 
+    float ROS2SystemComponent::GetExpectedSimulationLoopTime() const
+    {
+        return m_simulationLoopTimeMedian;
+    }
 } // namespace ROS2

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

@@ -64,6 +64,7 @@ namespace ROS2
         builtin_interfaces::msg::Time GetROSTimestamp() const override;
         void BroadcastTransform(const geometry_msgs::msg::TransformStamped& t, bool isDynamic) override;
         const ROS2Clock& GetSimulationClock() const override;
+        float GetExpectedSimulationLoopTime() const override;
         //////////////////////////////////////////////////////////////////////////
 
     protected:
@@ -89,5 +90,9 @@ namespace ROS2
         AZStd::unique_ptr<tf2_ros::StaticTransformBroadcaster> m_staticTFBroadcaster;
         AZStd::unique_ptr<ROS2Clock> m_simulationClock;
         NodeChangedEvent m_nodeChangedEvent;
+
+        AZStd::deque<float> m_simulationLoopTimes;
+        builtin_interfaces::msg::Time m_lastSimulationTime;
+        float m_simulationLoopTimeMedian = 1.f / 60.0f;
     };
 } // namespace ROS2