Add warning when period is exceeded.

Author: jmirabel

src/log.cpp

@@ -114,13 +114,15 @@ void Log::start_it() {
       timeorigin_;
 }
 
-void Log::stop_it() {
+double Log::stop_it() {
   struct timeval current;
   gettimeofday(&current, 0);
 
   time_stop_it_ =
       ((double)current.tv_sec + 0.000001 * (double)current.tv_usec) -
       timeorigin_;
+
+  return time_stop_it_ - time_start_it_;
 }
 
 void Log::save(std::string &fileName) {

src/log.hh

@@ -92,7 +92,8 @@ public:
 
   void save(std::string &fileName);
   void start_it();
-  void stop_it();
+  /// \return the elapsed time since the previous \ref start_it
+  double stop_it();
 };
 } // namespace rc_sot_system
 

src/roscontrol-sot-controller.cpp

@@ -967,7 +967,11 @@ void RCSotController::one_iteration() {
   readControl(controlValues_);
 
   // Chrono stop.
-  RcSotLog_.stop_it();
+  double it_duration = RcSotLog_.stop_it();
+  if (it_duration > dt_) {
+    ROS_WARN_THROTTLE(1, "Iteration duration (%d) bigger than max period (%d)",
+        it_duration, dt_);
+  }
 
   /// Store everything in Log.
   RcSotLog_.record(DataOneIter_);