Pull out the sleeping logic from chronocontroller and test it works

src/extension/ChronoController.cpp

@@ -24,30 +24,26 @@
 namespace NUClear {
 namespace extension {
 
+    /// The precision threshold to swap from sleeping on the condition variable to sleeping with nanosleep
+    constexpr std::chrono::milliseconds precise_threshold = std::chrono::milliseconds(50);
+
+    /**
+     * Duration cast the given type to nanoseconds
+     *
+     * @tparam T the type to cast
+     *
+     * @param t the value to cast
+     *
+     * @return the time value in nanoseconds
+     */
+    template <typename T>
+    std::chrono::nanoseconds ns(T&& t) {
+        return std::chrono::duration_cast<std::chrono::nanoseconds>(std::forward<T>(t));
+    }
+
     ChronoController::ChronoController(std::unique_ptr<NUClear::Environment> environment)
         : Reactor(std::move(environment)) {
 
-        // Estimate the accuracy of our cv wait and precise sleep
-        for (int i = 0; i < 3; ++i) {
-            // Estimate the accuracy of our cv wait
-            std::mutex test;
-            std::unique_lock<std::mutex> lock(test);
-            const auto cv_s = NUClear::clock::now();
-            wait.wait_for(lock, std::chrono::milliseconds(1));
-            const auto cv_e = NUClear::clock::now();
-            const auto cv_a = NUClear::clock::duration(cv_e - cv_s - std::chrono::milliseconds(1));
-
-            // Estimate the accuracy of our precise sleep
-            const auto ns_s = NUClear::clock::now();
-            util::precise_sleep(std::chrono::milliseconds(1));
-            const auto ns_e = NUClear::clock::now();
-            const auto ns_a = NUClear::clock::duration(ns_e - ns_s - std::chrono::milliseconds(1));
-
-            // Use the largest time we have seen
-            cv_accuracy = cv_a > cv_accuracy ? cv_a : cv_accuracy;
-            ns_accuracy = ns_a > ns_accuracy ? ns_a : ns_accuracy;
-        }
-
         on<Trigger<ChronoTask>>().then("Add Chrono task", [this](const std::shared_ptr<const ChronoTask>& task) {
             // Lock the mutex while we're doing stuff
             const std::lock_guard<std::mutex> lock(mutex);
@@ -131,6 +127,7 @@ namespace extension {
                     auto start  = NUClear::clock::now();
                     auto target = tasks.front().time;
 
+                    // Run the task if we are at or past the target time
                     if (target <= start) {
                         // Run our task and if it returns false remove it
                         const bool renew = tasks.front()();
@@ -147,43 +144,23 @@ namespace extension {
                             tasks.pop_back();
                         }
                     }
+                    // Wait if we are not at the target time
                     else {
-                        const NUClear::clock::duration time_until_task =
-                            std::chrono::duration_cast<NUClear::clock::duration>((target - start) / clock::rtf());
+                        // Calculate the real time to sleep given the rate at which time passes
+                        const auto time_until_task = ns((target - start) / clock::rtf());
 
                         if (clock::rtf() == 0.0) {
                             // If we are paused then just wait until we are unpaused
                             wait.wait(lock, [&] {
                                 return !running || clock::rtf() != 0.0 || NUClear::clock::now() != start;
                             });
                         }
-                        else if (time_until_task > cv_accuracy) {  // A long time in the future
+                        else if (time_until_task > precise_threshold) {  // A long time in the future
                             // Wait on the cv
-                            wait.wait_for(lock, time_until_task - cv_accuracy);
-
-                            // Update the accuracy of our cv wait
-                            const auto end   = NUClear::clock::now();
-                            const auto error = end - (target - cv_accuracy);  // when ended - when wanted to end
-                            if (error.count() > 0) {                          // only if we were late
-                                cv_accuracy = error > cv_accuracy ? error : ((cv_accuracy * 99 + error) / 100);
-                            }
+                            wait.wait_for(lock, time_until_task - precise_threshold);
                         }
-                        else if (time_until_task > ns_accuracy) {  // Somewhat close in time
-                            // Wait on nanosleep
-                            const NUClear::clock::duration sleep_time = time_until_task - ns_accuracy;
-                            util::precise_sleep(sleep_time);
-
-                            // Update the accuracy of our precise sleep
-                            const auto end   = NUClear::clock::now();
-                            const auto error = end - (target - ns_accuracy);  // when ended - when wanted to end
-                            if (error.count() > 0) {                          // only if we were late
-                                ns_accuracy = error > ns_accuracy ? error : ((ns_accuracy * 99 + error) / 100);
-                            }
-                        }
-                        else {
-                            while (NUClear::clock::now() < tasks.front().time) {
-                                // Spinlock until we get to the time
-                            }
+                        else {  // Within precise sleep threshold
+                            sleeper.sleep_for(time_until_task);
                         }
                     }
                 }

src/extension/ChronoController.hpp

@@ -26,7 +26,7 @@
 #include "../PowerPlant.hpp"
 #include "../Reactor.hpp"
 #include "../message/TimeTravel.hpp"
-#include "../util/precise_sleep.hpp"
+#include "../util/Sleeper.hpp"
 
 namespace NUClear {
 namespace extension {
@@ -48,10 +48,8 @@ namespace extension {
         /// If we are running or not
         bool running = true;
 
-        /// The temporal accuracy when waiting on a condition variable
-        NUClear::clock::duration cv_accuracy{0};
-        /// The temporal accuracy when waiting on nanosleep
-        NUClear::clock::duration ns_accuracy{0};
+        /// The class which is able to perform high precision sleeps
+        util::Sleeper sleeper;
     };
 
 }  // namespace extension

src/util/Sleeper.cpp

@@ -0,0 +1,136 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2023 NUClear Contributors
+ *
+ * This file is part of the NUClear codebase.
+ * See https://github.com/Fastcode/NUClear for further info.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated
+ * documentation files (the "Software"), to deal in the Software without restriction, including without limitation the
+ * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
+ * WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
+ * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
+ * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include "Sleeper.hpp"
+
+#include <chrono>
+
+#if defined(_WIN32)
+
+    #include <chrono>
+    #include <cstdint>
+
+    #include "platform.hpp"
+
+namespace NUClear {
+namespace util {
+
+    // Windows requires a waitable timer to sleep for a precise amount of time
+    class SleeperImpl {
+    public:
+        SleeperImpl() : timer(::CreateWaitableTimer(nullptr, TRUE, nullptr)) {}
+        ::HANDLE timer;
+    };
+
+    Sleeper::~Sleeper() {
+        ::CloseHandle(sleeper->timer);
+    }
+
+    void Sleeper::idle_sleep(const std::chrono::nanoseconds& ns) {
+        ::LARGE_INTEGER ft;
+        // TODO if ns is negative make it 0 as otherwise it'll become absolute time
+        // Negative for relative time, positive for absolute time
+        // Measures in 100ns increments so divide by 100
+        ft.QuadPart = -static_cast<int64_t>(ns.count() / 100);
+
+        ::SetWaitableTimer(impl->timer, &ft, 0, nullptr, nullptr, 0);
+        ::WaitForSingleObject(impl->timer, INFINITE);
+    }
+
+}  // namespace util
+}  // namespace NUClear
+
+#else
+
+    #include <cerrno>
+    #include <cstdint>
+    #include <ctime>
+
+namespace NUClear {
+namespace util {
+
+    // No specific implementation for precise sleep on linux
+    class SleeperImpl {};
+
+    // Sleep using nanosleep on linux
+    void Sleeper::idle_sleep(const std::chrono::nanoseconds& ns) {
+        if (ns <= std::chrono::nanoseconds(0)) {
+            return;
+        }
+        timespec ts{};
+        ts.tv_sec  = std::chrono::duration_cast<std::chrono::seconds>(ns).count();
+        ts.tv_nsec = (ns - std::chrono::seconds(ts.tv_sec)).count();
+
+        while (::nanosleep(&ts, &ts) == -1 && errno == EINTR) {
+        }
+    }
+
+}  // namespace util
+}  // namespace NUClear
+
+#endif
+
+namespace NUClear {
+namespace util {
+
+    Sleeper::Sleeper() : sleeper(std::make_unique<SleeperImpl>()) {}
+
+    // This must be in the .cpp file as we need the full definition of SleeperImpl
+    Sleeper::~Sleeper()                    = default;
+    Sleeper::Sleeper(Sleeper&&)            = default;
+    Sleeper& Sleeper::operator=(Sleeper&&) = default;
+
+    void NUClear::util::Sleeper::sleep_for(const std::chrono::nanoseconds& duration) {
+        sleep_until(std::chrono::steady_clock::now() + duration);
+    }
+
+    void NUClear::util::Sleeper::sleep_until(const std::chrono::steady_clock::time_point& target) {
+        using namespace std::chrono;
+
+        for (auto start = std::chrono::steady_clock::now(); start < target; start = std::chrono::steady_clock::now()) {
+            // If we can accurately sleep for the target amount of time then do so
+            if (target - start >= sleep_accuracy) {
+                // Sleep as accurately as we think we can
+                auto target_sleep_time = target - start - sleep_accuracy;
+                idle_sleep(target_sleep_time);
+                auto end = std::chrono::steady_clock::now();
+
+                // Update the idle sleep accuracy estimate using Welford's method
+                auto actual_sleep_time = end - start;
+
+                double sleep_error =
+                    duration_cast<duration<double, std::nano>>(actual_sleep_time - target_sleep_time).count();
+                double delta = sleep_error - mean;
+
+                count         = count + 1;
+                mean          = mean + (delta / count);
+                double delta2 = sleep_error - mean;
+                m2            = m2 + delta * delta2;
+
+                // Sleep accuracy with 3 standard deviations of the mean for a 99.7% confidence interval
+                sleep_accuracy = nanoseconds(std::lround(std::sqrt(m2 / count) * 3.0));
+            }
+        }
+    }
+
+}  // namespace util
+}  // namespace NUClear

src/util/Sleeper.hpp

@@ -0,0 +1,88 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2014 NUClear Contributors
+ *
+ * This file is part of the NUClear codebase.
+ * See https://github.com/Fastcode/NUClear for further info.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated
+ * documentation files (the "Software"), to deal in the Software without restriction, including without limitation the
+ * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
+ * WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
+ * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
+ * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef NUCLEAR_UTIL_SLEEPER_HPP
+#define NUCLEAR_UTIL_SLEEPER_HPP
+
+#include <chrono>
+
+namespace NUClear {
+namespace util {
+
+    class SleeperImpl;
+
+    /**
+     * A class that provides platform independent precise sleep functionality.
+     *
+     * The Sleeper class allows for sleeping for a specified duration of time.
+     * It will use the most accurate method available on the platform to sleep for the specified duration.
+     * It will then spin the CPU for the remaining time to ensure that the sleep is as accurate as possible.
+     */
+    class Sleeper {
+    public:
+        Sleeper();
+        ~Sleeper();
+        Sleeper(Sleeper&&);
+        Sleeper& operator=(Sleeper&&);
+
+        // No copying due to the unique_ptr
+        Sleeper(const Sleeper&)            = delete;
+        Sleeper& operator=(const Sleeper&) = delete;
+
+        /**
+         * Sleep for the specified duration.
+         *
+         * @param duration The duration to sleep for.
+         */
+        void sleep_for(const std::chrono::nanoseconds& duration);
+
+        /**
+         * Sleep until the specified time point.
+         *
+         * @param target The time point to sleep until.
+         */
+        void sleep_until(const std::chrono::steady_clock::time_point& target);
+
+    private:
+        /**
+         * Sleeps by putting the thread to sleep for the specified duration.
+         *
+         * @param ns The duration to sleep for.
+         */
+        void idle_sleep(const std::chrono::nanoseconds& ns);
+
+        /// The platform specific implementation of the Sleeper.
+        std::unique_ptr<SleeperImpl> sleeper;
+
+        /// Welfords method for calculating the mean and variance of the sleep function.
+        int count   = 0;
+        double mean = 0;
+        double m2   = 0;
+
+        /// The estimated accuracy of the sleep function.
+        std::chrono::nanoseconds sleep_accuracy{0};
+    };
+
+}  // namespace util
+}  // namespace NUClear
+
+#endif  // NUCLEAR_UTIL_SLEEPER_HPP