diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 26d7e69..16f82e3 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 0000000..98118b6 --- /dev/null +++ b/rclcpp/include/rclcpp/detail/mutex_two_priorities.hpp @@ -0,0 +1,76 @@ +// 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 +#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: + std::condition_variable hp_cv_; + std::condition_variable lp_cv_; + std::mutex cv_mutex_; + size_t hp_waiting_count_{0u}; + bool data_taken_{false}; +}; + +} // 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 c18df5b..967875b 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" @@ -81,12 +82,17 @@ protected: private: RCLCPP_DISABLE_COPY(MultiThreadedExecutor) - std::mutex wait_mutex_; + std::mutex wait_mutex_; // Unused. Leave it for ABI compatibility. size_t number_of_threads_; bool yield_before_execute_; std::chrono::nanoseconds next_exec_timeout_; std::set scheduled_timers_; + static std::unordered_map> wait_mutex_set_; + static std::mutex shared_wait_mutex_; + // These variables are declared as static variables for ABI-compatibiliity. + // And they mimic member variables needed to backport from master. }; } // 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 0000000..8deb864 --- /dev/null +++ b/rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp @@ -0,0 +1,104 @@ +// 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() +{ + std::unique_lock guard{parent_.cv_mutex_}; + if (parent_.data_taken_) { + ++parent_.hp_waiting_count_; + while (parent_.data_taken_) { + parent_.hp_cv_.wait(guard); + } + --parent_.hp_waiting_count_; + } + parent_.data_taken_ = true; +} + +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(); + } +} + +LowPriorityLockable::LowPriorityLockable(MutexTwoPriorities & parent) +: parent_(parent) +{} + +void +LowPriorityLockable::lock() +{ + std::unique_lock guard{parent_.cv_mutex_}; + while (parent_.data_taken_ || parent_.hp_waiting_count_) { + parent_.lp_cv_.wait(guard); + } + parent_.data_taken_ = true; +} + +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(); + } +} + +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 0dfdc35..0b262ab 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -17,13 +17,19 @@ #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_; +std::mutex MultiThreadedExecutor::shared_wait_mutex_; + MultiThreadedExecutor::MultiThreadedExecutor( const rclcpp::ExecutorOptions & options, size_t number_of_threads, @@ -33,6 +39,11 @@ MultiThreadedExecutor::MultiThreadedExecutor( yield_before_execute_(yield_before_execute), next_exec_timeout_(next_exec_timeout) { + { + std::lock_guard wait_lock( + MultiThreadedExecutor::shared_wait_mutex_); + 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 +62,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 +89,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 +118,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);