Improve RTF Stability and Precision (#3269)

* Fix delay calculation in each time step to achieve desired RTF
* Use a hybrid sleep/busy-loop for better precision
* Make Server_TEST more robust

---------

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>
(cherry picked from commit 6759b33)

Author: Addisu Z. Taddese

src/Server_TEST.cc

@@ -590,6 +590,9 @@ TEST_P(ServerFixture, RunNonBlockingPaused)
   while (*server.IterationCount() < 100)
     GZ_SLEEP_MS(100);
 
+  // Sleep one more time before checking because iterationCount might be updated
+  // before the iteration is complete
+  GZ_SLEEP_MS(100);
   EXPECT_EQ(100u, *server.IterationCount());
   EXPECT_FALSE(server.Running());
   EXPECT_FALSE(*server.Running(0));
@@ -610,6 +613,9 @@ TEST_P(ServerFixture, RunNonBlocking)
   while (*server.IterationCount() < 100)
     GZ_SLEEP_MS(100);
 
+  // Sleep one more time before checking because iterationCount might be updated
+  // before the iteration is complete
+  GZ_SLEEP_MS(100);
   EXPECT_EQ(100u, *server.IterationCount());
   EXPECT_FALSE(server.Running());
   EXPECT_FALSE(*server.Running(0));
@@ -763,6 +769,9 @@ TEST_P(ServerFixture, RunNonBlockingMultiple)
   while (*server.IterationCount() < 100)
     GZ_SLEEP_MS(100);
 
+  // Sleep one more time before checking because iterationCount might be updated
+  // before the iteration is complete
+  GZ_SLEEP_MS(100);
   EXPECT_EQ(100u, *server.IterationCount());
   EXPECT_FALSE(server.Running());
   EXPECT_FALSE(*server.Running(0));

src/SimulationRunner.cc

@@ -733,11 +733,6 @@ bool SimulationRunner::Run(const uint64_t _iterations)
   if (!this->currentInfo.paused)
     this->realTimeWatch.Start();
 
-  // Variables for time keeping.
-  std::chrono::steady_clock::time_point startTime;
-  std::chrono::steady_clock::duration sleepTime;
-  std::chrono::steady_clock::duration actualSleep;
-
   this->running = true;
 
   // Create the world statistics publisher.
@@ -844,6 +839,7 @@ bool SimulationRunner::Run(const uint64_t _iterations)
 
   // Execute all the systems until we are told to stop, or the number of
   // iterations is reached.
+  auto nextUpdateTime = std::chrono::steady_clock::now() + this->updatePeriod;
   while (this->running && (_iterations == 0 ||
        processedIterations < _iterations))
   {
@@ -859,32 +855,6 @@ bool SimulationRunner::Run(const uint64_t _iterations)
     // Update the step size and desired rtf
     this->UpdatePhysicsParams();
 
-    // Compute the time to sleep in order to match, as closely as possible,
-    // the update period.
-    sleepTime = 0ns;
-    actualSleep = 0ns;
-
-    sleepTime = std::max(0ns, this->prevUpdateRealTime +
-        this->updatePeriod - std::chrono::steady_clock::now() -
-        this->sleepOffset);
-
-    // Only sleep if needed.
-    if (sleepTime > 0ns)
-    {
-      GZ_PROFILE("Sleep");
-      // Get the current time, sleep for the duration needed to match the
-      // updatePeriod, and then record the actual time slept.
-      startTime = std::chrono::steady_clock::now();
-      std::this_thread::sleep_for(sleepTime);
-      actualSleep = std::chrono::steady_clock::now() - startTime;
-    }
-
-    // Exponentially average out the difference between expected sleep time
-    // and actual sleep time.
-    this->sleepOffset =
-      std::chrono::duration_cast<std::chrono::nanoseconds>(
-          (actualSleep - sleepTime) * 0.01 + this->sleepOffset * 0.99);
-
     // Update time information. This will update the iteration count, RTF,
     // and other values.
     this->UpdateCurrentInfo();
@@ -919,6 +889,59 @@ bool SimulationRunner::Run(const uint64_t _iterations)
     }
 
     this->resetInitiated = false;
+
+    // Only sleep when not paused.
+    if (!this->currentInfo.paused)
+    {
+      // A hybrid sleep/busy-wait strategy is used for precise timing. A simple
+      // sleep can suffer from wake-up latency due to CPU power-saving states
+      // (C-states), which causes RTF to undershoot. This strategy sleeps for
+      // long waits but busy-waits for the final moments to ensure precision.
+      // The threshold is a conservative value based on typical C-state
+      // latencies.
+      using namespace std::chrono_literals;
+
+      // Threshold at which we switch from sleeping to spinning. This should be
+      // larger than the typical OS + CPU C-state latency.
+      constexpr auto kSpinThreshold = 200us;
+
+      // If the scheduled update time is in the future...
+      if (nextUpdateTime > std::chrono::steady_clock::now())
+      {
+        // ...sleep until we are close to the target time.
+        auto sleepTarget = nextUpdateTime - kSpinThreshold;
+        if (sleepTarget > std::chrono::steady_clock::now())
+        {
+          std::this_thread::sleep_until(sleepTarget);
+        }
+
+        // ...then busy-wait for the final moments for precision.
+        while (std::chrono::steady_clock::now() < nextUpdateTime)
+        {
+          // Spin.
+        }
+      }
+
+      // Schedule the next update time.
+      auto now = std::chrono::steady_clock::now();
+      nextUpdateTime += this->updatePeriod;
+      if (nextUpdateTime < now)
+      {
+        nextUpdateTime = now + this->updatePeriod;
+      }
+    }
+    else
+    {
+      // We still need a small sleep to prevent this loop from spinning
+      // at 100% CPU when paused.
+      using namespace std::chrono_literals;
+      std::this_thread::sleep_for(1ms);
+
+      // When paused, pre-schedule the next update time for the near future.
+      // This ensures that when the simulation is un-paused, the first step
+      // has a valid schedule to meet, preventing a jump in RTF.
+      nextUpdateTime = std::chrono::steady_clock::now() + this->updatePeriod;
+    }
   }
 
   this->running = false;
@@ -939,9 +962,6 @@ void SimulationRunner::Step(const UpdateInfo &_info)
   // Publish info
   this->PublishStats();
 
-  // Record when the update step starts.
-  this->prevUpdateRealTime = std::chrono::steady_clock::now();
-
   this->levelMgr->UpdateLevelsState();
 
   // Handle pending systems

src/SimulationRunner.hh

@@ -448,13 +448,6 @@ namespace gz
       /// \brief Manager of distributing/receiving network work.
       private: std::unique_ptr<NetworkManager> networkMgr{nullptr};
 
-      /// \brief Wall time of the previous update.
-      private: std::chrono::steady_clock::time_point prevUpdateRealTime;
-
-      /// \brief A duration used to account for inaccuracies associated with
-      /// sleep durations.
-      private: std::chrono::steady_clock::duration sleepOffset{0};
-
       /// \brief This is the rate at which the systems are updated.
       /// The default update rate is 500hz, which is a period of 2ms.
       private: std::chrono::steady_clock::duration updatePeriod{2ms};