diff --git a/devices/wholeBodyDynamics/CMakeLists.txt b/devices/wholeBodyDynamics/CMakeLists.txt index b075062..730b7be 100644 --- 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/TimeProfiler.cpp b/devices/wholeBodyDynamics/TimeProfiler.cpp new file mode 100644 index 0000000..7ede91b --- /dev/null +++ b/devices/wholeBodyDynamics/TimeProfiler.cpp @@ -0,0 +1,185 @@ +/** + * @file TimeProfiler.cpp + * @authors Giulio Romualdi + * @copyright 2018 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2018 + */ + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "TimeProfiler.h" + +using namespace WholeBodyDynamics; + +std::string +printTimerDescription(const std::string& name, const TimerHandler::TimerDescription& description) +{ + std::stringstream ss; + ss << "|" << std::setw(30) << name << "|" << std::setw(15) << std::setprecision(13) + << description.averageDuration.count() << "|" << std::setw(10) << std::setprecision(8) + << description.timer.getInfo().deadlineMiss << "|" << std::setw(15) << std::setprecision(13) + << description.timer.getInfo().latestDeadlineMissDuration.count() << "|" << std::endl; + + return ss.str(); +}; + +Timer::Timer(const std::string& name, const std::chrono::duration& expectedDuration) +{ + this->setName(name); + this->setExpectedDuration(expectedDuration); +} + +void Timer::tic() +{ + m_initTime = std::chrono::system_clock::now(); + m_info.dealineMissDetected = false; +} + +void Timer::toc() +{ + m_info.duration = std::chrono::system_clock::now() - m_initTime; + if (m_info.duration > m_exprectedDuration) + { + m_info.dealineMissDetected = true; + m_info.deadlineMiss++; + m_info.latestDeadlineMissDuration = m_info.duration; + } +} + +void Timer::setName(const std::string& name) +{ + m_info.name = name; +} + +void Timer::setExpectedDuration(const std::chrono::duration& expectedDuration) +{ + m_exprectedDuration = expectedDuration; +} + +const Timer::Info& Timer::getInfo() const +{ + return m_info; +} + +void TimerHandler::setHorizon(unsigned int horizon) +{ + m_horizon = horizon; +} + +bool TimerHandler::addTimer(const std::string& name, const Timer& timer) +{ + auto it = m_timers.find(name); + if (it != m_timers.end()) + { + return false; + } + + m_timers[name].timer = timer; + + return true; +} + +void TimerHandler::tic(const std::string& key) +{ + // if the timer does not exist we add it + m_timers[key].timer.tic(); +} + +bool TimerHandler::toc(const std::string& key) +{ + auto it = m_timers.find(key); + if (it == m_timers.end()) + { + return false; + } + + it->second.timer.toc(); + return true; +} + +void TimerHandler::setVerbosity(bool verbosity) +{ + m_verbosity = verbosity; +} + +void TimerHandler::profiling() +{ + bool deadlineMissDetected = false; + for (auto& [name, timerDescription] : m_timers) + { + const auto& duration = timerDescription.timer.getInfo().duration; + + deadlineMissDetected = deadlineMissDetected || timerDescription.timer.getInfo().dealineMissDetected; + + // this automatically create the element if does bot exist + auto& queue = m_durations[name]; + queue.push_back(duration); + + // keep the queue size equal to horizon + if (queue.size() > m_horizon) + { + queue.pop_front(); + } + + // this should never happen + assert(queue.size() <= m_horizon); + + timerDescription.averageDuration + = std::accumulate(std::next(queue.begin()), queue.end(), queue.front()) + / double(queue.size()); + } + + if (m_verbosity) + { + if (m_verbosityCounter == 0) + { + std::stringstream ss; + std::string output; + + ss << "|" << std::setw(30) << "name |" << std::setw(15) << "tavarg (s) |" + << std::setw(10) << "dm |" << std::setw(15) << "tdm (s) |" << std::endl + << " " << std::setfill('-') << "|" << std::setw(30) << "|" << std::setw(15) + << "|" << std::setw(10) << "|" << std::setw(15) << "|" << std::endl; + + output = ss.str(); + + for (const auto& [name, timerDescription] : m_timers) + { + output += " " + printTimerDescription(name, timerDescription); + } + + yDebug() << output; + } + + if (deadlineMissDetected) + { + std::stringstream ss; + std::string output; + + ss << "---------------------------------- Deadline miss detected ----------------------------------" << std::endl; + ss << "|" << std::setw(30) << "name |" << std::setw(15) << "t (s) |" << std::endl + << " " << std::setfill('-') << "|" << std::setw(30) << "|" << std::setw(15) << std::endl; + + for (const auto& [name, timerDescription] : m_timers) + { + ss << "|" << std::setw(30) << name << "|" << std::setw(15) << std::setprecision(13) + << timerDescription.timer.getInfo().duration.count() << std::endl; + } + + yDebug() << ss.str(); + } + + m_verbosityCounter++; + if (m_verbosityCounter == m_horizon) + m_verbosityCounter = 0; + } +} diff --git a/devices/wholeBodyDynamics/TimeProfiler.h b/devices/wholeBodyDynamics/TimeProfiler.h new file mode 100644 index 0000000..4474c52 --- /dev/null +++ b/devices/wholeBodyDynamics/TimeProfiler.h @@ -0,0 +1,115 @@ +#ifndef TIME_PROFILER_H +#define TIME_PROFILER_H + +// std +#include +#include +#include + +namespace WholeBodyDynamics +{ + +/** + * Simple timer. + */ +class Timer +{ + +public: + struct Info + { + unsigned int deadlineMiss{0}; /**< Number of deadline miss */ + std::string name{"Timer"}; /**< Name associated to the timer */ + bool dealineMissDetected{false}; + std::chrono::duration latestDeadlineMissDuration{0}; /**< Average duration. */ + std::chrono::duration duration{0}; /**< Latest duration. */ + }; + + Timer(const std::string& name, const std::chrono::duration& expectedDuration); + + Timer() = default; + + void setName(const std::string& name); + + void setExpectedDuration(const std::chrono::duration& expectedDuration); + + /** + * Set initial time. + */ + inline void tic(); + + /** + * Set final time. + */ + inline void toc(); + + /** + * Get the average duration. + * @return average duration. + */ + const Info& getInfo() const; + +private: + std::chrono::time_point m_initTime; /**< Init time. */ + std::chrono::duration m_exprectedDuration{std::chrono::duration::max()}; + + Info m_info; +}; + +/** + * Simple Time profiler class + */ +class TimerHandler +{ +public: + + struct TimerDescription + { + ::WholeBodyDynamics::Timer timer; + std::chrono::duration averageDuration{0}; + }; + + /** + * Set the output period. + * @param maxCounter is the period (expressed in cycles). + */ + void setHorizon(unsigned int horizon); + + /** + * Add a new timer + * @param key is the name of the timer. + * @return true/false in case of success/failure. + */ + void tic(const std::string& key); + + /** + * Set the init time for the timer named "key" + * @param key is the name of the timer. + * @return true/false in case of success/failure. + */ + bool toc(const std::string& key); + + /** + * Print the profiling quantities. + */ + void profiling(); + + void setVerbosity(bool verbosity); + + bool addTimer(const std::string& name, const Timer& timer); + +private: + unsigned int m_horizon; /**< Counter useful to print the profiling quantities only every + m_maxCounter times. */ + + std::unordered_map m_timers; /**< Dictionary that contains all + the timers. */ + + std::unordered_map>> m_durations; + + bool m_verbosity{false}; + unsigned int m_verbosityCounter{0}; +}; +}; // namespace WholeBodyDynamics + +#endif // TIME_PROFILER_H diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp index 65e361a..10c08a3 100644 --- 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" @@ -39,6 +40,8 @@ WholeBodyDynamicsDevice::WholeBodyDynamicsDevice(): yarp::os::PeriodicThread(def isIMUAttached{false}, settingsEditor(settings) { + this->setPriority(10, 2); + // Calibration quantities calibrationBuffers.ongoingCalibration = false; calibrationBuffers.calibratingFTsensor.resize(0); @@ -1633,6 +1636,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 5 minutes + m_timerHandler.setHorizon(5.0 * 60 /getPeriod()); + m_timerHandler.addTimer("all", + WholeBodyDynamics::Timer("all", + std::chrono::duration(getPeriod()))); + m_timerHandler.setVerbosity(true); yDebug() << "wholeBodyDynamics Statistics: Configuration finished. Waiting attachAll to be called."; return true; @@ -2650,27 +2659,40 @@ void WholeBodyDynamicsDevice::publishEstimatedQuantities() // Only send estimation if a valid offset is available if( validOffsetAvailable ) { + + m_timerHandler.tic("Publish - torque"); //Send torques publishTorques(); + m_timerHandler.toc("Publish - torque"); + //Send external contacts if (useSkinForContacts) { + m_timerHandler.tic("Publish - contact"); publishContacts(); + m_timerHandler.toc("Publish - contact"); } + m_timerHandler.tic("Publish - wrench"); //Send external wrench estimates publishExternalWrenches(); + m_timerHandler.toc("Publish - wrench"); + + m_timerHandler.tic("Publish - gravity"); // Send gravity compensation torques publishGravityCompensation(); + m_timerHandler.toc("Publish - gravity"); //Send filtered inertia for gravity compensation //publishFilteredInertialForGravityCompensator(); //Send filtered force torque sensor measurment, if requested if( streamFilteredFT){ + m_timerHandler.tic("Publish - filtered FT"); publishFilteredFTWithoutOffset(); + m_timerHandler.toc("Publish - filtered FT"); } } } @@ -2870,6 +2892,9 @@ void WholeBodyDynamicsDevice::publishFilteredFTWithoutOffset() void WholeBodyDynamicsDevice::run() { + + m_timerHandler.tic("all"); + std::lock_guard guard(this->deviceMutex); if( correctlyConfigured ) @@ -2878,26 +2903,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 index c3bcdbf..5086c2c 100644 --- a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h +++ b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h @@ -31,6 +31,7 @@ #include #include "SixAxisForceTorqueMeasureHelpers.h" #include "GravityCompensationHelpers.h" +#include "TimeProfiler.h" #include #include @@ -753,6 +754,8 @@ class WholeBodyDynamicsDevice : public yarp::dev::DeviceDriver, iDynTree::JointDOFsDoubleArray m_gravityCompensationTorques; void resetGravityCompensation(); + WholeBodyDynamics::TimerHandler m_timerHandler; + public: // CONSTRUCTOR WholeBodyDynamicsDevice();