Improve ChronoController to use accurate OS timers with interruptibility

src/dsl/operation/ChronoTask.hpp

@@ -58,7 +58,7 @@ namespace dsl {
              *
              * @return `true` if the task updated the time to run to a new time
              */
-            bool operator()() {
+            bool run() {
                 return task(time);
             }
 
@@ -76,7 +76,8 @@ namespace dsl {
             /**
              * Compares tasks in order of soonest to execute first.
              *
-             * @param other The other task to compare to
+             * @param lhs The first task to compare
+             * @param rhs The second task to compare
              *
              * @return `true` if the other task is before this task
              */
@@ -85,14 +86,15 @@ namespace dsl {
             }
 
             /**
-             * Check if tasks share the same execution time.
+             * Compares tasks in order of soonest to execute first.
              *
-             * @param other The other task to compare to
+             * @param lhs The first task to compare
+             * @param rhs The second task to compare
              *
-             * @return `true` if the other task is at the same time as this task
+             * @return `true` if the other task is after this task
              */
-            friend bool operator==(const ChronoTask& lhs, const ChronoTask& rhs) {
-                return lhs.time == rhs.time;
+            friend bool operator<(const std::shared_ptr<ChronoTask>& lhs, const std::shared_ptr<ChronoTask>& rhs) {
+                return *lhs < *rhs;
             }
 
             /// The task function, takes the time as a reference so it can be updated for multiple runs

src/extension/ChronoController.cpp

@@ -22,77 +22,94 @@
 
 #include "ChronoController.hpp"
 
-#include <atomic>
-
-#include "../util/precise_sleep.hpp"
-
 namespace NUClear {
 namespace extension {
 
-    ChronoController::ChronoController(std::unique_ptr<NUClear::Environment> environment)
-        : Reactor(std::move(environment)) {
+    /**
+     * 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));
+    }
 
-        // 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;
+    void ChronoController::add(const std::shared_ptr<dsl::operation::ChronoTask>& task) {
+        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)) {
+            tasks.push_back(task);
+            std::push_heap(tasks.begin(), tasks.end(), std::greater<>());
         }
+    }
 
-        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);
+    void ChronoController::remove(const NUClear::id_t& id) {
+        const std::lock_guard<std::mutex> lock(mutex);
+
+        // Find the task
+        auto it = std::find_if(tasks.begin(), tasks.end(), [&](const auto& task) { return task->id == id; });
+
+        // Remove if it exists
+        if (it != tasks.end()) {
+            tasks.erase(it);
+            std::make_heap(tasks.begin(), tasks.end(), std::greater<>());
+        }
+
+        // Poke the system to make sure it's not waiting on something that's gone
+        sleeper.wake();
+    }
+
+    NUClear::clock::time_point ChronoController::next() {
+        const std::lock_guard<std::mutex> lock(mutex);
+
+        // If we have no tasks return a nullptr
+        if (tasks.empty()) {
+            return NUClear::clock::time_point::max();
+        }
+
+        auto target = tasks.front()->time;
+        auto now    = NUClear::clock::now();
 
-            // Add our new task to the heap if we are still running
-            if (running.load(std::memory_order_acquire)) {
-                tasks.push_back(*task);
+        // Run the task if we are at or past the target time
+        if (target <= now) {
+            auto task        = tasks.front();
+            const bool renew = task->run();
+            std::pop_heap(tasks.begin(), tasks.end(), std::greater<>());
+
+            if (renew) {
                 std::push_heap(tasks.begin(), tasks.end(), std::greater<>());
             }
+            else {
+                tasks.pop_back();
+            }
+        }
 
-            // Poke the system
-            wait.notify_all();
-        });
+        return target;
+    }
 
-        on<Trigger<dsl::operation::Unbind<ChronoTask>>>().then(
-            "Unbind Chrono Task",
-            [this](const dsl::operation::Unbind<ChronoTask>& unbind) {
-                // Lock the mutex while we're doing stuff
-                const std::lock_guard<std::mutex> lock(mutex);
-
-                // Find the task
-                auto it = std::find_if(tasks.begin(), tasks.end(), [&](const ChronoTask& task) {
-                    return task.id == unbind.id;
-                });
-
-                // Remove if it exists
-                if (it != tasks.end()) {
-                    tasks.erase(it);
-                    std::make_heap(tasks.begin(), tasks.end(), std::greater<>());
-                }
+    ChronoController::ChronoController(std::unique_ptr<NUClear::Environment> environment)
+        : Reactor(std::move(environment)) {
 
-                // Poke the system to make sure it's not waiting on something that's gone
-                wait.notify_all();
-            });
+        on<Trigger<ChronoTask>>().then("Add Chrono task", [this](const std::shared_ptr<const ChronoTask>& task) {
+            add(std::const_pointer_cast<ChronoTask>(task));
+            sleeper.wake();
+        });
+
+        on<Trigger<Unbind>>().then("Unbind Chrono Task", [this](const Unbind& unbind) {
+            remove(unbind.id);
+            sleeper.wake();
+        });
 
         // When we shutdown we notify so we quit now
         on<Shutdown>().then("Shutdown Chrono Controller", [this] {
             running.store(false, std::memory_order_release);
-            const std::lock_guard<std::mutex> lock(mutex);
-            wait.notify_all();
+            sleeper.wake();
         });
 
         on<Trigger<message::TimeTravel>>().then("Time Travel", [this](const message::TimeTravel& travel) {
@@ -105,93 +122,39 @@ namespace extension {
                     auto adjustment = travel.target - NUClear::clock::now();
                     clock::set_clock(travel.target, travel.rtf);
                     for (auto& task : tasks) {
-                        task.time += adjustment;
+                        task->time += adjustment;
                     }
 
                 } break;
                 case message::TimeTravel::Action::NEAREST: {
                     const clock::time_point nearest =
                         tasks.empty() ? travel.target
-                                      : std::min(travel.target, std::min_element(tasks.begin(), tasks.end())->time);
+                                      : std::min(travel.target, (*std::min_element(tasks.begin(), tasks.end()))->time);
                     clock::set_clock(nearest, travel.rtf);
                 } break;
             }
 
             // Poke the system
-            wait.notify_all();
+            sleeper.wake();
         });
 
         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);
+                // Run the next task and get the target time to wait until
+                auto target = next();
 
-                // 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(); });
-                }
-                else {
-                    auto start  = NUClear::clock::now();
-                    auto target = tasks.front().time;
-
-                    if (target <= start) {
-                        // Run our task and if it returns false remove it
-                        const bool renew = tasks.front()();
-
-                        // Move this to the back of the list
-                        std::pop_heap(tasks.begin(), tasks.end(), std::greater<>());
-
-                        if (renew) {
-                            // Put the item back in the list
-                            std::push_heap(tasks.begin(), tasks.end(), std::greater<>());
-                        }
-                        else {
-                            // Remove the item from the list
-                            tasks.pop_back();
-                        }
-                    }
-                    else {
-                        const NUClear::clock::duration time_until_task =
-                            std::chrono::duration_cast<NUClear::clock::duration>((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;
-                            });
-                        }
-                        else if (time_until_task > cv_accuracy) {  // 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);
-                            }
-                        }
-                        else {
-                            while (NUClear::clock::now() < tasks.front().time) {
-                                // Spinlock until we get to the time
-                            }
-                        }
-                    }
+                // Wait until the next task or we are woken
+                auto now = NUClear::clock::now();
+                if (target > now) {
+                    // Calculate the real time to sleep given the rate at which time passes
+                    const NUClear::clock::duration nuclear_sleep_time = target - now;
+                    const auto time_until_task =
+                        clock::rtf() == 0.0 ? std::chrono::steady_clock::time_point::max()
+                                            : std::chrono::steady_clock::now() + ns(nuclear_sleep_time / clock::rtf());
+
+                    sleeper.sleep_until(time_until_task);
                 }
             }
         });

src/extension/ChronoController.hpp

@@ -23,33 +23,55 @@
 #ifndef NUCLEAR_EXTENSION_CHRONO_CONTROLLER_HPP
 #define NUCLEAR_EXTENSION_CHRONO_CONTROLLER_HPP
 
+#include <memory>
+
 #include "../Reactor.hpp"
 #include "../message/TimeTravel.hpp"
+#include "../util/Sleeper.hpp"
 
 namespace NUClear {
 namespace extension {
 
     class ChronoController : public Reactor {
     private:
         using ChronoTask = NUClear::dsl::operation::ChronoTask;
+        using Unbind     = NUClear::dsl::operation::Unbind<ChronoTask>;
 
     public:
         explicit ChronoController(std::unique_ptr<NUClear::Environment> environment);
 
     private:
-        /// The list of tasks we need to process
-        std::vector<dsl::operation::ChronoTask> tasks;
-        /// The mutex we use to lock the task list
+        /**
+         * Add a new task to the ChronoController.
+         *
+         * @param task The task to add
+         */
+        void add(const std::shared_ptr<ChronoTask>& task);
+
+        /**
+         * Remove a task from the ChronoController.
+         *
+         * @param id The id of the task to remove
+         */
+        void remove(const NUClear::id_t& id);
+
+        /**
+         * Try to run the next task in the list
+         *
+         * @return the time point of the next task to run or max if there are no tasks
+         */
+        NUClear::clock::time_point next();
+
+        /// The mutex used to guard the tasks and running flag
         std::mutex mutex;
-        /// The condition variable we use to wait on
-        std::condition_variable wait;
+        /// The list of tasks we need to process
+        std::vector<std::shared_ptr<dsl::operation::ChronoTask>> tasks;
+
         /// If we are running or not
         std::atomic<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/precise_sleep.hpp → src/util/Sleeper.cpp

@@ -1,7 +1,7 @@
 /*
  * MIT License
  *
- * Copyright (c) 2014 NUClear Contributors
+ * Copyright (c) 2023 NUClear Contributors
  *
  * This file is part of the NUClear codebase.
  * See https://github.com/Fastcode/NUClear for further info.
@@ -20,17 +20,10 @@
  * 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 "Sleeper.hpp"
 
-#include <chrono>
-
-namespace NUClear {
-namespace util {
-
-    void precise_sleep(const std::chrono::nanoseconds& ns);
-
-}  // namespace util
-}  // namespace NUClear
-
-#endif  // NUCLEAR_UTIL_SLEEPER_HPP
+#if defined(_WIN32)
+    #include "SleeperWindows.ipp"
+#else
+    #include "SleeperPosix.ipp"
+#endif