diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index f4a6c895e7..1389ad8aeb 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -49,11 +49,13 @@ 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 @@ -84,6 +86,7 @@ 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 ) @@ -519,6 +522,19 @@ 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 1909b85815..6a813c222b 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -25,6 +25,7 @@ #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" @@ -43,13 +44,13 @@ class AnySubscriptionCallback using SharedPtrCallback = std::function)>; using SharedPtrWithInfoCallback = - std::function, const rmw_message_info_t &)>; + std::function, const rclcpp::MessageInfo &)>; using ConstSharedPtrCallback = std::function)>; using ConstSharedPtrWithInfoCallback = - std::function, const rmw_message_info_t &)>; + std::function, const rclcpp::MessageInfo &)>; using UniquePtrCallback = std::function; using UniquePtrWithInfoCallback = - std::function; + std::function; SharedPtrCallback shared_ptr_callback_; SharedPtrWithInfoCallback shared_ptr_with_info_callback_; @@ -155,7 +156,7 @@ class AnySubscriptionCallback } void dispatch( - std::shared_ptr message, const rmw_message_info_t & message_info) + std::shared_ptr message, const rclcpp::MessageInfo & message_info) { TRACEPOINT(callback_start, (const void *)this, false); if (shared_ptr_callback_) { @@ -181,7 +182,7 @@ class AnySubscriptionCallback } void dispatch_intra_process( - ConstMessageSharedPtr message, const rmw_message_info_t & message_info) + ConstMessageSharedPtr message, const rclcpp::MessageInfo & message_info) { TRACEPOINT(callback_start, (const void *)this, true); if (const_shared_ptr_callback_) { @@ -204,7 +205,7 @@ class AnySubscriptionCallback } void dispatch_intra_process( - MessageUniquePtr message, const rmw_message_info_t & message_info) + MessageUniquePtr message, const rclcpp::MessageInfo & 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 46467400fe..f7f1d5cb8c 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__CLIENT_HPP_ #define RCLCPP__CLIENT_HPP_ +#include #include #include #include @@ -62,6 +63,27 @@ 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; @@ -93,6 +115,20 @@ 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) @@ -113,6 +149,8 @@ class ClientBase std::shared_ptr context_; std::shared_ptr client_handle_; + + std::atomic in_use_by_wait_set_{false}; }; template @@ -171,6 +209,25 @@ 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 dfc33d5e4f..76d3cb1dca 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 RCLErrorBase, if rcl_shutdown fails + * \throw various exceptions derived from rclcpp::exceptions::RCLError, if rcl_shutdown fails */ RCLCPP_PUBLIC virtual diff --git a/rclcpp/include/rclcpp/guard_condition.hpp b/rclcpp/include/rclcpp/guard_condition.hpp new file mode 100644 index 0000000000..5088181022 --- /dev/null +++ b/rclcpp/include/rclcpp/guard_condition.hpp @@ -0,0 +1,100 @@ +// 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 new file mode 100644 index 0000000000..1653b0a505 --- /dev/null +++ b/rclcpp/include/rclcpp/message_info.hpp @@ -0,0 +1,52 @@ +// 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 98de3cba55..fc7b4435a4 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -143,6 +143,7 @@ #include #include "rclcpp/executors.hpp" +#include "rclcpp/guard_condition.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/node.hpp" #include "rclcpp/parameter.hpp" @@ -152,6 +153,7 @@ #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 9b7a120102..de720109a6 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__SERVICE_HPP_ #define RCLCPP__SERVICE_HPP_ +#include #include #include #include @@ -44,8 +45,7 @@ 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,12 +62,53 @@ class ServiceBase std::shared_ptr get_service_handle() const; - virtual std::shared_ptr create_request() = 0; - virtual std::shared_ptr create_request_header() = 0; - virtual void handle_request( + /// 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( 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) @@ -83,6 +124,8 @@ class ServiceBase std::shared_ptr service_handle_; bool owns_rcl_handle_ = true; + + std::atomic in_use_by_wait_set_{false}; }; template @@ -222,36 +265,63 @@ class Service : public ServiceBase { } - std::shared_ptr create_request() override + /// 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) { - return std::shared_ptr(new typename ServiceT::Request()); + return this->take_type_erased_request(&request_out, request_id_out); } - std::shared_ptr create_request_header() override + std::shared_ptr + create_request() override { - // 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); + return std::make_shared(); } - void handle_request( + std::shared_ptr + create_request_header() override + { + return std::make_shared(); + } + + void + handle_request( std::shared_ptr request_header, std::shared_ptr request) override { auto typed_request = std::static_pointer_cast(request); - auto response = std::shared_ptr(new typename ServiceT::Response); + auto response = std::make_shared(); any_callback_.dispatch(request_header, typed_request, response); - send_response(request_header, response); + send_response(*request_header, *response); } - void send_response( + [[deprecated("use the send_response() which takes references instead of shared pointers")]] + void + send_response( std::shared_ptr req_id, std::shared_ptr response) { - rcl_ret_t status = rcl_send_response(get_service_handle().get(), req_id.get(), response.get()); + send_response(*req_id, *response); + } - if (status != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error(status, "failed to send response"); + 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"); } } diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 2e920e286f..190a437d3c 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -38,6 +38,7 @@ #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" @@ -145,19 +146,17 @@ class Subscription : public SubscriptionBase // First create a SubscriptionIntraProcess which will be given to the intra-process manager. auto context = node_base->get_context(); - auto subscription_intra_process = std::make_shared< - rclcpp::experimental::SubscriptionIntraProcess< - CallbackMessageT, - AllocatorT, - typename MessageUniquePtr::deleter_type - >>( + using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess< + CallbackMessageT, + AllocatorT, + typename MessageUniquePtr::deleter_type>; + auto subscription_intra_process = std::make_shared( 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(), @@ -187,7 +186,8 @@ 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,19 +197,32 @@ class Subscription : public SubscriptionBase (void)options; } - /// Support dynamically setting the message memory strategy. + /// Take the next message from the inter-process subscription. /** - * 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. + * 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() */ - void set_message_memory_strategy( - typename message_memory_strategy::MessageMemoryStrategy::SharedPtr message_memory_strategy) + bool + take(CallbackMessageT & message_out, rclcpp::MessageInfo & message_info_out) { - message_memory_strategy_ = message_memory_strategy; + return this->take_type_erased(static_cast(&message_out), message_info_out); } - 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 @@ -218,15 +231,18 @@ 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 rmw_message_info_t & message_info) override + void + handle_message( + std::shared_ptr & message, + const rclcpp::MessageInfo & message_info) override { - if (matches_any_intra_process_publishers(&message_info.publisher_gid)) { + if (matches_any_intra_process_publishers(&message_info.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; @@ -237,7 +253,8 @@ class Subscription : public SubscriptionBase void handle_loaned_message( - void * loaned_message, const rmw_message_info_t & message_info) override + void * loaned_message, + const rclcpp::MessageInfo & 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 @@ -248,18 +265,21 @@ 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 23ecb5e004..6275c69a72 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -15,9 +15,12 @@ #ifndef RCLCPP__SUBSCRIPTION_BASE_HPP_ #define RCLCPP__SUBSCRIPTION_BASE_HPP_ +#include #include #include +#include #include +#include #include "rcl/subscription.h" @@ -27,6 +30,7 @@ #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" @@ -110,6 +114,45 @@ 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 @@ -132,12 +175,12 @@ class SubscriptionBase : public std::enable_shared_from_this RCLCPP_PUBLIC virtual void - handle_message(std::shared_ptr & message, const rmw_message_info_t & message_info) = 0; + handle_message(std::shared_ptr & message, const rclcpp::MessageInfo & message_info) = 0; RCLCPP_PUBLIC virtual void - handle_loaned_message(void * loaned_message, const rmw_message_info_t & message_info) = 0; + handle_loaned_message(void * loaned_message, const rclcpp::MessageInfo & message_info) = 0; /// Return the message borrowed in create_message. /** \param[in] message Shared pointer to the returned message. */ @@ -193,6 +236,23 @@ 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 @@ -205,6 +265,7 @@ 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); } @@ -229,6 +290,11 @@ 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 new file mode 100644 index 0000000000..597dd81d3e --- /dev/null +++ b/rclcpp/include/rclcpp/subscription_wait_set_mask.hpp @@ -0,0 +1,37 @@ +// 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 afeb63b663..dd754c0748 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__TIMER_HPP_ #define RCLCPP__TIMER_HPP_ +#include #include #include #include @@ -64,7 +65,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 RCLErrorBase some child class exception based on ret + * \throws rclcpp::exceptions::RCLError some child class exception based on ret */ RCLCPP_PUBLIC bool @@ -101,9 +102,25 @@ 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 83fa285f60..690df026bc 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::RCLErrorBase if the parsing fails + * \throws rclcpp::exceptions::RCLError if the parsing fails */ RCLCPP_PUBLIC std::vector diff --git a/rclcpp/include/rclcpp/wait_result.hpp b/rclcpp/include/rclcpp/wait_result.hpp new file mode 100644 index 0000000000..0dfa6974ea --- /dev/null +++ b/rclcpp/include/rclcpp/wait_result.hpp @@ -0,0 +1,155 @@ +// 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 new file mode 100644 index 0000000000..3ce65bf4f3 --- /dev/null +++ b/rclcpp/include/rclcpp/wait_result_kind.hpp @@ -0,0 +1,33 @@ +// 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 new file mode 100644 index 0000000000..4f0075cb0f --- /dev/null +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp @@ -0,0 +1,412 @@ +// 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 new file mode 100644 index 0000000000..2856ec7275 --- /dev/null +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp @@ -0,0 +1,72 @@ +// 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 new file mode 100644 index 0000000000..90b9df0178 --- /dev/null +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp @@ -0,0 +1,243 @@ +// 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 new file mode 100644 index 0000000000..907fc86bdf --- /dev/null +++ b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp @@ -0,0 +1,470 @@ +// 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 new file mode 100644 index 0000000000..5ebf32bb72 --- /dev/null +++ b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp @@ -0,0 +1,317 @@ +// 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 new file mode 100644 index 0000000000..d4d6c0c014 --- /dev/null +++ b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp @@ -0,0 +1,201 @@ +// 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 new file mode 100644 index 0000000000..ea74769ad4 --- /dev/null +++ b/rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp @@ -0,0 +1,379 @@ +// 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 new file mode 100644 index 0000000000..899d461ed3 --- /dev/null +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -0,0 +1,743 @@ +// 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 2837d5a16d..2f282349ae 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -15,6 +15,8 @@ #ifndef RCLCPP__WAITABLE_HPP_ #define RCLCPP__WAITABLE_HPP_ +#include + #include "rclcpp/macros.hpp" #include "rclcpp/visibility_control.hpp" @@ -94,7 +96,6 @@ 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. @@ -146,6 +147,23 @@ 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 c2f64e06f7..6a1d3934d4 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -70,6 +70,21 @@ 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 { @@ -177,3 +192,9 @@ 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 22f5c2f063..556c42ddde 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -305,99 +305,127 @@ Executor::execute_any_executable(AnyExecutable & any_exec) } } +static void -Executor::execute_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription) +take_and_do_error_handling( + const char * action_description, + const char * topic_or_service_name, + std::function take_action, + std::function handle_action) { - rmw_message_info_t message_info; - message_info.from_intra_process = false; + 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; if (subscription->is_serialized()) { - 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(); - } + // 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); + }); 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; - 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( + // 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( subscription->get_subscription_handle().get(), loaned_msg); if (RCL_RET_OK != ret) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "return_loaned_message failed for subscription on topic '%s': %s", + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "rcl_return_loaned_message_from_subscription() 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(); - 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(); - } + 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);}); 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(); - 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(); - } + 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);}); } void @@ -406,19 +434,11 @@ Executor::execute_client( { auto request_header = client->create_request_header(); std::shared_ptr response = client->create_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(); - } + 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);}); } void diff --git a/rclcpp/src/rclcpp/guard_condition.cpp b/rclcpp/src/rclcpp/guard_condition.cpp new file mode 100644 index 0000000000..22ca3f3223 --- /dev/null +++ b/rclcpp/src/rclcpp/guard_condition.cpp @@ -0,0 +1,80 @@ +// 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 new file mode 100644 index 0000000000..0c2eab81fa --- /dev/null +++ b/rclcpp/src/rclcpp/message_info.cpp @@ -0,0 +1,39 @@ +// 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 c7f8ff9449..fb35a87725 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -34,6 +34,21 @@ 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() { @@ -63,3 +78,9 @@ 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 314a8b837c..d7f0da1bbb 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -133,6 +133,48 @@ 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 { @@ -209,3 +251,25 @@ 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 6f3259dd64..6dc8759973 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -135,3 +135,9 @@ 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 new file mode 100644 index 0000000000..5640ad4d29 --- /dev/null +++ b/rclcpp/src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp @@ -0,0 +1,100 @@ +// 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 542b10a016..b76c7215e0 100644 --- a/rclcpp/src/rclcpp/waitable.cpp +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -51,3 +51,9 @@ 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 new file mode 100644 index 0000000000..474d1d4bcc --- /dev/null +++ b/rclcpp/test/test_guard_condition.cpp @@ -0,0 +1,87 @@ +// 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 2346917a38..95f9b96448 100644 --- a/rclcpp/test/test_subscription.cpp +++ b/rclcpp/test/test_subscription.cpp @@ -14,8 +14,10 @@ #include -#include +#include #include +#include +#include #include #include "rclcpp/exceptions.hpp" @@ -23,6 +25,8 @@ #include "test_msgs/msg/empty.hpp" +using namespace std::chrono_literals; + class TestSubscription : public ::testing::Test { public: @@ -235,13 +239,13 @@ TEST_F(TestSubscription, callback_bind) { using test_msgs::msg::Empty; { // Member callback for plain class - SubscriptionClass subscriptionObject; - subscriptionObject.CreateSubscription(); + SubscriptionClass subscription_object; + subscription_object.CreateSubscription(); } { // Member callback for class inheriting from rclcpp::Node - SubscriptionClassNodeInheritance subscriptionObject; - subscriptionObject.CreateSubscription(); + SubscriptionClassNodeInheritance subscription_object; + subscription_object.CreateSubscription(); } { // Member callback for class inheriting from testing::Test @@ -252,6 +256,79 @@ 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 new file mode 100644 index 0000000000..39fe4c1262 --- /dev/null +++ b/rclcpp/test/test_wait_set.cpp @@ -0,0 +1,264 @@ +// 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); + } +}