Avoid priority inversions when using FIFO scheduling

CMakeLists.txt

@@ -28,6 +28,7 @@ add_library(${PROJECT_NAME}
   src/filesystem_helper.cpp
   src/find_library.cpp
   src/env.cpp
+  src/mutex.cpp
   src/shared_library.cpp)
 target_include_directories(${PROJECT_NAME} PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
@@ -36,6 +37,18 @@ if(WIN32)
   target_compile_definitions(${PROJECT_NAME}
     PRIVATE "RCPPUTILS_BUILDING_LIBRARY")
 endif()
+
+option(USE_PI_MUTEX "Enables priority inheritance mutexes." ON)
+if(USE_PI_MUTEX)
+  if(CMAKE_SYSTEM_NAME STREQUAL Linux OR
+    CMAKE_SYSTEM_NAME STREQUAL VxWorks OR
+    CMAKE_SYSTEM_NAME STREQUAL QNX)
+    target_compile_definitions(${PROJECT_NAME} PUBLIC "RCPPUTILS_USE_PIMUTEX")
+  else()
+    message("Mutexes with priority inheritance are not supported on your system. Using std mutexes.")
+  endif()
+endif()
+
 ament_target_dependencies(${PROJECT_NAME} rcutils)
 
 # Export old-style CMake variables
@@ -73,6 +86,9 @@ if(BUILD_TESTING)
   ament_add_gtest(test_join test/test_join.cpp)
   target_link_libraries(test_join ${PROJECT_NAME})
 
+  ament_add_gtest(test_mutex test/test_mutex.cpp)
+  target_link_libraries(test_mutex ${PROJECT_NAME})
+
   ament_add_gtest(test_time test/test_time.cpp)
   target_link_libraries(test_time ${PROJECT_NAME})
   ament_target_dependencies(test_time rcutils)

Doxyfile

@@ -23,6 +23,7 @@ EXPAND_ONLY_PREDEF     = YES
 PREDEFINED             += __declspec(x)=
 PREDEFINED             += RCPPUTILS_PUBLIC=
 PREDEFINED             += RCPPUTILS_PUBLIC_TYPE=
+PREDEFINED             += RCPPUTILS_USE_PIMUTEX
 
 # Tag files that do not exist will produce a warning and cross-project linking will not work.
 TAGFILES += "../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/"

README.md

@@ -17,6 +17,7 @@ This package currently contains:
 * String helpers
 * File system helpers
 * Type traits helpers
+* Mutex classes that support thread priority inheritance
 * Class that dynamically loads, unloads and get symbols from shared libraries at run-time.
 
 Features are described in more detail at [docs/FEATURES.md](docs/FEATURES.md)

include/rcpputils/mutex.hpp

@@ -0,0 +1,70 @@
+// Copyright 2023 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 RCPPUTILS__MUTEX_HPP_
+#define RCPPUTILS__MUTEX_HPP_
+
+#include <mutex>
+
+#include "rcpputils/visibility_control.hpp"
+
+namespace rcpputils
+{
+#ifndef RCPPUTILS_USE_PIMUTEX
+
+// Fallback code path
+using PIMutex = std::mutex;
+using RecursivePIMutex = std::recursive_mutex;
+
+#else
+
+/**
+ * Mutex with enabled thread priority inheritance.
+ * In case your OS does not support thread priority inheritance for mutexes, or
+ * the CMake option USE_PI_MUTEX is turned off, then this class is just an alias for
+ * std::mutex.
+  **/
+class PIMutex : public std::mutex
+{
+public:
+  /// Creates a mutex with priority inheritance enabled
+  RCPPUTILS_PUBLIC
+  PIMutex();
+
+  /// Releases the mutex
+  RCPPUTILS_PUBLIC
+  ~PIMutex();
+};
+
+/**
+ * Recursive mutex with enabled thread priority inheritance.
+ * In case your OS does not support thread priority inheritance for mutexes, or
+ * the CMake option USE_PI_MUTEX is turned off, then this class is just an alias for
+ * std::recursive_mutex.
+ **/
+class RecursivePIMutex : public std::recursive_mutex
+{
+public:
+  /// Creates a recursive mutex with priority inheritance enabled
+  RCPPUTILS_PUBLIC
+  RecursivePIMutex();
+
+  /// Releases the mutex
+  RCPPUTILS_PUBLIC
+  ~RecursivePIMutex();
+};
+
+#endif  // RCPPUTILS_USE_PIMUTEX
+}  // namespace rcpputils
+#endif  // RCPPUTILS__MUTEX_HPP_

src/mutex.cpp

@@ -0,0 +1,72 @@
+// Copyright 2023 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 "rcpputils/mutex.hpp"
+
+#ifdef RCPPUTILS_USE_PIMUTEX
+
+#include <pthread.h>
+
+namespace rcpputils
+{
+
+PIMutex::PIMutex()
+{
+  // Destroy the underlying mutex
+  pthread_mutex_destroy(native_handle());
+
+  // Create mutex attribute with desired settings
+  pthread_mutexattr_t attr;
+  pthread_mutexattr_init(&attr);
+  pthread_mutexattr_setprotocol(&attr, PTHREAD_PRIO_INHERIT);
+
+  // Initialize the underlying mutex
+  if (pthread_mutex_init(native_handle(), &attr) != 0) {
+    throw std::runtime_error("Mutex initialization failed.");
+  }
+
+  // The attribute object isn't needed any more
+  pthread_mutexattr_destroy(&attr);
+}
+
+RecursivePIMutex::RecursivePIMutex()
+{
+  // Destroy the underlying mutex
+  pthread_mutex_destroy(native_handle());
+
+  // Create mutex attribute with desired settings
+  pthread_mutexattr_t attr;
+  pthread_mutexattr_init(&attr);
+  pthread_mutexattr_setprotocol(&attr, PTHREAD_PRIO_INHERIT);
+  pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE);
+
+  // Initialize the underlying mutex
+  if (pthread_mutex_init(native_handle(), &attr) != 0) {
+    throw std::runtime_error("Recursive mutex initialization failed.");
+  }
+
+  // The attribute object isn't needed any more
+  pthread_mutexattr_destroy(&attr);
+}
+
+PIMutex::~PIMutex()
+{
+}
+
+RecursivePIMutex::~RecursivePIMutex()
+{
+}
+
+}  // namespace rcpputils
+#endif  // RCPPUTILS_USE_PIMUTEX