From 063269bf6ada4155d7a6b8b29198b61319ab2078 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 12 Apr 2021 17:35:28 -0300 Subject: [PATCH 1/3] Use a different implementation of mutex two priorities Signed-off-by: Ivan Santiago Paunovic --- .../rclcpp/detail/mutex_two_priorities.hpp | 10 ++--- .../rclcpp/detail/mutex_two_priorities.cpp | 42 +++++++++++++++---- 2 files changed, 40 insertions(+), 12 deletions(-) diff --git a/rclcpp/include/rclcpp/detail/mutex_two_priorities.hpp b/rclcpp/include/rclcpp/detail/mutex_two_priorities.hpp index 6c5f69e98f..270cbb4ae2 100644 --- a/rclcpp/include/rclcpp/detail/mutex_two_priorities.hpp +++ b/rclcpp/include/rclcpp/detail/mutex_two_priorities.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__DETAIL__MUTEX_TWO_PRIORITIES_HPP_ #define RCLCPP__DETAIL__MUTEX_TWO_PRIORITIES_HPP_ +#include #include namespace rclcpp @@ -62,11 +63,10 @@ class MutexTwoPriorities get_low_priority_lockable(); private: - // Implementation detail: the idea here is that only one low priority thread can be - // trying to take the data_ mutex while the others are excluded by the barrier_ mutex. - // All high priority threads are already waiting for the data_ mutex. - std::mutex barrier_; - std::mutex data_; + std::condition_variable cv_; + std::mutex cv_mutex_; + size_t hp_waiting_count_{0u}; + bool data_taken_{false}; }; } // namespace detail diff --git a/rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp b/rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp index e7c7a091e8..58bef01de0 100644 --- a/rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp +++ b/rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp @@ -31,13 +31,34 @@ HighPriorityLockable::HighPriorityLockable(MutexTwoPriorities & parent) void HighPriorityLockable::lock() { - parent_.data_.lock(); + bool did_aument_count{false}; + std::unique_lock guard{parent_.cv_mutex_}; + if (parent_.data_taken_) { + ++parent_.hp_waiting_count_; + did_aument_count = true; + } + while (parent_.data_taken_) { + parent_.cv_.wait(guard); + } + if (did_aument_count) { + --parent_.hp_waiting_count_; + } + parent_.data_taken_ = true; } void HighPriorityLockable::unlock() { - parent_.data_.unlock(); + { + std::lock_guard guard{parent_.cv_mutex_}; + parent_.data_taken_ = false; + } + // We need to notify_all(), and not only one, + // because the notified thread might be low priority + // when a high priority thread is still waiting. + // In that case, the low priority thread will go to sleep again, + // release the mutex, and the high priority thread can continue. + parent_.cv_.notify_all(); } LowPriorityLockable::LowPriorityLockable(MutexTwoPriorities & parent) @@ -47,16 +68,23 @@ LowPriorityLockable::LowPriorityLockable(MutexTwoPriorities & parent) void LowPriorityLockable::lock() { - std::unique_lock barrier_guard{parent_.barrier_}; - parent_.data_.lock(); - barrier_guard.release(); + std::unique_lock guard{parent_.cv_mutex_}; + while (parent_.data_taken_ || parent_.hp_waiting_count_) { + parent_.cv_.wait(guard); + } + parent_.data_taken_ = true; } void LowPriorityLockable::unlock() { - std::lock_guard barrier_guard{parent_.barrier_, std::adopt_lock}; - parent_.data_.unlock(); + { + std::lock_guard guard{parent_.cv_mutex_}; + parent_.data_taken_ = false; + } + // See comment in HighPriorityLockable::unlock() to understand + // why not using notify_one(). + parent_.cv_.notify_all(); } HighPriorityLockable From 91ce2e7911fb804be97ef18adcb3f8bc7aa73851 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 13 Apr 2021 11:06:21 -0300 Subject: [PATCH 2/3] Beautify code Signed-off-by: Ivan Santiago Paunovic --- rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp b/rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp index 58bef01de0..7b2e420f10 100644 --- a/rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp +++ b/rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp @@ -31,16 +31,12 @@ HighPriorityLockable::HighPriorityLockable(MutexTwoPriorities & parent) void HighPriorityLockable::lock() { - bool did_aument_count{false}; std::unique_lock guard{parent_.cv_mutex_}; if (parent_.data_taken_) { ++parent_.hp_waiting_count_; - did_aument_count = true; - } - while (parent_.data_taken_) { - parent_.cv_.wait(guard); - } - if (did_aument_count) { + while (parent_.data_taken_) { + parent_.cv_.wait(guard); + } --parent_.hp_waiting_count_; } parent_.data_taken_ = true; From 909bca576ac3b2d4b3d8bd108efc21eda42ca618 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 13 Apr 2021 11:17:28 -0300 Subject: [PATCH 3/3] Use two conditional variables Signed-off-by: Ivan Santiago Paunovic --- .../rclcpp/detail/mutex_two_priorities.hpp | 3 ++- .../rclcpp/detail/mutex_two_priorities.cpp | 27 +++++++++++-------- 2 files changed, 18 insertions(+), 12 deletions(-) diff --git a/rclcpp/include/rclcpp/detail/mutex_two_priorities.hpp b/rclcpp/include/rclcpp/detail/mutex_two_priorities.hpp index 270cbb4ae2..98118b619f 100644 --- a/rclcpp/include/rclcpp/detail/mutex_two_priorities.hpp +++ b/rclcpp/include/rclcpp/detail/mutex_two_priorities.hpp @@ -63,7 +63,8 @@ class MutexTwoPriorities get_low_priority_lockable(); private: - std::condition_variable cv_; + std::condition_variable hp_cv_; + std::condition_variable lp_cv_; std::mutex cv_mutex_; size_t hp_waiting_count_{0u}; bool data_taken_{false}; diff --git a/rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp b/rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp index 7b2e420f10..8deb864f5f 100644 --- a/rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp +++ b/rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp @@ -35,7 +35,7 @@ HighPriorityLockable::lock() if (parent_.data_taken_) { ++parent_.hp_waiting_count_; while (parent_.data_taken_) { - parent_.cv_.wait(guard); + parent_.hp_cv_.wait(guard); } --parent_.hp_waiting_count_; } @@ -45,16 +45,17 @@ HighPriorityLockable::lock() void HighPriorityLockable::unlock() { + bool notify_lp{false}; { std::lock_guard guard{parent_.cv_mutex_}; parent_.data_taken_ = false; + notify_lp = 0u == parent_.hp_waiting_count_; + } + if (notify_lp) { + parent_.lp_cv_.notify_one(); + } else { + parent_.hp_cv_.notify_one(); } - // We need to notify_all(), and not only one, - // because the notified thread might be low priority - // when a high priority thread is still waiting. - // In that case, the low priority thread will go to sleep again, - // release the mutex, and the high priority thread can continue. - parent_.cv_.notify_all(); } LowPriorityLockable::LowPriorityLockable(MutexTwoPriorities & parent) @@ -66,7 +67,7 @@ LowPriorityLockable::lock() { std::unique_lock guard{parent_.cv_mutex_}; while (parent_.data_taken_ || parent_.hp_waiting_count_) { - parent_.cv_.wait(guard); + parent_.lp_cv_.wait(guard); } parent_.data_taken_ = true; } @@ -74,13 +75,17 @@ LowPriorityLockable::lock() void LowPriorityLockable::unlock() { + bool notify_lp{false}; { std::lock_guard guard{parent_.cv_mutex_}; parent_.data_taken_ = false; + notify_lp = 0u == parent_.hp_waiting_count_; + } + if (notify_lp) { + parent_.lp_cv_.notify_one(); + } else { + parent_.hp_cv_.notify_one(); } - // See comment in HighPriorityLockable::unlock() to understand - // why not using notify_one(). - parent_.cv_.notify_all(); } HighPriorityLockable