diff --git a/mars.orogen b/mars.orogen index 98d3ebb..77d0d89 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 7609bad..8f32451 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 dfed10d..ee6d60a 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