diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 95dc5311d1..dadaaa59b6 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -41,6 +41,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/clock.cpp src/rclcpp/context.cpp src/rclcpp/contexts/default_context.cpp + src/rclcpp/detail/add_guard_condition_to_rcl_wait_set.cpp src/rclcpp/detail/mutex_two_priorities.cpp src/rclcpp/detail/resolve_parameter_overrides.cpp src/rclcpp/detail/rmw_implementation_specific_payload.cpp diff --git a/rclcpp/include/rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp b/rclcpp/include/rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp new file mode 100644 index 0000000000..130cc90f37 --- /dev/null +++ b/rclcpp/include/rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp @@ -0,0 +1,39 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__DETAIL__ADD_GUARD_CONDITION_TO_RCL_WAIT_SET_HPP_ +#define RCLCPP__DETAIL__ADD_GUARD_CONDITION_TO_RCL_WAIT_SET_HPP_ + +#include "rclcpp/guard_condition.hpp" + +namespace rclcpp +{ +namespace detail +{ + +/// Adds the guard condition to a waitset +/** + * This function is thread-safe. + * \param[in] wait_set pointer to a wait set where to add the guard condition + */ +RCLCPP_PUBLIC +void +add_guard_condition_to_rcl_wait_set( + rcl_wait_set_t & wait_set, + const rclcpp::GuardCondition & guard_condition); + +} // namespace detail +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__ADD_GUARD_CONDITION_TO_RCL_WAIT_SET_HPP_ diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 13ed108348..4861337eaf 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -525,7 +525,7 @@ class Executor std::atomic_bool spinning; /// Guard condition for signaling the rmw layer to wake up for special events. - rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition(); + rclcpp::GuardCondition interrupt_guard_condition_; std::shared_ptr shutdown_guard_condition_; @@ -549,7 +549,7 @@ class Executor spin_once_impl(std::chrono::nanoseconds timeout); typedef std::map> WeakNodesToGuardConditionsMap; diff --git a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp index 695b35a226..92804681af 100644 --- a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp +++ b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp @@ -65,7 +65,7 @@ class StaticExecutorEntitiesCollector final init( rcl_wait_set_t * p_wait_set, rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy, - rcl_guard_condition_t * executor_guard_condition); + rclcpp::GuardCondition & executor_guard_condition); /// Finalize StaticExecutorEntitiesCollector to clear resources RCLCPP_PUBLIC @@ -330,7 +330,7 @@ class StaticExecutorEntitiesCollector final WeakCallbackGroupsToNodesMap weak_groups_to_nodes_associated_with_executor_; typedef std::map> WeakNodesToGuardConditionsMap; WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_; diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index 8b0a9be975..f3c05c146b 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -67,7 +67,7 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase const std::string & topic_name, rmw_qos_profile_t qos_profile, rclcpp::IntraProcessBufferType buffer_type) - : SubscriptionIntraProcessBase(topic_name, qos_profile), + : SubscriptionIntraProcessBase(context, topic_name, qos_profile), any_callback_(callback) { if (!std::is_same::value) { @@ -80,18 +80,6 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase qos_profile, allocator); - // Create the guard condition. - rcl_guard_condition_options_t guard_condition_options = - rcl_guard_condition_get_default_options(); - - gc_ = rcl_get_zero_initialized_guard_condition(); - rcl_ret_t ret = rcl_guard_condition_init( - &gc_, context->get_rcl_context().get(), guard_condition_options); - - if (RCL_RET_OK != ret) { - throw std::runtime_error("SubscriptionIntraProcess init error initializing guard condition"); - } - TRACEPOINT( rclcpp_subscription_callback_added, static_cast(this), @@ -104,15 +92,7 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase #endif } - ~SubscriptionIntraProcess() - { - if (rcl_guard_condition_fini(&gc_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Failed to destroy guard condition: %s", - rcutils_get_error_string().str); - } - } + ~SubscriptionIntraProcess() = default; bool is_ready(rcl_wait_set_t * wait_set) @@ -182,8 +162,7 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase void trigger_guard_condition() { - rcl_ret_t ret = rcl_trigger_guard_condition(&gc_); - (void)ret; + gc_.trigger(); } template diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index cf7ccbb6c9..6d8b569d82 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -26,6 +26,7 @@ #include "rmw/impl/cpp/demangle.hpp" #include "rclcpp/logging.hpp" +#include "rclcpp/guard_condition.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/waitable.hpp" @@ -45,8 +46,11 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable }; RCLCPP_PUBLIC - SubscriptionIntraProcessBase(const std::string & topic_name, rmw_qos_profile_t qos_profile) - : topic_name_(topic_name), qos_profile_(qos_profile) + SubscriptionIntraProcessBase( + rclcpp::Context::SharedPtr context, + const std::string & topic_name, + rmw_qos_profile_t qos_profile) + : gc_(context), topic_name_(topic_name), qos_profile_(qos_profile) {} virtual ~SubscriptionIntraProcessBase() = default; @@ -163,7 +167,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable protected: std::recursive_mutex reentrant_mutex_; - rcl_guard_condition_t gc_; + rclcpp::GuardCondition gc_; std::function on_new_message_callback_ {nullptr}; size_t unread_count_{0}; diff --git a/rclcpp/include/rclcpp/graph_listener.hpp b/rclcpp/include/rclcpp/graph_listener.hpp index 30305ad2ff..79357b952e 100644 --- a/rclcpp/include/rclcpp/graph_listener.hpp +++ b/rclcpp/include/rclcpp/graph_listener.hpp @@ -24,6 +24,7 @@ #include "rcl/guard_condition.h" #include "rcl/wait.h" #include "rclcpp/context.hpp" +#include "rclcpp/guard_condition.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" #include "rclcpp/visibility_control.hpp" @@ -187,7 +188,7 @@ class GraphListener : public std::enable_shared_from_this mutable std::mutex node_graph_interfaces_mutex_; std::vector node_graph_interfaces_; - rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition(); + rclcpp::GuardCondition interrupt_guard_condition_; rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set(); }; diff --git a/rclcpp/include/rclcpp/guard_condition.hpp b/rclcpp/include/rclcpp/guard_condition.hpp index 010c127a07..4ec7a73018 100644 --- a/rclcpp/include/rclcpp/guard_condition.hpp +++ b/rclcpp/include/rclcpp/guard_condition.hpp @@ -47,7 +47,9 @@ class GuardCondition RCLCPP_PUBLIC explicit GuardCondition( rclcpp::Context::SharedPtr context = - rclcpp::contexts::get_global_default_context()); + rclcpp::contexts::get_global_default_context(), + rcl_guard_condition_options_t guard_condition_options = + rcl_guard_condition_get_default_options()); RCLCPP_PUBLIC virtual @@ -59,6 +61,10 @@ class GuardCondition get_context() const; /// Return the underlying rcl guard condition structure. + RCLCPP_PUBLIC + rcl_guard_condition_t & + get_rcl_guard_condition(); + RCLCPP_PUBLIC const rcl_guard_condition_t & get_rcl_guard_condition() const; diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index da79643e4e..39c0fd2f20 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -64,9 +64,17 @@ class RCLCPP_PUBLIC MemoryStrategy virtual void clear_handles() = 0; virtual void remove_null_handles(rcl_wait_set_t * wait_set) = 0; - virtual void add_guard_condition(const rcl_guard_condition_t * guard_condition) = 0; + virtual void + add_guard_condition(const rclcpp::GuardCondition & guard_condition) = 0; + + virtual void + add_guard_condition(const rclcpp::GuardCondition::SharedPtr guard_condition) = 0; - virtual void remove_guard_condition(const rcl_guard_condition_t * guard_condition) = 0; + virtual void + remove_guard_condition(const rclcpp::GuardCondition & guard_condition) = 0; + + virtual void + remove_guard_condition(const rclcpp::GuardCondition::SharedPtr guard_condition) = 0; virtual void get_next_subscription( diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index 3f8b0021a0..8b90722237 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -104,13 +104,9 @@ class NodeBase : public NodeBaseInterface get_associated_with_executor_atomic() override; RCLCPP_PUBLIC - rcl_guard_condition_t * + rclcpp::GuardCondition & get_notify_guard_condition() override; - RCLCPP_PUBLIC - std::unique_lock - acquire_notify_guard_condition_lock() const override; - RCLCPP_PUBLIC bool get_use_intra_process_default() const override; @@ -122,6 +118,10 @@ class NodeBase : public NodeBaseInterface resolve_topic_or_service_name( const std::string & name, bool is_service, bool only_expand = false) const override; + RCLCPP_PUBLIC + void + trigger_notify_guard_condition() override; + private: RCLCPP_DISABLE_COPY(NodeBase) @@ -138,7 +138,7 @@ class NodeBase : public NodeBaseInterface /// Guard condition for notifying the Executor of changes to this node. mutable std::recursive_mutex notify_guard_condition_mutex_; - rcl_guard_condition_t notify_guard_condition_ = rcl_get_zero_initialized_guard_condition(); + rclcpp::GuardCondition notify_guard_condition_; bool notify_guard_condition_is_valid_; }; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp index a9c30dd09a..83bff7de62 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp @@ -24,6 +24,7 @@ #include "rclcpp/callback_group.hpp" #include "rclcpp/context.hpp" +#include "rclcpp/guard_condition.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/visibility_control.hpp" @@ -134,24 +135,17 @@ class NodeBaseInterface std::atomic_bool & get_associated_with_executor_atomic() = 0; - /// Return guard condition that should be notified when the internal node state changes. + /// Return a guard condition that should be notified when the internal node state changes. /** * For example, this should be notified when a publisher is added or removed. * - * \return the rcl_guard_condition_t if it is valid, else nullptr + * \return the GuardCondition if it is valid, else thow runtime error */ RCLCPP_PUBLIC virtual - rcl_guard_condition_t * + rclcpp::GuardCondition & get_notify_guard_condition() = 0; - /// Acquire and return a scoped lock that protects the notify guard condition. - /** This should be used when triggering the notify guard condition. */ - RCLCPP_PUBLIC - virtual - std::unique_lock - acquire_notify_guard_condition_lock() const = 0; - /// Return the default preference for using intra process communication. RCLCPP_PUBLIC virtual @@ -170,6 +164,12 @@ class NodeBaseInterface std::string resolve_topic_or_service_name( const std::string & name, bool is_service, bool only_expand = false) const = 0; + + /// Trigger the node's notify guard condition. + RCLCPP_PUBLIC + virtual + void + trigger_notify_guard_condition() = 0; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 76790b1245..80d6507662 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -21,6 +21,7 @@ #include "rcl/allocator.h" #include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" #include "rclcpp/memory_strategy.hpp" #include "rclcpp/node.hpp" #include "rclcpp/visibility_control.hpp" @@ -61,24 +62,24 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy allocator_ = std::make_shared(); } - void add_guard_condition(const rcl_guard_condition_t * guard_condition) override + void add_guard_condition(const rclcpp::GuardCondition & guard_condition) override { - for (const auto & existing_guard_condition : guard_conditions_) { - if (existing_guard_condition == guard_condition) { - return; - } - } - guard_conditions_.push_back(guard_condition); + add_guard_condition(&guard_condition); } - void remove_guard_condition(const rcl_guard_condition_t * guard_condition) override + void add_guard_condition(const rclcpp::GuardCondition::SharedPtr guard_condition) override { - for (auto it = guard_conditions_.begin(); it != guard_conditions_.end(); ++it) { - if (*it == guard_condition) { - guard_conditions_.erase(it); - break; - } - } + add_guard_condition(guard_condition.get()); + } + + void remove_guard_condition(const rclcpp::GuardCondition & guard_condition) override + { + remove_guard_condition(&guard_condition); + } + + void remove_guard_condition(const rclcpp::GuardCondition::SharedPtr guard_condition) override + { + remove_guard_condition(guard_condition.get()); } void clear_handles() override @@ -240,13 +241,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } for (auto guard_condition : guard_conditions_) { - if (rcl_wait_set_add_guard_condition(wait_set, guard_condition, NULL) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add guard_condition to wait set: %s", - rcl_get_error_string().str); - return false; - } + detail::add_guard_condition_to_rcl_wait_set(*wait_set, *guard_condition); } for (auto waitable : waitable_handles_) { @@ -500,11 +495,31 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } private: + void add_guard_condition(const rclcpp::GuardCondition * guard_condition) + { + for (const auto & existing_guard_condition : guard_conditions_) { + if (existing_guard_condition == guard_condition) { + return; + } + } + guard_conditions_.push_back(guard_condition); + } + + void remove_guard_condition(const rclcpp::GuardCondition * guard_condition) + { + for (auto it = guard_conditions_.begin(); it != guard_conditions_.end(); ++it) { + if (*it == guard_condition) { + guard_conditions_.erase(it); + break; + } + } + } + template using VectorRebind = std::vector::template rebind_alloc>; - VectorRebind guard_conditions_; + VectorRebind guard_conditions_; VectorRebind> subscription_handles_; VectorRebind> service_handles_; diff --git a/rclcpp/src/rclcpp/detail/add_guard_condition_to_rcl_wait_set.cpp b/rclcpp/src/rclcpp/detail/add_guard_condition_to_rcl_wait_set.cpp new file mode 100644 index 0000000000..85b4c6594a --- /dev/null +++ b/rclcpp/src/rclcpp/detail/add_guard_condition_to_rcl_wait_set.cpp @@ -0,0 +1,39 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" +#include "rclcpp/exceptions.hpp" + +namespace rclcpp +{ +namespace detail +{ + +void +add_guard_condition_to_rcl_wait_set( + rcl_wait_set_t & wait_set, + const rclcpp::GuardCondition & guard_condition) +{ + const auto & gc = guard_condition.get_rcl_guard_condition(); + + rcl_ret_t ret = rcl_wait_set_add_guard_condition(&wait_set, &gc, NULL); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to add guard condition to wait set"); + } +} + +} // namespace detail +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 20876668a4..2ba8a02ef9 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -43,19 +43,13 @@ using rclcpp::FutureReturnCode; Executor::Executor(const rclcpp::ExecutorOptions & options) : spinning(false), + interrupt_guard_condition_(options.context), shutdown_guard_condition_(std::make_shared(options.context)), memory_strategy_(options.memory_strategy) { // Store the context for later use. context_ = options.context; - rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options(); - rcl_ret_t ret = rcl_guard_condition_init( - &interrupt_guard_condition_, context_->get_rcl_context().get(), guard_condition_options); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "Failed to create interrupt guard condition in Executor constructor"); - } - shutdown_callback_handle_ = context_->add_on_shutdown_callback( [weak_gc = std::weak_ptr{shutdown_guard_condition_}]() { auto strong_gc = weak_gc.lock(); @@ -66,13 +60,13 @@ Executor::Executor(const rclcpp::ExecutorOptions & options) // The number of guard conditions is always at least 2: 1 for the ctrl-c guard cond, // and one for the executor's guard cond (interrupt_guard_condition_) - memory_strategy_->add_guard_condition(&shutdown_guard_condition_->get_rcl_guard_condition()); + memory_strategy_->add_guard_condition(shutdown_guard_condition_); // Put the executor's guard condition in - memory_strategy_->add_guard_condition(&interrupt_guard_condition_); + memory_strategy_->add_guard_condition(interrupt_guard_condition_); rcl_allocator_t allocator = memory_strategy_->get_allocator(); - ret = rcl_wait_set_init( + rcl_ret_t ret = rcl_wait_set_init( &wait_set_, 0, 2, 0, 0, 0, 0, context_->get_rcl_context().get(), @@ -82,12 +76,6 @@ Executor::Executor(const rclcpp::ExecutorOptions & options) "rclcpp", "failed to create wait set: %s", rcl_get_error_string().str); rcl_reset_error(); - if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy guard condition: %s", rcl_get_error_string().str); - rcl_reset_error(); - } throw_from_rcl_error(ret, "Failed to create wait set in Executor constructor"); } } @@ -118,7 +106,7 @@ Executor::~Executor() 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); + memory_strategy_->remove_guard_condition(*guard_condition); } weak_nodes_to_guard_conditions_.clear(); @@ -129,15 +117,8 @@ Executor::~Executor() "failed to destroy wait set: %s", rcl_get_error_string().str); rcl_reset_error(); } - // Finalize the interrupt guard condition. - if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy guard condition: %s", rcl_get_error_string().str); - rcl_reset_error(); - } // Remove and release the sigint guard condition - memory_strategy_->remove_guard_condition(&shutdown_guard_condition_->get_rcl_guard_condition()); + memory_strategy_->remove_guard_condition(shutdown_guard_condition_); // Remove shutdown callback handle registered to Context if (!context_->remove_on_shutdown_callback(shutdown_callback_handle_)) { @@ -234,17 +215,14 @@ 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(); + const auto & gc = node_ptr->get_notify_guard_condition(); + weak_nodes_to_guard_conditions_[node_ptr] = &gc; 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"); - } + interrupt_guard_condition_.trigger(); } // Add the node's notify condition to the guard condition handles - memory_strategy_->add_guard_condition(node_ptr->get_notify_guard_condition()); + memory_strategy_->add_guard_condition(gc); } } @@ -311,13 +289,9 @@ 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_nodes_to_guard_conditions_.erase(node_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"); - } + interrupt_guard_condition_.trigger(); } memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition()); } @@ -490,10 +464,7 @@ void Executor::cancel() { spinning.store(false); - 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 in cancel"); - } + interrupt_guard_condition_.trigger(); } void @@ -531,10 +502,7 @@ Executor::execute_any_executable(AnyExecutable & any_exec) any_exec.callback_group->can_be_taken_from().store(true); // Wake the wait, because it may need to be recalculated or work that // was previously blocked is now available. - 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 from execute_any_executable"); - } + interrupt_guard_condition_.trigger(); } static @@ -700,9 +668,9 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) 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; + const auto & guard_condition = node_guard_pair->second; weak_nodes_to_guard_conditions_.erase(weak_node_ptr); - memory_strategy_->remove_guard_condition(guard_condition); + memory_strategy_->remove_guard_condition(*guard_condition); } } } diff --git a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp index fada15e5aa..d2c1c8373f 100644 --- a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp @@ -23,6 +23,7 @@ #include "rclcpp/memory_strategy.hpp" #include "rclcpp/executors/static_single_threaded_executor.hpp" +#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" using rclcpp::executors::StaticExecutorEntitiesCollector; @@ -62,7 +63,7 @@ void StaticExecutorEntitiesCollector::init( rcl_wait_set_t * p_wait_set, rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy, - rcl_guard_condition_t * executor_guard_condition) + GuardCondition & executor_guard_condition) { // Empty initialize executable list exec_list_ = rclcpp::experimental::ExecutableList(); @@ -267,10 +268,7 @@ StaticExecutorEntitiesCollector::add_to_wait_set(rcl_wait_set_t * wait_set) // Add waitable guard conditions (one for each registered node) into the wait set. for (const auto & pair : weak_nodes_to_guard_conditions_) { auto & gc = pair.second; - rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, gc, NULL); - if (ret != RCL_RET_OK) { - throw std::runtime_error("Executor waitable: couldn't add guard condition to wait set"); - } + detail::add_guard_condition_to_rcl_wait_set(*wait_set, *gc); } return true; } @@ -327,7 +325,8 @@ StaticExecutorEntitiesCollector::add_callback_group( throw std::runtime_error("Callback group was already added to executor."); } if (is_new_node) { - weak_nodes_to_guard_conditions_[node_ptr] = node_ptr->get_notify_guard_condition(); + const auto & gc = node_ptr->get_notify_guard_condition(); + weak_nodes_to_guard_conditions_[node_ptr] = &gc; return true; } return false; @@ -433,8 +432,9 @@ StaticExecutorEntitiesCollector::is_ready(rcl_wait_set_t * p_wait_set) auto found_guard_condition = std::find_if( weak_nodes_to_guard_conditions_.begin(), weak_nodes_to_guard_conditions_.end(), [&](std::pair pair) -> bool { - return pair.second == p_wait_set->guard_conditions[i]; + const GuardCondition *> pair) -> bool { + const rcl_guard_condition_t & rcl_gc = pair.second->get_rcl_guard_condition(); + return &rcl_gc == p_wait_set->guard_conditions[i]; }); if (found_guard_condition != weak_nodes_to_guard_conditions_.end()) { return true; diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 5402d32460..1be938b13d 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -47,7 +47,7 @@ StaticSingleThreadedExecutor::spin() // Set memory_strategy_ and exec_list_ based on weak_nodes_ // Prepare wait_set_ based on memory_strategy_ - entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_); + entities_collector_->init(&wait_set_, memory_strategy_, interrupt_guard_condition_); while (rclcpp::ok(this->context_) && spinning.load()) { // Refresh wait set and wait for work @@ -81,7 +81,7 @@ StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_durati { // Make sure the entities collector has been initialized if (!entities_collector_->is_init()) { - entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_); + entities_collector_->init(&wait_set_, memory_strategy_, interrupt_guard_condition_); } auto start = std::chrono::steady_clock::now(); @@ -118,7 +118,7 @@ StaticSingleThreadedExecutor::spin_once_impl(std::chrono::nanoseconds timeout) { // Make sure the entities collector has been initialized if (!entities_collector_->is_init()) { - entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_); + entities_collector_->init(&wait_set_, memory_strategy_, interrupt_guard_condition_); } if (rclcpp::ok(context_) && spinning.load()) { @@ -138,9 +138,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 - if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error(rcl_get_error_string().str); - } + interrupt_guard_condition_.trigger(); } } @@ -151,9 +149,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 - if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error(rcl_get_error_string().str); - } + interrupt_guard_condition_.trigger(); } } @@ -170,9 +166,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) { - if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error(rcl_get_error_string().str); - } + interrupt_guard_condition_.trigger(); } } @@ -186,9 +180,7 @@ StaticSingleThreadedExecutor::remove_node( } // If the node was matched and removed, interrupt waiting if (notify) { - if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error(rcl_get_error_string().str); - } + interrupt_guard_condition_.trigger(); } } diff --git a/rclcpp/src/rclcpp/graph_listener.cpp b/rclcpp/src/rclcpp/graph_listener.cpp index 0af47acc5b..e0b516b595 100644 --- a/rclcpp/src/rclcpp/graph_listener.cpp +++ b/rclcpp/src/rclcpp/graph_listener.cpp @@ -22,6 +22,7 @@ #include "rcl/error_handling.h" #include "rcl/types.h" +#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/node.hpp" @@ -40,19 +41,9 @@ GraphListener::GraphListener(const std::shared_ptr & parent_context) : weak_parent_context_(parent_context), rcl_parent_context_(parent_context->get_rcl_context()), is_started_(false), - is_shutdown_(false) + is_shutdown_(false), + interrupt_guard_condition_(parent_context) { - // TODO(wjwwood): make a guard condition class in rclcpp so this can be tracked - // automatically with the rcl guard condition - // hold on to this context to prevent it from going out of scope while this - // guard condition is using it. - rcl_ret_t ret = rcl_guard_condition_init( - &interrupt_guard_condition_, - rcl_parent_context_.get(), - rcl_guard_condition_get_default_options()); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "failed to create interrupt guard condition"); - } } GraphListener::~GraphListener() @@ -159,10 +150,7 @@ GraphListener::run_loop() throw_from_rcl_error(ret, "failed to clear wait set"); } // Put the interrupt guard condition in the wait set. - ret = rcl_wait_set_add_guard_condition(&wait_set_, &interrupt_guard_condition_, NULL); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "failed to add interrupt guard condition to wait set"); - } + detail::add_guard_condition_to_rcl_wait_set(wait_set_, interrupt_guard_condition_); // Put graph guard conditions for each node into the wait set. std::vector graph_gc_indexes(node_graph_interfaces_size, 0u); @@ -211,19 +199,16 @@ GraphListener::run_loop() } static void -interrupt_(rcl_guard_condition_t * interrupt_guard_condition) +interrupt_(GuardCondition * interrupt_guard_condition) { - rcl_ret_t ret = rcl_trigger_guard_condition(interrupt_guard_condition); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "failed to trigger the interrupt guard condition"); - } + interrupt_guard_condition->trigger(); } static void acquire_nodes_lock_( std::mutex * node_graph_interfaces_barrier_mutex, std::mutex * node_graph_interfaces_mutex, - rcl_guard_condition_t * interrupt_guard_condition) + GuardCondition * interrupt_guard_condition) { { // Acquire this lock to prevent the run loop from re-locking the @@ -351,10 +336,6 @@ GraphListener::__shutdown() interrupt_(&interrupt_guard_condition_); listener_thread_.join(); } - rcl_ret_t ret = rcl_guard_condition_fini(&interrupt_guard_condition_); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "failed to finalize interrupt guard condition"); - } if (is_started_) { cleanup_wait_set(); } diff --git a/rclcpp/src/rclcpp/guard_condition.cpp b/rclcpp/src/rclcpp/guard_condition.cpp index 22ca3f3223..cae0c4ce5c 100644 --- a/rclcpp/src/rclcpp/guard_condition.cpp +++ b/rclcpp/src/rclcpp/guard_condition.cpp @@ -20,19 +20,20 @@ namespace rclcpp { -GuardCondition::GuardCondition(rclcpp::Context::SharedPtr context) +GuardCondition::GuardCondition( + rclcpp::Context::SharedPtr context, + rcl_guard_condition_options_t guard_condition_options) : context_(context), rcl_guard_condition_{rcl_get_zero_initialized_guard_condition()} { if (!context_) { throw std::invalid_argument("context argument unexpectedly nullptr"); } - rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options(); rcl_ret_t ret = rcl_guard_condition_init( &this->rcl_guard_condition_, context_->get_rcl_context().get(), guard_condition_options); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to create guard condition"); } } @@ -45,7 +46,7 @@ GuardCondition::~GuardCondition() } catch (const std::exception & exception) { RCLCPP_ERROR( rclcpp::get_logger("rclcpp"), - "Error in destruction of rcl guard condition: %s", exception.what()); + "failed to finalize guard condition: %s", exception.what()); } } } @@ -56,6 +57,12 @@ GuardCondition::get_context() const return context_; } +rcl_guard_condition_t & +GuardCondition::get_rcl_guard_condition() +{ + return rcl_guard_condition_; +} + const rcl_guard_condition_t & GuardCondition::get_rcl_guard_condition() const { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index 8fa46abe60..ba8ab50b03 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -44,30 +44,15 @@ NodeBase::NodeBase( node_handle_(nullptr), default_callback_group_(nullptr), associated_with_executor_(false), + notify_guard_condition_(context), notify_guard_condition_is_valid_(false) { - // Setup the guard condition that is notified when changes occur in the graph. - rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options(); - rcl_ret_t ret = rcl_guard_condition_init( - ¬ify_guard_condition_, context_->get_rcl_context().get(), guard_condition_options); - if (ret != RCL_RET_OK) { - throw_from_rcl_error(ret, "failed to create interrupt guard condition"); - } - - // Setup a safe exit lamda to clean up the guard condition in case of an error here. - auto finalize_notify_guard_condition = [this]() { - // Finalize the interrupt guard condition. - if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy guard condition: %s", rcl_get_error_string().str); - } - }; - // Create the rcl node and store it in a shared_ptr with a custom destructor. std::unique_ptr rcl_node(new rcl_node_t(rcl_get_zero_initialized_node())); std::shared_ptr logging_mutex = get_global_logging_mutex(); + + rcl_ret_t ret; { std::lock_guard guard(*logging_mutex); // TODO(ivanpauno): /rosout Qos should be reconfigurable. @@ -80,9 +65,6 @@ NodeBase::NodeBase( context_->get_rcl_context().get(), &rcl_node_options); } if (ret != RCL_RET_OK) { - // Finalize the interrupt guard condition. - finalize_notify_guard_condition(); - if (ret == RCL_RET_NODE_INVALID_NAME) { rcl_reset_error(); // discard rcl_node_init error int validation_result; @@ -160,11 +142,6 @@ NodeBase::~NodeBase() { std::lock_guard notify_condition_lock(notify_guard_condition_mutex_); notify_guard_condition_is_valid_ = false; - if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy guard condition: %s", rcl_get_error_string().str); - } } } @@ -262,20 +239,14 @@ NodeBase::get_associated_with_executor_atomic() return associated_with_executor_; } -rcl_guard_condition_t * +rclcpp::GuardCondition & NodeBase::get_notify_guard_condition() { std::lock_guard notify_condition_lock(notify_guard_condition_mutex_); if (!notify_guard_condition_is_valid_) { - return nullptr; + throw std::runtime_error("Trying to get invalid notify guard condition"); } - return ¬ify_guard_condition_; -} - -std::unique_lock -NodeBase::acquire_notify_guard_condition_lock() const -{ - return std::unique_lock(notify_guard_condition_mutex_); + return notify_guard_condition_; } bool @@ -310,3 +281,9 @@ NodeBase::resolve_topic_or_service_name( allocator.deallocate(output_cstr, allocator.state); return output; } + +void +NodeBase::trigger_notify_guard_condition() +{ + notify_guard_condition_.trigger(); +} diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp index 415b6277ec..fe479beff5 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -348,13 +348,7 @@ NodeGraph::notify_graph_change() } } graph_cv_.notify_all(); - { - auto notify_condition_lock = node_base_->acquire_notify_guard_condition_lock(); - rcl_ret_t ret = rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "failed to trigger notify guard condition"); - } - } + node_base_->trigger_notify_guard_condition(); } void diff --git a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp index dee39cb403..c0c3935fcb 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp @@ -41,15 +41,7 @@ NodeServices::add_service( } // Notify the executor that a new service was created using the parent Node. - { - auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); - if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { - throw std::runtime_error( - std::string("Failed to notify wait set on service creation: ") + - rmw_get_error_string().str - ); - } - } + node_base_->trigger_notify_guard_condition(); } void @@ -68,15 +60,7 @@ NodeServices::add_client( } // Notify the executor that a new client was created using the parent Node. - { - auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); - if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { - throw std::runtime_error( - std::string("Failed to notify wait set on client creation: ") + - rmw_get_error_string().str - ); - } - } + node_base_->trigger_notify_guard_condition(); } std::string diff --git a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp index 6936e26759..f54e64a216 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp @@ -41,11 +41,9 @@ NodeTimers::add_timer( } else { node_base_->get_default_callback_group()->add_timer(timer); } - if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { - throw std::runtime_error( - std::string("Failed to notify wait set on timer creation: ") + - rmw_get_error_string().str); - } + + node_base_->trigger_notify_guard_condition(); + TRACEPOINT( rclcpp_timer_link_node, static_cast(timer->get_timer_handle().get()), diff --git a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp index e3a4723630..e1fcde7691 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -61,14 +61,7 @@ NodeTopics::add_publisher( } // Notify the executor that a new publisher was created using the parent Node. - { - auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); - if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { - throw std::runtime_error( - std::string("Failed to notify wait set on publisher creation: ") + - rmw_get_error_string().str); - } - } + node_base_->trigger_notify_guard_condition(); } rclcpp::SubscriptionBase::SharedPtr @@ -110,14 +103,7 @@ NodeTopics::add_subscription( } // Notify the executor that a new subscription was created using the parent Node. - { - auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); - auto ret = rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()); - if (ret != RCL_RET_OK) { - using rclcpp::exceptions::throw_from_rcl_error; - throw_from_rcl_error(ret, "failed to notify wait set on subscription creation"); - } - } + node_base_->trigger_notify_guard_condition(); } rclcpp::node_interfaces::NodeBaseInterface * diff --git a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp index 0fd083788a..f6af1829b1 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp @@ -41,15 +41,7 @@ NodeWaitables::add_waitable( } // Notify the executor that a new waitable was created using the parent Node. - { - auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); - if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { - throw std::runtime_error( - std::string("Failed to notify wait set on waitable creation: ") + - rmw_get_error_string().str - ); - } - } + node_base_->trigger_notify_guard_condition(); } void diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp index 3b951f34de..0537ff3a0c 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "rclcpp/experimental/subscription_intra_process_base.hpp" +#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" using rclcpp::experimental::SubscriptionIntraProcessBase; @@ -21,8 +22,9 @@ SubscriptionIntraProcessBase::add_to_wait_set(rcl_wait_set_t * wait_set) { std::lock_guard lock(reentrant_mutex_); - rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &gc_, NULL); - return RCL_RET_OK == ret; + detail::add_guard_condition_to_rcl_wait_set(*wait_set, gc_); + + return true; } const char * diff --git a/rclcpp/test/benchmark/benchmark_executor.cpp b/rclcpp/test/benchmark/benchmark_executor.cpp index c1fb3cf52c..1d9f1d2d32 100644 --- a/rclcpp/test/benchmark/benchmark_executor.cpp +++ b/rclcpp/test/benchmark/benchmark_executor.cpp @@ -376,9 +376,8 @@ BENCHMARK_F( auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy, guard_condition); RCLCPP_SCOPE_EXIT(entities_collector_->fini()); reset_heap_counters(); diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 1cf90d9901..127f77a4d2 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -30,7 +30,9 @@ #include "rcl/error_handling.h" #include "rcl/time.h" #include "rclcpp/clock.hpp" +#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" #include "rclcpp/duration.hpp" +#include "rclcpp/guard_condition.hpp" #include "rclcpp/rclcpp.hpp" #include "test_msgs/msg/empty.hpp" @@ -352,40 +354,18 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) { class TestWaitable : public rclcpp::Waitable { public: - TestWaitable() - { - rcl_guard_condition_options_t guard_condition_options = - rcl_guard_condition_get_default_options(); - - gc_ = rcl_get_zero_initialized_guard_condition(); - rcl_ret_t ret = rcl_guard_condition_init( - &gc_, - rclcpp::contexts::get_global_default_context()->get_rcl_context().get(), - guard_condition_options); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } - } - - ~TestWaitable() - { - rcl_ret_t ret = rcl_guard_condition_fini(&gc_); - if (RCL_RET_OK != ret) { - fprintf(stderr, "failed to call rcl_guard_condition_fini\n"); - } - } + TestWaitable() = default; bool add_to_wait_set(rcl_wait_set_t * wait_set) override { - rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &gc_, NULL); - return RCL_RET_OK == ret; + rclcpp::detail::add_guard_condition_to_rcl_wait_set(*wait_set, gc_); + return true; } - bool trigger() + void trigger() { - rcl_ret_t ret = rcl_trigger_guard_condition(&gc_); - return RCL_RET_OK == ret; + gc_.trigger(); } bool @@ -420,7 +400,7 @@ class TestWaitable : public rclcpp::Waitable private: size_t count_ = 0; - rcl_guard_condition_t gc_; + rclcpp::GuardCondition gc_; }; TYPED_TEST(TestExecutors, spinAll) { diff --git a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp index fba322f9e2..6649d70539 100644 --- a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp +++ b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp @@ -141,12 +141,11 @@ TEST_F(TestStaticExecutorEntitiesCollector, init_bad_arguments) { RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); // Check memory strategy is nullptr rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy = nullptr; EXPECT_THROW( - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition), + entities_collector_->init(&wait_set, memory_strategy, guard_condition), std::runtime_error); } @@ -167,9 +166,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_basic_node) { auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy, guard_condition); RCLCPP_SCOPE_EXIT(entities_collector_->fini()); EXPECT_EQ( expected_number_of_entities->subscriptions, @@ -183,7 +180,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_basic_node) { entities_collector_->get_number_of_waitables()); EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface())); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy, guard_condition); EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions()); EXPECT_EQ(0u, entities_collector_->get_number_of_timers()); EXPECT_EQ(0u, entities_collector_->get_number_of_services()); @@ -214,10 +211,9 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_out_of_scope) { auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); // Expect weak_node pointers to be cleaned up and used - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy, guard_condition); RCLCPP_SCOPE_EXIT(entities_collector_->fini()); EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions()); EXPECT_EQ(0u, entities_collector_->get_number_of_timers()); @@ -286,9 +282,8 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) { auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy, guard_condition); RCLCPP_SCOPE_EXIT(entities_collector_->fini()); EXPECT_EQ( @@ -308,7 +303,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) { entities_collector_->get_number_of_waitables()); entities_collector_->remove_node(node->get_node_base_interface()); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy, guard_condition); EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions()); EXPECT_EQ(0u, entities_collector_->get_number_of_timers()); EXPECT_EQ(0u, entities_collector_->get_number_of_services()); @@ -365,9 +360,8 @@ TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_clear_ auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy, guard_condition); RCLCPP_SCOPE_EXIT(entities_collector_->fini()); { @@ -396,9 +390,8 @@ TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_resize auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy, guard_condition); RCLCPP_SCOPE_EXIT(entities_collector_->fini()); { @@ -434,9 +427,8 @@ TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_rcl_wait_failed) { auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy, guard_condition); RCLCPP_SCOPE_EXIT(entities_collector_->fini()); { @@ -483,9 +475,8 @@ TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_add_handles_to_wait auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy, guard_condition); RCLCPP_SCOPE_EXIT(entities_collector_->fini()); { @@ -515,14 +506,13 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_to_wait_set_nullptr) { auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy, guard_condition); RCLCPP_SCOPE_EXIT(entities_collector_->fini()); - RCLCPP_EXPECT_THROW_EQ( + EXPECT_THROW( entities_collector_->add_to_wait_set(nullptr), - std::runtime_error("Executor waitable: couldn't add guard condition to wait set")); + std::invalid_argument); rcl_reset_error(); EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface())); @@ -543,7 +533,6 @@ TEST_F(TestStaticExecutorEntitiesCollector, fill_memory_strategy_invalid_group) auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); @@ -553,7 +542,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, fill_memory_strategy_invalid_group) cb_group.reset(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy, guard_condition); RCLCPP_SCOPE_EXIT(entities_collector_->fini()); ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u); @@ -621,9 +610,8 @@ TEST_F( auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy, guard_condition); RCLCPP_SCOPE_EXIT(entities_collector_->fini()); cb_group->get_associated_with_executor_atomic().exchange(false); diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp index a7607f5d76..38388d9308 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp @@ -103,7 +103,7 @@ TEST_F(TestNodeService, add_service_rcl_trigger_guard_condition_error) "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( node_services->add_service(service, callback_group), - std::runtime_error("Failed to notify wait set on service creation: error not set")); + std::runtime_error("error not set")); } TEST_F(TestNodeService, add_client) @@ -130,7 +130,7 @@ TEST_F(TestNodeService, add_client_rcl_trigger_guard_condition_error) "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( node_services->add_client(client, callback_group), - std::runtime_error("Failed to notify wait set on client creation: error not set")); + std::runtime_error("error not set")); } TEST_F(TestNodeService, resolve_service_name) diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp index e206e75b6c..e1d5cd179d 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp @@ -86,5 +86,5 @@ TEST_F(TestNodeTimers, add_timer_rcl_trigger_guard_condition_error) "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( node_timers->add_timer(timer, callback_group), - std::runtime_error("Failed to notify wait set on timer creation: error not set")); + std::runtime_error("error not set")); } diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp index 16f1d92b86..fcba94494e 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp @@ -127,7 +127,7 @@ TEST_F(TestNodeTopics, add_publisher_rcl_trigger_guard_condition_error) "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( node_topics->add_publisher(publisher, callback_group), - std::runtime_error("Failed to notify wait set on publisher creation: error not set")); + std::runtime_error("error not set")); } TEST_F(TestNodeTopics, add_subscription) @@ -155,7 +155,7 @@ TEST_F(TestNodeTopics, add_subscription_rcl_trigger_guard_condition_error) "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( node_topics->add_subscription(subscription, callback_group), - std::runtime_error("failed to notify wait set on subscription creation: error not set")); + std::runtime_error("error not set")); } TEST_F(TestNodeTopics, resolve_topic_name) diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp index a12ef36eab..cb2db50084 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp @@ -96,5 +96,5 @@ TEST_F(TestNodeWaitables, add_waitable_rcl_trigger_guard_condition_error) "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( node_waitables->add_waitable(waitable, callback_group), - std::runtime_error("Failed to notify wait set on waitable creation: error not set")); + std::runtime_error("error not set")); } diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp index f57171ec8e..2c0f7bcf90 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -468,30 +468,30 @@ TEST_F(TestAllocatorMemoryStrategy, construct_destruct) { } TEST_F(TestAllocatorMemoryStrategy, add_remove_guard_conditions) { - rcl_guard_condition_t guard_condition1 = rcl_get_zero_initialized_guard_condition(); - rcl_guard_condition_t guard_condition2 = rcl_get_zero_initialized_guard_condition(); - rcl_guard_condition_t guard_condition3 = rcl_get_zero_initialized_guard_condition(); + rclcpp::GuardCondition guard_condition1; + rclcpp::GuardCondition guard_condition2; + rclcpp::GuardCondition guard_condition3; - EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition1)); - EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition2)); - EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition3)); + EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(guard_condition1)); + EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(guard_condition2)); + EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(guard_condition3)); EXPECT_EQ(3u, allocator_memory_strategy()->number_of_guard_conditions()); // Adding a second time should not add to vector - EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition1)); - EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition2)); - EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition3)); + EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(guard_condition1)); + EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(guard_condition2)); + EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(guard_condition3)); EXPECT_EQ(3u, allocator_memory_strategy()->number_of_guard_conditions()); - EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition1)); - EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition2)); - EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition3)); + EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(guard_condition1)); + EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(guard_condition2)); + EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(guard_condition3)); EXPECT_EQ(0u, allocator_memory_strategy()->number_of_guard_conditions()); // Removing second time should have no effect - EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition1)); - EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition2)); - EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition3)); + EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(guard_condition1)); + EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(guard_condition2)); + EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(guard_condition3)); EXPECT_EQ(0u, allocator_memory_strategy()->number_of_guard_conditions()); } @@ -585,24 +585,17 @@ TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_client) { TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_guard_condition) { auto node = create_node_with_disabled_callback_groups("node"); - rcl_guard_condition_t guard_condition = rcl_get_zero_initialized_guard_condition(); auto context = node->get_node_base_interface()->get_context(); - rcl_context_t * rcl_context = context->get_rcl_context().get(); - rcl_guard_condition_options_t guard_condition_options = { - rcl_get_default_allocator()}; - - EXPECT_EQ( - RCL_RET_OK, - rcl_guard_condition_init(&guard_condition, rcl_context, guard_condition_options)); - RCLCPP_SCOPE_EXIT( - { - EXPECT_EQ(RCL_RET_OK, rcl_guard_condition_fini(&guard_condition)); - }); - allocator_memory_strategy()->add_guard_condition(&guard_condition); + rclcpp::GuardCondition guard_condition(context); + + EXPECT_NO_THROW(rclcpp::GuardCondition guard_condition(context);); + + allocator_memory_strategy()->add_guard_condition(guard_condition); + RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities(); insufficient_capacities.size_of_guard_conditions = 0; - EXPECT_TRUE(TestAddHandlesToWaitSet(node, insufficient_capacities)); + EXPECT_THROW(TestAddHandlesToWaitSet(node, insufficient_capacities), std::runtime_error); } TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_timer) { diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp index fce0369edd..d5818a697f 100644 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -155,7 +155,7 @@ TEST_F(TestExecutor, add_callback_group_failed_trigger_guard_condition) { "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( dummy.add_callback_group(cb_group, node->get_node_base_interface(), true), - std::runtime_error("Failed to trigger guard condition on callback group add: error not set")); + std::runtime_error("error not set")); } TEST_F(TestExecutor, remove_callback_group_null_node) { @@ -186,7 +186,7 @@ TEST_F(TestExecutor, remove_callback_group_failed_trigger_guard_condition) { RCLCPP_EXPECT_THROW_EQ( dummy.remove_callback_group(cb_group, true), std::runtime_error( - "Failed to trigger guard condition on callback group remove: error not set")); + "error not set")); } TEST_F(TestExecutor, remove_node_not_associated) { @@ -325,7 +325,7 @@ TEST_F(TestExecutor, cancel_failed_trigger_guard_condition) { "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( dummy.cancel(), - std::runtime_error("Failed to trigger guard condition in cancel: error not set")); + std::runtime_error("error not set")); } TEST_F(TestExecutor, set_memory_strategy_nullptr) { @@ -360,7 +360,7 @@ TEST_F(TestExecutor, spin_once_failed_trigger_guard_condition) { RCLCPP_EXPECT_THROW_EQ( dummy.spin_once(std::chrono::milliseconds(1)), std::runtime_error( - "Failed to trigger guard condition from execute_any_executable: error not set")); + "error not set")); } TEST_F(TestExecutor, spin_some_fail_wait_set_clear) { diff --git a/rclcpp/test/rclcpp/test_graph_listener.cpp b/rclcpp/test/rclcpp/test_graph_listener.cpp index bd66c686d7..24c370e65d 100644 --- a/rclcpp/test/rclcpp/test_graph_listener.cpp +++ b/rclcpp/test/rclcpp/test_graph_listener.cpp @@ -87,7 +87,7 @@ TEST_F(TestGraphListener, error_construct_graph_listener) { auto graph_listener_error = std::make_shared(get_global_default_context()); graph_listener_error.reset(); - }, std::runtime_error("failed to create interrupt guard condition: error not set")); + }, std::runtime_error("failed to create guard condition: error not set")); } // Required for mocking_utils below @@ -169,7 +169,7 @@ TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_set_add_guard_condi "lib:rclcpp", rcl_wait_set_add_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( graph_listener_test->run_protected(), - std::runtime_error("failed to add interrupt guard condition to wait set: error not set")); + std::runtime_error("failed to add guard condition to wait set: error not set")); } TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_error) { @@ -292,9 +292,7 @@ TEST_F(TestGraphListener, test_graph_listener_shutdown_guard_fini_error_throw) { auto mock_wait_set_fini = mocking_utils::patch_and_return( "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR); - RCLCPP_EXPECT_THROW_EQ( - graph_listener_test->shutdown(), - std::runtime_error("failed to finalize interrupt guard condition: error not set")); + EXPECT_NO_THROW(graph_listener_test->shutdown()); graph_listener_test->mock_cleanup_wait_set(); }