diff --git a/examples/PeriodicThread/main.cpp b/examples/PeriodicThread/main.cpp index 27b9fa9da5..292124ed14 100644 --- a/examples/PeriodicThread/main.cpp +++ b/examples/PeriodicThread/main.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include @@ -9,7 +10,8 @@ class Thread : public BipedalLocomotion::System::PeriodicThread { public: - Thread(); + std::string name = "Thread"; + Thread(std::string name); ~Thread(); bool run() override; @@ -18,21 +20,22 @@ class Thread : public BipedalLocomotion::System::PeriodicThread bool Thread::run() { - BipedalLocomotion::log()->info("[Thread::run] Thread is running."); + BipedalLocomotion::log()->info("[Thread::run] {} is running.", name); return true; } -Thread::Thread() - : BipedalLocomotion::System::PeriodicThread(std::chrono::milliseconds(1000)){}; +Thread::Thread(std::string name) + : name(name) + , BipedalLocomotion::System::PeriodicThread(std::chrono::milliseconds(1000)){}; Thread::~Thread() { - BipedalLocomotion::log()->info("[Thread::~Thread] Thread is destroyed."); + BipedalLocomotion::log()->info("[Thread::~Thread] {} is destroyed.", name); }; bool Thread::threadInit() { - BipedalLocomotion::log()->info("[Thread::threadInit] Thread is initialized."); + BipedalLocomotion::log()->info("[Thread::threadInit] {} is initialized.", name); return true; } @@ -42,20 +45,26 @@ int main() auto barrier = BipedalLocomotion::System::Barrier::create(2); // Thread thread; - auto thread1 = Thread(); + auto thread1 = Thread("Thread 1"); thread1.start(barrier); BipedalLocomotion::clock().sleepFor(std::chrono::milliseconds(2000)); - auto thread2 = Thread(); + auto thread2 = Thread("Thread 2"); thread2.setPeriod(std::chrono::seconds(2)); thread2.start(barrier); - while (thread1.isRunning()) + while (thread1.isRunning() || thread2.isRunning()) { std::this_thread::sleep_for(std::chrono::milliseconds(4000)); thread1.stop(); BipedalLocomotion::log()->info("[main] Thread 1 is asked to stop."); + + if (!thread1.isRunning()) + { + thread2.stop(); + BipedalLocomotion::log()->info("[main] Thread 2 is asked to stop."); + } } BipedalLocomotion::log()->info("[main] About to exit the application."); return EXIT_SUCCESS; diff --git a/src/System/include/BipedalLocomotion/System/PeriodicThread.h b/src/System/include/BipedalLocomotion/System/PeriodicThread.h index 2dbd934b75..7538f75110 100644 --- a/src/System/include/BipedalLocomotion/System/PeriodicThread.h +++ b/src/System/include/BipedalLocomotion/System/PeriodicThread.h @@ -36,14 +36,21 @@ enum class PeriodicThreadState }; /** - * @brief This class implements a periodic thread. The user has to inherit from this class and - * implement the virtual methods. + * @brief The PeriodicThread class is designed to implement a periodic thread that performs a + * specific task at regular intervals. The task is defined by overriding the virtual run method. The + * class provides various methods to control the thread's lifecycle, including starting, stopping, + * suspending, and resuming the thread. It also allows configuring thread-specific parameters like + * priority and scheduling policy (on Linux systems). Additionally, it supports synchronization with + * other threads using barriers and can handle early wake-up scenarios to minimize latency. + * Finally, the class provides a mechanism to monitor the number of deadline misses, which can be + * useful for real-time applications. */ class PeriodicThread { public: #ifdef __linux__ // Default constructor + PeriodicThread(std::chrono::nanoseconds period = std::chrono::nanoseconds(100000), int maximumNumberOfAcceptedDeadlineMiss = -1, int priority = 0, @@ -68,7 +75,9 @@ class PeriodicThread /** * @brief Start the thread. * @param barrier barrier to synchronize the thread. If nullptr, the thread will start - * immediately, without waiting for other threads to reach the barrier. + * immediately, without waiting for other threads to reach the barrier. Instead, if for example + * the barrier is creted as std::make_shared(2), the thread + * will wait for another thread to reach the barrier before starting. * @return true if the thread was correctly started, false otherwise. */ bool start(std::shared_ptr barrier = nullptr); @@ -93,13 +102,13 @@ class PeriodicThread * stopped. * @return true if the thread is running, false otherwise. */ - bool isRunning(); + bool isRunning() const; /** * @brief Check if the thread is initialized. * @return true if the thread is initialized, false otherwise. */ - bool isInitialized(); + bool isInitialized() const; /** * @brief Set the period of the thread. @@ -108,14 +117,28 @@ class PeriodicThread */ bool setPeriod(std::chrono::nanoseconds period); + /** + * @brief Get the period of the thread. + * @return period of the thread. + */ + std::chrono::nanoseconds getPeriod() const; + #ifdef __linux__ /** - * @brief Set the policy of the thread. + * @brief Set the policy and priority of the thread before starting it. When starting, the + * thread will try to use this policy and priority. * @param policy policy of the thread. * @param priority priority of the thread. - * @return true if the policy was correctly set, false otherwise. + * @return true if the policy and priority are correctly set, false otherwise. */ bool setPolicy(int policy, int priority = 0); + + /** + * @brief Get the policy and prority of the thread. + * @return policy of the thread. + */ + void getPolicy(int& policy, int& priority) const; + #endif /** @@ -124,11 +147,17 @@ class PeriodicThread */ bool setMaximumNumberOfAcceptedDeadlineMiss(int maximumNumberOfAcceptedDeadlineMiss); + /** + * @brief Get the maximum number of accepted deadline miss. + * @return maximum number of accepted deadline miss. + */ + int getMaximumNumberOfAcceptedDeadlineMiss() const; + /** * @brief Get the number of deadline miss. * @return number of deadline miss. */ - int getNumberOfDeadlineMiss(); + int getNumberOfDeadlineMiss() const; /** * @brief Enable the early wake up. The thread will be awaken before and busy wait until the @@ -137,6 +166,12 @@ class PeriodicThread */ bool enableEarlyWakeUp(); + /** + * @brief Check if the early wake up is enabled. + * @return true if the early wake up is enabled, false otherwise. + */ + bool isEarlyWakeUpEnabled() const; + protected: /** * @brief This method is called at each iteration of the thread. @@ -153,33 +188,34 @@ class PeriodicThread private: /** - * @brief run the periodic thread. + * @brief This is the function being executed by the std::thread. */ void threadFunction(); /** - * @brief Advance the thread of one step, calling the user defined run function once. + * @brief Advance the thread of one step, calling the virtual run function once. */ void advance(); /** - * @brief Synchronize the thread. + * @brief Synchronize the thread with other threads with a barrier. */ void synchronize(); /** - * @brief Set the policy of the thread. - * @return true if the policy was correctly set, false otherwise. + * @brief Set the policy and priority of the thread. + * @return true if the policy and priority were correctly set, false otherwise. */ bool setPolicy(); - std::chrono::nanoseconds m_period = std::chrono::nanoseconds(100000); /**< Period of the - * thread. - */ + std::atomic m_period = std::chrono::nanoseconds(100000); /**< Period + * of the + * thread. + */ - int m_maximumNumberOfAcceptedDeadlineMiss = -1; /**< Maximum number of accepted deadline - * miss. - */ + std::atomic m_maximumNumberOfAcceptedDeadlineMiss = -1; /**< Maximum number of accepted + * deadline miss. + */ std::atomic m_deadlineMiss = 0; /**< Number of deadline miss. */ @@ -187,9 +223,9 @@ class PeriodicThread * thread. */ #ifdef __linux__ - int m_priority = 0; /**< Priority of the thread. */ + std::atomic m_priority = 0; /**< Priority of the thread. */ - int m_policy = SCHED_OTHER; /**< Policy of the thread. */ + std::atomic m_policy = SCHED_OTHER; /**< Policy of the thread. */ #endif std::thread m_thread; /**< Thread object. */ @@ -206,17 +242,15 @@ class PeriodicThread varies depending on the OS. For now we fix it to a constant value. Note that in real-time OS it might be smaller. */ - bool m_earlyWakeUp = false; /**< If true, the thread will be awaken before and busy wait until - the actual wake up time. */ - - std::condition_variable m_cv; /**< Condition variable to check for - * initialization. - */ - std::atomic m_ready = false; /**< Flag to signal that the inizialization task are - completed. */ + std::atomic m_earlyWakeUp = false; /**< If true, the thread will be awaken before and busy + wait until the actual wake up time. */ + + std::condition_variable m_cv; /**< Condition variable to check for initialization.*/ + std::mutex m_cvMutex; /**< Mutex to protect the condition variable. */ + std::atomic m_ready = false; /**< Flag to signal that the inizialization tasks of the + thread are completed. */ std::atomic m_initializationSuccessful = false; /**< Flag to signal the result of initialization */ - std::mutex m_cv_mtx; /**< Mutex to protect the condition variable. */ }; } // namespace System } // namespace BipedalLocomotion diff --git a/src/System/src/PeriodicThread.cpp b/src/System/src/PeriodicThread.cpp index 02b7d70637..3507c5cc2d 100644 --- a/src/System/src/PeriodicThread.cpp +++ b/src/System/src/PeriodicThread.cpp @@ -86,7 +86,7 @@ void PeriodicThread::threadFunction() // https://en.cppreference.com/w/cpp/thread/condition_variable: // Even if the shared variable is atomic, it must be modified while owning the mutex to // correctly publish the modification to the waiting thread. - std::lock_guard lock(m_cv_mtx); + std::lock_guard lock(m_cvMutex); m_ready.store(true); } m_cv.notify_one(); @@ -125,9 +125,9 @@ bool PeriodicThread::setPolicy() pthread_t nativeHandle = pthread_self(); // get the current thread parameters sched_param params; - params.sched_priority = m_priority; + params.sched_priority = m_priority.load(); // Set the scheduling policy to SCHED_FIFO and priority - int ret = pthread_setschedparam(nativeHandle, m_policy, ¶ms); + int ret = pthread_setschedparam(nativeHandle, m_policy.load(), ¶ms); if (ret != 0) { log()->error("{} Failed to set scheduling policy, with error: {}", logPrefix, ret); @@ -135,7 +135,7 @@ bool PeriodicThread::setPolicy() { log()->error("{} The calling thread does not have the appropriate privileges to set " "the requested scheduling policy and parameters. Try to run the " - "YarpLoggerDevice with 'sudo -E'.", + "application with 'sudo -E'.", logPrefix); } return false; @@ -143,7 +143,7 @@ bool PeriodicThread::setPolicy() { log()->debug("{} Scheduling policy set to {} with priority {}", logPrefix, - m_policy, + m_policy.load(), params.sched_priority); return true; } @@ -161,10 +161,16 @@ bool PeriodicThread::setPolicy(int policy, int priority) "started. The policy and priority cannot be changed."); return false; } - m_priority = priority; - m_policy = policy; + m_policy.store(policy); + m_priority.store(priority); return true; }; + +void PeriodicThread::getPolicy(int& policy, int& priority) const +{ + policy = m_policy.load(); + priority = m_priority.load(); +}; #endif bool PeriodicThread::setPeriod(std::chrono::nanoseconds period) @@ -179,6 +185,11 @@ bool PeriodicThread::setPeriod(std::chrono::nanoseconds period) return true; } +std::chrono::nanoseconds PeriodicThread::getPeriod() const +{ + return m_period.load(); +} + bool PeriodicThread::setMaximumNumberOfAcceptedDeadlineMiss(int maximumNumberOfAcceptedDeadlineMiss) { if (m_state.load() != PeriodicThreadState::INACTIVE) @@ -188,11 +199,16 @@ bool PeriodicThread::setMaximumNumberOfAcceptedDeadlineMiss(int maximumNumberOfA "accepted deadline miss cannot be changed."); return false; } - m_maximumNumberOfAcceptedDeadlineMiss = maximumNumberOfAcceptedDeadlineMiss; + m_maximumNumberOfAcceptedDeadlineMiss.store(maximumNumberOfAcceptedDeadlineMiss); return true; } -int PeriodicThread::getNumberOfDeadlineMiss() +int PeriodicThread::getMaximumNumberOfAcceptedDeadlineMiss() const +{ + return m_maximumNumberOfAcceptedDeadlineMiss.load(); +} + +int PeriodicThread::getNumberOfDeadlineMiss() const { return m_deadlineMiss.load(); } @@ -205,7 +221,7 @@ bool PeriodicThread::enableEarlyWakeUp() "already started. The early wake up cannot be changed."); return false; } - m_earlyWakeUp = true; + m_earlyWakeUp.store(true); return true; } @@ -265,7 +281,7 @@ bool PeriodicThread::resume() bool PeriodicThread::start(std::shared_ptr barrier) { - std::unique_lock lock(m_cv_mtx); // lock the mutex for the condition variable + std::unique_lock lock(m_cvMutex); // lock the mutex for the condition variable // only an inactive thread can be started if (m_state.load() != PeriodicThreadState::INACTIVE) @@ -297,24 +313,29 @@ bool PeriodicThread::start(std::shared_ptr b return m_initializationSuccessful.load(); }; -bool PeriodicThread::isRunning() +bool PeriodicThread::isRunning() const { return ((m_state.load() != PeriodicThreadState::INACTIVE) && (m_state.load() != PeriodicThreadState::STOPPED)); } -bool PeriodicThread::isInitialized() +bool PeriodicThread::isInitialized() const { return (m_state.load() == PeriodicThreadState::INITIALIZED); } +bool PeriodicThread::isEarlyWakeUpEnabled() const +{ + return (m_earlyWakeUp.load()); +} + void PeriodicThread::advance() { // get the current time auto now = BipedalLocomotion::clock().now(); // busy wait until wake up time - if (m_earlyWakeUp) + if (isEarlyWakeUpEnabled()) { while (now < m_wakeUpTime) { @@ -323,7 +344,7 @@ void PeriodicThread::advance() } // get the next wake up time - m_wakeUpTime = now + m_period; + m_wakeUpTime = now + m_period.load(); // run user overridden function, when not idling if (m_state.load() != PeriodicThreadState::IDLE) @@ -340,9 +361,9 @@ void PeriodicThread::advance() if (BipedalLocomotion::clock().now() > m_wakeUpTime) { m_deadlineMiss.fetch_add(1); // increment the number of deadline miss - if (m_maximumNumberOfAcceptedDeadlineMiss > 0) + if (m_maximumNumberOfAcceptedDeadlineMiss.load() > 0) { - if (m_deadlineMiss.load() > m_maximumNumberOfAcceptedDeadlineMiss) + if (m_deadlineMiss.load() > m_maximumNumberOfAcceptedDeadlineMiss.load()) { // we have to close the runner m_state.store(PeriodicThreadState::STOPPED); @@ -355,7 +376,7 @@ void PeriodicThread::advance() BipedalLocomotion::clock().yield(); // wait until the next deadline - if (m_earlyWakeUp) + if (isEarlyWakeUpEnabled()) { BipedalLocomotion::clock().sleepUntil(m_wakeUpTime - m_schedulerLatency); } else diff --git a/src/System/tests/PeriodicThreadTest.cpp b/src/System/tests/PeriodicThreadTest.cpp index 5206cb8e45..52e3f9b390 100644 --- a/src/System/tests/PeriodicThreadTest.cpp +++ b/src/System/tests/PeriodicThreadTest.cpp @@ -173,6 +173,9 @@ TEST_CASE("Test Periodic Thread", "[PeriodicThreadNotAllowed]") // try to enable early wake up REQUIRE(!thread.enableEarlyWakeUp()); + // check that early wake up has not enabled + REQUIRE(!thread.isEarlyWakeUpEnabled()); + // stop the thread thread.stop(); BipedalLocomotion::clock().sleepFor(period);