add pthread synchronization primitives to fix gcc8 bugs in std::condition_variable

Signed-off-by: Alberto Soragna <[email protected]>

Author: alsora

irobot_events_executor/include/rclcpp/sync_primitives.hpp

@@ -0,0 +1,154 @@
+//  Copyright (C) 2022, iRobot Corporation.
+
+#ifndef RCLCPP__SYNC_PRIMITIVES_HPP_
+#define RCLCPP__SYNC_PRIMITIVES_HPP_
+
+#include <cstdint>
+#include <sys/time.h>
+#include <pthread.h>
+
+namespace rclcpp
+{
+
+class RecursiveMutex
+{
+public:
+  RecursiveMutex()
+  {
+    pthread_mutexattr_t attr;
+    int ret = pthread_mutexattr_init(&attr);
+    assert(ret == 0);
+    ret = pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE);
+    assert(ret == 0);
+    ret = pthread_mutexattr_setprotocol(&attr, PTHREAD_PRIO_INHERIT);
+    assert(ret == 0);
+    ret = pthread_mutex_init(&m_mutex, &attr);
+    assert(ret == 0);
+    ret = pthread_mutexattr_destroy(&attr);
+    assert(ret == 0);
+    (void)ret;
+  }
+
+  // Not copyable or movable
+  RecursiveMutex(const RecursiveMutex &) = delete;
+  RecursiveMutex(RecursiveMutex &&) = delete;
+  RecursiveMutex &operator=(const RecursiveMutex &) = delete;
+  RecursiveMutex &operator=(RecursiveMutex &&) = delete;
+
+  ~RecursiveMutex()
+  {
+    int ret = pthread_mutex_destroy(&m_mutex);
+    assert(ret == 0);
+    (void)ret;
+  }
+
+  void lock()
+  {
+    int ret = pthread_mutex_lock(&m_mutex);
+    assert(ret == 0);
+    (void)ret;
+  }
+
+  void unlock()
+  {
+    int ret = pthread_mutex_unlock(&m_mutex);
+    assert(ret == 0);
+    (void)ret;
+  }
+
+  pthread_mutex_t& native_handle()
+  {
+    return m_mutex;
+  }
+
+private:
+  pthread_mutex_t m_mutex{};
+};
+
+class Lock
+{
+public:
+  explicit Lock(RecursiveMutex& mutex)
+  : m_mutex(&mutex), m_lock_cnt(1)
+  {
+    m_mutex->lock();
+  }
+
+  // Not copyable
+  Lock(const Lock&) = delete;
+  Lock &operator=(const Lock &) = delete;
+
+  /** Unlock the mutex associated with this lock. */
+  ~Lock()
+  {
+    while (m_lock_cnt > 0) {
+        m_mutex->unlock();
+        --m_lock_cnt;
+    }
+  }
+
+private:
+  RecursiveMutex *m_mutex;
+  int m_lock_cnt;
+};
+
+class ConditionVariable
+{
+public:
+  ConditionVariable()
+  {
+    pthread_condattr_t attr;
+    pthread_condattr_init(&attr);
+    int ret = pthread_condattr_setclock(&attr, CLOCK_MONOTONIC);
+    assert(ret == 0);
+    ret = pthread_cond_init(&m_cond, &attr);
+    assert(ret == 0);
+    ret = pthread_condattr_destroy(&attr);
+    assert(ret == 0);
+    (void)ret;
+  }
+
+  ~ConditionVariable()
+  {
+    int ret = pthread_cond_destroy(&m_cond);
+    assert(ret == 0);
+    (void)ret;
+  }
+
+  void notify_one()
+  {
+    pthread_cond_signal(&m_cond);
+  }
+
+  void wait(RecursiveMutex& mutex)
+  {
+    pthread_cond_wait(&this->m_cond, &mutex.native_handle());
+  }
+
+  bool wait(RecursiveMutex& mutex, uint64_t nanoseconds)
+  {
+    // Determine the absolute system time when timeout occurs.
+    timespec start {};
+    clock_gettime(CLOCK_MONOTONIC, &start);
+
+    static constexpr uint64_t NS_PER_SECONDS = 1000000000;
+
+    timespec wait {};
+    wait.tv_nsec = nanoseconds % NS_PER_SECONDS;
+    wait.tv_sec = nanoseconds / NS_PER_SECONDS;
+
+    timespec timeout {};
+    const uint64_t nanoseconds_sum = start.tv_nsec + wait.tv_nsec;
+    timeout.tv_nsec = nanoseconds_sum % NS_PER_SECONDS;
+    timeout.tv_sec = start.tv_sec + wait.tv_sec + nanoseconds_sum / NS_PER_SECONDS;
+
+    return pthread_cond_timedwait(&this->m_cond, &mutex.native_handle(), &timeout) == 0;
+  }
+
+private:
+  pthread_cond_t m_cond{};
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__SYNC_PRIMITIVES_HPP_

irobot_events_executor/include/rclcpp/timers_manager.hpp

@@ -16,6 +16,7 @@
 
 #include "rclcpp/context.hpp"
 #include "rclcpp/timer.hpp"
+#include "rclcpp/sync_primitives.hpp"
 
 namespace rclcpp
 {
@@ -485,9 +486,9 @@ class TimersManager
   // Thread used to run the timers execution task
   std::thread timers_thread_;
   // Protects access to timers
-  std::mutex timers_mutex_;
+  rclcpp::RecursiveMutex timers_mutex_;
   // Notifies the timers thread whenever timers are added/removed
-  std::condition_variable timers_cv_;
+  rclcpp::ConditionVariable timers_cv_;
   // Flag used as predicate by timers_cv_ that denotes one or more timers being added/removed
   bool timers_updated_ {false};
   // Indicates whether the timers thread is currently running or not

irobot_events_executor/src/rclcpp/timers_manager.cpp

@@ -36,7 +36,7 @@ void TimersManager::add_timer(rclcpp::TimerBase::SharedPtr timer)
 
   bool added = false;
   {
-    std::unique_lock<std::mutex> lock(timers_mutex_);
+    rclcpp::Lock lock(timers_mutex_);
     added = weak_timers_heap_.add_timer(timer);
     timers_updated_ = timers_updated_ || added;
   }
@@ -67,7 +67,7 @@ void TimersManager::stop()
 
   // Notify the timers manager thread to wake up
   {
-    std::unique_lock<std::mutex> lock(timers_mutex_);
+    rclcpp::Lock lock(timers_mutex_);
     timers_updated_ = true;
   }
   timers_cv_.notify_one();
@@ -86,7 +86,7 @@ std::chrono::nanoseconds TimersManager::get_head_timeout()
             "get_head_timeout() can't be used while timers thread is running");
   }
 
-  std::unique_lock<std::mutex> lock(timers_mutex_);
+  rclcpp::Lock lock(timers_mutex_);
   return this->get_head_timeout_unsafe();
 }
 
@@ -98,7 +98,7 @@ size_t TimersManager::get_number_ready_timers()
             "get_number_ready_timers() can't be used while timers thread is running");
   }
 
-  std::unique_lock<std::mutex> lock(timers_mutex_);
+  rclcpp::Lock lock(timers_mutex_);
   TimersHeap locked_heap = weak_timers_heap_.validate_and_lock();
   return locked_heap.get_number_ready_timers();
 }
@@ -111,7 +111,7 @@ void TimersManager::execute_ready_timers()
             "execute_ready_timers() can't be used while timers thread is running");
   }
 
-  std::unique_lock<std::mutex> lock(timers_mutex_);
+  rclcpp::Lock lock(timers_mutex_);
   this->execute_ready_timers_unsafe();
 }
 
@@ -123,7 +123,7 @@ bool TimersManager::execute_head_timer()
             "execute_head_timer() can't be used while timers thread is running");
   }
 
-  std::unique_lock<std::mutex> lock(timers_mutex_);
+  rclcpp::Lock lock(timers_mutex_);
 
   TimersHeap timers_heap = weak_timers_heap_.validate_and_lock();
 
@@ -153,7 +153,7 @@ void TimersManager::execute_ready_timer(const void * timer_id)
 {
   TimerPtr ready_timer;
   {
-    std::unique_lock<std::mutex> lock(timers_mutex_);
+    rclcpp::Lock lock(timers_mutex_);
     ready_timer = weak_timers_heap_.get_timer(timer_id);
   }
   if (ready_timer) {
@@ -228,18 +228,21 @@ void TimersManager::run_timers()
 {
   while (rclcpp::ok(context_) && running_) {
     // Lock mutex
-    std::unique_lock<std::mutex> lock(timers_mutex_);
+    rclcpp::Lock lock(timers_mutex_);
 
     std::chrono::nanoseconds time_to_sleep = get_head_timeout_unsafe();
 
     // No need to wait if a timer is already available
     if (time_to_sleep > std::chrono::nanoseconds::zero()) {
       if (time_to_sleep != std::chrono::nanoseconds::max()) {
         // Wait until timeout or notification that timers have been updated
-        timers_cv_.wait_for(lock, time_to_sleep, [this]() {return timers_updated_;});
+        uint64_t timeout = time_to_sleep.count();
+        timers_cv_.wait(timers_mutex_, timeout);
       } else {
         // Wait until notification that timers have been updated
-        timers_cv_.wait(lock, [this]() {return timers_updated_;});
+        while (!timers_updated_) {
+          timers_cv_.wait(timers_mutex_);
+        }
       }
     }
 
@@ -259,7 +262,7 @@ void TimersManager::clear()
 {
   {
     // Lock mutex and then clear all data structures
-    std::unique_lock<std::mutex> lock(timers_mutex_);
+    rclcpp::Lock lock(timers_mutex_);
     weak_timers_heap_.clear();
 
     timers_updated_ = true;
@@ -273,7 +276,7 @@ void TimersManager::remove_timer(TimerPtr timer)
 {
   bool removed = false;
   {
-    std::unique_lock<std::mutex> lock(timers_mutex_);
+    rclcpp::Lock lock(timers_mutex_);
     removed = weak_timers_heap_.remove_timer(timer);
 
     timers_updated_ = timers_updated_ || removed;