From 1d1615344f42d1335bd837705e1cdff0b9372273 Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Thu, 12 Aug 2021 13:01:25 +0100 Subject: [PATCH 1/3] Use rclcpp::GuardCondition Signed-off-by: Mauro Passerino --- rclcpp/CMakeLists.txt | 1 + .../add_guard_condition_to_rcl_wait_set.hpp | 40 ++++++++++ rclcpp/include/rclcpp/executor.hpp | 4 +- .../static_executor_entities_collector.hpp | 4 +- .../subscription_intra_process_base.hpp | 8 +- .../subscription_intra_process_buffer.hpp | 28 +------ rclcpp/include/rclcpp/graph_listener.hpp | 3 +- rclcpp/include/rclcpp/guard_condition.hpp | 9 ++- rclcpp/include/rclcpp/memory_strategy.hpp | 6 +- .../rclcpp/node_interfaces/node_base.hpp | 8 +- .../node_interfaces/node_base_interface.hpp | 14 +--- rclcpp/include/rclcpp/qos_event.hpp | 2 +- .../strategies/allocator_memory_strategy.hpp | 28 +++---- .../detail/storage_policy_common.hpp | 5 +- rclcpp/include/rclcpp/waitable.hpp | 9 +-- .../add_guard_condition_to_rcl_wait_set.cpp | 39 +++++++++ rclcpp/src/rclcpp/executor.cpp | 80 +++++++++---------- .../static_executor_entities_collector.cpp | 17 ++-- .../static_single_threaded_executor.cpp | 16 +--- rclcpp/src/rclcpp/graph_listener.cpp | 33 ++------ rclcpp/src/rclcpp/guard_condition.cpp | 15 +++- .../src/rclcpp/node_interfaces/node_base.cpp | 41 ++-------- .../src/rclcpp/node_interfaces/node_graph.cpp | 12 +-- .../rclcpp/node_interfaces/node_services.cpp | 28 +++---- .../rclcpp/node_interfaces/node_timers.cpp | 10 ++- .../rclcpp/node_interfaces/node_topics.cpp | 26 +++--- .../rclcpp/node_interfaces/node_waitables.cpp | 14 ++-- rclcpp/src/rclcpp/qos_event.cpp | 3 +- .../subscription_intra_process_base.cpp | 8 +- .../test/rclcpp/executors/test_executors.cpp | 37 ++------- ...est_static_executor_entities_collector.cpp | 6 +- .../node_interfaces/test_node_graph.cpp | 4 +- .../node_interfaces/test_node_topics.cpp | 2 +- .../node_interfaces/test_node_waitables.cpp | 2 +- .../test_allocator_memory_strategy.cpp | 61 +++++++------- rclcpp/test/rclcpp/test_graph_listener.cpp | 8 +- rclcpp/test/rclcpp/test_memory_strategy.cpp | 2 +- rclcpp/test/rclcpp/test_qos_event.cpp | 2 +- .../test_dynamic_storage.cpp | 2 +- .../wait_set_policies/test_static_storage.cpp | 2 +- .../test_storage_policy_common.cpp | 7 +- .../test_thread_safe_synchronization.cpp | 2 +- .../include/rclcpp_action/client.hpp | 2 +- .../include/rclcpp_action/server.hpp | 2 +- rclcpp_action/src/client.cpp | 6 +- rclcpp_action/src/server.cpp | 6 +- rclcpp_action/test/test_client.cpp | 2 +- rclcpp_action/test/test_server.cpp | 2 +- 48 files changed, 314 insertions(+), 354 deletions(-) create mode 100644 rclcpp/include/rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp create mode 100644 rclcpp/src/rclcpp/detail/add_guard_condition_to_rcl_wait_set.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 125cab556b..0072aaca8a 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/resolve_parameter_overrides.cpp src/rclcpp/detail/rmw_implementation_specific_payload.cpp src/rclcpp/detail/rmw_implementation_specific_publisher_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..3f9c426f85 --- /dev/null +++ b/rclcpp/include/rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp @@ -0,0 +1,40 @@ +// 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 reference to a wait set where to add the guard condition + * \param[in] guard_condition reference to the guard_condition to be added + */ +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 ca00f1309e..dfd88ccea2 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 2c4613c845..bda28cb74b 100644 --- a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp +++ b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp @@ -102,7 +102,7 @@ class StaticExecutorEntitiesCollector final * \throws std::runtime_error if it couldn't add guard condition to wait set */ RCLCPP_PUBLIC - bool + void add_to_wait_set(rcl_wait_set_t * wait_set) override; RCLCPP_PUBLIC @@ -328,7 +328,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_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index ae57521999..ba575c763c 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -25,6 +25,7 @@ #include "rcl/error_handling.h" +#include "rclcpp/guard_condition.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/waitable.hpp" @@ -41,9 +42,10 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable RCLCPP_PUBLIC SubscriptionIntraProcessBase( + rclcpp::Context::SharedPtr context, const std::string & topic_name, const rclcpp::QoS & qos_profile) - : topic_name_(topic_name), qos_profile_(qos_profile) + : gc_(context), topic_name_(topic_name), qos_profile_(qos_profile) {} virtual ~SubscriptionIntraProcessBase() = default; @@ -53,7 +55,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable get_number_of_ready_guard_conditions() {return 1;} RCLCPP_PUBLIC - bool + void add_to_wait_set(rcl_wait_set_t * wait_set); virtual bool @@ -79,7 +81,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable protected: std::recursive_mutex reentrant_mutex_; - rcl_guard_condition_t gc_; + rclcpp::GuardCondition gc_; private: virtual void diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index 05a830633c..3ec335f9f6 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -67,36 +67,13 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase const std::string & topic_name, const rclcpp::QoS & qos_profile, rclcpp::IntraProcessBufferType buffer_type) - : SubscriptionIntraProcessBase(topic_name, qos_profile) + : SubscriptionIntraProcessBase(context, topic_name, qos_profile) { // Create the intra-process buffer. buffer_ = rclcpp::experimental::create_intra_process_buffer( buffer_type, 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( - "SubscriptionIntraProcessBuffer init error initializing guard condition"); - } - } - - virtual ~SubscriptionIntraProcessBuffer() - { - 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); - } } bool @@ -130,8 +107,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase void trigger_guard_condition() { - rcl_ret_t ret = rcl_trigger_guard_condition(&gc_); - (void)ret; + gc_.trigger(); } BufferUniquePtr buffer_; 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..5104a6446c 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 @@ -58,6 +60,11 @@ class GuardCondition rclcpp::Context::SharedPtr get_context() const; + /// Return the underlying rcl guard condition structure. + RCLCPP_PUBLIC + rcl_guard_condition_t & + get_rcl_guard_condition(); + /// Return the underlying rcl guard condition structure. RCLCPP_PUBLIC const rcl_guard_condition_t & diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index da79643e4e..eadba3d6c7 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -64,9 +64,11 @@ 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 remove_guard_condition(const rcl_guard_condition_t * guard_condition) = 0; + virtual void + remove_guard_condition(const rclcpp::GuardCondition & 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 8a19a6c9df..74616a1cbc 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -114,13 +114,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; @@ -149,7 +145,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 ebe7b1b98e..b0f8fbd65e 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" @@ -142,24 +143,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 diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index b3bf61cea5..04231e1395 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -94,7 +94,7 @@ class QOSEventHandlerBase : public Waitable /// Add the Waitable to a wait set. RCLCPP_PUBLIC - bool + void add_to_wait_set(rcl_wait_set_t * wait_set) override; /// Check if the Waitable is ready. diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 43ef71682e..27faa73552 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,20 +62,20 @@ 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) { + if (existing_guard_condition == &guard_condition) { return; } } - guard_conditions_.push_back(guard_condition); + guard_conditions_.push_back(&guard_condition); } - void remove_guard_condition(const rcl_guard_condition_t * guard_condition) override + void remove_guard_condition(const rclcpp::GuardCondition & guard_condition) override { for (auto it = guard_conditions_.begin(); it != guard_conditions_.end(); ++it) { - if (*it == guard_condition) { + if (*it == &guard_condition) { guard_conditions_.erase(it); break; } @@ -240,22 +241,11 @@ 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_) { - if (!waitable->add_to_wait_set(wait_set)) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add waitable to wait set: %s", rcl_get_error_string().str); - return false; - } + waitable->add_to_wait_set(wait_set); } return true; } @@ -509,7 +499,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy using VectorRebind = std::vector::template rebind_alloc>; - VectorRebind guard_conditions_; + VectorRebind guard_conditions_; VectorRebind> subscription_handles_; VectorRebind> service_handles_; diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp index 24ed8b9130..2db3ff630a 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp @@ -383,10 +383,7 @@ class StoragePolicyCommon continue; } rclcpp::Waitable & waitable = *waitable_ptr_pair.second; - bool successful = waitable.add_to_wait_set(&rcl_wait_set_); - if (!successful) { - throw std::runtime_error("waitable unexpectedly failed to be added to wait set"); - } + waitable.add_to_wait_set(&rcl_wait_set_); } } diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 85deabda85..c78b0a885f 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -103,12 +103,11 @@ class Waitable /// Add the Waitable to a wait set. /** * \param[in] wait_set A handle to the wait set to add the Waitable to. - * \return `true` if the Waitable is added successfully, `false` otherwise. * \throws rclcpp::execptions::RCLError from rcl_wait_set_add_*() */ RCLCPP_PUBLIC virtual - bool + void add_to_wait_set(rcl_wait_set_t * wait_set) = 0; /// Check if the Waitable is ready. @@ -145,8 +144,7 @@ class Waitable * ```cpp * // ... create a wait set and a Waitable * // Add the Waitable to the wait set - * bool add_ret = waitable.add_to_wait_set(wait_set); - * // ... error handling + * waitable.add_to_wait_set(wait_set); * // Wait * rcl_ret_t wait_ret = rcl_wait(wait_set); * // ... error handling @@ -172,8 +170,7 @@ class Waitable * ```cpp * // ... create a wait set and a Waitable * // Add the Waitable to the wait set - * bool add_ret = waitable.add_to_wait_set(wait_set); - * // ... error handling + * waitable.add_to_wait_set(wait_set); * // Wait * rcl_ret_t wait_ret = rcl_wait(wait_set); * // ... error handling 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 a5d4050e70..c35224dd14 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -45,19 +45,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(); @@ -68,13 +62,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_.get()); // 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(), @@ -84,12 +78,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"); } } @@ -120,7 +108,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(); @@ -131,15 +119,9 @@ 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_.get()); + memory_strategy_->remove_guard_condition(interrupt_guard_condition_); // Remove shutdown callback handle registered to Context if (!context_->remove_on_shutdown_callback(shutdown_callback_handle_)) { @@ -234,17 +216,20 @@ 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"); + try { + interrupt_guard_condition_.trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to trigger guard condition on callback group add: ") + ex.what()); } } // 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); } } @@ -315,12 +300,14 @@ 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"); + try { + interrupt_guard_condition_.trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to trigger guard condition on callback group remove: ") + ex.what()); } } memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition()); @@ -494,9 +481,11 @@ 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"); + try { + 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()); } } @@ -541,9 +530,12 @@ 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"); + try { + interrupt_guard_condition_.trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to trigger guard condition from execute_any_executable: ") + ex.what()); } } @@ -710,9 +702,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 b6ceb5b331..bf50e062f3 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; @@ -108,7 +109,8 @@ StaticExecutorEntitiesCollector::execute(std::shared_ptr & data) std::lock_guard guard{new_nodes_mutex_}; for (const auto & weak_node : new_nodes_) { if (auto node_ptr = weak_node.lock()) { - 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; } } new_nodes_.clear(); @@ -265,18 +267,14 @@ StaticExecutorEntitiesCollector::refresh_wait_set(std::chrono::nanoseconds timeo } } -bool +void 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; } size_t StaticExecutorEntitiesCollector::get_number_of_ready_guard_conditions() @@ -441,8 +439,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 8350521fe3..683d300c06 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -139,9 +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 - if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error(rcl_get_error_string().str); - } + interrupt_guard_condition_.trigger(); } } @@ -152,9 +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 - if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error(rcl_get_error_string().str); - } + interrupt_guard_condition_.trigger(); } } @@ -171,9 +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) { - if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error(rcl_get_error_string().str); - } + interrupt_guard_condition_.trigger(); } } @@ -187,9 +181,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 f6d851dc54..e1a20a00bd 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -132,30 +132,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. @@ -168,9 +153,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; @@ -235,11 +217,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); - } } } @@ -340,20 +317,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 diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp index c993a4ed6f..07964448c6 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -533,12 +533,12 @@ 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"); - } + auto & node_gc = node_base_->get_notify_guard_condition(); + try { + node_gc.trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string("Failed to notify wait set on graph change: ") + ex.what()); } } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp index dee39cb403..c28196dda7 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp @@ -41,14 +41,12 @@ 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 - ); - } + auto & node_gc = node_base_->get_notify_guard_condition(); + try { + node_gc.trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string("Failed to notify wait set on service creation: ") + ex.what()); } } @@ -68,14 +66,12 @@ 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 - ); - } + auto & node_gc = node_base_->get_notify_guard_condition(); + try { + node_gc.trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string("Failed to notify wait set on client creation: ") + ex.what()); } } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp index 6936e26759..a68dc40e36 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp @@ -41,11 +41,15 @@ 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) { + + auto & node_gc = node_base_->get_notify_guard_condition(); + try { + node_gc.trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( - std::string("Failed to notify wait set on timer creation: ") + - rmw_get_error_string().str); + std::string("Failed to notify wait set on timer creation: ") + ex.what()); } + 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 c57fbcee5d..87139b6811 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -60,13 +60,12 @@ 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); - } + auto & node_gc = node_base_->get_notify_guard_condition(); + try { + node_gc.trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string("Failed to notify wait set on publisher creation: ") + ex.what()); } } @@ -108,13 +107,12 @@ 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"); - } + auto & node_gc = node_base_->get_notify_guard_condition(); + try { + node_gc.trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string("Failed to notify wait set on subscription creation: ") + ex.what()); } } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp index 0fd083788a..6aa75e735f 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp @@ -41,14 +41,12 @@ 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 - ); - } + auto & node_gc = node_base_->get_notify_guard_condition(); + try { + node_gc.trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string("Failed to notify wait set on waitable creation: ") + ex.what()); } } diff --git a/rclcpp/src/rclcpp/qos_event.cpp b/rclcpp/src/rclcpp/qos_event.cpp index 8af3918c48..daf1d3e01e 100644 --- a/rclcpp/src/rclcpp/qos_event.cpp +++ b/rclcpp/src/rclcpp/qos_event.cpp @@ -51,14 +51,13 @@ QOSEventHandlerBase::get_number_of_ready_events() } /// Add the Waitable to a wait set. -bool +void QOSEventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set) { rcl_ret_t ret = rcl_wait_set_add_event(wait_set, &event_handle_, &wait_set_event_index_); if (RCL_RET_OK != ret) { exceptions::throw_from_rcl_error(ret, "Couldn't add event to wait set"); } - return true; } /// Check if the Waitable is ready. diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp index 2738161675..b13123b65b 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -13,16 +13,14 @@ // 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; -bool +void 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_); } const char * diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 1cf90d9901..1fa2cbb4dd 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,17 @@ 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() = default; - ~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"); - } - } - - bool + void 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_); } - bool trigger() + void trigger() { - rcl_ret_t ret = rcl_trigger_guard_condition(&gc_); - return RCL_RET_OK == ret; + gc_.trigger(); } bool @@ -420,7 +399,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 dc81b5611e..1ea91029f4 100644 --- a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp +++ b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp @@ -230,7 +230,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_out_of_scope) { class TestWaitable : public rclcpp::Waitable { public: - bool add_to_wait_set(rcl_wait_set_t *) override {return true;} + void add_to_wait_set(rcl_wait_set_t *) override {} bool is_ready(rcl_wait_set_t *) override {return true;} @@ -513,9 +513,9 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_to_wait_set_nullptr) { entities_collector_->init(&wait_set, memory_strategy); RCPPUTILS_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())); diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp index 03f654a928..0b925d409f 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp @@ -556,9 +556,9 @@ TEST_F(TestNodeGraph, notify_graph_change_rcl_error) { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); - EXPECT_THROW( + RCLCPP_EXPECT_THROW_EQ( node()->get_node_graph_interface()->notify_graph_change(), - rclcpp::exceptions::RCLError); + std::runtime_error("Failed to notify wait set on graph change: error not set")); } TEST_F(TestNodeGraph, get_info_by_topic) diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp index bb59c83bb2..e727827f18 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp @@ -157,7 +157,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("Failed to notify wait set on subscription creation: 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..0043536786 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp @@ -28,7 +28,7 @@ class TestWaitable : public rclcpp::Waitable { public: - bool add_to_wait_set(rcl_wait_set_t *) override {return false;} + void add_to_wait_set(rcl_wait_set_t *) override {} bool is_ready(rcl_wait_set_t *) override {return false;} std::shared_ptr diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp index 09279ed024..30d30d220b 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -39,9 +39,11 @@ static bool test_waitable_result = false; class TestWaitable : public rclcpp::Waitable { public: - bool add_to_wait_set(rcl_wait_set_t *) override + void add_to_wait_set(rcl_wait_set_t *) override { - return test_waitable_result; + if (!test_waitable_result) { + throw std::runtime_error("TestWaitable add_to_wait_set failed"); + } } bool is_ready(rcl_wait_set_t *) override @@ -453,30 +455,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()); } @@ -569,24 +571,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)); - RCPPUTILS_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) { @@ -607,7 +602,9 @@ TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_waitable) { EXPECT_TRUE(allocator_memory_strategy()->add_handles_to_wait_set(nullptr)); test_waitable_result = false; - EXPECT_FALSE(allocator_memory_strategy()->add_handles_to_wait_set(nullptr)); + EXPECT_THROW( + allocator_memory_strategy()->add_handles_to_wait_set(nullptr), + std::runtime_error); // This calls TestWaitable's functions, so rcl errors are not set EXPECT_FALSE(rcl_error_is_set()); 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(); } diff --git a/rclcpp/test/rclcpp/test_memory_strategy.cpp b/rclcpp/test/rclcpp/test_memory_strategy.cpp index f20926bafa..c919d1981d 100644 --- a/rclcpp/test/rclcpp/test_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/test_memory_strategy.cpp @@ -35,7 +35,7 @@ typedef std::map take_data() override {return nullptr;} void execute(std::shared_ptr & data) override {(void)data;} diff --git a/rclcpp/test/rclcpp/test_qos_event.cpp b/rclcpp/test/rclcpp/test_qos_event.cpp index 960ffdcb4c..ebf0e14afb 100644 --- a/rclcpp/test/rclcpp/test_qos_event.cpp +++ b/rclcpp/test/rclcpp/test_qos_event.cpp @@ -315,7 +315,7 @@ TEST_F(TestQosEvent, add_to_wait_set) { { rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_add_event, RCL_RET_OK); - EXPECT_TRUE(handler.add_to_wait_set(&wait_set)); + EXPECT_NO_THROW(handler.add_to_wait_set(&wait_set)); } { diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp index 7a15e2cf49..9afb97536a 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp @@ -51,7 +51,7 @@ class TestWaitable : public rclcpp::Waitable TestWaitable() : is_ready_(false) {} - bool add_to_wait_set(rcl_wait_set_t *) override {return true;} + void add_to_wait_set(rcl_wait_set_t *) override {} bool is_ready(rcl_wait_set_t *) override {return is_ready_;} diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp index 1e4d5f805e..470d0de361 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp @@ -50,7 +50,7 @@ class TestWaitable : public rclcpp::Waitable public: TestWaitable() : is_ready_(false) {} - bool add_to_wait_set(rcl_wait_set_t *) override {return true;} + void add_to_wait_set(rcl_wait_set_t *) override {} bool is_ready(rcl_wait_set_t *) override {return is_ready_;} diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp index ba0f4d16e8..d217286da1 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp @@ -50,7 +50,12 @@ class TestWaitable : public rclcpp::Waitable public: TestWaitable() : is_ready_(false), add_to_wait_set_(false) {} - bool add_to_wait_set(rcl_wait_set_t *) override {return add_to_wait_set_;} + void add_to_wait_set(rcl_wait_set_t *) override + { + if (!add_to_wait_set_) { + throw std::runtime_error("waitable unexpectedly failed to be added to wait set"); + } + } bool is_ready(rcl_wait_set_t *) override {return is_ready_;} diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp index 99d07f1a19..347714bbf7 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp @@ -50,7 +50,7 @@ class TestWaitable : public rclcpp::Waitable public: TestWaitable() : is_ready_(false) {} - bool add_to_wait_set(rcl_wait_set_t *) override {return true;} + void add_to_wait_set(rcl_wait_set_t *) override {} bool is_ready(rcl_wait_set_t *) override {return is_ready_;} diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 8294f2be68..ed590247d9 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -108,7 +108,7 @@ class ClientBase : public rclcpp::Waitable /// \internal RCLCPP_ACTION_PUBLIC - bool + void add_to_wait_set(rcl_wait_set_t * wait_set) override; /// \internal diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index f9488fc1d5..63c99d51e0 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -109,7 +109,7 @@ class ServerBase : public rclcpp::Waitable /// Add all entities to a wait set. /// \internal RCLCPP_ACTION_PUBLIC - bool + void add_to_wait_set(rcl_wait_set_t * wait_set) override; /// Return true if any entity belonging to the action server is ready to be executed. diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 5839a4f932..84559bfa9e 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -252,12 +252,14 @@ ClientBase::get_number_of_ready_services() return pimpl_->num_services; } -bool +void ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set) { rcl_ret_t ret = rcl_action_wait_set_add_action_client( wait_set, pimpl_->client_handle.get(), nullptr, nullptr); - return RCL_RET_OK == ret; + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "ClientBase::add_to_wait_set() failed"); + } } bool diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index d1477300f1..e4765e9461 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -164,13 +164,15 @@ ServerBase::get_number_of_ready_guard_conditions() return pimpl_->num_guard_conditions_; } -bool +void ServerBase::add_to_wait_set(rcl_wait_set_t * wait_set) { std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); rcl_ret_t ret = rcl_action_wait_set_add_action_server( wait_set, pimpl_->action_server_.get(), NULL); - return RCL_RET_OK == ret; + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "ServerBase::add_to_wait_set() failed"); + } } bool diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index ae01964fd5..b911e3d71b 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -397,7 +397,7 @@ TEST_F(TestClient, is_ready) { ASSERT_EQ( RCL_RET_OK, rcl_wait_set_init(&wait_set, 10, 10, 10, 10, 10, 10, rcl_context, allocator)); - ASSERT_TRUE(action_client->add_to_wait_set(&wait_set)); + ASSERT_NO_THROW(action_client->add_to_wait_set(&wait_set)); EXPECT_TRUE(action_client->is_ready(&wait_set)); { diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 4d33c4db6d..25b38db34c 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -1107,7 +1107,7 @@ TEST_F(TestGoalRequestServer, is_ready_rcl_error) { { EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)); }); - ASSERT_TRUE(action_server_->add_to_wait_set(&wait_set)); + EXPECT_NO_THROW(action_server_->add_to_wait_set(&wait_set)); EXPECT_TRUE(action_server_->is_ready(&wait_set)); auto mock = mocking_utils::patch_and_return( From fa6e49338cf6f6ba5f1a5ba84c983ec288da15ff Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Fri, 13 Aug 2021 15:39:23 +0100 Subject: [PATCH 2/3] Pass GuardCondition ptr instead of ref to remove_guard_condition Before the api was taking a reference to a guard condition, then getting the address of it. But if a node had expired, we can't get the orig gc dereferencing a pointer, nor can we get an address of an out-of-scope guard condition. Signed-off-by: Mauro Passerino --- rclcpp/include/rclcpp/memory_strategy.hpp | 2 +- .../strategies/allocator_memory_strategy.hpp | 4 ++-- rclcpp/src/rclcpp/executor.cpp | 14 +++++++------- .../strategies/test_allocator_memory_strategy.cpp | 12 ++++++------ 4 files changed, 16 insertions(+), 16 deletions(-) diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index eadba3d6c7..0993c223c9 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -68,7 +68,7 @@ class RCLCPP_PUBLIC MemoryStrategy add_guard_condition(const rclcpp::GuardCondition & guard_condition) = 0; virtual void - remove_guard_condition(const rclcpp::GuardCondition & guard_condition) = 0; + remove_guard_condition(const rclcpp::GuardCondition * guard_condition) = 0; virtual void get_next_subscription( diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 27faa73552..d0bb9c7df2 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -72,10 +72,10 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy guard_conditions_.push_back(&guard_condition); } - void remove_guard_condition(const rclcpp::GuardCondition & guard_condition) override + void remove_guard_condition(const rclcpp::GuardCondition * guard_condition) override { for (auto it = guard_conditions_.begin(); it != guard_conditions_.end(); ++it) { - if (*it == &guard_condition) { + if (*it == guard_condition) { guard_conditions_.erase(it); break; } diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index c35224dd14..d3f6cb361f 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -107,8 +107,8 @@ Executor::~Executor() 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); + auto guard_condition = pair.second; + memory_strategy_->remove_guard_condition(guard_condition); } weak_nodes_to_guard_conditions_.clear(); @@ -120,8 +120,8 @@ Executor::~Executor() rcl_reset_error(); } // 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(shutdown_guard_condition_.get()); + memory_strategy_->remove_guard_condition(&interrupt_guard_condition_); // Remove shutdown callback handle registered to Context if (!context_->remove_on_shutdown_callback(shutdown_callback_handle_)) { @@ -310,7 +310,7 @@ Executor::remove_callback_group_from_map( "Failed to trigger guard condition on callback group remove: ") + ex.what()); } } - memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition()); + memory_strategy_->remove_guard_condition(&node_ptr->get_notify_guard_condition()); } } @@ -702,9 +702,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()) { - const auto & guard_condition = node_guard_pair->second; + 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/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp index 30d30d220b..c068e8f640 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -470,15 +470,15 @@ TEST_F(TestAllocatorMemoryStrategy, add_remove_guard_conditions) { 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()); } From 835d435fac1e66956b2f8bc43c6d1d0f8f7d46f8 Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Fri, 17 Sep 2021 16:49:39 +0100 Subject: [PATCH 3/3] Address PR comments Signed-off-by: Mauro Passerino --- .../rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp | 1 - rclcpp/src/rclcpp/node_interfaces/node_base.cpp | 2 +- rclcpp/src/rclcpp/node_interfaces/node_graph.cpp | 2 +- rclcpp/src/rclcpp/node_interfaces/node_services.cpp | 4 ++-- rclcpp/src/rclcpp/node_interfaces/node_timers.cpp | 2 +- rclcpp/src/rclcpp/node_interfaces/node_topics.cpp | 4 ++-- rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp | 2 +- rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp | 2 +- rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp | 4 ++-- rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp | 2 +- rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp | 4 ++-- rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp | 2 +- 12 files changed, 15 insertions(+), 16 deletions(-) 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 index 3f9c426f85..5f1a7e0df8 100644 --- 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 @@ -24,7 +24,6 @@ namespace detail /// Adds the guard condition to a waitset /** - * This function is thread-safe. * \param[in] wait_set reference to a wait set where to add the guard condition * \param[in] guard_condition reference to the guard_condition to be added */ diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index e1a20a00bd..e68d1a01eb 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -322,7 +322,7 @@ NodeBase::get_notify_guard_condition() { std::lock_guard notify_condition_lock(notify_guard_condition_mutex_); if (!notify_guard_condition_is_valid_) { - throw std::runtime_error("Trying to get invalid notify guard condition"); + throw std::runtime_error("failed to get notify guard condition because it is invalid"); } return notify_guard_condition_; } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp index 07964448c6..e13ec7cd4c 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -538,7 +538,7 @@ NodeGraph::notify_graph_change() node_gc.trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( - std::string("Failed to notify wait set on graph change: ") + ex.what()); + std::string("failed to notify wait set on graph change: ") + ex.what()); } } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp index c28196dda7..14ab1c82c4 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp @@ -46,7 +46,7 @@ NodeServices::add_service( node_gc.trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( - std::string("Failed to notify wait set on service creation: ") + ex.what()); + std::string("failed to notify wait set on service creation: ") + ex.what()); } } @@ -71,7 +71,7 @@ NodeServices::add_client( node_gc.trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( - std::string("Failed to notify wait set on client creation: ") + ex.what()); + std::string("failed to notify wait set on client creation: ") + ex.what()); } } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp index a68dc40e36..b463e8a0e7 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp @@ -47,7 +47,7 @@ NodeTimers::add_timer( node_gc.trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( - std::string("Failed to notify wait set on timer creation: ") + ex.what()); + std::string("failed to notify wait set on timer creation: ") + ex.what()); } TRACEPOINT( diff --git a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp index 87139b6811..fa188a2f9a 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -65,7 +65,7 @@ NodeTopics::add_publisher( node_gc.trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( - std::string("Failed to notify wait set on publisher creation: ") + ex.what()); + std::string("failed to notify wait set on publisher creation: ") + ex.what()); } } @@ -112,7 +112,7 @@ NodeTopics::add_subscription( node_gc.trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( - std::string("Failed to notify wait set on subscription creation: ") + ex.what()); + std::string("failed to notify wait set on subscription creation: ") + ex.what()); } } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp index 6aa75e735f..6f243f6025 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp @@ -46,7 +46,7 @@ NodeWaitables::add_waitable( node_gc.trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( - std::string("Failed to notify wait set on waitable creation: ") + ex.what()); + std::string("failed to notify wait set on waitable creation: ") + ex.what()); } } diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp index 0b925d409f..c25af15422 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp @@ -558,7 +558,7 @@ TEST_F(TestNodeGraph, notify_graph_change_rcl_error) "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( node()->get_node_graph_interface()->notify_graph_change(), - std::runtime_error("Failed to notify wait set on graph change: error not set")); + std::runtime_error("failed to notify wait set on graph change: error not set")); } TEST_F(TestNodeGraph, get_info_by_topic) diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp index a7607f5d76..ea55a1aac2 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("failed to notify wait set on service creation: 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("failed to notify wait set on client creation: 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 d368e0e875..d038a4b44d 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp @@ -87,5 +87,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("failed to notify wait set on timer creation: 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 e727827f18..a99633bec6 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp @@ -129,7 +129,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("failed to notify wait set on publisher creation: error not set")); } TEST_F(TestNodeTopics, add_subscription) @@ -157,7 +157,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("failed to notify wait set on subscription creation: 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 0043536786..a1f35c0cdc 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("failed to notify wait set on waitable creation: error not set")); }