wip

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

rclcpp/doc/wait_set_architecture.md

@@ -0,0 +1,161 @@
+# Architecture of the rclcpp::WaitSet Type
+
+The `rclcpp::WaitSet` type is actually a specific configuration of a the more general `rclcpp::WaitSetTemplate` class.
+
+The `rclcpp::WaitSetTemplate` class is design to have interchangeable implementations for controlling storage of items in the wait set, as well as to control access to the wait set functionality, i.e. whether or not it has thread-safety.
+
+In general the idea behind a wait set in rclcpp is that you gather a set of entities that can be waited on, e.g. subscriptions, timers, guard conditions, and then wait for one or more of them to be ready.
+These things are driven by asynchronous events, e.g. subscriptions become ready when new data arrives or timers become ready when their period has elapsed.
+
+At the moment, things that can be waited on are limited by what can be put into an `rcl_wait_set_t`, which includes subscriptions, service clients, service servers, timers, guard conditions, and events.
+In this context events are QoS events, as defined in the `rcl` and `rmw` interfaces.
+
+## Design Goals
+
+There are some design goals for the `rclcpp::WaitSet` family of classes:
+
+- minimize inheritance and avoid virtual functions
+  - Overhead of the executor and wait set design have been a common complaint and are often blamed for performance issues.
+  - Since the wait set is at the core of the wait-do loop of the system, aggressive optimization can actually pay off.
+- avoid unnecessary changes to the underlying `rcl_wait_set_t` instances or unnecessarily recreating it
+  - This can be expensive, because under the hood that requires changes to the middleware's object, e.g. a DDS wait set or similar.
+- minimize memory allocations in the wait() function, avoid them entirely if the set of things to wait on has not changed
+- prevent accidentally using an entity with more than wait set at the same time
+
+Several of these surround performance concerns, but others are concerned with safety and convenience for the user.
+
+## Architecture
+
+The `rclcpp::WaitSetTemplate` is the core fixture in the feature, which is using a policy-based design (see https://en.wikipedia.org/wiki/Modern_C%2B%2B_Design#Policy-based_design).
+
+It delegates storage and synchronization to policy template arguments to the `rclcpp::WaitSetTemplate` via the `StoragePolicy` and `SynchronizationPolicy` template arguments respectively.
+
+These policies can be mixed and matched to achieve different results.
+There are a few predefined combinations, `rclcpp::WaitSet` which is a non-thread-safe wait set with dynamic storage, `rclcpp::StaticWaitSet` which is a non-thread-safe wait set with fixed storage, and `rclcpp::ThreadSafeWaitSet` which is a thread-safe wait set with dynamic storage.
+They are all just aliases to `rclcpp::WaitSetTemplate` with difference combinations of policies.
+
+Users can extend the behavior by implementing their own policies if desired.
+
+This kind of design allows for specialization and code sharing without using standard polymorphism via virtual functions, and the benefit of that is that calls on the wait set class are always non-virtual, avoiding dispatching via the v-table.
+It also forces us to use templates, which means a head-only implementation, which will increase compile times, but will avoid some calls to shared libraries which on some systems has a small overhead too.
+
+These are aggressive optimizations, but it can actually be very beneficial in the inner-loop of applications, and the hope is that this approach will offer the best performance we can achieve.
+
+### StoragePolicy
+
+The storage policy is responsible for storing the entities in the wait set and providing accessors and setters (if appropriate).
+
+The storage policy is also responsible for maintaining ownership of the entities it is storing at the right times.
+For example, entities are always given to the wait set as `std::shared_ptr`'s and some policies might only maintain ownership for the lifetime of the wait set or until the entity is removed.
+Other policies might only hold ownership of the entities while waiting on them, i.e. while they are actively using them, but maintain only weak ownership otherwise.
+
+There are two built-in storage policies available, the `rclcpp::wait_set_policies::DynamicStorage` and the `rclcpp::wait_set_policies::StaticStorage<...>` classes.
+
+The `StaticStorage` policy requires that you fully specify the number of entities of each time that can be stored as template arguments and uses the `std:array` class as backing storage.
+Therefore, it also does not allow adding or removing entities after construction.
+Because entities cannot be removed and are therefore "always" in use by the wait set, it maintains shared ownership of the entities for the lifetime of the wait set.
+
+The `DynamicStorage` policy provides access to the entities and allows add and removing entities at any time after construction.
+It also only maintains shared ownership of the entities while actively waiting on them, which allows the entities to go out of scope while the wait set is idle.
+Entities that go out of scope in this manner will be removed from the wait set when it notices their weak pointers no longer lock to the entity's shared pointer.
+
+Both policies allow you to initialize the wait set with entities in the constructor.
+
+### SynchronizationPolicy
+
+The synchronization policy is responsible for synchronizing access to wait set operations that share access to the StoragePolicy.
+
+There are two built-in synchronization policies available, the `rclcpp::wait_set_policies::SequentialSynchronization` and the `rclcpp::wait_set_policies::ThreadSafeSynchronization` policies.
+
+The `rclcpp::wait_set_policies::SequentialSynchronization` policy essentially does nothing but pass through requests from the user-facing `rclcpp::WaitSetTemplate` interface to the `StoragePolicy` that is being used.
+
+The `rclcpp::wait_set_policies::ThreadSafeSynchronization` policy sequences access to the `StoragePolicy` being used so that it is safe to call methods like `add_subscription()` on the wait set while it is doing something else, like waiting with the `wait()` method.
+This synchronization comes at the expense of using mutexes and extra logic to organize the access, but provides some thread-safety.
+
+### Entity Intake and Management
+
+Entities, i.e. `rclcpp::SubscriptionBase`, `rclcpp::Waitable`, `rclcpp::ClientBase`, `rclcpp::ServiceBase`, `rclcpp::TimerBase`, `rclcpp::GuardCondition`, etc., may be passed to the wait set during construction.
+If the wait set has policies that allow for dynamic storage and supports adding and removing entities after construction, then entities can also be added and removed using method on the wait set.
+
+Either way, the entities are initially given to the wait set as a `std::shared_ptr` in order to maintain shared ownership, but depending on the storage policy it may be stored as a `std::weak_ptr` while the wait set is not actively using the entities.
+
+Also, when taking on new entities the wait set is responsible for ensuring that entity is not being used by another wait set (at least another instance of `rclcpp::WaitSetTemplate`).
+
+One more thing that plays into intake of entities is that a few entity types have some extra requirements, specifically subscriptions and waitables.
+
+Subscriptions (specifically `rclcpp::SubscriptionBase`) are complex objects that contain not only a subscription that can be waited on, but also may have `rclcpp::Waitable`'s inside in order to implement intra-process communication and for QoS events that are also implemented as `rclcpp::Waitable` instances.
+Therefore, you need a way to say which parts of the subscription should be added to the wait set, because you don't need to add all the entities that make up a subscription to a single wait set.
+The `rclcpp::SubscriptionWaitSetMask` class provides this mapping, and when adding subscriptions to a wait set you need to provide the subscription shared pointer as well as a mask instance.
+
+Waitables are complicated by the fact that they often are part of a larger piece of the API, as we saw with subscriptions, and therefore the waitable may need to keep additional things in scope while being used.
+So when adding a waitable to a wait set you may also provide an "associated entity" as a shared pointer to void, which is purely there to keep something in scope along with the waitable.
+
+#### EntityEntry classes
+
+In order to address the need to pair extra information with some entities (e.g. the subscription mask with the subscription entity) and to provide a convenient place to check if the entity is in use by another wait set and claim if not.
+
+There are a series of classes that are a variation of the `EntityEntry` concept, which wrap the incoming entities and their associated data into a single class, and also provide some safety when converting between the shared ownership and weak ownership if needed.
+
+They also provide RAII-style checking about whether or not an entity is already associated with a wait set.
+This is important because when ingesting multiple entities at the same time, e.g. via the constructor, because if you claim the first few entities to be associated with the wait set, but then find one that is already associated, then you need to abort and throw an exception.
+The RAII-style of these classes addresses this by "dissociating" the first few entities from the wait set when going out of scope due to the thrown exception.
+
+The process from intake to cleanup of the entity entries follows something like this, using waitables as an example:
+
+
+
+                                    ┌───────────────┐
+   ┌────────────────────────┐       │ WaitSet       │
+   │                        ├──────►│               ├───────────────┐
+   │ shared_ptr<Waitable>   │       │ constructor   │               │
+   │                        │       └───────────────┘  Implicit     │
+   │  and optional          │                                       │
+   │                        │       ┌───────────────┐  Conversion   │
+   │ shared_ptr<void>       │       │ add_waitable()│               │
+   │                        ├──────►│               ├───────────────┤
+   └────────────────────────┘       │ method        │               │
+                                    └───────────────┘               │
+                                                                    │
+                                                                    │
+                                                                    │
+          ┌──────────────────────┐        ┌─────────────────┐       │
+          │                      │        │                 │       │
+    ┌─────┤ ManagedWaitableEntry │◄──┬────┤  WaitableEntry  │◄──────┘
+    │     │                      │   │    │                 │
+    │     └─┬──────────────────┬─┘   │    └─┬─────────────┬─┘
+    │       │ shared_ptr+RAII  │     │      │ shared_ptr  │
+    │       └──────────────────┘     │      └─────────────┘
+    │                                │
+    │                        ┌───────┴───────────┐
+    │                        │Check that entity  │
+    │ Conversion to          │is not in use, then│
+    │ weak ownership         │claim it for this  │
+    │ if needed.             │wait set.          │
+    │                        └───────────────────┘
+    │
+    │     ┌───────────────────────────┐
+    │     │                           │
+    └────►│ WeakManagedWaitableEntry  │
+          │                           │
+          └─┬───────────────────────┬─┘
+            │ weak_ptr + RAII       │
+            └───────────────────────┘
+
+Using the diagram as a reference, you can see that the input from the user is converted into the entity entry class that, for now, just stores the entity and any associated data if it exists.
+At this point the entity comes in with shared ownership and stays as shared ownership in the basic entry class.
+Then this entry class is converted into a "managed" entry class, which will check that the entity is not associated with another wait set and associate it with the current wait set during construction.
+This managed entry class will also automatically disassociate the entity with the current wait set during destruction.
+The new managed entry class continues to keep shared ownership of the entity.
+
+Optionally, if the storage policy requires it, the managed entry class can be converted into a "weak" version, where in the entity is stored as a weak pointer instead of a shared pointer.
+In this case the entity will remain stored as a "weak managed" entry until it is removed or the wait set is destroyed.
+In the meantime, if the wait set needs to take shared ownership again, it can do that be getting a copy of the weak pointer(s) in the entry and locking them to get shared pointers.
+But the weak managed entry remains responsible for the RAII-style dissociation from the current wait set when destroyed.
+Unlike the managed entry that maintains shared ownership, the weak version will need to attempt to lock the weak pointer for the entity before trying to dissociate it from the wait set, because this association is maintained within the entity itself, through methods like `rclcpp::Waitable::exchange_in_use_by_wait_set_state()`.
+If the entity cannot be accessed via the weak pointer, then it is not explicitly dissociated by the managed entry, but it doesn't matter because the object has gone out of scope and has been deleted anyway.
+So the cleanup for the weak managed entry is best effort, but that does not pose an issue.
+
+#### The EntityEntryTemplate classes
+
+There is a template class called `rclcpp::wait_set_policies::detail::EntityEntryTemplate<EntityT>` (and associated managed and weak-managed versions) which is the foundation for most of the entity types which don't require special handling, like `rclcpp::GuardCondition` and `rclcpp::TimerBase` for example.
+Other entities like `rclcpp::SubscriptionBase` and `rclcpp::Waitable` have custom specializations of these classes to handle extra storage, custom constructors, and other extra logic to help them work.

rclcpp/include/rclcpp/wait_set_policies/already_associated_with_wait_set_exception.hpp

@@ -0,0 +1,43 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__WAIT_SET_POLICIES__ALREADY_ASSOCIATED_WITH_WAIT_SET_EXCEPTION_HPP_
+#define RCLCPP__WAIT_SET_POLICIES__ALREADY_ASSOCIATED_WITH_WAIT_SET_EXCEPTION_HPP_
+
+#include <stdexcept>
+
+#include "rmw/impl/cpp/demangle.hpp"
+
+namespace rclcpp
+{
+namespace wait_set_policies
+{
+
+class AlreadyAssociatedWithWaitSetException : public std::runtime_error
+{
+public:
+  template<typename EntityT>
+  explicit
+  AlreadyAssociatedWithWaitSetException(const EntityT & entity_instance)
+  : std::runtime_error(
+    "cannot associate " +
+    rmw::impl::cpp::demangle(entity_instance) +
+    " with wait set because it is already in use by a wait set")
+  {}
+};
+
+}  // namespace wait_set_policies
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAIT_SET_POLICIES__ALREADY_ASSOCIATED_WITH_WAIT_SET_EXCEPTION_HPP_

rclcpp/include/rclcpp/wait_set_policies/detail/client_entry.hpp

@@ -0,0 +1,41 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__WAIT_SET_POLICIES__DETAIL__CLIENT_ENTRY_HPP_
+#define RCLCPP__WAIT_SET_POLICIES__DETAIL__CLIENT_ENTRY_HPP_
+
+#include <memory>
+
+#include "rclcpp/client.hpp"
+#include "rclcpp/wait_set_policies/detail/entity_entry.hpp"
+
+namespace rclcpp
+{
+namespace wait_set_policies
+{
+namespace detail
+{
+
+using ClientEntry =
+  // EntityEntryTemplate<rclcpp::ClientBase, std::shared_ptr<rclcpp::ClientBase>>;
+  EntityEntryTemplate<rclcpp::ClientBase>;
+using WeakClientEntry =
+  // EntityEntryTemplate<rclcpp::ClientBase, std::weak_ptr<rclcpp::ClientBase>>;
+  EntityEntryTemplate<rclcpp::ClientBase>;
+
+}  // namespace detail
+}  // namespace wait_set_policies
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAIT_SET_POLICIES__DETAIL__CLIENT_ENTRY_HPP_

rclcpp/include/rclcpp/wait_set_policies/detail/entity_entry.hpp

@@ -0,0 +1,183 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__WAIT_SET_POLICIES__DETAIL__ENTITY_ENTRY_HPP_
+#define RCLCPP__WAIT_SET_POLICIES__DETAIL__ENTITY_ENTRY_HPP_
+
+#include <cassert>
+#include <memory>
+
+#include "rclcpp/wait_set_policies/already_associated_with_wait_set_exception.hpp"
+
+namespace rclcpp
+{
+namespace wait_set_policies
+{
+namespace detail
+{
+
+// Forward declaration for use in friend statement.
+template<typename EntityT>
+class ManagedEntityEntryTemplate;
+
+/// Encapsulating class for wait set entities, and gateway to the ManagedEntityEntryTemplate.
+/**
+ * The entity is stored as a std::shared_ptr.
+ *
+ * This is class can be converted to a "managed" version which ensures the
+ * entity is not associated with another wait set already, then associates it
+ * with the current wait set, and then dissociates it on destruction.
+ */
+template<typename EntityT>
+class EntityEntryTemplate
+{
+public:
+  EntityEntryTemplate(std::shared_ptr<EntityT> entity_in = nullptr)
+  : entity_(entity_in)
+  {}
+private:
+  std::shared_ptr<EntityT> entity_;
+
+  friend ManagedEntityEntryTemplate<EntityT>;
+};
+
+/// Managing class for wait set entities, with RAII-style (dis)association with the wait set.
+/**
+ * The entity is stored as a std::shared_ptr, but ths class can be converted
+ * (one way) into a weak version that stores it as a std::weak_ptr.
+ *
+ * This class will assert that the entity is not already associated with a
+ * wait set, while atomically indicating it is associated with this wait set
+ * to prevent other wait sets from using it, and then on destruction this class
+ * will disassociate it.
+ *
+ * \throws rclcpp::wait_set_policies::AlreadyAssociatedWithWaitSetException if entity
+ *   is already associated with a wait set.
+ */
+template<typename EntityT>
+class ManagedEntityEntryTemplate
+{
+public:
+  /// The only valid way to construct this is with an unmanaged entity entry.
+  explicit ManagedEntityEntryTemplate(const EntityEntryTemplate<EntityT> & unmanaged_entity_entry)
+  : entity_(unmanaged_entity_entry.entity_)
+  {
+    if (nullptr == entity_) {
+      throw std::invalid_argument("entity cannot be nullptr for a managed entry");
+    }
+    bool already_in_use = entity_->exchange_in_use_by_wait_set_state(true);
+    if (already_in_use) {
+      throw rclcpp::wait_set_policies::AlreadyAssociatedWithWaitSetException(*entity_);
+    }
+  }
+
+  // ManagedEntityEntryTemplate(const ManagedEntityEntryTemplate<EntityT> & other)
+  // {
+  //   if (other.should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_) {
+  //     throw std::runtime_error("")
+  //   }
+  // }
+
+  ~ManagedEntityEntryTemplate()
+  {
+    if ((nullptr != entity_)) {
+      bool was_in_use = entity_->exchange_in_use_by_wait_set_state(false);
+      assert(was_in_use);
+    }
+  }
+
+  /// Return the interal entity shared pointer.
+  std::shared_ptr<EntityT>
+  get_entity() const
+  {
+    return entity_;
+  }
+
+  /// Reset the entity.
+  /**
+   * Specializations of this class may reset more than one item.
+   * Having this method in all instantiations of this class provides uniform access.
+   */
+  // void
+  // reset() noexcept
+  // {
+  //   entity_.reset();
+  // }
+
+protected:
+  std::shared_ptr<EntityT> entity_;
+
+};
+
+/// Version of ManagedEntityEntryTemplate with weak ownership and best effort disassociation.
+/**
+ * The entity is stored as a std::weak_ptr, but on destruction, the entity is
+ * locked, and if not nullptr, then it will be marked as not in use by a wait set.
+ */
+template<typename EntityT>
+class WeakManagedEntityEntryTemplate
+{
+public:
+  /// Can only be constructed from a moved ManagedEntityEntryTemplate.
+  explicit WeakManagedEntityEntryTemplate(ManagedEntityEntryTemplate<EntityT> && moved_entity_entry)
+  : weak_entity_(moved_entity_entry.get_entity())
+  {}
+
+  ~WeakManagedEntityEntryTemplate()
+  {
+    auto entity = weak_entity_.lock();
+    if (nullptr != entity) {
+      bool was_in_use = entity->exchange_in_use_by_wait_set_state(false);
+      assert(was_in_use);
+    }
+  }
+
+  /// Return the interal entity weak pointer.
+  std::weak_ptr<EntityT>
+  get_weak_entity() const
+  {
+    return weak_entity_;
+  }
+
+  // /// Lock the entity.
+  // /**
+  //  * Specializations of this class may select from more than one item to lock.
+  //  * Having this method in all instantiations of this class provides uniform access.
+  //  */
+  // std::shared_ptr<EntityT>
+  // lock() const
+  // {
+  //   return weak_entity_.lock();
+  // }
+
+  // /// Return true if the entity has expired, otherwise false.
+  // /**
+  //  * Specializations of this class may select from more than one item to check.
+  //  * Having this method in all instantiations of this class provides uniform access.
+  //  */
+  // bool
+  // expired() const noexcept
+  // {
+  //   return weak_entity_.expired();
+  // }
+private:
+  std::weak_ptr<EntityT> weak_entity_;
+
+};
+
+}  // namespace detail
+}  // namespace wait_set_policies
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAIT_SET_POLICIES__DETAIL__ENTITY_ENTRY_HPP_

rclcpp/include/rclcpp/wait_set_policies/detail/guard_condition_entry.hpp

@@ -0,0 +1,41 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__WAIT_SET_POLICIES__DETAIL__GUARD_CONDITION_ENTRY_HPP_
+#define RCLCPP__WAIT_SET_POLICIES__DETAIL__GUARD_CONDITION_ENTRY_HPP_
+
+#include <memory>
+
+#include "rclcpp/guard_condition.hpp"
+#include "rclcpp/wait_set_policies/detail/entity_entry.hpp"
+
+namespace rclcpp
+{
+namespace wait_set_policies
+{
+namespace detail
+{
+
+using GuardConditionEntry =
+  // EntityEntryTemplate<rclcpp::GuardCondition, std::shared_ptr<rclcpp::GuardCondition>>;
+  EntityEntryTemplate<rclcpp::GuardCondition>;
+using WeakGuardConditionEntry =
+  // EntityEntryTemplate<rclcpp::GuardCondition, std::weak_ptr<rclcpp::GuardCondition>>;
+  EntityEntryTemplate<rclcpp::GuardCondition>;
+
+}  // namespace detail
+}  // namespace wait_set_policies
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAIT_SET_POLICIES__DETAIL__GUARD_CONDITION_ENTRY_HPP_

rclcpp/include/rclcpp/wait_set_policies/detail/service_entry.hpp

@@ -0,0 +1,41 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__WAIT_SET_POLICIES__DETAIL__SERVICE_ENTRY_HPP_
+#define RCLCPP__WAIT_SET_POLICIES__DETAIL__SERVICE_ENTRY_HPP_
+
+#include <memory>
+
+#include "rclcpp/service.hpp"
+#include "rclcpp/wait_set_policies/detail/entity_entry.hpp"
+
+namespace rclcpp
+{
+namespace wait_set_policies
+{
+namespace detail
+{
+
+using ServiceEntry =
+  // EntityEntryTemplate<rclcpp::ServiceBase, std::shared_ptr<rclcpp::ServiceBase>>;
+  EntityEntryTemplate<rclcpp::ServiceBase>;
+using WeakServiceEntry =
+  // EntityEntryTemplate<rclcpp::ServiceBase, std::weak_ptr<rclcpp::ServiceBase>>;
+  EntityEntryTemplate<rclcpp::ServiceBase>;
+
+}  // namespace detail
+}  // namespace wait_set_policies
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAIT_SET_POLICIES__DETAIL__SERVICE_ENTRY_HPP_

rclcpp/include/rclcpp/wait_set_policies/detail/subscription_entry.hpp

@@ -0,0 +1,403 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__WAIT_SET_POLICIES__DETAIL__SUBSCRIPTION_ENTRY_HPP_
+#define RCLCPP__WAIT_SET_POLICIES__DETAIL__SUBSCRIPTION_ENTRY_HPP_
+
+#include <memory>
+#include <optional>
+#include <vector>
+
+#include "rclcpp/subscription_base.hpp"
+#include "rclcpp/subscription_wait_set_mask.hpp"
+#include "rclcpp/wait_set_policies/detail/entity_entry.hpp"
+#include "rclcpp/wait_set_policies/detail/waitable_entry.hpp"
+
+namespace rclcpp
+{
+namespace wait_set_policies
+{
+namespace detail
+{
+
+/// See rclcpp::wait_set_policies::detail::EntityEntryTemplate.
+template<>
+class EntityEntryTemplate<rclcpp::SubscriptionBase>
+{
+  using EntityT = rclcpp::SubscriptionBase;
+
+public:
+  EntityEntryTemplate(
+    std::shared_ptr<EntityT> entity_in = nullptr,
+    const rclcpp::SubscriptionWaitSetMask & mask_in = {})
+  : entity_(entity_in),
+    mask_(mask_in)
+  {}
+
+  /// Create ManagedEntityEntryTemplate instances for the subscription and waitables if needed.
+  /**
+   * This method uses the SubscriptionWaitSetMask to determine how to decompose
+   * the various entities within the subscription into managed entries.
+   *
+   * It optionally returns a managed entry for the subscription, if the mask
+   * indicates it should, and then optionally any waitables for the
+   * intra-process communication and QoS events.
+   */
+  std::pair<
+    std::optional<ManagedEntityEntryTemplate<rclcpp::SubscriptionBase>>,
+    std::vector<ManagedEntityEntryTemplate<rclcpp::Waitable>>
+  >
+  manage();
+  // defined below ManagedEntityEntryTemplate<rclcpp::SubscriptionBase>.
+
+private:
+  std::shared_ptr<EntityT> entity_;
+  rclcpp::SubscriptionWaitSetMask mask_;
+};
+
+/// See rclcpp::wait_set_policies::detail::ManagedEntityEntryTemplate.
+template<>
+class ManagedEntityEntryTemplate<rclcpp::SubscriptionBase>
+{
+  using EntityT = rclcpp::SubscriptionBase;
+
+  /// Only constructed by EntityEntryTemplate<rclcpp::SubscriptionBase>::manage().
+  explicit ManagedEntityEntryTemplate(std::shared_ptr<EntityT> subscription)
+  : entity_(subscription)
+  {
+    if (nullptr == entity_) {
+      throw std::invalid_argument("entity cannot be nullptr for a managed entry");
+    }
+    bool already_in_use = entity_->exchange_in_use_by_wait_set_state(entity_.get(), true);
+    if (already_in_use) {
+      throw rclcpp::wait_set_policies::AlreadyAssociatedWithWaitSetException(*entity_);
+    }
+  }
+
+public:
+  // ManagedEntityEntryTemplate(const ManagedEntityEntryTemplate<EntityT> & other)
+  // {
+  //   if (other.should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_) {
+  //     throw std::runtime_error("")
+  //   }
+  // }
+
+  ~ManagedEntityEntryTemplate()
+  {
+    if ((nullptr != entity_)) {
+      bool was_in_use = entity_->exchange_in_use_by_wait_set_state(entity_.get(), false);
+      assert(was_in_use);
+    }
+  }
+
+  /// Return the interal entity shared pointer.
+  std::shared_ptr<EntityT>
+  get_entity() const
+  {
+    return entity_;
+  }
+
+  /// Reset the entity.
+  /**
+   * Specializations of this class may reset more than one item.
+   * Having this method in all instantiations of this class provides uniform access.
+   */
+  // void
+  // reset() noexcept
+  // {
+  //   entity_.reset();
+  // }
+
+private:
+  std::shared_ptr<EntityT> entity_;
+
+  friend EntityEntryTemplate<EntityT>;
+
+};
+
+std::pair<
+  std::optional<ManagedEntityEntryTemplate<rclcpp::SubscriptionBase>>,
+  std::vector<ManagedEntityEntryTemplate<rclcpp::Waitable>>
+>
+EntityEntryTemplate<rclcpp::SubscriptionBase>::manage()
+{
+  std::optional<ManagedEntityEntryTemplate<rclcpp::SubscriptionBase>> managed_subscription_entry;
+  std::vector<ManagedEntityEntryTemplate<rclcpp::Waitable>> waitables;
+
+  if (mask_.include_subscription) {
+    managed_subscription_entry = ManagedEntityEntryTemplate<rclcpp::SubscriptionBase>(entity_);
+  }
+  if (mask_.include_events) {
+    for (const auto & event_waitable : entity_->get_event_handlers()) {
+      waitables.emplace_back(EntityEntryTemplate<rclcpp::Waitable>(event_waitable, entity_));
+    }
+  }
+  if (mask_.include_intra_process_waitable) {
+    waitables.emplace_back(
+      EntityEntryTemplate<rclcpp::Waitable>(entity_->get_intra_process_waitable(), entity_)
+    );
+  }
+
+  return {managed_subscription_entry, waitables};
+}
+
+/// See rclcpp::wait_set_policies::detail::WeakManagedEntityEntryTemplate.
+template<>
+class WeakManagedEntityEntryTemplate<rclcpp::SubscriptionBase>
+{
+  using EntityT = rclcpp::SubscriptionBase;
+
+public:
+  /// Can only be constructed from a moved ManagedEntityEntryTemplate.
+  explicit WeakManagedEntityEntryTemplate(ManagedEntityEntryTemplate<EntityT> && moved_entity_entry)
+  : weak_entity_(moved_entity_entry.get_entity())
+  {}
+
+  ~WeakManagedEntityEntryTemplate()
+  {
+    auto entity = weak_entity_.lock();
+    if (nullptr != entity) {
+      bool was_in_use = entity->exchange_in_use_by_wait_set_state(entity.get(), false);
+      assert(was_in_use);
+    }
+  }
+
+  /// Return the interal entity weak pointer.
+  std::weak_ptr<EntityT>
+  get_weak_entity() const
+  {
+    return weak_entity_;
+  }
+
+  // /// Lock the entity.
+  // /**
+  //  * Specializations of this class may select from more than one item to lock.
+  //  * Having this method in all instantiations of this class provides uniform access.
+  //  */
+  // std::shared_ptr<EntityT>
+  // lock() const
+  // {
+  //   return weak_entity_.lock();
+  // }
+
+  // /// Return true if the entity has expired, otherwise false.
+  // /**
+  //  * Specializations of this class may select from more than one item to check.
+  //  * Having this method in all instantiations of this class provides uniform access.
+  //  */
+  // bool
+  // expired() const noexcept
+  // {
+  //   return weak_entity_.expired();
+  // }
+private:
+  std::weak_ptr<EntityT> weak_entity_;
+  std::weak_ptr<void> weak_associated_entity_;
+
+};
+
+using SubscriptionEntry = EntityEntryTemplate<rclcpp::SubscriptionBase>;
+using ManagedSubscriptionEntry = ManagedEntityEntryTemplate<rclcpp::SubscriptionBase>;
+using WeakManagedSubscriptionEntry = WeakManagedEntityEntryTemplate<rclcpp::SubscriptionBase>;
+
+// /// Specialization for Subscriptions.
+// template<>
+// class EntityEntryTemplate<rclcpp::SubscriptionBase>
+// {
+//   using EntityT = rclcpp::SubscriptionBase;
+//   std::shared_ptr<EntityT> entity_;
+//   rclcpp::SubscriptionWaitSetMask mask_;
+
+// public:
+//   EntityEntryTemplate(
+//     std::shared_ptr<EntityT> entity_in = nullptr,
+//     const rclcpp::SubscriptionWaitSetMask & mask_in = {})
+//   : entity_(entity_in),
+//     mask_(mask_in)
+//   {}
+
+//   ~EntityEntryTemplate()
+//   {
+//     if (should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_) {
+//       EntityEntryTemplate<rclcpp::SubscriptionBase>::cleanup(entity_, mask_);
+//     }
+//   }
+
+//   /// See EntityEntryTemplate::get_entity().
+//   std::shared_ptr<EntityT>
+//   get_entity() const
+//   {
+//     return entity_;
+//   }
+
+//   /// Return a const reference to the subscrption mask.
+//   const rclcpp::SubscriptionWaitSetMask &
+//   get_mask() const
+//   {
+//     return mask_;
+//   }
+
+//   /// See EntityEntryTemplate::manage().
+//   std::vector<WaitableEntry>
+//   manage()
+//   {
+//     if (nullptr == entity_) {
+//       throw std::runtime_error("manage() called on EntityEntry with null entity");
+//     }
+//     auto associate = [this](void * entity_part) {
+//       bool already_in_use = entity_->exchange_in_use_by_wait_set_state(entity_part, true);
+//       if (already_in_use) {
+//         throw rclcpp::wait_set_policies::AlreadyAssociatedWithWaitSetException(*entity_);
+//       }
+//     };
+
+//     if (mask_.include_subscription) {
+//       associate(entity_.get());
+//     }
+
+//     std::vector<WaitableEntry> decomposed_waitables;
+
+//     if (mask_.include_events) {
+//       for (auto event : entity_->get_event_handlers()) {
+//         // TODO(wjwwood): do we need to use the unmanage_function argument
+//         //   to utilize the exchange_in_use_by_wait_set_state of SubscriptionBase?
+//         decomposed_waitables.emplace_back(event, entity_);
+//         decomposed_waitables.back().manage();
+//       }
+//       // mask_.include_events = false;
+//     }
+//     if (mask_.include_intra_process_waitable) {
+//       auto waitable = entity_->get_intra_process_waitable();
+//       if (nullptr != waitable) {
+//         // TODO(wjwwood): do we need to use the unmanage_function argument
+//         //   to utilize the exchange_in_use_by_wait_set_state of SubscriptionBase?
+//         decomposed_waitables.emplace_back(waitable, entity_);
+//         decomposed_waitables.back().manage();
+//         // mask_.include_intra_process_waitable = false;
+//       }
+//     }
+
+//     should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_ = true;
+
+//     return decomposed_waitables;
+//   }
+
+//   /// See EntityEntryTemplate::reset().
+//   void
+//   reset() noexcept
+//   {
+//     entity_.reset();
+//   }
+
+// protected:
+//   bool should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_{false};
+
+//   static
+//   void
+//   cleanup(std::shared_ptr<EntityT> subscription, rclcpp::SubscriptionWaitSetMask mask)
+//   {
+//     if (subscription != nullptr) {
+//       auto dissociate = [&subscription](void * entity_part) {
+//         bool was_in_use = subscription->exchange_in_use_by_wait_set_state(entity_part, false);
+//         assert(was_in_use);
+//       };
+//       if (mask.include_subscription) {
+//         dissociate(subscription.get());
+//       }
+//       if (mask.include_events) {
+//         for (auto event : subscription->get_event_handlers()) {
+//           dissociate(event.get());
+//         }
+//       }
+//       if (mask.include_intra_process_waitable) {
+//         auto waitable = subscription->get_intra_process_waitable();
+//         if (nullptr != waitable) {
+//           dissociate(waitable.get());
+//         }
+//       }
+//     }
+//   }
+
+//   friend WeakEntityEntryTemplate<EntityT>;
+
+// };
+
+// /// Specialization for Subscriptions.
+// template<>
+// class WeakEntityEntryTemplate<rclcpp::SubscriptionBase>
+// {
+//   using EntityT = rclcpp::SubscriptionBase;
+//   std::weak_ptr<EntityT> weak_entity_;
+//   rclcpp::SubscriptionWaitSetMask mask_;
+
+// public:
+//   explicit WeakEntityEntryTemplate(EntityEntryTemplate<EntityT> && moved_entity_entry)
+//   : weak_entity_(moved_entity_entry.get_entity()),
+//     mask_(moved_entity_entry.mask_),
+//     should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_(
+//       moved_entity_entry.should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_
+//     )
+//   {
+//     moved_entity_entry.should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_ = false;
+//   }
+
+//   ~WeakEntityEntryTemplate()
+//   {
+//     auto entity = weak_entity_.lock();
+//     if (should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_) {
+//       EntityEntryTemplate<rclcpp::SubscriptionBase>::cleanup(entity, mask_);
+//     }
+//   }
+
+//   /// See WeakEntityEntryTemplate::get_weak_entity().
+//   std::weak_ptr<EntityT>
+//   get_weak_entity() const
+//   {
+//     return weak_entity_;
+//   }
+
+//   /// Return a const reference to the subscrption mask.
+//   const rclcpp::SubscriptionWaitSetMask &
+//   get_mask() const
+//   {
+//     return mask_;
+//   }
+
+//   /// See WeakEntityEntryTemplate::lock().
+//   std::shared_ptr<EntityT>
+//   lock() const
+//   {
+//     return weak_entity_.lock();
+//   }
+
+//   /// See WeakEntityEntryTemplate::expired().
+//   bool
+//   expired() const noexcept
+//   {
+//     return weak_entity_.expired();
+//   }
+
+// protected:
+//   bool should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_{false};
+
+// };
+
+// using SubscriptionEntry = EntityEntryTemplate<rclcpp::SubscriptionBase>;
+// using WeakSubscriptionEntry = WeakEntityEntryTemplate<rclcpp::SubscriptionBase>;
+
+}  // namespace detail
+}  // namespace wait_set_policies
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAIT_SET_POLICIES__DETAIL__SUBSCRIPTION_ENTRY_HPP_

rclcpp/include/rclcpp/wait_set_policies/detail/timer_entry.hpp

@@ -0,0 +1,41 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__WAIT_SET_POLICIES__DETAIL__TIMER_ENTRY_HPP_
+#define RCLCPP__WAIT_SET_POLICIES__DETAIL__TIMER_ENTRY_HPP_
+
+#include <memory>
+
+#include "rclcpp/timer.hpp"
+#include "rclcpp/wait_set_policies/detail/entity_entry.hpp"
+
+namespace rclcpp
+{
+namespace wait_set_policies
+{
+namespace detail
+{
+
+using TimerEntry =
+  // EntityEntryTemplate<rclcpp::TimerBase, std::shared_ptr<rclcpp::TimerBase>>;
+  EntityEntryTemplate<rclcpp::TimerBase>;
+using WeakTimerEntry =
+  // EntityEntryTemplate<rclcpp::TimerBase, std::weak_ptr<rclcpp::TimerBase>>;
+  EntityEntryTemplate<rclcpp::TimerBase>;
+
+}  // namespace detail
+}  // namespace wait_set_policies
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAIT_SET_POLICIES__DETAIL__TIMER_ENTRY_HPP_

rclcpp/include/rclcpp/wait_set_policies/detail/waitable_entry.hpp

@@ -0,0 +1,183 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__WAIT_SET_POLICIES__DETAIL__WAITABLE_ENTRY_HPP_
+#define RCLCPP__WAIT_SET_POLICIES__DETAIL__WAITABLE_ENTRY_HPP_
+
+#include <functional>
+#include <memory>
+#include <type_traits>
+
+#include "rclcpp/waitable.hpp"
+#include "rclcpp/wait_set_policies/detail/entity_entry.hpp"
+
+namespace rclcpp
+{
+namespace wait_set_policies
+{
+namespace detail
+{
+
+/// See rclcpp::wait_set_policies::detail::EntityEntryTemplate.
+template<>
+class EntityEntryTemplate<rclcpp::Waitable>
+{
+  using EntityT = rclcpp::Waitable;
+
+public:
+  EntityEntryTemplate(
+    std::shared_ptr<EntityT> entity_in = nullptr,
+    std::shared_ptr<void> associated_entity_in = nullptr)
+  : entity_(entity_in),
+    associated_entity_(associated_entity_in)
+  {}
+private:
+  std::shared_ptr<EntityT> entity_;
+  std::shared_ptr<void> associated_entity_;
+
+  friend ManagedEntityEntryTemplate<EntityT>;
+};
+
+/// See rclcpp::wait_set_policies::detail::ManagedEntityEntryTemplate.
+template<>
+class ManagedEntityEntryTemplate<rclcpp::Waitable>
+{
+  using EntityT = rclcpp::Waitable;
+
+public:
+  /// The only valid way to construct this is with an unmanaged entity entry.
+  explicit ManagedEntityEntryTemplate(const EntityEntryTemplate<EntityT> & unmanaged_entity_entry)
+  : entity_(unmanaged_entity_entry.entity_),
+    associated_entity_(unmanaged_entity_entry.associated_entity_)
+  {
+    if (nullptr == entity_) {
+      throw std::invalid_argument("entity cannot be nullptr for a managed entry");
+    }
+    bool already_in_use = entity_->exchange_in_use_by_wait_set_state(true);
+    if (already_in_use) {
+      throw rclcpp::wait_set_policies::AlreadyAssociatedWithWaitSetException(*entity_);
+    }
+  }
+
+  // ManagedEntityEntryTemplate(const ManagedEntityEntryTemplate<EntityT> & other)
+  // {
+  //   if (other.should_set_in_use_by_wait_set_of_entity_to_false_on_destruction_) {
+  //     throw std::runtime_error("")
+  //   }
+  // }
+
+  ~ManagedEntityEntryTemplate()
+  {
+    if ((nullptr != entity_)) {
+      bool was_in_use = entity_->exchange_in_use_by_wait_set_state(false);
+      assert(was_in_use);
+    }
+  }
+
+  /// Return the interal entity shared pointer.
+  std::shared_ptr<EntityT>
+  get_entity() const
+  {
+    return entity_;
+  }
+
+  /// Return the interal associated entity shared pointer.
+  std::shared_ptr<void>
+  get_associated_entity() const
+  {
+    return associated_entity_;
+  }
+
+  /// Reset the entity.
+  /**
+   * Specializations of this class may reset more than one item.
+   * Having this method in all instantiations of this class provides uniform access.
+   */
+  // void
+  // reset() noexcept
+  // {
+  //   entity_.reset();
+  // }
+
+protected:
+  std::shared_ptr<EntityT> entity_;
+  std::shared_ptr<void> associated_entity_;
+
+};
+
+/// See rclcpp::wait_set_policies::detail::WeakManagedEntityEntryTemplate.
+template<>
+class WeakManagedEntityEntryTemplate<rclcpp::Waitable>
+{
+  using EntityT = rclcpp::Waitable;
+
+public:
+  /// Can only be constructed from a moved ManagedEntityEntryTemplate.
+  explicit WeakManagedEntityEntryTemplate(ManagedEntityEntryTemplate<EntityT> && moved_entity_entry)
+  : weak_entity_(moved_entity_entry.get_entity()),
+    weak_associated_entity_(moved_entity_entry.get_associated_entity())
+  {}
+
+  ~WeakManagedEntityEntryTemplate()
+  {
+    auto entity = weak_entity_.lock();
+    if (nullptr != entity) {
+      bool was_in_use = entity->exchange_in_use_by_wait_set_state(false);
+      assert(was_in_use);
+    }
+  }
+
+  /// Return the interal entity weak pointer.
+  std::weak_ptr<EntityT>
+  get_weak_entity() const
+  {
+    return weak_entity_;
+  }
+
+  // /// Lock the entity.
+  // /**
+  //  * Specializations of this class may select from more than one item to lock.
+  //  * Having this method in all instantiations of this class provides uniform access.
+  //  */
+  // std::shared_ptr<EntityT>
+  // lock() const
+  // {
+  //   return weak_entity_.lock();
+  // }
+
+  // /// Return true if the entity has expired, otherwise false.
+  // /**
+  //  * Specializations of this class may select from more than one item to check.
+  //  * Having this method in all instantiations of this class provides uniform access.
+  //  */
+  // bool
+  // expired() const noexcept
+  // {
+  //   return weak_entity_.expired();
+  // }
+private:
+  std::weak_ptr<EntityT> weak_entity_;
+  std::weak_ptr<void> weak_associated_entity_;
+
+};
+
+using WaitableEntry = EntityEntryTemplate<rclcpp::Waitable>;
+using ManagedWaitableEntry = ManagedEntityEntryTemplate<rclcpp::Waitable>;
+using WeakManagedWaitableEntry = WeakManagedEntityEntryTemplate<rclcpp::Waitable>;
+
+}  // namespace detail
+}  // namespace wait_set_policies
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAIT_SET_POLICIES__DETAIL__WAITABLE_ENTRY_HPP_

rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp

@@ -28,7 +28,13 @@
 #include "rclcpp/subscription_wait_set_mask.hpp"
 #include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/wait_set_policies/detail/client_entry.hpp"
+#include "rclcpp/wait_set_policies/detail/guard_condition_entry.hpp"
+#include "rclcpp/wait_set_policies/detail/service_entry.hpp"
 #include "rclcpp/wait_set_policies/detail/storage_policy_common.hpp"
+#include "rclcpp/wait_set_policies/detail/subscription_entry.hpp"
+#include "rclcpp/wait_set_policies/detail/timer_entry.hpp"
+#include "rclcpp/wait_set_policies/detail/waitable_entry.hpp"
 #include "rclcpp/waitable.hpp"
 
 namespace rclcpp
@@ -42,124 +48,27 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo
 protected:
   using is_mutable = std::true_type;
 
-  class SubscriptionEntry
-  {
-    // (wjwwood): indent of 'public:' is weird, I know. uncrustify is dumb.
+  using WeakManagedSubscriptionEntry = detail::WeakManagedSubscriptionEntry;
+  using SubscriptionEntry = detail::SubscriptionEntry;
 
-public:
-    std::shared_ptr<rclcpp::SubscriptionBase> subscription;
-    rclcpp::SubscriptionWaitSetMask mask;
-
-    /// Conversion constructor, which is intentionally not marked explicit.
-    SubscriptionEntry(
-      std::shared_ptr<rclcpp::SubscriptionBase> subscription_in = nullptr,
-      const rclcpp::SubscriptionWaitSetMask & mask_in = {})
-    : subscription(std::move(subscription_in)),
-      mask(mask_in)
-    {}
-
-    void
-    reset() noexcept
-    {
-      subscription.reset();
-    }
-  };
-  class WeakSubscriptionEntry
-  {
-public:
-    std::weak_ptr<rclcpp::SubscriptionBase> subscription;
-    rclcpp::SubscriptionWaitSetMask mask;
-
-    explicit WeakSubscriptionEntry(
-      const std::shared_ptr<rclcpp::SubscriptionBase> & subscription_in,
-      const rclcpp::SubscriptionWaitSetMask & mask_in) noexcept
-    : subscription(subscription_in),
-      mask(mask_in)
-    {}
-
-    explicit WeakSubscriptionEntry(const SubscriptionEntry & other)
-    : subscription(other.subscription),
-      mask(other.mask)
-    {}
-
-    std::shared_ptr<rclcpp::SubscriptionBase>
-    lock() const
-    {
-      return subscription.lock();
-    }
-
-    bool
-    expired() const noexcept
-    {
-      return subscription.expired();
-    }
-  };
-  using SequenceOfWeakSubscriptions = std::vector<WeakSubscriptionEntry>;
+  using SequenceOfWeakSubscriptions = std::vector<WeakManagedSubscriptionEntry>;
   using SubscriptionsIterable = std::vector<SubscriptionEntry>;
 
-  using SequenceOfWeakGuardConditions = std::vector<std::weak_ptr<rclcpp::GuardCondition>>;
-  using GuardConditionsIterable = std::vector<std::shared_ptr<rclcpp::GuardCondition>>;
+  using SequenceOfWeakGuardConditions = std::vector<detail::WeakGuardConditionEntry>;
+  using GuardConditionsIterable = std::vector<detail::GuardConditionEntry>;
 
-  using SequenceOfWeakTimers = std::vector<std::weak_ptr<rclcpp::TimerBase>>;
-  using TimersIterable = std::vector<std::shared_ptr<rclcpp::TimerBase>>;
+  using SequenceOfWeakTimers = std::vector<detail::WeakTimerEntry>;
+  using TimersIterable = std::vector<detail::TimerEntry>;
 
-  using SequenceOfWeakClients = std::vector<std::weak_ptr<rclcpp::ClientBase>>;
-  using ClientsIterable = std::vector<std::shared_ptr<rclcpp::ClientBase>>;
+  using SequenceOfWeakClients = std::vector<detail::WeakClientEntry>;
+  using ClientsIterable = std::vector<detail::ClientEntry>;
 
-  using SequenceOfWeakServices = std::vector<std::weak_ptr<rclcpp::ServiceBase>>;
-  using ServicesIterable = std::vector<std::shared_ptr<rclcpp::ServiceBase>>;
+  using SequenceOfWeakServices = std::vector<detail::WeakServiceEntry>;
+  using ServicesIterable = std::vector<detail::ServiceEntry>;
 
-  class WaitableEntry
-  {
-public:
-    std::shared_ptr<rclcpp::Waitable> waitable;
-    std::shared_ptr<void> associated_entity;
+  using WeakWaitableEntry = detail::WeakWaitableEntry;
+  using WaitableEntry = detail::WaitableEntry;
 
-    /// Conversion constructor, which is intentionally not marked explicit.
-    WaitableEntry(
-      std::shared_ptr<rclcpp::Waitable> waitable_in = nullptr,
-      std::shared_ptr<void> associated_entity_in = nullptr) noexcept
-    : waitable(std::move(waitable_in)),
-      associated_entity(std::move(associated_entity_in))
-    {}
-
-    void
-    reset() noexcept
-    {
-      waitable.reset();
-      associated_entity.reset();
-    }
-  };
-  class WeakWaitableEntry
-  {
-public:
-    std::weak_ptr<rclcpp::Waitable> waitable;
-    std::weak_ptr<void> associated_entity;
-
-    explicit WeakWaitableEntry(
-      const std::shared_ptr<rclcpp::Waitable> & waitable_in,
-      const std::shared_ptr<void> & associated_entity_in) noexcept
-    : waitable(waitable_in),
-      associated_entity(associated_entity_in)
-    {}
-
-    explicit WeakWaitableEntry(const WaitableEntry & other)
-    : waitable(other.waitable),
-      associated_entity(other.associated_entity)
-    {}
-
-    std::shared_ptr<rclcpp::Waitable>
-    lock() const
-    {
-      return waitable.lock();
-    }
-
-    bool
-    expired() const noexcept
-    {
-      return waitable.expired();
-    }
-  };
   using SequenceOfWeakWaitables = std::vector<WeakWaitableEntry>;
   using WaitablesIterable = std::vector<WaitableEntry>;
 
@@ -184,8 +93,9 @@ public:
       services,
       waitables,
       context),
-    subscriptions_(subscriptions.cbegin(), subscriptions.cend()),
-    shared_subscriptions_(subscriptions_.size()),
+    // subscriptions_ is populated in the constructor dynamically, in order
+    // to respect the mask.
+    // shared_subscriptions_ is resized based on the result of subscriptions_.
     guard_conditions_(guard_conditions.cbegin(), guard_conditions.cend()),
     shared_guard_conditions_(guard_conditions_.size()),
     timers_(timers.cbegin(), timers.cend()),
@@ -194,9 +104,39 @@ public:
     shared_clients_(clients_.size()),
     services_(services.cbegin(), services.cend()),
     shared_services_(services_.size()),
-    waitables_(waitables.cbegin(), waitables.cend()),
-    shared_waitables_(waitables_.size())
-  {}
+    waitables_(waitables.cbegin(), waitables.cend())
+    // shared_waitables_ is resized based on the result of waitables_ after
+    // waitables from subscriptions are added, if any.
+  {
+    // Ensure subscriptions are not being used by other wait sets and extract
+    // their waitables, if they have them.
+    std::vector<WaitableEntry> waitables_from_subscriptions;
+    for (auto & subscription_entry : subscriptions) {
+      std::vector<WaitableEntry> local_waitables_from_subscriptions = subscription_entry.manage();
+      if (subscription_entry.get_mask().include_subscription) {
+        subscriptions_.push_back(subscription_entry);
+      }
+    }
+    // Add subscription entries from subscriptions, if the subscription mask indicates.
+    std::copy_if(
+      subscriptions.cbegin(),
+      subscriptions.cend(),
+      std::back_inserter(subscriptions_),
+      [](const auto & subscription_entry) {
+        return subscription_entry.mask.include_subscription;
+      }
+    );
+    shared_subscriptions_.resize(subscriptions_.size());
+    // Add waitables from subscriptions, if the subscription mask indicates.
+    for (const auto & subscription_entry : subscriptions) {
+      if (subscription_entry.mask.include_events) {
+        for (const auto & event : subscription_entry.subscription->get_event_handlers()) {
+          waitables_.push_back({event, subscription_entry.subscription});
+        }
+      }
+    }
+    shared_waitables_.resize(waitables_.size());
+  }
 
   ~DynamicStorage() = default;
 
@@ -243,7 +183,7 @@ public:
     if (this->storage_has_entity(*subscription, subscriptions_)) {
       throw std::runtime_error("subscription already in wait set");
     }
-    WeakSubscriptionEntry weak_entry{std::move(subscription), {}};
+    WeakManagedSubscriptionEntry weak_entry{std::move(subscription), {}};
     subscriptions_.push_back(std::move(weak_entry));
     this->storage_flag_for_resize();
   }

rclcpp/src/rclcpp/context.cpp

@@ -31,8 +31,6 @@
 #include "rcutils/error_handling.h"
 #include "rcutils/macros.h"
 
-#include "rmw/impl/cpp/demangle.hpp"
-
 #include "./logging_mutex.hpp"
 
 using rclcpp::Context;

rclcpp/src/rclcpp/publisher_base.cpp

@@ -25,7 +25,6 @@
 #include <vector>
 
 #include "rcutils/logging_macros.h"
-#include "rmw/impl/cpp/demangle.hpp"
 
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/allocator/allocator_deleter.hpp"

rclcpp/test/rclcpp/test_wait_set.cpp

@@ -14,6 +14,7 @@
 
 #include <gtest/gtest.h>
 
+#include <chrono>
 #include <memory>
 #include <utility>
 #include <vector>
@@ -342,6 +343,9 @@ TEST_F(TestWaitSet, get_result_from_wait_result) {
   EXPECT_EQ(&wait_set, &const_result.get_wait_set());
 }
 
+/*
+ * Fail to get wait_set from result when timeout (not ready).
+ */
 TEST_F(TestWaitSet, get_result_from_wait_result_not_ready_error) {
   rclcpp::WaitSet wait_set;
   auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
@@ -359,3 +363,97 @@ TEST_F(TestWaitSet, get_result_from_wait_result_not_ready_error) {
     const_result.get_wait_set(),
     std::runtime_error("cannot access wait set when the result was not ready"));
 }
+
+/*
+ * Fail to add item to wait set, which was added in the constructor of another wait set.
+ *
+ * Also ensure items in destroyed wait sets can be added to new wait sets afterwards.
+ */
+TEST_F(TestWaitSet, add_entity_in_constructor_and_dynamically_fails) {
+  auto node = std::make_shared<rclcpp::Node>("add_entity_in_constructor_and_dynamically_fails");
+
+  rclcpp::SubscriptionOptions subscription_options;
+  subscription_options.event_callbacks.deadline_callback = [](auto) {};
+  subscription_options.event_callbacks.liveliness_callback = [](auto) {};
+  auto do_nothing = [](std::shared_ptr<const test_msgs::msg::BasicTypes>) {};
+  auto sub = node->create_subscription<test_msgs::msg::BasicTypes>(
+    "~/test",
+    1,
+    do_nothing,
+    subscription_options);
+
+  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+
+  auto timer = node->create_wall_timer(std::chrono::seconds(1), []() {});
+
+  auto client = node->create_client<rcl_interfaces::srv::ListParameters>("~/empty");
+
+  auto srv_callback =
+    [](
+      const std::shared_ptr<rmw_request_id_t>,
+      const std::shared_ptr<rcl_interfaces::srv::ListParameters::Request>,
+      const std::shared_ptr<rcl_interfaces::srv::ListParameters::Response>
+    ) {};
+  auto service =
+    node->create_service<rcl_interfaces::srv::ListParameters>("~/test", srv_callback);
+
+  {
+    // Add all entities via constructor.
+    rclcpp::WaitSet wait_set(
+      {{sub}},
+      {guard_condition},
+      {timer},
+      {client},
+      {service}
+    );
+
+    // Expect all cannot be added to another wait set.
+    rclcpp::WaitSet other_wait_set;
+    EXPECT_THROW({
+      other_wait_set.add_subscription(sub);
+    }, std::runtime_error);
+    EXPECT_THROW({
+      other_wait_set.add_guard_condition(guard_condition);
+    }, std::runtime_error);
+    EXPECT_THROW({
+      other_wait_set.add_timer(timer);
+    }, std::runtime_error);
+    EXPECT_THROW({
+      other_wait_set.add_client(client);
+    }, std::runtime_error);
+    EXPECT_THROW({
+      other_wait_set.add_service(service);
+    }, std::runtime_error);
+  }
+}
+
+/*
+ * Ensure removing a subscription added via construction is removable with all mask items.
+ *
+ * This covers a case that had a bug in which the adding of items via construction was naive and
+ * did not behave as-if items were being added via `add_*` type methods, specifically for
+ * subscriptions, which had the mask logic.
+ */
+TEST_F(TestWaitSet, remove_subscription_which_was_added_via_construction) {
+  auto node =
+    std::make_shared<rclcpp::Node>("remove_subscription_which_was_added_via_construction");
+
+  rclcpp::SubscriptionOptions subscription_options;
+  subscription_options.event_callbacks.deadline_callback = [](auto) {};
+  subscription_options.event_callbacks.liveliness_callback = [](auto) {};
+  auto do_nothing = [](std::shared_ptr<const test_msgs::msg::BasicTypes>) {};
+  auto sub = node->create_subscription<test_msgs::msg::BasicTypes>(
+    "~/test",
+    1,
+    do_nothing,
+    subscription_options);
+
+  rclcpp::WaitSet wait_set({{sub}});
+
+  wait_set.remove_subscription(sub);
+}
+
+/*
+More test ideas:
+  - add to dynamic wait set, let go out of scope, destroy wait set (weak ptr to objects prevent exchange_in_use_by_wait_set_state(false), should be ok though)
+ */