From a008db47afa2cc062e4f8fde6f61ad63a5f05e34 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 13 Apr 2020 16:41:59 -0700 Subject: [PATCH] Revert "[rclcpp] add WaitSet class and modify entities to work without executor (#1047)" This reverts commit aaf8b3828cb4da7e5dd3f017e653f132e5b0ba8c. --- rclcpp/CMakeLists.txt | 16 - .../rclcpp/any_subscription_callback.hpp | 13 +- rclcpp/include/rclcpp/client.hpp | 57 -- rclcpp/include/rclcpp/context.hpp | 2 +- rclcpp/include/rclcpp/guard_condition.hpp | 100 --- rclcpp/include/rclcpp/message_info.hpp | 52 -- rclcpp/include/rclcpp/rclcpp.hpp | 2 - rclcpp/include/rclcpp/service.hpp | 106 +-- rclcpp/include/rclcpp/subscription.hpp | 72 +- rclcpp/include/rclcpp/subscription_base.hpp | 70 +- .../rclcpp/subscription_wait_set_mask.hpp | 37 - rclcpp/include/rclcpp/timer.hpp | 19 +- rclcpp/include/rclcpp/utilities.hpp | 2 +- rclcpp/include/rclcpp/wait_result.hpp | 155 ---- rclcpp/include/rclcpp/wait_result_kind.hpp | 33 - rclcpp/include/rclcpp/wait_set.hpp | 106 --- .../detail/storage_policy_common.hpp | 412 ---------- .../detail/synchronization_policy_common.hpp | 72 -- .../write_preferring_read_write_lock.hpp | 243 ------ .../wait_set_policies/dynamic_storage.hpp | 470 ----------- .../sequential_synchronization.hpp | 317 -------- .../wait_set_policies/static_storage.hpp | 201 ----- .../thread_safe_synchronization.hpp | 379 --------- rclcpp/include/rclcpp/wait_set_template.hpp | 743 ------------------ rclcpp/include/rclcpp/waitable.hpp | 20 +- rclcpp/src/rclcpp/client.cpp | 21 - rclcpp/src/rclcpp/executor.cpp | 176 ++--- rclcpp/src/rclcpp/guard_condition.cpp | 80 -- rclcpp/src/rclcpp/message_info.cpp | 39 - rclcpp/src/rclcpp/service.cpp | 21 - rclcpp/src/rclcpp/subscription_base.cpp | 64 -- rclcpp/src/rclcpp/timer.cpp | 6 - .../write_preferring_read_write_lock.cpp | 100 --- rclcpp/src/rclcpp/waitable.cpp | 6 - rclcpp/test/test_guard_condition.cpp | 87 -- rclcpp/test/test_subscription.cpp | 87 +- rclcpp/test/test_wait_set.cpp | 264 ------- 37 files changed, 139 insertions(+), 4511 deletions(-) delete mode 100644 rclcpp/include/rclcpp/guard_condition.hpp delete mode 100644 rclcpp/include/rclcpp/message_info.hpp delete mode 100644 rclcpp/include/rclcpp/subscription_wait_set_mask.hpp delete mode 100644 rclcpp/include/rclcpp/wait_result.hpp delete mode 100644 rclcpp/include/rclcpp/wait_result_kind.hpp delete mode 100644 rclcpp/include/rclcpp/wait_set.hpp delete mode 100644 rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp delete mode 100644 rclcpp/include/rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp delete mode 100644 rclcpp/include/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp delete mode 100644 rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp delete mode 100644 rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp delete mode 100644 rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp delete mode 100644 rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp delete mode 100644 rclcpp/include/rclcpp/wait_set_template.hpp delete mode 100644 rclcpp/src/rclcpp/guard_condition.cpp delete mode 100644 rclcpp/src/rclcpp/message_info.cpp delete mode 100644 rclcpp/src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp delete mode 100644 rclcpp/test/test_guard_condition.cpp delete mode 100644 rclcpp/test/test_wait_set.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 1389ad8aeb..f4a6c895e7 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -49,13 +49,11 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/executors/static_executor_entities_collector.cpp src/rclcpp/executors/static_single_threaded_executor.cpp src/rclcpp/graph_listener.cpp - src/rclcpp/guard_condition.cpp src/rclcpp/init_options.cpp src/rclcpp/intra_process_manager.cpp src/rclcpp/logger.cpp src/rclcpp/memory_strategies.cpp src/rclcpp/memory_strategy.cpp - src/rclcpp/message_info.cpp src/rclcpp/node.cpp src/rclcpp/node_options.cpp src/rclcpp/node_interfaces/node_base.cpp @@ -86,7 +84,6 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/timer.cpp src/rclcpp/type_support.cpp src/rclcpp/utilities.cpp - src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp src/rclcpp/waitable.cpp ) @@ -522,19 +519,6 @@ if(BUILD_TESTING) target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME}) endif() - ament_add_gtest(test_guard_condition test/test_guard_condition.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") - if(TARGET test_guard_condition) - target_link_libraries(test_guard_condition ${PROJECT_NAME}) - endif() - - ament_add_gtest(test_wait_set test/test_wait_set.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") - if(TARGET test_wait_set) - ament_target_dependencies(test_wait_set "test_msgs") - target_link_libraries(test_wait_set ${PROJECT_NAME}) - endif() - # Install test resources install( DIRECTORY test/resources diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 6a813c222b..1909b85815 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -25,7 +25,6 @@ #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/function_traits.hpp" -#include "rclcpp/message_info.hpp" #include "rclcpp/visibility_control.hpp" #include "tracetools/tracetools.h" #include "tracetools/utils.hpp" @@ -44,13 +43,13 @@ class AnySubscriptionCallback using SharedPtrCallback = std::function)>; using SharedPtrWithInfoCallback = - std::function, const rclcpp::MessageInfo &)>; + std::function, const rmw_message_info_t &)>; using ConstSharedPtrCallback = std::function)>; using ConstSharedPtrWithInfoCallback = - std::function, const rclcpp::MessageInfo &)>; + std::function, const rmw_message_info_t &)>; using UniquePtrCallback = std::function; using UniquePtrWithInfoCallback = - std::function; + std::function; SharedPtrCallback shared_ptr_callback_; SharedPtrWithInfoCallback shared_ptr_with_info_callback_; @@ -156,7 +155,7 @@ class AnySubscriptionCallback } void dispatch( - std::shared_ptr message, const rclcpp::MessageInfo & message_info) + std::shared_ptr message, const rmw_message_info_t & message_info) { TRACEPOINT(callback_start, (const void *)this, false); if (shared_ptr_callback_) { @@ -182,7 +181,7 @@ class AnySubscriptionCallback } void dispatch_intra_process( - ConstMessageSharedPtr message, const rclcpp::MessageInfo & message_info) + ConstMessageSharedPtr message, const rmw_message_info_t & message_info) { TRACEPOINT(callback_start, (const void *)this, true); if (const_shared_ptr_callback_) { @@ -205,7 +204,7 @@ class AnySubscriptionCallback } void dispatch_intra_process( - MessageUniquePtr message, const rclcpp::MessageInfo & message_info) + MessageUniquePtr message, const rmw_message_info_t & message_info) { TRACEPOINT(callback_start, (const void *)this, true); if (shared_ptr_callback_) { diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index f7f1d5cb8c..46467400fe 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -15,7 +15,6 @@ #ifndef RCLCPP__CLIENT_HPP_ #define RCLCPP__CLIENT_HPP_ -#include #include #include #include @@ -63,27 +62,6 @@ class ClientBase RCLCPP_PUBLIC virtual ~ClientBase(); - /// Take the next response for this client as a type erased pointer. - /** - * The type erased pointer allows for this method to be used in a type - * agnostic way along with ClientBase::create_response(), - * ClientBase::create_request_header(), and ClientBase::handle_response(). - * The typed version of this can be used if the Service type is known, - * \sa Client::take_response(). - * - * \param[out] response_out The type erased pointer to a Service Response into - * which the middleware will copy the response being taken. - * \param[out] request_header_out The request header to be filled by the - * middleware when taking, and which can be used to associte the response - * to a specific request. - * \returns true if the response was taken, otherwise false. - * \throws rclcpp::exceptions::RCLError based exceptions if the underlying - * rcl function fail. - */ - RCLCPP_PUBLIC - bool - take_type_erased_response(void * response_out, rmw_request_id_t & request_header_out); - RCLCPP_PUBLIC const char * get_service_name() const; @@ -115,20 +93,6 @@ class ClientBase virtual void handle_response( std::shared_ptr request_header, std::shared_ptr response) = 0; - /// Exchange the "in use by wait set" state for this client. - /** - * This is used to ensure this client is not used by multiple - * wait sets at the same time. - * - * \param[in] in_use_state the new state to exchange into the state, true - * indicates it is now in use by a wait set, and false is that it is no - * longer in use by a wait set. - * \returns the previous state. - */ - RCLCPP_PUBLIC - bool - exchange_in_use_by_wait_set_state(bool in_use_state); - protected: RCLCPP_DISABLE_COPY(ClientBase) @@ -149,8 +113,6 @@ class ClientBase std::shared_ptr context_; std::shared_ptr client_handle_; - - std::atomic in_use_by_wait_set_{false}; }; template @@ -209,25 +171,6 @@ class Client : public ClientBase { } - /// Take the next response for this client. - /** - * \sa ClientBase::take_type_erased_response(). - * - * \param[out] response_out The reference to a Service Response into - * which the middleware will copy the response being taken. - * \param[out] request_header_out The request header to be filled by the - * middleware when taking, and which can be used to associte the response - * to a specific request. - * \returns true if the response was taken, otherwise false. - * \throws rclcpp::exceptions::RCLError based exceptions if the underlying - * rcl function fail. - */ - bool - take_response(typename ServiceT::Response & response_out, rmw_request_id_t & request_header_out) - { - return this->take_type_erased_response(&response_out, request_header_out); - } - std::shared_ptr create_response() override { diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index 76d3cb1dca..dfc33d5e4f 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -159,7 +159,7 @@ class Context : public std::enable_shared_from_this * * \param[in] reason the description of why shutdown happened * \return true if shutdown was successful, false if context was already shutdown - * \throw various exceptions derived from rclcpp::exceptions::RCLError, if rcl_shutdown fails + * \throw various exceptions derived from RCLErrorBase, if rcl_shutdown fails */ RCLCPP_PUBLIC virtual diff --git a/rclcpp/include/rclcpp/guard_condition.hpp b/rclcpp/include/rclcpp/guard_condition.hpp deleted file mode 100644 index 5088181022..0000000000 --- a/rclcpp/include/rclcpp/guard_condition.hpp +++ /dev/null @@ -1,100 +0,0 @@ -// Copyright 2020 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__GUARD_CONDITION_HPP_ -#define RCLCPP__GUARD_CONDITION_HPP_ - -#include - -#include "rcl/guard_condition.h" - -#include "rclcpp/context.hpp" -#include "rclcpp/contexts/default_context.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/visibility_control.hpp" - -namespace rclcpp -{ - -/// A condition that can be waited on in a single wait set and asynchronously triggered. -class GuardCondition -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(GuardCondition) - - // TODO(wjwwood): support custom allocator, maybe restrict to polymorphic allocator - /// Construct the guard condition, optionally specifying which Context to use. - /** - * \param[in] context Optional custom context to be used. - * Defaults to using the global default context singleton. - * Shared ownership of the context is held with the guard condition until - * destruction. - * \throws std::invalid_argument if the context is nullptr. - * \throws rclcpp::exceptions::RCLError based exceptions when underlying - * rcl functions fail. - */ - RCLCPP_PUBLIC - explicit GuardCondition( - rclcpp::Context::SharedPtr context = - rclcpp::contexts::default_context::get_global_default_context()); - - RCLCPP_PUBLIC - virtual - ~GuardCondition(); - - /// Return the context used when creating this guard condition. - RCLCPP_PUBLIC - rclcpp::Context::SharedPtr - get_context() const; - - /// Return the underlying rcl guard condition structure. - RCLCPP_PUBLIC - const rcl_guard_condition_t & - get_rcl_guard_condition() const; - - /// Notify the wait set waiting on this condition, if any, that the condition had been met. - /** - * This function is thread-safe, and may be called concurrently with waiting - * on this guard condition in a wait set. - * - * \throws rclcpp::exceptions::RCLError based exceptions when underlying - * rcl functions fail. - */ - RCLCPP_PUBLIC - void - trigger(); - - /// Exchange the "in use by wait set" state for this guard condition. - /** - * This is used to ensure this guard condition is not used by multiple - * wait sets at the same time. - * - * \param[in] in_use_state the new state to exchange into the state, true - * indicates it is now in use by a wait set, and false is that it is no - * longer in use by a wait set. - * \returns the previous state. - */ - RCLCPP_PUBLIC - bool - exchange_in_use_by_wait_set_state(bool in_use_state); - -protected: - rclcpp::Context::SharedPtr context_; - rcl_guard_condition_t rcl_guard_condition_; - std::atomic in_use_by_wait_set_{false}; -}; - -} // namespace rclcpp - -#endif // RCLCPP__GUARD_CONDITION_HPP_ diff --git a/rclcpp/include/rclcpp/message_info.hpp b/rclcpp/include/rclcpp/message_info.hpp deleted file mode 100644 index 1653b0a505..0000000000 --- a/rclcpp/include/rclcpp/message_info.hpp +++ /dev/null @@ -1,52 +0,0 @@ -// Copyright 2020 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__MESSAGE_INFO_HPP_ -#define RCLCPP__MESSAGE_INFO_HPP_ - -#include "rmw/types.h" - -#include "rclcpp/visibility_control.hpp" - -namespace rclcpp -{ - -/// Additional meta data about messages taken from subscriptions. -class RCLCPP_PUBLIC MessageInfo -{ -public: - /// Default empty constructor. - MessageInfo() = default; - - /// Conversion constructor, which is intentionally not marked as explicit. - // cppcheck-suppress noExplicitConstructor - MessageInfo(const rmw_message_info_t & rmw_message_info); // NOLINT(runtime/explicit) - - virtual ~MessageInfo(); - - /// Return the message info as the underlying rmw message info type. - const rmw_message_info_t & - get_rmw_message_info() const; - - /// Return the message info as the underlying rmw message info type. - rmw_message_info_t & - get_rmw_message_info(); - -private: - rmw_message_info_t rmw_message_info_; -}; - -} // namespace rclcpp - -#endif // RCLCPP__MESSAGE_INFO_HPP_ diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index fc7b4435a4..98de3cba55 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -143,7 +143,6 @@ #include #include "rclcpp/executors.hpp" -#include "rclcpp/guard_condition.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/node.hpp" #include "rclcpp/parameter.hpp" @@ -153,7 +152,6 @@ #include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" -#include "rclcpp/wait_set.hpp" #include "rclcpp/waitable.hpp" #endif // RCLCPP__RCLCPP_HPP_ diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index de720109a6..9b7a120102 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -15,7 +15,6 @@ #ifndef RCLCPP__SERVICE_HPP_ #define RCLCPP__SERVICE_HPP_ -#include #include #include #include @@ -45,7 +44,8 @@ class ServiceBase RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase) RCLCPP_PUBLIC - explicit ServiceBase(std::shared_ptr node_handle); + explicit ServiceBase( + std::shared_ptr node_handle); RCLCPP_PUBLIC virtual ~ServiceBase(); @@ -62,53 +62,12 @@ class ServiceBase std::shared_ptr get_service_handle() const; - /// Take the next request from the service as a type erased pointer. - /** - * This type erased version of \sa Service::take_request() is useful when - * using the service in a type agnostic way with methods like - * ServiceBase::create_request(), ServiceBase::create_request_header(), and - * ServiceBase::handle_request(). - * - * \param[out] request_out The type erased pointer to a service request object - * into which the middleware will copy the taken request. - * \param[out] request_id_out The output id for the request which can be used - * to associate response with this request in the future. - * \returns true if the request was taken, otherwise false. - * \throws rclcpp::exceptions::RCLError based exceptions if the underlying - * rcl calls fail. - */ - RCLCPP_PUBLIC - bool - take_type_erased_request(void * request_out, rmw_request_id_t & request_id_out); - - virtual - std::shared_ptr - create_request() = 0; - - virtual - std::shared_ptr - create_request_header() = 0; - - virtual - void - handle_request( + virtual std::shared_ptr create_request() = 0; + virtual std::shared_ptr create_request_header() = 0; + virtual void handle_request( std::shared_ptr request_header, std::shared_ptr request) = 0; - /// Exchange the "in use by wait set" state for this service. - /** - * This is used to ensure this service is not used by multiple - * wait sets at the same time. - * - * \param[in] in_use_state the new state to exchange into the state, true - * indicates it is now in use by a wait set, and false is that it is no - * longer in use by a wait set. - * \returns the previous state. - */ - RCLCPP_PUBLIC - bool - exchange_in_use_by_wait_set_state(bool in_use_state); - protected: RCLCPP_DISABLE_COPY(ServiceBase) @@ -124,8 +83,6 @@ class ServiceBase std::shared_ptr service_handle_; bool owns_rcl_handle_ = true; - - std::atomic in_use_by_wait_set_{false}; }; template @@ -265,63 +222,36 @@ class Service : public ServiceBase { } - /// Take the next request from the service. - /** - * \sa ServiceBase::take_type_erased_request(). - * - * \param[out] request_out The reference to a service request object - * into which the middleware will copy the taken request. - * \param[out] request_id_out The output id for the request which can be used - * to associate response with this request in the future. - * \returns true if the request was taken, otherwise false. - * \throws rclcpp::exceptions::RCLError based exceptions if the underlying - * rcl calls fail. - */ - bool - take_request(typename ServiceT::Request & request_out, rmw_request_id_t & request_id_out) + std::shared_ptr create_request() override { - return this->take_type_erased_request(&request_out, request_id_out); + return std::shared_ptr(new typename ServiceT::Request()); } - std::shared_ptr - create_request() override + std::shared_ptr create_request_header() override { - return std::make_shared(); + // TODO(wjwwood): This should probably use rmw_request_id's allocator. + // (since it is a C type) + return std::shared_ptr(new rmw_request_id_t); } - std::shared_ptr - create_request_header() override - { - return std::make_shared(); - } - - void - handle_request( + void handle_request( std::shared_ptr request_header, std::shared_ptr request) override { auto typed_request = std::static_pointer_cast(request); - auto response = std::make_shared(); + auto response = std::shared_ptr(new typename ServiceT::Response); any_callback_.dispatch(request_header, typed_request, response); - send_response(*request_header, *response); + send_response(request_header, response); } - [[deprecated("use the send_response() which takes references instead of shared pointers")]] - void - send_response( + void send_response( std::shared_ptr req_id, std::shared_ptr response) { - send_response(*req_id, *response); - } + rcl_ret_t status = rcl_send_response(get_service_handle().get(), req_id.get(), response.get()); - void - send_response(rmw_request_id_t & req_id, typename ServiceT::Response & response) - { - rcl_ret_t ret = rcl_send_response(get_service_handle().get(), &req_id, &response); - - if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send response"); + if (status != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(status, "failed to send response"); } } diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 190a437d3c..2e920e286f 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -38,7 +38,6 @@ #include "rclcpp/experimental/subscription_intra_process.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" -#include "rclcpp/message_info.hpp" #include "rclcpp/message_memory_strategy.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/subscription_base.hpp" @@ -146,17 +145,19 @@ class Subscription : public SubscriptionBase // First create a SubscriptionIntraProcess which will be given to the intra-process manager. auto context = node_base->get_context(); - using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess< - CallbackMessageT, - AllocatorT, - typename MessageUniquePtr::deleter_type>; - auto subscription_intra_process = std::make_shared( + auto subscription_intra_process = std::make_shared< + rclcpp::experimental::SubscriptionIntraProcess< + CallbackMessageT, + AllocatorT, + typename MessageUniquePtr::deleter_type + >>( callback, options.get_allocator(), context, - this->get_topic_name(), // important to get like this, as it has the fully-qualified name + this->get_topic_name(), // important to get like this, as it has the fully-qualified name qos_profile, - resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback)); + resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback) + ); TRACEPOINT( rclcpp_subscription_init, (const void *)get_subscription_handle().get(), @@ -186,8 +187,7 @@ class Subscription : public SubscriptionBase } /// Called after construction to continue setup that requires shared_from_this(). - void - post_init_setup( + void post_init_setup( rclcpp::node_interfaces::NodeBaseInterface * node_base, const rclcpp::QoS & qos, const rclcpp::SubscriptionOptionsWithAllocator & options) @@ -197,32 +197,19 @@ class Subscription : public SubscriptionBase (void)options; } - /// Take the next message from the inter-process subscription. + /// Support dynamically setting the message memory strategy. /** - * Data may be taken (written) into the message_out and message_info_out even - * if false is returned. - * Specifically in the case of dropping redundant intra-process data, where - * data is received via both intra-process and inter-process (due to the - * underlying middleware being unabled to avoid this duplicate delivery) and - * so inter-process data from those intra-process publishers is ignored, but - * it has to be taken to know if it came from an intra-process publisher or - * not, and therefore could be dropped. - * - * \sa SubscriptionBase::take_type_erased() - * - * \param[out] message_out The message into which take will copy the data. - * \param[out] message_info_out The message info for the taken message. - * \returns true if data was taken and is valid, otherwise false - * \throws any rcl errors from rcl_take, \sa rclcpp::exceptions::throw_from_rcl_error() + * Behavior may be undefined if called while the subscription could be executing. + * \param[in] message_memory_strategy Shared pointer to the memory strategy to set. */ - bool - take(CallbackMessageT & message_out, rclcpp::MessageInfo & message_info_out) + void set_message_memory_strategy( + typename message_memory_strategy::MessageMemoryStrategy::SharedPtr message_memory_strategy) { - return this->take_type_erased(static_cast(&message_out), message_info_out); + message_memory_strategy_ = message_memory_strategy; } - std::shared_ptr - create_message() override + std::shared_ptr create_message() override { /* The default message memory strategy provides a dynamically allocated message on each call to * create_message, though alternative memory strategies that re-use a preallocated message may be @@ -231,18 +218,15 @@ class Subscription : public SubscriptionBase return message_memory_strategy_->borrow_message(); } - std::shared_ptr - create_serialized_message() override + std::shared_ptr create_serialized_message() override { return message_memory_strategy_->borrow_serialized_message(); } - void - handle_message( - std::shared_ptr & message, - const rclcpp::MessageInfo & message_info) override + void handle_message( + std::shared_ptr & message, const rmw_message_info_t & message_info) override { - if (matches_any_intra_process_publishers(&message_info.get_rmw_message_info().publisher_gid)) { + if (matches_any_intra_process_publishers(&message_info.publisher_gid)) { // In this case, the message will be delivered via intra process and // we should ignore this copy of the message. return; @@ -253,8 +237,7 @@ class Subscription : public SubscriptionBase void handle_loaned_message( - void * loaned_message, - const rclcpp::MessageInfo & message_info) override + void * loaned_message, const rmw_message_info_t & message_info) override { auto typed_message = static_cast(loaned_message); // message is loaned, so we have to make sure that the deleter does not deallocate the message @@ -265,21 +248,18 @@ class Subscription : public SubscriptionBase /// Return the borrowed message. /** \param message message to be returned */ - void - return_message(std::shared_ptr & message) override + void return_message(std::shared_ptr & message) override { auto typed_message = std::static_pointer_cast(message); message_memory_strategy_->return_message(typed_message); } - void - return_serialized_message(std::shared_ptr & message) override + void return_serialized_message(std::shared_ptr & message) override { message_memory_strategy_->return_serialized_message(message); } - bool - use_take_shared_method() const + bool use_take_shared_method() const { return any_callback_.use_take_shared_method(); } diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 6275c69a72..23ecb5e004 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -15,12 +15,9 @@ #ifndef RCLCPP__SUBSCRIPTION_BASE_HPP_ #define RCLCPP__SUBSCRIPTION_BASE_HPP_ -#include #include #include -#include #include -#include #include "rcl/subscription.h" @@ -30,7 +27,6 @@ #include "rclcpp/experimental/intra_process_manager.hpp" #include "rclcpp/experimental/subscription_intra_process_base.hpp" #include "rclcpp/macros.hpp" -#include "rclcpp/message_info.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/qos_event.hpp" #include "rclcpp/type_support_decl.hpp" @@ -114,45 +110,6 @@ class SubscriptionBase : public std::enable_shared_from_this rclcpp::QoS get_actual_qos() const; - /// Take the next inter-process message from the subscription as a type erased pointer. - /** - * \sa Subscription::take() for details on how this function works. - * - * The only difference is that it takes a type erased pointer rather than a - * reference to the exact message type. - * - * This type erased version facilitates using the subscriptions in a type - * agnostic way using SubscriptionBase::create_message() and - * SubscriptionBase::handle_message(). - * - * \param[out] message_out The type erased message pointer into which take - * will copy the data. - * \param[out] message_info_out The message info for the taken message. - * \returns true if data was taken and is valid, otherwise false - * \throws any rcl errors from rcl_take, \sa rclcpp::exceptions::throw_from_rcl_error() - */ - RCLCPP_PUBLIC - bool - take_type_erased(void * message_out, rclcpp::MessageInfo & message_info_out); - - /// Take the next inter-process message, in its serialized form, from the subscription. - /** - * For now, if data is taken (written) into the message_out and - * message_info_out then true will be returned. - * Unlike Subscription::take(), taking data serialized is not possible via - * intra-process for the time being, so it will not need to de-duplicate - * data in any case. - * - * \param[out] message_out The serialized message data structure used to - * store the taken message. - * \param[out] message_info_out The message info for the taken message. - * \returns true if data was taken and is valid, otherwise false - * \throws any rcl errors from rcl_take, \sa rclcpp::exceptions::throw_from_rcl_error() - */ - RCLCPP_PUBLIC - bool - take_serialized(rcl_serialized_message_t & message_out, rclcpp::MessageInfo & message_info_out); - /// Borrow a new message. /** \return Shared pointer to the fresh message. */ RCLCPP_PUBLIC @@ -175,12 +132,12 @@ class SubscriptionBase : public std::enable_shared_from_this RCLCPP_PUBLIC virtual void - handle_message(std::shared_ptr & message, const rclcpp::MessageInfo & message_info) = 0; + handle_message(std::shared_ptr & message, const rmw_message_info_t & message_info) = 0; RCLCPP_PUBLIC virtual void - handle_loaned_message(void * loaned_message, const rclcpp::MessageInfo & message_info) = 0; + handle_loaned_message(void * loaned_message, const rmw_message_info_t & message_info) = 0; /// Return the message borrowed in create_message. /** \param[in] message Shared pointer to the returned message. */ @@ -236,23 +193,6 @@ class SubscriptionBase : public std::enable_shared_from_this rclcpp::Waitable::SharedPtr get_intra_process_waitable() const; - /// Exchange state of whether or not a part of the subscription is used by a wait set. - /** - * Used to ensure parts of the subscription are not used with multiple wait - * sets simultaneously. - * - * \param[in] pointer_to_subscription_part address of a subscription part - * \param[in] in_use_state the new state to exchange, true means "now in use", - * and false means "no longer in use". - * \returns the current "in use" state. - * \throws std::invalid_argument If pointer_to_subscription_part is nullptr. - * \throws std::runtime_error If the pointer given is not a pointer to one of - * the parts of the subscription which can be used with a wait set. - */ - RCLCPP_PUBLIC - bool - exchange_in_use_by_wait_set_state(void * pointer_to_subscription_part, bool in_use_state); - protected: template void @@ -265,7 +205,6 @@ class SubscriptionBase : public std::enable_shared_from_this rcl_subscription_event_init, get_subscription_handle().get(), event_type); - qos_events_in_use_by_wait_set_.insert(std::make_pair(handler.get(), false)); event_handlers_.emplace_back(handler); } @@ -290,11 +229,6 @@ class SubscriptionBase : public std::enable_shared_from_this rosidl_message_type_support_t type_support_; bool is_serialized_; - - std::atomic subscription_in_use_by_wait_set_{false}; - std::atomic intra_process_subscription_waitable_in_use_by_wait_set_{false}; - std::unordered_map> qos_events_in_use_by_wait_set_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/subscription_wait_set_mask.hpp b/rclcpp/include/rclcpp/subscription_wait_set_mask.hpp deleted file mode 100644 index 597dd81d3e..0000000000 --- a/rclcpp/include/rclcpp/subscription_wait_set_mask.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright 2020 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__SUBSCRIPTION_WAIT_SET_MASK_HPP_ -#define RCLCPP__SUBSCRIPTION_WAIT_SET_MASK_HPP_ - -#include "rclcpp/visibility_control.hpp" - -namespace rclcpp -{ - -/// Options used to determine what parts of a subscription get added to or removed from a wait set. -class RCLCPP_PUBLIC SubscriptionWaitSetMask -{ -public: - /// If true, include the actual subscription. - bool include_subscription = true; - /// If true, include any events attached to the subscription. - bool include_events = true; - /// If true, include the waitable used to handle intra process communication. - bool include_intra_process_waitable = true; -}; - -} // namespace rclcpp - -#endif // RCLCPP__SUBSCRIPTION_WAIT_SET_MASK_HPP_ diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index dd754c0748..afeb63b663 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -15,7 +15,6 @@ #ifndef RCLCPP__TIMER_HPP_ #define RCLCPP__TIMER_HPP_ -#include #include #include #include @@ -65,7 +64,7 @@ class TimerBase /** * \return true if the timer has been cancelled, false otherwise * \throws std::runtime_error if the rcl_get_error_state returns 0 - * \throws rclcpp::exceptions::RCLError some child class exception based on ret + * \throws RCLErrorBase some child class exception based on ret */ RCLCPP_PUBLIC bool @@ -102,25 +101,9 @@ class TimerBase RCLCPP_PUBLIC bool is_ready(); - /// Exchange the "in use by wait set" state for this timer. - /** - * This is used to ensure this timer is not used by multiple - * wait sets at the same time. - * - * \param[in] in_use_state the new state to exchange into the state, true - * indicates it is now in use by a wait set, and false is that it is no - * longer in use by a wait set. - * \returns the previous state. - */ - RCLCPP_PUBLIC - bool - exchange_in_use_by_wait_set_state(bool in_use_state); - protected: Clock::SharedPtr clock_; std::shared_ptr timer_handle_; - - std::atomic in_use_by_wait_set_{false}; }; diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index 690df026bc..83fa285f60 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -121,7 +121,7 @@ init_and_remove_ros_arguments( * \param[in] argv Argument vector. * \returns Members of the argument vector that are not ROS arguments. * \throws anything throw_from_rcl_error can throw - * \throws rclcpp::exceptions::RCLError if the parsing fails + * \throws rclcpp::exceptions::RCLErrorBase if the parsing fails */ RCLCPP_PUBLIC std::vector diff --git a/rclcpp/include/rclcpp/wait_result.hpp b/rclcpp/include/rclcpp/wait_result.hpp deleted file mode 100644 index 0dfa6974ea..0000000000 --- a/rclcpp/include/rclcpp/wait_result.hpp +++ /dev/null @@ -1,155 +0,0 @@ -// Copyright 2020 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__WAIT_RESULT_HPP_ -#define RCLCPP__WAIT_RESULT_HPP_ - -#include -#include -#include - -#include "rcl/wait.h" - -#include "rclcpp/macros.hpp" -#include "rclcpp/wait_result_kind.hpp" - -namespace rclcpp -{ - -// TODO(wjwwood): the union-like design of this class could be replaced with -// std::variant, when we have access to that... -/// Interface for introspecting a wait set after waiting on it. -/** - * This class: - * - * - provides the result of waiting, i.e. ready, timeout, or empty, and - * - holds the ownership of the entities of the wait set, if needed, and - * - provides the necessary information for iterating over the wait set. - * - * This class is only valid as long as the wait set which created it is valid, - * and it must be deleted before the wait set is deleted, as it contains a - * back reference to the wait set. - * - * An instance of this, which is returned from rclcpp::WaitSetTemplate::wait(), - * will cause the wait set to keep ownership of the entities because it only - * holds a reference to the sequences of them, rather than taking a copy. - * Also, in the thread-safe case, an instance of this will cause the wait set, - * to block calls which modify the sequences of the entities, e.g. add/remove - * guard condition or subscription methods. - * - * \tparam WaitSetT The wait set type which created this class. - */ -template -class WaitResult final -{ -public: - /// Create WaitResult from a "ready" result. - /** - * \param[in] wait_set A reference to the wait set, which this class - * will keep for the duration of its lifetime. - */ - static - WaitResult - from_ready_wait_result_kind(WaitSetT & wait_set) - { - return WaitResult(WaitResultKind::Ready, wait_set); - } - - /// Create WaitResult from a "timeout" result. - static - WaitResult - from_timeout_wait_result_kind() - { - return WaitResult(WaitResultKind::Timeout); - } - - /// Create WaitResult from a "empty" result. - static - WaitResult - from_empty_wait_result_kind() - { - return WaitResult(WaitResultKind::Empty); - } - - /// Return the kind of the WaitResult. - WaitResultKind - kind() const - { - return wait_result_kind_; - } - - /// Return the rcl wait set. - const WaitSetT & - get_wait_set() const - { - if (this->kind() != WaitResultKind::Ready) { - throw std::runtime_error("cannot access wait set when the result was not ready"); - } - // This should never happen, defensive (and debug mode) check only. - assert(wait_set_pointer_); - return *wait_set_pointer_; - } - - /// Return the rcl wait set. - WaitSetT & - get_wait_set() - { - if (this->kind() != WaitResultKind::Ready) { - throw std::runtime_error("cannot access wait set when the result was not ready"); - } - // This should never happen, defensive (and debug mode) check only. - assert(wait_set_pointer_); - return *wait_set_pointer_; - } - - WaitResult(WaitResult && other) noexcept - : wait_result_kind_(other.wait_result_kind_), - wait_set_pointer_(std::exchange(other.wait_set_pointer_, nullptr)) - {} - - ~WaitResult() - { - if (wait_set_pointer_) { - wait_set_pointer_->wait_result_release(); - } - } - -private: - RCLCPP_DISABLE_COPY(WaitResult) - - explicit WaitResult(WaitResultKind wait_result_kind) - : wait_result_kind_(wait_result_kind) - { - // Should be enforced by the static factory methods on this class. - assert(WaitResultKind::Ready != wait_result_kind); - } - - WaitResult(WaitResultKind wait_result_kind, WaitSetT & wait_set) - : wait_result_kind_(wait_result_kind), - wait_set_pointer_(&wait_set) - { - // Should be enforced by the static factory methods on this class. - assert(WaitResultKind::Ready == wait_result_kind); - // Secure thread-safety (if provided) and shared ownership (if needed). - wait_set_pointer_->wait_result_acquire(); - } - - const WaitResultKind wait_result_kind_; - - WaitSetT * wait_set_pointer_ = nullptr; -}; - -} // namespace rclcpp - -#endif // RCLCPP__WAIT_RESULT_HPP_ diff --git a/rclcpp/include/rclcpp/wait_result_kind.hpp b/rclcpp/include/rclcpp/wait_result_kind.hpp deleted file mode 100644 index 3ce65bf4f3..0000000000 --- a/rclcpp/include/rclcpp/wait_result_kind.hpp +++ /dev/null @@ -1,33 +0,0 @@ -// Copyright 2020 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__WAIT_RESULT_KIND_HPP_ -#define RCLCPP__WAIT_RESULT_KIND_HPP_ - -#include "rclcpp/visibility_control.hpp" - -namespace rclcpp -{ - -/// Represents the various kinds of results from waiting on a wait set. -enum RCLCPP_PUBLIC WaitResultKind -{ - Ready, // - -#include "rcl/wait.h" - -#include "rclcpp/guard_condition.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/visibility_control.hpp" -#include "rclcpp/wait_set_policies/dynamic_storage.hpp" -#include "rclcpp/wait_set_policies/sequential_synchronization.hpp" -#include "rclcpp/wait_set_policies/static_storage.hpp" -#include "rclcpp/wait_set_policies/thread_safe_synchronization.hpp" -#include "rclcpp/wait_set_template.hpp" - -namespace rclcpp -{ - -/// Most common user configuration of a WaitSet, which is dynamic but not thread-safe. -/** - * This wait set allows you to add and remove items dynamically, and it will - * automatically remove items that are let out of scope each time wait() or - * prune_destroyed_entities() is called. - * - * It will not, however, provide thread-safety for adding and removing entities - * while waiting. - * - * \sa rclcpp::WaitSetTemplate for API documentation - */ -using WaitSet = rclcpp::WaitSetTemplate< - rclcpp::wait_set_policies::SequentialSynchronization, - rclcpp::wait_set_policies::DynamicStorage ->; - -/// WaitSet configuration which does not allow changes after construction. -/** - * This wait set requires that you specify all entities at construction, and - * prevents you from calling the typical add and remove functions. - * It also requires that you specify how many of each item there will be as a - * template argument. - * - * It will share ownership of the entities until destroyed, therefore it will - * prevent the destruction of entities so long as the wait set exists, even if - * the user lets their copy of the shared pointer to the entity go out of scope. - * - * Since the wait set cannot be mutated, it does not need to be thread-safe. - * - * \sa rclcpp::WaitSetTemplate for API documentation - */ -template< - std::size_t NumberOfSubscriptions, - std::size_t NumberOfGuardCondtions, - std::size_t NumberOfTimers, - std::size_t NumberOfClients, - std::size_t NumberOfServices, - std::size_t NumberOfWaitables -> -using StaticWaitSet = rclcpp::WaitSetTemplate< - rclcpp::wait_set_policies::SequentialSynchronization, - rclcpp::wait_set_policies::StaticStorage< - NumberOfSubscriptions, - NumberOfGuardCondtions, - NumberOfTimers, - NumberOfClients, - NumberOfServices, - NumberOfWaitables - > ->; - -/// Like WaitSet, this configuration is dynamic, but is also thread-safe. -/** - * This wait set allows you to add and remove items dynamically, and it will - * automatically remove items that are let out of scope each time wait() or - * prune_destroyed_entities() is called. - * - * It will also ensure that adding and removing items explicitly from the - * wait set is done in a thread-safe way, protecting against concurrent add and - * deletes, as well as add and deletes during a wait(). - * This thread-safety comes at some overhead and the use of thread - * synchronization primitives. - * - * \sa rclcpp::WaitSetTemplate for API documentation - */ -using ThreadSafeWaitSet = rclcpp::WaitSetTemplate< - rclcpp::wait_set_policies::ThreadSafeSynchronization, - rclcpp::wait_set_policies::DynamicStorage ->; - -} // namespace rclcpp - -#endif // RCLCPP__WAIT_SET_HPP_ 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 deleted file mode 100644 index 4f0075cb0f..0000000000 --- a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp +++ /dev/null @@ -1,412 +0,0 @@ -// Copyright 2020 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__WAIT_SET_POLICIES__DETAIL__STORAGE_POLICY_COMMON_HPP_ -#define RCLCPP__WAIT_SET_POLICIES__DETAIL__STORAGE_POLICY_COMMON_HPP_ - -#include -#include -#include - -#include "rcl/wait.h" - -#include "rclcpp/exceptions.hpp" -#include "rclcpp/logging.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/visibility_control.hpp" -#include "rclcpp/waitable.hpp" - -namespace rclcpp -{ -namespace wait_set_policies -{ -namespace detail -{ - -/// Common structure for storage policies, which provides rcl wait set access. -template -class StoragePolicyCommon -{ -protected: - template< - class SubscriptionsIterable, - class GuardConditionsIterable, - class ExtraGuardConditionsIterable, - class TimersIterable, - class ClientsIterable, - class ServicesIterable, - class WaitablesIterable - > - explicit - StoragePolicyCommon( - const SubscriptionsIterable & subscriptions, - const GuardConditionsIterable & guard_conditions, - const ExtraGuardConditionsIterable & extra_guard_conditions, - const TimersIterable & timers, - const ClientsIterable & clients, - const ServicesIterable & services, - const WaitablesIterable & waitables, - rclcpp::Context::SharedPtr context - ) - : rcl_wait_set_(rcl_get_zero_initialized_wait_set()), context_(context) - { - // Check context is not nullptr. - if (nullptr == context) { - throw std::invalid_argument("context is nullptr"); - } - // Accumulate total contributions from waitables. - size_t subscriptions_from_waitables = 0; - size_t guard_conditions_from_waitables = 0; - size_t timers_from_waitables = 0; - size_t clients_from_waitables = 0; - size_t services_from_waitables = 0; - size_t events_from_waitables = 0; - for (const auto & waitable_entry : waitables) { - rclcpp::Waitable & waitable = *waitable_entry.waitable.get(); - subscriptions_from_waitables += waitable.get_number_of_ready_subscriptions(); - guard_conditions_from_waitables += waitable.get_number_of_ready_guard_conditions(); - timers_from_waitables += waitable.get_number_of_ready_timers(); - clients_from_waitables += waitable.get_number_of_ready_clients(); - services_from_waitables += waitable.get_number_of_ready_services(); - events_from_waitables += waitable.get_number_of_ready_events(); - } - // Initialize wait set using initial inputs. - rcl_ret_t ret = rcl_wait_set_init( - &rcl_wait_set_, - subscriptions.size() + subscriptions_from_waitables, - guard_conditions.size() + extra_guard_conditions.size() + guard_conditions_from_waitables, - timers.size() + timers_from_waitables, - clients.size() + clients_from_waitables, - services.size() + services_from_waitables, - events_from_waitables, - context_->get_rcl_context().get(), - // TODO(wjwwood): support custom allocator, maybe restrict to polymorphic allocator - rcl_get_default_allocator()); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } - - // (Re)build the wait set for the first time. - this->storage_rebuild_rcl_wait_set_with_sets( - subscriptions, - guard_conditions, - extra_guard_conditions, - timers, - clients, - services, - waitables); - } - - ~StoragePolicyCommon() - { - rcl_ret_t ret = rcl_wait_set_fini(&rcl_wait_set_); - if (RCL_RET_OK != ret) { - try { - rclcpp::exceptions::throw_from_rcl_error(ret); - } catch (const std::exception & exception) { - RCLCPP_ERROR( - rclcpp::get_logger("rclcpp"), - "Error in destruction of rcl wait set: %s", exception.what()); - } - } - } - - template - std::pair - get_raw_pointer_from_smart_pointer(const std::shared_ptr & shared_pointer) - { - return {nullptr, shared_pointer.get()}; - } - - template - std::pair, EntityT *> - get_raw_pointer_from_smart_pointer(const std::weak_ptr & weak_pointer) - { - auto shared_pointer = weak_pointer.lock(); - return {shared_pointer, shared_pointer.get()}; - } - - /// Rebuild the wait set, preparing it for the next wait call. - /** - * The wait set is rebuilt by: - * - * - resizing the wait set if needed, - * - clearing the wait set if not already done by resizing, and - * - re-adding the entities. - */ - template< - class SubscriptionsIterable, - class GuardConditionsIterable, - class ExtraGuardConditionsIterable, - class TimersIterable, - class ClientsIterable, - class ServicesIterable, - class WaitablesIterable - > - void - storage_rebuild_rcl_wait_set_with_sets( - const SubscriptionsIterable & subscriptions, - const GuardConditionsIterable & guard_conditions, - const ExtraGuardConditionsIterable & extra_guard_conditions, - const TimersIterable & timers, - const ClientsIterable & clients, - const ServicesIterable & services, - const WaitablesIterable & waitables - ) - { - bool was_resized = false; - // Resize the wait set, but only if it needs to be. - if (needs_resize_) { - // Resizing with rcl_wait_set_resize() is a no-op if nothing has changed, - // but tracking the need to resize in this class avoids an unnecessary - // library call (rcl is most likely a separate shared library) each wait - // loop. - // Also, since static storage wait sets will never need resizing, so it - // avoids completely redundant calls to this function in that case. - // Accumulate total contributions from waitables. - size_t subscriptions_from_waitables = 0; - size_t guard_conditions_from_waitables = 0; - size_t timers_from_waitables = 0; - size_t clients_from_waitables = 0; - size_t services_from_waitables = 0; - size_t events_from_waitables = 0; - for (const auto & waitable_entry : waitables) { - auto waitable_ptr_pair = get_raw_pointer_from_smart_pointer(waitable_entry.waitable); - if (nullptr == waitable_ptr_pair.second) { - // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. - if (HasStrongOwnership) { - // This will not happen in fixed sized storage, as it holds - // shared ownership the whole time and is never in need of pruning. - throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); - } - // Flag for pruning. - needs_pruning_ = true; - continue; - } - rclcpp::Waitable & waitable = *waitable_ptr_pair.second; - subscriptions_from_waitables += waitable.get_number_of_ready_subscriptions(); - guard_conditions_from_waitables += waitable.get_number_of_ready_guard_conditions(); - timers_from_waitables += waitable.get_number_of_ready_timers(); - clients_from_waitables += waitable.get_number_of_ready_clients(); - services_from_waitables += waitable.get_number_of_ready_services(); - events_from_waitables += waitable.get_number_of_ready_events(); - } - rcl_ret_t ret = rcl_wait_set_resize( - &rcl_wait_set_, - subscriptions.size() + subscriptions_from_waitables, - guard_conditions.size() + extra_guard_conditions.size() + guard_conditions_from_waitables, - timers.size() + timers_from_waitables, - clients.size() + clients_from_waitables, - services.size() + services_from_waitables, - events_from_waitables - ); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } - was_resized = true; - // Assumption: the calling code ensures this function is not called - // concurrently with functions that set this variable to true, either - // with documentation (as is the case for the SequentialSychronization - // policy), or with synchronization primitives (as is the case with - // the ThreadSafeSynchronization policy). - needs_resize_ = false; - } - - // Now clear the wait set, but only if it was not resized, as resizing also - // clears the wait set. - if (!was_resized) { - rcl_ret_t ret = rcl_wait_set_clear(&rcl_wait_set_); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } - } - - // Add subscriptions. - for (const auto & subscription_entry : subscriptions) { - auto subscription_ptr_pair = - get_raw_pointer_from_smart_pointer(subscription_entry.subscription); - if (nullptr == subscription_ptr_pair.second) { - // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. - if (HasStrongOwnership) { - // This will not happen in fixed sized storage, as it holds - // shared ownership the whole time and is never in need of pruning. - throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); - } - // Flag for pruning. - needs_pruning_ = true; - continue; - } - rcl_ret_t ret = rcl_wait_set_add_subscription( - &rcl_wait_set_, - subscription_ptr_pair.second->get_subscription_handle().get(), - nullptr); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } - } - - // Setup common code to add guard_conditions. - auto add_guard_conditions = - [this](const auto & inner_guard_conditions) - { - for (const auto & guard_condition : inner_guard_conditions) { - auto guard_condition_ptr_pair = get_raw_pointer_from_smart_pointer(guard_condition); - if (nullptr == guard_condition_ptr_pair.second) { - // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. - if (HasStrongOwnership) { - // This will not happen in fixed sized storage, as it holds - // shared ownership the whole time and is never in need of pruning. - throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); - } - // Flag for pruning. - needs_pruning_ = true; - continue; - } - rcl_ret_t ret = rcl_wait_set_add_guard_condition( - &rcl_wait_set_, - &guard_condition_ptr_pair.second->get_rcl_guard_condition(), - nullptr); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } - } - }; - - // Add guard conditions. - add_guard_conditions(guard_conditions); - - // Add extra guard conditions. - add_guard_conditions(extra_guard_conditions); - - // Add timers. - for (const auto & timer : timers) { - auto timer_ptr_pair = get_raw_pointer_from_smart_pointer(timer); - if (nullptr == timer_ptr_pair.second) { - // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. - if (HasStrongOwnership) { - // This will not happen in fixed sized storage, as it holds - // shared ownership the whole time and is never in need of pruning. - throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); - } - // Flag for pruning. - needs_pruning_ = true; - continue; - } - rcl_ret_t ret = rcl_wait_set_add_timer( - &rcl_wait_set_, - timer_ptr_pair.second->get_timer_handle().get(), - nullptr); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } - } - - // Add clients. - for (const auto & client : clients) { - auto client_ptr_pair = get_raw_pointer_from_smart_pointer(client); - if (nullptr == client_ptr_pair.second) { - // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. - if (HasStrongOwnership) { - // This will not happen in fixed sized storage, as it holds - // shared ownership the whole time and is never in need of pruning. - throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); - } - // Flag for pruning. - needs_pruning_ = true; - continue; - } - rcl_ret_t ret = rcl_wait_set_add_client( - &rcl_wait_set_, - client_ptr_pair.second->get_client_handle().get(), - nullptr); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } - } - - // Add services. - for (const auto & service : services) { - auto service_ptr_pair = get_raw_pointer_from_smart_pointer(service); - if (nullptr == service_ptr_pair.second) { - // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. - if (HasStrongOwnership) { - // This will not happen in fixed sized storage, as it holds - // shared ownership the whole time and is never in need of pruning. - throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); - } - // Flag for pruning. - needs_pruning_ = true; - continue; - } - rcl_ret_t ret = rcl_wait_set_add_service( - &rcl_wait_set_, - service_ptr_pair.second->get_service_handle().get(), - nullptr); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } - } - - // Add waitables. - for (auto & waitable_entry : waitables) { - auto waitable_ptr_pair = get_raw_pointer_from_smart_pointer(waitable_entry.waitable); - if (nullptr == waitable_ptr_pair.second) { - // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. - if (HasStrongOwnership) { - // This will not happen in fixed sized storage, as it holds - // shared ownership the whole time and is never in need of pruning. - throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); - } - // Flag for pruning. - needs_pruning_ = true; - 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"); - } - } - } - - const rcl_wait_set_t & - storage_get_rcl_wait_set() const - { - return rcl_wait_set_; - } - - rcl_wait_set_t & - storage_get_rcl_wait_set() - { - return rcl_wait_set_; - } - - void - storage_flag_for_resize() - { - needs_resize_ = true; - } - - rcl_wait_set_t rcl_wait_set_; - rclcpp::Context::SharedPtr context_; - - bool needs_pruning_ = false; - bool needs_resize_ = false; -}; - -} // namespace detail -} // namespace wait_set_policies -} // namespace rclcpp - -#endif // RCLCPP__WAIT_SET_POLICIES__DETAIL__STORAGE_POLICY_COMMON_HPP_ diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp deleted file mode 100644 index 2856ec7275..0000000000 --- a/rclcpp/include/rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp +++ /dev/null @@ -1,72 +0,0 @@ -// Copyright 2020 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__WAIT_SET_POLICIES__DETAIL__SYNCHRONIZATION_POLICY_COMMON_HPP_ -#define RCLCPP__WAIT_SET_POLICIES__DETAIL__SYNCHRONIZATION_POLICY_COMMON_HPP_ - -#include -#include - -namespace rclcpp -{ -namespace wait_set_policies -{ -namespace detail -{ - -/// Common structure for synchronization policies. -class SynchronizationPolicyCommon -{ -protected: - SynchronizationPolicyCommon() = default; - ~SynchronizationPolicyCommon() = default; - - std::function - create_loop_predicate( - std::chrono::nanoseconds time_to_wait_ns, - std::chrono::steady_clock::time_point start) - { - if (time_to_wait_ns >= std::chrono::nanoseconds(0)) { - // If time_to_wait_ns is >= 0 schedule against a deadline. - auto deadline = start + time_to_wait_ns; - return [deadline]() -> bool {return std::chrono::steady_clock::now() < deadline;}; - } else { - // In the case of time_to_wait_ns < 0, just always return true to loop forever. - return []() -> bool {return true;}; - } - } - - std::chrono::nanoseconds - calculate_time_left_to_wait( - std::chrono::nanoseconds original_time_to_wait_ns, - std::chrono::steady_clock::time_point start) - { - std::chrono::nanoseconds time_left_to_wait; - if (original_time_to_wait_ns < std::chrono::nanoseconds(0)) { - time_left_to_wait = original_time_to_wait_ns; - } else { - time_left_to_wait = original_time_to_wait_ns - (std::chrono::steady_clock::now() - start); - if (time_left_to_wait < std::chrono::nanoseconds(0)) { - time_left_to_wait = std::chrono::nanoseconds(0); - } - } - return time_left_to_wait; - } -}; - -} // namespace detail -} // namespace wait_set_policies -} // namespace rclcpp - -#endif // RCLCPP__WAIT_SET_POLICIES__DETAIL__SYNCHRONIZATION_POLICY_COMMON_HPP_ diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp deleted file mode 100644 index 90b9df0178..0000000000 --- a/rclcpp/include/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp +++ /dev/null @@ -1,243 +0,0 @@ -// Copyright 2020 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__WAIT_SET_POLICIES__DETAIL__WRITE_PREFERRING_READ_WRITE_LOCK_HPP_ -#define RCLCPP__WAIT_SET_POLICIES__DETAIL__WRITE_PREFERRING_READ_WRITE_LOCK_HPP_ - -#include -#include -#include - -#include "rclcpp/visibility_control.hpp" - -namespace rclcpp -{ -namespace wait_set_policies -{ -namespace detail -{ - -/// Writer-perferring read-write lock. -/** - * This class is based on an implementation of a "write-preferring RW lock" as described in this - * wikipedia page: - * - * https://en.wikipedia.org/wiki/Readers%E2%80%93writer_lock#Using_a_condition_variable_and_a_mutex - * - * Copying here for posterity: - * - * \verbatim - * For a write-preferring RW lock one can use two integer counters and one boolean flag: - * - * num_readers_active: the number of readers that have acquired the lock (integer) - * num_writers_waiting: the number of writers waiting for access (integer) - * writer_active: whether a writer has acquired the lock (boolean). - * - * Initially num_readers_active and num_writers_waiting are zero and writer_active is false. - * - * The lock and release operations can be implemented as - * - * Begin Read - * - * Lock g - * While num_writers_waiting > 0 or writer_active: - * wait cond, g[a] - * Increment num_readers_active - * Unlock g. - * - * End Read - * - * Lock g - * Decrement num_readers_active - * If num_readers_active = 0: - * Notify cond (broadcast) - * Unlock g. - * - * Begin Write - * - * Lock g - * Increment num_writers_waiting - * While num_readers_active > 0 or writer_active is true: - * wait cond, g - * Decrement num_writers_waiting - * Set writer_active to true - * Unlock g. - * - * End Write - * - * Lock g - * Set writer_active to false - * Notify cond (broadcast) - * Unlock g. - * \endverbatim - * - * It will prefer any waiting write calls to any waiting read calls, meaning - * that excessive write calls can starve read calls. - * - * This class diverges from that design in two important ways. - * First, it is a single reader, single writer version. - * Second, it allows for user defined code to be run after a writer enters the - * waiting state, and the purpose of this feature is to allow the user to - * interrupt any potentially long blocking read activities. - * - * Together these two features allow new waiting writers to not only ensure - * they get the lock before any queued readers, but also that it can safely - * interrupt read activities if needed, without allowing new read activities to - * start before it gains the lock. - * - * The first difference prevents the case that a multiple read activities occur - * at the same time but the writer can only reliably interrupt one of them. - * By preventing multiple read activities concurrently, this case is avoided. - * The second difference allows the user to define how to interrupt read - * activity that could be blocking the write activities that need to happen - * as soon as possible. - * - * To implement the differences, this class replaces the "num_readers_active" - * counter with a "reader_active" boolean. - * It also changes the "Begin Read" section from above, like this: - * - * \verbatim - * Begin Read - * - * Lock g - * While num_writers_waiting > 0 or writer_active or reader_active: // changed - * wait cond, g[a] - * Set reader_active to true // changed - * Unlock g. - * \endverbatim - * - * And changes the "End Read" section from above, like this: - * - * \verbatim - * End Read - * - * Lock g - * Set reader_active to false // changed - * Notify cond (broadcast) // changed, now unconditional - * Unlock g. - * \endverbatim - * - * The "Begin Write" section is also updated as follows: - * - * \verbatim - * Begin Write - * - * Lock g - * Increment num_writers_waiting - * Call user defined enter_waiting function // new - * While reader_active is true or writer_active is true: // changed - * wait cond, g - * Decrement num_writers_waiting - * Set writer_active to true - * Unlock g. - * \endverbatim - * - * The implementation uses a single condition variable, single lock, and several - * state variables. - * - * The typical use of this class is as follows: - * - * class MyClass - * { - * WritePreferringReadWriteLock wprw_lock_; - * public: - * MyClass() {} - * void do_some_reading() - * { - * using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; - * std::lock_guard lock(wprw_lock_.get_read_mutex()); - * // Do reading... - * } - * void do_some_writing() - * { - * using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; - * std::lock_guard lock(wprw_lock_.get_write_mutex()); - * // Do writing... - * } - * }; - */ -class WritePreferringReadWriteLock final -{ -public: - RCLCPP_PUBLIC - explicit WritePreferringReadWriteLock(std::function enter_waiting_function = nullptr); - - /// Read mutex for the WritePreferringReadWriteLock. - /** - * Implements the "C++ named requirements: BasicLockable". - */ - class RCLCPP_PUBLIC ReadMutex - { -public: - void - lock(); - - void - unlock(); - -protected: - explicit ReadMutex(WritePreferringReadWriteLock & parent_lock); - - WritePreferringReadWriteLock & parent_lock_; - - friend WritePreferringReadWriteLock; - }; - - /// Write mutex for the WritePreferringReadWriteLock. - /** - * Implements the "C++ named requirements: BasicLockable". - */ - class RCLCPP_PUBLIC WriteMutex - { -public: - void - lock(); - - void - unlock(); - -protected: - explicit WriteMutex(WritePreferringReadWriteLock & parent_lock); - - WritePreferringReadWriteLock & parent_lock_; - - friend WritePreferringReadWriteLock; - }; - - /// Return read mutex which can be used with standard constructs like std::lock_guard. - RCLCPP_PUBLIC - ReadMutex & - get_read_mutex(); - - /// Return write mutex which can be used with standard constructs like std::lock_guard. - RCLCPP_PUBLIC - WriteMutex & - get_write_mutex(); - -protected: - bool reader_active_ = false; - std::size_t number_of_writers_waiting_ = 0; - bool writer_active_ = false; - std::mutex mutex_; - std::condition_variable condition_variable_; - ReadMutex read_mutex_; - WriteMutex write_mutex_; - std::function enter_waiting_function_; -}; - -} // namespace detail -} // namespace wait_set_policies -} // namespace rclcpp - -#endif // RCLCPP__WAIT_SET_POLICIES__DETAIL__WRITE_PREFERRING_READ_WRITE_LOCK_HPP_ diff --git a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp deleted file mode 100644 index 907fc86bdf..0000000000 --- a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp +++ /dev/null @@ -1,470 +0,0 @@ -// Copyright 2020 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__WAIT_SET_POLICIES__DYNAMIC_STORAGE_HPP_ -#define RCLCPP__WAIT_SET_POLICIES__DYNAMIC_STORAGE_HPP_ - -#include -#include -#include -#include - -#include "rclcpp/client.hpp" -#include "rclcpp/guard_condition.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/service.hpp" -#include "rclcpp/subscription_base.hpp" -#include "rclcpp/subscription_wait_set_mask.hpp" -#include "rclcpp/timer.hpp" -#include "rclcpp/visibility_control.hpp" -#include "rclcpp/wait_set_policies/detail/storage_policy_common.hpp" -#include "rclcpp/waitable.hpp" - -namespace rclcpp -{ -namespace wait_set_policies -{ - -/// WaitSet policy that provides dynamically sized storage. -class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCommon -{ -protected: - using is_mutable = std::true_type; - - class SubscriptionEntry - { - // (wjwwood): indent of 'public:' is weird, I know. uncrustify is dumb. - -public: - std::shared_ptr subscription; - rclcpp::SubscriptionWaitSetMask mask; - - /// Conversion constructor, which is intentionally not marked explicit. - SubscriptionEntry( - const std::shared_ptr & subscription_in = nullptr, - const rclcpp::SubscriptionWaitSetMask & mask_in = {}) - : subscription(subscription_in), - mask(mask_in) - {} - - void - reset() noexcept - { - subscription.reset(); - } - }; - class WeakSubscriptionEntry - { -public: - std::weak_ptr subscription; - rclcpp::SubscriptionWaitSetMask mask; - - explicit WeakSubscriptionEntry( - const std::shared_ptr & subscription_in, - const rclcpp::SubscriptionWaitSetMask & mask_in) noexcept - : subscription(subscription_in), - mask(mask_in) - {} - - explicit WeakSubscriptionEntry(const SubscriptionEntry & other) - : subscription(other.subscription), - mask(other.mask) - {} - - std::shared_ptr - lock() const - { - return subscription.lock(); - } - - bool - expired() const noexcept - { - return subscription.expired(); - } - }; - using SequenceOfWeakSubscriptions = std::vector; - using SubscriptionsIterable = std::vector; - - using SequenceOfWeakGuardConditions = std::vector>; - using GuardConditionsIterable = std::vector>; - - using SequenceOfWeakTimers = std::vector>; - using TimersIterable = std::vector>; - - using SequenceOfWeakClients = std::vector>; - using ClientsIterable = std::vector>; - - using SequenceOfWeakServices = std::vector>; - using ServicesIterable = std::vector>; - - class WaitableEntry - { -public: - std::shared_ptr waitable; - std::shared_ptr associated_entity; - - /// Conversion constructor, which is intentionally not marked explicit. - WaitableEntry( - const std::shared_ptr & waitable_in = nullptr, - const std::shared_ptr & associated_entity_in = nullptr) noexcept - : waitable(waitable_in), - associated_entity(associated_entity_in) - {} - - void - reset() noexcept - { - waitable.reset(); - associated_entity.reset(); - } - }; - class WeakWaitableEntry - { -public: - std::weak_ptr waitable; - std::weak_ptr associated_entity; - - explicit WeakWaitableEntry( - const std::shared_ptr & waitable_in, - const std::shared_ptr & associated_entity_in) noexcept - : waitable(waitable_in), - associated_entity(associated_entity_in) - {} - - explicit WeakWaitableEntry(const WaitableEntry & other) - : waitable(other.waitable), - associated_entity(other.associated_entity) - {} - - std::shared_ptr - lock() const - { - return waitable.lock(); - } - - bool - expired() const noexcept - { - return waitable.expired(); - } - }; - using SequenceOfWeakWaitables = std::vector; - using WaitablesIterable = std::vector; - - template - explicit - DynamicStorage( - const SubscriptionsIterable & subscriptions, - const GuardConditionsIterable & guard_conditions, - const ArrayOfExtraGuardConditions & extra_guard_conditions, - const TimersIterable & timers, - const ClientsIterable & clients, - const ServicesIterable & services, - const WaitablesIterable & waitables, - rclcpp::Context::SharedPtr context - ) - : StoragePolicyCommon( - subscriptions, - guard_conditions, - extra_guard_conditions, - timers, - clients, - services, - waitables, - context), - subscriptions_(subscriptions.cbegin(), subscriptions.cend()), - shared_subscriptions_(subscriptions_.size()), - guard_conditions_(guard_conditions.cbegin(), guard_conditions.cend()), - shared_guard_conditions_(guard_conditions_.size()), - timers_(timers.cbegin(), timers.cend()), - shared_timers_(timers_.size()), - clients_(clients.cbegin(), clients.cend()), - shared_clients_(clients_.size()), - services_(services.cbegin(), services.cend()), - shared_services_(services_.size()), - waitables_(waitables.cbegin(), waitables.cend()), - shared_waitables_(waitables_.size()) - {} - - ~DynamicStorage() = default; - - template - void - storage_rebuild_rcl_wait_set(const ArrayOfExtraGuardConditions & extra_guard_conditions) - { - this->storage_rebuild_rcl_wait_set_with_sets( - subscriptions_, - guard_conditions_, - extra_guard_conditions, - timers_, - clients_, - services_, - waitables_ - ); - } - - template - static - bool - storage_has_entity(const EntityT & entity, const SequenceOfEntitiesT & entities) - { - return std::any_of( - entities.cbegin(), - entities.cend(), - [&entity](const auto & inner) {return &entity == inner.lock().get();}); - } - - template - static - auto - storage_find_entity(const EntityT & entity, const SequenceOfEntitiesT & entities) - { - return std::find_if( - entities.cbegin(), - entities.cend(), - [&entity](const auto & inner) {return &entity == inner.lock().get();}); - } - - void - storage_add_subscription(std::shared_ptr && subscription) - { - if (this->storage_has_entity(*subscription, subscriptions_)) { - throw std::runtime_error("subscription already in wait set"); - } - WeakSubscriptionEntry weak_entry{std::move(subscription), {}}; - subscriptions_.push_back(std::move(weak_entry)); - this->storage_flag_for_resize(); - } - - void - storage_remove_subscription(std::shared_ptr && subscription) - { - auto it = this->storage_find_entity(*subscription, subscriptions_); - if (subscriptions_.cend() == it) { - throw std::runtime_error("subscription not in wait set"); - } - subscriptions_.erase(it); - this->storage_flag_for_resize(); - } - - void - storage_add_guard_condition(std::shared_ptr && guard_condition) - { - if (this->storage_has_entity(*guard_condition, guard_conditions_)) { - throw std::runtime_error("guard_condition already in wait set"); - } - guard_conditions_.push_back(std::move(guard_condition)); - this->storage_flag_for_resize(); - } - - void - storage_remove_guard_condition(std::shared_ptr && guard_condition) - { - auto it = this->storage_find_entity(*guard_condition, guard_conditions_); - if (guard_conditions_.cend() == it) { - throw std::runtime_error("guard_condition not in wait set"); - } - guard_conditions_.erase(it); - this->storage_flag_for_resize(); - } - - void - storage_add_timer(std::shared_ptr && timer) - { - if (this->storage_has_entity(*timer, timers_)) { - throw std::runtime_error("timer already in wait set"); - } - timers_.push_back(std::move(timer)); - this->storage_flag_for_resize(); - } - - void - storage_remove_timer(std::shared_ptr && timer) - { - auto it = this->storage_find_entity(*timer, timers_); - if (timers_.cend() == it) { - throw std::runtime_error("timer not in wait set"); - } - timers_.erase(it); - this->storage_flag_for_resize(); - } - - void - storage_add_client(std::shared_ptr && client) - { - if (this->storage_has_entity(*client, clients_)) { - throw std::runtime_error("client already in wait set"); - } - clients_.push_back(std::move(client)); - this->storage_flag_for_resize(); - } - - void - storage_remove_client(std::shared_ptr && client) - { - auto it = this->storage_find_entity(*client, clients_); - if (clients_.cend() == it) { - throw std::runtime_error("client not in wait set"); - } - clients_.erase(it); - this->storage_flag_for_resize(); - } - - void - storage_add_service(std::shared_ptr && service) - { - if (this->storage_has_entity(*service, services_)) { - throw std::runtime_error("service already in wait set"); - } - services_.push_back(std::move(service)); - this->storage_flag_for_resize(); - } - - void - storage_remove_service(std::shared_ptr && service) - { - auto it = this->storage_find_entity(*service, services_); - if (services_.cend() == it) { - throw std::runtime_error("service not in wait set"); - } - services_.erase(it); - this->storage_flag_for_resize(); - } - - void - storage_add_waitable( - std::shared_ptr && waitable, - std::shared_ptr && associated_entity) - { - if (this->storage_has_entity(*waitable, waitables_)) { - throw std::runtime_error("waitable already in wait set"); - } - WeakWaitableEntry weak_entry(std::move(waitable), std::move(associated_entity)); - waitables_.push_back(std::move(weak_entry)); - this->storage_flag_for_resize(); - } - - void - storage_remove_waitable(std::shared_ptr && waitable) - { - auto it = this->storage_find_entity(*waitable, waitables_); - if (waitables_.cend() == it) { - throw std::runtime_error("waitable not in wait set"); - } - waitables_.erase(it); - this->storage_flag_for_resize(); - } - - // this is noexcept because: - // - std::weak_ptr::expired is noexcept - // - the erase-remove idiom is noexcept, since we're not using the ExecutionPolicy version - // - std::vector::erase is noexcept if the assignment operator of T is also - // - and, the operator= for std::weak_ptr is noexcept - void - storage_prune_deleted_entities() noexcept - { - // reusable (templated) lambda for removal predicate - auto p = - [](const auto & weak_ptr) { - // remove entries which have expired - return weak_ptr.expired(); - }; - // remove guard conditions which have been deleted - guard_conditions_.erase(std::remove_if(guard_conditions_.begin(), guard_conditions_.end(), p)); - timers_.erase(std::remove_if(timers_.begin(), timers_.end(), p)); - clients_.erase(std::remove_if(clients_.begin(), clients_.end(), p)); - services_.erase(std::remove_if(services_.begin(), services_.end(), p)); - waitables_.erase(std::remove_if(waitables_.begin(), waitables_.end(), p)); - } - - void - storage_acquire_ownerships() - { - if (++ownership_reference_counter_ > 1) { - // Avoid redundant locking. - return; - } - // Setup common locking function. - auto lock_all = [](const auto & weak_ptrs, auto & shared_ptrs) { - shared_ptrs.resize(weak_ptrs.size()); - size_t index = 0; - for (const auto & weak_ptr : weak_ptrs) { - shared_ptrs[index++] = weak_ptr.lock(); - } - }; - // Lock all the weak pointers and hold them until released. - lock_all(guard_conditions_, shared_guard_conditions_); - lock_all(timers_, shared_timers_); - lock_all(clients_, shared_clients_); - lock_all(services_, shared_services_); - - // We need a specialized version of this for waitables. - auto lock_all_waitables = [](const auto & weak_ptrs, auto & shared_ptrs) { - shared_ptrs.resize(weak_ptrs.size()); - size_t index = 0; - for (const auto & weak_ptr : weak_ptrs) { - shared_ptrs[index++] = WaitableEntry{ - weak_ptr.waitable.lock(), - weak_ptr.associated_entity.lock()}; - } - }; - lock_all_waitables(waitables_, shared_waitables_); - } - - void - storage_release_ownerships() - { - if (--ownership_reference_counter_ > 0) { - // Avoid releasing ownership until reference count is 0. - return; - } - // "Unlock" all shared pointers by resetting them. - auto reset_all = [](auto & shared_ptrs) { - for (auto & shared_ptr : shared_ptrs) { - shared_ptr.reset(); - } - }; - reset_all(shared_guard_conditions_); - reset_all(shared_timers_); - reset_all(shared_clients_); - reset_all(shared_services_); - reset_all(shared_waitables_); - } - - size_t ownership_reference_counter_ = 0; - - SequenceOfWeakSubscriptions subscriptions_; - SubscriptionsIterable shared_subscriptions_; - - SequenceOfWeakGuardConditions guard_conditions_; - GuardConditionsIterable shared_guard_conditions_; - - SequenceOfWeakTimers timers_; - TimersIterable shared_timers_; - - SequenceOfWeakClients clients_; - ClientsIterable shared_clients_; - - SequenceOfWeakServices services_; - ServicesIterable shared_services_; - - SequenceOfWeakWaitables waitables_; - WaitablesIterable shared_waitables_; -}; - -} // namespace wait_set_policies -} // namespace rclcpp - -#endif // RCLCPP__WAIT_SET_POLICIES__DYNAMIC_STORAGE_HPP_ diff --git a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp deleted file mode 100644 index 5ebf32bb72..0000000000 --- a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp +++ /dev/null @@ -1,317 +0,0 @@ -// Copyright 2020 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__WAIT_SET_POLICIES__SEQUENTIAL_SYNCHRONIZATION_HPP_ -#define RCLCPP__WAIT_SET_POLICIES__SEQUENTIAL_SYNCHRONIZATION_HPP_ - -#include -#include -#include -#include - -#include "rclcpp/client.hpp" -#include "rclcpp/exceptions.hpp" -#include "rclcpp/guard_condition.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/service.hpp" -#include "rclcpp/subscription_base.hpp" -#include "rclcpp/subscription_wait_set_mask.hpp" -#include "rclcpp/timer.hpp" -#include "rclcpp/visibility_control.hpp" -#include "rclcpp/wait_result.hpp" -#include "rclcpp/wait_result_kind.hpp" -#include "rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp" -#include "rclcpp/waitable.hpp" - -namespace rclcpp -{ -namespace wait_set_policies -{ - -/// WaitSet policy that explicitly provides no thread synchronization. -class SequentialSynchronization : public detail::SynchronizationPolicyCommon -{ -protected: - explicit SequentialSynchronization(rclcpp::Context::SharedPtr) {} - ~SequentialSynchronization() = default; - - /// Return any "extra" guard conditions needed to implement the synchronization policy. - /** - * Since this policy provides no thread-safety, it also needs no extra guard - * conditions to implement it. - */ - const std::array, 0> & - get_extra_guard_conditions() - { - static const std::array, 0> empty{}; - return empty; - } - - /// Add subscription without thread-safety. - /** - * Does not throw, but storage function may throw. - */ - void - sync_add_subscription( - std::shared_ptr && subscription, - const rclcpp::SubscriptionWaitSetMask & mask, - std::function< - void(std::shared_ptr&&, const rclcpp::SubscriptionWaitSetMask &) - > add_subscription_function) - { - // Explicitly no thread synchronization. - add_subscription_function(std::move(subscription), mask); - } - - /// Remove guard condition without thread-safety. - /** - * Does not throw, but storage function may throw. - */ - void - sync_remove_subscription( - std::shared_ptr && subscription, - const rclcpp::SubscriptionWaitSetMask & mask, - std::function< - void(std::shared_ptr&&, const rclcpp::SubscriptionWaitSetMask &) - > remove_subscription_function) - { - // Explicitly no thread synchronization. - remove_subscription_function(std::move(subscription), mask); - } - - /// Add guard condition without thread-safety. - /** - * Does not throw, but storage function may throw. - */ - void - sync_add_guard_condition( - std::shared_ptr && guard_condition, - std::function&&)> add_guard_condition_function) - { - // Explicitly no thread synchronization. - add_guard_condition_function(std::move(guard_condition)); - } - - /// Remove guard condition without thread-safety. - /** - * Does not throw, but storage function may throw. - */ - void - sync_remove_guard_condition( - std::shared_ptr && guard_condition, - std::function&&)> remove_guard_condition_function) - { - // Explicitly no thread synchronization. - remove_guard_condition_function(std::move(guard_condition)); - } - - /// Add timer without thread-safety. - /** - * Does not throw, but storage function may throw. - */ - void - sync_add_timer( - std::shared_ptr && timer, - std::function&&)> add_timer_function) - { - // Explicitly no thread synchronization. - add_timer_function(std::move(timer)); - } - - /// Remove timer without thread-safety. - /** - * Does not throw, but storage function may throw. - */ - void - sync_remove_timer( - std::shared_ptr && timer, - std::function&&)> remove_timer_function) - { - // Explicitly no thread synchronization. - remove_timer_function(std::move(timer)); - } - - /// Add client without thread-safety. - /** - * Does not throw, but storage function may throw. - */ - void - sync_add_client( - std::shared_ptr && client, - std::function&&)> add_client_function) - { - // Explicitly no thread synchronization. - add_client_function(std::move(client)); - } - - /// Remove client without thread-safety. - /** - * Does not throw, but storage function may throw. - */ - void - sync_remove_client( - std::shared_ptr && client, - std::function&&)> remove_client_function) - { - // Explicitly no thread synchronization. - remove_client_function(std::move(client)); - } - - /// Add service without thread-safety. - /** - * Does not throw, but storage function may throw. - */ - void - sync_add_service( - std::shared_ptr && service, - std::function&&)> add_service_function) - { - // Explicitly no thread synchronization. - add_service_function(std::move(service)); - } - - /// Remove service without thread-safety. - /** - * Does not throw, but storage function may throw. - */ - void - sync_remove_service( - std::shared_ptr && service, - std::function&&)> remove_service_function) - { - // Explicitly no thread synchronization. - remove_service_function(std::move(service)); - } - - /// Add waitable without thread-safety. - /** - * Does not throw, but storage function may throw. - */ - void - sync_add_waitable( - std::shared_ptr && waitable, - std::shared_ptr && associated_entity, - std::function< - void(std::shared_ptr&&, std::shared_ptr&&) - > add_waitable_function) - { - // Explicitly no thread synchronization. - add_waitable_function(std::move(waitable), std::move(associated_entity)); - } - - /// Remove waitable without thread-safety. - /** - * Does not throw, but storage function may throw. - */ - void - sync_remove_waitable( - std::shared_ptr && waitable, - std::function&&)> remove_waitable_function) - { - // Explicitly no thread synchronization. - remove_waitable_function(std::move(waitable)); - } - - /// Prune deleted entities without thread-safety. - /** - * Does not throw, but storage function may throw. - */ - void - sync_prune_deleted_entities(std::function prune_deleted_entities_function) - { - // Explicitly no thread synchronization. - prune_deleted_entities_function(); - } - - /// Implements wait without any thread-safety. - template - WaitResultT - sync_wait( - std::chrono::nanoseconds time_to_wait_ns, - std::function rebuild_rcl_wait_set, - std::function get_rcl_wait_set, - std::function create_wait_result) - { - // Assumption: this function assumes that some measure has been taken to - // ensure none of the entities being waited on by the wait set are allowed - // to go out of scope and therefore be deleted. - // In the case of the StaticStorage policy, this is ensured because it - // retains shared ownership of all entites for the duration of its own life. - // In the case of the DynamicStorage policy, this is ensured by the function - // which calls this function, by acquiring shared ownership of the entites - // for the duration of this function. - - // Setup looping predicate. - auto start = std::chrono::steady_clock::now(); - std::function should_loop = this->create_loop_predicate(time_to_wait_ns, start); - - // Wait until exit condition is met. - do { - // Rebuild the wait set. - // This will resize the wait set if needed, due to e.g. adding or removing - // entities since the last wait, but this should never occur in static - // storage wait sets since they cannot be changed after construction. - // This will also clear the wait set and re-add all the entities, which - // prepares it to be waited on again. - rebuild_rcl_wait_set(); - - rcl_wait_set_t & rcl_wait_set = get_rcl_wait_set(); - - // Wait unconditionally until timeout condition occurs since we assume - // there are no conditions that would require the wait to stop and reset, - // like asynchronously adding or removing an entity, i.e. explicitly - // providing no thread-safety. - - // Calculate how much time there is left to wait, unless blocking indefinitely. - auto time_left_to_wait_ns = this->calculate_time_left_to_wait(time_to_wait_ns, start); - - // Then wait for entities to become ready. - rcl_ret_t ret = rcl_wait(&rcl_wait_set, time_left_to_wait_ns.count()); - if (RCL_RET_OK == ret) { - // Something has become ready in the wait set, and since this class - // did not add anything to it, it is a user entity that is ready. - return create_wait_result(WaitResultKind::Ready); - } else if (RCL_RET_TIMEOUT == ret) { - // The wait set timed out, exit the loop. - break; - } else if (RCL_RET_WAIT_SET_EMPTY == ret) { - // Wait set was empty, return Empty. - return create_wait_result(WaitResultKind::Empty); - } else { - // Some other error case, throw. - rclcpp::exceptions::throw_from_rcl_error(ret); - } - } while (should_loop()); - - // Wait did not result in ready items, return timeout. - return create_wait_result(WaitResultKind::Timeout); - } - - void - sync_wait_result_acquire() - { - // Explicitly do nothing. - } - - void - sync_wait_result_release() - { - // Explicitly do nothing. - } -}; - -} // namespace wait_set_policies -} // namespace rclcpp - -#endif // RCLCPP__WAIT_SET_POLICIES__SEQUENTIAL_SYNCHRONIZATION_HPP_ diff --git a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp deleted file mode 100644 index d4d6c0c014..0000000000 --- a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp +++ /dev/null @@ -1,201 +0,0 @@ -// Copyright 2020 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__WAIT_SET_POLICIES__STATIC_STORAGE_HPP_ -#define RCLCPP__WAIT_SET_POLICIES__STATIC_STORAGE_HPP_ - -#include -#include - -#include "rclcpp/client.hpp" -#include "rclcpp/guard_condition.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/service.hpp" -#include "rclcpp/subscription_base.hpp" -#include "rclcpp/subscription_wait_set_mask.hpp" -#include "rclcpp/timer.hpp" -#include "rclcpp/visibility_control.hpp" -#include "rclcpp/wait_set_policies/detail/storage_policy_common.hpp" -#include "rclcpp/waitable.hpp" - -namespace rclcpp -{ -namespace wait_set_policies -{ - -/// WaitSet policy that explicitly provides fixed sized storage only. -/** - * Note the underlying rcl_wait_set_t is still dynamically allocated, but only - * once during construction, and deallocated once during destruction. - */ -template< - std::size_t NumberOfSubscriptions, - std::size_t NumberOfGuardCondtions, - std::size_t NumberOfTimers, - std::size_t NumberOfClients, - std::size_t NumberOfServices, - std::size_t NumberOfWaitables -> -class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCommon -{ -protected: - using is_mutable = std::false_type; - - class SubscriptionEntry - { -public: - std::shared_ptr subscription; - rclcpp::SubscriptionWaitSetMask mask; - - /// Conversion constructor, which is intentionally not marked explicit. - SubscriptionEntry( - const std::shared_ptr & subscription_in = nullptr, - const rclcpp::SubscriptionWaitSetMask & mask_in = {}) - : subscription(subscription_in), - mask(mask_in) - {} - }; - using ArrayOfSubscriptions = std::array< - SubscriptionEntry, - NumberOfSubscriptions - >; - using SubscriptionsIterable = ArrayOfSubscriptions; - - using ArrayOfGuardConditions = std::array< - std::shared_ptr, - NumberOfGuardCondtions - >; - using GuardConditionsIterable = ArrayOfGuardConditions; - - using ArrayOfTimers = std::array< - std::shared_ptr, - NumberOfTimers - >; - using TimersIterable = ArrayOfTimers; - - using ArrayOfClients = std::array< - std::shared_ptr, - NumberOfClients - >; - using ClientsIterable = ArrayOfClients; - - using ArrayOfServices = std::array< - std::shared_ptr, - NumberOfServices - >; - using ServicesIterable = ArrayOfServices; - - struct WaitableEntry - { - /// Conversion constructor, which is intentionally not marked explicit. - WaitableEntry( - const std::shared_ptr & waitable_in = nullptr, - const std::shared_ptr & associated_entity_in = nullptr) noexcept - : waitable(waitable_in), - associated_entity(associated_entity_in) - {} - - std::shared_ptr waitable; - std::shared_ptr associated_entity; - }; - using ArrayOfWaitables = std::array< - WaitableEntry, - NumberOfWaitables - >; - using WaitablesIterable = ArrayOfWaitables; - - template - explicit - StaticStorage( - const ArrayOfSubscriptions & subscriptions, - const ArrayOfGuardConditions & guard_conditions, - const ArrayOfExtraGuardConditions & extra_guard_conditions, - const ArrayOfTimers & timers, - const ArrayOfClients & clients, - const ArrayOfServices & services, - const ArrayOfWaitables & waitables, - rclcpp::Context::SharedPtr context - ) - : StoragePolicyCommon( - subscriptions, - guard_conditions, - extra_guard_conditions, - timers, - clients, - services, - waitables, - context), - subscriptions_(subscriptions), - guard_conditions_(guard_conditions), - timers_(timers), - clients_(clients), - services_(services), - waitables_(waitables) - {} - - ~StaticStorage() = default; - - template - void - storage_rebuild_rcl_wait_set(const ArrayOfExtraGuardConditions & extra_guard_conditions) - { - this->storage_rebuild_rcl_wait_set_with_sets( - subscriptions_, - guard_conditions_, - extra_guard_conditions, - timers_, - clients_, - services_, - waitables_ - ); - } - - // storage_add_subscription() explicitly not declared here - // storage_remove_subscription() explicitly not declared here - // storage_add_guard_condition() explicitly not declared here - // storage_remove_guard_condition() explicitly not declared here - // storage_add_timer() explicitly not declared here - // storage_remove_timer() explicitly not declared here - // storage_add_client() explicitly not declared here - // storage_remove_client() explicitly not declared here - // storage_add_service() explicitly not declared here - // storage_remove_service() explicitly not declared here - // storage_add_waitable() explicitly not declared here - // storage_remove_waitable() explicitly not declared here - // storage_prune_deleted_entities() explicitly not declared here - - void - storage_acquire_ownerships() - { - // Explicitly do nothing. - } - - void - storage_release_ownerships() - { - // Explicitly do nothing. - } - - const ArrayOfSubscriptions subscriptions_; - const ArrayOfGuardConditions guard_conditions_; - const ArrayOfTimers timers_; - const ArrayOfClients clients_; - const ArrayOfServices services_; - const ArrayOfWaitables waitables_; -}; - -} // namespace wait_set_policies -} // namespace rclcpp - -#endif // RCLCPP__WAIT_SET_POLICIES__STATIC_STORAGE_HPP_ diff --git a/rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp b/rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp deleted file mode 100644 index ea74769ad4..0000000000 --- a/rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp +++ /dev/null @@ -1,379 +0,0 @@ -// Copyright 2020 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__WAIT_SET_POLICIES__THREAD_SAFE_SYNCHRONIZATION_HPP_ -#define RCLCPP__WAIT_SET_POLICIES__THREAD_SAFE_SYNCHRONIZATION_HPP_ - -#include -#include -#include -#include - -#include "rclcpp/client.hpp" -#include "rclcpp/exceptions.hpp" -#include "rclcpp/guard_condition.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/service.hpp" -#include "rclcpp/subscription_base.hpp" -#include "rclcpp/subscription_wait_set_mask.hpp" -#include "rclcpp/timer.hpp" -#include "rclcpp/visibility_control.hpp" -#include "rclcpp/wait_result.hpp" -#include "rclcpp/wait_result_kind.hpp" -#include "rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp" -#include "rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp" -#include "rclcpp/waitable.hpp" - -namespace rclcpp -{ -namespace wait_set_policies -{ - -/// WaitSet policy that provides thread-safe synchronization for the wait set. -/** - * This class uses a "write-preferring RW lock" so that adding items to, and - * removing items from, the wait set will take priority over reading, i.e. - * waiting. - * This is done since add and remove calls will interrupt the wait set anyways - * so it is wasteful to do "fair" locking when there are many add/remove - * operations queued up. - * - * There are some things to consider about the thread-safety provided by this - * policy. - * There are two categories of activities, reading and writing activities. - * The writing activities include all of the add and remove methods, as well as - * the prune_deleted_entities() method. - * The reading methods include the wait() method and keeping a WaitResult in - * scope. - * The reading and writing activities will not be run at the same time, and one - * will block the other. - * Therefore, if you are holding a WaitResult in scope, and try to add or - * remove an entity at the same time, they will block each other. - * The write activities will try to interrupt the wait() method by triggering - * a guard condition, but they have no way of causing the WaitResult to release - * its lock. - */ -class ThreadSafeSynchronization : public detail::SynchronizationPolicyCommon -{ -protected: - explicit ThreadSafeSynchronization(rclcpp::Context::SharedPtr context) - : extra_guard_conditions_{{std::make_shared(context)}}, - wprw_lock_([this]() {this->interrupt_waiting_wait_set();}) - {} - ~ThreadSafeSynchronization() = default; - - /// Return any "extra" guard conditions needed to implement the synchronization policy. - /** - * This policy has one guard condition which is used to interrupt the wait - * set when adding and removing entities. - */ - const std::array, 1> & - get_extra_guard_conditions() - { - return extra_guard_conditions_; - } - - /// Interrupt any waiting wait set. - /** - * Used to interrupt the wait set when adding or removing items. - */ - void - interrupt_waiting_wait_set() - { - extra_guard_conditions_[0]->trigger(); - } - - /// Add subscription. - void - sync_add_subscription( - std::shared_ptr && subscription, - const rclcpp::SubscriptionWaitSetMask & mask, - std::function< - void(std::shared_ptr&&, const rclcpp::SubscriptionWaitSetMask &) - > add_subscription_function) - { - using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; - std::lock_guard lock(wprw_lock_.get_write_mutex()); - add_subscription_function(std::move(subscription), mask); - } - - /// Remove guard condition. - void - sync_remove_subscription( - std::shared_ptr && subscription, - const rclcpp::SubscriptionWaitSetMask & mask, - std::function< - void(std::shared_ptr&&, const rclcpp::SubscriptionWaitSetMask &) - > remove_subscription_function) - { - using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; - std::lock_guard lock(wprw_lock_.get_write_mutex()); - remove_subscription_function(std::move(subscription), mask); - } - - /// Add guard condition. - void - sync_add_guard_condition( - std::shared_ptr && guard_condition, - std::function&&)> add_guard_condition_function) - { - using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; - std::lock_guard lock(wprw_lock_.get_write_mutex()); - add_guard_condition_function(std::move(guard_condition)); - } - - /// Remove guard condition. - void - sync_remove_guard_condition( - std::shared_ptr && guard_condition, - std::function&&)> remove_guard_condition_function) - { - using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; - std::lock_guard lock(wprw_lock_.get_write_mutex()); - remove_guard_condition_function(std::move(guard_condition)); - } - - /// Add timer. - void - sync_add_timer( - std::shared_ptr && timer, - std::function&&)> add_timer_function) - { - using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; - std::lock_guard lock(wprw_lock_.get_write_mutex()); - add_timer_function(std::move(timer)); - } - - /// Remove timer. - void - sync_remove_timer( - std::shared_ptr && timer, - std::function&&)> remove_timer_function) - { - using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; - std::lock_guard lock(wprw_lock_.get_write_mutex()); - remove_timer_function(std::move(timer)); - } - - /// Add client. - void - sync_add_client( - std::shared_ptr && client, - std::function&&)> add_client_function) - { - using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; - std::lock_guard lock(wprw_lock_.get_write_mutex()); - add_client_function(std::move(client)); - } - - /// Remove client. - void - sync_remove_client( - std::shared_ptr && client, - std::function&&)> remove_client_function) - { - using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; - std::lock_guard lock(wprw_lock_.get_write_mutex()); - remove_client_function(std::move(client)); - } - - /// Add service. - void - sync_add_service( - std::shared_ptr && service, - std::function&&)> add_service_function) - { - using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; - std::lock_guard lock(wprw_lock_.get_write_mutex()); - add_service_function(std::move(service)); - } - - /// Remove service. - void - sync_remove_service( - std::shared_ptr && service, - std::function&&)> remove_service_function) - { - using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; - std::lock_guard lock(wprw_lock_.get_write_mutex()); - remove_service_function(std::move(service)); - } - - /// Add waitable. - void - sync_add_waitable( - std::shared_ptr && waitable, - std::shared_ptr && associated_entity, - std::function< - void(std::shared_ptr&&, std::shared_ptr&&) - > add_waitable_function) - { - using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; - std::lock_guard lock(wprw_lock_.get_write_mutex()); - add_waitable_function(std::move(waitable), std::move(associated_entity)); - } - - /// Remove waitable. - void - sync_remove_waitable( - std::shared_ptr && waitable, - std::function&&)> remove_waitable_function) - { - using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; - std::lock_guard lock(wprw_lock_.get_write_mutex()); - remove_waitable_function(std::move(waitable)); - } - - /// Prune deleted entities. - void - sync_prune_deleted_entities(std::function prune_deleted_entities_function) - { - using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; - std::lock_guard lock(wprw_lock_.get_write_mutex()); - prune_deleted_entities_function(); - } - - /// Implements wait. - template - WaitResultT - sync_wait( - std::chrono::nanoseconds time_to_wait_ns, - std::function rebuild_rcl_wait_set, - std::function get_rcl_wait_set, - std::function create_wait_result) - { - // Assumption: this function assumes that some measure has been taken to - // ensure none of the entities being waited on by the wait set are allowed - // to go out of scope and therefore be deleted. - // In the case of the StaticStorage policy, this is ensured because it - // retains shared ownership of all entites for the duration of its own life. - // In the case of the DynamicStorage policy, this is ensured by the function - // which calls this function, by acquiring shared ownership of the entites - // for the duration of this function. - - // Setup looping predicate. - auto start = std::chrono::steady_clock::now(); - std::function should_loop = this->create_loop_predicate(time_to_wait_ns, start); - - // Wait until exit condition is met. - do { - { - // We have to prevent the entity sets from being mutated while building - // the rcl wait set. - using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; - std::lock_guard lock(wprw_lock_.get_read_mutex()); - // Rebuild the wait set. - // This will resize the wait set if needed, due to e.g. adding or removing - // entities since the last wait, but this should never occur in static - // storage wait sets since they cannot be changed after construction. - // This will also clear the wait set and re-add all the entities, which - // prepares it to be waited on again. - rebuild_rcl_wait_set(); - } - - rcl_wait_set_t & rcl_wait_set = get_rcl_wait_set(); - - // Wait unconditionally until timeout condition occurs since we assume - // there are no conditions that would require the wait to stop and reset, - // like asynchronously adding or removing an entity, i.e. explicitly - // providing no thread-safety. - - // Calculate how much time there is left to wait, unless blocking indefinitely. - auto time_left_to_wait_ns = this->calculate_time_left_to_wait(time_to_wait_ns, start); - - // Then wait for entities to become ready. - - // It is ok to wait while not having the lock acquired, because the state - // in the rcl wait set will not be updated until this method calls - // rebuild_rcl_wait_set(). - rcl_ret_t ret = rcl_wait(&rcl_wait_set, time_left_to_wait_ns.count()); - if (RCL_RET_OK == ret) { - // Something has become ready in the wait set, first check if it was - // the guard condition added by this class and/or a user defined guard condition. - const rcl_guard_condition_t * interrupt_guard_condition_ptr = - &(extra_guard_conditions_[0]->get_rcl_guard_condition()); - bool was_interrupted_by_this_class = false; - bool any_user_guard_conditions_triggered = false; - for (size_t index = 0; index < rcl_wait_set.size_of_guard_conditions; ++index) { - const rcl_guard_condition_t * current = rcl_wait_set.guard_conditions[index]; - if (nullptr != current) { - // Something is ready. - if (rcl_wait_set.guard_conditions[index] == interrupt_guard_condition_ptr) { - // This means that this class triggered a guard condition to interrupt this wait. - was_interrupted_by_this_class = true; - } else { - // This means it was a user guard condition. - any_user_guard_conditions_triggered = true; - } - } - } - - if (!was_interrupted_by_this_class || any_user_guard_conditions_triggered) { - // In this case we know: - // - something was ready - // - it was either: - // - not interrupted by this class, or - // - maybe it was, but there were also user defined guard conditions. - // - // We cannot ignore user defined guard conditions, but we can ignore - // other kinds of user defined entities, because they will still be - // ready next time we wait, whereas guard conditions are cleared. - // Therefore we need to create a WaitResult and return it. - - // The WaitResult will call sync_wait_result_acquire() and - // sync_wait_result_release() to ensure thread-safety by preventing - // the mutation of the entity sets while introspecting after waiting. - return create_wait_result(WaitResultKind::Ready); - } - // If we get here the we interrupted the wait set and there were no user - // guard conditions that needed to be handled. - // So we will loop and it will re-acquire the lock and rebuild the - // rcl wait set. - } else if (RCL_RET_TIMEOUT == ret) { - // The wait set timed out, exit the loop. - break; - } else if (RCL_RET_WAIT_SET_EMPTY == ret) { - // Wait set was empty, return Empty. - return create_wait_result(WaitResultKind::Empty); - } else { - // Some other error case, throw. - rclcpp::exceptions::throw_from_rcl_error(ret); - } - } while (should_loop()); - - // Wait did not result in ready items, return timeout. - return create_wait_result(WaitResultKind::Timeout); - } - - void - sync_wait_result_acquire() - { - wprw_lock_.get_read_mutex().lock(); - } - - void - sync_wait_result_release() - { - wprw_lock_.get_read_mutex().unlock(); - } - -protected: - std::array, 1> extra_guard_conditions_; - rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock wprw_lock_; -}; - -} // namespace wait_set_policies -} // namespace rclcpp - -#endif // RCLCPP__WAIT_SET_POLICIES__THREAD_SAFE_SYNCHRONIZATION_HPP_ diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp deleted file mode 100644 index 899d461ed3..0000000000 --- a/rclcpp/include/rclcpp/wait_set_template.hpp +++ /dev/null @@ -1,743 +0,0 @@ -// Copyright 2020 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__WAIT_SET_TEMPLATE_HPP_ -#define RCLCPP__WAIT_SET_TEMPLATE_HPP_ - -#include -#include -#include - -#include "rcl/wait.h" - -#include "rclcpp/client.hpp" -#include "rclcpp/context.hpp" -#include "rclcpp/contexts/default_context.hpp" -#include "rclcpp/guard_condition.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/scope_exit.hpp" -#include "rclcpp/service.hpp" -#include "rclcpp/subscription_base.hpp" -#include "rclcpp/subscription_wait_set_mask.hpp" -#include "rclcpp/timer.hpp" -#include "rclcpp/visibility_control.hpp" -#include "rclcpp/wait_result.hpp" -#include "rclcpp/waitable.hpp" - -namespace rclcpp -{ - -/// Encapsulates sets of waitable items which can be waited on as a group. -/** - * This class uses the rcl_wait_set_t as storage, but it also helps manage the - * ownership of associated rclcpp types. - */ -template -class WaitSetTemplate final : private SynchronizationPolicy, private StoragePolicy -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(WaitSetTemplate) - - using typename StoragePolicy::SubscriptionEntry; - using typename StoragePolicy::WaitableEntry; - - /// Construct a wait set with optional initial waitable entities and optional custom context. - /** - * For the waitables, they have additionally an "associated" entity, which - * you can read more about in the add and remove functions for those types - * in this class. - * - * \param[in] subscriptions Vector of subscriptions to be added. - * \param[in] guard_conditions Vector of guard conditions to be added. - * \param[in] timers Vector of timers to be added. - * \param[in] waitables Vector of waitables and their associated entity to be added. - * \param[in] context Custom context to be used, defaults to global default. - * \throws std::invalid_argument If context is nullptr. - */ - explicit - WaitSetTemplate( - const typename StoragePolicy::SubscriptionsIterable & subscriptions = {}, - const typename StoragePolicy::GuardConditionsIterable & guard_conditions = {}, - const typename StoragePolicy::TimersIterable & timers = {}, - const typename StoragePolicy::ClientsIterable & clients = {}, - const typename StoragePolicy::ServicesIterable & services = {}, - const typename StoragePolicy::WaitablesIterable & waitables = {}, - rclcpp::Context::SharedPtr context = - rclcpp::contexts::default_context::get_global_default_context()) - : SynchronizationPolicy(context), - StoragePolicy( - subscriptions, - guard_conditions, - // this method comes from the SynchronizationPolicy - this->get_extra_guard_conditions(), - timers, - clients, - services, - waitables, - context) - {} - - /// Return the internal rcl wait set object. - /** - * This method provides no thread-safety when accessing this structure. - * The state of this structure can be updated at anytime by methods like - * wait(), add_*(), remove_*(), etc. - */ - const rcl_wait_set_t & - get_rcl_wait_set() const - { - // this method comes from the StoragePolicy - return this->storage_get_rcl_wait_set(); - } - - /// Add a subscription to this wait set. - /** - * \sa add_guard_condition() for details of how this method works. - * - * Additionally to the documentation for add_guard_condition, this method - * has a mask parameter which allows you to control which parts of the - * subscription is added to the wait set with this call. - * For example, you might want to include the actual subscription to this - * wait set, but add the intra-process waitable to another wait set. - * If intra-process is disabled, no error will occur, it will just be skipped. - * - * When introspecting after waiting, this subscription's shared pointer will - * be the Waitable's (intra-process or the QoS Events) "associated entity" - * pointer, for more easily figuring out which subscription which waitable - * goes with afterwards. - * - * \param[in] subscription Subscription to be added. - * \param[in] mask A class which controls which parts of the subscription to add. - * \throws std::invalid_argument if subscription is nullptr. - * \throws std::runtime_error if subscription has already been added or is - * associated with another wait set. - * \throws exceptions based on the policies used. - */ - void - add_subscription( - std::shared_ptr subscription, - rclcpp::SubscriptionWaitSetMask mask = {}) - { - if (nullptr == subscription) { - throw std::invalid_argument("subscription is nullptr"); - } - // this method comes from the SynchronizationPolicy - this->sync_add_subscription( - std::move(subscription), - mask, - [this]( - std::shared_ptr && inner_subscription, - const rclcpp::SubscriptionWaitSetMask & mask) - { - // These methods comes from the StoragePolicy, and may not exist for - // fixed sized storage policies. - // It will throw if the subscription has already been added. - if (mask.include_subscription) { - auto local_subscription = inner_subscription; - bool already_in_use = - local_subscription->exchange_in_use_by_wait_set_state(local_subscription.get(), true); - if (already_in_use) { - throw std::runtime_error("subscription already associated with a wait set"); - } - this->storage_add_subscription(std::move(local_subscription)); - } - if (mask.include_events) { - for (auto event : inner_subscription->get_event_handlers()) { - auto local_subscription = inner_subscription; - bool already_in_use = - local_subscription->exchange_in_use_by_wait_set_state(event.get(), true); - if (already_in_use) { - throw std::runtime_error("subscription event already associated with a wait set"); - } - this->storage_add_waitable(std::move(event), std::move(local_subscription)); - } - } - if (mask.include_intra_process_waitable) { - auto local_subscription = inner_subscription; - auto waitable = inner_subscription->get_intra_process_waitable(); - if (nullptr != waitable) { - bool already_in_use = local_subscription->exchange_in_use_by_wait_set_state( - waitable.get(), - true); - if (already_in_use) { - throw std::runtime_error( - "subscription intra-process waitable already associated with a wait set"); - } - this->storage_add_waitable( - std::move(inner_subscription->get_intra_process_waitable()), - std::move(local_subscription)); - } - } - }); - } - - /// Remove a subscription from this wait set. - /** - * \sa remove_guard_condition() for details of how this method works. - * - * Additionally to the documentation for add_guard_condition, this method - * has a mask parameter which allows you to control which parts of the - * subscription is added to the wait set with this call. - * You may remove items selectively from the wait set in a different order - * than they were added. - * - * \param[in] subscription Subscription to be removed. - * \param[in] mask A class which controls which parts of the subscription to remove. - * \throws std::invalid_argument if subscription is nullptr. - * \throws std::runtime_error if subscription is not part of the wait set. - * \throws exceptions based on the policies used. - */ - void - remove_subscription( - std::shared_ptr subscription, - rclcpp::SubscriptionWaitSetMask mask = {}) - { - if (nullptr == subscription) { - throw std::invalid_argument("subscription is nullptr"); - } - // this method comes from the SynchronizationPolicy - this->sync_remove_subscription( - std::move(subscription), - mask, - [this]( - std::shared_ptr && inner_subscription, - const rclcpp::SubscriptionWaitSetMask & mask) - { - // This method comes from the StoragePolicy, and it may not exist for - // fixed sized storage policies. - // It will throw if the subscription is not in the wait set. - if (mask.include_subscription) { - auto local_subscription = inner_subscription; - local_subscription->exchange_in_use_by_wait_set_state(local_subscription.get(), false); - this->storage_remove_subscription(std::move(local_subscription)); - } - if (mask.include_events) { - for (auto event : inner_subscription->get_event_handlers()) { - auto local_subscription = inner_subscription; - local_subscription->exchange_in_use_by_wait_set_state(event.get(), false); - this->storage_remove_waitable(std::move(event)); - } - } - if (mask.include_intra_process_waitable) { - auto local_waitable = inner_subscription->get_intra_process_waitable(); - inner_subscription->exchange_in_use_by_wait_set_state(local_waitable.get(), false); - if (nullptr != local_waitable) { - // This is the case when intra process is disabled for the subscription. - this->storage_remove_waitable(std::move(local_waitable)); - } - } - }); - } - - /// Add a guard condition to this wait set. - /** - * Guard condition is added to the wait set, and shared ownership is held - * while waiting. - * However, if between calls to wait() the guard condition's reference count - * goes to zero, it will be implicitly removed on the next call to wait(). - * - * Except in the case of a fixed sized storage, where changes to the wait set - * cannot occur after construction, in which case it holds shared ownership - * at all times until the wait set is destroy, but this method also does not - * exist on a fixed sized wait set. - * - * This function may be thread-safe depending on the SynchronizationPolicy - * used with this class. - * Using the ThreadSafeWaitSetPolicy will ensure that wait() is interrupted - * and returns before this function adds the guard condition. - * Otherwise, it is not safe to call this function concurrently with wait(). - * - * This function will not be enabled (will not be available) if the - * StoragePolicy does not allow editing of the wait set after initialization. - * - * \param[in] guard_condition Guard condition to be added. - * \throws std::invalid_argument if guard_condition is nullptr. - * \throws std::runtime_error if guard_condition has already been added or is - * associated with another wait set. - * \throws exceptions based on the policies used. - */ - void - add_guard_condition(std::shared_ptr guard_condition) - { - if (nullptr == guard_condition) { - throw std::invalid_argument("guard_condition is nullptr"); - } - // this method comes from the SynchronizationPolicy - this->sync_add_guard_condition( - std::move(guard_condition), - [this](std::shared_ptr && inner_guard_condition) { - bool already_in_use = inner_guard_condition->exchange_in_use_by_wait_set_state(true); - if (already_in_use) { - throw std::runtime_error("guard condition already in use by another wait set"); - } - // This method comes from the StoragePolicy, and it may not exist for - // fixed sized storage policies. - // It will throw if the guard condition has already been added. - this->storage_add_guard_condition(std::move(inner_guard_condition)); - }); - } - - /// Remove a guard condition from this wait set. - /** - * Guard condition is removed from the wait set, and if needed the shared - * ownership is released. - * - * This function may be thread-safe depending on the SynchronizationPolicy - * used with this class. - * Using the ThreadSafeWaitSetPolicy will ensure that wait() is interrupted - * and returns before this function removes the guard condition. - * Otherwise, it is not safe to call this function concurrently with wait(). - * - * This function will not be enabled (will not be available) if the - * StoragePolicy does not allow editing of the wait set after initialization. - * - * \param[in] guard_condition Guard condition to be removed. - * \throws std::invalid_argument if guard_condition is nullptr. - * \throws std::runtime_error if guard_condition is not part of the wait set. - * \throws exceptions based on the policies used. - */ - void - remove_guard_condition(std::shared_ptr guard_condition) - { - if (nullptr == guard_condition) { - throw std::invalid_argument("guard_condition is nullptr"); - } - // this method comes from the SynchronizationPolicy - this->sync_remove_guard_condition( - std::move(guard_condition), - [this](std::shared_ptr && inner_guard_condition) { - inner_guard_condition->exchange_in_use_by_wait_set_state(false); - // This method comes from the StoragePolicy, and it may not exist for - // fixed sized storage policies. - // It will throw if the guard condition is not in the wait set. - this->storage_remove_guard_condition(std::move(inner_guard_condition)); - }); - } - - /// Add a timer to this wait set. - /** - * \sa add_guard_condition() for details of how this method works. - * - * \param[in] timer Timer to be added. - * \throws std::invalid_argument if timer is nullptr. - * \throws std::runtime_error if timer has already been added or is - * associated with another wait set. - * \throws exceptions based on the policies used. - */ - void - add_timer(std::shared_ptr timer) - { - if (nullptr == timer) { - throw std::invalid_argument("timer is nullptr"); - } - // this method comes from the SynchronizationPolicy - this->sync_add_timer( - std::move(timer), - [this](std::shared_ptr && inner_timer) { - bool already_in_use = inner_timer->exchange_in_use_by_wait_set_state(true); - if (already_in_use) { - throw std::runtime_error("timer already in use by another wait set"); - } - // This method comes from the StoragePolicy, and it may not exist for - // fixed sized storage policies. - // It will throw if the timer has already been added. - this->storage_add_timer(std::move(inner_timer)); - }); - } - - /// Remove a timer from this wait set. - /** - * \sa remove_guard_condition() for details of how this method works. - * - * \param[in] timer Timer to be removed. - * \throws std::invalid_argument if timer is nullptr. - * \throws std::runtime_error if timer is not part of the wait set. - * \throws exceptions based on the policies used. - */ - void - remove_timer(std::shared_ptr timer) - { - if (nullptr == timer) { - throw std::invalid_argument("timer is nullptr"); - } - // this method comes from the SynchronizationPolicy - this->sync_remove_timer( - std::move(timer), - [this](std::shared_ptr && inner_timer) { - inner_timer->exchange_in_use_by_wait_set_state(false); - // This method comes from the StoragePolicy, and it may not exist for - // fixed sized storage policies. - // It will throw if the timer is not in the wait set. - this->storage_remove_timer(std::move(inner_timer)); - }); - } - - /// Add a client to this wait set. - /** - * \sa add_guard_condition() for details of how this method works. - * - * \param[in] client Client to be added. - * \throws std::invalid_argument if client is nullptr. - * \throws std::runtime_error if client has already been added or is - * associated with another wait set. - * \throws exceptions based on the policies used. - */ - void - add_client(std::shared_ptr client) - { - if (nullptr == client) { - throw std::invalid_argument("client is nullptr"); - } - // this method comes from the SynchronizationPolicy - this->sync_add_client( - std::move(client), - [this](std::shared_ptr && inner_client) { - bool already_in_use = inner_client->exchange_in_use_by_wait_set_state(true); - if (already_in_use) { - throw std::runtime_error("client already in use by another wait set"); - } - // This method comes from the StoragePolicy, and it may not exist for - // fixed sized storage policies. - // It will throw if the client has already been added. - this->storage_add_client(std::move(inner_client)); - }); - } - - /// Remove a client from this wait set. - /** - * \sa remove_guard_condition() for details of how this method works. - * - * \param[in] client Client to be removed. - * \throws std::invalid_argument if client is nullptr. - * \throws std::runtime_error if client is not part of the wait set. - * \throws exceptions based on the policies used. - */ - void - remove_client(std::shared_ptr client) - { - if (nullptr == client) { - throw std::invalid_argument("client is nullptr"); - } - // this method comes from the SynchronizationPolicy - this->sync_remove_client( - std::move(client), - [this](std::shared_ptr && inner_client) { - inner_client->exchange_in_use_by_wait_set_state(false); - // This method comes from the StoragePolicy, and it may not exist for - // fixed sized storage policies. - // It will throw if the client is not in the wait set. - this->storage_remove_client(std::move(inner_client)); - }); - } - - /// Add a service to this wait set. - /** - * \sa add_guard_condition() for details of how this method works. - * - * \param[in] service Service to be added. - * \throws std::invalid_argument if service is nullptr. - * \throws std::runtime_error if service has already been added or is - * associated with another wait set. - * \throws exceptions based on the policies used. - */ - void - add_service(std::shared_ptr service) - { - if (nullptr == service) { - throw std::invalid_argument("service is nullptr"); - } - // this method comes from the SynchronizationPolicy - this->sync_add_service( - std::move(service), - [this](std::shared_ptr && inner_service) { - bool already_in_use = inner_service->exchange_in_use_by_wait_set_state(true); - if (already_in_use) { - throw std::runtime_error("service already in use by another wait set"); - } - // This method comes from the StoragePolicy, and it may not exist for - // fixed sized storage policies. - // It will throw if the service has already been added. - this->storage_add_service(std::move(inner_service)); - }); - } - - /// Remove a service from this wait set. - /** - * \sa remove_guard_condition() for details of how this method works. - * - * \param[in] service Service to be removed. - * \throws std::invalid_argument if service is nullptr. - * \throws std::runtime_error if service is not part of the wait set. - * \throws exceptions based on the policies used. - */ - void - remove_service(std::shared_ptr service) - { - if (nullptr == service) { - throw std::invalid_argument("service is nullptr"); - } - // this method comes from the SynchronizationPolicy - this->sync_remove_service( - std::move(service), - [this](std::shared_ptr && inner_service) { - inner_service->exchange_in_use_by_wait_set_state(false); - // This method comes from the StoragePolicy, and it may not exist for - // fixed sized storage policies. - // It will throw if the service is not in the wait set. - this->storage_remove_service(std::move(inner_service)); - }); - } - - /// Add a waitable to this wait set. - /** - * \sa add_guard_condition() for details of how this method works. - * - * Additionally, this function has an optional parameter which can be used to - * more quickly associate this waitable with an entity when it is ready, and - * so that the ownership maybe held in order to keep the waitable's parent in - * scope while waiting. - * If it is set to nullptr it will be ignored. - * The destruction of the associated entity's shared pointer will not cause - * the waitable to be removed, but it will cause the associated entity pointer - * to be nullptr when introspecting this waitable after waiting. - * - * Note that rclcpp::QOSEventHandlerBase are just a special case of - * rclcpp::Waitable and can be added with this function. - * - * \param[in] waitable Waitable to be added. - * \param[in] associated_entity Type erased shared pointer associated with the waitable. - * This may be nullptr. - * \throws std::invalid_argument if waitable is nullptr. - * \throws std::runtime_error if waitable has already been added or is - * associated with another wait set. - * \throws exceptions based on the policies used. - */ - void - add_waitable( - std::shared_ptr waitable, - std::shared_ptr associated_entity = nullptr) - { - if (nullptr == waitable) { - throw std::invalid_argument("waitable is nullptr"); - } - // this method comes from the SynchronizationPolicy - this->sync_add_waitable( - std::move(waitable), - std::move(associated_entity), - [this]( - std::shared_ptr && inner_waitable, - std::shared_ptr && associated_entity) - { - bool already_in_use = inner_waitable->exchange_in_use_by_wait_set_state(true); - if (already_in_use) { - throw std::runtime_error("waitable already in use by another wait set"); - } - // This method comes from the StoragePolicy, and it may not exist for - // fixed sized storage policies. - // It will throw if the waitable has already been added. - this->storage_add_waitable(std::move(inner_waitable), std::move(associated_entity)); - }); - } - - /// Remove a waitable from this wait set. - /** - * \sa remove_guard_condition() for details of how this method works. - * - * \param[in] waitable Waitable to be removed. - * \throws std::invalid_argument if waitable is nullptr. - * \throws std::runtime_error if waitable is not part of the wait set. - * \throws exceptions based on the policies used. - */ - void - remove_waitable(std::shared_ptr waitable) - { - if (nullptr == waitable) { - throw std::invalid_argument("waitable is nullptr"); - } - // this method comes from the SynchronizationPolicy - this->sync_remove_waitable( - std::move(waitable), - [this](std::shared_ptr && inner_waitable) { - inner_waitable->exchange_in_use_by_wait_set_state(false); - // This method comes from the StoragePolicy, and it may not exist for - // fixed sized storage policies. - // It will throw if the waitable is not in the wait set. - this->storage_remove_waitable(std::move(inner_waitable)); - }); - } - - /// Remove any destroyed entities from the wait set. - /** - * When the storage policy does not maintain shared ownership for the life - * of the wait set, e.g. the DynamicStorage policy, it is possible for an - * entity to go out of scope and be deleted without this wait set noticing. - * Therefore there are weak references in this wait set which need to be - * periodically cleared. - * This function performs that clean up. - * - * Since this involves removing entities from the wait set, and is only - * needed if the wait set does not keep ownership of the added entities, the - * storage policies which are static will not need this function and therefore - * do not provide this function. - * - * \throws exceptions based on the policies used. - */ - void - prune_deleted_entities() - { - // this method comes from the SynchronizationPolicy - this->sync_prune_deleted_entities( - [this]() { - // This method comes from the StoragePolicy, and it may not exist for - // fixed sized storage policies. - this->storage_prune_deleted_entities(); - }); - } - - /// Wait for any of the entities in the wait set to be ready, or a period of time to pass. - /** - * This function will return when either one of the entities within this wait - * set is ready, or a period of time has passed, which ever is first. - * The term "ready" means different things for different entities, but - * generally it means some condition is met asynchronously for which this - * function waits. - * - * This function can either wait for a period of time, do no waiting - * (non-blocking), or wait indefinitely, all based on the value of the - * time_to_wait parameter. - * Waiting is always measured against the std::chrono::steady_clock. - * If waiting indefinitely, the Timeout result is not possible. - * There is no "cancel wait" function on this class, but if you want to wait - * indefinitely but have a way to asynchronously interrupt this method, then - * you can use a dedicated rclcpp::GuardCondition for that purpose. - * - * This function will modify the internal rcl_wait_set_t, so introspecting - * the wait set during a call to wait is never safe. - * You should always wait, then introspect, and then, only when done - * introspecting, wait again. - * - * It may be thread-safe to add and remove entities to the wait set - * concurrently with this function, depending on the SynchronizationPolicy - * that is used. - * With the rclcpp::wait_set_policies::ThreadSafeSynchronization policy this - * function will stop waiting to allow add or remove of an entity, and then - * resume waiting, so long as the timeout has not been reached. - * - * \param[in] time_to_wait If > 0, time to wait for entities to be ready, - * if == 0, check if anything is ready without blocking, or - * if < 0, wait indefinitely until one of the items is ready. - * Default is -1, so wait indefinitely. - * \returns Ready when one of the entities is ready, or - * \returns Timeout when the given time to wait is exceeded, not possible - * when time_to_wait is < 0, or - * \returns Empty if the wait set is empty, avoiding the possibility of - * waiting indefinitely on an empty wait set. - * \throws rclcpp::exceptions::RCLError on unhandled rcl errors - */ - template - RCUTILS_WARN_UNUSED - WaitResult - wait(std::chrono::duration time_to_wait = std::chrono::duration(-1)) - { - auto time_to_wait_ns = std::chrono::duration_cast(time_to_wait); - - // ensure the ownership of the entities in the wait set is shared for the duration of wait - this->storage_acquire_ownerships(); - RCLCPP_SCOPE_EXIT({this->storage_release_ownerships();}); - - // this method comes from the SynchronizationPolicy - return this->template sync_wait>( - // pass along the time_to_wait duration as nanoseconds - time_to_wait_ns, - // this method provides the ability to rebuild the wait set, if needed - [this]() { - // This method comes from the StoragePolicy - this->storage_rebuild_rcl_wait_set( - // This method comes from the SynchronizationPolicy - this->get_extra_guard_conditions() - ); - }, - // this method provides access to the rcl wait set - [this]() -> rcl_wait_set_t & { - // This method comes from the StoragePolicy - return this->storage_get_rcl_wait_set(); - }, - // this method provides a way to create the WaitResult - [this](WaitResultKind wait_result_kind) -> WaitResult { - // convert the result into a WaitResult - switch (wait_result_kind) { - case WaitResultKind::Ready: - return WaitResult::from_ready_wait_result_kind(*this); - case WaitResultKind::Timeout: - return WaitResult::from_timeout_wait_result_kind(); - case WaitResultKind::Empty: - return WaitResult::from_empty_wait_result_kind(); - default: - auto msg = "unknown WaitResultKind with value: " + std::to_string(wait_result_kind); - throw std::runtime_error(msg); - } - } - ); - } - -private: - // Add WaitResult type as a friend so it can call private methods for - // acquiring and releasing resources as the WaitResult is initialized and - // destructed, respectively. - friend WaitResult; - - /// Called by the WaitResult's constructor to place a hold on ownership and thread-safety. - /** - * Should only be called in pairs with wait_result_release(). - * - * \throws std::runtime_error If called twice before wait_result_release(). - */ - void - wait_result_acquire() - { - if (wait_result_holding_) { - throw std::runtime_error("wait_result_acquire() called while already holding"); - } - wait_result_holding_ = true; - // this method comes from the SynchronizationPolicy - this->sync_wait_result_acquire(); - // this method comes from the StoragePolicy - this->storage_acquire_ownerships(); - } - - /// Called by the WaitResult's destructor to release resources. - /** - * Should only be called if wait_result_acquire() has been called. - * - * \throws std::runtime_error If called before wait_result_acquire(). - */ - void - wait_result_release() - { - if (!wait_result_holding_) { - throw std::runtime_error("wait_result_release() called while not holding"); - } - wait_result_holding_ = false; - // this method comes from the StoragePolicy - this->storage_release_ownerships(); - // this method comes from the SynchronizationPolicy - this->sync_wait_result_release(); - } - - bool wait_result_holding_ = false; -}; - -} // namespace rclcpp - -#endif // RCLCPP__WAIT_SET_TEMPLATE_HPP_ diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 2f282349ae..2837d5a16d 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -15,8 +15,6 @@ #ifndef RCLCPP__WAITABLE_HPP_ #define RCLCPP__WAITABLE_HPP_ -#include - #include "rclcpp/macros.hpp" #include "rclcpp/visibility_control.hpp" @@ -96,6 +94,7 @@ class Waitable size_t get_number_of_ready_guard_conditions(); + // TODO(jacobperron): smart pointer? /// Add the Waitable to a wait set. /** * \param[in] wait_set A handle to the wait set to add the Waitable to. @@ -147,23 +146,6 @@ class Waitable virtual void execute() = 0; - - /// Exchange the "in use by wait set" state for this timer. - /** - * This is used to ensure this timer is not used by multiple - * wait sets at the same time. - * - * \param[in] in_use_state the new state to exchange into the state, true - * indicates it is now in use by a wait set, and false is that it is no - * longer in use by a wait set. - * \returns the previous state. - */ - RCLCPP_PUBLIC - bool - exchange_in_use_by_wait_set_state(bool in_use_state); - -private: - std::atomic in_use_by_wait_set_{false}; }; // class Waitable } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index 6a1d3934d4..c2f64e06f7 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -70,21 +70,6 @@ ClientBase::~ClientBase() client_handle_.reset(); } -bool -ClientBase::take_type_erased_response(void * response_out, rmw_request_id_t & request_header_out) -{ - rcl_ret_t ret = rcl_take_response( - this->get_client_handle().get(), - &request_header_out, - response_out); - if (RCL_RET_CLIENT_TAKE_FAILED == ret) { - return false; - } else if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } - return true; -} - const char * ClientBase::get_service_name() const { @@ -192,9 +177,3 @@ ClientBase::get_rcl_node_handle() const { return node_handle_.get(); } - -bool -ClientBase::exchange_in_use_by_wait_set_state(bool in_use_state) -{ - return in_use_by_wait_set_.exchange(in_use_state); -} diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 556c42ddde..22f5c2f063 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -305,127 +305,99 @@ Executor::execute_any_executable(AnyExecutable & any_exec) } } -static void -take_and_do_error_handling( - const char * action_description, - const char * topic_or_service_name, - std::function take_action, - std::function handle_action) +Executor::execute_subscription( + rclcpp::SubscriptionBase::SharedPtr subscription) { - bool taken = false; - try { - taken = take_action(); - } catch (const rclcpp::exceptions::RCLError & rcl_error) { - RCLCPP_ERROR( - rclcpp::get_logger("rclcpp"), - "executor %s '%s' unexpectedly failed: %s", - action_description, - topic_or_service_name, - rcl_error.what()); - } - if (taken) { - handle_action(); - } else { - // Message or Service was not taken for some reason. - // Note that this can be normal, if the underlying middleware needs to - // interrupt wait spuriously it is allowed. - // So in that case the executor cannot tell the difference in a - // spurious wake up and an entity actually having data until trying - // to take the data. - RCLCPP_DEBUG( - rclcpp::get_logger("rclcpp"), - "executor %s '%s' failed to take anything", - action_description, - topic_or_service_name); - } -} - -void -Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rclcpp::MessageInfo message_info; - message_info.get_rmw_message_info().from_intra_process = false; + rmw_message_info_t message_info; + message_info.from_intra_process = false; if (subscription->is_serialized()) { - // This is the case where a copy of the serialized message is taken from - // the middleware via inter-process communication. - std::shared_ptr serialized_msg = - subscription->create_serialized_message(); - take_and_do_error_handling( - "taking a serialized message from topic", - subscription->get_topic_name(), - [&]() {return subscription->take_serialized(*serialized_msg.get(), message_info);}, - [&]() - { - auto void_serialized_msg = std::static_pointer_cast(serialized_msg); - subscription->handle_message(void_serialized_msg, message_info); - }); + auto serialized_msg = subscription->create_serialized_message(); + auto ret = rcl_take_serialized_message( + subscription->get_subscription_handle().get(), + serialized_msg.get(), &message_info, nullptr); + if (RCL_RET_OK == ret) { + auto void_serialized_msg = std::static_pointer_cast(serialized_msg); + subscription->handle_message(void_serialized_msg, message_info); + } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "take_serialized failed for subscription on topic '%s': %s", + subscription->get_topic_name(), rcl_get_error_string().str); + rcl_reset_error(); + } subscription->return_serialized_message(serialized_msg); } else if (subscription->can_loan_messages()) { - // This is the case where a loaned message is taken from the middleware via - // inter-process communication, given to the user for their callback, - // and then returned. void * loaned_msg = nullptr; - // TODO(wjwwood): refactor this into methods on subscription when LoanedMessage - // is extened to support subscriptions as well. - take_and_do_error_handling( - "taking a loaned message from topic", - subscription->get_topic_name(), - [&]() - { - rcl_ret_t ret = rcl_take_loaned_message( - subscription->get_subscription_handle().get(), - &loaned_msg, - &message_info.get_rmw_message_info(), - nullptr); - if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { - return false; - } else if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } - return true; - }, - [&]() {subscription->handle_loaned_message(loaned_msg, message_info);}); - rcl_ret_t ret = rcl_return_loaned_message_from_subscription( + auto ret = rcl_take_loaned_message( + subscription->get_subscription_handle().get(), + &loaned_msg, + &message_info, + nullptr); + if (RCL_RET_OK == ret) { + subscription->handle_loaned_message(loaned_msg, message_info); + } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "take_loaned failed for subscription on topic '%s': %s", + subscription->get_topic_name(), rcl_get_error_string().str); + rcl_reset_error(); + } + ret = rcl_return_loaned_message_from_subscription( subscription->get_subscription_handle().get(), loaned_msg); if (RCL_RET_OK != ret) { - RCLCPP_ERROR( - rclcpp::get_logger("rclcpp"), - "rcl_return_loaned_message_from_subscription() failed for subscription on topic '%s': %s", + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "return_loaned_message failed for subscription on topic '%s': %s", subscription->get_topic_name(), rcl_get_error_string().str); } loaned_msg = nullptr; } else { - // This case is taking a copy of the message data from the middleware via - // inter-process communication. std::shared_ptr message = subscription->create_message(); - take_and_do_error_handling( - "taking a message from topic", - subscription->get_topic_name(), - [&]() {return subscription->take_type_erased(message.get(), message_info);}, - [&]() {subscription->handle_message(message, message_info);}); + auto ret = rcl_take( + subscription->get_subscription_handle().get(), + message.get(), &message_info, nullptr); + if (RCL_RET_OK == ret) { + subscription->handle_message(message, message_info); + } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "could not deserialize serialized message on topic '%s': %s", + subscription->get_topic_name(), rcl_get_error_string().str); + rcl_reset_error(); + } subscription->return_message(message); } } void -Executor::execute_timer(rclcpp::TimerBase::SharedPtr timer) +Executor::execute_timer( + rclcpp::TimerBase::SharedPtr timer) { timer->execute_callback(); } void -Executor::execute_service(rclcpp::ServiceBase::SharedPtr service) +Executor::execute_service( + rclcpp::ServiceBase::SharedPtr service) { auto request_header = service->create_request_header(); std::shared_ptr request = service->create_request(); - take_and_do_error_handling( - "taking a service server request from service", - service->get_service_name(), - [&]() {return service->take_type_erased_request(request.get(), *request_header);}, - [&]() {service->handle_request(request_header, request);}); + rcl_ret_t status = rcl_take_request( + service->get_service_handle().get(), + request_header.get(), + request.get()); + if (status == RCL_RET_OK) { + service->handle_request(request_header, request); + } else if (status != RCL_RET_SERVICE_TAKE_FAILED) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "take request failed for server of service '%s': %s", + service->get_service_name(), rcl_get_error_string().str); + rcl_reset_error(); + } } void @@ -434,11 +406,19 @@ Executor::execute_client( { auto request_header = client->create_request_header(); std::shared_ptr response = client->create_response(); - take_and_do_error_handling( - "taking a service client response from service", - client->get_service_name(), - [&]() {return client->take_type_erased_response(response.get(), *request_header);}, - [&]() {client->handle_response(request_header, response);}); + rcl_ret_t status = rcl_take_response( + client->get_client_handle().get(), + request_header.get(), + response.get()); + if (status == RCL_RET_OK) { + client->handle_response(request_header, response); + } else if (status != RCL_RET_CLIENT_TAKE_FAILED) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "take response failed for client of service '%s': %s", + client->get_service_name(), rcl_get_error_string().str); + rcl_reset_error(); + } } void diff --git a/rclcpp/src/rclcpp/guard_condition.cpp b/rclcpp/src/rclcpp/guard_condition.cpp deleted file mode 100644 index 22ca3f3223..0000000000 --- a/rclcpp/src/rclcpp/guard_condition.cpp +++ /dev/null @@ -1,80 +0,0 @@ -// Copyright 2020 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/guard_condition.hpp" - -#include "rclcpp/exceptions.hpp" -#include "rclcpp/logging.hpp" - -namespace rclcpp -{ - -GuardCondition::GuardCondition(rclcpp::Context::SharedPtr context) -: 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); - } -} - -GuardCondition::~GuardCondition() -{ - rcl_ret_t ret = rcl_guard_condition_fini(&this->rcl_guard_condition_); - if (RCL_RET_OK != ret) { - try { - rclcpp::exceptions::throw_from_rcl_error(ret); - } catch (const std::exception & exception) { - RCLCPP_ERROR( - rclcpp::get_logger("rclcpp"), - "Error in destruction of rcl guard condition: %s", exception.what()); - } - } -} - -rclcpp::Context::SharedPtr -GuardCondition::get_context() const -{ - return context_; -} - -const rcl_guard_condition_t & -GuardCondition::get_rcl_guard_condition() const -{ - return rcl_guard_condition_; -} - -void -GuardCondition::trigger() -{ - rcl_ret_t ret = rcl_trigger_guard_condition(&rcl_guard_condition_); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } -} - -bool -GuardCondition::exchange_in_use_by_wait_set_state(bool in_use_state) -{ - return in_use_by_wait_set_.exchange(in_use_state); -} - -} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/message_info.cpp b/rclcpp/src/rclcpp/message_info.cpp deleted file mode 100644 index 0c2eab81fa..0000000000 --- a/rclcpp/src/rclcpp/message_info.cpp +++ /dev/null @@ -1,39 +0,0 @@ -// Copyright 2020 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/message_info.hpp" - -namespace rclcpp -{ - -MessageInfo::MessageInfo(const rmw_message_info_t & rmw_message_info) -: rmw_message_info_(rmw_message_info) -{} - -MessageInfo::~MessageInfo() -{} - -const rmw_message_info_t & -MessageInfo::get_rmw_message_info() const -{ - return rmw_message_info_; -} - -rmw_message_info_t & -MessageInfo::get_rmw_message_info() -{ - return rmw_message_info_; -} - -} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index fb35a87725..c7f8ff9449 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -34,21 +34,6 @@ ServiceBase::ServiceBase(std::shared_ptr node_handle) ServiceBase::~ServiceBase() {} -bool -ServiceBase::take_type_erased_request(void * request_out, rmw_request_id_t & request_id_out) -{ - rcl_ret_t ret = rcl_take_request( - this->get_service_handle().get(), - &request_id_out, - request_out); - if (RCL_RET_CLIENT_TAKE_FAILED == ret) { - return false; - } else if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } - return true; -} - const char * ServiceBase::get_service_name() { @@ -78,9 +63,3 @@ ServiceBase::get_rcl_node_handle() const { return node_handle_.get(); } - -bool -ServiceBase::exchange_in_use_by_wait_set_state(bool in_use_state) -{ - return in_use_by_wait_set_.exchange(in_use_state); -} diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index d7f0da1bbb..314a8b837c 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -133,48 +133,6 @@ SubscriptionBase::get_actual_qos() const return rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos); } -bool -SubscriptionBase::take_type_erased(void * message_out, rclcpp::MessageInfo & message_info_out) -{ - rcl_ret_t ret = rcl_take( - this->get_subscription_handle().get(), - message_out, - &message_info_out.get_rmw_message_info(), - nullptr // rmw_subscription_allocation_t is unused here - ); - if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { - return false; - } else if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } - if ( - matches_any_intra_process_publishers(&message_info_out.get_rmw_message_info().publisher_gid)) - { - // In this case, the message will be delivered via intra-process and - // we should ignore this copy of the message. - return false; - } - return true; -} - -bool -SubscriptionBase::take_serialized( - rcl_serialized_message_t & message_out, - rclcpp::MessageInfo & message_info_out) -{ - rcl_ret_t ret = rcl_take_serialized_message( - this->get_subscription_handle().get(), - &message_out, - &message_info_out.get_rmw_message_info(), - nullptr); - if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { - return false; - } else if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } - return true; -} - const rosidl_message_type_support_t & SubscriptionBase::get_message_type_support_handle() const { @@ -251,25 +209,3 @@ SubscriptionBase::matches_any_intra_process_publishers(const rmw_gid_t * sender_ } return ipm->matches_any_publishers(sender_gid); } - -bool -SubscriptionBase::exchange_in_use_by_wait_set_state( - void * pointer_to_subscription_part, - bool in_use_state) -{ - if (nullptr == pointer_to_subscription_part) { - throw std::invalid_argument("pointer_to_subscription_part is unexpectedly nullptr"); - } - if (this == pointer_to_subscription_part) { - return subscription_in_use_by_wait_set_.exchange(in_use_state); - } - if (get_intra_process_waitable().get() == pointer_to_subscription_part) { - return intra_process_subscription_waitable_in_use_by_wait_set_.exchange(in_use_state); - } - for (const auto & qos_event : event_handlers_) { - if (qos_event.get() == pointer_to_subscription_part) { - return qos_events_in_use_by_wait_set_[qos_event.get()].exchange(in_use_state); - } - } - throw std::runtime_error("given pointer_to_subscription_part does not match any part"); -} diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index 6dc8759973..6f3259dd64 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -135,9 +135,3 @@ TimerBase::get_timer_handle() { return timer_handle_; } - -bool -TimerBase::exchange_in_use_by_wait_set_state(bool in_use_state) -{ - return in_use_by_wait_set_.exchange(in_use_state); -} diff --git a/rclcpp/src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp b/rclcpp/src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp deleted file mode 100644 index 5640ad4d29..0000000000 --- a/rclcpp/src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp +++ /dev/null @@ -1,100 +0,0 @@ -// Copyright 2020 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/wait_set_policies/detail/write_preferring_read_write_lock.hpp" - -namespace rclcpp -{ -namespace wait_set_policies -{ -namespace detail -{ - -WritePreferringReadWriteLock::WritePreferringReadWriteLock( - std::function enter_waiting_function) -: read_mutex_(*this), write_mutex_(*this), enter_waiting_function_(enter_waiting_function) -{} - -WritePreferringReadWriteLock::ReadMutex & -WritePreferringReadWriteLock::get_read_mutex() -{ - return read_mutex_; -} - -WritePreferringReadWriteLock::WriteMutex & -WritePreferringReadWriteLock::get_write_mutex() -{ - return write_mutex_; -} - -WritePreferringReadWriteLock::ReadMutex::ReadMutex(WritePreferringReadWriteLock & parent_lock) -: parent_lock_(parent_lock) -{} - -void -WritePreferringReadWriteLock::ReadMutex::lock() -{ - std::unique_lock lock(parent_lock_.mutex_); - while ( - parent_lock_.number_of_writers_waiting_ > 0 || - parent_lock_.writer_active_ || - parent_lock_.reader_active_) - { - parent_lock_.condition_variable_.wait(lock); - } - parent_lock_.reader_active_ = true; - // implicit unlock of parent_lock_.mutex_ -} - -void -WritePreferringReadWriteLock::ReadMutex::unlock() -{ - std::unique_lock lock(parent_lock_.mutex_); - parent_lock_.reader_active_ = false; - parent_lock_.condition_variable_.notify_all(); - // implicit unlock of parent_lock_.mutex_ -} - -WritePreferringReadWriteLock::WriteMutex::WriteMutex(WritePreferringReadWriteLock & parent_lock) -: parent_lock_(parent_lock) -{} - -void -WritePreferringReadWriteLock::WriteMutex::lock() -{ - std::unique_lock lock(parent_lock_.mutex_); - parent_lock_.number_of_writers_waiting_ += 1; - if (nullptr != parent_lock_.enter_waiting_function_) { - parent_lock_.enter_waiting_function_(); - } - while (parent_lock_.reader_active_ || parent_lock_.writer_active_) { - parent_lock_.condition_variable_.wait(lock); - } - parent_lock_.number_of_writers_waiting_ -= 1; - parent_lock_.writer_active_ = true; - // implicit unlock of parent_lock_.mutex_ -} - -void -WritePreferringReadWriteLock::WriteMutex::unlock() -{ - std::unique_lock lock(parent_lock_.mutex_); - parent_lock_.writer_active_ = false; - parent_lock_.condition_variable_.notify_all(); - // implicit unlock of parent_lock_.mutex_ -} - -} // namespace detail -} // namespace wait_set_policies -} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp index b76c7215e0..542b10a016 100644 --- a/rclcpp/src/rclcpp/waitable.cpp +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -51,9 +51,3 @@ Waitable::get_number_of_ready_guard_conditions() { return 0u; } - -bool -Waitable::exchange_in_use_by_wait_set_state(bool in_use_state) -{ - return in_use_by_wait_set_.exchange(in_use_state); -} diff --git a/rclcpp/test/test_guard_condition.cpp b/rclcpp/test/test_guard_condition.cpp deleted file mode 100644 index 474d1d4bcc..0000000000 --- a/rclcpp/test/test_guard_condition.cpp +++ /dev/null @@ -1,87 +0,0 @@ -// Copyright 2020 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 - -#include - -#include "rclcpp/rclcpp.hpp" - -class TestGuardCondition : public ::testing::Test -{ -protected: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } -}; - -/* - * Testing normal construction and destruction. - */ -TEST_F(TestGuardCondition, construction_and_destruction) { - { - auto gc = std::make_shared(); - (void)gc; - } - - { - // invalid context (nullptr) - ASSERT_THROW( - { - auto gc = std::make_shared(nullptr); - (void)gc; - }, std::invalid_argument); - } - - { - // invalid context (uninitialized) - auto context = std::make_shared(); - ASSERT_THROW( - { - auto gc = std::make_shared(context); - (void)gc; - }, rclcpp::exceptions::RCLInvalidArgument); - } -} - -/* - * Testing context accessor. - */ -TEST_F(TestGuardCondition, get_context) { - { - auto gc = std::make_shared(); - gc->get_context(); - } -} - -/* - * Testing rcl guard condition accessor. - */ -TEST_F(TestGuardCondition, get_rcl_guard_condition) { - { - auto gc = std::make_shared(); - gc->get_rcl_guard_condition(); - } -} - -/* - * Testing tigger method. - */ -TEST_F(TestGuardCondition, trigger) { - { - auto gc = std::make_shared(); - gc->trigger(); - } -} diff --git a/rclcpp/test/test_subscription.cpp b/rclcpp/test/test_subscription.cpp index 95f9b96448..2346917a38 100644 --- a/rclcpp/test/test_subscription.cpp +++ b/rclcpp/test/test_subscription.cpp @@ -14,10 +14,8 @@ #include -#include -#include #include -#include +#include #include #include "rclcpp/exceptions.hpp" @@ -25,8 +23,6 @@ #include "test_msgs/msg/empty.hpp" -using namespace std::chrono_literals; - class TestSubscription : public ::testing::Test { public: @@ -239,13 +235,13 @@ TEST_F(TestSubscription, callback_bind) { using test_msgs::msg::Empty; { // Member callback for plain class - SubscriptionClass subscription_object; - subscription_object.CreateSubscription(); + SubscriptionClass subscriptionObject; + subscriptionObject.CreateSubscription(); } { // Member callback for class inheriting from rclcpp::Node - SubscriptionClassNodeInheritance subscription_object; - subscription_object.CreateSubscription(); + SubscriptionClassNodeInheritance subscriptionObject; + subscriptionObject.CreateSubscription(); } { // Member callback for class inheriting from testing::Test @@ -256,79 +252,6 @@ TEST_F(TestSubscription, callback_bind) { } } -/* - Testing take. - */ -TEST_F(TestSubscription, take) { - initialize(); - using test_msgs::msg::Empty; - auto do_nothing = [](std::shared_ptr) {FAIL();}; - { - auto sub = node->create_subscription("~/test_take", 1, do_nothing); - test_msgs::msg::Empty msg; - rclcpp::MessageInfo msg_info; - EXPECT_FALSE(sub->take(msg, msg_info)); - } - { - rclcpp::SubscriptionOptions so; - so.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; - auto sub = node->create_subscription("~/test_take", 1, do_nothing, so); - rclcpp::PublisherOptions po; - po.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; - auto pub = node->create_publisher("~/test_take", 1, po); - { - test_msgs::msg::Empty msg; - pub->publish(msg); - } - test_msgs::msg::Empty msg; - rclcpp::MessageInfo msg_info; - bool message_recieved = false; - auto start = std::chrono::steady_clock::now(); - do { - message_recieved = sub->take(msg, msg_info); - std::this_thread::sleep_for(100ms); - } while (!message_recieved && std::chrono::steady_clock::now() - start < 10s); - EXPECT_TRUE(message_recieved); - } - // TODO(wjwwood): figure out a good way to test the intra-process exclusion behavior. -} - -/* - Testing take_serialized. - */ -TEST_F(TestSubscription, take_serialized) { - initialize(); - using test_msgs::msg::Empty; - auto do_nothing = [](std::shared_ptr) {FAIL();}; - { - auto sub = node->create_subscription("~/test_take", 1, do_nothing); - std::shared_ptr msg = sub->create_serialized_message(); - rclcpp::MessageInfo msg_info; - EXPECT_FALSE(sub->take_serialized(*msg, msg_info)); - } - { - rclcpp::SubscriptionOptions so; - so.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; - auto sub = node->create_subscription("~/test_take", 1, do_nothing, so); - rclcpp::PublisherOptions po; - po.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; - auto pub = node->create_publisher("~/test_take", 1, po); - { - test_msgs::msg::Empty msg; - pub->publish(msg); - } - std::shared_ptr msg = sub->create_serialized_message(); - rclcpp::MessageInfo msg_info; - bool message_recieved = false; - auto start = std::chrono::steady_clock::now(); - do { - message_recieved = sub->take_serialized(*msg, msg_info); - std::this_thread::sleep_for(100ms); - } while (!message_recieved && std::chrono::steady_clock::now() - start < 10s); - EXPECT_TRUE(message_recieved); - } -} - /* Testing subscription with intraprocess enabled and invalid QoS */ diff --git a/rclcpp/test/test_wait_set.cpp b/rclcpp/test/test_wait_set.cpp deleted file mode 100644 index 39fe4c1262..0000000000 --- a/rclcpp/test/test_wait_set.cpp +++ /dev/null @@ -1,264 +0,0 @@ -// Copyright 2020 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 - -#include -#include - -#include "rcl_interfaces/srv/list_parameters.hpp" -#include "rclcpp/rclcpp.hpp" -#include "test_msgs/msg/basic_types.hpp" - -class TestWaitSet : public ::testing::Test -{ -protected: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } -}; - -/* - * Testing normal construction and destruction. - */ -TEST_F(TestWaitSet, construction_and_destruction) { - { - rclcpp::WaitSet wait_set; - (void)wait_set; - } - - { - rclcpp::WaitSet wait_set( - std::vector{}, - std::vector{}, - std::vector{}, - std::vector{}, - std::vector{}, - std::vector{}); - (void)wait_set; - } - - { - auto gc = std::make_shared(); - rclcpp::WaitSet wait_set({}, {gc}); - (void)wait_set; - } - - { - auto context = std::make_shared(); - context->init(0, nullptr); - auto gc = std::make_shared(context); - rclcpp::WaitSet wait_set({}, {gc}, {}, {}, {}, {}, context); - (void)wait_set; - } - - { - // invalid context (nullptr) - ASSERT_THROW( - { - rclcpp::WaitSet wait_set( - std::vector{}, - std::vector{}, - std::vector{}, - std::vector{}, - std::vector{}, - std::vector{}, - nullptr); - (void)wait_set; - }, std::invalid_argument); - } - - { - // invalid context (uninitialized) - auto context = std::make_shared(); - ASSERT_THROW( - { - rclcpp::WaitSet wait_set( - std::vector{}, - std::vector{}, - std::vector{}, - std::vector{}, - std::vector{}, - std::vector{}, - context); - (void)wait_set; - }, rclcpp::exceptions::RCLInvalidArgument); - } -} - -/* - * Testing rcl wait set accessor. - */ -TEST_F(TestWaitSet, get_rcl_wait_set) { - { - rclcpp::WaitSet wait_set; - wait_set.get_rcl_wait_set(); - } -} - -/* - * Testing add/remove for guard condition methods. - */ -TEST_F(TestWaitSet, add_remove_guard_condition) { - // normal, mixed initialization - { - auto gc = std::make_shared(); - auto gc2 = std::make_shared(); - rclcpp::WaitSet wait_set({}, {gc}); - wait_set.add_guard_condition(gc2); - wait_set.remove_guard_condition(gc2); - wait_set.remove_guard_condition(gc); - } - - // out of order removal - { - auto gc = std::make_shared(); - auto gc2 = std::make_shared(); - rclcpp::WaitSet wait_set({}, {gc}); - wait_set.add_guard_condition(gc2); - wait_set.remove_guard_condition(gc); - wait_set.remove_guard_condition(gc2); - } - - // start empty, normal - { - auto gc = std::make_shared(); - rclcpp::WaitSet wait_set; - wait_set.add_guard_condition(gc); - wait_set.remove_guard_condition(gc); - } - - // add invalid (nullptr) - { - rclcpp::WaitSet wait_set; - ASSERT_THROW( - { - wait_set.add_guard_condition(nullptr); - }, std::invalid_argument); - } - - // double add - { - auto gc = std::make_shared(); - rclcpp::WaitSet wait_set; - wait_set.add_guard_condition(gc); - ASSERT_THROW( - { - wait_set.add_guard_condition(gc); - }, std::runtime_error); - } - - // remove invalid (nullptr) - { - rclcpp::WaitSet wait_set; - ASSERT_THROW( - { - wait_set.remove_guard_condition(nullptr); - }, std::invalid_argument); - } - - // remove unrelated - { - auto gc = std::make_shared(); - auto gc2 = std::make_shared(); - rclcpp::WaitSet wait_set({}, {gc}); - ASSERT_THROW( - { - wait_set.remove_guard_condition(gc2); - }, std::runtime_error); - } - - // double remove - { - auto gc = std::make_shared(); - rclcpp::WaitSet wait_set({}, {gc}); - wait_set.remove_guard_condition(gc); - ASSERT_THROW( - { - wait_set.remove_guard_condition(gc); - }, std::runtime_error); - } - - // remove from empty - { - auto gc = std::make_shared(); - rclcpp::WaitSet wait_set; - ASSERT_THROW( - { - wait_set.remove_guard_condition(gc); - }, std::runtime_error); - } -} - -/* - * Testing adding each entity to two separate wait sets. - */ -TEST_F(TestWaitSet, add_guard_condition_to_two_different_wait_set) { - { - rclcpp::WaitSet wait_set1; - rclcpp::WaitSet wait_set2; - auto node = std::make_shared("add_guard_condition_to_two_different_wait_set"); - - auto guard_condition = std::make_shared(); - wait_set1.add_guard_condition(guard_condition); - ASSERT_THROW( - { - wait_set2.add_guard_condition(guard_condition); - }, std::runtime_error); - - auto do_nothing = [](const std::shared_ptr) {}; - auto sub = node->create_subscription("~/test", 1, do_nothing); - wait_set1.add_subscription(sub); - ASSERT_THROW( - { - wait_set2.add_subscription(sub); - }, std::runtime_error); - - auto timer = node->create_wall_timer(std::chrono::seconds(1), []() {}); - wait_set1.add_timer(timer); - ASSERT_THROW( - { - wait_set2.add_timer(timer); - }, std::runtime_error); - - auto client = node->create_client("~/test"); - wait_set1.add_client(client); - ASSERT_THROW( - { - wait_set2.add_client(client); - }, std::runtime_error); - - auto srv_do_nothing = []( - const std::shared_ptr, - std::shared_ptr) {}; - auto service = - node->create_service("~/test", srv_do_nothing); - wait_set1.add_service(service); - ASSERT_THROW( - { - wait_set2.add_service(service); - }, std::runtime_error); - - rclcpp::PublisherOptions po; - po.event_callbacks.deadline_callback = [](rclcpp::QOSDeadlineOfferedInfo &) {}; - auto pub = node->create_publisher("~/test", 1, po); - auto qos_event = pub->get_event_handlers()[0]; - wait_set1.add_waitable(qos_event, pub); - ASSERT_THROW( - { - wait_set2.add_waitable(qos_event, pub); - }, std::runtime_error); - } -}