From 4870b7761e837ba11fdc7a15177430ce1dc96277 Mon Sep 17 00:00:00 2001 From: hsgwa Date: Fri, 16 Apr 2021 15:17:13 +0900 Subject: [PATCH] backport mutex_two_priorities #1516 Signed-off-by: hsgwa --- rclcpp/CMakeLists.txt | 1 + .../rclcpp/detail/mutex_two_priorities.hpp | 75 +++++++++++++++++++ .../executors/multi_threaded_executor.hpp | 3 + .../rclcpp/detail/mutex_two_priorities.cpp | 75 +++++++++++++++++++ .../executors/multi_threaded_executor.cpp | 18 ++++- 5 files changed, 169 insertions(+), 3 deletions(-) create mode 100644 rclcpp/include/rclcpp/detail/mutex_two_priorities.hpp create mode 100644 rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 26d7e69362..16f82e3a88 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -39,6 +39,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/clock.cpp src/rclcpp/context.cpp src/rclcpp/contexts/default_context.cpp + src/rclcpp/detail/mutex_two_priorities.cpp src/rclcpp/detail/rmw_implementation_specific_payload.cpp src/rclcpp/detail/rmw_implementation_specific_publisher_payload.cpp src/rclcpp/detail/rmw_implementation_specific_subscription_payload.cpp diff --git a/rclcpp/include/rclcpp/detail/mutex_two_priorities.hpp b/rclcpp/include/rclcpp/detail/mutex_two_priorities.hpp new file mode 100644 index 0000000000..6c5f69e98f --- /dev/null +++ b/rclcpp/include/rclcpp/detail/mutex_two_priorities.hpp @@ -0,0 +1,75 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__DETAIL__MUTEX_TWO_PRIORITIES_HPP_ +#define RCLCPP__DETAIL__MUTEX_TWO_PRIORITIES_HPP_ + +#include + +namespace rclcpp +{ +namespace detail +{ +/// \internal A mutex that has two locking mechanism, one with higher priority than the other. +/** + * After the current mutex owner release the lock, a thread that used the high + * priority mechanism will have priority over threads that used the low priority mechanism. + */ +class MutexTwoPriorities +{ +public: + class HighPriorityLockable + { +public: + explicit HighPriorityLockable(MutexTwoPriorities & parent); + + void lock(); + + void unlock(); + +private: + MutexTwoPriorities & parent_; + }; + + class LowPriorityLockable + { +public: + explicit LowPriorityLockable(MutexTwoPriorities & parent); + + void lock(); + + void unlock(); + +private: + MutexTwoPriorities & parent_; + }; + + HighPriorityLockable + get_high_priority_lockable(); + + LowPriorityLockable + 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_; +}; + +} // namespace detail +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__MUTEX_TWO_PRIORITIES_HPP_ diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp index c18df5b7bc..4b99d8c941 100644 --- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp @@ -22,6 +22,7 @@ #include #include +#include "rclcpp/detail/mutex_two_priorities.hpp" #include "rclcpp/executor.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/memory_strategies.hpp" @@ -87,6 +88,8 @@ class MultiThreadedExecutor : public rclcpp::Executor std::chrono::nanoseconds next_exec_timeout_; std::set scheduled_timers_; + static std::unordered_map> wait_mutex_set_; }; } // namespace executors diff --git a/rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp b/rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp new file mode 100644 index 0000000000..e7c7a091e8 --- /dev/null +++ b/rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp @@ -0,0 +1,75 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/detail/mutex_two_priorities.hpp" + +#include + +namespace rclcpp +{ +namespace detail +{ + +using LowPriorityLockable = MutexTwoPriorities::LowPriorityLockable; +using HighPriorityLockable = MutexTwoPriorities::HighPriorityLockable; + +HighPriorityLockable::HighPriorityLockable(MutexTwoPriorities & parent) +: parent_(parent) +{} + +void +HighPriorityLockable::lock() +{ + parent_.data_.lock(); +} + +void +HighPriorityLockable::unlock() +{ + parent_.data_.unlock(); +} + +LowPriorityLockable::LowPriorityLockable(MutexTwoPriorities & parent) +: parent_(parent) +{} + +void +LowPriorityLockable::lock() +{ + std::unique_lock barrier_guard{parent_.barrier_}; + parent_.data_.lock(); + barrier_guard.release(); +} + +void +LowPriorityLockable::unlock() +{ + std::lock_guard barrier_guard{parent_.barrier_, std::adopt_lock}; + parent_.data_.unlock(); +} + +HighPriorityLockable +MutexTwoPriorities::get_high_priority_lockable() +{ + return HighPriorityLockable{*this}; +} + +LowPriorityLockable +MutexTwoPriorities::get_low_priority_lockable() +{ + return LowPriorityLockable{*this}; +} + +} // namespace detail +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index 0dfdc35467..933d5f900c 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -17,13 +17,18 @@ #include #include #include +#include #include #include "rclcpp/utilities.hpp" #include "rclcpp/scope_exit.hpp" +using rclcpp::detail::MutexTwoPriorities; using rclcpp::executors::MultiThreadedExecutor; +std::unordered_map> +MultiThreadedExecutor::wait_mutex_set_; + MultiThreadedExecutor::MultiThreadedExecutor( const rclcpp::ExecutorOptions & options, size_t number_of_threads, @@ -33,6 +38,7 @@ MultiThreadedExecutor::MultiThreadedExecutor( yield_before_execute_(yield_before_execute), next_exec_timeout_(next_exec_timeout) { + wait_mutex_set_[this] = std::make_shared(); number_of_threads_ = number_of_threads ? number_of_threads : std::thread::hardware_concurrency(); if (number_of_threads_ == 0) { number_of_threads_ = 1; @@ -51,7 +57,9 @@ MultiThreadedExecutor::spin() std::vector threads; size_t thread_id = 0; { - std::lock_guard wait_lock(wait_mutex_); + auto wait_mutex = MultiThreadedExecutor::wait_mutex_set_[this]; + auto low_priority_wait_mutex = wait_mutex->get_low_priority_lockable(); + std::lock_guard wait_lock(low_priority_wait_mutex); for (; thread_id < number_of_threads_ - 1; ++thread_id) { auto func = std::bind(&MultiThreadedExecutor::run, this, thread_id); threads.emplace_back(func); @@ -76,7 +84,9 @@ MultiThreadedExecutor::run(size_t) while (rclcpp::ok(this->context_) && spinning.load()) { rclcpp::AnyExecutable any_exec; { - std::lock_guard wait_lock(wait_mutex_); + auto wait_mutex = MultiThreadedExecutor::wait_mutex_set_[this]; + auto low_priority_wait_mutex = wait_mutex->get_low_priority_lockable(); + std::lock_guard wait_lock(low_priority_wait_mutex); if (!rclcpp::ok(this->context_) || !spinning.load()) { return; } @@ -103,7 +113,9 @@ MultiThreadedExecutor::run(size_t) execute_any_executable(any_exec); if (any_exec.timer) { - std::lock_guard wait_lock(wait_mutex_); + auto wait_mutex = MultiThreadedExecutor::wait_mutex_set_[this]; + auto high_priority_wait_mutex = wait_mutex->get_high_priority_lockable(); + std::lock_guard wait_lock(high_priority_wait_mutex); auto it = scheduled_timers_.find(any_exec.timer); if (it != scheduled_timers_.end()) { scheduled_timers_.erase(it);