Change interrupt guard condition to shared_ptr

Check if guard condition is valid before adding it to the waitable

Signed-off-by: Michael Carroll <[email protected]>

Author: mjcarroll

rclcpp/include/rclcpp/executor.hpp

@@ -637,8 +637,9 @@ class Executor
   std::atomic_bool spinning;
 
   /// Guard condition for signaling the rmw layer to wake up for special events.
-  rclcpp::GuardCondition interrupt_guard_condition_;
+  std::shared_ptr<rclcpp::GuardCondition> interrupt_guard_condition_;
 
+  /// Guard condition for signaling the rmw layer to wake up for system shutdown. 
   std::shared_ptr<rclcpp::GuardCondition> shutdown_guard_condition_;
 
   /// Wait set for managing entities that the rmw layer waits on.

rclcpp/src/rclcpp/callback_group.cpp

@@ -43,8 +43,8 @@ CallbackGroup::~CallbackGroup()
 {
   trigger_notify_guard_condition();
 }
-
 bool
+
 CallbackGroup::has_valid_node()
 {
   return nullptr != this->get_context_();

rclcpp/src/rclcpp/executor.cpp

@@ -38,14 +38,11 @@
 using namespace std::chrono_literals;
 
 using rclcpp::exceptions::throw_from_rcl_error;
-using rclcpp::AnyExecutable;
 using rclcpp::Executor;
-using rclcpp::ExecutorOptions;
-using rclcpp::FutureReturnCode;
 
 Executor::Executor(const rclcpp::ExecutorOptions & options)
 : spinning(false),
-  interrupt_guard_condition_(options.context),
+  interrupt_guard_condition_(std::make_shared<rclcpp::GuardCondition>(options.context)),
   shutdown_guard_condition_(std::make_shared<rclcpp::GuardCondition>(options.context)),
   memory_strategy_(options.memory_strategy)
 {
@@ -65,7 +62,7 @@ Executor::Executor(const rclcpp::ExecutorOptions & options)
   memory_strategy_->add_guard_condition(*shutdown_guard_condition_.get());
 
   // Put the executor's guard condition in
-  memory_strategy_->add_guard_condition(interrupt_guard_condition_);
+  memory_strategy_->add_guard_condition(*interrupt_guard_condition_.get());
   rcl_allocator_t allocator = memory_strategy_->get_allocator();
 
   rcl_ret_t ret = rcl_wait_set_init(
@@ -127,7 +124,7 @@ Executor::~Executor()
   }
   // Remove and release the sigint guard condition
   memory_strategy_->remove_guard_condition(shutdown_guard_condition_.get());
-  memory_strategy_->remove_guard_condition(&interrupt_guard_condition_);
+  memory_strategy_->remove_guard_condition(interrupt_guard_condition_.get());
 
   // Remove shutdown callback handle registered to Context
   if (!context_->remove_on_shutdown_callback(shutdown_callback_handle_)) {
@@ -231,7 +228,7 @@ Executor::add_callback_group_to_map(
   if (notify) {
     // Interrupt waiting to handle new node
     try {
-      interrupt_guard_condition_.trigger();
+      interrupt_guard_condition_->trigger();
     } catch (const rclcpp::exceptions::RCLError & ex) {
       throw std::runtime_error(
               std::string(
@@ -279,10 +276,10 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt
       }
     });
 
-  const auto & gc = node_ptr->get_notify_guard_condition();
-  weak_nodes_to_guard_conditions_[node_ptr] = &gc;
+  const auto gc = node_ptr->get_notify_guard_condition();
+  weak_nodes_to_guard_conditions_[node_ptr] = gc.get();
   // Add the node's notify condition to the guard condition handles
-  memory_strategy_->add_guard_condition(gc);
+  memory_strategy_->add_guard_condition(*gc);
   weak_nodes_.push_back(node_ptr);
 }
 
@@ -319,7 +316,7 @@ Executor::remove_callback_group_from_map(
 
     if (notify) {
       try {
-        interrupt_guard_condition_.trigger();
+        interrupt_guard_condition_->trigger();
       } catch (const rclcpp::exceptions::RCLError & ex) {
         throw std::runtime_error(
                 std::string(
@@ -387,7 +384,7 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node
     }
   }
 
-  memory_strategy_->remove_guard_condition(&node_ptr->get_notify_guard_condition());
+  memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition().get());
   weak_nodes_to_guard_conditions_.erase(node_ptr);
 
   std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
@@ -500,7 +497,7 @@ Executor::cancel()
 {
   spinning.store(false);
   try {
-    interrupt_guard_condition_.trigger();
+    interrupt_guard_condition_->trigger();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string("Failed to trigger guard condition in cancel: ") + ex.what());
@@ -549,7 +546,7 @@ Executor::execute_any_executable(AnyExecutable & any_exec)
   // Wake the wait, because it may need to be recalculated or work that
   // was previously blocked is now available.
   try {
-    interrupt_guard_condition_.trigger();
+    interrupt_guard_condition_->trigger();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string(

rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp

@@ -100,11 +100,12 @@ ExecutorNotifyWaitable::take_data()
 }
 
 void
-ExecutorNotifyWaitable::add_guard_condition(rclcpp::GuardCondition::WeakPtr guard_condition)
+ExecutorNotifyWaitable::add_guard_condition(rclcpp::GuardCondition::WeakPtr weak_guard_condition)
 {
   std::lock_guard<std::mutex> lock(guard_condition_mutex_);
-  if (notify_guard_conditions_.count(guard_condition) == 0) {
-    notify_guard_conditions_.insert(guard_condition);
+  auto guard_condition = weak_guard_condition.lock();
+  if (guard_condition && notify_guard_conditions_.count(weak_guard_condition) == 0) {
+    notify_guard_conditions_.insert(weak_guard_condition);
   }
 }
 

rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp

@@ -109,8 +109,7 @@ StaticExecutorEntitiesCollector::execute(std::shared_ptr<void> & data)
   std::lock_guard<std::mutex> guard{new_nodes_mutex_};
   for (const auto & weak_node : new_nodes_) {
     if (auto node_ptr = weak_node.lock()) {
-      const auto & gc = node_ptr->get_notify_guard_condition();
-      weak_nodes_to_guard_conditions_[node_ptr] = &gc;
+      weak_nodes_to_guard_conditions_[node_ptr] = node_ptr->get_notify_guard_condition().get();
     }
   }
   new_nodes_.clear();

rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp

@@ -139,7 +139,7 @@ StaticSingleThreadedExecutor::add_callback_group(
   bool is_new_node = entities_collector_->add_callback_group(group_ptr, node_ptr);
   if (is_new_node && notify) {
     // Interrupt waiting to handle new node
-    interrupt_guard_condition_.trigger();
+    interrupt_guard_condition_->trigger();
   }
 }
 
@@ -150,7 +150,7 @@ StaticSingleThreadedExecutor::add_node(
   bool is_new_node = entities_collector_->add_node(node_ptr);
   if (is_new_node && notify) {
     // Interrupt waiting to handle new node
-    interrupt_guard_condition_.trigger();
+    interrupt_guard_condition_->trigger();
   }
 }
 
@@ -167,7 +167,7 @@ StaticSingleThreadedExecutor::remove_callback_group(
   bool node_removed = entities_collector_->remove_callback_group(group_ptr);
   // If the node was matched and removed, interrupt waiting
   if (node_removed && notify) {
-    interrupt_guard_condition_.trigger();
+    interrupt_guard_condition_->trigger();
   }
 }
 
@@ -181,7 +181,7 @@ StaticSingleThreadedExecutor::remove_node(
   }
   // If the node was matched and removed, interrupt waiting
   if (notify) {
-    interrupt_guard_condition_.trigger();
+    interrupt_guard_condition_->trigger();
   }
 }