From ee885e6e05552e92243dd13cb6693143a00d7412 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 1 Apr 2020 06:34:05 -0700 Subject: [PATCH 01/21] add rclcpp::GuardCondition wrapping rcl_guard_condition_t Signed-off-by: William Woodall --- rclcpp/CMakeLists.txt | 7 ++ rclcpp/include/rclcpp/guard_condition.hpp | 83 +++++++++++++++++++++ rclcpp/include/rclcpp/rclcpp.hpp | 1 + rclcpp/src/rclcpp/guard_condition.cpp | 74 +++++++++++++++++++ rclcpp/test/test_guard_condition.cpp | 87 +++++++++++++++++++++++ 5 files changed, 252 insertions(+) create mode 100644 rclcpp/include/rclcpp/guard_condition.hpp create mode 100644 rclcpp/src/rclcpp/guard_condition.cpp create mode 100644 rclcpp/test/test_guard_condition.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index f4a6c895e7..97811aa3e4 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -49,6 +49,7 @@ 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 @@ -519,6 +520,12 @@ 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() + # Install test resources install( DIRECTORY test/resources diff --git a/rclcpp/include/rclcpp/guard_condition.hpp b/rclcpp/include/rclcpp/guard_condition.hpp new file mode 100644 index 0000000000..77011166f3 --- /dev/null +++ b/rclcpp/include/rclcpp/guard_condition.hpp @@ -0,0 +1,83 @@ +// 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 "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::RCLErrorBase 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_condtion() 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::RCLErrorBase based exceptions when underlying + * rcl functions fail. + */ + RCLCPP_PUBLIC + void + trigger(); + +protected: + rclcpp::Context::SharedPtr context_; + rcl_guard_condition_t rcl_guard_condition_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__GUARD_CONDITION_HPP_ diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index 98de3cba55..1c4031c3de 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -143,6 +143,7 @@ #include #include "rclcpp/executors.hpp" +#include "rclcpp/guard_condition.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/node.hpp" #include "rclcpp/parameter.hpp" diff --git a/rclcpp/src/rclcpp/guard_condition.cpp b/rclcpp/src/rclcpp/guard_condition.cpp new file mode 100644 index 0000000000..51accdc1bc --- /dev/null +++ b/rclcpp/src/rclcpp/guard_condition.cpp @@ -0,0 +1,74 @@ +// 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_condtion() 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); + } +} + +} // namespace rclcpp diff --git a/rclcpp/test/test_guard_condition.cpp b/rclcpp/test/test_guard_condition.cpp new file mode 100644 index 0000000000..fa80f18678 --- /dev/null +++ b/rclcpp/test/test_guard_condition.cpp @@ -0,0 +1,87 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "rclcpp/rclcpp.hpp" + +class TestGuardCondition : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } +}; + +/* + * Testing normal construction and destruction. + */ +TEST_F(TestGuardCondition, construction_and_destruction) { + { + auto gc = std::make_shared(); + (void)gc; + } + + { + // invalid context (nullptr) + ASSERT_THROW( + { + auto gc = std::make_shared(nullptr); + (void)gc; + }, std::invalid_argument); + } + + { + // invalid context (uninitialized) + auto context = std::make_shared(); + ASSERT_THROW( + { + auto gc = std::make_shared(context); + (void)gc; + }, rclcpp::exceptions::RCLInvalidArgument); + } +} + +/* + * Testing context accessor. + */ +TEST_F(TestGuardCondition, get_context) { + { + auto gc = std::make_shared(); + gc->get_context(); + } +} + +/* + * Testing rcl guard condition accessor. + */ +TEST_F(TestGuardCondition, get_rcl_guard_condition) { + { + auto gc = std::make_shared(); + gc->get_rcl_guard_condtion(); + } +} + +/* + * Testing tigger method. + */ +TEST_F(TestGuardCondition, trigger) { + { + auto gc = std::make_shared(); + gc->trigger(); + } +} From 34eceeb37cb06d4dc7e87a83c7ef9a553e5f4910 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 3 Apr 2020 05:25:40 -0700 Subject: [PATCH 02/21] WIP second wait set refactor, just guard conditions so far Signed-off-by: William Woodall --- rclcpp/CMakeLists.txt | 6 + rclcpp/include/rclcpp/rclcpp.hpp | 1 + rclcpp/include/rclcpp/wait_result.hpp | 155 +++++++++ rclcpp/include/rclcpp/wait_result_kind.hpp | 31 ++ rclcpp/include/rclcpp/wait_set.hpp | 108 ++++++ .../detail/storage_policy_common.hpp | 224 +++++++++++++ .../detail/synchronization_policy_common.hpp | 72 ++++ .../wait_set_policies/dynamic_storage.hpp | 161 +++++++++ .../sequential_synchronization.hpp | 159 +++++++++ .../wait_set_policies/static_storage.hpp | 97 ++++++ rclcpp/include/rclcpp/wait_set_template.hpp | 312 ++++++++++++++++++ rclcpp/test/test_wait_set.cpp | 197 +++++++++++ 12 files changed, 1523 insertions(+) create mode 100644 rclcpp/include/rclcpp/wait_result.hpp create mode 100644 rclcpp/include/rclcpp/wait_result_kind.hpp create mode 100644 rclcpp/include/rclcpp/wait_set.hpp create mode 100644 rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp create mode 100644 rclcpp/include/rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp create mode 100644 rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp create mode 100644 rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp create mode 100644 rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp create mode 100644 rclcpp/include/rclcpp/wait_set_template.hpp create mode 100644 rclcpp/test/test_wait_set.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 97811aa3e4..cd421abb58 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -526,6 +526,12 @@ if(BUILD_TESTING) 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) + target_link_libraries(test_wait_set ${PROJECT_NAME}) + endif() + # Install test resources install( DIRECTORY test/resources diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index 1c4031c3de..fc7b4435a4 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -153,6 +153,7 @@ #include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" +#include "rclcpp/wait_set.hpp" #include "rclcpp/waitable.hpp" #endif // RCLCPP__RCLCPP_HPP_ diff --git a/rclcpp/include/rclcpp/wait_result.hpp b/rclcpp/include/rclcpp/wait_result.hpp new file mode 100644 index 0000000000..0dfa6974ea --- /dev/null +++ b/rclcpp/include/rclcpp/wait_result.hpp @@ -0,0 +1,155 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__WAIT_RESULT_HPP_ +#define RCLCPP__WAIT_RESULT_HPP_ + +#include +#include +#include + +#include "rcl/wait.h" + +#include "rclcpp/macros.hpp" +#include "rclcpp/wait_result_kind.hpp" + +namespace rclcpp +{ + +// TODO(wjwwood): the union-like design of this class could be replaced with +// std::variant, when we have access to that... +/// Interface for introspecting a wait set after waiting on it. +/** + * This class: + * + * - provides the result of waiting, i.e. ready, timeout, or empty, and + * - holds the ownership of the entities of the wait set, if needed, and + * - provides the necessary information for iterating over the wait set. + * + * This class is only valid as long as the wait set which created it is valid, + * and it must be deleted before the wait set is deleted, as it contains a + * back reference to the wait set. + * + * An instance of this, which is returned from rclcpp::WaitSetTemplate::wait(), + * will cause the wait set to keep ownership of the entities because it only + * holds a reference to the sequences of them, rather than taking a copy. + * Also, in the thread-safe case, an instance of this will cause the wait set, + * to block calls which modify the sequences of the entities, e.g. add/remove + * guard condition or subscription methods. + * + * \tparam WaitSetT The wait set type which created this class. + */ +template +class WaitResult final +{ +public: + /// Create WaitResult from a "ready" result. + /** + * \param[in] wait_set A reference to the wait set, which this class + * will keep for the duration of its lifetime. + */ + static + WaitResult + from_ready_wait_result_kind(WaitSetT & wait_set) + { + return WaitResult(WaitResultKind::Ready, wait_set); + } + + /// Create WaitResult from a "timeout" result. + static + WaitResult + from_timeout_wait_result_kind() + { + return WaitResult(WaitResultKind::Timeout); + } + + /// Create WaitResult from a "empty" result. + static + WaitResult + from_empty_wait_result_kind() + { + return WaitResult(WaitResultKind::Empty); + } + + /// Return the kind of the WaitResult. + WaitResultKind + kind() const + { + return wait_result_kind_; + } + + /// Return the rcl wait set. + const WaitSetT & + get_wait_set() const + { + if (this->kind() != WaitResultKind::Ready) { + throw std::runtime_error("cannot access wait set when the result was not ready"); + } + // This should never happen, defensive (and debug mode) check only. + assert(wait_set_pointer_); + return *wait_set_pointer_; + } + + /// Return the rcl wait set. + WaitSetT & + get_wait_set() + { + if (this->kind() != WaitResultKind::Ready) { + throw std::runtime_error("cannot access wait set when the result was not ready"); + } + // This should never happen, defensive (and debug mode) check only. + assert(wait_set_pointer_); + return *wait_set_pointer_; + } + + WaitResult(WaitResult && other) noexcept + : wait_result_kind_(other.wait_result_kind_), + wait_set_pointer_(std::exchange(other.wait_set_pointer_, nullptr)) + {} + + ~WaitResult() + { + if (wait_set_pointer_) { + wait_set_pointer_->wait_result_release(); + } + } + +private: + RCLCPP_DISABLE_COPY(WaitResult) + + explicit WaitResult(WaitResultKind wait_result_kind) + : wait_result_kind_(wait_result_kind) + { + // Should be enforced by the static factory methods on this class. + assert(WaitResultKind::Ready != wait_result_kind); + } + + WaitResult(WaitResultKind wait_result_kind, WaitSetT & wait_set) + : wait_result_kind_(wait_result_kind), + wait_set_pointer_(&wait_set) + { + // Should be enforced by the static factory methods on this class. + assert(WaitResultKind::Ready == wait_result_kind); + // Secure thread-safety (if provided) and shared ownership (if needed). + wait_set_pointer_->wait_result_acquire(); + } + + const WaitResultKind wait_result_kind_; + + WaitSetT * wait_set_pointer_ = nullptr; +}; + +} // namespace rclcpp + +#endif // RCLCPP__WAIT_RESULT_HPP_ diff --git a/rclcpp/include/rclcpp/wait_result_kind.hpp b/rclcpp/include/rclcpp/wait_result_kind.hpp new file mode 100644 index 0000000000..98ada0a31a --- /dev/null +++ b/rclcpp/include/rclcpp/wait_result_kind.hpp @@ -0,0 +1,31 @@ +// 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_ + +namespace rclcpp +{ + +/// Represents the various kinds of results from waiting on a wait set. +enum WaitResultKind +{ + Ready, // + +#include "rcl/wait.h" + +#include "rclcpp/guard_condition.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rclcpp/wait_set_policies/dynamic_storage.hpp" +#include "rclcpp/wait_set_policies/sequential_synchronization.hpp" +#include "rclcpp/wait_set_policies/static_storage.hpp" +// #include "rclcpp/wait_set_policies/thread_safe_synchronization.hpp" +#include "rclcpp/wait_set_template.hpp" + +namespace rclcpp +{ + +/// Most common user configuration of a WaitSet, which is dynamic but not thread-safe. +/** + * This wait set allows you to add and remove items dynamically, and it will + * automatically remove items that are let out of scope each time wait() or + * prune_destroyed_entities() is called. + * + * It will not, however, provide thread-safety for adding and removing entities + * while waiting. + * + * \sa rclcpp::WaitSetTemplate for API documentation + */ +using WaitSet = rclcpp::WaitSetTemplate< + rclcpp::wait_set_policies::DynamicStorage, + rclcpp::wait_set_policies::SequentialSynchronization +>; + +/// 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 NumberOfEvents, + // std::size_t NumberOfWaitables +> +using StaticWaitSet = rclcpp::WaitSetTemplate< + rclcpp::wait_set_policies::StaticStorage< + // NumberOfSubscriptions, + NumberOfGuardCondtions + // NumberOfTimers, + // NumberOfClients, + // NumberOfServices, + // NumberOfEvents, + // NumberOfWaitables + >, + rclcpp::wait_set_policies::SequentialSynchronization +>; + +/// 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::DynamicStorage, +// rclcpp::wait_set_policies::ThreadSafeSynchronization +// >; + +} // namespace rclcpp + +#endif // RCLCPP__WAIT_SET_HPP_ diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp new file mode 100644 index 0000000000..35f0d427c4 --- /dev/null +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp @@ -0,0 +1,224 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__WAIT_SET_POLICIES__DETAIL__STORAGE_POLICY_COMMON_HPP_ +#define RCLCPP__WAIT_SET_POLICIES__DETAIL__STORAGE_POLICY_COMMON_HPP_ + +#include +#include + +#include "rcl/wait.h" + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/guard_condition.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace wait_set_policies +{ +namespace detail +{ + +/// Common structure for storage policies, which provides rcl wait set access. +template +class StoragePolicyCommon +{ +protected: + template< + // class SubscriptionsIterable, + class GuardConditionsIterable + // class ServicesIterable, + // class ClientsIterable, + // class TimersIterable, + // class EventsIterable, + // class WaitablesIterable + > + explicit + StoragePolicyCommon( + const GuardConditionsIterable & guard_conditions, + 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"); + } + // Initialize wait set using initial inputs. + rcl_ret_t ret = rcl_wait_set_init( + &rcl_wait_set_, + 0, // subs + guard_conditions.size(), + 0, // timers + 0, // clients + 0, // services + 0, // events + 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(guard_conditions); + } + + ~StoragePolicyCommon() + { + rcl_ret_t ret = rcl_wait_set_fini(&rcl_wait_set_); + if (RCL_RET_OK != ret) { + try { + rclcpp::exceptions::throw_from_rcl_error(ret); + } catch (const std::exception & exception) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "Error in destruction of rcl wait set: %s", exception.what()); + } + } + } + + template + std::pair + get_raw_pointer_from_smart_pointer(const std::shared_ptr & shared_pointer) + { + return {nullptr, shared_pointer.get()}; + } + + template + std::pair, EntityT *> + get_raw_pointer_from_smart_pointer(const std::weak_ptr & weak_pointer) + { + auto shared_pointer = weak_pointer.lock(); + return {shared_pointer, shared_pointer.get()}; + } + + /// Rebuild the wait set, preparing it for the next wait call. + /** + * The wait set is rebuilt by: + * + * - resizing the wait set if needed, + * - clearing the wait set if not already done by resizing, and + * - re-adding the entities. + */ + template< + // class SubscriptionsIterable, + class GuardConditionsIterable + // class ServicesIterable, + // class ClientsIterable, + // class TimersIterable, + // class EventsIterable, + // class WaitablesIterable + > + void + storage_rebuild_rcl_wait_set_with_sets( + const GuardConditionsIterable & guard_conditions + ) + { + 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. + rcl_ret_t ret = rcl_wait_set_resize( + &rcl_wait_set_, + 0, // subscriptions_size + guard_conditions.size(), + 0, // timers_size + 0, // clients_size + 0, // services_size + 0 // events_size + ); + 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 guard conditions. + for (const auto & guard_condition : 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_condtion(), + nullptr); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + } + } + + const rcl_wait_set_t & + storage_get_rcl_wait_set() const + { + return rcl_wait_set_; + } + + rcl_wait_set_t & + storage_get_rcl_wait_set() + { + return rcl_wait_set_; + } + + void + storage_flag_for_resize() + { + needs_resize_ = true; + } + + rcl_wait_set_t rcl_wait_set_; + rclcpp::Context::SharedPtr context_; + + bool needs_pruning_ = false; + bool needs_resize_ = false; +}; + +} // namespace detail +} // namespace wait_set_policies +} // namespace rclcpp + +#endif // RCLCPP__WAIT_SET_POLICIES__DETAIL__STORAGE_POLICY_COMMON_HPP_ diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp new file mode 100644 index 0000000000..064433f103 --- /dev/null +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp @@ -0,0 +1,72 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__WAIT_SET_POLICIES__DETAIL__SYNCHRONIZATION_POLICY_COMMON_HPP_ +#define RCLCPP__WAIT_SET_POLICIES__DETAIL__SYNCHRONIZATION_POLICY_COMMON_HPP_ + +#include +#include + +namespace rclcpp +{ +namespace wait_set_policies +{ +namespace detail +{ + +/// Common structure for synchronization policies. +class SynchronizationPolicyCommon +{ +protected: + SynchronizationPolicyCommon() = default; + ~SynchronizationPolicyCommon() = default; + + std::function + create_loop_predicate( + std::chrono::nanoseconds time_to_wait_ns, + std::chrono::steady_clock::time_point start) + { + if (time_to_wait_ns >= std::chrono::nanoseconds(0)) { + // If time_to_wait_ns is >= 0 schedule against a deadline. + auto deadline = start + time_to_wait_ns; + return [deadline]() -> bool { return std::chrono::steady_clock::now() < deadline; }; + } else { + // In the case of time_to_wait_ns < 0, just always return true to loop forever. + return []() -> bool { return true; }; + } + } + + std::chrono::nanoseconds + calculate_time_left_to_wait( + std::chrono::nanoseconds original_time_to_wait_ns, + std::chrono::steady_clock::time_point start) + { + std::chrono::nanoseconds time_left_to_wait; + if (original_time_to_wait_ns < std::chrono::nanoseconds(0)) { + time_left_to_wait = original_time_to_wait_ns; + } else { + time_left_to_wait = original_time_to_wait_ns - (std::chrono::steady_clock::now() - start); + if (time_left_to_wait < std::chrono::nanoseconds(0)) { + time_left_to_wait = std::chrono::nanoseconds(0); + } + } + return time_left_to_wait; + } +}; + +} // namespace detail +} // namespace wait_set_policies +} // namespace rclcpp + +#endif // RCLCPP__WAIT_SET_POLICIES__DETAIL__SYNCHRONIZATION_POLICY_COMMON_HPP_ diff --git a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp new file mode 100644 index 0000000000..f1d37bb321 --- /dev/null +++ b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp @@ -0,0 +1,161 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__WAIT_SET_POLICIES__DYNAMIC_STORAGE_HPP_ +#define RCLCPP__WAIT_SET_POLICIES__DYNAMIC_STORAGE_HPP_ + +#include +#include +#include + +#include "rclcpp/guard_condition.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rclcpp/wait_set_policies/detail/storage_policy_common.hpp" + +namespace rclcpp +{ +namespace wait_set_policies +{ + +/// WaitSet policy that provides dynamically sized storage. +class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCommon +{ +protected: + using is_mutable = std::true_type; + + using SequenceOfWeakGuardConditions = std::vector>; + using GuardConditionsIterable = std::vector>; + + explicit + DynamicStorage( + const GuardConditionsIterable & guard_conditions, + rclcpp::Context::SharedPtr context + ) + : StoragePolicyCommon(guard_conditions, context), + guard_conditions_(guard_conditions.cbegin(), guard_conditions.cend()), + shared_guard_conditions_(guard_conditions_.size()) + {} + + ~DynamicStorage() = default; + + void + storage_rebuild_rcl_wait_set() + { + this->storage_rebuild_rcl_wait_set_with_sets( + guard_conditions_ + ); + } + + bool + storage_has_guard_condition(const rclcpp::GuardCondition & guard_condition) + { + return std::any_of( + guard_conditions_.cbegin(), + guard_conditions_.cend(), + [&guard_condition](const auto & gc) { return &guard_condition == gc.lock().get(); }); + } + + auto + storage_find_guard_condition(const rclcpp::GuardCondition & guard_condition) + { + return std::find_if( + guard_conditions_.begin(), + guard_conditions_.end(), + [&guard_condition](const auto & gc) { return &guard_condition == gc.lock().get(); }); + } + + void + storage_add_guard_condition(std::shared_ptr && guard_condition) + { + if (this->storage_has_guard_condition(*guard_condition)) { + throw std::runtime_error("guard_condition already in wait set"); + } + guard_conditions_.push_back(std::move(guard_condition)); + this->storage_flag_for_resize(); + } + + void + storage_remove_guard_condition(std::shared_ptr && guard_condition) + { + auto it = this->storage_find_guard_condition(*guard_condition); + if (guard_conditions_.cend() == it) { + throw std::runtime_error("guard_condition not in wait set"); + } + guard_conditions_.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)); + } + + 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_); + } + + 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_); + } + + size_t ownership_reference_counter_ = 0; + SequenceOfWeakGuardConditions guard_conditions_; + GuardConditionsIterable shared_guard_conditions_; +}; + +} // namespace wait_set_policies +} // namespace rclcpp + +#endif // RCLCPP__WAIT_SET_POLICIES__DYNAMIC_STORAGE_HPP_ diff --git a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp new file mode 100644 index 0000000000..295bde94ca --- /dev/null +++ b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp @@ -0,0 +1,159 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__WAIT_SET_POLICIES__SEQUENTIAL_SYNCHRONIZATION_HPP_ +#define RCLCPP__WAIT_SET_POLICIES__SEQUENTIAL_SYNCHRONIZATION_HPP_ + +#include +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/guard_condition.hpp" +#include "rclcpp/macros.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" + +namespace rclcpp +{ +namespace wait_set_policies +{ + +/// WaitSet policy that explicitly provides no thread synchronization. +class SequentialSynchronization : public detail::SynchronizationPolicyCommon +{ +protected: + SequentialSynchronization() = default; + ~SequentialSynchronization() = default; + + /// Add guard condition without thread-safety. + /** + * Does not throw, but storage function may throw. + */ + void + sync_add_guard_condition( + std::shared_ptr && guard_condition, + std::function &&)> add_guard_condition_function) + { + // Explicitly no thread synchronization. + add_guard_condition_function(std::move(guard_condition)); + } + + /// Remove guard condition without thread-safety. + /** + * Does not throw, but storage function may throw. + */ + void + sync_remove_guard_condition( + std::shared_ptr && guard_condition, + std::function &&)> remove_guard_condition_function) + { + // Explicitly no thread synchronization. + remove_guard_condition_function(std::move(guard_condition)); + } + + /// Prune deleted entities without thread-safety. + /** + * Does not throw, but storage function may throw. + */ + void + sync_prune_deleted_entities(std::function prune_deleted_entities_function) + { + // Explicitly no thread synchronization. + prune_deleted_entities_function(); + } + + /// Implements wait without any thread-safety. + template + WaitResultT + sync_wait( + std::chrono::nanoseconds time_to_wait_ns, + std::function rebuild_rcl_wait_set, + std::function get_rcl_wait_set, + std::function create_wait_result) + { + // Assumption: this function assumes that some measure has been taken to + // ensure none of the entities being waited on by the wait set are allowed + // to go out of scope and therefore be deleted. + // In the case of the StaticStorage policy, this is ensured because it + // retains shared ownership of all entites for the duration of its own life. + // In the case of the DynamicStorage policy, this is ensured by the function + // which calls this function, by acquiring shared ownership of the entites + // for the duration of this function. + + // Setup looping predicate. + auto start = std::chrono::steady_clock::now(); + std::function should_loop = this->create_loop_predicate(time_to_wait_ns, start); + + // Wait until exit condition is met. + do { + // Rebuild the wait set. + // This will resize the wait set if needed, due to e.g. adding or removing + // entities since the laster wait, but this should never occur in static + // storage wait sets since they cannot be changed after construction. + // This will also clear the wait set and re-add all the entities, which + // prepares it to be waited on again. + rebuild_rcl_wait_set(); + + rcl_wait_set_t & rcl_wait_set = get_rcl_wait_set(); + + // Wait unconditionally until timeout condition occurs since we assume + // there are no conditions that would require the wait to stop and reset, + // like asynchronously adding or removing an entity, i.e. explicitly + // providing no thread-safety. + + // Calculate how much time there is left to wait, unless blocking indefinitely. + auto time_left_to_wait_ns = this->calculate_time_left_to_wait(time_to_wait_ns, start); + + // Then wait for entities to become ready. + rcl_ret_t ret = rcl_wait(&rcl_wait_set, time_left_to_wait_ns.count()); + if (RCL_RET_OK == ret) { + // Something has become ready in the wait set, and since this class + // did not add anything to it, it is a user entity that is ready. + return create_wait_result(WaitResultKind::Ready); + } else if (RCL_RET_TIMEOUT == ret) { + // The wait set timed out, exit the loop. + break; + } else if (RCL_RET_WAIT_SET_EMPTY == ret) { + // Wait set was empty, return Empty. + return create_wait_result(WaitResultKind::Empty); + } else { + // Some other error case, throw. + rclcpp::exceptions::throw_from_rcl_error(ret); + } + } while (should_loop()); + + // Wait did not result in ready items, return timeout. + return create_wait_result(WaitResultKind::Timeout); + } + + void + sync_wait_result_acquire() + { + // Explicitly do nothing. + } + + void + sync_wait_result_release() + { + // Explicitly do nothing. + } +}; + +} // namespace wait_set_policies +} // namespace rclcpp + +#endif // RCLCPP__WAIT_SET_POLICIES__SEQUENTIAL_SYNCHRONIZATION_HPP_ diff --git a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp new file mode 100644 index 0000000000..a4f0cccc11 --- /dev/null +++ b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp @@ -0,0 +1,97 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__WAIT_SET_POLICIES__STATIC_STORAGE_HPP_ +#define RCLCPP__WAIT_SET_POLICIES__STATIC_STORAGE_HPP_ + +#include +#include + +#include "rclcpp/guard_condition.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rclcpp/wait_set_policies/detail/storage_policy_common.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 NumberOfEvents, + // std::size_t NumberOfWaitables +> +class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCommon +{ +protected: + using is_mutable = std::false_type; + + using ArrayOfGuardConditions = std::array< + std::shared_ptr, + NumberOfGuardCondtions + >; + using GuardConditionsIterable = ArrayOfGuardConditions; + + explicit + StaticStorage( + const ArrayOfGuardConditions & guard_conditions, + rclcpp::Context::SharedPtr context + ) + : StoragePolicyCommon(guard_conditions, context), + guard_conditions_(guard_conditions) + {} + + ~StaticStorage() = default; + + void + storage_rebuild_rcl_wait_set() + { + this->storage_rebuild_rcl_wait_set_with_sets( + guard_conditions_ + ); + } + + // storage_add_guard_condition() explicitly not declared here + // storage_remove_guard_condition() 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 ArrayOfGuardConditions guard_conditions_; +}; + +} // namespace wait_set_policies +} // namespace rclcpp + +#endif // RCLCPP__WAIT_SET_POLICIES__STATIC_STORAGE_HPP_ diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp new file mode 100644 index 0000000000..9e430c9eee --- /dev/null +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -0,0 +1,312 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__WAIT_SET_TEMPLATE_HPP_ +#define RCLCPP__WAIT_SET_TEMPLATE_HPP_ + +#include +#include + +#include "rcl/wait.h" + +#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/visibility_control.hpp" +#include "rclcpp/wait_result.hpp" + +namespace rclcpp +{ + +/// Encapsulates sets of waitable items which can be waited on as a group. +/** + * This class uses the rcl_wait_set_t as storage, but it also helps manage the + * ownership of associated rclcpp types. + */ +template +class WaitSetTemplate final : private StoragePolicy, private SynchronizationPolicy +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(WaitSetTemplate) + + /// Construct a wait set with optional initial waitable entities and optional custom context. + /** + * \param[in] guard_conditions Vector of guard conditions 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::GuardConditionsIterable & guard_conditions = {}, + rclcpp::Context::SharedPtr context = + rclcpp::contexts::default_context::get_global_default_context()) + : StoragePolicy(guard_conditions, context), SynchronizationPolicy() + {} + + /// 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. + */ + RCLCPP_PUBLIC + 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 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. + * \throws exceptions based on the policies used. + */ + void + add_guard_condition(std::shared_ptr guard_condition) + { + if (nullptr == guard_condition) { + throw std::invalid_argument("guard_condition is nullptr"); + } + // this method comes from the SynchronizationPolicy + this->sync_add_guard_condition( + std::move(guard_condition), + [this](std::shared_ptr && inner_guard_condition) { + // This method comes from the StoragePolicy, and it may not exist for + // fixed sized storage policies. + // It will throw if the guard condition has already been added. + this->storage_add_guard_condition(std::move(inner_guard_condition)); + }); + } + + /// Remove a guard condition from this wait set. + /** + * Guard condition is removed from the wait set, and if needed the shared + * ownership is released. + * + * This function may be thread-safe depending on the SynchronizationPolicy + * used with this class. + * Using the ThreadSafeWaitSetPolicy will ensure that wait() is interrupted + * and returns before this function removes the guard condition. + * Otherwise, it is not safe to call this function concurrently with wait(). + * + * This function will not be enabled (will not be available) if the + * StoragePolicy does not allow editing of the wait set after initialization. + * + * \param[in] guard_condition Guard condition to be removed. + * \throws std::invalid_argument if guard_condition is nullptr. + * \throws std::runtime_error if guard_condition is not part of the wait set. + * \throws exceptions based on the policies used. + */ + void + remove_guard_condition(std::shared_ptr guard_condition) + { + if (nullptr == guard_condition) { + throw std::invalid_argument("guard_condition is nullptr"); + } + // this method comes from the SynchronizationPolicy + this->sync_remove_guard_condition( + std::move(guard_condition), + [this](std::shared_ptr && inner_guard_condition) { + // 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)); + }); + } + + /// 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::stead_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 waiting, + * introspect 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::RCLErrorBase on unhandled rcl errors + */ + template + RCUTILS_WARN_UNUSED + WaitResult + wait(std::chrono::duration time_to_wait = std::chrono::duration(-1)) + { + auto time_to_wait_ns = std::chrono::duration_cast(time_to_wait); + + // ensure the ownership of the entities in the wait set is shared for the duration of wait + this->storage_acquire_ownerships(); + RCLCPP_SCOPE_EXIT({this->storage_release_ownerships();}); + + // this method comes from the SynchronizationPolicy + return this->template sync_wait>( + // pass along the time_to_wait duration as nanoseconds + time_to_wait_ns, + // this method provides the ability to rebuild the wait set, if needed + [this]() { + // This method comes from the StoragePolicy + this->storage_rebuild_rcl_wait_set(); + }, + // this method provides access to the rcl wait set + [this]() -> rcl_wait_set_t & { + // This method comes from the StoragePolicy + return this->storage_get_rcl_wait_set(); + }, + // this method provides a way to create the WaitResult + [this](WaitResultKind wait_result_kind) -> WaitResult { + // convert the result into a WaitResult + switch (wait_result_kind) { + case WaitResultKind::Ready: + return WaitResult::from_ready_wait_result_kind(*this); + case WaitResultKind::Timeout: + return WaitResult::from_timeout_wait_result_kind(); + case WaitResultKind::Empty: + return WaitResult::from_empty_wait_result_kind(); + default: + auto msg = "unknown WaitResultKind with value: " + std::to_string(wait_result_kind); + throw std::runtime_error(msg); + } + } + ); + } + +private: + // Add WaitResult type as a friend so it can call private methods for + // acquiring and releasing resources as the WaitResult is initialized and + // destructed, respectively. + friend WaitResult; + + /// Called by the WaitResult's constructor to place a hold on ownership and thread-safety. + /** + * Should only be called in pairs with wait_result_release(). + * + * \throws std::runtime_error If called twice before wait_result_release(). + */ + void + wait_result_acquire() + { + if (wait_result_holding_) { + throw std::runtime_error("wait_result_acquire() called while already holding"); + } + wait_result_holding_ = true; + // this method comes from the SynchronizationPolicy + this->sync_wait_result_acquire(); + // this method comes from the StoragePolicy + this->storage_acquire_ownerships(); + } + + /// Called by the WaitResult's destructor to release resources. + /** + * Should only be called if wait_result_acquire() has been called. + * + * \throws std::runtime_error If called before wait_result_acquire(). + */ + void + wait_result_release() + { + if (!wait_result_holding_) { + throw std::runtime_error("wait_result_release() called while not holding"); + } + wait_result_holding_ = false; + // this method comes from the StoragePolicy + this->storage_release_ownerships(); + // this method comes from the SynchronizationPolicy + this->sync_wait_result_release(); + } + + bool wait_result_holding_ = false; +}; + +} // namespace rclcpp + +#endif // RCLCPP__WAIT_SET_TEMPLATE_HPP_ diff --git a/rclcpp/test/test_wait_set.cpp b/rclcpp/test/test_wait_set.cpp new file mode 100644 index 0000000000..b1a1e3921a --- /dev/null +++ b/rclcpp/test/test_wait_set.cpp @@ -0,0 +1,197 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rclcpp/rclcpp.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{}); + (void)wait_set; + } + + { + auto gc = std::make_shared(); + rclcpp::WaitSet wait_set({gc}); + (void)wait_set; + } + + { + auto context = std::make_shared(); + context->init(0, nullptr); + auto gc = std::make_shared(context); + rclcpp::WaitSet wait_set({gc}, context); + (void)wait_set; + } + + { + // invalid context (nullptr) + ASSERT_THROW( + { + rclcpp::WaitSet wait_set(std::vector{}, nullptr); + (void)wait_set; + }, std::invalid_argument); + } + + { + // invalid context (uninitialized) + auto context = std::make_shared(); + ASSERT_THROW( + { + rclcpp::WaitSet wait_set(std::vector{}, context); + (void)wait_set; + }, rclcpp::exceptions::RCLInvalidArgument); + } +} + +/* + * Testing rcl wait set accessor. + */ +TEST_F(TestWaitSet, get_rcl_wait_set) { + { + rclcpp::WaitSet wait_set; + wait_set.get_rcl_wait_set(); + } +} + +/* + * Testing add/remove for guard condition methods. + */ +TEST_F(TestWaitSet, add_remove_guard_condition) { + // normal, mixed initialization + { + auto gc = std::make_shared(); + auto gc2 = std::make_shared(); + rclcpp::WaitSet wait_set({gc}); + wait_set.add_guard_condition(gc2); + wait_set.remove_guard_condition(gc2); + wait_set.remove_guard_condition(gc); + } + + // out of order removal + { + auto gc = std::make_shared(); + auto gc2 = std::make_shared(); + rclcpp::WaitSet wait_set({gc}); + wait_set.add_guard_condition(gc2); + wait_set.remove_guard_condition(gc); + wait_set.remove_guard_condition(gc2); + } + + // start empty, normal + { + auto gc = std::make_shared(); + rclcpp::WaitSet wait_set; + wait_set.add_guard_condition(gc); + wait_set.remove_guard_condition(gc); + } + + // add invalid (nullptr) + { + rclcpp::WaitSet wait_set; + ASSERT_THROW( + { + wait_set.add_guard_condition(nullptr); + }, std::invalid_argument); + } + + // double add + { + auto gc = std::make_shared(); + rclcpp::WaitSet wait_set; + wait_set.add_guard_condition(gc); + ASSERT_THROW( + { + wait_set.add_guard_condition(gc); + }, std::runtime_error); + } + + // remove invalid (nullptr) + { + rclcpp::WaitSet wait_set; + ASSERT_THROW( + { + wait_set.remove_guard_condition(nullptr); + }, std::invalid_argument); + } + + // remove unrelated + { + auto gc = std::make_shared(); + auto gc2 = std::make_shared(); + rclcpp::WaitSet wait_set({gc}); + ASSERT_THROW( + { + wait_set.remove_guard_condition(gc2); + }, std::runtime_error); + } + + // double remove + { + auto gc = std::make_shared(); + rclcpp::WaitSet wait_set({gc}); + wait_set.remove_guard_condition(gc); + ASSERT_THROW( + { + wait_set.remove_guard_condition(gc); + }, std::runtime_error); + } + + // remove from empty + { + auto gc = std::make_shared(); + rclcpp::WaitSet wait_set; + ASSERT_THROW( + { + wait_set.remove_guard_condition(gc); + }, std::runtime_error); + } + + // remove after delete, checking weak ownership behavior + // { + // auto gc = std::make_shared(); + // rclcpp::WaitSet wait_set; + // wait_set.add_guard_condition(gc); + // gc.reset(); + // ASSERT_THROW( + // { + // // gc should be missing at this point + // wait_set.remove_guard_condition(gc); + // }, std::runtime_error); + // } + // Note this case does not fail because you cannot pass a "reset" shared pointer to gc + // and expect it to try and find the original pointer. + // Instead it throws due to gc being nullptr. +} From 2f0ca89ac9922681f37de8b91e877ed2ae0f6d17 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 6 Apr 2020 09:01:59 -0700 Subject: [PATCH 03/21] fix typo Signed-off-by: William Woodall --- rclcpp/include/rclcpp/guard_condition.hpp | 2 +- .../rclcpp/wait_set_policies/detail/storage_policy_common.hpp | 2 +- rclcpp/src/rclcpp/guard_condition.cpp | 2 +- rclcpp/test/test_guard_condition.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rclcpp/include/rclcpp/guard_condition.hpp b/rclcpp/include/rclcpp/guard_condition.hpp index 77011166f3..29c0f23e7a 100644 --- a/rclcpp/include/rclcpp/guard_condition.hpp +++ b/rclcpp/include/rclcpp/guard_condition.hpp @@ -59,7 +59,7 @@ class GuardCondition /// Return the underlying rcl guard condition structure. RCLCPP_PUBLIC const rcl_guard_condition_t & - get_rcl_guard_condtion() const; + get_rcl_guard_condition() const; /// Notify the wait set waiting on this condition, if any, that the condition had been met. /** diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp index 35f0d427c4..11e4853e79 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp @@ -184,7 +184,7 @@ class StoragePolicyCommon } rcl_ret_t ret = rcl_wait_set_add_guard_condition( &rcl_wait_set_, - &guard_condition_ptr_pair.second->get_rcl_guard_condtion(), + &guard_condition_ptr_pair.second->get_rcl_guard_condition(), nullptr); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); diff --git a/rclcpp/src/rclcpp/guard_condition.cpp b/rclcpp/src/rclcpp/guard_condition.cpp index 51accdc1bc..20b3719e76 100644 --- a/rclcpp/src/rclcpp/guard_condition.cpp +++ b/rclcpp/src/rclcpp/guard_condition.cpp @@ -57,7 +57,7 @@ GuardCondition::get_context() const } const rcl_guard_condition_t & -GuardCondition::get_rcl_guard_condtion() const +GuardCondition::get_rcl_guard_condition() const { return rcl_guard_condition_; } diff --git a/rclcpp/test/test_guard_condition.cpp b/rclcpp/test/test_guard_condition.cpp index fa80f18678..474d1d4bcc 100644 --- a/rclcpp/test/test_guard_condition.cpp +++ b/rclcpp/test/test_guard_condition.cpp @@ -72,7 +72,7 @@ TEST_F(TestGuardCondition, get_context) { TEST_F(TestGuardCondition, get_rcl_guard_condition) { { auto gc = std::make_shared(); - gc->get_rcl_guard_condtion(); + gc->get_rcl_guard_condition(); } } From 34679c37381859916b8092dda9abc22b4aa570ff Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 6 Apr 2020 09:02:22 -0700 Subject: [PATCH 04/21] removing a question/todo, I think this is fine as is Signed-off-by: William Woodall --- rclcpp/include/rclcpp/waitable.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 2837d5a16d..5f79079a4e 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -94,7 +94,6 @@ class Waitable size_t get_number_of_ready_guard_conditions(); - // TODO(jacobperron): smart pointer? /// Add the Waitable to a wait set. /** * \param[in] wait_set A handle to the wait set to add the Waitable to. From 637b6eeaf48424a4c06ba39839faad0468a03aab Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 6 Apr 2020 09:38:08 -0700 Subject: [PATCH 05/21] added subscriptions and waitable to wait sets Signed-off-by: William Woodall --- .../rclcpp/subscription_wait_set_mask.hpp | 35 +++ rclcpp/include/rclcpp/wait_set.hpp | 18 +- .../detail/storage_policy_common.hpp | 168 +++++++++-- .../wait_set_policies/dynamic_storage.hpp | 186 +++++++++++- .../sequential_synchronization.hpp | 91 ++++++ .../wait_set_policies/static_storage.hpp | 59 +++- rclcpp/include/rclcpp/wait_set_template.hpp | 265 +++++++++++++++++- rclcpp/test/test_wait_set.cpp | 32 ++- 8 files changed, 785 insertions(+), 69 deletions(-) create mode 100644 rclcpp/include/rclcpp/subscription_wait_set_mask.hpp diff --git a/rclcpp/include/rclcpp/subscription_wait_set_mask.hpp b/rclcpp/include/rclcpp/subscription_wait_set_mask.hpp new file mode 100644 index 0000000000..694c312cb8 --- /dev/null +++ b/rclcpp/include/rclcpp/subscription_wait_set_mask.hpp @@ -0,0 +1,35 @@ +// 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_ + +namespace rclcpp +{ + +/// Options used to determine what parts of a subscription get added to or removed from a wait set. +class SubscriptionWaitSetMask +{ +public: + /// If true, include the actual subscription. + bool include_subscription = true; + /// If true, include any events attached to the subscription. + bool include_events = true; + /// If true, include the waitable used to handle intra process communication. + bool include_intra_process_waitable = true; +}; + +} // namespace rclcpp + +#endif // RCLCPP__SUBSCRIPTION_WAIT_SET_MASK_HPP_ diff --git a/rclcpp/include/rclcpp/wait_set.hpp b/rclcpp/include/rclcpp/wait_set.hpp index 17c07bd592..cc0894fa6f 100644 --- a/rclcpp/include/rclcpp/wait_set.hpp +++ b/rclcpp/include/rclcpp/wait_set.hpp @@ -63,23 +63,21 @@ using WaitSet = rclcpp::WaitSetTemplate< * \sa rclcpp::WaitSetTemplate for API documentation */ template< - // std::size_t NumberOfSubscriptions, - std::size_t NumberOfGuardCondtions - // std::size_t NumberOfTimers, + std::size_t NumberOfSubscriptions, + std::size_t NumberOfGuardCondtions, + std::size_t NumberOfTimers, // std::size_t NumberOfClients, // std::size_t NumberOfServices, - // std::size_t NumberOfEvents, - // std::size_t NumberOfWaitables + std::size_t NumberOfWaitables > using StaticWaitSet = rclcpp::WaitSetTemplate< rclcpp::wait_set_policies::StaticStorage< - // NumberOfSubscriptions, - NumberOfGuardCondtions - // NumberOfTimers, + NumberOfSubscriptions, + NumberOfGuardCondtions, + NumberOfTimers, // NumberOfClients, // NumberOfServices, - // NumberOfEvents, - // NumberOfWaitables + NumberOfWaitables >, rclcpp::wait_set_policies::SequentialSynchronization >; diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp index 11e4853e79..cfcbbe9304 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp @@ -15,16 +15,17 @@ #ifndef RCLCPP__WAIT_SET_POLICIES__DETAIL__STORAGE_POLICY_COMMON_HPP_ #define RCLCPP__WAIT_SET_POLICIES__DETAIL__STORAGE_POLICY_COMMON_HPP_ -#include #include +#include +#include #include "rcl/wait.h" #include "rclcpp/exceptions.hpp" -#include "rclcpp/guard_condition.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/visibility_control.hpp" +#include "rclcpp/waitable.hpp" namespace rclcpp { @@ -39,17 +40,19 @@ class StoragePolicyCommon { protected: template< - // class SubscriptionsIterable, - class GuardConditionsIterable + class SubscriptionsIterable, + class GuardConditionsIterable, + class TimersIterable, // class ServicesIterable, // class ClientsIterable, - // class TimersIterable, - // class EventsIterable, - // class WaitablesIterable + class WaitablesIterable > explicit StoragePolicyCommon( + const SubscriptionsIterable & subscriptions, const GuardConditionsIterable & guard_conditions, + const TimersIterable & timers, + const WaitablesIterable & waitables, rclcpp::Context::SharedPtr context ) : rcl_wait_set_(rcl_get_zero_initialized_wait_set()), context_(context) @@ -58,15 +61,31 @@ class StoragePolicyCommon 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_, - 0, // subs - guard_conditions.size(), - 0, // timers - 0, // clients - 0, // services - 0, // events + subscriptions.size() + subscriptions_from_waitables, + guard_conditions.size() + guard_conditions_from_waitables, + timers.size() + timers_from_waitables, + clients_from_waitables, + 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()); @@ -75,7 +94,7 @@ class StoragePolicyCommon } // (Re)build the wait set for the first time. - this->storage_rebuild_rcl_wait_set_with_sets(guard_conditions); + this->storage_rebuild_rcl_wait_set_with_sets(subscriptions, guard_conditions, timers, waitables); } ~StoragePolicyCommon() @@ -116,17 +135,19 @@ class StoragePolicyCommon * - re-adding the entities. */ template< - // class SubscriptionsIterable, - class GuardConditionsIterable + class SubscriptionsIterable, + class GuardConditionsIterable, + class TimersIterable, // class ServicesIterable, // class ClientsIterable, - // class TimersIterable, - // class EventsIterable, - // class WaitablesIterable + class WaitablesIterable > void storage_rebuild_rcl_wait_set_with_sets( - const GuardConditionsIterable & guard_conditions + const SubscriptionsIterable & subscriptions, + const GuardConditionsIterable & guard_conditions, + const TimersIterable & timers, + const WaitablesIterable & waitables ) { bool was_resized = false; @@ -138,14 +159,42 @@ class StoragePolicyCommon // 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_, - 0, // subscriptions_size - guard_conditions.size(), - 0, // timers_size - 0, // clients_size - 0, // services_size - 0 // events_size + subscriptions.size() + subscriptions_from_waitables, + guard_conditions.size() + guard_conditions_from_waitables, + timers.size() + timers_from_waitables, + clients_from_waitables, + services_from_waitables, + events_from_waitables ); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); @@ -168,6 +217,29 @@ class StoragePolicyCommon } } + // Add subscriptions. + for (const auto & subscription : subscriptions) { + auto subscription_ptr_pair = get_raw_pointer_from_smart_pointer(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); + } + } + // Add guard conditions. for (const auto & guard_condition : guard_conditions) { auto guard_condition_ptr_pair = get_raw_pointer_from_smart_pointer(guard_condition); @@ -190,6 +262,50 @@ class StoragePolicyCommon rclcpp::exceptions::throw_from_rcl_error(ret); } } + + // 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 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 & diff --git a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp index f1d37bb321..adf6e96abb 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp @@ -21,8 +21,11 @@ #include "rclcpp/guard_condition.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/subscription_base.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 { @@ -35,17 +38,76 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo protected: using is_mutable = std::true_type; + using SequenceOfWeakSubscriptions = std::vector>; + using SubscriptionsIterable = std::vector>; + using SequenceOfWeakGuardConditions = std::vector>; using GuardConditionsIterable = std::vector>; + using SequenceOfWeakTimers = std::vector>; + using TimersIterable = std::vector>; + + struct WaitableEntry + { + void + reset() noexcept + { + waitable.reset(); + associated_entity.reset(); + } + + std::shared_ptr waitable; + std::shared_ptr associated_entity; + }; + struct WeakWaitableEntry + { + WeakWaitableEntry( + const std::shared_ptr & waitable_in, + const std::shared_ptr & associated_entity_in) noexcept + : waitable(waitable_in), + associated_entity(associated_entity_in) + {} + + explicit WeakWaitableEntry(const WaitableEntry & other) + : waitable(other.waitable), + associated_entity(other.associated_entity) + {} + + std::shared_ptr + lock() const + { + return waitable.lock(); + } + + bool + expired() const noexcept + { + return waitable.expired(); + } + + std::weak_ptr waitable; + std::weak_ptr associated_entity; + }; + using SequenceOfWeakWaitables = std::vector; + using WaitablesIterable = std::vector; + explicit DynamicStorage( + const SubscriptionsIterable & subscriptions, const GuardConditionsIterable & guard_conditions, + const TimersIterable & timers, + const WaitablesIterable & waitables, rclcpp::Context::SharedPtr context ) - : StoragePolicyCommon(guard_conditions, context), + : StoragePolicyCommon(subscriptions, guard_conditions, timers, 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()) + shared_guard_conditions_(guard_conditions_.size()), + timers_(timers.cbegin(), timers.cend()), + shared_timers_(timers_.size()), + waitables_(waitables.cbegin(), waitables.cend()), + shared_waitables_(waitables_.size()) {} ~DynamicStorage() = default; @@ -54,32 +116,60 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo storage_rebuild_rcl_wait_set() { this->storage_rebuild_rcl_wait_set_with_sets( - guard_conditions_ + subscriptions_, + guard_conditions_, + timers_, + waitables_ ); } + template + static bool - storage_has_guard_condition(const rclcpp::GuardCondition & guard_condition) + storage_has_entity(const EntityT & entity, const SequenceOfEntitiesT & entities) { return std::any_of( - guard_conditions_.cbegin(), - guard_conditions_.cend(), - [&guard_condition](const auto & gc) { return &guard_condition == gc.lock().get(); }); + entities.cbegin(), + entities.cend(), + [&entity](const auto & inner) { return &entity == inner.lock().get(); }); } + template + static auto - storage_find_guard_condition(const rclcpp::GuardCondition & guard_condition) + storage_find_entity(const EntityT & entity, const SequenceOfEntitiesT & entities) { return std::find_if( - guard_conditions_.begin(), - guard_conditions_.end(), - [&guard_condition](const auto & gc) { return &guard_condition == gc.lock().get(); }); + entities.cbegin(), + entities.cend(), + [&entity](const auto & inner) { return &entity == inner.lock().get(); }); + } + + void + storage_add_subscription(std::shared_ptr && subscription) + { + if (this->storage_has_entity(*subscription, subscriptions_)) { + throw std::runtime_error("subscription already in wait set"); + } + subscriptions_.push_back(std::move(subscription)); + this->storage_flag_for_resize(); + } + + void + storage_remove_subscription(std::shared_ptr && subscription) + { + auto it = this->storage_find_entity(*subscription, subscriptions_); + if (subscriptions_.cend() == it) { + throw std::runtime_error("subscription not in wait set"); + } + subscriptions_.erase(it); + this->storage_flag_for_resize(); } void storage_add_guard_condition(std::shared_ptr && guard_condition) { - if (this->storage_has_guard_condition(*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)); @@ -89,7 +179,7 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo void storage_remove_guard_condition(std::shared_ptr && guard_condition) { - auto it = this->storage_find_guard_condition(*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"); } @@ -97,6 +187,51 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo this->storage_flag_for_resize(); } + void + storage_add_timer(std::shared_ptr && timer) + { + if (this->storage_has_entity(*timer, timers_)) { + throw std::runtime_error("timer already in wait set"); + } + timers_.push_back(std::move(timer)); + this->storage_flag_for_resize(); + } + + void + storage_remove_timer(std::shared_ptr && timer) + { + auto it = this->storage_find_entity(*timer, timers_); + if (timers_.cend() == it) { + throw std::runtime_error("timer not in wait set"); + } + timers_.erase(it); + this->storage_flag_for_resize(); + } + + void + storage_add_waitable( + std::shared_ptr && waitable, + std::shared_ptr && associated_entity) + { + if (this->storage_has_entity(*waitable, waitables_)) { + throw std::runtime_error("waitable already in wait set"); + } + WeakWaitableEntry weak_entry(std::move(waitable), std::move(associated_entity)); + waitables_.push_back(std::move(weak_entry)); + this->storage_flag_for_resize(); + } + + void + storage_remove_waitable(std::shared_ptr && waitable) + { + auto it = this->storage_find_entity(*waitable, waitables_); + if (waitables_.cend() == it) { + throw std::runtime_error("waitable not in wait set"); + } + waitables_.erase(it); + this->storage_flag_for_resize(); + } + // this is noexcept because: // - std::weak_ptr::expired is noexcept // - the erase-remove idiom is noexcept, since we're not using the ExecutionPolicy version @@ -113,6 +248,8 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo }; // 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)); + waitables_.erase(std::remove_if(waitables_.begin(), waitables_.end(), p)); } void @@ -132,6 +269,17 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo }; // Lock all the weak pointers and hold them until released. lock_all(guard_conditions_, shared_guard_conditions_); + lock_all(timers_, shared_timers_); + + // 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++] = {weak_ptr.waitable.lock(), weak_ptr.associated_entity.lock()}; + } + }; + lock_all_waitables(waitables_, shared_waitables_); } void @@ -148,11 +296,23 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo } }; reset_all(shared_guard_conditions_); + reset_all(shared_timers_); + 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_; + + SequenceOfWeakWaitables waitables_; + WaitablesIterable shared_waitables_; }; } // namespace wait_set_policies diff --git a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp index 295bde94ca..36873755b1 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp @@ -22,10 +22,14 @@ #include "rclcpp/exceptions.hpp" #include "rclcpp/guard_condition.hpp" #include "rclcpp/macros.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 { @@ -39,6 +43,38 @@ class SequentialSynchronization : public detail::SynchronizationPolicyCommon SequentialSynchronization() = default; ~SequentialSynchronization() = default; + /// Add subscription without thread-safety. + /** + * Does not throw, but storage function may throw. + */ + void + sync_add_subscription( + std::shared_ptr && subscription, + const rclcpp::SubscriptionWaitSetMask & mask, + std::function< + void (std::shared_ptr &&, const rclcpp::SubscriptionWaitSetMask &) + > add_subscription_function) + { + // Explicitly no thread synchronization. + add_subscription_function(std::move(subscription), mask); + } + + /// Remove guard condition without thread-safety. + /** + * Does not throw, but storage function may throw. + */ + void + sync_remove_subscription( + std::shared_ptr && subscription, + const rclcpp::SubscriptionWaitSetMask & mask, + std::function< + void (std::shared_ptr &&, const rclcpp::SubscriptionWaitSetMask &) + > remove_subscription_function) + { + // Explicitly no thread synchronization. + remove_subscription_function(std::move(subscription), mask); + } + /// Add guard condition without thread-safety. /** * Does not throw, but storage function may throw. @@ -65,6 +101,61 @@ class SequentialSynchronization : public detail::SynchronizationPolicyCommon remove_guard_condition_function(std::move(guard_condition)); } + /// Add timer without thread-safety. + /** + * Does not throw, but storage function may throw. + */ + void + sync_add_timer( + std::shared_ptr && timer, + std::function &&)> add_timer_function) + { + // Explicitly no thread synchronization. + add_timer_function(std::move(timer)); + } + + /// Remove timer without thread-safety. + /** + * Does not throw, but storage function may throw. + */ + void + sync_remove_timer( + std::shared_ptr && timer, + std::function &&)> remove_timer_function) + { + // Explicitly no thread synchronization. + remove_timer_function(std::move(timer)); + } + + /// Add waitable without thread-safety. + /** + * Does not throw, but storage function may throw. + */ + void + sync_add_waitable( + std::shared_ptr && waitable, + std::shared_ptr && associated_entity, + std::function< + void (std::shared_ptr &&, std::shared_ptr &&) + > add_waitable_function) + { + // Explicitly no thread synchronization. + add_waitable_function(std::move(waitable), std::move(associated_entity)); + } + + /// Remove waitable without thread-safety. + /** + * Does not throw, but storage function may throw. + */ + void + sync_remove_waitable( + std::shared_ptr && waitable, + std::function &&)> remove_waitable_function) + { + // Explicitly no thread synchronization. + remove_waitable_function(std::move(waitable)); + } + /// Prune deleted entities without thread-safety. /** * Does not throw, but storage function may throw. diff --git a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp index a4f0cccc11..4bdc985ca7 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp @@ -20,8 +20,11 @@ #include "rclcpp/guard_condition.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/subscription_base.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 { @@ -34,32 +37,60 @@ namespace wait_set_policies * once during construction, and deallocated once during destruction. */ template< - // std::size_t NumberOfSubscriptions, - std::size_t NumberOfGuardCondtions - // std::size_t NumberOfTimers, + std::size_t NumberOfSubscriptions, + std::size_t NumberOfGuardCondtions, + std::size_t NumberOfTimers, // std::size_t NumberOfClients, // std::size_t NumberOfServices, - // std::size_t NumberOfEvents, - // std::size_t NumberOfWaitables + std::size_t NumberOfWaitables > class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCommon { protected: using is_mutable = std::false_type; + using ArrayOfSubscriptions = std::array< + std::shared_ptr, + NumberOfSubscriptions + >; + using SubscriptionsIterable = ArrayOfSubscriptions; + using ArrayOfGuardConditions = std::array< std::shared_ptr, NumberOfGuardCondtions >; using GuardConditionsIterable = ArrayOfGuardConditions; + using ArrayOfTimers = std::array< + std::shared_ptr, + NumberOfTimers + >; + using TimersIterable = ArrayOfTimers; + + struct WaitableEntry + { + std::shared_ptr waitable; + std::shared_ptr associated_entity; + }; + using ArrayOfWaitables = std::array< + WaitableEntry, + NumberOfWaitables + >; + using WaitablesIterable = ArrayOfWaitables; + explicit StaticStorage( + const ArrayOfSubscriptions & subscriptions, const ArrayOfGuardConditions & guard_conditions, + const ArrayOfTimers & timers, + const ArrayOfWaitables & waitables, rclcpp::Context::SharedPtr context ) - : StoragePolicyCommon(guard_conditions, context), - guard_conditions_(guard_conditions) + : StoragePolicyCommon(subscriptions, guard_conditions, timers, waitables, context), + subscriptions_(subscriptions), + guard_conditions_(guard_conditions), + timers_(timers), + waitables_(waitables) {} ~StaticStorage() = default; @@ -68,12 +99,21 @@ class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCom storage_rebuild_rcl_wait_set() { this->storage_rebuild_rcl_wait_set_with_sets( - guard_conditions_ + subscriptions_, + guard_conditions_, + timers_, + 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_waitable() explicitly not declared here + // storage_remove_waitable() explicitly not declared here // storage_prune_deleted_entities() explicitly not declared here void @@ -88,7 +128,10 @@ class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCom // Explicitly do nothing. } + const ArrayOfSubscriptions subscriptions_; const ArrayOfGuardConditions guard_conditions_; + const ArrayOfTimers timers_; + const ArrayOfWaitables waitables_; }; } // namespace wait_set_policies diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp index 9e430c9eee..771ed04f30 100644 --- a/rclcpp/include/rclcpp/wait_set_template.hpp +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -25,8 +25,12 @@ #include "rclcpp/guard_condition.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/scope_exit.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 { @@ -42,18 +46,37 @@ class WaitSetTemplate final : private StoragePolicy, private SynchronizationPoli public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(WaitSetTemplate) + 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( + // TODO(wjwwood): support subscription wait set masks in constructor + const typename StoragePolicy::SubscriptionsIterable & subscriptions = {}, const typename StoragePolicy::GuardConditionsIterable & guard_conditions = {}, + const typename StoragePolicy::TimersIterable & timers = {}, + const typename StoragePolicy::WaitablesIterable & waitables = {}, rclcpp::Context::SharedPtr context = rclcpp::contexts::default_context::get_global_default_context()) - : StoragePolicy(guard_conditions, context), SynchronizationPolicy() + : StoragePolicy( + subscriptions, + guard_conditions, + timers, + waitables, + context), + SynchronizationPolicy() {} /// Return the internal rcl wait set object. @@ -70,6 +93,121 @@ class WaitSetTemplate final : private StoragePolicy, private SynchronizationPoli 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. + * \throws exceptions based on the policies used. + */ + void + add_subscription( + std::shared_ptr subscription, + rclcpp::SubscriptionWaitSetMask mask = {}) + { + if (nullptr == subscription) { + throw std::invalid_argument("subscription is nullptr"); + } + // this method comes from the SynchronizationPolicy + this->sync_add_subscription( + std::move(subscription), + mask, + [this]( + std::shared_ptr && inner_subscription, + const rclcpp::SubscriptionWaitSetMask & mask) + { + // These methods comes from the StoragePolicy, and may not exist for + // fixed sized storage policies. + // It will throw if the subscription has already been added. + if (mask.include_subscription) { + auto local_subscription = inner_subscription; + 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; + this->storage_add_waitable(std::move(event), std::move(local_subscription)); + } + } + if (mask.include_intra_process_waitable) { + auto local_subscription = inner_subscription; + this->storage_add_waitable( + std::move(inner_subscription->get_intra_process_waitable()), + std::move(local_subscription)); + } + }); + } + + /// Remove a subscription from this wait set. + /** + * \sa remove_guard_condition() for details of how this method works. + * + * Additionally to the documentation for add_guard_condition, this method + * has a mask parameter which allows you to control which parts of the + * subscription is added to the wait set with this call. + * You may remove items selectively from the wait set in a different order + * than they were added. + * + * \param[in] subscription Subscription to be removed. + * \param[in] mask A class which controls which parts of the subscription to remove. + * \throws std::invalid_argument if subscription is nullptr. + * \throws std::runtime_error if subscription is not part of the wait set. + * \throws exceptions based on the policies used. + */ + void + remove_subscription( + std::shared_ptr subscription, + rclcpp::SubscriptionWaitSetMask mask = {}) + { + if (nullptr == subscription) { + throw std::invalid_argument("subscription is nullptr"); + } + // this method comes from the SynchronizationPolicy + this->sync_remove_subscription( + std::move(subscription), + mask, + [this]( + std::shared_ptr && inner_subscription, + const rclcpp::SubscriptionWaitSetMask & mask) + { + // This method comes from the StoragePolicy, and it may not exist for + // fixed sized storage policies. + // It will throw if the subscription is not in the wait set. + if (mask.include_subscription) { + auto local_subscription = inner_subscription; + 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; + this->storage_remove_waitable(std::move(event)); + } + } + if (mask.include_intra_process_waitable) { + auto local_waitable = inner_subscription->get_intra_process_waitable(); + 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 @@ -149,6 +287,127 @@ class WaitSetTemplate final : private StoragePolicy, private SynchronizationPoli }); } + /// 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. + * \throws exceptions based on the policies used. + */ + void + add_timer(std::shared_ptr timer) + { + if (nullptr == timer) { + throw std::invalid_argument("timer is nullptr"); + } + // this method comes from the SynchronizationPolicy + this->sync_add_timer( + std::move(timer), + [this](std::shared_ptr && inner_timer) { + // This method comes from the StoragePolicy, and it may not exist for + // fixed sized storage policies. + // It will throw if the timer has already been added. + this->storage_add_timer(std::move(inner_timer)); + }); + } + + /// Remove a timer from this wait set. + /** + * \sa remove_guard_condition() for details of how this method works. + * + * \param[in] timer Timer to be removed. + * \throws std::invalid_argument if timer is nullptr. + * \throws std::runtime_error if timer is not part of the wait set. + * \throws exceptions based on the policies used. + */ + void + remove_timer(std::shared_ptr timer) + { + if (nullptr == timer) { + throw std::invalid_argument("timer is nullptr"); + } + // this method comes from the SynchronizationPolicy + this->sync_remove_timer( + std::move(timer), + [this](std::shared_ptr && inner_timer) { + // 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 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. + * \throws exceptions based on the policies used. + */ + void + add_waitable( + std::shared_ptr waitable, + std::shared_ptr associated_entity = nullptr) + { + if (nullptr == waitable) { + throw std::invalid_argument("waitable is nullptr"); + } + // this method comes from the SynchronizationPolicy + this->sync_add_waitable( + std::move(waitable), + std::move(associated_entity), + [this](std::shared_ptr && inner_waitable) { + // 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)); + }); + } + + /// Remove a waitable from this wait set. + /** + * \sa remove_guard_condition() for details of how this method works. + * + * \param[in] waitable Waitable to be removed. + * \throws std::invalid_argument if waitable is nullptr. + * \throws std::runtime_error if waitable is not part of the wait set. + * \throws exceptions based on the policies used. + */ + void + remove_waitable(std::shared_ptr waitable) + { + if (nullptr == waitable) { + throw std::invalid_argument("waitable is nullptr"); + } + // this method comes from the SynchronizationPolicy + this->sync_remove_waitable( + std::move(waitable), + [this](std::shared_ptr && inner_waitable) { + // 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 @@ -196,8 +455,8 @@ class WaitSetTemplate final : private StoragePolicy, private SynchronizationPoli * * 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 waiting, - * introspect again. + * 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 diff --git a/rclcpp/test/test_wait_set.cpp b/rclcpp/test/test_wait_set.cpp index b1a1e3921a..dfec37cd07 100644 --- a/rclcpp/test/test_wait_set.cpp +++ b/rclcpp/test/test_wait_set.cpp @@ -38,13 +38,17 @@ TEST_F(TestWaitSet, construction_and_destruction) { } { - rclcpp::WaitSet wait_set(std::vector{}); + rclcpp::WaitSet wait_set( + std::vector{}, + std::vector{}, + std::vector{}, + std::vector{}); (void)wait_set; } { auto gc = std::make_shared(); - rclcpp::WaitSet wait_set({gc}); + rclcpp::WaitSet wait_set({}, {gc}); (void)wait_set; } @@ -52,7 +56,7 @@ TEST_F(TestWaitSet, construction_and_destruction) { auto context = std::make_shared(); context->init(0, nullptr); auto gc = std::make_shared(context); - rclcpp::WaitSet wait_set({gc}, context); + rclcpp::WaitSet wait_set({}, {gc}, {}, {}, context); (void)wait_set; } @@ -60,7 +64,12 @@ TEST_F(TestWaitSet, construction_and_destruction) { // invalid context (nullptr) ASSERT_THROW( { - rclcpp::WaitSet wait_set(std::vector{}, nullptr); + rclcpp::WaitSet wait_set( + std::vector{}, + std::vector{}, + std::vector{}, + std::vector{}, + nullptr); (void)wait_set; }, std::invalid_argument); } @@ -70,7 +79,12 @@ TEST_F(TestWaitSet, construction_and_destruction) { auto context = std::make_shared(); ASSERT_THROW( { - rclcpp::WaitSet wait_set(std::vector{}, context); + rclcpp::WaitSet wait_set( + std::vector{}, + std::vector{}, + std::vector{}, + std::vector{}, + context); (void)wait_set; }, rclcpp::exceptions::RCLInvalidArgument); } @@ -94,7 +108,7 @@ TEST_F(TestWaitSet, add_remove_guard_condition) { { auto gc = std::make_shared(); auto gc2 = std::make_shared(); - rclcpp::WaitSet wait_set({gc}); + rclcpp::WaitSet wait_set({}, {gc}); wait_set.add_guard_condition(gc2); wait_set.remove_guard_condition(gc2); wait_set.remove_guard_condition(gc); @@ -104,7 +118,7 @@ TEST_F(TestWaitSet, add_remove_guard_condition) { { auto gc = std::make_shared(); auto gc2 = std::make_shared(); - rclcpp::WaitSet wait_set({gc}); + rclcpp::WaitSet wait_set({}, {gc}); wait_set.add_guard_condition(gc2); wait_set.remove_guard_condition(gc); wait_set.remove_guard_condition(gc2); @@ -151,7 +165,7 @@ TEST_F(TestWaitSet, add_remove_guard_condition) { { auto gc = std::make_shared(); auto gc2 = std::make_shared(); - rclcpp::WaitSet wait_set({gc}); + rclcpp::WaitSet wait_set({}, {gc}); ASSERT_THROW( { wait_set.remove_guard_condition(gc2); @@ -161,7 +175,7 @@ TEST_F(TestWaitSet, add_remove_guard_condition) { // double remove { auto gc = std::make_shared(); - rclcpp::WaitSet wait_set({gc}); + rclcpp::WaitSet wait_set({}, {gc}); wait_set.remove_guard_condition(gc); ASSERT_THROW( { From 51835e413ab0aca9b579c39e805c2d9b4fc9047f Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 7 Apr 2020 05:44:07 -0700 Subject: [PATCH 06/21] improve usability with subscriptions and wait sets Signed-off-by: William Woodall --- .../detail/storage_policy_common.hpp | 5 +- .../wait_set_policies/dynamic_storage.hpp | 90 ++++++++++++++++--- .../wait_set_policies/static_storage.hpp | 25 +++++- rclcpp/include/rclcpp/wait_set_template.hpp | 1 + rclcpp/test/test_wait_set.cpp | 6 +- 5 files changed, 108 insertions(+), 19 deletions(-) diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp index cfcbbe9304..66139b4197 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp @@ -218,8 +218,9 @@ class StoragePolicyCommon } // Add subscriptions. - for (const auto & subscription : subscriptions) { - auto subscription_ptr_pair = get_raw_pointer_from_smart_pointer(subscription); + 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) { diff --git a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp index adf6e96abb..4154ffdcdd 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp @@ -22,6 +22,7 @@ #include "rclcpp/guard_condition.hpp" #include "rclcpp/macros.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" @@ -38,8 +39,58 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo protected: using is_mutable = std::true_type; - using SequenceOfWeakSubscriptions = std::vector>; - using SubscriptionsIterable = std::vector>; + class SubscriptionEntry + { + public: + std::shared_ptr subscription; + rclcpp::SubscriptionWaitSetMask mask; + + /// Conversion constructor, which is intentionally not marked explicit. + SubscriptionEntry( + const std::shared_ptr & subscription_in = nullptr, + const rclcpp::SubscriptionWaitSetMask & mask_in = {}) + : subscription(subscription_in), + mask(mask_in) + {} + + void + reset() noexcept + { + subscription.reset(); + } + }; + class WeakSubscriptionEntry + { + public: + std::weak_ptr subscription; + rclcpp::SubscriptionWaitSetMask mask; + + explicit WeakSubscriptionEntry( + const std::shared_ptr & subscription_in, + const rclcpp::SubscriptionWaitSetMask & mask_in) noexcept + : subscription(subscription_in), + mask(mask_in) + {} + + explicit WeakSubscriptionEntry(const SubscriptionEntry & other) + : subscription(other.subscription), + mask(other.mask) + {} + + std::shared_ptr + lock() const + { + return subscription.lock(); + } + + bool + expired() const noexcept + { + return subscription.expired(); + } + }; + using SequenceOfWeakSubscriptions = std::vector; + using SubscriptionsIterable = std::vector; using SequenceOfWeakGuardConditions = std::vector>; using GuardConditionsIterable = std::vector>; @@ -47,21 +98,34 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo using SequenceOfWeakTimers = std::vector>; using TimersIterable = std::vector>; - struct WaitableEntry + class WaitableEntry { + public: + std::shared_ptr waitable; + std::shared_ptr associated_entity; + + /// Conversion constructor, which is intentionally not marked explicit. + WaitableEntry( + const std::shared_ptr & waitable_in = nullptr, + const std::shared_ptr & associated_entity_in = nullptr) noexcept + : waitable(waitable_in), + associated_entity(associated_entity_in) + {} + void reset() noexcept { waitable.reset(); associated_entity.reset(); } - - std::shared_ptr waitable; - std::shared_ptr associated_entity; }; - struct WeakWaitableEntry + class WeakWaitableEntry { - WeakWaitableEntry( + public: + std::weak_ptr waitable; + std::weak_ptr associated_entity; + + explicit WeakWaitableEntry( const std::shared_ptr & waitable_in, const std::shared_ptr & associated_entity_in) noexcept : waitable(waitable_in), @@ -84,9 +148,6 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo { return waitable.expired(); } - - std::weak_ptr waitable; - std::weak_ptr associated_entity; }; using SequenceOfWeakWaitables = std::vector; using WaitablesIterable = std::vector; @@ -151,7 +212,8 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo if (this->storage_has_entity(*subscription, subscriptions_)) { throw std::runtime_error("subscription already in wait set"); } - subscriptions_.push_back(std::move(subscription)); + WeakSubscriptionEntry weak_entry{std::move(subscription), {}}; + subscriptions_.push_back(std::move(weak_entry)); this->storage_flag_for_resize(); } @@ -276,7 +338,9 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo shared_ptrs.resize(weak_ptrs.size()); size_t index = 0; for (const auto & weak_ptr : weak_ptrs) { - shared_ptrs[index++] = {weak_ptr.waitable.lock(), weak_ptr.associated_entity.lock()}; + shared_ptrs[index++] = WaitableEntry{ + weak_ptr.waitable.lock(), + weak_ptr.associated_entity.lock()}; } }; lock_all_waitables(waitables_, shared_waitables_); diff --git a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp index 4bdc985ca7..796704154a 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp @@ -21,6 +21,7 @@ #include "rclcpp/guard_condition.hpp" #include "rclcpp/macros.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" @@ -49,8 +50,22 @@ class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCom protected: using is_mutable = std::false_type; + class SubscriptionEntry + { + public: + std::shared_ptr subscription; + rclcpp::SubscriptionWaitSetMask mask; + + /// Conversion constructor, which is intentionally not marked explicit. + SubscriptionEntry( + const std::shared_ptr & subscription_in = nullptr, + const rclcpp::SubscriptionWaitSetMask & mask_in = {}) + : subscription(subscription_in), + mask(mask_in) + {} + }; using ArrayOfSubscriptions = std::array< - std::shared_ptr, + SubscriptionEntry, NumberOfSubscriptions >; using SubscriptionsIterable = ArrayOfSubscriptions; @@ -69,6 +84,14 @@ class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCom struct WaitableEntry { + /// Conversion constructor, which is intentionally not marked explicit. + WaitableEntry( + const std::shared_ptr & waitable_in = nullptr, + const std::shared_ptr & associated_entity_in = nullptr) noexcept + : waitable(waitable_in), + associated_entity(associated_entity_in) + {} + std::shared_ptr waitable; std::shared_ptr associated_entity; }; diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp index 771ed04f30..76aa99e8f1 100644 --- a/rclcpp/include/rclcpp/wait_set_template.hpp +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -46,6 +46,7 @@ class WaitSetTemplate final : private StoragePolicy, private SynchronizationPoli 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. diff --git a/rclcpp/test/test_wait_set.cpp b/rclcpp/test/test_wait_set.cpp index dfec37cd07..ddd91c56c9 100644 --- a/rclcpp/test/test_wait_set.cpp +++ b/rclcpp/test/test_wait_set.cpp @@ -39,7 +39,7 @@ TEST_F(TestWaitSet, construction_and_destruction) { { rclcpp::WaitSet wait_set( - std::vector{}, + std::vector{}, std::vector{}, std::vector{}, std::vector{}); @@ -65,7 +65,7 @@ TEST_F(TestWaitSet, construction_and_destruction) { ASSERT_THROW( { rclcpp::WaitSet wait_set( - std::vector{}, + std::vector{}, std::vector{}, std::vector{}, std::vector{}, @@ -80,7 +80,7 @@ TEST_F(TestWaitSet, construction_and_destruction) { ASSERT_THROW( { rclcpp::WaitSet wait_set( - std::vector{}, + std::vector{}, std::vector{}, std::vector{}, std::vector{}, From c58fff767cc7c178d41fc5ea942b6f358418af05 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 7 Apr 2020 08:26:08 -0700 Subject: [PATCH 07/21] adding take to subscription so it can be used without the executor Signed-off-by: William Woodall --- rclcpp/include/rclcpp/subscription.hpp | 56 ++++++++++++++++++-------- rclcpp/test/test_subscription.cpp | 51 ++++++++++++++++++++--- 2 files changed, 86 insertions(+), 21 deletions(-) diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 2e920e286f..e9a582c0bc 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -145,19 +145,17 @@ class Subscription : public SubscriptionBase // First create a SubscriptionIntraProcess which will be given to the intra-process manager. auto context = node_base->get_context(); - auto subscription_intra_process = std::make_shared< - rclcpp::experimental::SubscriptionIntraProcess< - CallbackMessageT, - AllocatorT, - typename MessageUniquePtr::deleter_type - >>( + using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess< + CallbackMessageT, + AllocatorT, + typename MessageUniquePtr::deleter_type>; + auto subscription_intra_process = std::make_shared( callback, options.get_allocator(), context, - this->get_topic_name(), // important to get like this, as it has the fully-qualified name + this->get_topic_name(), // important to get like this, as it has the fully-qualified name qos_profile, - resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback) - ); + resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback)); TRACEPOINT( rclcpp_subscription_init, (const void *)get_subscription_handle().get(), @@ -197,16 +195,42 @@ class Subscription : public SubscriptionBase (void)options; } - /// Support dynamically setting the message memory strategy. + /// Take the next message from the inter-process subscription. /** - * Behavior may be undefined if called while the subscription could be executing. - * \param[in] message_memory_strategy Shared pointer to the memory strategy to set. + * Data may be taken (written) into the message_out and message_info_out even + * if false is returned. + * Specifically in the case of dropping redundant intra-process data, where + * data is received via both intra-process and inter-process (due to the + * underlying middleware being unabled to avoid this duplicate delivery) and + * so inter-process data from those intra-process publishers is ignored, but + * it has to be taken to know if it came from an intra-process publisher or + * not, and therefore could be dropped. + * + * \param[out] message_out The message into which take will copy the data. + * \param[out] message_info_out The message info for the taken message. + * \returns true if data was taken and is valid, otherwise false + * \throws any rcl errors from rcl_take, \sa rclcpp::exceptions::throw_from_rcl_error() */ - void set_message_memory_strategy( - typename message_memory_strategy::MessageMemoryStrategy::SharedPtr message_memory_strategy) + bool + take(CallbackMessageT & message_out, rmw_message_info_t & message_info_out) { - message_memory_strategy_ = message_memory_strategy; + rcl_ret_t ret = rcl_take( + this->get_subscription_handle().get(), + &message_out, + &message_info_out, + 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.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; } std::shared_ptr create_message() override diff --git a/rclcpp/test/test_subscription.cpp b/rclcpp/test/test_subscription.cpp index 2346917a38..17ba3a6411 100644 --- a/rclcpp/test/test_subscription.cpp +++ b/rclcpp/test/test_subscription.cpp @@ -14,8 +14,10 @@ #include -#include +#include #include +#include +#include #include #include "rclcpp/exceptions.hpp" @@ -23,6 +25,8 @@ #include "test_msgs/msg/empty.hpp" +using namespace std::chrono_literals; + class TestSubscription : public ::testing::Test { public: @@ -235,13 +239,13 @@ TEST_F(TestSubscription, callback_bind) { using test_msgs::msg::Empty; { // Member callback for plain class - SubscriptionClass subscriptionObject; - subscriptionObject.CreateSubscription(); + SubscriptionClass subscription_object; + subscription_object.CreateSubscription(); } { // Member callback for class inheriting from rclcpp::Node - SubscriptionClassNodeInheritance subscriptionObject; - subscriptionObject.CreateSubscription(); + SubscriptionClassNodeInheritance subscription_object; + subscription_object.CreateSubscription(); } { // Member callback for class inheriting from testing::Test @@ -252,6 +256,43 @@ TEST_F(TestSubscription, callback_bind) { } } +/* + Testing take. + */ +TEST_F(TestSubscription, take) { + initialize(); + using test_msgs::msg::Empty; + auto do_nothing = [](std::shared_ptr){}; + { + auto sub = node->create_subscription("~/test_take", 1, do_nothing); + test_msgs::msg::Empty msg; + rmw_message_info_t msg_info; + EXPECT_FALSE(sub->take(msg, msg_info)); + } + { + rclcpp::SubscriptionOptions so; + so.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + auto sub = node->create_subscription("~/test_take", 1, do_nothing, so); + rclcpp::PublisherOptions po; + po.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + auto pub = node->create_publisher("~/test_take", 1, po); + { + test_msgs::msg::Empty msg; + pub->publish(msg); + } + test_msgs::msg::Empty msg; + rmw_message_info_t 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 subscription with intraprocess enabled and invalid QoS */ From 2866c720c87ca4cfad15fce8e8b12580043ce757 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 7 Apr 2020 09:28:11 -0700 Subject: [PATCH 08/21] add rclcpp::MessageInfo to replace use of rmw_message_info_t Signed-off-by: William Woodall --- rclcpp/CMakeLists.txt | 1 + .../rclcpp/any_subscription_callback.hpp | 13 ++--- rclcpp/include/rclcpp/message_info.hpp | 52 +++++++++++++++++++ rclcpp/include/rclcpp/subscription.hpp | 15 +++--- rclcpp/include/rclcpp/subscription_base.hpp | 5 +- rclcpp/src/rclcpp/message_info.cpp | 39 ++++++++++++++ rclcpp/test/test_subscription.cpp | 4 +- 7 files changed, 113 insertions(+), 16 deletions(-) create mode 100644 rclcpp/include/rclcpp/message_info.hpp create mode 100644 rclcpp/src/rclcpp/message_info.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index cd421abb58..a89e485fa5 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -55,6 +55,7 @@ set(${PROJECT_NAME}_SRCS 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 diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 1909b85815..6a813c222b 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -25,6 +25,7 @@ #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/function_traits.hpp" +#include "rclcpp/message_info.hpp" #include "rclcpp/visibility_control.hpp" #include "tracetools/tracetools.h" #include "tracetools/utils.hpp" @@ -43,13 +44,13 @@ class AnySubscriptionCallback using SharedPtrCallback = std::function)>; using SharedPtrWithInfoCallback = - std::function, const rmw_message_info_t &)>; + std::function, const rclcpp::MessageInfo &)>; using ConstSharedPtrCallback = std::function)>; using ConstSharedPtrWithInfoCallback = - std::function, const rmw_message_info_t &)>; + std::function, const rclcpp::MessageInfo &)>; using UniquePtrCallback = std::function; using UniquePtrWithInfoCallback = - std::function; + std::function; SharedPtrCallback shared_ptr_callback_; SharedPtrWithInfoCallback shared_ptr_with_info_callback_; @@ -155,7 +156,7 @@ class AnySubscriptionCallback } void dispatch( - std::shared_ptr message, const rmw_message_info_t & message_info) + std::shared_ptr message, const rclcpp::MessageInfo & message_info) { TRACEPOINT(callback_start, (const void *)this, false); if (shared_ptr_callback_) { @@ -181,7 +182,7 @@ class AnySubscriptionCallback } void dispatch_intra_process( - ConstMessageSharedPtr message, const rmw_message_info_t & message_info) + ConstMessageSharedPtr message, const rclcpp::MessageInfo & message_info) { TRACEPOINT(callback_start, (const void *)this, true); if (const_shared_ptr_callback_) { @@ -204,7 +205,7 @@ class AnySubscriptionCallback } void dispatch_intra_process( - MessageUniquePtr message, const rmw_message_info_t & message_info) + MessageUniquePtr message, const rclcpp::MessageInfo & message_info) { TRACEPOINT(callback_start, (const void *)this, true); if (shared_ptr_callback_) { diff --git a/rclcpp/include/rclcpp/message_info.hpp b/rclcpp/include/rclcpp/message_info.hpp new file mode 100644 index 0000000000..1653b0a505 --- /dev/null +++ b/rclcpp/include/rclcpp/message_info.hpp @@ -0,0 +1,52 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__MESSAGE_INFO_HPP_ +#define RCLCPP__MESSAGE_INFO_HPP_ + +#include "rmw/types.h" + +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// Additional meta data about messages taken from subscriptions. +class RCLCPP_PUBLIC MessageInfo +{ +public: + /// Default empty constructor. + MessageInfo() = default; + + /// Conversion constructor, which is intentionally not marked as explicit. + // cppcheck-suppress noExplicitConstructor + MessageInfo(const rmw_message_info_t & rmw_message_info); // NOLINT(runtime/explicit) + + virtual ~MessageInfo(); + + /// Return the message info as the underlying rmw message info type. + const rmw_message_info_t & + get_rmw_message_info() const; + + /// Return the message info as the underlying rmw message info type. + rmw_message_info_t & + get_rmw_message_info(); + +private: + rmw_message_info_t rmw_message_info_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__MESSAGE_INFO_HPP_ diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index e9a582c0bc..115800bb66 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -38,6 +38,7 @@ #include "rclcpp/experimental/subscription_intra_process.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/message_info.hpp" #include "rclcpp/message_memory_strategy.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/subscription_base.hpp" @@ -212,12 +213,12 @@ class Subscription : public SubscriptionBase * \throws any rcl errors from rcl_take, \sa rclcpp::exceptions::throw_from_rcl_error() */ bool - take(CallbackMessageT & message_out, rmw_message_info_t & message_info_out) + take(CallbackMessageT & message_out, rclcpp::MessageInfo & message_info_out) { rcl_ret_t ret = rcl_take( this->get_subscription_handle().get(), &message_out, - &message_info_out, + &message_info_out.get_rmw_message_info(), nullptr // rmw_subscription_allocation_t is unused here ); if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { @@ -225,7 +226,9 @@ class Subscription : public SubscriptionBase } else if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } - if (matches_any_intra_process_publishers(&message_info_out.publisher_gid)) { + 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; @@ -248,9 +251,9 @@ class Subscription : public SubscriptionBase } void handle_message( - std::shared_ptr & message, const rmw_message_info_t & message_info) override + std::shared_ptr & message, const rclcpp::MessageInfo & message_info) override { - if (matches_any_intra_process_publishers(&message_info.publisher_gid)) { + if (matches_any_intra_process_publishers(&message_info.get_rmw_message_info().publisher_gid)) { // In this case, the message will be delivered via intra process and // we should ignore this copy of the message. return; @@ -261,7 +264,7 @@ class Subscription : public SubscriptionBase void handle_loaned_message( - void * loaned_message, const rmw_message_info_t & message_info) override + void * loaned_message, const rclcpp::MessageInfo & message_info) override { auto typed_message = static_cast(loaned_message); // message is loaned, so we have to make sure that the deleter does not deallocate the message diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 23ecb5e004..1340cbf815 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -27,6 +27,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" @@ -132,12 +133,12 @@ class SubscriptionBase : public std::enable_shared_from_this RCLCPP_PUBLIC virtual void - handle_message(std::shared_ptr & message, const rmw_message_info_t & message_info) = 0; + handle_message(std::shared_ptr & message, const rclcpp::MessageInfo & message_info) = 0; RCLCPP_PUBLIC virtual void - handle_loaned_message(void * loaned_message, const rmw_message_info_t & message_info) = 0; + handle_loaned_message(void * loaned_message, const rclcpp::MessageInfo & message_info) = 0; /// Return the message borrowed in create_message. /** \param[in] message Shared pointer to the returned message. */ diff --git a/rclcpp/src/rclcpp/message_info.cpp b/rclcpp/src/rclcpp/message_info.cpp new file mode 100644 index 0000000000..0c2eab81fa --- /dev/null +++ b/rclcpp/src/rclcpp/message_info.cpp @@ -0,0 +1,39 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/message_info.hpp" + +namespace rclcpp +{ + +MessageInfo::MessageInfo(const rmw_message_info_t & rmw_message_info) +: rmw_message_info_(rmw_message_info) +{} + +MessageInfo::~MessageInfo() +{} + +const rmw_message_info_t & +MessageInfo::get_rmw_message_info() const +{ + return rmw_message_info_; +} + +rmw_message_info_t & +MessageInfo::get_rmw_message_info() +{ + return rmw_message_info_; +} + +} // namespace rclcpp diff --git a/rclcpp/test/test_subscription.cpp b/rclcpp/test/test_subscription.cpp index 17ba3a6411..7b259d8005 100644 --- a/rclcpp/test/test_subscription.cpp +++ b/rclcpp/test/test_subscription.cpp @@ -266,7 +266,7 @@ TEST_F(TestSubscription, take) { { auto sub = node->create_subscription("~/test_take", 1, do_nothing); test_msgs::msg::Empty msg; - rmw_message_info_t msg_info; + rclcpp::MessageInfo msg_info; EXPECT_FALSE(sub->take(msg, msg_info)); } { @@ -281,7 +281,7 @@ TEST_F(TestSubscription, take) { pub->publish(msg); } test_msgs::msg::Empty msg; - rmw_message_info_t msg_info; + rclcpp::MessageInfo msg_info; bool message_recieved = false; auto start = std::chrono::steady_clock::now(); do { From 3f1f8cd6a501fd6174d760855c3f7d36c5a386d0 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 8 Apr 2020 04:58:04 -0700 Subject: [PATCH 09/21] refactor Subscription and Executor so they can be used separately Signed-off-by: William Woodall --- rclcpp/include/rclcpp/subscription.hpp | 34 ++--- rclcpp/include/rclcpp/subscription_base.hpp | 37 ++++++ rclcpp/src/rclcpp/executor.cpp | 136 ++++++++++++-------- rclcpp/src/rclcpp/subscription_base.cpp | 42 ++++++ 4 files changed, 174 insertions(+), 75 deletions(-) diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 115800bb66..a79a290937 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -207,6 +207,8 @@ class Subscription : public SubscriptionBase * 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 @@ -215,28 +217,11 @@ class Subscription : public SubscriptionBase bool take(CallbackMessageT & 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; + return this->take_type_erased(static_cast(&message_out), message_info_out); } - std::shared_ptr create_message() override + std::shared_ptr + create_message() override { /* The default message memory strategy provides a dynamically allocated message on each call to * create_message, though alternative memory strategies that re-use a preallocated message may be @@ -245,13 +230,16 @@ class Subscription : public SubscriptionBase return message_memory_strategy_->borrow_message(); } - std::shared_ptr create_serialized_message() override + std::shared_ptr + create_serialized_message() override { return message_memory_strategy_->borrow_serialized_message(); } - void handle_message( - std::shared_ptr & message, const rclcpp::MessageInfo & message_info) override + void + handle_message( + std::shared_ptr & message, + const rclcpp::MessageInfo & message_info) override { 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 diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 1340cbf815..d2e3ffd109 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -111,6 +111,43 @@ class SubscriptionBase : public std::enable_shared_from_this rclcpp::QoS get_actual_qos() const; + /// Take the next inter-process message from the subscription as a type erased poiner. + /** + * \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() + */ + 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() + */ + bool + take_serialized(rmw_serialized_message_t & message_out, rclcpp::MessageInfo & message_info_out); + /// Borrow a new message. /** \return Shared pointer to the fresh message. */ RCLCPP_PUBLIC diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 22f5c2f063..89f2222932 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -306,82 +306,114 @@ Executor::execute_any_executable(AnyExecutable & any_exec) } void -Executor::execute_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription) +Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) { - rmw_message_info_t message_info; - message_info.from_intra_process = false; + rclcpp::MessageInfo message_info; + message_info.get_rmw_message_info().from_intra_process = false; + + // Reduce code duplication by generalize error handling into lambda. + auto take_message_and_do_error_handling = + [&subscription]( + const char * message_adjective, + std::function take_action, + std::function handle_action) + { + bool taken; + try { + taken = take_action(); + } catch (const rclcpp::exceptions::RCLError & rcl_error) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "executor taking a %smessage from topic '%s' unexpectedly failed: %s", + message_adjective, + subscription->get_topic_name(), + rcl_error.what()); + } + if (taken) { + handle_action(); + } else { + // Message 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 a subscription actually having data until trying + // to take the data. + RCLCPP_DEBUG( + rclcpp::get_logger("rclcpp"), + "executor taking a %smessage from topic '%s' failed to take anything", + message_adjective, + subscription->get_topic_name()); + } + }; if (subscription->is_serialized()) { - auto serialized_msg = subscription->create_serialized_message(); - auto ret = rcl_take_serialized_message( - subscription->get_subscription_handle().get(), - serialized_msg.get(), &message_info, nullptr); - if (RCL_RET_OK == ret) { - auto void_serialized_msg = std::static_pointer_cast(serialized_msg); - subscription->handle_message(void_serialized_msg, message_info); - } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "take_serialized failed for subscription on topic '%s': %s", - subscription->get_topic_name(), rcl_get_error_string().str); - rcl_reset_error(); - } + // This is the case where a copy of the serialized message is taken from + // the middleware via inter-process communication. + std::shared_ptr serialized_msg = + subscription->create_serialized_message(); + take_message_and_do_error_handling( + "serialzed ", + [&]() { return subscription->take_serialized(*serialized_msg.get(), message_info); }, + [&]() + { + auto void_serialized_msg = std::static_pointer_cast(serialized_msg); + subscription->handle_message(void_serialized_msg, message_info); + }); subscription->return_serialized_message(serialized_msg); } else if (subscription->can_loan_messages()) { + // This is the case where a loaned message is taken from the middleware via + // inter-process communication, given to the user for their callback, + // and then returned. void * loaned_msg = nullptr; - auto ret = rcl_take_loaned_message( - subscription->get_subscription_handle().get(), - &loaned_msg, - &message_info, - nullptr); - if (RCL_RET_OK == ret) { - subscription->handle_loaned_message(loaned_msg, message_info); - } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "take_loaned failed for subscription on topic '%s': %s", - subscription->get_topic_name(), rcl_get_error_string().str); - rcl_reset_error(); - } - ret = rcl_return_loaned_message_from_subscription( + // TODO(wjwwood): refactor this into methods on subscription when LoanedMessage + // is extened to support subscriptions as well. + take_message_and_do_error_handling( + "loaned ", + [&]() + { + rcl_ret_t ret = rcl_take_loaned_message( + subscription->get_subscription_handle().get(), + &loaned_msg, + &message_info.get_rmw_message_info(), + nullptr); + if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { + return false; + } else if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + return true; + }, + [&]() { subscription->handle_loaned_message(loaned_msg, message_info); }); + rcl_ret_t ret = rcl_return_loaned_message_from_subscription( subscription->get_subscription_handle().get(), loaned_msg); if (RCL_RET_OK != ret) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "return_loaned_message failed for subscription on topic '%s': %s", + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "rcl_return_loaned_message_from_subscription() failed for subscription on topic '%s': %s", subscription->get_topic_name(), rcl_get_error_string().str); } loaned_msg = nullptr; } else { + // This case is taking a copy of the message data from the middleware via + // inter-process communication. std::shared_ptr message = subscription->create_message(); - auto ret = rcl_take( - subscription->get_subscription_handle().get(), - message.get(), &message_info, nullptr); - if (RCL_RET_OK == ret) { - subscription->handle_message(message, message_info); - } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "could not deserialize serialized message on topic '%s': %s", - subscription->get_topic_name(), rcl_get_error_string().str); - rcl_reset_error(); - } + take_message_and_do_error_handling( + "", // no message adjective + [&]() { return subscription->take_type_erased(message.get(), message_info); }, + [&]() { subscription->handle_message(message, message_info); }); subscription->return_message(message); } } void -Executor::execute_timer( - rclcpp::TimerBase::SharedPtr timer) +Executor::execute_timer(rclcpp::TimerBase::SharedPtr timer) { timer->execute_callback(); } void -Executor::execute_service( - rclcpp::ServiceBase::SharedPtr service) +Executor::execute_service(rclcpp::ServiceBase::SharedPtr service) { auto request_header = service->create_request_header(); std::shared_ptr request = service->create_request(); diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 314a8b837c..2102330972 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -133,6 +133,48 @@ SubscriptionBase::get_actual_qos() const return rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos); } +bool +SubscriptionBase::take_type_erased(void * message_out, rclcpp::MessageInfo & message_info_out) +{ + rcl_ret_t ret = rcl_take( + this->get_subscription_handle().get(), + message_out, + &message_info_out.get_rmw_message_info(), + nullptr // rmw_subscription_allocation_t is unused here + ); + if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { + return false; + } else if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + if ( + matches_any_intra_process_publishers(&message_info_out.get_rmw_message_info().publisher_gid)) + { + // In this case, the message will be delivered via intra-process and + // we should ignore this copy of the message. + return false; + } + return true; +} + +bool +SubscriptionBase::take_serialized( + rmw_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 { From 08381e371145d024f93530b540d4bacc90a5afa2 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 8 Apr 2020 05:11:53 -0700 Subject: [PATCH 10/21] style and cpplint Signed-off-by: William Woodall --- rclcpp/include/rclcpp/context.hpp | 2 +- rclcpp/include/rclcpp/guard_condition.hpp | 6 +-- rclcpp/include/rclcpp/subscription.hpp | 15 ++++-- rclcpp/include/rclcpp/timer.hpp | 2 +- rclcpp/include/rclcpp/utilities.hpp | 2 +- .../detail/storage_policy_common.hpp | 6 ++- .../detail/synchronization_policy_common.hpp | 6 +-- .../wait_set_policies/dynamic_storage.hpp | 53 ++++++++++--------- .../sequential_synchronization.hpp | 25 ++++----- .../wait_set_policies/static_storage.hpp | 2 +- rclcpp/include/rclcpp/wait_set_template.hpp | 5 +- rclcpp/src/rclcpp/executor.cpp | 14 ++--- rclcpp/test/test_subscription.cpp | 2 +- 13 files changed, 77 insertions(+), 63 deletions(-) diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index dfc33d5e4f..76d3cb1dca 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -159,7 +159,7 @@ class Context : public std::enable_shared_from_this * * \param[in] reason the description of why shutdown happened * \return true if shutdown was successful, false if context was already shutdown - * \throw various exceptions derived from RCLErrorBase, if rcl_shutdown fails + * \throw various exceptions derived from rclcpp::exceptions::RCLError, if rcl_shutdown fails */ RCLCPP_PUBLIC virtual diff --git a/rclcpp/include/rclcpp/guard_condition.hpp b/rclcpp/include/rclcpp/guard_condition.hpp index 29c0f23e7a..3d15cc7416 100644 --- a/rclcpp/include/rclcpp/guard_condition.hpp +++ b/rclcpp/include/rclcpp/guard_condition.hpp @@ -39,13 +39,13 @@ class GuardCondition * 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::RCLErrorBase based exceptions when underlying + * \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::contexts::default_context::get_global_default_context()); RCLCPP_PUBLIC virtual @@ -66,7 +66,7 @@ class GuardCondition * This function is thread-safe, and may be called concurrently with waiting * on this guard condition in a wait set. * - * \throws rclcpp::exceptions::RCLErrorBase based exceptions when underlying + * \throws rclcpp::exceptions::RCLError based exceptions when underlying * rcl functions fail. */ RCLCPP_PUBLIC diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index a79a290937..190a437d3c 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -186,7 +186,8 @@ class Subscription : public SubscriptionBase } /// Called after construction to continue setup that requires shared_from_this(). - void post_init_setup( + void + post_init_setup( rclcpp::node_interfaces::NodeBaseInterface * node_base, const rclcpp::QoS & qos, const rclcpp::SubscriptionOptionsWithAllocator & options) @@ -252,7 +253,8 @@ class Subscription : public SubscriptionBase void handle_loaned_message( - void * loaned_message, const rclcpp::MessageInfo & message_info) override + void * loaned_message, + const rclcpp::MessageInfo & message_info) override { auto typed_message = static_cast(loaned_message); // message is loaned, so we have to make sure that the deleter does not deallocate the message @@ -263,18 +265,21 @@ class Subscription : public SubscriptionBase /// Return the borrowed message. /** \param message message to be returned */ - void return_message(std::shared_ptr & message) override + void + return_message(std::shared_ptr & message) override { auto typed_message = std::static_pointer_cast(message); message_memory_strategy_->return_message(typed_message); } - void return_serialized_message(std::shared_ptr & message) override + void + return_serialized_message(std::shared_ptr & message) override { message_memory_strategy_->return_serialized_message(message); } - bool use_take_shared_method() const + bool + use_take_shared_method() const { return any_callback_.use_take_shared_method(); } diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index afeb63b663..23b1be2789 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -64,7 +64,7 @@ class TimerBase /** * \return true if the timer has been cancelled, false otherwise * \throws std::runtime_error if the rcl_get_error_state returns 0 - * \throws RCLErrorBase some child class exception based on ret + * \throws rclcpp::exceptions::RCLError some child class exception based on ret */ RCLCPP_PUBLIC bool diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index 83fa285f60..690df026bc 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -121,7 +121,7 @@ init_and_remove_ros_arguments( * \param[in] argv Argument vector. * \returns Members of the argument vector that are not ROS arguments. * \throws anything throw_from_rcl_error can throw - * \throws rclcpp::exceptions::RCLErrorBase if the parsing fails + * \throws rclcpp::exceptions::RCLError if the parsing fails */ RCLCPP_PUBLIC std::vector diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp index 66139b4197..9c39a33c02 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp @@ -94,7 +94,11 @@ class StoragePolicyCommon } // (Re)build the wait set for the first time. - this->storage_rebuild_rcl_wait_set_with_sets(subscriptions, guard_conditions, timers, waitables); + this->storage_rebuild_rcl_wait_set_with_sets( + subscriptions, + guard_conditions, + timers, + waitables); } ~StoragePolicyCommon() diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp index 064433f103..2856ec7275 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp @@ -32,7 +32,7 @@ class SynchronizationPolicyCommon SynchronizationPolicyCommon() = default; ~SynchronizationPolicyCommon() = default; - std::function + std::function create_loop_predicate( std::chrono::nanoseconds time_to_wait_ns, std::chrono::steady_clock::time_point start) @@ -40,10 +40,10 @@ class SynchronizationPolicyCommon 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; }; + 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; }; + return []() -> bool {return true;}; } } diff --git a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp index 4154ffdcdd..47720157bb 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp @@ -16,8 +16,9 @@ #define RCLCPP__WAIT_SET_POLICIES__DYNAMIC_STORAGE_HPP_ #include -#include #include +#include +#include #include "rclcpp/guard_condition.hpp" #include "rclcpp/macros.hpp" @@ -41,7 +42,9 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo class SubscriptionEntry { - public: + // (wjwwood): indent of 'public:' is weird, I know. uncrustify is dumb. + +public: std::shared_ptr subscription; rclcpp::SubscriptionWaitSetMask mask; @@ -61,7 +64,7 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo }; class WeakSubscriptionEntry { - public: +public: std::weak_ptr subscription; rclcpp::SubscriptionWaitSetMask mask; @@ -100,7 +103,7 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo class WaitableEntry { - public: +public: std::shared_ptr waitable; std::shared_ptr associated_entity; @@ -121,7 +124,7 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo }; class WeakWaitableEntry { - public: +public: std::weak_ptr waitable; std::weak_ptr associated_entity; @@ -192,7 +195,7 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo return std::any_of( entities.cbegin(), entities.cend(), - [&entity](const auto & inner) { return &entity == inner.lock().get(); }); + [&entity](const auto & inner) {return &entity == inner.lock().get();}); } template @@ -203,7 +206,7 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo return std::find_if( entities.cbegin(), entities.cend(), - [&entity](const auto & inner) { return &entity == inner.lock().get(); }); + [&entity](const auto & inner) {return &entity == inner.lock().get();}); } void @@ -323,26 +326,26 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo } // 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(); - } - }; + 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_); // 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()}; - } - }; + 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_); } @@ -355,10 +358,10 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo } // "Unlock" all shared pointers by resetting them. auto reset_all = [](auto & shared_ptrs) { - for (auto & shared_ptr : shared_ptrs) { - shared_ptr.reset(); - } - }; + for (auto & shared_ptr : shared_ptrs) { + shared_ptr.reset(); + } + }; reset_all(shared_guard_conditions_); reset_all(shared_timers_); reset_all(shared_waitables_); diff --git a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp index 36873755b1..9fb48b6183 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "rclcpp/exceptions.hpp" #include "rclcpp/guard_condition.hpp" @@ -52,7 +53,7 @@ class SequentialSynchronization : public detail::SynchronizationPolicyCommon std::shared_ptr && subscription, const rclcpp::SubscriptionWaitSetMask & mask, std::function< - void (std::shared_ptr &&, const rclcpp::SubscriptionWaitSetMask &) + void(std::shared_ptr&&, const rclcpp::SubscriptionWaitSetMask &) > add_subscription_function) { // Explicitly no thread synchronization. @@ -68,7 +69,7 @@ class SequentialSynchronization : public detail::SynchronizationPolicyCommon std::shared_ptr && subscription, const rclcpp::SubscriptionWaitSetMask & mask, std::function< - void (std::shared_ptr &&, const rclcpp::SubscriptionWaitSetMask &) + void(std::shared_ptr&&, const rclcpp::SubscriptionWaitSetMask &) > remove_subscription_function) { // Explicitly no thread synchronization. @@ -82,7 +83,7 @@ class SequentialSynchronization : public detail::SynchronizationPolicyCommon void sync_add_guard_condition( std::shared_ptr && guard_condition, - std::function &&)> add_guard_condition_function) + std::function&&)> add_guard_condition_function) { // Explicitly no thread synchronization. add_guard_condition_function(std::move(guard_condition)); @@ -95,7 +96,7 @@ class SequentialSynchronization : public detail::SynchronizationPolicyCommon void sync_remove_guard_condition( std::shared_ptr && guard_condition, - std::function &&)> remove_guard_condition_function) + std::function&&)> remove_guard_condition_function) { // Explicitly no thread synchronization. remove_guard_condition_function(std::move(guard_condition)); @@ -108,7 +109,7 @@ class SequentialSynchronization : public detail::SynchronizationPolicyCommon void sync_add_timer( std::shared_ptr && timer, - std::function &&)> add_timer_function) + std::function&&)> add_timer_function) { // Explicitly no thread synchronization. add_timer_function(std::move(timer)); @@ -121,7 +122,7 @@ class SequentialSynchronization : public detail::SynchronizationPolicyCommon void sync_remove_timer( std::shared_ptr && timer, - std::function &&)> remove_timer_function) + std::function&&)> remove_timer_function) { // Explicitly no thread synchronization. remove_timer_function(std::move(timer)); @@ -136,7 +137,7 @@ class SequentialSynchronization : public detail::SynchronizationPolicyCommon std::shared_ptr && waitable, std::shared_ptr && associated_entity, std::function< - void (std::shared_ptr &&, std::shared_ptr &&) + void(std::shared_ptr&&, std::shared_ptr&&) > add_waitable_function) { // Explicitly no thread synchronization. @@ -150,7 +151,7 @@ class SequentialSynchronization : public detail::SynchronizationPolicyCommon void sync_remove_waitable( std::shared_ptr && waitable, - std::function &&)> remove_waitable_function) + std::function&&)> remove_waitable_function) { // Explicitly no thread synchronization. remove_waitable_function(std::move(waitable)); @@ -161,7 +162,7 @@ class SequentialSynchronization : public detail::SynchronizationPolicyCommon * Does not throw, but storage function may throw. */ void - sync_prune_deleted_entities(std::function prune_deleted_entities_function) + sync_prune_deleted_entities(std::function prune_deleted_entities_function) { // Explicitly no thread synchronization. prune_deleted_entities_function(); @@ -172,9 +173,9 @@ class SequentialSynchronization : public detail::SynchronizationPolicyCommon WaitResultT sync_wait( std::chrono::nanoseconds time_to_wait_ns, - std::function rebuild_rcl_wait_set, + std::function rebuild_rcl_wait_set, std::function get_rcl_wait_set, - std::function create_wait_result) + std::function create_wait_result) { // Assumption: this function assumes that some measure has been taken to // ensure none of the entities being waited on by the wait set are allowed @@ -187,7 +188,7 @@ class SequentialSynchronization : public detail::SynchronizationPolicyCommon // Setup looping predicate. auto start = std::chrono::steady_clock::now(); - std::function should_loop = this->create_loop_predicate(time_to_wait_ns, start); + std::function should_loop = this->create_loop_predicate(time_to_wait_ns, start); // Wait until exit condition is met. do { diff --git a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp index 796704154a..f2cdafa7fe 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp @@ -52,7 +52,7 @@ class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCom class SubscriptionEntry { - public: +public: std::shared_ptr subscription; rclcpp::SubscriptionWaitSetMask mask; diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp index 76aa99e8f1..e57d67df70 100644 --- a/rclcpp/include/rclcpp/wait_set_template.hpp +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -17,6 +17,7 @@ #include #include +#include #include "rcl/wait.h" @@ -70,7 +71,7 @@ class WaitSetTemplate final : private StoragePolicy, private SynchronizationPoli const typename StoragePolicy::TimersIterable & timers = {}, const typename StoragePolicy::WaitablesIterable & waitables = {}, rclcpp::Context::SharedPtr context = - rclcpp::contexts::default_context::get_global_default_context()) + rclcpp::contexts::default_context::get_global_default_context()) : StoragePolicy( subscriptions, guard_conditions, @@ -475,7 +476,7 @@ class WaitSetTemplate final : private StoragePolicy, private SynchronizationPoli * 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::RCLErrorBase on unhandled rcl errors + * \throws rclcpp::exceptions::RCLError on unhandled rcl errors */ template RCUTILS_WARN_UNUSED diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 89f2222932..84c3f2b5a7 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -314,9 +314,9 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) // Reduce code duplication by generalize error handling into lambda. auto take_message_and_do_error_handling = [&subscription]( - const char * message_adjective, - std::function take_action, - std::function handle_action) + const char * message_adjective, + std::function take_action, + std::function handle_action) { bool taken; try { @@ -353,7 +353,7 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) subscription->create_serialized_message(); take_message_and_do_error_handling( "serialzed ", - [&]() { return subscription->take_serialized(*serialized_msg.get(), message_info); }, + [&]() {return subscription->take_serialized(*serialized_msg.get(), message_info);}, [&]() { auto void_serialized_msg = std::static_pointer_cast(serialized_msg); @@ -383,7 +383,7 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) } return true; }, - [&]() { subscription->handle_loaned_message(loaned_msg, message_info); }); + [&]() {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); @@ -400,8 +400,8 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) std::shared_ptr message = subscription->create_message(); take_message_and_do_error_handling( "", // no message adjective - [&]() { return subscription->take_type_erased(message.get(), message_info); }, - [&]() { subscription->handle_message(message, message_info); }); + [&]() {return subscription->take_type_erased(message.get(), message_info);}, + [&]() {subscription->handle_message(message, message_info);}); subscription->return_message(message); } } diff --git a/rclcpp/test/test_subscription.cpp b/rclcpp/test/test_subscription.cpp index 7b259d8005..3324c268db 100644 --- a/rclcpp/test/test_subscription.cpp +++ b/rclcpp/test/test_subscription.cpp @@ -262,7 +262,7 @@ TEST_F(TestSubscription, callback_bind) { TEST_F(TestSubscription, take) { initialize(); using test_msgs::msg::Empty; - auto do_nothing = [](std::shared_ptr){}; + auto do_nothing = [](std::shared_ptr) {}; { auto sub = node->create_subscription("~/test_take", 1, do_nothing); test_msgs::msg::Empty msg; From b8af0c5834c91b88c6ebb71fad2a0b93a3df009a Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 8 Apr 2020 10:31:21 -0700 Subject: [PATCH 11/21] fixup take_serialized() and add tests for it Signed-off-by: William Woodall --- rclcpp/include/rclcpp/subscription_base.hpp | 2 +- rclcpp/src/rclcpp/subscription_base.cpp | 2 +- rclcpp/test/test_subscription.cpp | 38 ++++++++++++++++++++- 3 files changed, 39 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index d2e3ffd109..abe764de33 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -146,7 +146,7 @@ class SubscriptionBase : public std::enable_shared_from_this * \throws any rcl errors from rcl_take, \sa rclcpp::exceptions::throw_from_rcl_error() */ bool - take_serialized(rmw_serialized_message_t & message_out, rclcpp::MessageInfo & message_info_out); + take_serialized(rcl_serialized_message_t & message_out, rclcpp::MessageInfo & message_info_out); /// Borrow a new message. /** \return Shared pointer to the fresh message. */ diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 2102330972..8c2e8d031c 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -159,7 +159,7 @@ SubscriptionBase::take_type_erased(void * message_out, rclcpp::MessageInfo & mes bool SubscriptionBase::take_serialized( - rmw_serialized_message_t & message_out, + rcl_serialized_message_t & message_out, rclcpp::MessageInfo & message_info_out) { rcl_ret_t ret = rcl_take_serialized_message( diff --git a/rclcpp/test/test_subscription.cpp b/rclcpp/test/test_subscription.cpp index 3324c268db..d64959fe80 100644 --- a/rclcpp/test/test_subscription.cpp +++ b/rclcpp/test/test_subscription.cpp @@ -262,7 +262,7 @@ TEST_F(TestSubscription, callback_bind) { TEST_F(TestSubscription, take) { initialize(); using test_msgs::msg::Empty; - auto do_nothing = [](std::shared_ptr) {}; + auto do_nothing = [](std::shared_ptr) { FAIL(); }; { auto sub = node->create_subscription("~/test_take", 1, do_nothing); test_msgs::msg::Empty msg; @@ -293,6 +293,42 @@ TEST_F(TestSubscription, take) { // TODO(wjwwood): figure out a good way to test the intra-process exclusion behavior. } +/* + Testing take_serialized. + */ +TEST_F(TestSubscription, take_serialized) { + initialize(); + using test_msgs::msg::Empty; + auto do_nothing = [](std::shared_ptr) { FAIL(); }; + { + auto sub = node->create_subscription("~/test_take", 1, do_nothing); + std::shared_ptr msg = sub->create_serialized_message(); + rclcpp::MessageInfo msg_info; + EXPECT_FALSE(sub->take_serialized(*msg, msg_info)); + } + { + rclcpp::SubscriptionOptions so; + so.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + auto sub = node->create_subscription("~/test_take", 1, do_nothing, so); + rclcpp::PublisherOptions po; + po.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + auto pub = node->create_publisher("~/test_take", 1, po); + { + test_msgs::msg::Empty msg; + pub->publish(msg); + } + std::shared_ptr msg = sub->create_serialized_message(); + rclcpp::MessageInfo msg_info; + bool message_recieved = false; + auto start = std::chrono::steady_clock::now(); + do { + message_recieved = sub->take_serialized(*msg, msg_info); + std::this_thread::sleep_for(100ms); + } while (!message_recieved && std::chrono::steady_clock::now() - start < 10s); + EXPECT_TRUE(message_recieved); + } +} + /* Testing subscription with intraprocess enabled and invalid QoS */ From 036ee3e27cf889a49ac3dbb162d5c7063a32f7b5 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 8 Apr 2020 13:40:30 -0700 Subject: [PATCH 12/21] add support for client and service to wait set Signed-off-by: William Woodall --- rclcpp/include/rclcpp/client.hpp | 40 ++++++ rclcpp/include/rclcpp/service.hpp | 89 ++++++++++--- rclcpp/include/rclcpp/wait_set.hpp | 8 +- .../detail/storage_policy_common.hpp | 68 ++++++++-- .../wait_set_policies/dynamic_storage.hpp | 79 +++++++++++- .../sequential_synchronization.hpp | 54 ++++++++ .../wait_set_policies/static_storage.hpp | 39 +++++- rclcpp/include/rclcpp/wait_set_template.hpp | 110 ++++++++++++++++ rclcpp/src/rclcpp/client.cpp | 15 +++ rclcpp/src/rclcpp/executor.cpp | 122 ++++++++---------- rclcpp/src/rclcpp/service.cpp | 15 +++ rclcpp/test/test_subscription.cpp | 4 +- rclcpp/test/test_wait_set.cpp | 8 +- 13 files changed, 547 insertions(+), 104 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 46467400fe..6651661781 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -62,6 +62,27 @@ class ClientBase RCLCPP_PUBLIC virtual ~ClientBase(); + /// Take the next response for this client as a type erased pointer. + /** + * The type erased pointer allows for this method to be used in a type + * agnostic way along with ClientBase::create_response(), + * ClientBase::create_request_header(), and ClientBase::handle_response(). + * The typed version of this can be used if the Service type is known, + * \sa Client::take_response(). + * + * \param[out] response_out The type erased pointer to a Service Response into + * which the middleware will copy the response being taken. + * \param[out] request_header_out The request header to be filled by the + * middleware when taking, and which can be used to associte the response + * to a specific request. + * \returns true if the response was taken, otherwise false. + * \throws rclcpp::exceptions::RCLError based exceptions if the underlying + * rcl function fail. + */ + RCLCPP_PUBLIC + bool + take_type_erased_response(void * response_out, rmw_request_id_t & request_header_out); + RCLCPP_PUBLIC const char * get_service_name() const; @@ -171,6 +192,25 @@ class Client : public ClientBase { } + /// Take the next response for this client. + /** + * \sa ClientBase::take_type_erased_response(). + * + * \param[out] response_out The reference to a Service Response into + * which the middleware will copy the response being taken. + * \param[out] request_header_out The request header to be filled by the + * middleware when taking, and which can be used to associte the response + * to a specific request. + * \returns true if the response was taken, otherwise false. + * \throws rclcpp::exceptions::RCLError based exceptions if the underlying + * rcl function fail. + */ + bool + take_response(typename ServiceT::Response & response_out, rmw_request_id_t & request_header_out) + { + return this->take_type_erased_response(&response_out, request_header_out); + } + std::shared_ptr create_response() override { diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 9b7a120102..fc3184697d 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -44,8 +44,7 @@ class ServiceBase RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase) RCLCPP_PUBLIC - explicit ServiceBase( - std::shared_ptr node_handle); + explicit ServiceBase(std::shared_ptr node_handle); RCLCPP_PUBLIC virtual ~ServiceBase(); @@ -62,9 +61,36 @@ class ServiceBase std::shared_ptr get_service_handle() const; - virtual std::shared_ptr create_request() = 0; - virtual std::shared_ptr create_request_header() = 0; - virtual void handle_request( + /// Take the next request from the service as a type erased pointer. + /** + * This type erased version of \sa Service::take_request() is useful when + * using the service in a type agnostic way with methods like + * ServiceBase::create_request(), ServiceBase::create_request_header(), and + * ServiceBase::handle_request(). + * + * \param[out] request_out The type erased pointer to a service request object + * into which the middleware will copy the taken request. + * \param[out] request_id_out The output id for the request which can be used + * to associate response with this request in the future. + * \returns true if the request was taken, otherwise false. + * \throws rclcpp::exceptions::RCLError based exceptions if the underlying + * rcl calls fail. + */ + RCLCPP_PUBLIC + bool + take_type_erased_request(void * request_out, rmw_request_id_t & request_id_out); + + virtual + std::shared_ptr + create_request() = 0; + + virtual + std::shared_ptr + create_request_header() = 0; + + virtual + void + handle_request( std::shared_ptr request_header, std::shared_ptr request) = 0; @@ -222,36 +248,63 @@ class Service : public ServiceBase { } - std::shared_ptr create_request() override + /// Take the next request from the service. + /** + * \sa ServiceBase::take_type_erased_request(). + * + * \param[out] request_out The reference to a service request object + * into which the middleware will copy the taken request. + * \param[out] request_id_out The output id for the request which can be used + * to associate response with this request in the future. + * \returns true if the request was taken, otherwise false. + * \throws rclcpp::exceptions::RCLError based exceptions if the underlying + * rcl calls fail. + */ + bool + take_request(typename ServiceT::Request & request_out, rmw_request_id_t & request_id_out) { - return std::shared_ptr(new typename ServiceT::Request()); + return this->take_type_erased_request(&request_out, request_id_out); } - std::shared_ptr create_request_header() override + std::shared_ptr + create_request() override { - // TODO(wjwwood): This should probably use rmw_request_id's allocator. - // (since it is a C type) - return std::shared_ptr(new rmw_request_id_t); + return std::make_shared(); } - void handle_request( + std::shared_ptr + create_request_header() override + { + return std::make_shared(); + } + + void + handle_request( std::shared_ptr request_header, std::shared_ptr request) override { auto typed_request = std::static_pointer_cast(request); - auto response = std::shared_ptr(new typename ServiceT::Response); + auto response = std::make_shared(); any_callback_.dispatch(request_header, typed_request, response); - send_response(request_header, response); + send_response(*request_header, *response); } - void send_response( + [[deprecated("use the send_response() which takes references instead of shared pointers")]] + void + send_response( std::shared_ptr req_id, std::shared_ptr response) { - rcl_ret_t status = rcl_send_response(get_service_handle().get(), req_id.get(), response.get()); + send_response(*req_id, *response); + } - if (status != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error(status, "failed to send response"); + void + send_response(rmw_request_id_t & req_id, typename ServiceT::Response & response) + { + rcl_ret_t ret = rcl_send_response(get_service_handle().get(), &req_id, &response); + + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send response"); } } diff --git a/rclcpp/include/rclcpp/wait_set.hpp b/rclcpp/include/rclcpp/wait_set.hpp index cc0894fa6f..5d74edc016 100644 --- a/rclcpp/include/rclcpp/wait_set.hpp +++ b/rclcpp/include/rclcpp/wait_set.hpp @@ -66,8 +66,8 @@ template< std::size_t NumberOfSubscriptions, std::size_t NumberOfGuardCondtions, std::size_t NumberOfTimers, - // std::size_t NumberOfClients, - // std::size_t NumberOfServices, + std::size_t NumberOfClients, + std::size_t NumberOfServices, std::size_t NumberOfWaitables > using StaticWaitSet = rclcpp::WaitSetTemplate< @@ -75,8 +75,8 @@ using StaticWaitSet = rclcpp::WaitSetTemplate< NumberOfSubscriptions, NumberOfGuardCondtions, NumberOfTimers, - // NumberOfClients, - // NumberOfServices, + NumberOfClients, + NumberOfServices, NumberOfWaitables >, rclcpp::wait_set_policies::SequentialSynchronization diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp index 9c39a33c02..a8b97c72a7 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp @@ -43,8 +43,8 @@ class StoragePolicyCommon class SubscriptionsIterable, class GuardConditionsIterable, class TimersIterable, - // class ServicesIterable, - // class ClientsIterable, + class ClientsIterable, + class ServicesIterable, class WaitablesIterable > explicit @@ -52,6 +52,8 @@ class StoragePolicyCommon const SubscriptionsIterable & subscriptions, const GuardConditionsIterable & guard_conditions, const TimersIterable & timers, + const ClientsIterable & clients, + const ServicesIterable & services, const WaitablesIterable & waitables, rclcpp::Context::SharedPtr context ) @@ -83,8 +85,8 @@ class StoragePolicyCommon subscriptions.size() + subscriptions_from_waitables, guard_conditions.size() + guard_conditions_from_waitables, timers.size() + timers_from_waitables, - clients_from_waitables, - services_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 @@ -98,6 +100,8 @@ class StoragePolicyCommon subscriptions, guard_conditions, timers, + clients, + services, waitables); } @@ -142,8 +146,8 @@ class StoragePolicyCommon class SubscriptionsIterable, class GuardConditionsIterable, class TimersIterable, - // class ServicesIterable, - // class ClientsIterable, + class ClientsIterable, + class ServicesIterable, class WaitablesIterable > void @@ -151,6 +155,8 @@ class StoragePolicyCommon const SubscriptionsIterable & subscriptions, const GuardConditionsIterable & guard_conditions, const TimersIterable & timers, + const ClientsIterable & clients, + const ServicesIterable & services, const WaitablesIterable & waitables ) { @@ -196,8 +202,8 @@ class StoragePolicyCommon subscriptions.size() + subscriptions_from_waitables, guard_conditions.size() + guard_conditions_from_waitables, timers.size() + timers_from_waitables, - clients_from_waitables, - services_from_waitables, + clients.size() + clients_from_waitables, + services.size() + services_from_waitables, events_from_waitables ); if (RCL_RET_OK != ret) { @@ -291,6 +297,52 @@ class StoragePolicyCommon } } + // 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); diff --git a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp index 47720157bb..b77398fee4 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp @@ -20,8 +20,10 @@ #include #include +#include "rclcpp/client.hpp" #include "rclcpp/guard_condition.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/service.hpp" #include "rclcpp/subscription_base.hpp" #include "rclcpp/subscription_wait_set_mask.hpp" #include "rclcpp/timer.hpp" @@ -101,6 +103,12 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo using SequenceOfWeakTimers = std::vector>; using TimersIterable = std::vector>; + using SequenceOfWeakClients = std::vector>; + using ClientsIterable = std::vector>; + + using SequenceOfWeakServices = std::vector>; + using ServicesIterable = std::vector>; + class WaitableEntry { public: @@ -160,16 +168,29 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo const SubscriptionsIterable & subscriptions, const GuardConditionsIterable & guard_conditions, const TimersIterable & timers, + const ClientsIterable & clients, + const ServicesIterable & services, const WaitablesIterable & waitables, rclcpp::Context::SharedPtr context ) - : StoragePolicyCommon(subscriptions, guard_conditions, timers, waitables, context), + : StoragePolicyCommon( + subscriptions, + 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()) {} @@ -183,6 +204,8 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo subscriptions_, guard_conditions_, timers_, + clients_, + services_, waitables_ ); } @@ -273,6 +296,48 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo this->storage_flag_for_resize(); } + void + storage_add_client(std::shared_ptr && client) + { + if (this->storage_has_entity(*client, clients_)) { + throw std::runtime_error("client already in wait set"); + } + clients_.push_back(std::move(client)); + this->storage_flag_for_resize(); + } + + void + storage_remove_client(std::shared_ptr && client) + { + auto it = this->storage_find_entity(*client, clients_); + if (clients_.cend() == it) { + throw std::runtime_error("client not in wait set"); + } + clients_.erase(it); + this->storage_flag_for_resize(); + } + + void + storage_add_service(std::shared_ptr && service) + { + if (this->storage_has_entity(*service, services_)) { + throw std::runtime_error("service already in wait set"); + } + services_.push_back(std::move(service)); + this->storage_flag_for_resize(); + } + + void + storage_remove_service(std::shared_ptr && service) + { + auto it = this->storage_find_entity(*service, services_); + if (services_.cend() == it) { + throw std::runtime_error("service not in wait set"); + } + services_.erase(it); + this->storage_flag_for_resize(); + } + void storage_add_waitable( std::shared_ptr && waitable, @@ -314,6 +379,8 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo // 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)); } @@ -335,6 +402,8 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo // 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) { @@ -364,6 +433,8 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo }; reset_all(shared_guard_conditions_); reset_all(shared_timers_); + reset_all(shared_clients_); + reset_all(shared_services_); reset_all(shared_waitables_); } @@ -378,6 +449,12 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo SequenceOfWeakTimers timers_; TimersIterable shared_timers_; + SequenceOfWeakClients clients_; + ClientsIterable shared_clients_; + + SequenceOfWeakServices services_; + ServicesIterable shared_services_; + SequenceOfWeakWaitables waitables_; WaitablesIterable shared_waitables_; }; diff --git a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp index 9fb48b6183..12b3814170 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp @@ -20,9 +20,11 @@ #include #include +#include "rclcpp/client.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/guard_condition.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/service.hpp" #include "rclcpp/subscription_base.hpp" #include "rclcpp/subscription_wait_set_mask.hpp" #include "rclcpp/timer.hpp" @@ -128,6 +130,58 @@ class SequentialSynchronization : public detail::SynchronizationPolicyCommon remove_timer_function(std::move(timer)); } + /// Add client without thread-safety. + /** + * Does not throw, but storage function may throw. + */ + void + sync_add_client( + std::shared_ptr && client, + std::function&&)> add_client_function) + { + // Explicitly no thread synchronization. + add_client_function(std::move(client)); + } + + /// Remove client without thread-safety. + /** + * Does not throw, but storage function may throw. + */ + void + sync_remove_client( + std::shared_ptr && client, + std::function&&)> remove_client_function) + { + // Explicitly no thread synchronization. + remove_client_function(std::move(client)); + } + + /// Add service without thread-safety. + /** + * Does not throw, but storage function may throw. + */ + void + sync_add_service( + std::shared_ptr && service, + std::function&&)> add_service_function) + { + // Explicitly no thread synchronization. + add_service_function(std::move(service)); + } + + /// Remove service without thread-safety. + /** + * Does not throw, but storage function may throw. + */ + void + sync_remove_service( + std::shared_ptr && service, + std::function&&)> remove_service_function) + { + // Explicitly no thread synchronization. + remove_service_function(std::move(service)); + } + /// Add waitable without thread-safety. /** * Does not throw, but storage function may throw. diff --git a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp index f2cdafa7fe..43eec62a61 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp @@ -18,8 +18,10 @@ #include #include +#include "rclcpp/client.hpp" #include "rclcpp/guard_condition.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/service.hpp" #include "rclcpp/subscription_base.hpp" #include "rclcpp/subscription_wait_set_mask.hpp" #include "rclcpp/timer.hpp" @@ -41,8 +43,8 @@ template< std::size_t NumberOfSubscriptions, std::size_t NumberOfGuardCondtions, std::size_t NumberOfTimers, - // std::size_t NumberOfClients, - // std::size_t NumberOfServices, + std::size_t NumberOfClients, + std::size_t NumberOfServices, std::size_t NumberOfWaitables > class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCommon @@ -82,6 +84,18 @@ class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCom >; using TimersIterable = ArrayOfTimers; + using ArrayOfClients = std::array< + std::shared_ptr, + NumberOfClients + >; + using ClientsIterable = ArrayOfClients; + + using ArrayOfServices = std::array< + std::shared_ptr, + NumberOfServices + >; + using ServicesIterable = ArrayOfServices; + struct WaitableEntry { /// Conversion constructor, which is intentionally not marked explicit. @@ -106,13 +120,24 @@ class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCom const ArrayOfSubscriptions & subscriptions, const ArrayOfGuardConditions & guard_conditions, const ArrayOfTimers & timers, + const ArrayOfClients & clients, + const ArrayOfServices & services, const ArrayOfWaitables & waitables, rclcpp::Context::SharedPtr context ) - : StoragePolicyCommon(subscriptions, guard_conditions, timers, waitables, context), + : StoragePolicyCommon( + subscriptions, + guard_conditions, + timers, + clients, + services, + waitables, + context), subscriptions_(subscriptions), guard_conditions_(guard_conditions), timers_(timers), + clients_(clients), + services_(services), waitables_(waitables) {} @@ -125,6 +150,8 @@ class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCom subscriptions_, guard_conditions_, timers_, + clients_, + services_, waitables_ ); } @@ -135,6 +162,10 @@ class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCom // 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 @@ -154,6 +185,8 @@ class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCom const ArrayOfSubscriptions subscriptions_; const ArrayOfGuardConditions guard_conditions_; const ArrayOfTimers timers_; + const ArrayOfClients clients_; + const ArrayOfServices services_; const ArrayOfWaitables waitables_; }; diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp index e57d67df70..af9a06c725 100644 --- a/rclcpp/include/rclcpp/wait_set_template.hpp +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -21,11 +21,13 @@ #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" @@ -69,6 +71,8 @@ class WaitSetTemplate final : private StoragePolicy, private SynchronizationPoli 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()) @@ -76,6 +80,8 @@ class WaitSetTemplate final : private StoragePolicy, private SynchronizationPoli subscriptions, guard_conditions, timers, + clients, + services, waitables, context), SynchronizationPolicy() @@ -341,6 +347,110 @@ class WaitSetTemplate final : private StoragePolicy, private SynchronizationPoli }); } + /// 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. + * \throws exceptions based on the policies used. + */ + void + add_client(std::shared_ptr client) + { + if (nullptr == client) { + throw std::invalid_argument("client is nullptr"); + } + // this method comes from the SynchronizationPolicy + this->sync_add_client( + std::move(client), + [this](std::shared_ptr && inner_client) { + // This method comes from the StoragePolicy, and it may not exist for + // fixed sized storage policies. + // It will throw if the client has already been added. + this->storage_add_client(std::move(inner_client)); + }); + } + + /// Remove a client from this wait set. + /** + * \sa remove_guard_condition() for details of how this method works. + * + * \param[in] client Client to be removed. + * \throws std::invalid_argument if client is nullptr. + * \throws std::runtime_error if client is not part of the wait set. + * \throws exceptions based on the policies used. + */ + void + remove_client(std::shared_ptr client) + { + if (nullptr == client) { + throw std::invalid_argument("client is nullptr"); + } + // this method comes from the SynchronizationPolicy + this->sync_remove_client( + std::move(client), + [this](std::shared_ptr && inner_client) { + // 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. + * \throws exceptions based on the policies used. + */ + void + add_service(std::shared_ptr service) + { + if (nullptr == service) { + throw std::invalid_argument("service is nullptr"); + } + // this method comes from the SynchronizationPolicy + this->sync_add_service( + std::move(service), + [this](std::shared_ptr && inner_service) { + // This method comes from the StoragePolicy, and it may not exist for + // fixed sized storage policies. + // It will throw if the service has already been added. + this->storage_add_service(std::move(inner_service)); + }); + } + + /// Remove a service from this wait set. + /** + * \sa remove_guard_condition() for details of how this method works. + * + * \param[in] service Service to be removed. + * \throws std::invalid_argument if service is nullptr. + * \throws std::runtime_error if service is not part of the wait set. + * \throws exceptions based on the policies used. + */ + void + remove_service(std::shared_ptr service) + { + if (nullptr == service) { + throw std::invalid_argument("service is nullptr"); + } + // this method comes from the SynchronizationPolicy + this->sync_remove_service( + std::move(service), + [this](std::shared_ptr && inner_service) { + // 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. diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index c2f64e06f7..eb42a4ba65 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -70,6 +70,21 @@ ClientBase::~ClientBase() client_handle_.reset(); } +bool +ClientBase::take_type_erased_response(void * response_out, rmw_request_id_t & request_header_out) +{ + rcl_ret_t ret = rcl_take_response( + this->get_client_handle().get(), + &request_header_out, + response_out); + if (RCL_RET_CLIENT_TAKE_FAILED == ret) { + return false; + } else if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + return true; +} + const char * ClientBase::get_service_name() const { diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 84c3f2b5a7..65880a7a1d 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -305,54 +305,56 @@ Executor::execute_any_executable(AnyExecutable & any_exec) } } +static +void +take_and_do_error_handling( + const char * action_description, + const char * topic_or_service_name, + std::function take_action, + std::function handle_action) +{ + bool taken; + 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; - // Reduce code duplication by generalize error handling into lambda. - auto take_message_and_do_error_handling = - [&subscription]( - const char * message_adjective, - std::function take_action, - std::function handle_action) - { - bool taken; - try { - taken = take_action(); - } catch (const rclcpp::exceptions::RCLError & rcl_error) { - RCLCPP_ERROR( - rclcpp::get_logger("rclcpp"), - "executor taking a %smessage from topic '%s' unexpectedly failed: %s", - message_adjective, - subscription->get_topic_name(), - rcl_error.what()); - } - if (taken) { - handle_action(); - } else { - // Message 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 a subscription actually having data until trying - // to take the data. - RCLCPP_DEBUG( - rclcpp::get_logger("rclcpp"), - "executor taking a %smessage from topic '%s' failed to take anything", - message_adjective, - subscription->get_topic_name()); - } - }; - if (subscription->is_serialized()) { // This is the case where a copy of the serialized message is taken from // the middleware via inter-process communication. std::shared_ptr serialized_msg = subscription->create_serialized_message(); - take_message_and_do_error_handling( - "serialzed ", + take_and_do_error_handling( + "taking a serialized message from topic", + subscription->get_topic_name(), [&]() {return subscription->take_serialized(*serialized_msg.get(), message_info);}, [&]() { @@ -367,8 +369,9 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) void * loaned_msg = nullptr; // TODO(wjwwood): refactor this into methods on subscription when LoanedMessage // is extened to support subscriptions as well. - take_message_and_do_error_handling( - "loaned ", + take_and_do_error_handling( + "taking a loaned message from topic", + subscription->get_topic_name(), [&]() { rcl_ret_t ret = rcl_take_loaned_message( @@ -398,8 +401,9 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) // This case is taking a copy of the message data from the middleware via // inter-process communication. std::shared_ptr message = subscription->create_message(); - take_message_and_do_error_handling( - "", // no message adjective + 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); @@ -417,19 +421,11 @@ Executor::execute_service(rclcpp::ServiceBase::SharedPtr service) { auto request_header = service->create_request_header(); std::shared_ptr request = service->create_request(); - rcl_ret_t status = rcl_take_request( - service->get_service_handle().get(), - request_header.get(), - request.get()); - if (status == RCL_RET_OK) { - service->handle_request(request_header, request); - } else if (status != RCL_RET_SERVICE_TAKE_FAILED) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "take request failed for server of service '%s': %s", - service->get_service_name(), rcl_get_error_string().str); - rcl_reset_error(); - } + take_and_do_error_handling( + "taking a service server request from service", + service->get_service_name(), + [&]() {return service->take_type_erased_request(request.get(), *request_header);}, + [&]() {service->handle_request(request_header, request);}); } void @@ -438,19 +434,11 @@ Executor::execute_client( { auto request_header = client->create_request_header(); std::shared_ptr response = client->create_response(); - rcl_ret_t status = rcl_take_response( - client->get_client_handle().get(), - request_header.get(), - response.get()); - if (status == RCL_RET_OK) { - client->handle_response(request_header, response); - } else if (status != RCL_RET_CLIENT_TAKE_FAILED) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "take response failed for client of service '%s': %s", - client->get_service_name(), rcl_get_error_string().str); - rcl_reset_error(); - } + take_and_do_error_handling( + "taking a service client response from service", + client->get_service_name(), + [&]() {return client->take_type_erased_response(response.get(), *request_header);}, + [&]() {client->handle_response(request_header, response);}); } void diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index c7f8ff9449..bd457370fe 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -34,6 +34,21 @@ ServiceBase::ServiceBase(std::shared_ptr node_handle) ServiceBase::~ServiceBase() {} +bool +ServiceBase::take_type_erased_request(void * request_out, rmw_request_id_t & request_id_out) +{ + rcl_ret_t ret = rcl_take_request( + this->get_service_handle().get(), + &request_id_out, + request_out); + if (RCL_RET_CLIENT_TAKE_FAILED == ret) { + return false; + } else if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + return true; +} + const char * ServiceBase::get_service_name() { diff --git a/rclcpp/test/test_subscription.cpp b/rclcpp/test/test_subscription.cpp index d64959fe80..95f9b96448 100644 --- a/rclcpp/test/test_subscription.cpp +++ b/rclcpp/test/test_subscription.cpp @@ -262,7 +262,7 @@ TEST_F(TestSubscription, callback_bind) { TEST_F(TestSubscription, take) { initialize(); using test_msgs::msg::Empty; - auto do_nothing = [](std::shared_ptr) { FAIL(); }; + auto do_nothing = [](std::shared_ptr) {FAIL();}; { auto sub = node->create_subscription("~/test_take", 1, do_nothing); test_msgs::msg::Empty msg; @@ -299,7 +299,7 @@ TEST_F(TestSubscription, take) { TEST_F(TestSubscription, take_serialized) { initialize(); using test_msgs::msg::Empty; - auto do_nothing = [](std::shared_ptr) { FAIL(); }; + auto do_nothing = [](std::shared_ptr) {FAIL();}; { auto sub = node->create_subscription("~/test_take", 1, do_nothing); std::shared_ptr msg = sub->create_serialized_message(); diff --git a/rclcpp/test/test_wait_set.cpp b/rclcpp/test/test_wait_set.cpp index ddd91c56c9..2fff5ff68e 100644 --- a/rclcpp/test/test_wait_set.cpp +++ b/rclcpp/test/test_wait_set.cpp @@ -42,6 +42,8 @@ TEST_F(TestWaitSet, construction_and_destruction) { std::vector{}, std::vector{}, std::vector{}, + std::vector{}, + std::vector{}, std::vector{}); (void)wait_set; } @@ -56,7 +58,7 @@ TEST_F(TestWaitSet, construction_and_destruction) { auto context = std::make_shared(); context->init(0, nullptr); auto gc = std::make_shared(context); - rclcpp::WaitSet wait_set({}, {gc}, {}, {}, context); + rclcpp::WaitSet wait_set({}, {gc}, {}, {}, {}, {}, context); (void)wait_set; } @@ -68,6 +70,8 @@ TEST_F(TestWaitSet, construction_and_destruction) { std::vector{}, std::vector{}, std::vector{}, + std::vector{}, + std::vector{}, std::vector{}, nullptr); (void)wait_set; @@ -83,6 +87,8 @@ TEST_F(TestWaitSet, construction_and_destruction) { std::vector{}, std::vector{}, std::vector{}, + std::vector{}, + std::vector{}, std::vector{}, context); (void)wait_set; From 0575ee561f4284ca1a1bef4822408bbf9545e9b2 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 8 Apr 2020 23:41:36 -0700 Subject: [PATCH 13/21] fix typo Signed-off-by: William Woodall --- .../rclcpp/wait_set_policies/sequential_synchronization.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp index 12b3814170..b020d6e296 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp @@ -248,7 +248,7 @@ class SequentialSynchronization : public detail::SynchronizationPolicyCommon do { // Rebuild the wait set. // This will resize the wait set if needed, due to e.g. adding or removing - // entities since the laster wait, but this should never occur in static + // 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. From a32ddc638d184d419c2f5abf2137e7ebe47731fc Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 8 Apr 2020 23:42:36 -0700 Subject: [PATCH 14/21] fix typo Signed-off-by: William Woodall --- rclcpp/include/rclcpp/wait_set_template.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp index af9a06c725..1be8adf80f 100644 --- a/rclcpp/include/rclcpp/wait_set_template.hpp +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -559,7 +559,7 @@ class WaitSetTemplate final : private StoragePolicy, private SynchronizationPoli * 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::stead_clock. + * 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 From c9042d048bb46924beaa73402ac2af7398e59adc Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 8 Apr 2020 23:46:34 -0700 Subject: [PATCH 15/21] fix review comment Signed-off-by: William Woodall --- rclcpp/src/rclcpp/executor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 65880a7a1d..556c42ddde 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -313,7 +313,7 @@ take_and_do_error_handling( std::function take_action, std::function handle_action) { - bool taken; + bool taken = false; try { taken = take_action(); } catch (const rclcpp::exceptions::RCLError & rcl_error) { From 2ff0f0b819c717d8de4995c398aa858ba6518a95 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 9 Apr 2020 06:45:35 -0700 Subject: [PATCH 16/21] add thread-safe wait set policy Signed-off-by: William Woodall --- rclcpp/CMakeLists.txt | 1 + rclcpp/include/rclcpp/wait_set.hpp | 18 +- .../detail/storage_policy_common.hpp | 61 +-- .../write_preferring_read_write_lock.hpp | 243 +++++++++++ .../wait_set_policies/dynamic_storage.hpp | 7 +- .../sequential_synchronization.hpp | 14 +- .../wait_set_policies/static_storage.hpp | 7 +- .../thread_safe_synchronization.hpp | 379 ++++++++++++++++++ rclcpp/include/rclcpp/wait_set_template.hpp | 17 +- .../write_preferring_read_write_lock.cpp | 100 +++++ 10 files changed, 806 insertions(+), 41 deletions(-) create mode 100644 rclcpp/include/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp create mode 100644 rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp create mode 100644 rclcpp/src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index a89e485fa5..7426a1319d 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -86,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 ) diff --git a/rclcpp/include/rclcpp/wait_set.hpp b/rclcpp/include/rclcpp/wait_set.hpp index 5d74edc016..5cb8f791ca 100644 --- a/rclcpp/include/rclcpp/wait_set.hpp +++ b/rclcpp/include/rclcpp/wait_set.hpp @@ -25,7 +25,7 @@ #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_policies/thread_safe_synchronization.hpp" #include "rclcpp/wait_set_template.hpp" namespace rclcpp @@ -43,8 +43,8 @@ namespace rclcpp * \sa rclcpp::WaitSetTemplate for API documentation */ using WaitSet = rclcpp::WaitSetTemplate< - rclcpp::wait_set_policies::DynamicStorage, - rclcpp::wait_set_policies::SequentialSynchronization + rclcpp::wait_set_policies::SequentialSynchronization, + rclcpp::wait_set_policies::DynamicStorage >; /// WaitSet configuration which does not allow changes after construction. @@ -71,6 +71,7 @@ template< std::size_t NumberOfWaitables > using StaticWaitSet = rclcpp::WaitSetTemplate< + rclcpp::wait_set_policies::SequentialSynchronization, rclcpp::wait_set_policies::StaticStorage< NumberOfSubscriptions, NumberOfGuardCondtions, @@ -78,8 +79,7 @@ using StaticWaitSet = rclcpp::WaitSetTemplate< NumberOfClients, NumberOfServices, NumberOfWaitables - >, - rclcpp::wait_set_policies::SequentialSynchronization + > >; /// Like WaitSet, this configuration is dynamic, but is also thread-safe. @@ -96,10 +96,10 @@ using StaticWaitSet = rclcpp::WaitSetTemplate< * * \sa rclcpp::WaitSetTemplate for API documentation */ -// using ThreadSafeWaitSet = rclcpp::WaitSetTemplate< -// rclcpp::wait_set_policies::DynamicStorage, -// rclcpp::wait_set_policies::ThreadSafeSynchronization -// >; +using ThreadSafeWaitSet = rclcpp::WaitSetTemplate< + rclcpp::wait_set_policies::ThreadSafeSynchronization, + rclcpp::wait_set_policies::DynamicStorage +>; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp index a8b97c72a7..4f0075cb0f 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp @@ -42,6 +42,7 @@ class StoragePolicyCommon template< class SubscriptionsIterable, class GuardConditionsIterable, + class ExtraGuardConditionsIterable, class TimersIterable, class ClientsIterable, class ServicesIterable, @@ -51,6 +52,7 @@ class StoragePolicyCommon StoragePolicyCommon( const SubscriptionsIterable & subscriptions, const GuardConditionsIterable & guard_conditions, + const ExtraGuardConditionsIterable & extra_guard_conditions, const TimersIterable & timers, const ClientsIterable & clients, const ServicesIterable & services, @@ -83,7 +85,7 @@ class StoragePolicyCommon rcl_ret_t ret = rcl_wait_set_init( &rcl_wait_set_, subscriptions.size() + subscriptions_from_waitables, - guard_conditions.size() + guard_conditions_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, @@ -99,6 +101,7 @@ class StoragePolicyCommon this->storage_rebuild_rcl_wait_set_with_sets( subscriptions, guard_conditions, + extra_guard_conditions, timers, clients, services, @@ -145,6 +148,7 @@ class StoragePolicyCommon template< class SubscriptionsIterable, class GuardConditionsIterable, + class ExtraGuardConditionsIterable, class TimersIterable, class ClientsIterable, class ServicesIterable, @@ -154,6 +158,7 @@ class StoragePolicyCommon 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, @@ -200,7 +205,7 @@ class StoragePolicyCommon rcl_ret_t ret = rcl_wait_set_resize( &rcl_wait_set_, subscriptions.size() + subscriptions_from_waitables, - guard_conditions.size() + guard_conditions_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, @@ -251,28 +256,38 @@ class StoragePolicyCommon } } - // Add guard conditions. - for (const auto & guard_condition : 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"); + // 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); + } } - // 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) { diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp new file mode 100644 index 0000000000..4dd3694817 --- /dev/null +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp @@ -0,0 +1,243 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__WAIT_SET_POLICIES__DETAIL__WRITE_PREFERRING_READ_WRITE_LOCK_HPP_ +#define RCLCPP__WAIT_SET_POLICIES__DETAIL__WRITE_PREFERRING_READ_WRITE_LOCK_HPP_ + +#include +#include +#include + +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace wait_set_policies +{ +namespace detail +{ + +/// Writer-perferring read-write lock. +/** + * This class is based on an implementation of a "write-preferring RW lock" as described in this + * wikipedia page: + * + * https://en.wikipedia.org/wiki/Readers%E2%80%93writer_lock#Using_a_condition_variable_and_a_mutex + * + * Copying here for posterity: + * + * \verbatim + * For a write-preferring RW lock one can use two integer counters and one boolean flag: + * + * num_readers_active: the number of readers that have acquired the lock (integer) + * num_writers_waiting: the number of writers waiting for access (integer) + * writer_active: whether a writer has acquired the lock (boolean). + * + * Initially num_readers_active and num_writers_waiting are zero and writer_active is false. + * + * The lock and release operations can be implemented as + * + * Begin Read + * + * Lock g + * While num_writers_waiting > 0 or writer_active: + * wait cond, g[a] + * Increment num_readers_active + * Unlock g. + * + * End Read + * + * Lock g + * Decrement num_readers_active + * If num_readers_active = 0: + * Notify cond (broadcast) + * Unlock g. + * + * Begin Write + * + * Lock g + * Increment num_writers_waiting + * While num_readers_active > 0 or writer_active is true: + * wait cond, g + * Decrement num_writers_waiting + * Set writer_active to true + * Unlock g. + * + * End Write + * + * Lock g + * Set writer_active to false + * Notify cond (broadcast) + * Unlock g. + * \endverbatim + * + * It will prefer any waiting write calls to any waiting read calls, meaning + * that excessive write calls can starve read calls. + * + * This class diverges from that design in two important ways. + * First, it is a single reader, single writer version. + * Second, it allows for user defined code to be run after a writer enters the + * waiting state, and the purpose of this feature is to allow the user to + * interrupt any potentially long blocking read activities. + * + * Together these two features allow new waiting writers to not only ensure + * they get the lock before any queued readers, but also that it can safely + * interrupt read activities if needed, without allowing new read activities to + * start before it gains the lock. + * + * The first difference prevents the case that a multiple read activities occur + * at the same time but the writer can only reliably interrupt one of them. + * By preventing multiple read activities concurrently, this case is avoided. + * The second difference allows the user to define how to interrupt read + * activity that could be blocking the write activities that need to happen + * as soon as possible. + * + * To implement the differences, this class replaces the "num_readers_active" + * counter with a "reader_active" boolean. + * It also changes the "Begin Read" section from above, like this: + * + * \verbatim + * Begin Read + * + * Lock g + * While num_writers_waiting > 0 or writer_active or reader_active: // changed + * wait cond, g[a] + * Set reader_active to true // changed + * Unlock g. + * \endverbatim + * + * And changes the "End Read" section from above, like this: + * + * \verbatim + * End Read + * + * Lock g + * Set reader_active to false // changed + * Notify cond (broadcast) // changed, now unconditional + * Unlock g. + * \endverbatim + * + * The "Begin Write" section is also updated as follows: + * + * \verbatim + * Begin Write + * + * Lock g + * Increment num_writers_waiting + * Call user defined enter_waiting function // new + * While reader_active is true or writer_active is true: // changed + * wait cond, g + * Decrement num_writers_waiting + * Set writer_active to true + * Unlock g. + * \endverbatim + * + * The implementation uses a single condition variable, single lock, and several + * state variables. + * + * The typical use of this class is as follows: + * + * class MyClass + * { + * WritePreferringReadWriteLock wprw_lock_; + * public: + * MyClass() {} + * void do_some_reading() + * { + * using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + * std::lock_guard lock(wprw_lock_.get_read_mutex()); + * // Do reading... + * } + * void do_some_writing() + * { + * using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + * std::lock_guard lock(wprw_lock_.get_write_mutex()); + * // Do writing... + * } + * }; + */ +class WritePreferringReadWriteLock final +{ +public: + RCLCPP_PUBLIC + explicit WritePreferringReadWriteLock(std::function enter_waiting_function = nullptr); + + /// Read mutex for the WritePreferringReadWriteLock. + /** + * Implements the "C++ named requirements: BasicLockable". + */ + class RCLCPP_PUBLIC ReadMutex + { + public: + void + lock(); + + void + unlock(); + + protected: + explicit ReadMutex(WritePreferringReadWriteLock & parent_lock); + + WritePreferringReadWriteLock & parent_lock_; + + friend WritePreferringReadWriteLock; + }; + + /// Write mutex for the WritePreferringReadWriteLock. + /** + * Implements the "C++ named requirements: BasicLockable". + */ + class RCLCPP_PUBLIC WriteMutex + { + public: + void + lock(); + + void + unlock(); + + protected: + explicit WriteMutex(WritePreferringReadWriteLock & parent_lock); + + WritePreferringReadWriteLock & parent_lock_; + + friend WritePreferringReadWriteLock; + }; + + /// Return read mutex which can be used with standard constructs like std::lock_guard. + RCLCPP_PUBLIC + ReadMutex & + get_read_mutex(); + + /// Return write mutex which can be used with standard constructs like std::lock_guard. + RCLCPP_PUBLIC + WriteMutex & + get_write_mutex(); + +protected: + bool reader_active_ = false; + std::size_t number_of_writers_waiting_ = 0; + bool writer_active_ = false; + std::mutex mutex_; + std::condition_variable condition_variable_; + ReadMutex read_mutex_; + WriteMutex write_mutex_; + std::function enter_waiting_function_; +}; + +} // namespace detail +} // namespace wait_set_policies +} // namespace rclcpp + +#endif // RCLCPP__WAIT_SET_POLICIES__DETAIL__WRITE_PREFERRING_READ_WRITE_LOCK_HPP_ diff --git a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp index b77398fee4..907fc86bdf 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp @@ -163,10 +163,12 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo using SequenceOfWeakWaitables = std::vector; using WaitablesIterable = std::vector; + template explicit DynamicStorage( const SubscriptionsIterable & subscriptions, const GuardConditionsIterable & guard_conditions, + const ArrayOfExtraGuardConditions & extra_guard_conditions, const TimersIterable & timers, const ClientsIterable & clients, const ServicesIterable & services, @@ -176,6 +178,7 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo : StoragePolicyCommon( subscriptions, guard_conditions, + extra_guard_conditions, timers, clients, services, @@ -197,12 +200,14 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo ~DynamicStorage() = default; + template void - storage_rebuild_rcl_wait_set() + 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_, diff --git a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp index b020d6e296..5ebf32bb72 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp @@ -43,9 +43,21 @@ namespace wait_set_policies class SequentialSynchronization : public detail::SynchronizationPolicyCommon { protected: - SequentialSynchronization() = default; + explicit SequentialSynchronization(rclcpp::Context::SharedPtr) {} ~SequentialSynchronization() = default; + /// Return any "extra" guard conditions needed to implement the synchronization policy. + /** + * Since this policy provides no thread-safety, it also needs no extra guard + * conditions to implement it. + */ + const std::array, 0> & + get_extra_guard_conditions() + { + static const std::array, 0> empty{}; + return empty; + } + /// Add subscription without thread-safety. /** * Does not throw, but storage function may throw. diff --git a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp index 43eec62a61..d4d6c0c014 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp @@ -115,10 +115,12 @@ class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCom >; using WaitablesIterable = ArrayOfWaitables; + template explicit StaticStorage( const ArrayOfSubscriptions & subscriptions, const ArrayOfGuardConditions & guard_conditions, + const ArrayOfExtraGuardConditions & extra_guard_conditions, const ArrayOfTimers & timers, const ArrayOfClients & clients, const ArrayOfServices & services, @@ -128,6 +130,7 @@ class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCom : StoragePolicyCommon( subscriptions, guard_conditions, + extra_guard_conditions, timers, clients, services, @@ -143,12 +146,14 @@ class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCom ~StaticStorage() = default; + template void - storage_rebuild_rcl_wait_set() + 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_, diff --git a/rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp b/rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp new file mode 100644 index 0000000000..ea74769ad4 --- /dev/null +++ b/rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp @@ -0,0 +1,379 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__WAIT_SET_POLICIES__THREAD_SAFE_SYNCHRONIZATION_HPP_ +#define RCLCPP__WAIT_SET_POLICIES__THREAD_SAFE_SYNCHRONIZATION_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/client.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/guard_condition.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/subscription_base.hpp" +#include "rclcpp/subscription_wait_set_mask.hpp" +#include "rclcpp/timer.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rclcpp/wait_result.hpp" +#include "rclcpp/wait_result_kind.hpp" +#include "rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp" +#include "rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp" +#include "rclcpp/waitable.hpp" + +namespace rclcpp +{ +namespace wait_set_policies +{ + +/// WaitSet policy that provides thread-safe synchronization for the wait set. +/** + * This class uses a "write-preferring RW lock" so that adding items to, and + * removing items from, the wait set will take priority over reading, i.e. + * waiting. + * This is done since add and remove calls will interrupt the wait set anyways + * so it is wasteful to do "fair" locking when there are many add/remove + * operations queued up. + * + * There are some things to consider about the thread-safety provided by this + * policy. + * There are two categories of activities, reading and writing activities. + * The writing activities include all of the add and remove methods, as well as + * the prune_deleted_entities() method. + * The reading methods include the wait() method and keeping a WaitResult in + * scope. + * The reading and writing activities will not be run at the same time, and one + * will block the other. + * Therefore, if you are holding a WaitResult in scope, and try to add or + * remove an entity at the same time, they will block each other. + * The write activities will try to interrupt the wait() method by triggering + * a guard condition, but they have no way of causing the WaitResult to release + * its lock. + */ +class ThreadSafeSynchronization : public detail::SynchronizationPolicyCommon +{ +protected: + explicit ThreadSafeSynchronization(rclcpp::Context::SharedPtr context) + : extra_guard_conditions_{{std::make_shared(context)}}, + wprw_lock_([this]() {this->interrupt_waiting_wait_set();}) + {} + ~ThreadSafeSynchronization() = default; + + /// Return any "extra" guard conditions needed to implement the synchronization policy. + /** + * This policy has one guard condition which is used to interrupt the wait + * set when adding and removing entities. + */ + const std::array, 1> & + get_extra_guard_conditions() + { + return extra_guard_conditions_; + } + + /// Interrupt any waiting wait set. + /** + * Used to interrupt the wait set when adding or removing items. + */ + void + interrupt_waiting_wait_set() + { + extra_guard_conditions_[0]->trigger(); + } + + /// Add subscription. + void + sync_add_subscription( + std::shared_ptr && subscription, + const rclcpp::SubscriptionWaitSetMask & mask, + std::function< + void(std::shared_ptr&&, const rclcpp::SubscriptionWaitSetMask &) + > add_subscription_function) + { + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_write_mutex()); + add_subscription_function(std::move(subscription), mask); + } + + /// Remove guard condition. + void + sync_remove_subscription( + std::shared_ptr && subscription, + const rclcpp::SubscriptionWaitSetMask & mask, + std::function< + void(std::shared_ptr&&, const rclcpp::SubscriptionWaitSetMask &) + > remove_subscription_function) + { + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_write_mutex()); + remove_subscription_function(std::move(subscription), mask); + } + + /// Add guard condition. + void + sync_add_guard_condition( + std::shared_ptr && guard_condition, + std::function&&)> add_guard_condition_function) + { + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_write_mutex()); + add_guard_condition_function(std::move(guard_condition)); + } + + /// Remove guard condition. + void + sync_remove_guard_condition( + std::shared_ptr && guard_condition, + std::function&&)> remove_guard_condition_function) + { + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_write_mutex()); + remove_guard_condition_function(std::move(guard_condition)); + } + + /// Add timer. + void + sync_add_timer( + std::shared_ptr && timer, + std::function&&)> add_timer_function) + { + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_write_mutex()); + add_timer_function(std::move(timer)); + } + + /// Remove timer. + void + sync_remove_timer( + std::shared_ptr && timer, + std::function&&)> remove_timer_function) + { + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_write_mutex()); + remove_timer_function(std::move(timer)); + } + + /// Add client. + void + sync_add_client( + std::shared_ptr && client, + std::function&&)> add_client_function) + { + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_write_mutex()); + add_client_function(std::move(client)); + } + + /// Remove client. + void + sync_remove_client( + std::shared_ptr && client, + std::function&&)> remove_client_function) + { + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_write_mutex()); + remove_client_function(std::move(client)); + } + + /// Add service. + void + sync_add_service( + std::shared_ptr && service, + std::function&&)> add_service_function) + { + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_write_mutex()); + add_service_function(std::move(service)); + } + + /// Remove service. + void + sync_remove_service( + std::shared_ptr && service, + std::function&&)> remove_service_function) + { + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_write_mutex()); + remove_service_function(std::move(service)); + } + + /// Add waitable. + void + sync_add_waitable( + std::shared_ptr && waitable, + std::shared_ptr && associated_entity, + std::function< + void(std::shared_ptr&&, std::shared_ptr&&) + > add_waitable_function) + { + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_write_mutex()); + add_waitable_function(std::move(waitable), std::move(associated_entity)); + } + + /// Remove waitable. + void + sync_remove_waitable( + std::shared_ptr && waitable, + std::function&&)> remove_waitable_function) + { + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_write_mutex()); + remove_waitable_function(std::move(waitable)); + } + + /// Prune deleted entities. + void + sync_prune_deleted_entities(std::function prune_deleted_entities_function) + { + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_write_mutex()); + prune_deleted_entities_function(); + } + + /// Implements wait. + template + WaitResultT + sync_wait( + std::chrono::nanoseconds time_to_wait_ns, + std::function rebuild_rcl_wait_set, + std::function get_rcl_wait_set, + std::function create_wait_result) + { + // Assumption: this function assumes that some measure has been taken to + // ensure none of the entities being waited on by the wait set are allowed + // to go out of scope and therefore be deleted. + // In the case of the StaticStorage policy, this is ensured because it + // retains shared ownership of all entites for the duration of its own life. + // In the case of the DynamicStorage policy, this is ensured by the function + // which calls this function, by acquiring shared ownership of the entites + // for the duration of this function. + + // Setup looping predicate. + auto start = std::chrono::steady_clock::now(); + std::function should_loop = this->create_loop_predicate(time_to_wait_ns, start); + + // Wait until exit condition is met. + do { + { + // We have to prevent the entity sets from being mutated while building + // the rcl wait set. + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_read_mutex()); + // Rebuild the wait set. + // This will resize the wait set if needed, due to e.g. adding or removing + // entities since the last wait, but this should never occur in static + // storage wait sets since they cannot be changed after construction. + // This will also clear the wait set and re-add all the entities, which + // prepares it to be waited on again. + rebuild_rcl_wait_set(); + } + + rcl_wait_set_t & rcl_wait_set = get_rcl_wait_set(); + + // Wait unconditionally until timeout condition occurs since we assume + // there are no conditions that would require the wait to stop and reset, + // like asynchronously adding or removing an entity, i.e. explicitly + // providing no thread-safety. + + // Calculate how much time there is left to wait, unless blocking indefinitely. + auto time_left_to_wait_ns = this->calculate_time_left_to_wait(time_to_wait_ns, start); + + // Then wait for entities to become ready. + + // It is ok to wait while not having the lock acquired, because the state + // in the rcl wait set will not be updated until this method calls + // rebuild_rcl_wait_set(). + rcl_ret_t ret = rcl_wait(&rcl_wait_set, time_left_to_wait_ns.count()); + if (RCL_RET_OK == ret) { + // Something has become ready in the wait set, first check if it was + // the guard condition added by this class and/or a user defined guard condition. + const rcl_guard_condition_t * interrupt_guard_condition_ptr = + &(extra_guard_conditions_[0]->get_rcl_guard_condition()); + bool was_interrupted_by_this_class = false; + bool any_user_guard_conditions_triggered = false; + for (size_t index = 0; index < rcl_wait_set.size_of_guard_conditions; ++index) { + const rcl_guard_condition_t * current = rcl_wait_set.guard_conditions[index]; + if (nullptr != current) { + // Something is ready. + if (rcl_wait_set.guard_conditions[index] == interrupt_guard_condition_ptr) { + // This means that this class triggered a guard condition to interrupt this wait. + was_interrupted_by_this_class = true; + } else { + // This means it was a user guard condition. + any_user_guard_conditions_triggered = true; + } + } + } + + if (!was_interrupted_by_this_class || any_user_guard_conditions_triggered) { + // In this case we know: + // - something was ready + // - it was either: + // - not interrupted by this class, or + // - maybe it was, but there were also user defined guard conditions. + // + // We cannot ignore user defined guard conditions, but we can ignore + // other kinds of user defined entities, because they will still be + // ready next time we wait, whereas guard conditions are cleared. + // Therefore we need to create a WaitResult and return it. + + // The WaitResult will call sync_wait_result_acquire() and + // sync_wait_result_release() to ensure thread-safety by preventing + // the mutation of the entity sets while introspecting after waiting. + return create_wait_result(WaitResultKind::Ready); + } + // If we get here the we interrupted the wait set and there were no user + // guard conditions that needed to be handled. + // So we will loop and it will re-acquire the lock and rebuild the + // rcl wait set. + } else if (RCL_RET_TIMEOUT == ret) { + // The wait set timed out, exit the loop. + break; + } else if (RCL_RET_WAIT_SET_EMPTY == ret) { + // Wait set was empty, return Empty. + return create_wait_result(WaitResultKind::Empty); + } else { + // Some other error case, throw. + rclcpp::exceptions::throw_from_rcl_error(ret); + } + } while (should_loop()); + + // Wait did not result in ready items, return timeout. + return create_wait_result(WaitResultKind::Timeout); + } + + void + sync_wait_result_acquire() + { + wprw_lock_.get_read_mutex().lock(); + } + + void + sync_wait_result_release() + { + wprw_lock_.get_read_mutex().unlock(); + } + +protected: + std::array, 1> extra_guard_conditions_; + rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock wprw_lock_; +}; + +} // namespace wait_set_policies +} // namespace rclcpp + +#endif // RCLCPP__WAIT_SET_POLICIES__THREAD_SAFE_SYNCHRONIZATION_HPP_ diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp index 1be8adf80f..5f012aaed1 100644 --- a/rclcpp/include/rclcpp/wait_set_template.hpp +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -43,8 +43,8 @@ namespace rclcpp * This class uses the rcl_wait_set_t as storage, but it also helps manage the * ownership of associated rclcpp types. */ -template -class WaitSetTemplate final : private StoragePolicy, private SynchronizationPolicy +template +class WaitSetTemplate final : private SynchronizationPolicy, private StoragePolicy { public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(WaitSetTemplate) @@ -76,15 +76,17 @@ class WaitSetTemplate final : private StoragePolicy, private SynchronizationPoli const typename StoragePolicy::WaitablesIterable & waitables = {}, rclcpp::Context::SharedPtr context = rclcpp::contexts::default_context::get_global_default_context()) - : StoragePolicy( + : SynchronizationPolicy(context), + StoragePolicy( subscriptions, guard_conditions, + // this method comes from the SynchronizationPolicy + this->get_extra_guard_conditions(), timers, clients, services, waitables, - context), - SynchronizationPolicy() + context) {} /// Return the internal rcl wait set object. @@ -606,7 +608,10 @@ class WaitSetTemplate final : private StoragePolicy, private SynchronizationPoli // 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->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 & { diff --git a/rclcpp/src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp b/rclcpp/src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp new file mode 100644 index 0000000000..83cc2387cc --- /dev/null +++ b/rclcpp/src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp @@ -0,0 +1,100 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp" + +namespace rclcpp +{ +namespace wait_set_policies +{ +namespace detail +{ + +WritePreferringReadWriteLock::WritePreferringReadWriteLock( + std::function enter_waiting_function) +: read_mutex_(*this), write_mutex_(*this), enter_waiting_function_(enter_waiting_function) +{} + +WritePreferringReadWriteLock::ReadMutex & +WritePreferringReadWriteLock::get_read_mutex() +{ + return read_mutex_; +} + +WritePreferringReadWriteLock::WriteMutex & +WritePreferringReadWriteLock::get_write_mutex() +{ + return write_mutex_; +} + +WritePreferringReadWriteLock::ReadMutex::ReadMutex(WritePreferringReadWriteLock & parent_lock) +: parent_lock_(parent_lock) +{} + +void +WritePreferringReadWriteLock::ReadMutex::lock() +{ + std::unique_lock lock(parent_lock_.mutex_); + while ( + parent_lock_.number_of_writers_waiting_ > 0 || + parent_lock_.writer_active_ || + parent_lock_.reader_active_) + { + parent_lock_.condition_variable_.wait(lock); + } + parent_lock_.reader_active_ = true; + // implicit unlock of parent_lock_.mutex_ +} + +void +WritePreferringReadWriteLock::ReadMutex::unlock() +{ + std::unique_lock lock(parent_lock_.mutex_); + parent_lock_.reader_active_ = false; + parent_lock_.condition_variable_.notify_all(); + // implicit unlock of parent_lock_.mutex_ +} + +WritePreferringReadWriteLock::WriteMutex::WriteMutex(WritePreferringReadWriteLock & parent_lock) +: parent_lock_(parent_lock) +{} + +void +WritePreferringReadWriteLock::WriteMutex::lock() +{ + std::unique_lock lock(parent_lock_.mutex_); + parent_lock_.number_of_writers_waiting_ += 1; + if (nullptr != parent_lock_.enter_waiting_function_) { + parent_lock_.enter_waiting_function_(); + } + while (parent_lock_.reader_active_ || parent_lock_.writer_active_) { + parent_lock_.condition_variable_.wait(lock); + } + parent_lock_.number_of_writers_waiting_ -= 1; + parent_lock_.writer_active_ = true; + // implicit unlock of parent_lock_.mutex_ +} + +void +WritePreferringReadWriteLock::WriteMutex::unlock() +{ + std::unique_lock lock(parent_lock_.mutex_); + parent_lock_.writer_active_ = false; + parent_lock_.condition_variable_.notify_all(); + // implicit unlock of parent_lock_.mutex_ +} + +} // namespace detail +} // namespace wait_set_policies +} // namespace rclcpp From 4dad5aab99fcc7391913345834254b158a4bb4bd Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 9 Apr 2020 14:49:00 -0700 Subject: [PATCH 17/21] add check for use with multiple wait set Signed-off-by: William Woodall --- rclcpp/CMakeLists.txt | 1 + rclcpp/include/rclcpp/client.hpp | 16 ++++ rclcpp/include/rclcpp/guard_condition.hpp | 16 ++++ rclcpp/include/rclcpp/service.hpp | 16 ++++ rclcpp/include/rclcpp/subscription_base.hpp | 28 ++++++- rclcpp/include/rclcpp/timer.hpp | 16 ++++ .../write_preferring_read_write_lock.hpp | 12 +-- rclcpp/include/rclcpp/wait_set_template.hpp | 79 ++++++++++++++++--- rclcpp/include/rclcpp/waitable.hpp | 18 +++++ rclcpp/src/rclcpp/client.cpp | 6 ++ rclcpp/src/rclcpp/guard_condition.cpp | 6 ++ rclcpp/src/rclcpp/service.cpp | 6 ++ rclcpp/src/rclcpp/subscription_base.cpp | 22 ++++++ rclcpp/src/rclcpp/timer.cpp | 6 ++ .../write_preferring_read_write_lock.cpp | 2 +- rclcpp/src/rclcpp/waitable.cpp | 6 ++ rclcpp/test/test_wait_set.cpp | 63 +++++++++++++++ 17 files changed, 300 insertions(+), 19 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 7426a1319d..1389ad8aeb 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -531,6 +531,7 @@ if(BUILD_TESTING) 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() diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 6651661781..eef367f489 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__CLIENT_HPP_ #define RCLCPP__CLIENT_HPP_ +#include #include #include #include @@ -114,6 +115,19 @@ class ClientBase virtual void handle_response( std::shared_ptr request_header, std::shared_ptr response) = 0; + /// Exchange the "in use by wait set" state for this client. + /** + * This is used to ensure this client is not used by multiple + * wait sets at the same time. + * + * \param[in] in_use_state the new state to exchange into the state, true + * indicates it is now in use by a wait set, and false is that it is no + * longer in use by a wait set. + * \returns the previous state. + */ + bool + exchange_in_use_by_wait_set_state(bool in_use_state); + protected: RCLCPP_DISABLE_COPY(ClientBase) @@ -134,6 +148,8 @@ class ClientBase std::shared_ptr context_; std::shared_ptr client_handle_; + + std::atomic in_use_by_wait_set_{false}; }; template diff --git a/rclcpp/include/rclcpp/guard_condition.hpp b/rclcpp/include/rclcpp/guard_condition.hpp index 3d15cc7416..e73b6bd789 100644 --- a/rclcpp/include/rclcpp/guard_condition.hpp +++ b/rclcpp/include/rclcpp/guard_condition.hpp @@ -15,6 +15,8 @@ #ifndef RCLCPP__GUARD_CONDITION_HPP_ #define RCLCPP__GUARD_CONDITION_HPP_ +#include + #include "rcl/guard_condition.h" #include "rclcpp/context.hpp" @@ -73,9 +75,23 @@ class GuardCondition 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. + */ + bool + exchange_in_use_by_wait_set_state(bool in_use_state); + protected: rclcpp::Context::SharedPtr context_; rcl_guard_condition_t rcl_guard_condition_; + std::atomic in_use_by_wait_set_{false}; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index fc3184697d..e0235803c2 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__SERVICE_HPP_ #define RCLCPP__SERVICE_HPP_ +#include #include #include #include @@ -94,6 +95,19 @@ class ServiceBase std::shared_ptr request_header, std::shared_ptr request) = 0; + /// Exchange the "in use by wait set" state for this service. + /** + * This is used to ensure this service is not used by multiple + * wait sets at the same time. + * + * \param[in] in_use_state the new state to exchange into the state, true + * indicates it is now in use by a wait set, and false is that it is no + * longer in use by a wait set. + * \returns the previous state. + */ + bool + exchange_in_use_by_wait_set_state(bool in_use_state); + protected: RCLCPP_DISABLE_COPY(ServiceBase) @@ -109,6 +123,8 @@ class ServiceBase std::shared_ptr service_handle_; bool owns_rcl_handle_ = true; + + std::atomic in_use_by_wait_set_{false}; }; template diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index abe764de33..7c806f3f4e 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -15,9 +15,12 @@ #ifndef RCLCPP__SUBSCRIPTION_BASE_HPP_ #define RCLCPP__SUBSCRIPTION_BASE_HPP_ +#include #include #include +#include #include +#include #include "rcl/subscription.h" @@ -111,7 +114,7 @@ class SubscriptionBase : public std::enable_shared_from_this rclcpp::QoS get_actual_qos() const; - /// Take the next inter-process message from the subscription as a type erased poiner. + /// Take the next inter-process message from the subscription as a type erased pointer. /** * \sa Subscription::take() for details on how this function works. * @@ -231,6 +234,23 @@ class SubscriptionBase : public std::enable_shared_from_this rclcpp::Waitable::SharedPtr get_intra_process_waitable() const; + /// Exchange state of whether or not a part of the subscription is used by a wait set. + /** + * Used to ensure parts of the subscription are not used with multiple wait + * sets simultaneously. + * + * \param[in] pointer_to_subscription_part address of a subscription part + * \param[in] in_use_state the new state to exchange, true means "now in use", + * and false means "no longer in use". + * \returns the current "in use" state. + * \throws std::invalid_argument If pointer_to_subscription_part is nullptr. + * \throws std::runtime_error If the pointer given is not a pointer to one of + * the parts of the subscription which can be used with a wait set. + */ + RCLCPP_PUBLIC + bool + exchange_in_use_by_wait_set_state(void * pointer_to_subscription_part, bool in_use_state); + protected: template void @@ -243,6 +263,7 @@ class SubscriptionBase : public std::enable_shared_from_this rcl_subscription_event_init, get_subscription_handle().get(), event_type); + qos_events_in_use_by_wait_set_.insert(std::make_pair(handler.get(), false)); event_handlers_.emplace_back(handler); } @@ -267,6 +288,11 @@ class SubscriptionBase : public std::enable_shared_from_this rosidl_message_type_support_t type_support_; bool is_serialized_; + + std::atomic subscription_in_use_by_wait_set_{false}; + std::atomic intra_process_subscription_waitable_in_use_by_wait_set_{false}; + std::unordered_map> qos_events_in_use_by_wait_set_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 23b1be2789..d82d1cf8b8 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__TIMER_HPP_ #define RCLCPP__TIMER_HPP_ +#include #include #include #include @@ -101,9 +102,24 @@ class TimerBase RCLCPP_PUBLIC bool is_ready(); + /// Exchange the "in use by wait set" state for this timer. + /** + * This is used to ensure this timer is not used by multiple + * wait sets at the same time. + * + * \param[in] in_use_state the new state to exchange into the state, true + * indicates it is now in use by a wait set, and false is that it is no + * longer in use by a wait set. + * \returns the previous state. + */ + bool + exchange_in_use_by_wait_set_state(bool in_use_state); + protected: Clock::SharedPtr clock_; std::shared_ptr timer_handle_; + + std::atomic in_use_by_wait_set_{false}; }; diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp index 4dd3694817..90b9df0178 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp @@ -171,7 +171,7 @@ class WritePreferringReadWriteLock final { public: RCLCPP_PUBLIC - explicit WritePreferringReadWriteLock(std::function enter_waiting_function = nullptr); + explicit WritePreferringReadWriteLock(std::function enter_waiting_function = nullptr); /// Read mutex for the WritePreferringReadWriteLock. /** @@ -179,14 +179,14 @@ class WritePreferringReadWriteLock final */ class RCLCPP_PUBLIC ReadMutex { - public: +public: void lock(); void unlock(); - protected: +protected: explicit ReadMutex(WritePreferringReadWriteLock & parent_lock); WritePreferringReadWriteLock & parent_lock_; @@ -200,14 +200,14 @@ class WritePreferringReadWriteLock final */ class RCLCPP_PUBLIC WriteMutex { - public: +public: void lock(); void unlock(); - protected: +protected: explicit WriteMutex(WritePreferringReadWriteLock & parent_lock); WritePreferringReadWriteLock & parent_lock_; @@ -233,7 +233,7 @@ class WritePreferringReadWriteLock final std::condition_variable condition_variable_; ReadMutex read_mutex_; WriteMutex write_mutex_; - std::function enter_waiting_function_; + std::function enter_waiting_function_; }; } // namespace detail diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp index 5f012aaed1..97543cb51f 100644 --- a/rclcpp/include/rclcpp/wait_set_template.hpp +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -122,7 +122,8 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli * \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. + * \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 @@ -146,19 +147,39 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // 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; - this->storage_add_waitable( - std::move(inner_subscription->get_intra_process_waitable()), - std::move(local_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)); + } } }); } @@ -200,16 +221,19 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // 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)); @@ -241,7 +265,8 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli * * \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. + * \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 @@ -254,6 +279,10 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli this->sync_add_guard_condition( std::move(guard_condition), [this](std::shared_ptr && inner_guard_condition) { + bool already_in_use = inner_guard_condition->exchange_in_use_by_wait_set_state(true); + if (already_in_use) { + throw std::runtime_error("guard condition already in use by another wait set"); + } // This method comes from the StoragePolicy, and it may not exist for // fixed sized storage policies. // It will throw if the guard condition has already been added. @@ -290,6 +319,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli this->sync_remove_guard_condition( std::move(guard_condition), [this](std::shared_ptr && inner_guard_condition) { + inner_guard_condition->exchange_in_use_by_wait_set_state(false); // This method comes from the StoragePolicy, and it may not exist for // fixed sized storage policies. // It will throw if the guard condition is not in the wait set. @@ -303,7 +333,8 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli * * \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. + * \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 @@ -316,6 +347,10 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli this->sync_add_timer( std::move(timer), [this](std::shared_ptr && inner_timer) { + bool already_in_use = inner_timer->exchange_in_use_by_wait_set_state(true); + if (already_in_use) { + throw std::runtime_error("timer already in use by another wait set"); + } // This method comes from the StoragePolicy, and it may not exist for // fixed sized storage policies. // It will throw if the timer has already been added. @@ -342,6 +377,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli this->sync_remove_timer( std::move(timer), [this](std::shared_ptr && inner_timer) { + inner_timer->exchange_in_use_by_wait_set_state(false); // This method comes from the StoragePolicy, and it may not exist for // fixed sized storage policies. // It will throw if the timer is not in the wait set. @@ -355,7 +391,8 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli * * \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. + * \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 @@ -368,6 +405,10 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli this->sync_add_client( std::move(client), [this](std::shared_ptr && inner_client) { + bool already_in_use = inner_client->exchange_in_use_by_wait_set_state(true); + if (already_in_use) { + throw std::runtime_error("client already in use by another wait set"); + } // This method comes from the StoragePolicy, and it may not exist for // fixed sized storage policies. // It will throw if the client has already been added. @@ -394,6 +435,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli this->sync_remove_client( std::move(client), [this](std::shared_ptr && inner_client) { + inner_client->exchange_in_use_by_wait_set_state(false); // This method comes from the StoragePolicy, and it may not exist for // fixed sized storage policies. // It will throw if the client is not in the wait set. @@ -407,7 +449,8 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli * * \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. + * \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 @@ -420,6 +463,10 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli this->sync_add_service( std::move(service), [this](std::shared_ptr && inner_service) { + bool already_in_use = inner_service->exchange_in_use_by_wait_set_state(true); + if (already_in_use) { + throw std::runtime_error("service already in use by another wait set"); + } // This method comes from the StoragePolicy, and it may not exist for // fixed sized storage policies. // It will throw if the service has already been added. @@ -446,6 +493,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli this->sync_remove_service( std::move(service), [this](std::shared_ptr && inner_service) { + inner_service->exchange_in_use_by_wait_set_state(false); // This method comes from the StoragePolicy, and it may not exist for // fixed sized storage policies. // It will throw if the service is not in the wait set. @@ -473,7 +521,8 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli * \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. + * \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 @@ -488,11 +537,18 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli this->sync_add_waitable( std::move(waitable), std::move(associated_entity), - [this](std::shared_ptr && inner_waitable) { + [this]( + std::shared_ptr && inner_waitable, + std::shared_ptr && associated_entity) + { + bool already_in_use = inner_waitable->exchange_in_use_by_wait_set_state(true); + if (already_in_use) { + throw std::runtime_error("waitable already in use by another wait set"); + } // This method comes from the StoragePolicy, and it may not exist for // fixed sized storage policies. // It will throw if the waitable has already been added. - this->storage_add_waitable(std::move(inner_waitable)); + this->storage_add_waitable(std::move(inner_waitable), std::move(associated_entity)); }); } @@ -515,6 +571,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli this->sync_remove_waitable( std::move(waitable), [this](std::shared_ptr && inner_waitable) { + inner_waitable->exchange_in_use_by_wait_set_state(false); // This method comes from the StoragePolicy, and it may not exist for // fixed sized storage policies. // It will throw if the waitable is not in the wait set. diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 5f79079a4e..6aa5b238c0 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -15,6 +15,8 @@ #ifndef RCLCPP__WAITABLE_HPP_ #define RCLCPP__WAITABLE_HPP_ +#include + #include "rclcpp/macros.hpp" #include "rclcpp/visibility_control.hpp" @@ -145,6 +147,22 @@ class Waitable virtual void execute() = 0; + + /// Exchange the "in use by wait set" state for this timer. + /** + * This is used to ensure this timer is not used by multiple + * wait sets at the same time. + * + * \param[in] in_use_state the new state to exchange into the state, true + * indicates it is now in use by a wait set, and false is that it is no + * longer in use by a wait set. + * \returns the previous state. + */ + bool + exchange_in_use_by_wait_set_state(bool in_use_state); + +private: + std::atomic in_use_by_wait_set_{false}; }; // class Waitable } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index eb42a4ba65..6a1d3934d4 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -192,3 +192,9 @@ ClientBase::get_rcl_node_handle() const { return node_handle_.get(); } + +bool +ClientBase::exchange_in_use_by_wait_set_state(bool in_use_state) +{ + return in_use_by_wait_set_.exchange(in_use_state); +} diff --git a/rclcpp/src/rclcpp/guard_condition.cpp b/rclcpp/src/rclcpp/guard_condition.cpp index 20b3719e76..22ca3f3223 100644 --- a/rclcpp/src/rclcpp/guard_condition.cpp +++ b/rclcpp/src/rclcpp/guard_condition.cpp @@ -71,4 +71,10 @@ GuardCondition::trigger() } } +bool +GuardCondition::exchange_in_use_by_wait_set_state(bool in_use_state) +{ + return in_use_by_wait_set_.exchange(in_use_state); +} + } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index bd457370fe..fb35a87725 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -78,3 +78,9 @@ ServiceBase::get_rcl_node_handle() const { return node_handle_.get(); } + +bool +ServiceBase::exchange_in_use_by_wait_set_state(bool in_use_state) +{ + return in_use_by_wait_set_.exchange(in_use_state); +} diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 8c2e8d031c..d7f0da1bbb 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -251,3 +251,25 @@ SubscriptionBase::matches_any_intra_process_publishers(const rmw_gid_t * sender_ } return ipm->matches_any_publishers(sender_gid); } + +bool +SubscriptionBase::exchange_in_use_by_wait_set_state( + void * pointer_to_subscription_part, + bool in_use_state) +{ + if (nullptr == pointer_to_subscription_part) { + throw std::invalid_argument("pointer_to_subscription_part is unexpectedly nullptr"); + } + if (this == pointer_to_subscription_part) { + return subscription_in_use_by_wait_set_.exchange(in_use_state); + } + if (get_intra_process_waitable().get() == pointer_to_subscription_part) { + return intra_process_subscription_waitable_in_use_by_wait_set_.exchange(in_use_state); + } + for (const auto & qos_event : event_handlers_) { + if (qos_event.get() == pointer_to_subscription_part) { + return qos_events_in_use_by_wait_set_[qos_event.get()].exchange(in_use_state); + } + } + throw std::runtime_error("given pointer_to_subscription_part does not match any part"); +} diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index 6f3259dd64..6dc8759973 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -135,3 +135,9 @@ TimerBase::get_timer_handle() { return timer_handle_; } + +bool +TimerBase::exchange_in_use_by_wait_set_state(bool in_use_state) +{ + return in_use_by_wait_set_.exchange(in_use_state); +} diff --git a/rclcpp/src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp b/rclcpp/src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp index 83cc2387cc..5640ad4d29 100644 --- a/rclcpp/src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp +++ b/rclcpp/src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp @@ -22,7 +22,7 @@ namespace detail { WritePreferringReadWriteLock::WritePreferringReadWriteLock( - std::function enter_waiting_function) + std::function enter_waiting_function) : read_mutex_(*this), write_mutex_(*this), enter_waiting_function_(enter_waiting_function) {} diff --git a/rclcpp/src/rclcpp/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp index 542b10a016..b76c7215e0 100644 --- a/rclcpp/src/rclcpp/waitable.cpp +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -51,3 +51,9 @@ Waitable::get_number_of_ready_guard_conditions() { return 0u; } + +bool +Waitable::exchange_in_use_by_wait_set_state(bool in_use_state) +{ + return in_use_by_wait_set_.exchange(in_use_state); +} diff --git a/rclcpp/test/test_wait_set.cpp b/rclcpp/test/test_wait_set.cpp index 2fff5ff68e..b9b05a6aa9 100644 --- a/rclcpp/test/test_wait_set.cpp +++ b/rclcpp/test/test_wait_set.cpp @@ -17,7 +17,9 @@ #include #include +#include "rcl_interfaces/srv/list_parameters.hpp" #include "rclcpp/rclcpp.hpp" +#include "test_msgs/msg/basic_types.hpp" class TestWaitSet : public ::testing::Test { @@ -215,3 +217,64 @@ TEST_F(TestWaitSet, add_remove_guard_condition) { // and expect it to try and find the original pointer. // Instead it throws due to gc being nullptr. } + +/* + * Testing adding each entity to two separate wait sets. + */ +TEST_F(TestWaitSet, add_guard_condition_to_two_different_wait_set) { + { + rclcpp::WaitSet wait_set1; + rclcpp::WaitSet wait_set2; + auto node = std::make_shared("add_guard_condition_to_two_different_wait_set"); + + auto guard_condition = std::make_shared(); + wait_set1.add_guard_condition(guard_condition); + ASSERT_THROW( + { + wait_set2.add_guard_condition(guard_condition); + }, std::runtime_error); + + auto do_nothing = [](const std::shared_ptr) {}; + auto sub = node->create_subscription("~/test", 1, do_nothing); + wait_set1.add_subscription(sub); + ASSERT_THROW( + { + wait_set2.add_subscription(sub); + }, std::runtime_error); + + auto timer = node->create_wall_timer(std::chrono::seconds(1), []() {}); + wait_set1.add_timer(timer); + ASSERT_THROW( + { + wait_set2.add_timer(timer); + }, std::runtime_error); + + auto client = node->create_client("~/test"); + wait_set1.add_client(client); + ASSERT_THROW( + { + wait_set2.add_client(client); + }, std::runtime_error); + + auto srv_do_nothing = []( + const std::shared_ptr, + std::shared_ptr) {}; + auto service = + node->create_service("~/test", srv_do_nothing); + wait_set1.add_service(service); + ASSERT_THROW( + { + wait_set2.add_service(service); + }, std::runtime_error); + + rclcpp::PublisherOptions po; + po.event_callbacks.deadline_callback = [](rclcpp::QOSDeadlineOfferedInfo &) {}; + auto pub = node->create_publisher("~/test", 1, po); + auto qos_event = pub->get_event_handlers()[0]; + wait_set1.add_waitable(qos_event, pub); + ASSERT_THROW( + { + wait_set2.add_waitable(qos_event, pub); + }, std::runtime_error); + } +} From eee418fedd7dc254aa9a1d7181125951383f637c Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 10 Apr 2020 06:29:16 -0700 Subject: [PATCH 18/21] fixup visibility macro usage Signed-off-by: William Woodall --- rclcpp/include/rclcpp/client.hpp | 1 + rclcpp/include/rclcpp/guard_condition.hpp | 1 + rclcpp/include/rclcpp/service.hpp | 1 + rclcpp/include/rclcpp/timer.hpp | 1 + rclcpp/include/rclcpp/wait_set_template.hpp | 1 - rclcpp/include/rclcpp/waitable.hpp | 1 + 6 files changed, 5 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index eef367f489..f7f1d5cb8c 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -125,6 +125,7 @@ class ClientBase * 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); diff --git a/rclcpp/include/rclcpp/guard_condition.hpp b/rclcpp/include/rclcpp/guard_condition.hpp index e73b6bd789..5088181022 100644 --- a/rclcpp/include/rclcpp/guard_condition.hpp +++ b/rclcpp/include/rclcpp/guard_condition.hpp @@ -85,6 +85,7 @@ class GuardCondition * 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); diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index e0235803c2..de720109a6 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -105,6 +105,7 @@ class ServiceBase * 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); diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index d82d1cf8b8..dd754c0748 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -112,6 +112,7 @@ class TimerBase * 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); diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp index 97543cb51f..f26c68b895 100644 --- a/rclcpp/include/rclcpp/wait_set_template.hpp +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -95,7 +95,6 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli * The state of this structure can be updated at anytime by methods like * wait(), add_*(), remove_*(), etc. */ - RCLCPP_PUBLIC const rcl_wait_set_t & get_rcl_wait_set() const { diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 6aa5b238c0..2f282349ae 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -158,6 +158,7 @@ class Waitable * 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); From c89f4811c9a6f55f5b49ee4d28f2e671db3d9185 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 10 Apr 2020 06:31:05 -0700 Subject: [PATCH 19/21] remove vestigial test case Signed-off-by: William Woodall --- rclcpp/test/test_wait_set.cpp | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/rclcpp/test/test_wait_set.cpp b/rclcpp/test/test_wait_set.cpp index b9b05a6aa9..39fe4c1262 100644 --- a/rclcpp/test/test_wait_set.cpp +++ b/rclcpp/test/test_wait_set.cpp @@ -200,22 +200,6 @@ TEST_F(TestWaitSet, add_remove_guard_condition) { wait_set.remove_guard_condition(gc); }, std::runtime_error); } - - // remove after delete, checking weak ownership behavior - // { - // auto gc = std::make_shared(); - // rclcpp::WaitSet wait_set; - // wait_set.add_guard_condition(gc); - // gc.reset(); - // ASSERT_THROW( - // { - // // gc should be missing at this point - // wait_set.remove_guard_condition(gc); - // }, std::runtime_error); - // } - // Note this case does not fail because you cannot pass a "reset" shared pointer to gc - // and expect it to try and find the original pointer. - // Instead it throws due to gc being nullptr. } /* From 571ca322b9138033b122b624acce0db79ae946b4 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 10 Apr 2020 07:56:03 -0700 Subject: [PATCH 20/21] move visibility macro fixes Signed-off-by: William Woodall --- rclcpp/include/rclcpp/subscription_base.hpp | 2 ++ rclcpp/include/rclcpp/subscription_wait_set_mask.hpp | 4 +++- rclcpp/include/rclcpp/wait_result_kind.hpp | 4 +++- 3 files changed, 8 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 7c806f3f4e..6275c69a72 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -131,6 +131,7 @@ class SubscriptionBase : public std::enable_shared_from_this * \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); @@ -148,6 +149,7 @@ class SubscriptionBase : public std::enable_shared_from_this * \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); diff --git a/rclcpp/include/rclcpp/subscription_wait_set_mask.hpp b/rclcpp/include/rclcpp/subscription_wait_set_mask.hpp index 694c312cb8..597dd81d3e 100644 --- a/rclcpp/include/rclcpp/subscription_wait_set_mask.hpp +++ b/rclcpp/include/rclcpp/subscription_wait_set_mask.hpp @@ -15,11 +15,13 @@ #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 SubscriptionWaitSetMask +class RCLCPP_PUBLIC SubscriptionWaitSetMask { public: /// If true, include the actual subscription. diff --git a/rclcpp/include/rclcpp/wait_result_kind.hpp b/rclcpp/include/rclcpp/wait_result_kind.hpp index 98ada0a31a..3ce65bf4f3 100644 --- a/rclcpp/include/rclcpp/wait_result_kind.hpp +++ b/rclcpp/include/rclcpp/wait_result_kind.hpp @@ -15,11 +15,13 @@ #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 WaitResultKind +enum RCLCPP_PUBLIC WaitResultKind { Ready, // Date: Fri, 10 Apr 2020 07:56:21 -0700 Subject: [PATCH 21/21] remove vestigial TODO Signed-off-by: William Woodall --- rclcpp/include/rclcpp/wait_set_template.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp index f26c68b895..899d461ed3 100644 --- a/rclcpp/include/rclcpp/wait_set_template.hpp +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -67,7 +67,6 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli */ explicit WaitSetTemplate( - // TODO(wjwwood): support subscription wait set masks in constructor const typename StoragePolicy::SubscriptionsIterable & subscriptions = {}, const typename StoragePolicy::GuardConditionsIterable & guard_conditions = {}, const typename StoragePolicy::TimersIterable & timers = {},