diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 561beb5d3e..a05436a4de 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..28504620c5 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,7 +82,7 @@ class MultiThreadedExecutor : public rclcpp::Executor private: RCLCPP_DISABLE_COPY(MultiThreadedExecutor) - std::mutex wait_mutex_; + detail::MutexTwoPriorities wait_mutex_; size_t number_of_threads_; bool yield_before_execute_; std::chrono::nanoseconds next_exec_timeout_; 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..a9b01b0c34 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -22,6 +22,7 @@ #include "rclcpp/utilities.hpp" #include "rclcpp/scope_exit.hpp" +using rclcpp::detail::MutexTwoPriorities; using rclcpp::executors::MultiThreadedExecutor; MultiThreadedExecutor::MultiThreadedExecutor( @@ -51,7 +52,8 @@ MultiThreadedExecutor::spin() std::vector threads; size_t thread_id = 0; { - std::lock_guard wait_lock(wait_mutex_); + 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 +78,8 @@ MultiThreadedExecutor::run(size_t) while (rclcpp::ok(this->context_) && spinning.load()) { rclcpp::AnyExecutable any_exec; { - std::lock_guard wait_lock(wait_mutex_); + 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 +106,8 @@ MultiThreadedExecutor::run(size_t) execute_any_executable(any_exec); if (any_exec.timer) { - std::lock_guard wait_lock(wait_mutex_); + 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);