Compute delay given a fixed initial reference to avoid drifts

robotology#2488

doc/release/master/fix_periodic_thread_drift.md

@@ -0,0 +1,12 @@
+fix_periodic_thread_drift {#master}
+-----------
+
+### Libraries
+
+#### `os`
+
+##### `PeriodicThread`
+
+* Fixed a bug that caused the underlying thread to accumulate error over time,
+  potentially leading to significant drifts after a longer while. Also affects
+  `yarp::os::Timer` (#2488).

src/libYARP_os/src/yarp/os/PeriodicThread.cpp

@@ -28,8 +28,6 @@ class yarp::os::PeriodicThread::Private : public ThreadImpl
     mutable std::mutex mutex;
 
     double elapsed;
-    double sleepPeriod;
-
     bool suspended;
     double totalUsed;    //total time taken iterations
     unsigned int count;  //number of iterations from last reset
@@ -39,7 +37,10 @@ class yarp::os::PeriodicThread::Private : public ThreadImpl
     double sumUsedSq;    //cumulative sum sq of estimated thread tun
     double previousRun;  //time when last iteration started
     double currentRun;   //time when this iteration started
+    double refTime;      //absolute reference time for delay calculation
+    unsigned int countOffset; //iteration to count from for delay calculation
     bool scheduleReset;
+    bool scheduleAdapt;
 
     using NowFuncPtr = double (*)();
     using DelayFuncPtr = void (*)(double);
@@ -55,15 +56,17 @@ class yarp::os::PeriodicThread::Private : public ThreadImpl
         sumUsedSq = 0;
         sumTSq = 0;
         elapsed = 0;
+        refTime = nowFunc();
+        countOffset = 0;
         scheduleReset = false;
+        scheduleAdapt = false;
     }
 
 public:
     Private(PeriodicThread* owner, double p, ShouldUseSystemClock useSystemClock) :
             adaptedPeriod(p),
             owner(owner),
             elapsed(0),
-            sleepPeriod(adaptedPeriod),
             suspended(false),
             totalUsed(0),
             count(0),
@@ -73,7 +76,10 @@ class yarp::os::PeriodicThread::Private : public ThreadImpl
             sumUsedSq(0),
             previousRun(0),
             currentRun(0),
+            refTime(0),
+            countOffset(0),
             scheduleReset(false),
+            scheduleAdapt(false),
             nowFunc(useSystemClock == ShouldUseSystemClock::Yes ? SystemClock::nowSystem : yarp::os::Time::now),
             delayFunc(useSystemClock == ShouldUseSystemClock::Yes ? SystemClock::delaySystem : yarp::os::Time::delay)
     {
@@ -158,6 +164,12 @@ class yarp::os::PeriodicThread::Private : public ThreadImpl
         lock();
         currentRun = nowFunc();
 
+        if (scheduleAdapt) {
+            countOffset = count;
+            refTime = currentRun;
+            scheduleAdapt = false;
+        }
+
         if (scheduleReset) {
             _resetStat();
         }
@@ -192,19 +204,21 @@ class yarp::os::PeriodicThread::Private : public ThreadImpl
 
         lock();
         count++;
-        double elapsed = nowFunc() - currentRun;
+        double now = nowFunc();
+        double elapsed = now - currentRun;
+        double sleepPeriod = refTime + adaptedPeriod * (count - countOffset) - now;
         //save last
         totalUsed += elapsed;
         sumUsedSq += elapsed * elapsed;
         unlock();
 
-        sleepPeriod = adaptedPeriod - elapsed; // everything is in [seconds] except period, for it is used in the interface as [ms]
-
         delayFunc(sleepPeriod);
     }
 
     void run() override
     {
+        refTime = nowFunc();
+
         while (!isClosing()) {
             step();
         }
@@ -222,7 +236,10 @@ class yarp::os::PeriodicThread::Private : public ThreadImpl
 
     bool setPeriod(double period)
     {
+        lock();
         adaptedPeriod = period;
+        scheduleAdapt = true;
+        unlock();
         return true;
     }
 

tests/libYARP_os/PeriodicThreadTest.cpp

@@ -310,7 +310,8 @@ TEST_CASE("os::PeriodicThreadTest", "[yarp::os]")
     SECTION("testing rate thread precision")
     {
         bool success = false;
-        double acceptedThreshold = 0.10;
+        double acceptedThreshold = 0.001;
+        double duration = 1.0;
 
         char message[255];
 
@@ -319,38 +320,41 @@ TEST_CASE("os::PeriodicThreadTest", "[yarp::os]")
         desiredPeriod = 0.015;
         sprintf(message, "Thread1 requested period: %f[s]", desiredPeriod);
         INFO(message);
-        actualPeriod = test(desiredPeriod, 1);
-        if( (actualPeriod > (desiredPeriod*(1-acceptedThreshold))) && (actualPeriod < (desiredPeriod * (1+acceptedThreshold))) )
+        actualPeriod = test(desiredPeriod, duration);
+        if( (actualPeriod > (desiredPeriod*(duration-acceptedThreshold))) && (actualPeriod < (desiredPeriod * (duration+acceptedThreshold))) )
             success = true;
         sprintf(message, "Thread1 estimated period: %f[s]", actualPeriod);
         INFO(message);
         sprintf(message, "Period NOT within range of %d%%", (int)(acceptedThreshold*100));
         if(!success)
             WARN(message);
+        CHECK(success);
 
         desiredPeriod = 0.010;
         sprintf(message, "Thread2 requested period: %f[s]", desiredPeriod);
         INFO(message);
-        actualPeriod = test(desiredPeriod, 1);
-        if( (actualPeriod > (desiredPeriod*(1-acceptedThreshold))) && (actualPeriod < (desiredPeriod * (1+acceptedThreshold))) )
+        actualPeriod = test(desiredPeriod, duration);
+        if( (actualPeriod > (desiredPeriod*(duration-acceptedThreshold))) && (actualPeriod < (desiredPeriod * (duration+acceptedThreshold))) )
             success = true;
         sprintf(message, "Thread2 estimated period: %f[s]", actualPeriod);
         INFO(message);
         sprintf(message, "Period NOT within range of %d%%", (int)(acceptedThreshold*100));
         if(!success)
             WARN(message);
+        CHECK(success);
 
         desiredPeriod = 0.001;
         sprintf(message, "Thread3 requested period: %f[s]", desiredPeriod);
         INFO(message);
-        actualPeriod = test(desiredPeriod, 1);
-        if( (actualPeriod > (desiredPeriod*(1-acceptedThreshold))) && (actualPeriod < (desiredPeriod * (1+acceptedThreshold))) )
+        actualPeriod = test(desiredPeriod, duration);
+        if( (actualPeriod > (desiredPeriod*(duration-acceptedThreshold))) && (actualPeriod < (desiredPeriod * (duration+acceptedThreshold))) )
             success = true;
         sprintf(message, "Thread3 estimated period: %f[s]", actualPeriod);
         INFO(message);
         sprintf(message, "Period NOT within range of %d%%", (int)(acceptedThreshold*100));
         if(!success)
             WARN(message);
+        CHECK(success);
 
         INFO("Testing askToStop() function");
         PeriodicThread4 thread(0.010);