Add in a new TimeTravel mesage to allow the system to change the NUCl…

…ear clock and ensure that chrono events continue to work (#102)

This PR adds in a new TimeTravel mesage to allow the system to change
the NUClear clock and ensure that chrono events continue to work.

The following time travel types are added:

- `RELATIVE` Adjust clock and move all chrono tasks with it
- `ABSOLUTE` Adjust clock to target time and leave chrono tasks where
they are
- `NEAREST` Adjust clock to as close to target as possible without
skipping any chrono tasks

---------

Co-authored-by: Tom0Brien <[email protected]>

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 <array>
+#include <atomic>
 #include <chrono>
+#include <mutex>
 
 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 <typename = void>
+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<std::mutex> lock(mutex);
+        // Load the current state
+        const auto& current = data[active.load()];
+        const int n         = static_cast<int>((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<std::mutex> lock(mutex);
+        // Load the current state
+        const int n = static_cast<int>((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 <typename T>
+    duration static dc(const T& t) {
+        return std::chrono::duration_cast<duration>(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<ClockData, 3> data;  // NOLINT(cppcoreguidelines-avoid-non-const-global-variables)
+
+    /// @brief The active clock data index.
+    static std::atomic<int> active;  // NOLINT(cppcoreguidelines-avoid-non-const-global-variables)
 };
 
-#endif
+template <typename T>
+// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
+std::mutex nuclear_clock<T>::mutex;
+template <typename T>
+// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
+std::array<typename nuclear_clock<T>::ClockData, 3> nuclear_clock<T>::data =
+    std::array<typename nuclear_clock<T>::ClockData, 3>{};
+template <typename T>
+// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
+std::atomic<int> nuclear_clock<T>::active{0};
+
+using clock = nuclear_clock<>;
+
 
 }  // namespace NUClear
 

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<Trigger<message::TimeTravel>>().then("Time Travel", [this](const message::TimeTravel& travel) {
+                const std::lock_guard<std::mutex> 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<Always, Priority::REALTIME>().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<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() || 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
+                                }
+                            }
+                        }
                     }
                 }
             });

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

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

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)