Sensor frequency smoothness improvements (#194)

* Improve sensor ticking in ROS2

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

* Adjust to review.

Signed-off-by: Michał Pełka <[email protected]>

---------

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

Gems/ROS2/Code/Source/Clock/SimulationClock.h → Gems/ROS2/Code/Include/ROS2/Clock/SimulationClock.h

@@ -7,6 +7,8 @@
  */
 #pragma once
 
+#include <AzCore/std/chrono/chrono.h>
+#include <AzCore/std/containers/deque.h>
 #include <builtin_interfaces/msg/time.hpp>
 #include <rclcpp/publisher.hpp>
 #include <rosgraph_msgs/msg/clock.hpp>
@@ -16,6 +18,8 @@ namespace ROS2
     //! Simulation clock which can tick and serve time stamps.
     class SimulationClock
     {
+        static constexpr size_t FramesNumberForStats = 60;
+
     public:
         //! Get simulation time as ROS2 message.
         //! @see ROS2Requests::GetROSTimestamp() for more details.
@@ -25,10 +29,17 @@ namespace ROS2
         //! This will publish current time to the ROS 2 `/clock` topic.
         void Tick();
 
+        //! Returns an expected loop time of simulation. It is an estimation from past frames.
+        AZStd::chrono::duration<float, AZStd::chrono::seconds::period> GetExpectedSimulationLoopTime() const;
+
     private:
         //! Get the time since start of sim, scaled with t_simulationTickScale
         int64_t GetElapsedTimeMicroseconds() const;
 
+        AZ::s64 m_lastExecutionTime{ 0 };
+
         rclcpp::Publisher<rosgraph_msgs::msg::Clock>::SharedPtr m_clockPublisher;
+        AZ::s64 m_currentMedian{ 0 };
+        AZStd::deque<AZ::s64> m_frameTimes;
     };
 } // namespace ROS2

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

@@ -11,6 +11,7 @@
 #include <AzCore/Interface/Interface.h>
 #include <builtin_interfaces/msg/time.hpp>
 #include <geometry_msgs/msg/transform_stamped.hpp>
+#include <ROS2/Clock/SimulationClock.h>
 #include <rclcpp/node.hpp>
 
 namespace ROS2
@@ -50,6 +51,11 @@ namespace ROS2
         //! @note Transforms are already published by each ROS2FrameComponent.
         //! Use this function directly only when default behavior of ROS2FrameComponent is not sufficient.
         virtual void BroadcastTransform(const geometry_msgs::msg::TransformStamped& t, bool isDynamic) const = 0;
+
+        //! Obtains a simulation clock that is used across simulation.
+        //! @returns constant reference to currently running clock.
+        virtual const SimulationClock& GetSimulationClock() const = 0;
+
     };
 
     class ROS2BusTraits : public AZ::EBusTraits

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

@@ -53,6 +53,7 @@ namespace ROS2
         //! Visualisation can be turned on or off in SensorConfiguration.
         virtual void Visualise(){};
 
-        float m_timeElapsedSinceLastTick = 0.0f;
+        //! The number of ticks that are expected to pass to trigger next measurement.
+        AZ::s32 m_tickCountDown{ 0 };
     };
 } // namespace ROS2

Gems/ROS2/Code/Source/Clock/SimulationClock.cpp

@@ -6,8 +6,11 @@
  *
  */
 
-#include "SimulationClock.h"
 #include <AzCore/Time/ITime.h>
+#include <AzCore/std/algorithm.h>
+#include <AzCore/std/containers/deque.h>
+#include <AzCore/std/containers/vector.h>
+#include <ROS2/Clock/SimulationClock.h>
 #include <ROS2/ROS2Bus.h>
 #include <rclcpp/qos.hpp>
 
@@ -36,8 +39,14 @@ namespace ROS2
         }
     }
 
+    AZStd::chrono::duration<float, AZStd::chrono::seconds::period> SimulationClock::GetExpectedSimulationLoopTime() const
+    {
+        return AZStd::chrono::duration<AZ::s64, AZStd::chrono::microseconds::period>(m_currentMedian);
+    }
+
     void SimulationClock::Tick()
     {
+        auto elapsed = GetElapsedTimeMicroseconds();
         if (!m_clockPublisher)
         { // Lazy construct
             auto ros2Node = ROS2Interface::Get()->GetNode();
@@ -49,5 +58,18 @@ namespace ROS2
         rosgraph_msgs::msg::Clock msg;
         msg.clock = GetROSTimestamp();
         m_clockPublisher->publish(msg);
+
+        AZ::s64 deltaTime = elapsed - m_lastExecutionTime;
+        m_lastExecutionTime = elapsed;
+
+        // statistics on execution time
+        m_frameTimes.push_back(deltaTime);
+        if (m_frameTimes.size() > FramesNumberForStats)
+        {
+            m_frameTimes.pop_front();
+        }
+        AZStd::vector<AZ::s64> frameTimeSorted{ m_frameTimes.begin(), m_frameTimes.end() };
+        AZStd::sort(frameTimeSorted.begin(), frameTimeSorted.end());
+        m_currentMedian = frameTimeSorted[frameTimeSorted.size() / 2];
     }
 } // namespace ROS2

Gems/ROS2/Code/Source/ROS2SystemComponent.cpp

@@ -127,6 +127,11 @@ namespace ROS2
         return m_ros2Node;
     }
 
+    const SimulationClock& ROS2SystemComponent::GetSimulationClock() const
+    {
+        return m_simulationClock;
+    }
+
     void ROS2SystemComponent::BroadcastTransform(const geometry_msgs::msg::TransformStamped& t, bool isDynamic) const
     {
         if (isDynamic)

Gems/ROS2/Code/Source/ROS2SystemComponent.h

@@ -11,8 +11,8 @@
 #include <AzCore/Component/Component.h>
 #include <AzCore/Component/TickBus.h>
 #include <AzCore/std/smart_ptr/unique_ptr.h>
-#include <Clock/SimulationClock.h>
 #include <Lidar/LidarSystem.h>
+#include <ROS2/Clock/SimulationClock.h>
 #include <ROS2/ROS2Bus.h>
 #include <builtin_interfaces/msg/time.hpp>
 #include <memory>
@@ -46,6 +46,7 @@ namespace ROS2
         std::shared_ptr<rclcpp::Node> GetNode() const override;
         builtin_interfaces::msg::Time GetROSTimestamp() const override;
         void BroadcastTransform(const geometry_msgs::msg::TransformStamped& t, bool isDynamic) const override;
+        const SimulationClock& GetSimulationClock() const override;
         //////////////////////////////////////////////////////////////////////////
 
     protected:

Gems/ROS2/Code/Source/Sensor/ROS2SensorComponent.cpp

@@ -73,22 +73,16 @@ namespace ROS2
         {
             return;
         }
-
-        auto frequency = m_sensorConfiguration.m_frequency;
-
-        auto frameTime = frequency == 0 ? 1 : 1 / frequency;
-
-        m_timeElapsedSinceLastTick += deltaTime;
-        if (m_timeElapsedSinceLastTick < frameTime)
-            return;
-
-        m_timeElapsedSinceLastTick -= frameTime;
-        if (deltaTime > frameTime)
-        { // Frequency higher than possible, not catching up, just keep going with each frame.
-            m_timeElapsedSinceLastTick = 0.0f;
+        m_tickCountDown--;
+        if (m_tickCountDown <= 0)
+        {
+            const AZStd::chrono::duration<float, AZStd::chrono::seconds::period> expectedLoopTime =
+                ROS2Interface::Get()->GetSimulationClock().GetExpectedSimulationLoopTime();
+            const auto frequency = m_sensorConfiguration.m_frequency;
+            const auto frameTime = frequency == 0.f ? 1.f : 1.f / frequency;
+            const float numberOfFrames = frameTime / expectedLoopTime.count();
+            m_tickCountDown = AZStd::round(numberOfFrames);
+            FrequencyTick();
         }
-
-        // Note that sensor frequency can be limited by simulation tick rate (if higher sensor Hz is desired).
-        FrequencyTick();
     }
 } // namespace ROS2

Gems/ROS2/Code/ros2_files.cmake

@@ -14,7 +14,6 @@ set(FILES
         Source/Camera/ROS2CameraSensorComponent.cpp
         Source/Camera/ROS2CameraSensorComponent.h
         Source/Clock/SimulationClock.cpp
-        Source/Clock/SimulationClock.h
         Source/Communication/QoS.cpp
         Source/Communication/TopicConfiguration.cpp
         Source/Frame/NamespaceConfiguration.cpp

Gems/ROS2/Code/ros2_header_files.cmake

@@ -4,6 +4,7 @@
 # SPDX-License-Identifier: Apache-2.0 OR MIT
 
 set(FILES
+        Include/ROS2/Clock/SimulationClock.h
         Include/ROS2/Frame/NamespaceConfiguration.h
         Include/ROS2/Frame/ROS2FrameComponent.h
         Include/ROS2/Frame/ROS2Transform.h