diff --git a/CMakeLists.txt b/CMakeLists.txt index d0c47554..8540aa63 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -98,6 +98,11 @@ if(BUILD_TESTING) ament_add_gmock(test_async_function_handler test/test_async_function_handler.cpp) target_link_libraries(test_async_function_handler realtime_tools thread_priority) ament_target_dependencies(test_async_function_handler lifecycle_msgs rclcpp_lifecycle) + + if(NOT WIN32) + ament_add_gmock(realtime_mutex_tests test/realtime_mutex_tests.cpp) + target_link_libraries(realtime_mutex_tests realtime_tools) + endif() endif() # Install diff --git a/include/realtime_tools/mutex.hpp b/include/realtime_tools/mutex.hpp new file mode 100644 index 00000000..f781d232 --- /dev/null +++ b/include/realtime_tools/mutex.hpp @@ -0,0 +1,201 @@ +// Copyright 2024 PAL Robotics S.L. +// +// 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. + +/// \author Sai Kishor Kothakota + +#ifndef REALTIME_TOOLS__MUTEX_HPP_ +#define REALTIME_TOOLS__MUTEX_HPP_ + +#ifdef _WIN32 +#error "The mutex.hpp header is not supported on Windows platforms" +#endif + +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief A pthread mutex wrapper that provides a mutex with the priority inheritance + * protocol and a priority ceiling of 99. + * The mutex is also error checked and robust. + * This mutex is intended to be used in real-time contexts. + * @note This mutex is not recursive. + */ +namespace realtime_tools +{ +namespace detail +{ +struct error_mutex_type_t +{ + static constexpr int value = PTHREAD_MUTEX_ERRORCHECK; +}; + +struct recursive_mutex_type_t +{ + static constexpr int value = PTHREAD_MUTEX_RECURSIVE; +}; + +struct stalled_robustness_t +{ + static constexpr int value = PTHREAD_MUTEX_STALLED; +}; + +struct robust_robustness_t +{ + static constexpr int value = PTHREAD_MUTEX_ROBUST; +}; +/** + * @brief A class template that provides a pthread mutex with the priority inheritance protocol + * + * @tparam MutexType The type of the mutex. It can be one of the following: PTHREAD_MUTEX_NORMAL, PTHREAD_MUTEX_RECURSIVE, PTHREAD_MUTEX_ERRORCHECK, PTHREAD_MUTEX_DEFAULT + * @tparam MutexRobustness The robustness of the mutex. It can be one of the following: PTHREAD_MUTEX_STALLED, PTHREAD_MUTEX_ROBUST + */ +template +class mutex +{ +public: + using native_handle_type = pthread_mutex_t *; + using type = MutexType; + using robustness = MutexRobustness; + + mutex() + { + pthread_mutexattr_t attr; + + const auto attr_destroy = [](pthread_mutexattr_t * mutex_attr) { + // Destroy the mutex attributes + const auto res_destroy = pthread_mutexattr_destroy(mutex_attr); + if (res_destroy != 0) { + throw std::system_error( + res_destroy, std::generic_category(), "Failed to destroy mutex attribute"); + } + }; + using attr_cleanup_t = std::unique_ptr; + auto attr_cleanup = attr_cleanup_t(&attr, attr_destroy); + + // Initialize the mutex attributes + const auto res_attr = pthread_mutexattr_init(&attr); + if (res_attr != 0) { + throw std::system_error( + res_attr, std::system_category(), "Failed to initialize mutex attribute"); + } + + // Set the mutex type to MutexType + const auto res_type = pthread_mutexattr_settype(&attr, MutexType::value); + + if (res_type != 0) { + throw std::system_error(res_type, std::system_category(), "Failed to set mutex type"); + } + + // Set the mutex attribute to use the protocol PTHREAD_PRIO_INHERIT + const auto res_protocol = pthread_mutexattr_setprotocol(&attr, PTHREAD_PRIO_INHERIT); + if (res_protocol != 0) { + throw std::system_error(res_protocol, std::system_category(), "Failed to set mutex protocol"); + } + + // Set the mutex attribute robustness to MutexRobustness + const auto res_robust = pthread_mutexattr_setrobust(&attr, MutexRobustness::value); + if (res_robust != 0) { + throw std::system_error(res_robust, std::system_category(), "Failed to set mutex robustness"); + } + + // Initialize the mutex with the attributes + const auto res_init = pthread_mutex_init(&mutex_, &attr); + if (res_init != 0) { + throw std::system_error(res_init, std::system_category(), "Failed to initialize mutex"); + } + } + + ~mutex() + { + const auto res = pthread_mutex_destroy(&mutex_); + if (res != 0) { + std::cerr << "Failed to destroy mutex : " << std::strerror(res) << std::endl; + } + } + + mutex(const mutex &) = delete; + + mutex & operator=(const mutex &) = delete; + + native_handle_type native_handle() noexcept { return &mutex_; } + + void lock() + { + const auto res = pthread_mutex_lock(&mutex_); + if (res == 0) { + return; + } + if (res == EOWNERDEAD) { + const auto res_consistent = pthread_mutex_consistent(&mutex_); + if (res_consistent != 0) { + throw std::runtime_error( + std::string("Failed to make mutex consistent : ") + std::strerror(res_consistent)); + } + std::cerr << "Mutex owner died, but the mutex is consistent now. This shouldn't happen!" + << std::endl; + } else if (res == EDEADLK) { + throw std::system_error(res, std::system_category(), "Deadlock detected"); + } else { + throw std::runtime_error(std::string("Failed to lock mutex : ") + std::strerror(res)); + } + } + + void unlock() noexcept + { + // As per the requirements of BasicLockable concept, unlock should not throw + const auto res = pthread_mutex_unlock(&mutex_); + if (res != 0) { + std::cerr << "Failed to unlock mutex : " << std::strerror(res) << std::endl; + } + } + + bool try_lock() + { + const auto res = pthread_mutex_trylock(&mutex_); + if (res == 0) { + return true; + } + if (res == EBUSY) { + return false; + } else if (res == EOWNERDEAD) { + const auto res_consistent = pthread_mutex_consistent(&mutex_); + if (res_consistent != 0) { + throw std::runtime_error( + std::string("Failed to make mutex consistent : ") + std::strerror(res_consistent)); + } + std::cerr << "Mutex owner died, but the mutex is consistent now. This shouldn't happen!" + << std::endl; + } else if (res == EDEADLK) { + throw std::system_error(res, std::system_category(), "Deadlock detected"); + } else { + throw std::runtime_error(std::string("Failed to try lock mutex : ") + std::strerror(res)); + } + return true; + } + +private: + pthread_mutex_t mutex_; +}; +} // namespace detail +using prio_inherit_mutex = detail::mutex; +using prio_inherit_recursive_mutex = + detail::mutex; +} // namespace realtime_tools + +#endif // REALTIME_TOOLS__MUTEX_HPP_ diff --git a/test/realtime_mutex_tests.cpp b/test/realtime_mutex_tests.cpp new file mode 100644 index 00000000..691529c2 --- /dev/null +++ b/test/realtime_mutex_tests.cpp @@ -0,0 +1,341 @@ +// Copyright 2024 PAL Robotics S.L. +// +// 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. + +/// \author Sai Kishor Kothakota + +#include + +#include +#include + +#include + +TEST(PriorityInheritanceMutexTests, lock_unlock) +{ + // The mutex is locked and unlocked correctly + realtime_tools::prio_inherit_mutex mutex; + mutex.lock(); + mutex.unlock(); +} + +TEST(PriorityInheritanceMutexTests, lock_unlock_multithreaded) +{ + // The mutex is locked and unlocked correctly in a multithreaded environment + realtime_tools::prio_inherit_mutex mutex; + std::thread t1([&mutex]() { + mutex.lock(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + mutex.unlock(); + }); + std::thread t2([&mutex]() { + mutex.lock(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + mutex.unlock(); + }); + t1.join(); + t2.join(); +} + +TEST(PriorityInheritanceMutexTests, recursive_lock_lock_unlock_multithreaded) +{ + // The mutex is locked and unlocked correctly in a multithreaded environment + realtime_tools::prio_inherit_recursive_mutex mutex; + std::thread t1([&mutex]() { + mutex.lock(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + mutex.lock(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + mutex.unlock(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + mutex.unlock(); + }); + std::thread t2([&mutex]() { + mutex.lock(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + mutex.unlock(); + }); + t1.join(); + t2.join(); +} + +TEST(PriorityInheritanceMutexTests, lock_unlock_multithreaded_multiple_mutexes) +{ + // The mutex is locked and unlocked correctly in a multithreaded environment with multiple mutexes + realtime_tools::prio_inherit_mutex mutex1; + realtime_tools::prio_inherit_mutex mutex2; + std::thread t1([&mutex1, &mutex2]() { + mutex1.lock(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + mutex1.unlock(); + mutex2.lock(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + mutex2.unlock(); + }); + std::thread t2([&mutex1, &mutex2]() { + mutex1.lock(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + mutex1.unlock(); + mutex2.lock(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + mutex2.unlock(); + }); + t1.join(); + t2.join(); +} + +TEST(PriorityInheritanceMutexTests, lock_unlock_multithreaded_multiple_mutexes_different_types) +{ + // The mutex is locked and unlocked correctly in a multithreaded environment with multiple mutexes + realtime_tools::prio_inherit_mutex mutex1; + realtime_tools::prio_inherit_recursive_mutex mutex2; + std::thread t1([&mutex1, &mutex2]() { + mutex1.lock(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + mutex1.unlock(); + mutex2.lock(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + mutex2.unlock(); + }); + std::thread t2([&mutex1, &mutex2]() { + mutex1.lock(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + mutex1.unlock(); + mutex2.lock(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + mutex2.unlock(); + }); + t1.join(); + t2.join(); +} + +TEST(PriorityInheritanceMutexTests, lock_unlock_recursive_mutex) +{ + // Test to check that the mutex is recursive + realtime_tools::prio_inherit_recursive_mutex mutex; + mutex.lock(); + mutex.lock(); + mutex.unlock(); + mutex.unlock(); +} + +TEST(PriorityInheritanceMutexTests, lock_unlock_multithreaded_recursive_mutex_multiple_mutexes) +{ + realtime_tools::prio_inherit_recursive_mutex mutex1; + realtime_tools::prio_inherit_recursive_mutex mutex2; + std::thread t1([&mutex1, &mutex2]() { + mutex1.lock(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + mutex1.unlock(); + mutex2.lock(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + mutex2.unlock(); + }); + std::thread t2([&mutex1, &mutex2]() { + mutex1.lock(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + mutex1.unlock(); + mutex2.lock(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + mutex2.unlock(); + }); + t1.join(); + t2.join(); +} + +TEST(PriorityInheritanceMutexTests, lock_unlock_multithreaded_mutex_one_thread_dies) +{ + realtime_tools::prio_inherit_mutex mutex; + std::thread t1([&mutex]() { + mutex.lock(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + // simulating no unlock. Due to the robustness of the mutex, the mutex should be consistent + }); + std::thread t2([&mutex]() { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + mutex.lock(); + mutex.unlock(); + }); + t1.join(); + t2.join(); + mutex.lock(); + mutex.unlock(); +} + +TEST(PriorityInheritanceMutexTests, lock_unlock_multithreaded_recursive_mutex_one_thread_dies) +{ + realtime_tools::prio_inherit_recursive_mutex mutex; + std::thread t1([&mutex]() { + mutex.lock(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + // simulating no unlock. Due to the robustness of the mutex, the mutex should be consistent + }); + std::thread t2([&mutex]() { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + mutex.lock(); + mutex.unlock(); + }); + t1.join(); + t2.join(); + mutex.lock(); + mutex.unlock(); +} + +TEST(PriorityInheritanceMutexTests, lock_guard_mutex) +{ + realtime_tools::prio_inherit_mutex mutex; + { + std::lock_guard lock(mutex); + } + + realtime_tools::prio_inherit_recursive_mutex recursive_mutex; + { + std::lock_guard lock(recursive_mutex); + } +} + +TEST(PriorityInheritanceMutexTests, unique_lock_mutex) +{ + realtime_tools::prio_inherit_mutex mutex; + { + std::unique_lock lock(mutex); + } + + realtime_tools::prio_inherit_recursive_mutex recursive_mutex; + { + std::unique_lock lock(recursive_mutex); + } +} + +TEST(PriorityInheritanceMutexTests, try_lock_mutex) +{ + { + realtime_tools::prio_inherit_mutex mutex; + ASSERT_TRUE(mutex.try_lock()); + ASSERT_THROW(mutex.try_lock(), std::system_error) + << "Mutex is already locked in the same thread"; + std::thread t([&mutex]() { + ASSERT_FALSE(mutex.try_lock()) + << "try_lock should pass when checking from a different thread"; + }); + t.join(); + mutex.unlock(); + ASSERT_TRUE(mutex.try_lock()); + mutex.unlock(); + } + + { + realtime_tools::prio_inherit_recursive_mutex recursive_mutex; + ASSERT_TRUE(recursive_mutex.try_lock()); + ASSERT_TRUE(recursive_mutex.try_lock()); + ASSERT_TRUE(recursive_mutex.try_lock()); + recursive_mutex.unlock(); + recursive_mutex.unlock(); + recursive_mutex.unlock(); + } +} + +TEST(PriorityInheritanceMutexTests, standard_lock_test) +{ + realtime_tools::prio_inherit_mutex mutex1; + realtime_tools::prio_inherit_mutex mutex2; + { + std::lock(mutex1, mutex2); + // do work + mutex1.unlock(); + mutex2.unlock(); + } + { + std::scoped_lock lock(mutex1, mutex2); + } + ASSERT_TRUE(mutex1.try_lock()); + ASSERT_TRUE(mutex2.try_lock()); + mutex1.unlock(); + mutex2.unlock(); +} + +TEST(PriorityInheritanceMutexTests, native_handle_mutex) +{ + { + realtime_tools::prio_inherit_mutex mutex; + auto native_handle = mutex.native_handle(); + ASSERT_NE(native_handle, nullptr); + } + + { + realtime_tools::prio_inherit_recursive_mutex recursive_mutex; + auto native_handle = recursive_mutex.native_handle(); + ASSERT_NE(native_handle, nullptr); + } +} + +TEST(PriorityInheritanceMutexTests, test_mutex_lock_functionality) +{ + // Trying to lock again should throw an exception + realtime_tools::prio_inherit_mutex mutex; + mutex.lock(); + ASSERT_THROW(mutex.lock(), std::system_error); + mutex.unlock(); + ASSERT_NO_THROW(mutex.lock()); + ASSERT_THROW(mutex.try_lock(), std::system_error); + mutex.unlock(); + ASSERT_NO_THROW(mutex.try_lock()); + mutex.unlock(); +} + +TEST(PriorityInheritanceMutexTests, test_lock_constructors) +{ + realtime_tools::prio_inherit_mutex mutex; + { + std::unique_lock lock(mutex, std::defer_lock); + ASSERT_FALSE(lock.owns_lock()); + lock.lock(); + ASSERT_TRUE(lock.owns_lock()); + lock.unlock(); + } + { + std::unique_lock lock(mutex, std::try_to_lock); + ASSERT_TRUE(lock.owns_lock()); + } +} + +TEST(PriorityInheritanceMutexTests, test_deadlock_detection) +{ + realtime_tools::prio_inherit_mutex mutex; + mutex.lock(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ASSERT_THROW(mutex.try_lock(), std::system_error); + ASSERT_THROW(mutex.lock(), std::system_error); + // In a different thread, try to lock the mutex should not throw an exception + std::thread t([&mutex]() { ASSERT_FALSE(mutex.try_lock()); }); + t.join(); + mutex.unlock(); +} + +TEST(PriorityInheritanceMutexTests, test_mutex_reflection) +{ + static_assert( + std::is_same< + realtime_tools::prio_inherit_mutex::type, + realtime_tools::detail::error_mutex_type_t>::value == true); + static_assert( + std::is_same< + realtime_tools::prio_inherit_recursive_mutex::type, + realtime_tools::detail::recursive_mutex_type_t>::value == true); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}