Avoid priority inversions in rclcpp

rclcpp/include/rclcpp/callback_group.hpp

@@ -16,9 +16,7 @@
 #define RCLCPP__CALLBACK_GROUP_HPP_
 
 #include <atomic>
-#include <functional>
-#include <memory>
-#include <mutex>
+#include <string>
 #include <vector>
 
 #include "rclcpp/client.hpp"
@@ -30,6 +28,7 @@
 #include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rclcpp/waitable.hpp"
+#include "rcpputils/mutex.hpp"
 
 namespace rclcpp
 {
@@ -221,7 +220,7 @@ class CallbackGroup
 
   CallbackGroupType type_;
   // Mutex to protect the subsequent vectors of pointers.
-  mutable std::mutex mutex_;
+  mutable rcpputils::PIMutex mutex_;
   std::atomic_bool associated_with_executor_;
   std::vector<rclcpp::SubscriptionBase::WeakPtr> subscription_ptrs_;
   std::vector<rclcpp::TimerBase::WeakPtr> timer_ptrs_;

rclcpp/include/rclcpp/client.hpp

@@ -19,7 +19,6 @@
 #include <future>
 #include <unordered_map>
 #include <memory>
-#include <mutex>
 #include <optional>  // NOLINT, cpplint doesn't think this is a cpp std header
 #include <sstream>
 #include <string>
@@ -45,6 +44,8 @@
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
 
+#include "rcpputils/mutex.hpp"
+
 #include "rmw/error_handling.h"
 #include "rmw/impl/cpp/demangle.hpp"
 #include "rmw/rmw.h"
@@ -364,7 +365,7 @@ class ClientBase
 
   std::atomic<bool> in_use_by_wait_set_{false};
 
-  std::recursive_mutex callback_mutex_;
+  rcpputils::RecursivePIMutex callback_mutex_;
   std::function<void(size_t)> on_new_response_callback_{nullptr};
 };
 
@@ -830,7 +831,7 @@ class Client : public ClientBase
       std::chrono::time_point<std::chrono::system_clock>,
       CallbackInfoVariant>>
   pending_requests_;
-  std::mutex pending_requests_mutex_;
+  rcpputils::PIMutex pending_requests_mutex_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/clock.hpp

@@ -17,7 +17,6 @@
 
 #include <functional>
 #include <memory>
-#include <mutex>
 
 #include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/macros.hpp"
@@ -27,6 +26,7 @@
 #include "rcl/time.h"
 #include "rcutils/time.h"
 #include "rcutils/types/rcutils_ret.h"
+#include "rcpputils/mutex.hpp"
 
 namespace rclcpp
 {
@@ -204,7 +204,7 @@ class Clock
 
   /// Get the clock's mutex
   RCLCPP_PUBLIC
-  std::mutex &
+  rcpputils::PIMutex &
   get_clock_mutex() noexcept;
 
   // Add a callback to invoke if the jump threshold is exceeded.

rclcpp/include/rclcpp/context.hpp

@@ -18,7 +18,6 @@
 #include <condition_variable>
 #include <functional>
 #include <memory>
-#include <mutex>
 #include <string>
 #include <typeindex>
 #include <typeinfo>
@@ -33,6 +32,7 @@
 #include "rclcpp/init_options.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rcpputils/mutex.hpp"
 
 namespace rclcpp
 {
@@ -377,15 +377,15 @@ class Context : public std::enable_shared_from_this<Context>
   std::recursive_mutex sub_contexts_mutex_;
 
   std::unordered_set<std::shared_ptr<OnShutdownCallback>> on_shutdown_callbacks_;
-  mutable std::mutex on_shutdown_callbacks_mutex_;
+  mutable rcpputils::PIMutex on_shutdown_callbacks_mutex_;
 
   std::unordered_set<std::shared_ptr<PreShutdownCallback>> pre_shutdown_callbacks_;
-  mutable std::mutex pre_shutdown_callbacks_mutex_;
+  mutable rcpputils::PIMutex pre_shutdown_callbacks_mutex_;
 
   /// Condition variable for timed sleep (see sleep_for).
   std::condition_variable interrupt_condition_variable_;
   /// Mutex for protecting the global condition variable.
-  std::mutex interrupt_mutex_;
+  rcpputils::PIMutex interrupt_mutex_;
 
   /// Keep shared ownership of global vector of weak contexts
   std::shared_ptr<WeakContextsWrapper> weak_contexts_;

rclcpp/include/rclcpp/executor.hpp

@@ -23,13 +23,13 @@
 #include <list>
 #include <map>
 #include <memory>
-#include <mutex>
 #include <string>
 #include <vector>
 
 #include "rcl/guard_condition.h"
 #include "rcl/wait.h"
 #include "rcpputils/scope_exit.hpp"
+#include "rcpputils/mutex.hpp"
 
 #include "rclcpp/context.hpp"
 #include "rclcpp/contexts/default_context.hpp"
@@ -545,7 +545,7 @@ class Executor
   rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set();
 
   // Mutex to protect the subsequent memory_strategy_.
-  mutable std::mutex mutex_;
+  mutable rcpputils::PIMutex mutex_;
 
   /// The memory strategy: an interface for handling user-defined memory allocation strategies.
   memory_strategy::MemoryStrategy::SharedPtr

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -17,7 +17,6 @@
 
 #include <chrono>
 #include <memory>
-#include <mutex>
 #include <set>
 #include <thread>
 #include <unordered_map>
@@ -27,6 +26,8 @@
 #include "rclcpp/memory_strategies.hpp"
 #include "rclcpp/visibility_control.hpp"
 
+#include "rcpputils/mutex.hpp"
+
 namespace rclcpp
 {
 namespace executors
@@ -81,7 +82,7 @@ class MultiThreadedExecutor : public rclcpp::Executor
 private:
   RCLCPP_DISABLE_COPY(MultiThreadedExecutor)
 
-  std::mutex wait_mutex_;
+  rcpputils::PIMutex wait_mutex_;
   size_t number_of_threads_;
   bool yield_before_execute_;
   std::chrono::nanoseconds next_exec_timeout_;

rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp

@@ -30,6 +30,8 @@
 #include "rclcpp/visibility_control.hpp"
 #include "rclcpp/waitable.hpp"
 
+#include "rcpputils/mutex.hpp"
+
 namespace rclcpp
 {
 namespace executors
@@ -338,7 +340,7 @@ class StaticExecutorEntitiesCollector final
   std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
 
   // Mutex to protect vector of new nodes.
-  std::mutex new_nodes_mutex_;
+  rcpputils::PIMutex new_nodes_mutex_;
   std::vector<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> new_nodes_;
 
   /// Wait set for managing entities that the rmw layer waits on.

rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp

@@ -15,7 +15,6 @@
 #ifndef RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_
 #define RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_
 
-#include <mutex>
 #include <stdexcept>
 #include <utility>
 #include <vector>
@@ -26,6 +25,8 @@
 #include "rclcpp/macros.hpp"
 #include "rclcpp/visibility_control.hpp"
 
+#include "rcpputils/mutex.hpp"
+
 namespace rclcpp
 {
 namespace experimental
@@ -181,7 +182,7 @@ class RingBufferImplementation : public BufferImplementationBase<BufferT>
   size_t read_index_;
   size_t size_;
 
-  mutable std::mutex mutex_;
+  mutable rcpputils::PIMutex mutex_;
 };
 
 }  // namespace buffers

rclcpp/include/rclcpp/graph_listener.hpp

@@ -17,7 +17,6 @@
 
 #include <atomic>
 #include <memory>
-#include <mutex>
 #include <thread>
 #include <vector>
 
@@ -28,6 +27,7 @@
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_graph_interface.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rcpputils/mutex.hpp"
 
 namespace rclcpp
 {
@@ -187,10 +187,10 @@ class GraphListener : public std::enable_shared_from_this<GraphListener>
   std::thread listener_thread_;
   bool is_started_;
   std::atomic_bool is_shutdown_;
-  mutable std::mutex shutdown_mutex_;
+  mutable rcpputils::PIMutex shutdown_mutex_;
 
-  mutable std::mutex node_graph_interfaces_barrier_mutex_;
-  mutable std::mutex node_graph_interfaces_mutex_;
+  mutable rcpputils::PIMutex node_graph_interfaces_barrier_mutex_;
+  mutable rcpputils::PIMutex node_graph_interfaces_mutex_;
   std::vector<rclcpp::node_interfaces::NodeGraphInterface *> node_graph_interfaces_;
 
   rclcpp::GuardCondition interrupt_guard_condition_;

rclcpp/include/rclcpp/init_options.hpp

@@ -16,10 +16,10 @@
 #define RCLCPP__INIT_OPTIONS_HPP_
 
 #include <memory>
-#include <mutex>
 
 #include "rcl/init_options.h"
 #include "rclcpp/visibility_control.hpp"
+#include "rcpputils/mutex.hpp"
 
 namespace rclcpp
 {
@@ -104,7 +104,7 @@ class InitOptions
   void
   finalize_init_options_impl();
 
-  mutable std::mutex init_options_mutex_;
+  mutable rcpputils::PIMutex init_options_mutex_;
   std::unique_ptr<rcl_init_options_t> init_options_;
   bool initialize_logging_{true};
 };

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -17,7 +17,6 @@
 
 #include <atomic>
 #include <memory>
-#include <mutex>
 #include <string>
 #include <vector>
 
@@ -28,6 +27,8 @@
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/visibility_control.hpp"
 
+#include "rcpputils/mutex.hpp"
+
 namespace rclcpp
 {
 namespace node_interfaces
@@ -146,13 +147,13 @@ class NodeBase : public NodeBaseInterface, public std::enable_shared_from_this<N
   std::shared_ptr<rcl_node_t> node_handle_;
 
   rclcpp::CallbackGroup::SharedPtr default_callback_group_;
-  std::mutex callback_groups_mutex_;
+  rcpputils::PIMutex callback_groups_mutex_;
   std::vector<rclcpp::CallbackGroup::WeakPtr> callback_groups_;
 
   std::atomic_bool associated_with_executor_;
 
   /// Guard condition for notifying the Executor of changes to this node.
-  mutable std::recursive_mutex notify_guard_condition_mutex_;
+  mutable rcpputils::RecursivePIMutex notify_guard_condition_mutex_;
   rclcpp::GuardCondition notify_guard_condition_;
   bool notify_guard_condition_is_valid_;
 };

rclcpp/include/rclcpp/node_interfaces/node_graph.hpp

@@ -20,7 +20,6 @@
 #include <condition_variable>
 #include <map>
 #include <memory>
-#include <mutex>
 #include <string>
 #include <tuple>
 #include <utility>
@@ -33,6 +32,9 @@
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/node_interfaces/node_graph_interface.hpp"
 #include "rclcpp/visibility_control.hpp"
+
+#include "rcpputils/mutex.hpp"
+
 #include "rmw/topic_endpoint_info_array.h"
 
 namespace rclcpp
@@ -163,7 +165,7 @@ class NodeGraph : public NodeGraphInterface
   std::atomic_bool should_add_to_graph_listener_;
 
   /// Mutex to guard the graph event related data structures.
-  mutable std::mutex graph_mutex_;
+  mutable rcpputils::PIMutex graph_mutex_;
   /// For notifying waiting threads (wait_for_graph_change()) on changes (notify_graph_change()).
   std::condition_variable graph_cv_;
   /// Weak references to graph events out on loan.

rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp

@@ -33,6 +33,8 @@
 
 #include "statistics_msgs/msg/metrics_message.hpp"
 
+#include "rcpputils/mutex.hpp"
+
 namespace rclcpp
 {
 namespace topic_statistics
@@ -227,7 +229,7 @@ class SubscriptionTopicStatistics
   }
 
   /// Mutex to protect the subsequence vectors
-  mutable std::mutex mutex_;
+  mutable rcpputils::PIMutex mutex_;
   /// Collection of statistics collectors
   std::vector<std::unique_ptr<TopicStatsCollector>> subscriber_statistics_collectors_{};
   /// Node name used to generate topic statistics messages to be published

rclcpp/include/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp

@@ -17,10 +17,11 @@
 
 #include <condition_variable>
 #include <functional>
-#include <mutex>
 
 #include "rclcpp/visibility_control.hpp"
 
+#include "rcpputils/mutex.hpp"
+
 namespace rclcpp
 {
 namespace wait_set_policies
@@ -229,7 +230,7 @@ class WritePreferringReadWriteLock final
   bool reader_active_ = false;
   std::size_t number_of_writers_waiting_ = 0;
   bool writer_active_ = false;
-  std::mutex mutex_;
+  rcpputils::PIMutex mutex_;
   std::condition_variable condition_variable_;
   ReadMutex read_mutex_;
   WriteMutex write_mutex_;