add some setters and update unit tests

Author: LoreMoretti

src/System/include/BipedalLocomotion/System/PeriodicThread.h

@@ -109,6 +109,25 @@ class PeriodicThread
      */
     bool setPolicy(int policy, int priority = 0);
 
+    /**
+     * @brief Set the maximum number of accepted deadline miss.
+     * @param maximumNumberOfAcceptedDeadlineMiss maximum number of accepted deadline miss.
+     */
+    bool setMaximumNumberOfAcceptedDeadlineMiss(int maximumNumberOfAcceptedDeadlineMiss);
+
+    /**
+     * @brief Get the number of deadline miss.
+     * @return number of deadline miss.
+     */
+    int getNumberOfDeadlineMiss();
+
+    /**
+     * @brief Enable the early wake up. The thread will be awaken before and busy wait until the
+     * actual wake up time.
+     * @return true if the early wake up was correctly set, false otherwise.
+     */
+    bool enableEarlyWakeUp();
+
 protected:
     /**
      * @brief This method is called at each iteration of the thread.
@@ -153,12 +172,11 @@ class PeriodicThread
                                                      * miss.
                                                      */
 
-    int m_deadlineMiss = 0; /**< Number of deadline miss. */
+    std::atomic<int> m_deadlineMiss = 0; /**< Number of deadline miss. */
 
     std::chrono::nanoseconds m_wakeUpTime = std::chrono::nanoseconds(0); /**< Wake up time of the
                                                                           * thread.
                                                                           */
-
     int m_priority = 0; /**< Priority of the thread. */
 
     int m_policy = SCHED_OTHER; /**< Policy of the thread. */

src/System/src/PeriodicThread.cpp

@@ -115,6 +115,36 @@ bool PeriodicThread::setPeriod(std::chrono::nanoseconds period)
     return true;
 }
 
+bool PeriodicThread::setMaximumNumberOfAcceptedDeadlineMiss(int maximumNumberOfAcceptedDeadlineMiss)
+{
+    if (m_state.load() != PeriodicThreadState::INACTIVE)
+    {
+        BipedalLocomotion::log()->error("[PeriodicThread::setMaximumNumberOfAcceptedDeadlineMiss] "
+                                        "The thread has already started. The maximum number of "
+                                        "accepted deadline miss cannot be changed.");
+        return false;
+    }
+    m_maximumNumberOfAcceptedDeadlineMiss = maximumNumberOfAcceptedDeadlineMiss;
+    return true;
+}
+
+int PeriodicThread::getNumberOfDeadlineMiss()
+{
+    return m_deadlineMiss.load();
+}
+
+bool PeriodicThread::enableEarlyWakeUp()
+{
+    if (m_state.load() != PeriodicThreadState::INACTIVE)
+    {
+        BipedalLocomotion::log()->error("[PeriodicThread::enableEarlyWakeUp] The thread has "
+                                        "already started. The early wake up cannot be changed.");
+        return false;
+    }
+    m_earlyWakeUp = true;
+    return true;
+}
+
 bool PeriodicThread::threadInit()
 {
     return true;
@@ -232,10 +262,10 @@ void PeriodicThread::advance()
     // check if the deadline is missed
     if (BipedalLocomotion::clock().now() > m_wakeUpTime)
     {
-        m_deadlineMiss++;
+        m_deadlineMiss.fetch_add(1); // increment the number of deadline miss
         if (m_maximumNumberOfAcceptedDeadlineMiss > 0)
         {
-            if (m_deadlineMiss > m_maximumNumberOfAcceptedDeadlineMiss)
+            if (m_deadlineMiss.load() > m_maximumNumberOfAcceptedDeadlineMiss)
             {
                 // we have to close the runner
                 m_state.store(PeriodicThreadState::STOPPED);