Bugfixes to the rate class.

src/core/utility/rate.cpp

@@ -9,7 +9,6 @@ using namespace argos;
 CRate::CRate(Real f_rate) {
    SetRate(f_rate);
    ::gettimeofday(&m_tPast, NULL);
-   DEBUG("start time: %ld:%ld\n", m_tPast.tv_sec, m_tPast.tv_usec);
 }
 
 /****************************************/
@@ -18,22 +17,27 @@ CRate::CRate(Real f_rate) {
 void CRate::Sleep() {
    /* Get current time */
    ::gettimeofday(&m_tNow, NULL);
-   DEBUG("current time: %ld:%ld\n", m_tNow.tv_sec, m_tNow.tv_usec);
    /* Calculate difference between past call and now */
    timersub(&m_tNow, &m_tPast, &m_tDiff);
    UInt64 unMicroSecDiff =
       m_tDiff.tv_sec * 1000000 +
-      m_tPast.tv_usec;
-   DEBUG("diff time: %ld:%ld\n", m_tDiff.tv_sec, m_tDiff.tv_usec);
+      m_tDiff.tv_usec;
    /* Sleep if necessary */
    if(unMicroSecDiff < m_unNominalPeriod) {
       m_tSleepPeriod.tv_sec = (m_unNominalPeriod - unMicroSecDiff) / 1e6;
       m_tSleepPeriod.tv_nsec = (m_unNominalPeriod - unMicroSecDiff) * 1000;
-      DEBUG("sleep period: %ld:%ld\n", m_tSleepPeriod.tv_sec, m_tSleepPeriod.tv_nsec);
       ::nanosleep(&m_tSleepPeriod, NULL);
    }
+   else {
+      LOGERR << "[WARNING] Nominal rate "
+             << m_fNominalRate
+             << " loops per sec delayed by "
+             << (unMicroSecDiff - m_unNominalPeriod)
+             << " microseconds"
+             << std::endl;
+   }
    /* Update past for next call */
-   m_tPast = m_tNow;
+   ::gettimeofday(&m_tPast, NULL);
 }
 
 /****************************************/

src/testing/unit/test-rate.cpp

@@ -1,13 +1,15 @@
+#include <argos3/core/utility/logging/argos_log.h>
 #include <argos3/core/utility/rate.h>
 #include <iostream>
 
 using namespace argos;
 
 int main() {
-   CRate cRate(1);
+   CRate cRate(10000);
    std::cout << "start" << std::endl;
    for(int i = 0; i < 10; ++i) {
       cRate.Sleep();
       std::cout << "i = " << i << std::endl;
+      LOGERR.Flush();
    }
 }