add mutex in add/remove_node and wait_for_work to protect concurrent use/change of memory_strategy_ (#837)

Signed-off-by: Dirk Thomas <dirk-thomas@users.noreply.github.com>

rclcpp/include/rclcpp/executor.hpp

@@ -22,6 +22,7 @@
 #include <iostream>
 #include <list>
 #include <memory>
+#include <mutex>
 #include <string>
 #include <vector>
 
@@ -356,6 +357,9 @@ protected:
   /// Wait set for managing entities that the rmw layer waits on.
   rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set();
 
+  // Mutex to protect the subsequent memory_strategy_.
+  std::mutex memory_strategy_mutex_;
+
   /// The memory strategy: an interface for handling user-defined memory allocation strategies.
   memory_strategy::MemoryStrategy::SharedPtr memory_strategy_;
 

rclcpp/src/rclcpp/executor.cpp

@@ -140,6 +140,7 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt
     }
   }
   // Add the node's notify condition to the guard condition handles
+  std::unique_lock<std::mutex> lock(memory_strategy_mutex_);
   memory_strategy_->add_guard_condition(node_ptr->get_notify_guard_condition());
 }
 
@@ -178,6 +179,7 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node
       }
     }
   }
+  std::unique_lock<std::mutex> lock(memory_strategy_mutex_);
   memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition());
 }
 
@@ -423,6 +425,9 @@ Executor::execute_client(
 void
 Executor::wait_for_work(std::chrono::nanoseconds timeout)
 {
+  {
+    std::unique_lock<std::mutex> lock(memory_strategy_mutex_);
+
     // Collect the subscriptions and timers to be waited on
     memory_strategy_->clear_handles();
     bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_);
@@ -461,6 +466,7 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
     if (!memory_strategy_->add_handles_to_wait_set(&wait_set_)) {
       throw std::runtime_error("Couldn't fill wait set");
     }
+  }
   rcl_ret_t status =
     rcl_wait(&wait_set_, std::chrono::duration_cast<std::chrono::nanoseconds>(timeout).count());
   if (status == RCL_RET_WAIT_SET_EMPTY) {