diff --git a/src/clock.hpp b/src/clock.hpp index 8db78d417..9526f6450 100644 --- a/src/clock.hpp +++ b/src/clock.hpp @@ -23,36 +23,131 @@ #ifndef NUCLEAR_CLOCK_HPP #define NUCLEAR_CLOCK_HPP +// Default to using the system clock but allow it to be overridden by the user +#ifndef NUCLEAR_CLOCK_TYPE + #define NUCLEAR_CLOCK_TYPE std::chrono::system_clock +#endif // NUCLEAR_CLOCK_TYPE + +#include +#include #include +#include namespace NUClear { -#ifdef NUCLEAR_CLOCK_TYPE -/// @brief The custom base clock that is used when defining the NUClear clock -using base_clock = NUCLEAR_CLOCK_TYPE; -#else -/// @brief The default base clock that is used when defining the NUClear clock -using base_clock = std::chrono::steady_clock; -#endif // NUCLEAR_CLOCK_TYPE +/** + * @brief A clock class that extends a base clock type and allows for clock adjustment and setting. + */ +template +struct nuclear_clock : public NUCLEAR_CLOCK_TYPE { + using base_clock = NUCLEAR_CLOCK_TYPE; + + /** + * @brief Get the current time of the clock. + * @return The current time of the clock. + */ + static time_point now() { + const ClockData current = data[active.load()]; // Take a copy in case it changes + return current.epoch + dc((base_clock::now() - current.base_from) * current.rtf); + } + + /** + * @brief Adjust the clock by a specified duration and real-time factor. + * @param adjustment The duration by which to adjust the clock. + * @param rtf The real-time factor to apply to the clock. + */ + static void adjust_clock(const duration& adjustment, const double& rtf = 1.0) { + const std::lock_guard lock(mutex); + // Load the current state + const auto& current = data[active.load()]; + const int n = static_cast((active.load() + 1) % data.size()); + auto& next = data[n]; + + // Perform the update + auto base = base_clock::now(); + next.epoch = current.epoch + adjustment + dc((base - current.base_from) * current.rtf); + next.base_from = base; + next.rtf = rtf; + active = n; + } -#ifndef NUCLEAR_CUSTOM_CLOCK + /** + * @brief Set the clock to a specified time and real-time factor. + * @param time The time to set the clock to. + * @param rtf The real-time factor to apply to the clock. + */ + static void set_clock(const time_point& time, const double& rtf = 1.0) { + const std::lock_guard lock(mutex); + // Load the current state + const int n = static_cast((active.load() + 1) % data.size()); + auto& next = data[n]; -/// @brief The clock that is used throughout the entire nuclear system -using clock = base_clock; + // Perform the update + auto base = base_clock::now(); + next.epoch = time; + next.base_from = base; + next.rtf = rtf; + active = n; + } -#else -struct clock { - using rep = base_clock::rep; - using period = base_clock::period; - using duration = base_clock::duration; - using time_point = base_clock::time_point; - static constexpr bool is_steady = false; + /** + * @brief Get the real-time factor of the clock. + * @return The real-time factor of the clock. + */ + static double rtf() { + return data[active.load()].rtf; + } - static time_point now(); +private: + /** + * @brief Convert a duration to the clock's duration type. + * @tparam T The type of the duration. + * @param t The duration to convert. + * @return The converted duration. + */ + template + duration static dc(const T& t) { + return std::chrono::duration_cast(t); + } + + /** + * @brief Data structure to hold clock information. + */ + struct ClockData { + /// When the clock was last updated under the true time + time_point base_from = base_clock::now(); + /// Our calculated time when the clock was last updated in simulated time + time_point epoch = base_from; + /// The real time factor of the simulated clock + double rtf = 1.0; + + ClockData() = default; + }; + + /// @brief The mutex to protect the clock data. + static std::mutex mutex; // NOLINT(cppcoreguidelines-avoid-non-const-global-variables) + + /// @brief The clock data for the system. + static std::array data; // NOLINT(cppcoreguidelines-avoid-non-const-global-variables) + + /// @brief The active clock data index. + static std::atomic active; // NOLINT(cppcoreguidelines-avoid-non-const-global-variables) }; -#endif +template +// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) +std::mutex nuclear_clock::mutex; +template +// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) +std::array::ClockData, 3> nuclear_clock::data = + std::array::ClockData, 3>{}; +template +// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) +std::atomic nuclear_clock::active{0}; + +using clock = nuclear_clock<>; + } // namespace NUClear diff --git a/src/extension/ChronoController.hpp b/src/extension/ChronoController.hpp index 50ff94fb8..d39ea32fe 100644 --- a/src/extension/ChronoController.hpp +++ b/src/extension/ChronoController.hpp @@ -25,6 +25,7 @@ #include "../PowerPlant.hpp" #include "../Reactor.hpp" +#include "../message/TimeTravel.hpp" #include "../util/precise_sleep.hpp" namespace NUClear { @@ -100,6 +101,33 @@ namespace extension { wait.notify_all(); }); + on>().then("Time Travel", [this](const message::TimeTravel& travel) { + const std::lock_guard lock(mutex); + + // Adjust clock to target time and leave chrono tasks where they are + switch (travel.type) { + case message::TimeTravel::Action::ABSOLUTE: clock::set_clock(travel.target, travel.rtf); break; + case message::TimeTravel::Action::RELATIVE: { + auto adjustment = travel.target - NUClear::clock::now(); + clock::set_clock(travel.target, travel.rtf); + for (auto& task : tasks) { + task.time += adjustment; + } + + } break; + case message::TimeTravel::Action::NEAREST: { + auto next_task = + std::min_element(tasks.begin(), tasks.end(), [](const ChronoTask& a, const ChronoTask& b) { + return a.time < b.time; + }); + clock::set_clock(std::min(next_task->time, travel.target), travel.rtf); + } break; + } + + // Poke the system + wait.notify_all(); + }); + on().then("Chrono Controller", [this] { // Run until we are told to stop while (running.load()) { @@ -115,33 +143,7 @@ namespace extension { auto start = NUClear::clock::now(); auto target = tasks.front().time; - if (target - start > cv_accuracy) { - // Wait on the cv - wait.wait_until(lock, target - 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 (target - start > ns_accuracy) { - // Wait on nanosleep - util::precise_sleep(target - start - ns_accuracy); - - // 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 - } - + if (target <= start) { // Run our task and if it returns false remove it const bool renew = tasks.front()(); @@ -157,6 +159,45 @@ namespace extension { tasks.pop_back(); } } + else { + const NUClear::clock::duration time_until_task = + std::chrono::duration_cast((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() || 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 + } + } + } } } }); diff --git a/src/message/TimeTravel.hpp b/src/message/TimeTravel.hpp new file mode 100644 index 000000000..c2010b8ec --- /dev/null +++ b/src/message/TimeTravel.hpp @@ -0,0 +1,63 @@ +/* + * MIT License + * + * Copyright (c) 2024 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_MESSAGE_TIME_TRAVEL_HPP +#define NUCLEAR_MESSAGE_TIME_TRAVEL_HPP + +#include "../clock.hpp" + +namespace NUClear { +namespace message { + /** + * @brief This message is used to adjust the time of the system clock and the rate at which time passes. + * + * Using this message allows the NUClear system to adapt to the change by adjusting any time based operations + * to the new time and rate. + */ + struct TimeTravel { + enum class Action { + /// @brief Adjust clock and move all chrono tasks with it + RELATIVE, + + /// @brief Adjust clock to target time and leave chrono tasks where they are + ABSOLUTE, + + /// @brief Adjust clock to as close to target as possible without skipping any chrono tasks + NEAREST, + }; + + /// @brief The target time to set the clock to + clock::time_point target = clock::now(); + /// @brief The rate at which time should pass + double rtf = 1.0; + /// @brief The type of time travel to perform + Action type = Action::RELATIVE; + + TimeTravel() = default; + TimeTravel(const clock::time_point& target, double rtf = 1.0, Action type = Action::RELATIVE) + : target(target), rtf(rtf), type(type) {} + }; + +} // namespace message +} // namespace NUClear + +#endif // NUCLEAR_MESSAGE_TIME_TRAVEL_HPP diff --git a/src/util/platform.hpp b/src/util/platform.hpp index 52048ee6f..6cc6d9abd 100644 --- a/src/util/platform.hpp +++ b/src/util/platform.hpp @@ -81,6 +81,8 @@ // Whoever thought this was a good idea was a terrible person #undef ERROR + #undef RELATIVE + #undef ABSOLUTE // Make the windows shutdown functions look like the posix ones #define SHUT_RD SD_RECEIVE diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index cc7004de7..2a1158efc 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -42,7 +42,13 @@ if(CATCH_FOUND) "util/*.cpp" "util/network/*.cpp" "util/serialise/*.cpp" - "test_util/*.cpp" + ) + + file(GLOB test_util_src "test_util/*.cpp") + add_library(test_util STATIC ${test_util_src}) + target_link_libraries(test_util PUBLIC NUClear::nuclear) + target_include_directories( + test_util SYSTEM PUBLIC ${CATCH_INCLUDE_DIRS} ${PROJECT_BINARY_DIR}/include "${PROJECT_SOURCE_DIR}/src" ) # Some tests must be executed as individual binaries @@ -52,12 +58,9 @@ if(CATCH_FOUND) get_filename_component(test_name ${test_src} NAME_WE) add_executable(${test_name} ${test_src}) - target_link_libraries(${test_name} NUClear::nuclear) + target_link_libraries(${test_name} NUClear::nuclear test_util) set_target_properties(${test_name} PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/individual") target_include_directories(${test_name} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) - target_include_directories( - ${test_name} SYSTEM PRIVATE ${CATCH_INCLUDE_DIRS} ${PROJECT_BINARY_DIR}/include "${PROJECT_SOURCE_DIR}/src" - ) # Enable warnings, and all warnings are errors add_test(${test_name} test_nuclear) @@ -66,18 +69,12 @@ if(CATCH_FOUND) add_executable(test_nuclear ${test_src}) target_include_directories(test_nuclear PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) - target_link_libraries(test_nuclear NUClear::nuclear) - target_include_directories( - test_nuclear SYSTEM PRIVATE ${CATCH_INCLUDE_DIRS} ${PROJECT_BINARY_DIR}/include "${PROJECT_SOURCE_DIR}/src" - ) + target_link_libraries(test_nuclear NUClear::nuclear test_util) add_test(test_nuclear test_nuclear) add_executable(test_network networktest.cpp) - target_link_libraries(test_network NUClear::nuclear) + target_link_libraries(test_network NUClear::nuclear test_util) target_include_directories(test_network PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) - target_include_directories( - test_network SYSTEM PRIVATE ${CATCH_INCLUDE_DIRS} ${PROJECT_BINARY_DIR}/include "${PROJECT_SOURCE_DIR}/src" - ) endif(BUILD_TESTS) endif(CATCH_FOUND) diff --git a/tests/individual/BaseClock.cpp b/tests/individual/BaseClock.cpp index eae611fd6..788cdd2c6 100644 --- a/tests/individual/BaseClock.cpp +++ b/tests/individual/BaseClock.cpp @@ -24,8 +24,8 @@ #include #include // for localtime_r/s -// This define declares that we are using system_clock as the base clock for NUClear -#define NUCLEAR_CLOCK_TYPE std::chrono::system_clock +// This define declares that we are using steady_clock as the base clock for NUClear +#define NUCLEAR_CLOCK_TYPE std::chrono::steady_clock #include #include @@ -38,7 +38,7 @@ namespace { // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) -std::vector> times; +std::vector> times; // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) std::mutex times_mutex; constexpr int n_time = 100; @@ -56,7 +56,7 @@ class TestReactor : public NUClear::Reactor { on>().then( [this](const NUClear::message::ReactionStatistics& stats) { const std::lock_guard lock(times_mutex); - times.push_back(std::make_pair(stats.finished, std::chrono::system_clock::now())); + times.push_back(std::make_pair(stats.finished, std::chrono::steady_clock::now())); if (times.size() > n_time) { powerplant.shutdown(); } @@ -68,7 +68,7 @@ class TestReactor : public NUClear::Reactor { TEST_CASE("Testing base clock works correctly", "[api][base_clock]") { INFO("Ensure NUClear base_clock is the correct type"); - STATIC_REQUIRE(std::is_same::value); + STATIC_REQUIRE(std::is_base_of::value); // Construct the powerplant NUClear::Configuration config; @@ -105,8 +105,11 @@ TEST_CASE("Testing base clock works correctly", "[api][base_clock]") { // Compute the differences between the time pairs int match_count = 0; for (const auto& time_pairs : times) { - const std::time_t ntt = NUClear::clock::to_time_t(time_pairs.first); - const std::time_t stt = NUClear::clock::to_time_t(time_pairs.second); + using namespace std::chrono; // NOLINT(google-build-using-namespace) + const std::time_t ntt = system_clock::to_time_t( + system_clock::time_point(duration_cast(time_pairs.first.time_since_epoch()))); + const std::time_t stt = system_clock::to_time_t( + system_clock::time_point(duration_cast(time_pairs.second.time_since_epoch()))); std::tm result{}; #ifdef WIN32 diff --git a/tests/individual/CustomClock.cpp b/tests/individual/CustomClock.cpp deleted file mode 100644 index 040b6edd6..000000000 --- a/tests/individual/CustomClock.cpp +++ /dev/null @@ -1,93 +0,0 @@ -/* - * MIT License - * - * Copyright (c) 2018 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. - */ - -#define CATCH_CONFIG_MAIN -#include - -// This define declares that we are using a custom clock and it should try to link -#define NUCLEAR_CUSTOM_CLOCK -#include - -#include "test_util/TestBase.hpp" - -// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) -std::chrono::steady_clock::time_point start = std::chrono::steady_clock::now(); - -namespace NUClear { -clock::time_point clock::now() { - - // Add half the time since we started (time moving at half speed) - auto now = std::chrono::steady_clock::now(); - return clock::time_point(start + (now - start) / 2); -} -} // namespace NUClear - -// Anonymous namespace to keep everything file local -namespace { - -template -struct Message {}; - -// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables) -std::vector> times; - -class TestReactor : public test_util::TestBase { -public: - TestReactor(std::unique_ptr environment) : TestBase(std::move(environment), false) { - - // Collect steady clock times as well as NUClear clock times - on>().then([] { // - times.emplace_back(std::chrono::steady_clock::now(), NUClear::clock::now()); - }); - - // Collect until the watchdog times out - on>().then([this] { // - powerplant.shutdown(); - }); - } -}; -} // namespace - -TEST_CASE("Testing custom clock works correctly", "[api][custom_clock]") { - - NUClear::Configuration config; - config.thread_count = 1; - NUClear::PowerPlant plant(config); - plant.install(); - plant.start(); - - // Calculate the average ratio delta time for steady and custom clocks - double steady_total = 0; - double custom_total = 0; - - for (int i = 0; i + 1 < int(times.size()); ++i) { - using namespace std::chrono; // NOLINT(google-build-using-namespace) fine in function scope - steady_total += duration_cast>(times[i + 1].first - times[i].first).count(); - custom_total += duration_cast>(times[i + 1].second - times[i].second).count(); - } - - // The ratio should be about 0.5 - REQUIRE((custom_total / steady_total) == Approx(0.5)); - - // The amount of time that passed should be (n - 1) * 2 * 10ms - REQUIRE(steady_total == Approx(2.0 * (times.size() - 1) * 1e-2).margin(1e-3)); -} diff --git a/tests/individual/TimeTravel.cpp b/tests/individual/TimeTravel.cpp new file mode 100644 index 000000000..11a70640d --- /dev/null +++ b/tests/individual/TimeTravel.cpp @@ -0,0 +1,144 @@ +#define CATCH_CONFIG_MAIN +#include +#include + +#include "test_util/TestBase.hpp" + +namespace { + +using TestUnits = std::chrono::duration>; +constexpr int64_t EVENT_1_TIME = 4; +constexpr int64_t EVENT_2_TIME = 8; + +struct Results { + struct TimePair { + NUClear::clock::time_point nuclear; + std::chrono::steady_clock::time_point steady; + }; + + TimePair start; + TimePair zero; + std::array events; +}; + +class TestReactor : public test_util::TestBase { +public: + TestReactor(std::unique_ptr environment) : TestBase(std::move(environment), false) { + + on().then([this] { + // Reset clock to zero + NUClear::clock::set_clock(NUClear::clock::time_point()); + results.zero = Results::TimePair{NUClear::clock::now(), std::chrono::steady_clock::now()}; + + // Emit a chrono task to run at time EVENT_1_TIME + emit(std::make_unique( + [this](NUClear::clock::time_point&) { + results.events[0] = Results::TimePair{NUClear::clock::now(), std::chrono::steady_clock::now()}; + return false; + }, + NUClear::clock::time_point(TestUnits(EVENT_1_TIME)), + 1)); + + // Emit a chrono task to run at time EVENT_2_TIME, and shutdown + emit(std::make_unique( + [this](NUClear::clock::time_point&) { + results.events[1] = Results::TimePair{NUClear::clock::now(), std::chrono::steady_clock::now()}; + powerplant.shutdown(); + return false; + }, + NUClear::clock::time_point(TestUnits(EVENT_2_TIME)), + 2)); + + // Time travel! + emit( + std::make_unique(NUClear::clock::time_point(adjustment), rtf, action)); + + results.start = Results::TimePair{NUClear::clock::now(), std::chrono::steady_clock::now()}; + }); + } + + // Time travel action + NUClear::message::TimeTravel::Action action = NUClear::message::TimeTravel::Action::RELATIVE; + + // Time adjustment + NUClear::clock::duration adjustment = std::chrono::milliseconds(0); + + // Real-time factor + double rtf = 1.0; + + // Results struct + Results results; +}; + +} // anonymous namespace + +TEST_CASE("Test time travel correctly changes the time for non zero rtf", "[time_travel][chrono_controller]") { + + using Action = NUClear::message::TimeTravel::Action; + + const NUClear::Configuration config; + auto plant = std::make_shared(config); + auto& reactor = plant->install(); + + // Set the reactor fields to the values we want to test + const Action action = GENERATE(Action::RELATIVE, Action::ABSOLUTE, Action::NEAREST); + const int64_t adjustment = GENERATE(-4, -2, 0, 2, 4, 6, 8, 10); + const double rtf = GENERATE(0.5, 1.0, 2.0); + CAPTURE(action, adjustment, rtf); + reactor.action = action; + reactor.adjustment = TestUnits(adjustment); + reactor.rtf = rtf; + + // Start the powerplant + plant->start(); + + // Expected results + std::array expected{}; + switch (action) { + case Action::RELATIVE: expected = {EVENT_1_TIME, EVENT_2_TIME}; break; + case Action::ABSOLUTE: + expected = { + std::max(int64_t(0), int64_t(EVENT_1_TIME - adjustment)), + std::max(int64_t(0), int64_t(EVENT_2_TIME - adjustment)), + }; + break; + case Action::NEAREST: + expected = adjustment < EVENT_1_TIME + ? std::array{EVENT_1_TIME - adjustment, EVENT_2_TIME - adjustment} + : std::array{0, EVENT_2_TIME - EVENT_1_TIME}; + break; + default: throw std::runtime_error("Unknown action"); + } + + std::array expected_nuclear = {TestUnits(expected[0]), TestUnits(expected[1])}; + std::array expected_steady = {TestUnits(std::lround(double(expected[0]) / rtf)), + TestUnits(std::lround(double(expected[1]) / rtf))}; + + const auto& r = reactor.results; + const auto& n_start = reactor.results.start.nuclear; + const auto& s_start = reactor.results.start.steady; + + auto round_to_test_units = [](const auto& duration) { + const double d = std::chrono::duration_cast>(duration).count(); + const double t = (TestUnits::period::den * d) / TestUnits::period::num; + return TestUnits(std::lround(t)); + }; + + std::array actual_nuclear = { + round_to_test_units(r.events[0].nuclear - n_start), + round_to_test_units(r.events[1].nuclear - n_start), + }; + std::array actual_steady = { + round_to_test_units(r.events[0].steady - s_start), + round_to_test_units(r.events[1].steady - s_start), + }; + + const TestUnits actual_adjustment(round_to_test_units(r.start.nuclear - r.zero.nuclear)); + const TestUnits expected_adjustment(std::min(adjustment, action == Action::NEAREST ? EVENT_1_TIME : adjustment)); + CHECK(round_to_test_units(r.zero.nuclear.time_since_epoch()) == TestUnits(0)); + CHECK(expected_nuclear[0] == actual_nuclear[0]); + CHECK(expected_nuclear[1] == actual_nuclear[1]); + CHECK(expected_steady[0] == actual_steady[0]); + CHECK(expected_steady[1] == actual_steady[1]); + CHECK(expected_adjustment == actual_adjustment); +} diff --git a/tests/individual/TimeTravelFrozen.cpp b/tests/individual/TimeTravelFrozen.cpp new file mode 100644 index 000000000..5773a5f7f --- /dev/null +++ b/tests/individual/TimeTravelFrozen.cpp @@ -0,0 +1,116 @@ +#define CATCH_CONFIG_MAIN +#include +#include +#include + +#include "test_util/TestBase.hpp" + +namespace { + +constexpr std::chrono::milliseconds EVENT_1_TIME = std::chrono::milliseconds(4); +constexpr std::chrono::milliseconds EVENT_2_TIME = std::chrono::milliseconds(8); +constexpr std::chrono::milliseconds SHUTDOWN_TIME = std::chrono::milliseconds(12); + +struct WaitForShutdown {}; + +class TestReactor : public test_util::TestBase { +public: + TestReactor(std::unique_ptr environment) : TestBase(std::move(environment), false) { + + on().then([this] { + // Reset clock to zero + NUClear::clock::set_clock(NUClear::clock::time_point(), 0.0); + + // Emit a chrono task to run at time EVENT_1_TIME + emit(std::make_unique( + [this](NUClear::clock::time_point&) { + events.push_back("Event 1"); + return false; + }, + NUClear::clock::time_point(EVENT_1_TIME), + 1)); + + // Emit a chrono task to run at time EVENT_2_TIME + emit(std::make_unique( + [this](NUClear::clock::time_point&) { + events.push_back("Event 2"); + return false; + }, + NUClear::clock::time_point(EVENT_2_TIME), + 2)); + + // Time travel + emit( + std::make_unique(NUClear::clock::time_point(adjustment), rtf, action)); + + // Shutdown after steady clock amount of time + emit(std::make_unique()); + }); + + on>().then([this] { + std::this_thread::sleep_for(SHUTDOWN_TIME); + events.push_back("Finished"); + powerplant.shutdown(); + }); + } + + // Time travel action + NUClear::message::TimeTravel::Action action = NUClear::message::TimeTravel::Action::RELATIVE; + + // Time adjustment + NUClear::clock::duration adjustment = std::chrono::milliseconds(0); + + // Real-time factor + double rtf = 1.0; + + // Events + std::vector events = {}; +}; + +} // anonymous namespace + +TEST_CASE("Test time travel correctly changes the time for non zero rtf", "[time_travel][chrono_controller]") { + + using Action = NUClear::message::TimeTravel::Action; + + const NUClear::Configuration config; + auto plant = std::make_shared(config); + auto& reactor = plant->install(); + + // Set the reactor fields to the values we want to test + const Action action = GENERATE(Action::RELATIVE, Action::ABSOLUTE, Action::NEAREST); + const int64_t adjustment = GENERATE(-4, -2, 0, 2, 4, 6, 8, 10); + CAPTURE(action, adjustment); + reactor.action = action; + reactor.adjustment = std::chrono::milliseconds(adjustment); + reactor.rtf = 0.0; + + // Start the powerplant + plant->start(); + + // Expected results + std::vector expected; + switch (action) { + case Action::RELATIVE: expected = {"Finished"}; break; + case Action::ABSOLUTE: + if (std::chrono::milliseconds(adjustment) < EVENT_1_TIME) { + expected = {"Finished"}; + } + else if (std::chrono::milliseconds(adjustment) < EVENT_2_TIME) { + expected = {"Event 1", "Finished"}; + } + else { + expected = {"Event 1", "Event 2", "Finished"}; + } + break; + case Action::NEAREST: + expected = std::chrono::milliseconds(adjustment) < EVENT_1_TIME + ? std::vector{"Finished"} + : std::vector{"Event 1", "Finished"}; + break; + default: throw std::runtime_error("Unknown action"); + } + + INFO(test_util::diff_string(expected, reactor.events)); + CHECK(expected == reactor.events); +}