From c73520c65a8f9fec14e7953303cd946010826a88 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Thu, 5 Dec 2024 11:01:55 +0100 Subject: [PATCH 1/3] Use ROS 2 clock for sensor triggering, reintroduce median filtering to FPS calculation MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michał Pełka --- Gems/ROS2/Code/Include/ROS2/Clock/ROS2Clock.h | 1 - Gems/ROS2/Code/Include/ROS2/ROS2Bus.h | 3 +++ .../ROS2/Sensor/Events/EventSourceAdapter.h | 2 +- .../ROS2/Sensor/Events/PhysicsBasedSource.h | 2 ++ .../ROS2/Sensor/Events/TickBasedSource.h | 10 +++++--- .../Sensor/Events/PhysicsBasedSource.cpp | 13 ++++++++-- .../Source/Sensor/Events/TickBasedSource.cpp | 12 +++++++--- .../SystemComponents/ROS2SystemComponent.cpp | 24 ++++++++++++++++++- .../SystemComponents/ROS2SystemComponent.h | 5 ++++ 9 files changed, 61 insertions(+), 11 deletions(-) diff --git a/Gems/ROS2/Code/Include/ROS2/Clock/ROS2Clock.h b/Gems/ROS2/Code/Include/ROS2/Clock/ROS2Clock.h index 80cb1fd35..18f600c4e 100644 --- a/Gems/ROS2/Code/Include/ROS2/Clock/ROS2Clock.h +++ b/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(); diff --git a/Gems/ROS2/Code/Include/ROS2/ROS2Bus.h b/Gems/ROS2/Code/Include/ROS2/ROS2Bus.h index 9491e62a2..f5d27e180 100644 --- a/Gems/ROS2/Code/Include/ROS2/ROS2Bus.h +++ b/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 diff --git a/Gems/ROS2/Code/Include/ROS2/Sensor/Events/EventSourceAdapter.h b/Gems/ROS2/Code/Include/ROS2/Sensor/Events/EventSourceAdapter.h index 5e23dc6af..3eb669d1a 100644 --- a/Gems/ROS2/Code/Include/ROS2/Sensor/Events/EventSourceAdapter.h +++ b/Gems/ROS2/Code/Include/ROS2/Sensor/Events/EventSourceAdapter.h @@ -180,10 +180,10 @@ namespace ROS2 { return false; } - const float sourceFrequencyEstimation = 1.0f / sourceDeltaTime; const float numberOfFrames = m_adaptedFrequency <= sourceFrequencyEstimation ? (sourceFrequencyEstimation / m_adaptedFrequency) : 1.0f; + AZ_Printf("EventSourceAdapter", "Number of frames: %f", numberOfFrames); m_tickCounter = aznumeric_cast(AZStd::round(numberOfFrames)); return true; } diff --git a/Gems/ROS2/Code/Include/ROS2/Sensor/Events/PhysicsBasedSource.h b/Gems/ROS2/Code/Include/ROS2/Sensor/Events/PhysicsBasedSource.h index 2e7d79b69..6bb9eb1ee 100644 --- a/Gems/ROS2/Code/Include/ROS2/Sensor/Events/PhysicsBasedSource.h +++ b/Gems/ROS2/Code/Include/ROS2/Sensor/Events/PhysicsBasedSource.h @@ -11,6 +11,7 @@ #include #include #include +#include #include 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 diff --git a/Gems/ROS2/Code/Include/ROS2/Sensor/Events/TickBasedSource.h b/Gems/ROS2/Code/Include/ROS2/Sensor/Events/TickBasedSource.h index 93a09b44a..6c4f47e0f 100644 --- a/Gems/ROS2/Code/Include/ROS2/Sensor/Events/TickBasedSource.h +++ b/Gems/ROS2/Code/Include/ROS2/Sensor/Events/TickBasedSource.h @@ -11,14 +11,14 @@ #include #include #include - +#include 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 + : public SensorEventSource , 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 m_deltaTimes; + constexpr static size_t m_maxDeltaTimes = 100; }; } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Sensor/Events/PhysicsBasedSource.cpp b/Gems/ROS2/Code/Source/Sensor/Events/PhysicsBasedSource.cpp index cc76df9ea..729249227 100644 --- a/Gems/ROS2/Code/Source/Sensor/Events/PhysicsBasedSource.cpp +++ b/Gems/ROS2/Code/Source/Sensor/Events/PhysicsBasedSource.cpp @@ -7,6 +7,8 @@ */ #include + +#include #include #include @@ -23,10 +25,17 @@ 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(); + + float deltaSimTime = ROS2Conversions::GetTimeDifference(m_lastSimulationTime, simulationTime); + m_sourceEvent.Signal(sceneHandle, deltaSimTime); + m_lastSimulationTime = simulationTime; }); auto* sceneInterface = AZ::Interface::Get(); diff --git a/Gems/ROS2/Code/Source/Sensor/Events/TickBasedSource.cpp b/Gems/ROS2/Code/Source/Sensor/Events/TickBasedSource.cpp index d8ee31077..d606e965b 100644 --- a/Gems/ROS2/Code/Source/Sensor/Events/TickBasedSource.cpp +++ b/Gems/ROS2/Code/Source/Sensor/Events/TickBasedSource.cpp @@ -8,7 +8,9 @@ #include #include - +#include +#include +#include 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); + auto simTimestamp = ROS2Interface::Get()->GetExpectedSimulationLoopTime(); + m_sourceEvent.Signal(simTimestamp); + } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/SystemComponents/ROS2SystemComponent.cpp b/Gems/ROS2/Code/Source/SystemComponents/ROS2SystemComponent.cpp index 148655a4e..da70eba6a 100644 --- a/Gems/ROS2/Code/Source/SystemComponents/ROS2SystemComponent.cpp +++ b/Gems/ROS2/Code/Source/SystemComponents/ROS2SystemComponent.cpp @@ -30,11 +30,14 @@ #include #include #include - +#include +#include +#include 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) { @@ -230,6 +233,25 @@ namespace ROS2 m_simulationClock->Tick(); m_executor->spin_some(); } + // Calculate simulation loop time + const auto simTimestamp = m_simulationClock->GetROSTimestamp(); + float deltaSimTime = ROS2Conversions::GetTimeDifference(m_lastSimulationTime, simTimestamp); + + // Filter out the outliers + m_simulationLoopTimes.push_back(deltaSimTime); + if (m_simulationLoopTimes.size() > FramesNumberForStats) + { + m_simulationLoopTimes.pop_front(); + } + AZStd::vector frameTimeSorted{ m_simulationLoopTimes.begin(), m_simulationLoopTimes.end() }; + AZStd::sort(frameTimeSorted.begin(), frameTimeSorted.end()); + m_currentMedian = frameTimeSorted[frameTimeSorted.size() / 2]; + + m_lastSimulationTime = simTimestamp; } + float ROS2SystemComponent::GetExpectedSimulationLoopTime() const + { + return m_currentMedian; + } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/SystemComponents/ROS2SystemComponent.h b/Gems/ROS2/Code/Source/SystemComponents/ROS2SystemComponent.h index 9f0f83362..359bae916 100644 --- a/Gems/ROS2/Code/Source/SystemComponents/ROS2SystemComponent.h +++ b/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 m_staticTFBroadcaster; AZStd::unique_ptr m_simulationClock; NodeChangedEvent m_nodeChangedEvent; + + AZStd::deque m_simulationLoopTimes; + builtin_interfaces::msg::Time m_lastSimulationTime; + float m_currentMedian = 1.f/60.0f; }; } // namespace ROS2 From 2c4e3f7609d28980a84ffde97ddb0c3166c84ca9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Tue, 10 Dec 2024 14:10:49 +0100 Subject: [PATCH 2/3] Update Gems/ROS2/Code/Include/ROS2/Sensor/Events/EventSourceAdapter.h MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michał Pełka --- Gems/ROS2/Code/Include/ROS2/Sensor/Events/EventSourceAdapter.h | 1 - 1 file changed, 1 deletion(-) diff --git a/Gems/ROS2/Code/Include/ROS2/Sensor/Events/EventSourceAdapter.h b/Gems/ROS2/Code/Include/ROS2/Sensor/Events/EventSourceAdapter.h index 3eb669d1a..59c275916 100644 --- a/Gems/ROS2/Code/Include/ROS2/Sensor/Events/EventSourceAdapter.h +++ b/Gems/ROS2/Code/Include/ROS2/Sensor/Events/EventSourceAdapter.h @@ -183,7 +183,6 @@ namespace ROS2 const float sourceFrequencyEstimation = 1.0f / sourceDeltaTime; const float numberOfFrames = m_adaptedFrequency <= sourceFrequencyEstimation ? (sourceFrequencyEstimation / m_adaptedFrequency) : 1.0f; - AZ_Printf("EventSourceAdapter", "Number of frames: %f", numberOfFrames); m_tickCounter = aznumeric_cast(AZStd::round(numberOfFrames)); return true; } From 93ae61967963e2703ae520f3f8e6cbd06de9c2aa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Mon, 30 Dec 2024 10:10:44 +0100 Subject: [PATCH 3/3] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Jan Hanca Signed-off-by: Michał Pełka --- Gems/ROS2/Code/Source/Sensor/Events/PhysicsBasedSource.cpp | 4 ++-- Gems/ROS2/Code/Source/Sensor/Events/TickBasedSource.cpp | 2 +- .../ROS2/Code/Source/SystemComponents/ROS2SystemComponent.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Gems/ROS2/Code/Source/Sensor/Events/PhysicsBasedSource.cpp b/Gems/ROS2/Code/Source/Sensor/Events/PhysicsBasedSource.cpp index 729249227..a6ac00dc0 100644 --- a/Gems/ROS2/Code/Source/Sensor/Events/PhysicsBasedSource.cpp +++ b/Gems/ROS2/Code/Source/Sensor/Events/PhysicsBasedSource.cpp @@ -25,7 +25,7 @@ namespace ROS2 void PhysicsBasedSource::Start() { m_onSceneSimulationEventHandler.Disconnect(); - const auto *ros2Interface = ROS2Interface::Get(); + const auto* ros2Interface = ROS2Interface::Get(); AZ_Assert(ros2Interface, "ROS2 interface is not initialized."); m_onSceneSimulationEventHandler = AzPhysics::SceneEvents::OnSceneSimulationFinishHandler( @@ -33,7 +33,7 @@ namespace ROS2 { const auto simulationTime = ros2Interface->GetROSTimestamp(); - float deltaSimTime = ROS2Conversions::GetTimeDifference(m_lastSimulationTime, simulationTime); + const float deltaSimTime = ROS2Conversions::GetTimeDifference(m_lastSimulationTime, simulationTime); m_sourceEvent.Signal(sceneHandle, deltaSimTime); m_lastSimulationTime = simulationTime; }); diff --git a/Gems/ROS2/Code/Source/Sensor/Events/TickBasedSource.cpp b/Gems/ROS2/Code/Source/Sensor/Events/TickBasedSource.cpp index d606e965b..a9fa9ddb9 100644 --- a/Gems/ROS2/Code/Source/Sensor/Events/TickBasedSource.cpp +++ b/Gems/ROS2/Code/Source/Sensor/Events/TickBasedSource.cpp @@ -40,7 +40,7 @@ namespace ROS2 { AZ_UNUSED(time); AZ_UNUSED(deltaTime); - auto simTimestamp = ROS2Interface::Get()->GetExpectedSimulationLoopTime(); + const auto simTimestamp = ROS2Interface::Get()->GetExpectedSimulationLoopTime(); m_sourceEvent.Signal(simTimestamp); } diff --git a/Gems/ROS2/Code/Source/SystemComponents/ROS2SystemComponent.cpp b/Gems/ROS2/Code/Source/SystemComponents/ROS2SystemComponent.cpp index da70eba6a..be79fb8df 100644 --- a/Gems/ROS2/Code/Source/SystemComponents/ROS2SystemComponent.cpp +++ b/Gems/ROS2/Code/Source/SystemComponents/ROS2SystemComponent.cpp @@ -235,7 +235,7 @@ namespace ROS2 } // Calculate simulation loop time const auto simTimestamp = m_simulationClock->GetROSTimestamp(); - float deltaSimTime = ROS2Conversions::GetTimeDifference(m_lastSimulationTime, simTimestamp); + const float deltaSimTime = ROS2Conversions::GetTimeDifference(m_lastSimulationTime, simTimestamp); // Filter out the outliers m_simulationLoopTimes.push_back(deltaSimTime);