diff --git a/src/extension/ChronoController.cpp b/src/extension/ChronoController.cpp index 1a67edbf9..970eedf8f 100644 --- a/src/extension/ChronoController.cpp +++ b/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 + std::chrono::nanoseconds ns(T&& t) { + return std::chrono::duration_cast(std::forward(t)); + } + ChronoController::ChronoController(std::unique_ptr 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 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>().then("Add Chrono task", [this](const std::shared_ptr& task) { // Lock the mutex while we're doing stuff const std::lock_guard 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,9 +144,10 @@ 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((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 @@ -157,33 +155,12 @@ namespace extension { 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); } } } diff --git a/src/extension/ChronoController.hpp b/src/extension/ChronoController.hpp index cb97ff2b0..1ccb81d43 100644 --- a/src/extension/ChronoController.hpp +++ b/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 diff --git a/src/util/Sleeper.cpp b/src/util/Sleeper.cpp new file mode 100644 index 000000000..64ad8ace4 --- /dev/null +++ b/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 + +#if defined(_WIN32) + + #include + #include + + #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(ns.count() / 100); + + ::SetWaitableTimer(impl->timer, &ft, 0, nullptr, nullptr, 0); + ::WaitForSingleObject(impl->timer, INFINITE); + } + +} // namespace util +} // namespace NUClear + +#else + + #include + #include + #include + +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(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()) {} + + // 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>(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 diff --git a/src/util/Sleeper.hpp b/src/util/Sleeper.hpp new file mode 100644 index 000000000..5e414942b --- /dev/null +++ b/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 + +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 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 diff --git a/src/util/precise_sleep.cpp b/src/util/precise_sleep.cpp deleted file mode 100644 index 27559f6fc..000000000 --- a/src/util/precise_sleep.cpp +++ /dev/null @@ -1,74 +0,0 @@ -/* - * 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 "precise_sleep.hpp" - -#include - -#if defined(_WIN32) - - #include - #include - - #include "platform.hpp" - -namespace NUClear { -namespace util { - - void precise_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(ns.count() / 100); - - ::HANDLE timer = ::CreateWaitableTimer(nullptr, TRUE, nullptr); - ::SetWaitableTimer(timer, &ft, 0, nullptr, nullptr, 0); - ::WaitForSingleObject(timer, INFINITE); - ::CloseHandle(timer); - } - -} // namespace util -} // namespace NUClear - -#else - - #include - #include - #include - -namespace NUClear { -namespace util { - - void precise_sleep(const std::chrono::nanoseconds& ns) { - timespec ts{}; - ts.tv_sec = std::chrono::duration_cast(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 diff --git a/src/util/precise_sleep.hpp b/src/util/precise_sleep.hpp deleted file mode 100644 index fe19fbbb7..000000000 --- a/src/util/precise_sleep.hpp +++ /dev/null @@ -1,36 +0,0 @@ -/* - * 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 - -namespace NUClear { -namespace util { - - void precise_sleep(const std::chrono::nanoseconds& ns); - -} // namespace util -} // namespace NUClear - -#endif // NUCLEAR_UTIL_SLEEPER_HPP diff --git a/tests/tests/util/Sleeper.cpp b/tests/tests/util/Sleeper.cpp new file mode 100644 index 000000000..cf16ddd23 --- /dev/null +++ b/tests/tests/util/Sleeper.cpp @@ -0,0 +1,68 @@ +#include "util/Sleeper.hpp" + +#include +#include + +#include "util/update_current_thread_priority.hpp" + +namespace NUClear { +namespace util { + + constexpr std::chrono::milliseconds max_error{2}; + + SCENARIO("Sleeper provides precise sleep functionality", "[Sleeper]") { + + /// Set the priority to maximum to enable realtime to make more accurate sleeps + update_current_thread_priority(1000); + + GIVEN("A Sleeper object") { + Sleeper sleeper; + // Sleep a few times to seed the sleep accuracy + for (int i = 0; i < 100; ++i) { + sleeper.sleep_for(std::chrono::milliseconds(1)); + } + + // Sleep for a negative duration, 0, 10, and 20 milliseconds + int sleep_ms = GENERATE(-10, 0, 10, 20); + + WHEN("Sleeping for a specified duration") { + auto sleep_duration = std::chrono::milliseconds(sleep_ms); + auto expected_duration = std::chrono::milliseconds(std::max(0, sleep_ms)); + + auto start_time = std::chrono::steady_clock::now(); + + sleeper.sleep_for(sleep_duration); + + auto end_time = std::chrono::steady_clock::now(); + auto actual_duration = std::chrono::duration_cast(end_time - start_time); + + THEN("The sleep duration should be close to the specified duration") { + REQUIRE(actual_duration >= expected_duration); + REQUIRE(actual_duration <= expected_duration + max_error); + } + } + + WHEN("Sleeping until a specific time point") { + auto sleep_duration = std::chrono::milliseconds(sleep_ms); + auto expected_duration = std::chrono::milliseconds(std::max(0, sleep_ms)); + + auto start_time = std::chrono::steady_clock::now(); + auto target_time = start_time + sleep_duration; + auto expected_time = start_time + expected_duration; + + sleeper.sleep_until(target_time); + + auto end_time = std::chrono::steady_clock::now(); + + THEN("The wake-up time should be close to the target time") { + auto delta_ns = std::chrono::duration_cast(end_time - expected_time); + CAPTURE(delta_ns); + REQUIRE(end_time >= expected_time); + REQUIRE(end_time <= expected_time + max_error); // Allow for small discrepancies + } + } + } + } + +} // namespace util +} // namespace NUClear