[wholeBodyDynamics] Use the time profiler in the whole-body dynamics …

…device
robotology · Mar 7, 2022 · e44b223 · e44b223
1 parent f3da262
commit e44b223
Show file tree

Hide file tree

Showing 3 changed files with 32 additions and 1 deletion.
diff --git a/devices/wholeBodyDynamics/CMakeLists.txt b/devices/wholeBodyDynamics/CMakeLists.txt
@@ -22,7 +22,8 @@ if(ENABLE_wholebodydynamics)
 
     yarp_add_plugin(wholeBodyDynamicsDevice WholeBodyDynamicsDevice.h WholeBodyDynamicsDevice.cpp
                                             SixAxisForceTorqueMeasureHelpers.h SixAxisForceTorqueMeasureHelpers.cpp
-                                            GravityCompensationHelpers.h GravityCompensationHelpers.cpp)
+                                            GravityCompensationHelpers.h GravityCompensationHelpers.cpp
+                                            TimeProfiler.h TimeProfiler.cpp)
 
     target_link_libraries(wholeBodyDynamicsDevice   wholeBodyDynamicsSettings
                                                     wholeBodyDynamics_IDLServer

diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp
@@ -1,3 +1,4 @@
+#include "TimeProfiler.h"
 #define SKIN_EVENTS_TIMEOUT 0.2
 #include "WholeBodyDynamicsDevice.h"
 
@@ -1633,6 +1634,12 @@ bool WholeBodyDynamicsDevice::open(os::Searchable& config)
         yDebug() << "wholeBodyDynamics Statistics: Filtered FT ports opened in " << yarp::os::Time::now() - tick << "s.";
     }
 
+    // print the clock once every 1.0 seconds
+    m_timerHandler.setHorizon(1.0/getPeriod());
+    m_timerHandler.addTimer("all",
+                            WholeBodyDynamics::Timer("all",
+                                                     std::chrono::duration<double>(getPeriod())));
+
     yDebug() << "wholeBodyDynamics Statistics: Configuration finished. Waiting attachAll to be called.";
 
     return true;
@@ -2817,6 +2824,9 @@ void WholeBodyDynamicsDevice::publishFilteredFTWithoutOffset()
 
 void WholeBodyDynamicsDevice::run()
 {
+
+    m_timerHandler.tic("all");
+
     std::lock_guard<std::mutex> guard(this->deviceMutex);
 
     if( correctlyConfigured )
@@ -2825,26 +2835,43 @@ void WholeBodyDynamicsDevice::run()
         //this->reconfigureClassFromSettings();
 
         // Read sensor readings
+        m_timerHandler.tic("read sensors");
         this->readSensors();
+        m_timerHandler.toc("read sensors");
 
         // Filter sensor and remove offset
+        m_timerHandler.tic("remove offset");
         this->filterSensorsAndRemoveSensorOffsets();
+        m_timerHandler.toc("remove offset");
 
         // Update kinematics
+        m_timerHandler.tic("kinematics");
         this->updateKinematics();
+        m_timerHandler.toc("kinematics");
 
         // Read contacts info from the skin or from assume contact location
+        m_timerHandler.tic("contact points");
         this->readContactPoints();
+        m_timerHandler.toc("contact points");
 
         // Compute calibration if we are in calibration mode
+        m_timerHandler.tic("calibration");
         this->computeCalibration();
+        m_timerHandler.toc("calibration");
 
         // Compute estimated external forces and internal joint torques
+        m_timerHandler.tic("compute forces");
         this->computeExternalForcesAndJointTorques();
+        m_timerHandler.toc("compute forces");
 
         // Publish estimated quantities
+        m_timerHandler.tic("publish");
         this->publishEstimatedQuantities();
+        m_timerHandler.toc("publish");
     }
+
+    m_timerHandler.toc("all");
+    m_timerHandler.profiling();
 }
 
 bool WholeBodyDynamicsDevice::detachAll()

diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h
@@ -31,6 +31,7 @@
 #include <wholeBodyDynamics_IDLServer.h>
 #include "SixAxisForceTorqueMeasureHelpers.h"
 #include "GravityCompensationHelpers.h"
+#include "TimeProfiler.h"
 
 #include <vector>
 #include <mutex>
@@ -746,6 +747,8 @@ class WholeBodyDynamicsDevice :  public yarp::dev::DeviceDriver,
     iDynTree::JointDOFsDoubleArray m_gravityCompensationTorques;
     void resetGravityCompensation();
 
+    WholeBodyDynamics::TimerHandler m_timerHandler;
+
 public:
     // CONSTRUCTOR
     WholeBodyDynamicsDevice();