From fb1f2e0abd1a2afb670117c6cbca3e05297146e3 Mon Sep 17 00:00:00 2001 From: Steffen Planthaber Date: Tue, 31 Aug 2021 18:37:41 +0200 Subject: [PATCH] Calculate and write the realtime factor --- mars.orogen | 4 ++++ tasks/Task.cpp | 13 ++++++++++++- tasks/Task.hpp | 3 +++ 3 files changed, 19 insertions(+), 1 deletion(-) diff --git a/mars.orogen b/mars.orogen index 98d3ebb7..77d0d896 100644 --- a/mars.orogen +++ b/mars.orogen @@ -112,6 +112,10 @@ task_context "Task" do output_port('time', 'double') output_port('simulated_time', '/base/Time') + output_port('system_time', '/base/Time') + output_port('loop_time_real', 'double') + output_port('loop_time_sim', 'double') + output_port('realtime_factor', 'double') port_driven 'control_action' diff --git a/tasks/Task.cpp b/tasks/Task.cpp index 7609bad8..8f324511 100644 --- a/tasks/Task.cpp +++ b/tasks/Task.cpp @@ -601,7 +601,18 @@ void Task::receiveData( const data_broker::DataPackage& package, int id) { - _simulated_time.write(base::Time::fromMilliseconds(simulatorInterface->getTime())); + base::Time simtime = base::Time::fromMilliseconds(simulatorInterface->getTime()); + base::Time systemtime = base::Time::now(); + _simulated_time.write(simtime); + _system_time.write(systemtime); + double realdiff = (systemtime - lastSimUpdateSystemTime).toSeconds(); + double simdiff = (simtime - lastSimUpdateSimTime).toSeconds(); + _loop_time_real.write(realdiff); + _loop_time_sim.write(simdiff); + _realtime_factor.write(simdiff/realdiff); + + lastSimUpdateSimTime = simtime; + lastSimUpdateSystemTime = systemtime; } bool Task::setGravity_internal(::base::Vector3d const & value){ diff --git a/tasks/Task.hpp b/tasks/Task.hpp index dfed10d9..ee6d60a7 100644 --- a/tasks/Task.hpp +++ b/tasks/Task.hpp @@ -78,6 +78,9 @@ namespace mars { pthread_t thread_info; static lib_manager::LibManager* libManager; + base::Time lastSimUpdateSimTime; + base::Time lastSimUpdateSystemTime; + mars::interfaces::PluginInterface* multisimPlugin; int getOptionCount(const std::vector