Large Refactor Branch

.vscode/settings.json

@@ -12,8 +12,12 @@
   "editor.rulers": [120],
   "cmake.configureOnOpen": true,
   "C_Cpp.default.configurationProvider": "vector-of-bool.cmake-tools",
+  "files.associations": {
+    "*.ipp": "cpp"
+  },
   "sonarlint.connectedMode.project": {
     "connectionId": "fastcode",
     "projectKey": "Fastcode_NUClear"
-  }
+  },
+  "cSpell.language": "en-GB"
 }

src/extension/ChronoController.cpp

@@ -22,43 +22,35 @@
 
 #include "ChronoController.hpp"
 
-#include <atomic>
-
-#include "../util/precise_sleep.hpp"
-
 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);
 
             // Add our new task to the heap if we are still running
-            if (running.load(std::memory_order_acquire)) {
+            if (running) {
                 tasks.push_back(*task);
                 std::push_heap(tasks.begin(), tasks.end(), std::greater<>());
             }
@@ -91,7 +83,7 @@ namespace extension {
         // When we shutdown we notify so we quit now
         on<Shutdown>().then("Shutdown Chrono Controller", [this] {
             const std::lock_guard<std::mutex> lock(mutex);
-            running.store(false, std::memory_order_release);
+            running = false;
             wait.notify_all();
         });
 
@@ -123,19 +115,20 @@ namespace extension {
 
         on<Always, Priority::REALTIME>().then("Chrono Controller", [this] {
             // Run until we are told to stop
-            while (running.load(std::memory_order_acquire)) {
+            while (running) {
 
                 // Acquire the mutex lock so we can wait on it
                 std::unique_lock<std::mutex> lock(mutex);
 
                 // If we have no chrono tasks wait until we are notified
                 if (tasks.empty()) {
-                    wait.wait(lock, [this] { return !running.load(std::memory_order_acquire) || !tasks.empty(); });
+                    wait.wait(lock, [this] { return !running || !tasks.empty(); });
                 }
                 else {
                     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()();
@@ -152,44 +145,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.load(std::memory_order_acquire) || clock::rtf() != 0.0
-                                       || NUClear::clock::now() != start;
+                                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);
-                            }
-                        }
-                        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);
-                            }
+                            wait.wait_for(lock, time_until_task - precise_threshold);
                         }
-                        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

@@ -25,6 +25,7 @@
 
 #include "../Reactor.hpp"
 #include "../message/TimeTravel.hpp"
+#include "../util/Sleeper.hpp"
 
 namespace NUClear {
 namespace extension {
@@ -37,19 +38,17 @@ namespace extension {
         explicit ChronoController(std::unique_ptr<NUClear::Environment> environment);
 
     private:
+        /// The mutex used to guard the tasks and running flag
+        std::mutex mutex;
         /// The list of tasks we need to process
         std::vector<dsl::operation::ChronoTask> tasks;
-        /// The mutex we use to lock the task list
-        std::mutex mutex;
         /// The condition variable we use to wait on
         std::condition_variable wait;
         /// If we are running or not
-        std::atomic<bool> running{true};
+        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,139 @@
+/*
+ * 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>
+#include <cmath>
+
+#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
+    // This can't be static because the windows version can't be static
+    // NOLINTNEXTLINE(readability-convert-member-functions-to-static)
+    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&&) noexcept            = default;
+    Sleeper& Sleeper::operator=(Sleeper&&) noexcept = 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;  // NOLINT(google-build-using-namespace) fine in function scope
+
+        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
+                const auto actual_sleep_time = end - start;
+
+                const double sleep_error =
+                    duration_cast<duration<double, std::nano>>(actual_sleep_time - target_sleep_time).count();
+                const double delta = sleep_error - mean;
+
+                count               = count + 1;
+                mean                = mean + (delta / count);
+                const 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