diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 480fab0510..6c902d2f76 100644 --- 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> - WeakNodesToGuardConditionsMap; - typedef std::map> 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 index d20099c478..00c3dd0a12 100644 --- 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(