Fix data race in IntraProcessManager::lowest_available_capacity

rclcpp/src/rclcpp/intra_process_manager.cpp

@@ -238,6 +238,8 @@ IntraProcessManager::lowest_available_capacity(const uint64_t intra_process_publ
 {
   size_t capacity = std::numeric_limits<size_t>::max();
 
+  std::shared_lock<std::shared_timed_mutex> lock(mutex_);
+
   auto publisher_it = pub_to_subs_.find(intra_process_publisher_id);
   if (publisher_it == pub_to_subs_.end()) {
     // Publisher is either invalid or no longer exists.

rclcpp/test/rclcpp/test_publisher.cpp

@@ -14,12 +14,14 @@
 
 #include <gtest/gtest.h>
 
+#include <atomic>
 #include <chrono>
 #include <functional>
 #include <future>
 #include <memory>
 #include <stdexcept>
 #include <string>
+#include <thread>
 #include <utility>
 #include <vector>
 
@@ -701,6 +703,31 @@ TEST_F(TestPublisher, lowest_available_ipm_capacity) {
   ASSERT_EQ(history_depth - 1u, pub_ipm_enabled->lowest_available_ipm_capacity());
 }
 
+TEST_F(TestPublisher, lowest_available_ipm_capacity_concurrent_subscriptions) {
+  initialize(rclcpp::NodeOptions().use_intra_process_comms(true));
+
+  auto pub = node->create_publisher<test_msgs::msg::Strings>("topic", 10);
+  auto do_nothing = [](std::shared_ptr<const test_msgs::msg::Strings>) {};
+
+  // Query the capacity while intra-process subscriptions are added and removed.
+  std::atomic<bool> done{false};
+  std::thread reader([&pub, &done]() {
+      while (!done) {
+        pub->lowest_available_ipm_capacity();
+      }
+    });
+
+  for (int i = 0; i < 50; ++i) {
+    std::vector<rclcpp::Subscription<test_msgs::msg::Strings>::SharedPtr> subs;
+    for (int j = 0; j < 20; ++j) {
+      subs.push_back(node->create_subscription<test_msgs::msg::Strings>("topic", 10, do_nothing));
+    }
+  }
+  done = true;
+  reader.join();
+  EXPECT_EQ(0u, pub->lowest_available_ipm_capacity());
+}
+
 INSTANTIATE_TEST_SUITE_P(
   TestWaitForAllAckedWithParm,
   TestPublisherWaitForAllAcked,