Notify guard condition of Node not to be used in Executor

it is only for the waitset of GraphListener Signed-off-by: Chen Lihui <lihui.chen@sony.com>
ros2 · May 24, 2021 · d7b1a74 · d7b1a74
1 parent 328b7c9
commit d7b1a74
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 42 deletions.
diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp
@@ -547,20 +547,11 @@ class Executor
   virtual void
   spin_once_impl(std::chrono::nanoseconds timeout);
 
-  typedef std::map<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
-      const rcl_guard_condition_t *,
-      std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>
-    WeakNodesToGuardConditionsMap;
-
   typedef std::map<rclcpp::CallbackGroup::WeakPtr,
       const rcl_guard_condition_t *,
       std::owner_less<rclcpp::CallbackGroup::WeakPtr>>
     WeakCallbackGroupsToGuardConditionsMap;
 
-  /// maps nodes to guard conditions
-  WeakNodesToGuardConditionsMap
-  weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
-
   /// maps callback groups to guard conditions
   WeakCallbackGroupsToGuardConditionsMap
   weak_groups_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);

diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp
@@ -116,11 +116,6 @@ Executor::~Executor()
   weak_groups_associated_with_executor_to_nodes_.clear();
   weak_groups_to_nodes_associated_with_executor_.clear();
   weak_groups_to_nodes_.clear();
-  for (const auto & pair : weak_nodes_to_guard_conditions_) {
-    auto & guard_condition = pair.second;
-    memory_strategy_->remove_guard_condition(guard_condition);
-  }
-  weak_nodes_to_guard_conditions_.clear();
   for (const auto & pair : weak_groups_to_guard_conditions_) {
     auto & guard_condition = pair.second;
     memory_strategy_->remove_guard_condition(guard_condition);
@@ -219,8 +214,7 @@ Executor::add_callback_group_to_map(
   if (has_executor.exchange(true)) {
     throw std::runtime_error("Callback group has already been added to an executor.");
   }
-  bool is_new_node = !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) &&
-    !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_);
+
   rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr;
   auto insert_info =
     weak_groups_to_nodes.insert(std::make_pair(weak_group_ptr, node_ptr));
@@ -230,24 +224,17 @@ Executor::add_callback_group_to_map(
   }
   // Also add to the map that contains all callback groups
   weak_groups_to_nodes_.insert(std::make_pair(weak_group_ptr, node_ptr));
-  if (is_new_node) {
-    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr);
-    weak_nodes_to_guard_conditions_[node_weak_ptr] = node_ptr->get_notify_guard_condition();
-    rclcpp::CallbackGroup::WeakPtr group_weak_ptr(group_ptr);
-    weak_groups_to_guard_conditions_[group_weak_ptr] =
-      &group_ptr->get_notify_guard_condition()->get_rcl_guard_condition();
-    if (notify) {
-      // Interrupt waiting to handle new node
-      rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_);
-      if (ret != RCL_RET_OK) {
-        throw_from_rcl_error(ret, "Failed to trigger guard condition on callback group add");
-      }
+  weak_groups_to_guard_conditions_[weak_group_ptr] =
+    &group_ptr->get_notify_guard_condition()->get_rcl_guard_condition();
+  // Add the callback_group's notify condition to the guard condition handles
+  memory_strategy_->add_guard_condition(
+    &group_ptr->get_notify_guard_condition()->get_rcl_guard_condition());
+  if (notify) {
+    // Interrupt waiting to handle new node
+    rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_);
+    if (ret != RCL_RET_OK) {
+      throw_from_rcl_error(ret, "Failed to trigger guard condition on callback group add");
     }
-    // Add the node's notify condition to the guard condition handles
-    memory_strategy_->add_guard_condition(node_ptr->get_notify_guard_condition());
-    // Add the callback_group's notify condition to the guard condition handles
-    memory_strategy_->add_guard_condition(
-      &group_ptr->get_notify_guard_condition()->get_rcl_guard_condition());
   }
 }
 
@@ -314,15 +301,13 @@ Executor::remove_callback_group_from_map(
   if (!has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) &&
     !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_))
   {
-    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr);
-    weak_nodes_to_guard_conditions_.erase(node_weak_ptr);
+    weak_groups_to_guard_conditions_.erase(weak_group_ptr);
     if (notify) {
       rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_);
       if (ret != RCL_RET_OK) {
         throw_from_rcl_error(ret, "Failed to trigger guard condition on callback group remove");
       }
     }
-    memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition());
     memory_strategy_->remove_guard_condition(
       &group_ptr->get_notify_guard_condition()->get_rcl_guard_condition());
   }
@@ -703,12 +688,6 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
         auto weak_node_ptr = pair.second;
         if (weak_group_ptr.expired() || weak_node_ptr.expired()) {
           invalid_group_ptrs.push_back(weak_group_ptr);
-          auto node_guard_pair = weak_nodes_to_guard_conditions_.find(weak_node_ptr);
-          if (node_guard_pair != weak_nodes_to_guard_conditions_.end()) {
-            auto guard_condition = node_guard_pair->second;
-            weak_nodes_to_guard_conditions_.erase(weak_node_ptr);
-            memory_strategy_->remove_guard_condition(guard_condition);
-          }
         }
       }
       std::for_each(