niklas/rclcpp
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

[rclcpp] add WaitSet class and modify entities to work without executor (#1047)

Browse source 
* add rclcpp::GuardCondition wrapping rcl_guard_condition_t

Signed-off-by: William Woodall <william@osrfoundation.org>

* WIP second wait set refactor, just guard conditions so far

Signed-off-by: William Woodall <william@osrfoundation.org>

* fix typo

Signed-off-by: William Woodall <william@osrfoundation.org>

* removing a question/todo, I think this is fine as is

Signed-off-by: William Woodall <william@osrfoundation.org>

* added subscriptions and waitable to wait sets

Signed-off-by: William Woodall <william@osrfoundation.org>

* improve usability with subscriptions and wait sets

Signed-off-by: William Woodall <william@osrfoundation.org>

* adding take to subscription so it can be used without the executor

Signed-off-by: William Woodall <william@osrfoundation.org>

* add rclcpp::MessageInfo to replace use of rmw_message_info_t

Signed-off-by: William Woodall <william@osrfoundation.org>

* refactor Subscription and Executor so they can be used separately

Signed-off-by: William Woodall <william@osrfoundation.org>

* style and cpplint

Signed-off-by: William Woodall <william@osrfoundation.org>

* fixup take_serialized() and add tests for it

Signed-off-by: William Woodall <william@osrfoundation.org>

* add support for client and service to wait set

Signed-off-by: William Woodall <william@osrfoundation.org>

* fix typo

Signed-off-by: William Woodall <william@osrfoundation.org>

* fix typo

Signed-off-by: William Woodall <william@osrfoundation.org>

* fix review comment

Signed-off-by: William Woodall <william@osrfoundation.org>

* add thread-safe wait set policy

Signed-off-by: William Woodall <william@osrfoundation.org>

* add check for use with multiple wait set

Signed-off-by: William Woodall <william@osrfoundation.org>

* fixup visibility macro usage

Signed-off-by: William Woodall <william@osrfoundation.org>

* remove vestigial test case

Signed-off-by: William Woodall <william@osrfoundation.org>

* move visibility macro fixes

Signed-off-by: William Woodall <william@osrfoundation.org>

* remove vestigial TODO

Signed-off-by: William Woodall <william@osrfoundation.org>
This commit is contained in:
William Woodall 2020-04-13 16:41:59 -07:00 • committed by GitHub
parent 0f0b83368a
commit aaf8b3828c
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
37 changed files with 4511 additions and 139 deletions
Download patch file Download diff file Expand all files Collapse all files

16
rclcpp/CMakeLists.txt
View file

@ -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

13
rclcpp/include/rclcpp/any_subscription_callback.hpp
View file

@ -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<void (const std::shared_ptr<MessageT>)>;
using SharedPtrWithInfoCallback =
std::function<void (const std::shared_ptr<MessageT>, const rmw_message_info_t &)>;
std::function<void (const std::shared_ptr<MessageT>, const rclcpp::MessageInfo &)>;
using ConstSharedPtrCallback = std::function<void (const std::shared_ptr<const MessageT>)>;
using ConstSharedPtrWithInfoCallback =
std::function<void (const std::shared_ptr<const MessageT>, const rmw_message_info_t &)>;
std::function<void (const std::shared_ptr<const MessageT>, const rclcpp::MessageInfo &)>;
using UniquePtrCallback = std::function<void (MessageUniquePtr)>;
using UniquePtrWithInfoCallback =
std::function<void (MessageUniquePtr, const rmw_message_info_t &)>;
std::function<void (MessageUniquePtr, const rclcpp::MessageInfo &)>;
SharedPtrCallback shared_ptr_callback_;
SharedPtrWithInfoCallback shared_ptr_with_info_callback_;
@ -155,7 +156,7 @@ public:
}
void dispatch(
std::shared_ptr<MessageT> message, const rmw_message_info_t & message_info)
std::shared_ptr<MessageT> message, const rclcpp::MessageInfo & message_info)
{
TRACEPOINT(callback_start, (const void *)this, false);
if (shared_ptr_callback_) {
@ -181,7 +182,7 @@ public:
}
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 @@ public:
}
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_) {

57
rclcpp/include/rclcpp/client.hpp
View file

@ -15,6 +15,7 @@
#ifndef RCLCPP__CLIENT_HPP_
#define RCLCPP__CLIENT_HPP_
#include <atomic>
#include <future>
#include <map>
#include <memory>
@ -62,6 +63,27 @@ public:
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 @@ public:
virtual void handle_response(
std::shared_ptr<rmw_request_id_t> request_header, std::shared_ptr<void> 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 @@ protected:
std::shared_ptr<rclcpp::Context> context_;
std::shared_ptr<rcl_client_t> client_handle_;
std::atomic<bool> in_use_by_wait_set_{false};
};
template<typename ServiceT>
@ -171,6 +209,25 @@ public:
{
}
/// 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<void>
create_response() override
{

2
rclcpp/include/rclcpp/context.hpp
View file

@ -159,7 +159,7 @@ public:
*
* \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

100
rclcpp/include/rclcpp/guard_condition.hpp Normal file
View file

@ -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 <atomic>
#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<bool> in_use_by_wait_set_{false};
};
} // namespace rclcpp
#endif // RCLCPP__GUARD_CONDITION_HPP_

52
rclcpp/include/rclcpp/message_info.hpp Normal file
View file

@ -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_

2
rclcpp/include/rclcpp/rclcpp.hpp
View file

@ -143,6 +143,7 @@
#include <memory>
#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_

106
rclcpp/include/rclcpp/service.hpp
View file

@ -15,6 +15,7 @@
#ifndef RCLCPP__SERVICE_HPP_
#define RCLCPP__SERVICE_HPP_
#include <atomic>
#include <functional>
#include <iostream>
#include <memory>
@ -44,8 +45,7 @@ public:
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase)
RCLCPP_PUBLIC
explicit ServiceBase(
std::shared_ptr<rcl_node_t> node_handle);
explicit ServiceBase(std::shared_ptr<rcl_node_t> node_handle);
RCLCPP_PUBLIC
virtual ~ServiceBase();
@ -62,12 +62,53 @@ public:
std::shared_ptr<const rcl_service_t>
get_service_handle() const;
virtual std::shared_ptr<void> create_request() = 0;
virtual std::shared_ptr<rmw_request_id_t> 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<void>
create_request() = 0;
virtual
std::shared_ptr<rmw_request_id_t>
create_request_header() = 0;
virtual
void
handle_request(
std::shared_ptr<rmw_request_id_t> request_header,
std::shared_ptr<void> 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 @@ protected:
std::shared_ptr<rcl_service_t> service_handle_;
bool owns_rcl_handle_ = true;
std::atomic<bool> in_use_by_wait_set_{false};
};
template<typename ServiceT>
@ -222,36 +265,63 @@ public:
{
}
std::shared_ptr<void> 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<void>(new typename ServiceT::Request());
return this->take_type_erased_request(&request_out, request_id_out);
}
std::shared_ptr<rmw_request_id_t> create_request_header() override
std::shared_ptr<void>
create_request() override
{
// TODO(wjwwood): This should probably use rmw_request_id's allocator.
// (since it is a C type)
return std::shared_ptr<rmw_request_id_t>(new rmw_request_id_t);
return std::make_shared<typename ServiceT::Request>();
}
void handle_request(
std::shared_ptr<rmw_request_id_t>
create_request_header() override
{
return std::make_shared<rmw_request_id_t>();
}
void
handle_request(
std::shared_ptr<rmw_request_id_t> request_header,
std::shared_ptr<void> request) override
{
auto typed_request = std::static_pointer_cast<typename ServiceT::Request>(request);
auto response = std::shared_ptr<typename ServiceT::Response>(new typename ServiceT::Response);
auto response = std::make_shared<typename ServiceT::Response>();
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<rmw_request_id_t> req_id,
std::shared_ptr<typename ServiceT::Response> 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");
}
}

66
rclcpp/include/rclcpp/subscription.hpp
View file

@ -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 @@ public:
// 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<
using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess<
CallbackMessageT,
AllocatorT,
typename MessageUniquePtr::deleter_type
>>(
typename MessageUniquePtr::deleter_type>;
auto subscription_intra_process = std::make_shared<SubscriptionIntraProcessT>(
callback,
options.get_allocator(),
context,
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 @@ public:
}
/// 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<AllocatorT> & options)
@ -197,19 +197,32 @@ public:
(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<CallbackMessageT,
AllocatorT>::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<void *>(&message_out), message_info_out);
}
std::shared_ptr<void> create_message() override
std::shared_ptr<void>
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 @@ public:
return message_memory_strategy_->borrow_message();
}
std::shared_ptr<rcl_serialized_message_t> create_serialized_message() override
std::shared_ptr<rcl_serialized_message_t>
create_serialized_message() override
{
return message_memory_strategy_->borrow_serialized_message();
}
void handle_message(
std::shared_ptr<void> & message, const rmw_message_info_t & message_info) override
void
handle_message(
std::shared_ptr<void> & 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 @@ public:
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<CallbackMessageT *>(loaned_message);
// message is loaned, so we have to make sure that the deleter does not deallocate the message
@ -248,18 +265,21 @@ public:
/// Return the borrowed message.
/** \param message message to be returned */
void return_message(std::shared_ptr<void> & message) override
void
return_message(std::shared_ptr<void> & message) override
{
auto typed_message = std::static_pointer_cast<CallbackMessageT>(message);
message_memory_strategy_->return_message(typed_message);
}
void return_serialized_message(std::shared_ptr<rcl_serialized_message_t> & message) override
void
return_serialized_message(std::shared_ptr<rcl_serialized_message_t> & 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();
}

70
rclcpp/include/rclcpp/subscription_base.hpp
View file

@ -15,9 +15,12 @@
#ifndef RCLCPP__SUBSCRIPTION_BASE_HPP_
#define RCLCPP__SUBSCRIPTION_BASE_HPP_
#include <atomic>
#include <memory>
#include <string>
#include <unordered_map>
#include <vector>
#include <utility>
#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 @@ public:
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 @@ public:
RCLCPP_PUBLIC
virtual
void
handle_message(std::shared_ptr<void> & message, const rmw_message_info_t & message_info) = 0;
handle_message(std::shared_ptr<void> & 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 @@ public:
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<typename EventCallbackT>
void
@ -205,6 +265,7 @@ protected:
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 @@ private:
rosidl_message_type_support_t type_support_;
bool is_serialized_;
std::atomic<bool> subscription_in_use_by_wait_set_{false};
std::atomic<bool> intra_process_subscription_waitable_in_use_by_wait_set_{false};
std::unordered_map<rclcpp::QOSEventHandlerBase *,
std::atomic<bool>> qos_events_in_use_by_wait_set_;
};
} // namespace rclcpp

37
rclcpp/include/rclcpp/subscription_wait_set_mask.hpp Normal file
View file

@ -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_

19
rclcpp/include/rclcpp/timer.hpp
View file

@ -15,6 +15,7 @@
#ifndef RCLCPP__TIMER_HPP_
#define RCLCPP__TIMER_HPP_
#include <atomic>
#include <chrono>
#include <functional>
#include <memory>
@ -64,7 +65,7 @@ public:
/**
* \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 @@ public:
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<rcl_timer_t> timer_handle_;
std::atomic<bool> in_use_by_wait_set_{false};
};

2
rclcpp/include/rclcpp/utilities.hpp
View file

@ -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<std::string>

155
rclcpp/include/rclcpp/wait_result.hpp Normal file
View file

@ -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 <cassert>
#include <functional>
#include <stdexcept>
#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 WaitSetT>
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_

33
rclcpp/include/rclcpp/wait_result_kind.hpp Normal file
View file

@ -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, //<! Kind used when something in the wait set was ready.
Timeout, //<! Kind used when the wait resulted in a timeout.
Empty, //<! Kind used when trying to wait on an empty wait set.
};
} // namespace rclcpp
#endif // RCLCPP__WAIT_RESULT_KIND_HPP_

106
rclcpp/include/rclcpp/wait_set.hpp Normal file
View file

@ -0,0 +1,106 @@
// 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_HPP_
#define RCLCPP__WAIT_SET_HPP_
#include <memory>
#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_

412
rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp Normal file
View file

@ -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 <memory>
#include <stdexcept>
#include <utility>
#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<bool HasStrongOwnership>
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<class EntityT>
std::pair<void *, EntityT *>
get_raw_pointer_from_smart_pointer(const std::shared_ptr<EntityT> & shared_pointer)
{
return {nullptr, shared_pointer.get()};
}
template<class EntityT>
std::pair<std::shared_ptr<EntityT>, EntityT *>
get_raw_pointer_from_smart_pointer(const std::weak_ptr<EntityT> & 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_

72
rclcpp/include/rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp Normal file
View file

@ -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 <chrono>
#include <functional>
namespace rclcpp
{
namespace wait_set_policies
{
namespace detail
{
/// Common structure for synchronization policies.
class SynchronizationPolicyCommon
{
protected:
SynchronizationPolicyCommon() = default;
~SynchronizationPolicyCommon() = default;
std::function<bool()>
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_

243
rclcpp/include/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp Normal file
View file

@ -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 <condition_variable>
#include <functional>
#include <mutex>
#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<WritePreferringReadWriteLock::ReadMutex> lock(wprw_lock_.get_read_mutex());
* // Do reading...
* }
* void do_some_writing()
* {
* using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
* std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
* // Do writing...
* }
* };
*/
class WritePreferringReadWriteLock final
{
public:
RCLCPP_PUBLIC
explicit WritePreferringReadWriteLock(std::function<void()> 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<void()> enter_waiting_function_;
};
} // namespace detail
} // namespace wait_set_policies
} // namespace rclcpp
#endif // RCLCPP__WAIT_SET_POLICIES__DETAIL__WRITE_PREFERRING_READ_WRITE_LOCK_HPP_

470
rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp Normal file
View file

@ -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 <algorithm>
#include <memory>
#include <utility>
#include <vector>
#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<false>
{
protected:
using is_mutable = std::true_type;
class SubscriptionEntry
{
// (wjwwood): indent of 'public:' is weird, I know. uncrustify is dumb.
public:
std::shared_ptr<rclcpp::SubscriptionBase> subscription;
rclcpp::SubscriptionWaitSetMask mask;
/// Conversion constructor, which is intentionally not marked explicit.
SubscriptionEntry(
const std::shared_ptr<rclcpp::SubscriptionBase> & 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<rclcpp::SubscriptionBase> subscription;
rclcpp::SubscriptionWaitSetMask mask;
explicit WeakSubscriptionEntry(
const std::shared_ptr<rclcpp::SubscriptionBase> & 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<rclcpp::SubscriptionBase>
lock() const
{
return subscription.lock();
}
bool
expired() const noexcept
{
return subscription.expired();
}
};
using SequenceOfWeakSubscriptions = std::vector<WeakSubscriptionEntry>;
using SubscriptionsIterable = std::vector<SubscriptionEntry>;
using SequenceOfWeakGuardConditions = std::vector<std::weak_ptr<rclcpp::GuardCondition>>;
using GuardConditionsIterable = std::vector<std::shared_ptr<rclcpp::GuardCondition>>;
using SequenceOfWeakTimers = std::vector<std::weak_ptr<rclcpp::TimerBase>>;
using TimersIterable = std::vector<std::shared_ptr<rclcpp::TimerBase>>;
using SequenceOfWeakClients = std::vector<std::weak_ptr<rclcpp::ClientBase>>;
using ClientsIterable = std::vector<std::shared_ptr<rclcpp::ClientBase>>;
using SequenceOfWeakServices = std::vector<std::weak_ptr<rclcpp::ServiceBase>>;
using ServicesIterable = std::vector<std::shared_ptr<rclcpp::ServiceBase>>;
class WaitableEntry
{
public:
std::shared_ptr<rclcpp::Waitable> waitable;
std::shared_ptr<void> associated_entity;
/// Conversion constructor, which is intentionally not marked explicit.
WaitableEntry(
const std::shared_ptr<rclcpp::Waitable> & waitable_in = nullptr,
const std::shared_ptr<void> & 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<rclcpp::Waitable> waitable;
std::weak_ptr<void> associated_entity;
explicit WeakWaitableEntry(
const std::shared_ptr<rclcpp::Waitable> & waitable_in,
const std::shared_ptr<void> & 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<rclcpp::Waitable>
lock() const
{
return waitable.lock();
}
bool
expired() const noexcept
{
return waitable.expired();
}
};
using SequenceOfWeakWaitables = std::vector<WeakWaitableEntry>;
using WaitablesIterable = std::vector<WaitableEntry>;
template<class ArrayOfExtraGuardConditions>
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<class ArrayOfExtraGuardConditions>
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<class EntityT, class SequenceOfEntitiesT>
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<class EntityT, class SequenceOfEntitiesT>
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<rclcpp::SubscriptionBase> && 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<rclcpp::SubscriptionBase> && 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<rclcpp::GuardCondition> && 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<rclcpp::GuardCondition> && 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<rclcpp::TimerBase> && 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<rclcpp::TimerBase> && 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<rclcpp::ClientBase> && 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<rclcpp::ClientBase> && 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<rclcpp::ServiceBase> && 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<rclcpp::ServiceBase> && 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<rclcpp::Waitable> && waitable,
std::shared_ptr<void> && 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<rclcpp::Waitable> && 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_

317
rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp Normal file
View file

@ -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 <chrono>
#include <functional>
#include <memory>
#include <utility>
#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<std::shared_ptr<rclcpp::GuardCondition>, 0> &
get_extra_guard_conditions()
{
static const std::array<std::shared_ptr<rclcpp::GuardCondition>, 0> empty{};
return empty;
}
/// Add subscription without thread-safety.
/**
* Does not throw, but storage function may throw.
*/
void
sync_add_subscription(
std::shared_ptr<rclcpp::SubscriptionBase> && subscription,
const rclcpp::SubscriptionWaitSetMask & mask,
std::function<
void(std::shared_ptr<rclcpp::SubscriptionBase>&&, 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<rclcpp::SubscriptionBase> && subscription,
const rclcpp::SubscriptionWaitSetMask & mask,
std::function<
void(std::shared_ptr<rclcpp::SubscriptionBase>&&, 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<rclcpp::GuardCondition> && guard_condition,
std::function<void(std::shared_ptr<rclcpp::GuardCondition>&&)> 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<rclcpp::GuardCondition> && guard_condition,
std::function<void(std::shared_ptr<rclcpp::GuardCondition>&&)> 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<rclcpp::TimerBase> && timer,
std::function<void(std::shared_ptr<rclcpp::TimerBase>&&)> 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<rclcpp::TimerBase> && timer,
std::function<void(std::shared_ptr<rclcpp::TimerBase>&&)> 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<rclcpp::ClientBase> && client,
std::function<void(std::shared_ptr<rclcpp::ClientBase>&&)> 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<rclcpp::ClientBase> && client,
std::function<void(std::shared_ptr<rclcpp::ClientBase>&&)> 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<rclcpp::ServiceBase> && service,
std::function<void(std::shared_ptr<rclcpp::ServiceBase>&&)> 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<rclcpp::ServiceBase> && service,
std::function<void(std::shared_ptr<rclcpp::ServiceBase>&&)> 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<rclcpp::Waitable> && waitable,
std::shared_ptr<void> && associated_entity,
std::function<
void(std::shared_ptr<rclcpp::Waitable>&&, std::shared_ptr<void>&&)
> 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<rclcpp::Waitable> && waitable,
std::function<void(std::shared_ptr<rclcpp::Waitable>&&)> 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<void()> prune_deleted_entities_function)
{
// Explicitly no thread synchronization.
prune_deleted_entities_function();
}
/// Implements wait without any thread-safety.
template<class WaitResultT>
WaitResultT
sync_wait(
std::chrono::nanoseconds time_to_wait_ns,
std::function<void()> rebuild_rcl_wait_set,
std::function<rcl_wait_set_t & ()> get_rcl_wait_set,
std::function<WaitResultT(WaitResultKind wait_result_kind)> 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<bool()> 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_

201
rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp Normal file
View file

@ -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 <array>
#include <memory>
#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<true>
{
protected:
using is_mutable = std::false_type;
class SubscriptionEntry
{
public:
std::shared_ptr<rclcpp::SubscriptionBase> subscription;
rclcpp::SubscriptionWaitSetMask mask;
/// Conversion constructor, which is intentionally not marked explicit.
SubscriptionEntry(
const std::shared_ptr<rclcpp::SubscriptionBase> & 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<rclcpp::GuardCondition>,
NumberOfGuardCondtions
>;
using GuardConditionsIterable = ArrayOfGuardConditions;
using ArrayOfTimers = std::array<
std::shared_ptr<rclcpp::TimerBase>,
NumberOfTimers
>;
using TimersIterable = ArrayOfTimers;
using ArrayOfClients = std::array<
std::shared_ptr<rclcpp::ClientBase>,
NumberOfClients
>;
using ClientsIterable = ArrayOfClients;
using ArrayOfServices = std::array<
std::shared_ptr<rclcpp::ServiceBase>,
NumberOfServices
>;
using ServicesIterable = ArrayOfServices;
struct WaitableEntry
{
/// Conversion constructor, which is intentionally not marked explicit.
WaitableEntry(
const std::shared_ptr<rclcpp::Waitable> & waitable_in = nullptr,
const std::shared_ptr<void> & associated_entity_in = nullptr) noexcept
: waitable(waitable_in),
associated_entity(associated_entity_in)
{}
std::shared_ptr<rclcpp::Waitable> waitable;
std::shared_ptr<void> associated_entity;
};
using ArrayOfWaitables = std::array<
WaitableEntry,
NumberOfWaitables
>;
using WaitablesIterable = ArrayOfWaitables;
template<class ArrayOfExtraGuardConditions>
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<class ArrayOfExtraGuardConditions>
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_

379
rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp Normal file
View file

@ -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 <chrono>
#include <functional>
#include <memory>
#include <utility>
#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<rclcpp::GuardCondition>(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<std::shared_ptr<rclcpp::GuardCondition>, 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<rclcpp::SubscriptionBase> && subscription,
const rclcpp::SubscriptionWaitSetMask & mask,
std::function<
void(std::shared_ptr<rclcpp::SubscriptionBase>&&, const rclcpp::SubscriptionWaitSetMask &)
> add_subscription_function)
{
using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
add_subscription_function(std::move(subscription), mask);
}
/// Remove guard condition.
void
sync_remove_subscription(
std::shared_ptr<rclcpp::SubscriptionBase> && subscription,
const rclcpp::SubscriptionWaitSetMask & mask,
std::function<
void(std::shared_ptr<rclcpp::SubscriptionBase>&&, const rclcpp::SubscriptionWaitSetMask &)
> remove_subscription_function)
{
using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
remove_subscription_function(std::move(subscription), mask);
}
/// Add guard condition.
void
sync_add_guard_condition(
std::shared_ptr<rclcpp::GuardCondition> && guard_condition,
std::function<void(std::shared_ptr<rclcpp::GuardCondition>&&)> add_guard_condition_function)
{
using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
std::lock_guard<WritePreferringReadWriteLock::WriteMutex> 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<rclcpp::GuardCondition> && guard_condition,
std::function<void(std::shared_ptr<rclcpp::GuardCondition>&&)> remove_guard_condition_function)
{
using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
remove_guard_condition_function(std::move(guard_condition));
}
/// Add timer.
void
sync_add_timer(
std::shared_ptr<rclcpp::TimerBase> && timer,
std::function<void(std::shared_ptr<rclcpp::TimerBase>&&)> add_timer_function)
{
using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
add_timer_function(std::move(timer));
}
/// Remove timer.
void
sync_remove_timer(
std::shared_ptr<rclcpp::TimerBase> && timer,
std::function<void(std::shared_ptr<rclcpp::TimerBase>&&)> remove_timer_function)
{
using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
remove_timer_function(std::move(timer));
}
/// Add client.
void
sync_add_client(
std::shared_ptr<rclcpp::ClientBase> && client,
std::function<void(std::shared_ptr<rclcpp::ClientBase>&&)> add_client_function)
{
using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
add_client_function(std::move(client));
}
/// Remove client.
void
sync_remove_client(
std::shared_ptr<rclcpp::ClientBase> && client,
std::function<void(std::shared_ptr<rclcpp::ClientBase>&&)> remove_client_function)
{
using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
remove_client_function(std::move(client));
}
/// Add service.
void
sync_add_service(
std::shared_ptr<rclcpp::ServiceBase> && service,
std::function<void(std::shared_ptr<rclcpp::ServiceBase>&&)> add_service_function)
{
using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
add_service_function(std::move(service));
}
/// Remove service.
void
sync_remove_service(
std::shared_ptr<rclcpp::ServiceBase> && service,
std::function<void(std::shared_ptr<rclcpp::ServiceBase>&&)> remove_service_function)
{
using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
remove_service_function(std::move(service));
}
/// Add waitable.
void
sync_add_waitable(
std::shared_ptr<rclcpp::Waitable> && waitable,
std::shared_ptr<void> && associated_entity,
std::function<
void(std::shared_ptr<rclcpp::Waitable>&&, std::shared_ptr<void>&&)
> add_waitable_function)
{
using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
std::lock_guard<WritePreferringReadWriteLock::WriteMutex> 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<rclcpp::Waitable> && waitable,
std::function<void(std::shared_ptr<rclcpp::Waitable>&&)> remove_waitable_function)
{
using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
remove_waitable_function(std::move(waitable));
}
/// Prune deleted entities.
void
sync_prune_deleted_entities(std::function<void()> prune_deleted_entities_function)
{
using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock;
std::lock_guard<WritePreferringReadWriteLock::WriteMutex> lock(wprw_lock_.get_write_mutex());
prune_deleted_entities_function();
}
/// Implements wait.
template<class WaitResultT>
WaitResultT
sync_wait(
std::chrono::nanoseconds time_to_wait_ns,
std::function<void()> rebuild_rcl_wait_set,
std::function<rcl_wait_set_t & ()> get_rcl_wait_set,
std::function<WaitResultT(WaitResultKind wait_result_kind)> 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<bool()> 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<WritePreferringReadWriteLock::ReadMutex> 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<std::shared_ptr<rclcpp::GuardCondition>, 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_

743
rclcpp/include/rclcpp/wait_set_template.hpp Normal file
View file

@ -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 <chrono>
#include <memory>
#include <utility>
#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 SynchronizationPolicy, class StoragePolicy>
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<rclcpp::SubscriptionBase> 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<rclcpp::SubscriptionBase> && 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<rclcpp::SubscriptionBase> 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<rclcpp::SubscriptionBase> && 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<rclcpp::GuardCondition> 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<rclcpp::GuardCondition> && 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<rclcpp::GuardCondition> 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<rclcpp::GuardCondition> && 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<rclcpp::TimerBase> 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<rclcpp::TimerBase> && 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<rclcpp::TimerBase> 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<rclcpp::TimerBase> && 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<rclcpp::ClientBase> 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<rclcpp::ClientBase> && 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<rclcpp::ClientBase> 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<rclcpp::ClientBase> && 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<rclcpp::ServiceBase> 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<rclcpp::ServiceBase> && 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<rclcpp::ServiceBase> 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<rclcpp::ServiceBase> && 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<rclcpp::Waitable> waitable,
std::shared_ptr<void> 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<rclcpp::Waitable> && inner_waitable,
std::shared_ptr<void> && 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<rclcpp::Waitable> 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<rclcpp::Waitable> && 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<class Rep = int64_t, class Period = std::milli>
RCUTILS_WARN_UNUSED
WaitResult<WaitSetTemplate>
wait(std::chrono::duration<Rep, Period> time_to_wait = std::chrono::duration<Rep, Period>(-1))
{
auto time_to_wait_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(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<WaitResult<WaitSetTemplate>>(
// 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<WaitSetTemplate> {
// convert the result into a WaitResult
switch (wait_result_kind) {
case WaitResultKind::Ready:
return WaitResult<WaitSetTemplate>::from_ready_wait_result_kind(*this);
case WaitResultKind::Timeout:
return WaitResult<WaitSetTemplate>::from_timeout_wait_result_kind();
case WaitResultKind::Empty:
return WaitResult<WaitSetTemplate>::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<WaitSetTemplate>;
/// 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_

20
rclcpp/include/rclcpp/waitable.hpp
View file

@ -15,6 +15,8 @@
#ifndef RCLCPP__WAITABLE_HPP_
#define RCLCPP__WAITABLE_HPP_
#include <atomic>
#include "rclcpp/macros.hpp"
#include "rclcpp/visibility_control.hpp"
@ -94,7 +96,6 @@ public:
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 @@ public:
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<bool> in_use_by_wait_set_{false};
}; // class Waitable
} // namespace rclcpp

21
rclcpp/src/rclcpp/client.cpp
View file

@ -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);
}

164
rclcpp/src/rclcpp/executor.cpp
View file

@ -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<bool()> take_action,
std::function<void()> 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) {
// This is the case where a copy of the serialized message is taken from
// the middleware via inter-process communication.
std::shared_ptr<rcl_serialized_message_t> 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<void>(serialized_msg);
subscription->handle_message(void_serialized_msg, message_info);
} else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) {
RCUTILS_LOG_ERROR_NAMED(
"rclcpp",
"take_serialized failed for subscription on topic '%s': %s",
subscription->get_topic_name(), rcl_get_error_string().str);
rcl_reset_error();
}
});
subscription->return_serialized_message(serialized_msg);
} else if (subscription->can_loan_messages()) {
// This is the case where a loaned message is taken from the middleware via
// inter-process communication, given to the user for their callback,
// and then returned.
void * loaned_msg = nullptr;
auto ret = rcl_take_loaned_message(
// 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,
&message_info.get_rmw_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();
if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) {
return false;
} else if (RCL_RET_OK != ret) {
rclcpp::exceptions::throw_from_rcl_error(ret);
}
ret = rcl_return_loaned_message_from_subscription(
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<void> 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<void> 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<void> 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

80
rclcpp/src/rclcpp/guard_condition.cpp Normal file
View file

@ -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

39
rclcpp/src/rclcpp/message_info.cpp Normal file
View file

@ -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

21
rclcpp/src/rclcpp/service.cpp
View file

@ -34,6 +34,21 @@ ServiceBase::ServiceBase(std::shared_ptr<rcl_node_t> 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);
}

64
rclcpp/src/rclcpp/subscription_base.cpp
View file

@ -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");
}

6
rclcpp/src/rclcpp/timer.cpp
View file

@ -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);
}

100
rclcpp/src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp Normal file
View file

@ -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<void()> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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

6
rclcpp/src/rclcpp/waitable.cpp
View file

@ -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);
}

87
rclcpp/test/test_guard_condition.cpp Normal file
View file

@ -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 <gtest/gtest.h>
#include <memory>
#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<rclcpp::GuardCondition>();
(void)gc;
}
{
// invalid context (nullptr)
ASSERT_THROW(
{
auto gc = std::make_shared<rclcpp::GuardCondition>(nullptr);
(void)gc;
}, std::invalid_argument);
}
{
// invalid context (uninitialized)
auto context = std::make_shared<rclcpp::Context>();
ASSERT_THROW(
{
auto gc = std::make_shared<rclcpp::GuardCondition>(context);
(void)gc;
}, rclcpp::exceptions::RCLInvalidArgument);
}
}
/*
* Testing context accessor.
*/
TEST_F(TestGuardCondition, get_context) {
{
auto gc = std::make_shared<rclcpp::GuardCondition>();
gc->get_context();
}
}
/*
* Testing rcl guard condition accessor.
*/
TEST_F(TestGuardCondition, get_rcl_guard_condition) {
{
auto gc = std::make_shared<rclcpp::GuardCondition>();
gc->get_rcl_guard_condition();
}
}
/*
* Testing tigger method.
*/
TEST_F(TestGuardCondition, trigger) {
{
auto gc = std::make_shared<rclcpp::GuardCondition>();
gc->trigger();
}
}

87
rclcpp/test/test_subscription.cpp
View file

@ -14,8 +14,10 @@
#include <gtest/gtest.h>
#include <string>
#include <chrono>
#include <memory>
#include <string>
#include <thread>
#include <vector>
#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<const test_msgs::msg::Empty>) {FAIL();};
{
auto sub = node->create_subscription<test_msgs::msg::Empty>("~/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_msgs::msg::Empty>("~/test_take", 1, do_nothing, so);
rclcpp::PublisherOptions po;
po.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable;
auto pub = node->create_publisher<test_msgs::msg::Empty>("~/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<const rcl_serialized_message_t>) {FAIL();};
{
auto sub = node->create_subscription<test_msgs::msg::Empty>("~/test_take", 1, do_nothing);
std::shared_ptr<rcl_serialized_message_t> 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_msgs::msg::Empty>("~/test_take", 1, do_nothing, so);
rclcpp::PublisherOptions po;
po.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable;
auto pub = node->create_publisher<test_msgs::msg::Empty>("~/test_take", 1, po);
{
test_msgs::msg::Empty msg;
pub->publish(msg);
}
std::shared_ptr<rcl_serialized_message_t> 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
*/

264
rclcpp/test/test_wait_set.cpp Normal file
View file

@ -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 <gtest/gtest.h>
#include <memory>
#include <vector>
#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<rclcpp::WaitSet::SubscriptionEntry>{},
std::vector<rclcpp::GuardCondition::SharedPtr>{},
std::vector<rclcpp::TimerBase::SharedPtr>{},
std::vector<rclcpp::ClientBase::SharedPtr>{},
std::vector<rclcpp::ServiceBase::SharedPtr>{},
std::vector<rclcpp::WaitSet::WaitableEntry>{});
(void)wait_set;
}
{
auto gc = std::make_shared<rclcpp::GuardCondition>();
rclcpp::WaitSet wait_set({}, {gc});
(void)wait_set;
}
{
auto context = std::make_shared<rclcpp::Context>();
context->init(0, nullptr);
auto gc = std::make_shared<rclcpp::GuardCondition>(context);
rclcpp::WaitSet wait_set({}, {gc}, {}, {}, {}, {}, context);
(void)wait_set;
}
{
// invalid context (nullptr)
ASSERT_THROW(
{
rclcpp::WaitSet wait_set(
std::vector<rclcpp::WaitSet::SubscriptionEntry>{},
std::vector<rclcpp::GuardCondition::SharedPtr>{},
std::vector<rclcpp::TimerBase::SharedPtr>{},
std::vector<rclcpp::ClientBase::SharedPtr>{},
std::vector<rclcpp::ServiceBase::SharedPtr>{},
std::vector<rclcpp::WaitSet::WaitableEntry>{},
nullptr);
(void)wait_set;
}, std::invalid_argument);
}
{
// invalid context (uninitialized)
auto context = std::make_shared<rclcpp::Context>();
ASSERT_THROW(
{
rclcpp::WaitSet wait_set(
std::vector<rclcpp::WaitSet::SubscriptionEntry>{},
std::vector<rclcpp::GuardCondition::SharedPtr>{},
std::vector<rclcpp::TimerBase::SharedPtr>{},
std::vector<rclcpp::ClientBase::SharedPtr>{},
std::vector<rclcpp::ServiceBase::SharedPtr>{},
std::vector<rclcpp::WaitSet::WaitableEntry>{},
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<rclcpp::GuardCondition>();
auto gc2 = std::make_shared<rclcpp::GuardCondition>();
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<rclcpp::GuardCondition>();
auto gc2 = std::make_shared<rclcpp::GuardCondition>();
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::GuardCondition>();
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::GuardCondition>();
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<rclcpp::GuardCondition>();
auto gc2 = std::make_shared<rclcpp::GuardCondition>();
rclcpp::WaitSet wait_set({}, {gc});
ASSERT_THROW(
{
wait_set.remove_guard_condition(gc2);
}, std::runtime_error);
}
// double remove
{
auto gc = std::make_shared<rclcpp::GuardCondition>();
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::GuardCondition>();
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<rclcpp::Node>("add_guard_condition_to_two_different_wait_set");
auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
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<test_msgs::msg::BasicTypes>) {};
auto sub = node->create_subscription<test_msgs::msg::BasicTypes>("~/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<rcl_interfaces::srv::ListParameters>("~/test");
wait_set1.add_client(client);
ASSERT_THROW(
{
wait_set2.add_client(client);
}, std::runtime_error);
auto srv_do_nothing = [](
const std::shared_ptr<rcl_interfaces::srv::ListParameters::Request>,
std::shared_ptr<rcl_interfaces::srv::ListParameters::Response>) {};
auto service =
node->create_service<rcl_interfaces::srv::ListParameters>("~/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_msgs::msg::BasicTypes>("~/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);
}
}