wip

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

rclcpp/CHANGELOG.rst

@@ -2,6 +2,13 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+11.0.0 (2021-05-18)
+-------------------
+* Allow declare uninitialized parameters (`#1673 <https://github.com/ros2/rclcpp/issues/1673>`_)
+* Fix syntax issue with gcc (`#1674 <https://github.com/ros2/rclcpp/issues/1674>`_)
+* [service] Don't use a weak_ptr to avoid leaking (`#1668 <https://github.com/ros2/rclcpp/issues/1668>`_)
+* Contributors: Ivan Santiago Paunovic, Jacob Perron, William Woodall
+
 10.0.0 (2021-05-11)
 -------------------
 * Fix doc typo (`#1663 <https://github.com/ros2/rclcpp/issues/1663>`_)

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/allocator/allocator_deleter.hpp

@@ -95,7 +95,7 @@ void set_allocator_for_deleter(AllocatorDeleter<T> * deleter, Alloc * alloc)
 template<typename Alloc, typename T>
 using Deleter = typename std::conditional<
   std::is_same<typename std::allocator_traits<Alloc>::template rebind_alloc<T>,
-  typename std::allocator<void>::template rebind<T>::other>::value,
+  std::allocator<T>>::value,
   std::default_delete<T>,
   AllocatorDeleter<Alloc>
   >::type;

rclcpp/include/rclcpp/exceptions/exceptions.hpp

@@ -282,8 +282,8 @@ class ParameterModifiedInCallbackException : public std::runtime_error
   using std::runtime_error::runtime_error;
 };
 
-/// Thrown when a parameter override wasn't provided and one was required.
-class NoParameterOverrideProvided : public std::runtime_error
+/// Thrown when an uninitialized parameter is accessed.
+class ParameterUninitializedException : public std::runtime_error
 {
 public:
   /// Construct an instance.
@@ -291,8 +291,8 @@ public:
    * \param[in] name the name of the parameter.
    * \param[in] message custom exception message.
    */
-  explicit NoParameterOverrideProvided(const std::string & name)
-  : std::runtime_error("parameter '" + name + "' requires an user provided parameter override")
+  explicit ParameterUninitializedException(const std::string & name)
+  : std::runtime_error("parameter '" + name + "' is not initialized")
   {}
 };
 

rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp

@@ -24,6 +24,7 @@
 #include "rclcpp/experimental/buffers/intra_process_buffer.hpp"
 #include "rclcpp/experimental/buffers/ring_buffer_implementation.hpp"
 #include "rclcpp/intra_process_buffer_type.hpp"
+#include "rclcpp/qos.hpp"
 
 namespace rclcpp
 {
@@ -37,13 +38,13 @@ template<
 typename rclcpp::experimental::buffers::IntraProcessBuffer<MessageT, Alloc, Deleter>::UniquePtr
 create_intra_process_buffer(
   IntraProcessBufferType buffer_type,
-  rmw_qos_profile_t qos,
+  const rclcpp::QoS & qos,
   std::shared_ptr<Alloc> allocator)
 {
   using MessageSharedPtr = std::shared_ptr<const MessageT>;
   using MessageUniquePtr = std::unique_ptr<MessageT, Deleter>;
 
-  size_t buffer_size = qos.depth;
+  size_t buffer_size = qos.depth();
 
   using rclcpp::experimental::buffers::IntraProcessBuffer;
   typename IntraProcessBuffer<MessageT, Alloc, Deleter>::UniquePtr buffer;

rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

@@ -305,25 +305,6 @@ public:
   get_subscription_intra_process(uint64_t intra_process_subscription_id);
 
 private:
-  struct SubscriptionInfo
-  {
-    SubscriptionInfo() = default;
-
-    rclcpp::experimental::SubscriptionIntraProcessBase::WeakPtr subscription;
-    rmw_qos_profile_t qos;
-    const char * topic_name;
-    bool use_take_shared_method;
-  };
-
-  struct PublisherInfo
-  {
-    PublisherInfo() = default;
-
-    rclcpp::PublisherBase::WeakPtr publisher;
-    rmw_qos_profile_t qos;
-    const char * topic_name;
-  };
-
   struct SplittedSubscriptions
   {
     std::vector<uint64_t> take_shared_subscriptions;
@@ -331,10 +312,10 @@ private:
   };
 
   using SubscriptionMap =
-    std::unordered_map<uint64_t, SubscriptionInfo>;
+    std::unordered_map<uint64_t, rclcpp::experimental::SubscriptionIntraProcessBase::WeakPtr>;
 
   using PublisherMap =
-    std::unordered_map<uint64_t, PublisherInfo>;
+    std::unordered_map<uint64_t, rclcpp::PublisherBase::WeakPtr>;
 
   using PublisherToSubscriptionIdsMap =
     std::unordered_map<uint64_t, SplittedSubscriptions>;
@@ -350,7 +331,9 @@ private:
 
   RCLCPP_PUBLIC
   bool
-  can_communicate(PublisherInfo pub_info, SubscriptionInfo sub_info) const;
+  can_communicate(
+    rclcpp::PublisherBase::SharedPtr pub,
+    rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr sub) const;
 
   template<
     typename MessageT,
@@ -366,7 +349,7 @@ private:
       if (subscription_it == subscriptions_.end()) {
         throw std::runtime_error("subscription has unexpectedly gone out of scope");
       }
-      auto subscription_base = subscription_it->second.subscription.lock();
+      auto subscription_base = subscription_it->second.lock();
       if (subscription_base) {
         auto subscription = std::dynamic_pointer_cast<
           rclcpp::experimental::SubscriptionIntraProcessBuffer<MessageT, Alloc, Deleter>
@@ -404,7 +387,7 @@ private:
       if (subscription_it == subscriptions_.end()) {
         throw std::runtime_error("subscription has unexpectedly gone out of scope");
       }
-      auto subscription_base = subscription_it->second.subscription.lock();
+      auto subscription_base = subscription_it->second.lock();
       if (subscription_base) {
         auto subscription = std::dynamic_pointer_cast<
           rclcpp::experimental::SubscriptionIntraProcessBuffer<MessageT, Alloc, Deleter>

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -31,6 +31,7 @@
 #include "rclcpp/experimental/create_intra_process_buffer.hpp"
 #include "rclcpp/experimental/subscription_intra_process_base.hpp"
 #include "rclcpp/experimental/subscription_intra_process_buffer.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/waitable.hpp"
 #include "tracetools/tracetools.h"
@@ -72,7 +73,7 @@ public:
     std::shared_ptr<Alloc> allocator,
     rclcpp::Context::SharedPtr context,
     const std::string & topic_name,
-    rmw_qos_profile_t qos_profile,
+    const rclcpp::QoS & qos_profile,
     rclcpp::IntraProcessBufferType buffer_type)
   : SubscriptionIntraProcessBuffer<MessageT, Alloc, Deleter>(
       allocator,

rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp

@@ -25,6 +25,7 @@
 
 #include "rcl/error_handling.h"
 
+#include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/waitable.hpp"
 
@@ -39,7 +40,9 @@ public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase)
 
   RCLCPP_PUBLIC
-  SubscriptionIntraProcessBase(const std::string & topic_name, rmw_qos_profile_t qos_profile)
+  SubscriptionIntraProcessBase(
+    const std::string & topic_name,
+    const rclcpp::QoS & qos_profile)
   : topic_name_(topic_name), qos_profile_(qos_profile)
   {}
 
@@ -71,7 +74,7 @@ public:
   get_topic_name() const;
 
   RCLCPP_PUBLIC
-  rmw_qos_profile_t
+  QoS
   get_actual_qos() const;
 
 protected:
@@ -83,7 +86,7 @@ private:
   trigger_guard_condition() = 0;
 
   std::string topic_name_;
-  rmw_qos_profile_t qos_profile_;
+  QoS qos_profile_;
 };
 
 }  // namespace experimental

rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp

@@ -30,6 +30,7 @@
 #include "rclcpp/experimental/buffers/intra_process_buffer.hpp"
 #include "rclcpp/experimental/create_intra_process_buffer.hpp"
 #include "rclcpp/experimental/subscription_intra_process_base.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/waitable.hpp"
 #include "tracetools/tracetools.h"
@@ -64,7 +65,7 @@ public:
     std::shared_ptr<Alloc> allocator,
     rclcpp::Context::SharedPtr context,
     const std::string & topic_name,
-    rmw_qos_profile_t qos_profile,
+    const rclcpp::QoS & qos_profile,
     rclcpp::IntraProcessBufferType buffer_type)
   : SubscriptionIntraProcessBase(topic_name, qos_profile)
   {

rclcpp/include/rclcpp/publisher.hpp

@@ -186,15 +186,15 @@ public:
       // Get the intra process manager instance for this context.
       auto ipm = context->get_sub_context<rclcpp::experimental::IntraProcessManager>();
       // Register the publisher with the intra process manager.
-      if (qos.get_rmw_qos_profile().history == RMW_QOS_POLICY_HISTORY_KEEP_ALL) {
+      if (qos.history() != rclcpp::HistoryPolicy::KeepLast) {
         throw std::invalid_argument(
-                "intraprocess communication is not allowed with keep all history qos policy");
+                "intraprocess communication allowed only with keep last history qos policy");
       }
-      if (qos.get_rmw_qos_profile().depth == 0) {
+      if (qos.depth() == 0) {
         throw std::invalid_argument(
                 "intraprocess communication is not allowed with a zero qos history depth value");
       }
-      if (qos.get_rmw_qos_profile().durability != RMW_QOS_POLICY_DURABILITY_VOLATILE) {
+      if (qos.durability() != rclcpp::DurabilityPolicy::Volatile) {
         throw std::invalid_argument(
                 "intraprocess communication allowed only with volatile durability");
       }

rclcpp/include/rclcpp/service.hpp

@@ -177,25 +177,16 @@ public:
     using rosidl_typesupport_cpp::get_service_type_support_handle;
     auto service_type_support_handle = get_service_type_support_handle<ServiceT>();
 
-    std::weak_ptr<rcl_node_t> weak_node_handle(node_handle_);
     // rcl does the static memory allocation here
     service_handle_ = std::shared_ptr<rcl_service_t>(
-      new rcl_service_t, [weak_node_handle, service_name](rcl_service_t * service)
+      new rcl_service_t, [handle = node_handle_, service_name](rcl_service_t * service)
       {
-        auto handle = weak_node_handle.lock();
-        if (handle) {
-          if (rcl_service_fini(service, handle.get()) != RCL_RET_OK) {
-            RCLCPP_ERROR(
-              rclcpp::get_node_logger(handle.get()).get_child("rclcpp"),
-              "Error in destruction of rcl service handle: %s",
-              rcl_get_error_string().str);
-            rcl_reset_error();
-          }
-        } else {
-          RCLCPP_ERROR_STREAM(
-            rclcpp::get_logger("rclcpp"),
-            "Error in destruction of rcl service handle " << service_name <<
-              ": the Node Handle was destructed too early. You will leak memory");
+        if (rcl_service_fini(service, handle.get()) != RCL_RET_OK) {
+          RCLCPP_ERROR(
+            rclcpp::get_node_logger(handle.get()).get_child("rclcpp"),
+            "Error in destruction of rcl service handle: %s",
+            rcl_get_error_string().str);
+          rcl_reset_error();
         }
         delete service;
       });

rclcpp/include/rclcpp/subscription.hpp

@@ -183,16 +183,16 @@ public:
       using rclcpp::detail::resolve_intra_process_buffer_type;
 
       // Check if the QoS is compatible with intra-process.
-      rmw_qos_profile_t qos_profile = get_actual_qos().get_rmw_qos_profile();
-      if (qos_profile.history == RMW_QOS_POLICY_HISTORY_KEEP_ALL) {
+      auto qos_profile = get_actual_qos();
+      if (qos_profile.history() != rclcpp::HistoryPolicy::KeepLast) {
         throw std::invalid_argument(
-                "intraprocess communication is not allowed with keep all history qos policy");
+                "intraprocess communication allowed only with keep last history qos policy");
       }
-      if (qos_profile.depth == 0) {
+      if (qos_profile.depth() == 0) {
         throw std::invalid_argument(
                 "intraprocess communication is not allowed with 0 depth qos policy");
       }
-      if (qos_profile.durability != RMW_QOS_POLICY_DURABILITY_VOLATILE) {
+      if (qos_profile.durability() != rclcpp::DurabilityPolicy::Volatile) {
         throw std::invalid_argument(
                 "intraprocess communication allowed only with volatile durability");
       }

rclcpp/include/rclcpp/time_source.hpp

@@ -74,14 +74,14 @@ public:
     const rclcpp::QoS & qos = rclcpp::ClockQoS(),
     bool use_clock_thread = true);
 
-  /// Attack node to the time source.
+  /// Attach node to the time source.
   /**
    * \param node std::shared pointer to a initialized node
    */
   RCLCPP_PUBLIC
   void attachNode(rclcpp::Node::SharedPtr node);
 
-  /// Attack node to the time source.
+  /// Attach node to the time source.
   /**
    * If the parameter `use_sim_time` is `true` then the source time is the simulation time,
    * otherwise the source time is defined by the system.

rclcpp/include/rclcpp/type_adapter.hpp

@@ -179,18 +179,22 @@ struct TypeAdapter<T, void, std::enable_if_t<ImplicitTypeAdapter<T>::is_speciali
 
 /// Assigns the custom type implicitly to the given custom type/ros message type pair.
 /**
+ * Note: this macro needs to be used in the root namespace.
+ * We cannot use ::rclcpp to protect against this, due to how GCC interprets the
+ * syntax, see: https://stackoverflow.com/a/2781537
+ *
  * \sa TypeAdapter
  * \sa ImplicitTypeAdapter
  */
 #define RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(CustomType, ROSMessageType) \
   template<> \
-  struct ::rclcpp::ImplicitTypeAdapter<CustomType> \
-  : public ::rclcpp::TypeAdapter<CustomType, ROSMessageType> \
+  struct rclcpp::ImplicitTypeAdapter<CustomType> \
+    : public rclcpp::TypeAdapter<CustomType, ROSMessageType> \
   { \
     static_assert( \
       is_specialized::value, \
       "Cannot use custom type as ros type when there is no TypeAdapter for that pair"); \
-  };
+  }
 
 }  // namespace rclcpp
 

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/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp</name>
-  <version>10.0.0</version>
+  <version>11.0.0</version>
   <description>The ROS client library in C++.</description>
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
   <maintainer email="mabel@openrobotics.org">Mabel Zhang</maintainer>

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/executor.cpp

@@ -268,7 +268,9 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt
   // If the node already has an executor
   std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
   if (has_executor.exchange(true)) {
-    throw std::runtime_error("Node has already been added to an executor.");
+    throw std::runtime_error(
+            std::string("Node '") + node_ptr->get_fully_qualified_name() +
+            "' has already been added to an executor.");
   }
   std::lock_guard<std::mutex> guard{mutex_};
   for (auto & weak_group : node_ptr->get_callback_groups()) {

rclcpp/src/rclcpp/intra_process_manager.cpp

@@ -36,23 +36,26 @@ IntraProcessManager::add_publisher(rclcpp::PublisherBase::SharedPtr publisher)
 {
   std::unique_lock<std::shared_timed_mutex> lock(mutex_);
 
-  auto id = IntraProcessManager::get_next_unique_id();
+  uint64_t pub_id = IntraProcessManager::get_next_unique_id();
 
-  publishers_[id].publisher = publisher;
-  publishers_[id].topic_name = publisher->get_topic_name();
-  publishers_[id].qos = publisher->get_actual_qos().get_rmw_qos_profile();
+  publishers_[pub_id] = publisher;
 
   // Initialize the subscriptions storage for this publisher.
-  pub_to_subs_[id] = SplittedSubscriptions();
+  pub_to_subs_[pub_id] = SplittedSubscriptions();
 
   // create an entry for the publisher id and populate with already existing subscriptions
   for (auto & pair : subscriptions_) {
-    if (can_communicate(publishers_[id], pair.second)) {
-      insert_sub_id_for_pub(pair.first, id, pair.second.use_take_shared_method);
+    auto subscription = pair.second.lock();
+    if (!subscription) {
+      continue;
+    }
+    if (can_communicate(publisher, subscription)) {
+      uint64_t sub_id = pair.first;
+      insert_sub_id_for_pub(sub_id, pub_id, subscription->use_take_shared_method());
     }
   }
 
-  return id;
+  return pub_id;
 }
 
 uint64_t
@@ -60,21 +63,23 @@ IntraProcessManager::add_subscription(SubscriptionIntraProcessBase::SharedPtr su
 {
   std::unique_lock<std::shared_timed_mutex> lock(mutex_);
 
-  auto id = IntraProcessManager::get_next_unique_id();
+  uint64_t sub_id = IntraProcessManager::get_next_unique_id();
 
-  subscriptions_[id].subscription = subscription;
-  subscriptions_[id].topic_name = subscription->get_topic_name();
-  subscriptions_[id].qos = subscription->get_actual_qos();
-  subscriptions_[id].use_take_shared_method = subscription->use_take_shared_method();
+  subscriptions_[sub_id] = subscription;
 
   // adds the subscription id to all the matchable publishers
   for (auto & pair : publishers_) {
-    if (can_communicate(pair.second, subscriptions_[id])) {
-      insert_sub_id_for_pub(id, pair.first, subscriptions_[id].use_take_shared_method);
+    auto publisher = pair.second.lock();
+    if (!publisher) {
+      continue;
+    }
+    if (can_communicate(publisher, subscription)) {
+      uint64_t pub_id = pair.first;
+      insert_sub_id_for_pub(sub_id, pub_id, subscription->use_take_shared_method());
     }
   }
 
-  return id;
+  return sub_id;
 }
 
 void
@@ -116,7 +121,7 @@ IntraProcessManager::matches_any_publishers(const rmw_gid_t * id) const
   std::shared_lock<std::shared_timed_mutex> lock(mutex_);
 
   for (auto & publisher_pair : publishers_) {
-    auto publisher = publisher_pair.second.publisher.lock();
+    auto publisher = publisher_pair.second.lock();
     if (!publisher) {
       continue;
     }
@@ -157,7 +162,7 @@ IntraProcessManager::get_subscription_intra_process(uint64_t intra_process_subsc
   if (subscription_it == subscriptions_.end()) {
     return nullptr;
   } else {
-    auto subscription = subscription_it->second.subscription.lock();
+    auto subscription = subscription_it->second.lock();
     if (subscription) {
       return subscription;
     } else {
@@ -204,25 +209,16 @@ IntraProcessManager::insert_sub_id_for_pub(
 
 bool
 IntraProcessManager::can_communicate(
-  PublisherInfo pub_info,
-  SubscriptionInfo sub_info) const
+  rclcpp::PublisherBase::SharedPtr pub,
+  rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr sub) const
 {
   // publisher and subscription must be on the same topic
-  if (strcmp(pub_info.topic_name, sub_info.topic_name) != 0) {
+  if (strcmp(pub->get_topic_name(), sub->get_topic_name()) != 0) {
     return false;
   }
 
-  // TODO(alsora): the following checks for qos compatibility should be provided by the RMW
-  // a reliable subscription can't be connected with a best effort publisher
-  if (
-    sub_info.qos.reliability == RMW_QOS_POLICY_RELIABILITY_RELIABLE &&
-    pub_info.qos.reliability == RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT)
-  {
-    return false;
-  }
-
-  // a publisher and a subscription with different durability can't communicate
-  if (sub_info.qos.durability != pub_info.qos.durability) {
+  auto check_result = rclcpp::qos_check_compatible(pub->get_actual_qos(), sub->get_actual_qos());
+  if (check_result.compatibility == rclcpp::QoSCompatibility::Error) {
     return false;
   }
 

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -349,6 +349,21 @@ __declare_parameter_common(
     initial_value = &overrides_it->second;
   }
 
+  // If there is no initial value, then skip initialization
+  if (initial_value->get_type() == rclcpp::PARAMETER_NOT_SET) {
+    // Add declared parameters to storage (without a value)
+    parameter_infos[name].descriptor.name = name;
+    if (parameter_descriptor.dynamic_typing) {
+      parameter_infos[name].descriptor.type = rclcpp::PARAMETER_NOT_SET;
+    } else {
+      parameter_infos[name].descriptor.type = parameter_descriptor.type;
+    }
+    parameters_out[name] = parameter_infos.at(name);
+    rcl_interfaces::msg::SetParametersResult result;
+    result.successful = true;
+    return result;
+  }
+
   // Check with the user's callback to see if the initial value can be set.
   std::vector<rclcpp::Parameter> parameter_wrappers {rclcpp::Parameter(name, *initial_value)};
   // This function also takes care of default vs initial value.
@@ -413,14 +428,6 @@ declare_parameter_helper(
     parameter_descriptor.type = static_cast<uint8_t>(type);
   }
 
-  if (
-    rclcpp::PARAMETER_NOT_SET == default_value.get_type() &&
-    overrides.find(name) == overrides.end() &&
-    parameter_descriptor.dynamic_typing == false)
-  {
-    throw rclcpp::exceptions::NoParameterOverrideProvided(name);
-  }
-
   rcl_interfaces::msg::ParameterEvent parameter_event;
   auto result = __declare_parameter_common(
     name,
@@ -806,14 +813,21 @@ NodeParameters::get_parameters(const std::vector<std::string> & names) const
 rclcpp::Parameter
 NodeParameters::get_parameter(const std::string & name) const
 {
-  rclcpp::Parameter parameter;
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
 
-  if (get_parameter(name, parameter)) {
-    return parameter;
+  auto param_iter = parameters_.find(name);
+  if (
+    parameters_.end() != param_iter &&
+    (param_iter->second.value.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET ||
+    param_iter->second.descriptor.dynamic_typing))
+  {
+    return rclcpp::Parameter{name, param_iter->second.value};
   } else if (this->allow_undeclared_) {
-    return parameter;
-  } else {
+    return rclcpp::Parameter{};
+  } else if (parameters_.end() == param_iter) {
     throw rclcpp::exceptions::ParameterNotDeclaredException(name);
+  } else {
+    throw rclcpp::exceptions::ParameterUninitializedException(name);
   }
 }
 

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/src/rclcpp/subscription_intra_process_base.cpp

@@ -31,7 +31,7 @@ SubscriptionIntraProcessBase::get_topic_name() const
   return topic_name_.c_str();
 }
 
-rmw_qos_profile_t
+rclcpp::QoS
 SubscriptionIntraProcessBase::get_actual_qos() const
 {
   return qos_profile_;

rclcpp/test/rclcpp/test_intra_process_manager.cpp

@@ -52,8 +52,8 @@ class PublisherBase
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(PublisherBase)
 
-  PublisherBase()
-  : qos(rclcpp::QoS(10)),
+  explicit PublisherBase(rclcpp::QoS qos = rclcpp::QoS(10))
+  : qos_profile(qos),
     topic_name("topic")
   {}
 
@@ -76,7 +76,7 @@ public:
   rclcpp::QoS
   get_actual_qos() const
   {
-    return qos;
+    return qos_profile;
   }
 
   bool
@@ -93,7 +93,7 @@ public:
     return false;
   }
 
-  rclcpp::QoS qos;
+  rclcpp::QoS qos_profile;
   std::string topic_name;
   uint64_t intra_process_publisher_id_;
   IntraProcessManagerWeakPtr weak_ipm_;
@@ -111,9 +111,9 @@ public:
 
   RCLCPP_SMART_PTR_DEFINITIONS(Publisher<T, Alloc>)
 
-  Publisher()
+  explicit Publisher(rclcpp::QoS qos = rclcpp::QoS(10))
+  : PublisherBase(qos)
   {
-    qos = rclcpp::QoS(10);
     auto allocator = std::make_shared<Alloc>();
     message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
   }
@@ -194,8 +194,8 @@ class SubscriptionIntraProcessBase
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase)
 
-  SubscriptionIntraProcessBase()
-  : qos_profile(rmw_qos_profile_default), topic_name("topic")
+  explicit SubscriptionIntraProcessBase(rclcpp::QoS qos = rclcpp::QoS(10))
+  : qos_profile(qos), topic_name("topic")
   {}
 
   virtual ~SubscriptionIntraProcessBase() {}
@@ -203,7 +203,7 @@ public:
   virtual bool
   use_take_shared_method() const = 0;
 
-  rmw_qos_profile_t
+  QoS
   get_actual_qos()
   {
     return qos_profile;
@@ -215,7 +215,7 @@ public:
     return topic_name;
   }
 
-  rmw_qos_profile_t qos_profile;
+  rclcpp::QoS qos_profile;
   const char * topic_name;
 };
 
@@ -228,8 +228,8 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcessBuffer)
 
-  SubscriptionIntraProcessBuffer()
-  : take_shared_method(false)
+  explicit SubscriptionIntraProcessBuffer(rclcpp::QoS qos)
+  : SubscriptionIntraProcessBase(qos), take_shared_method(false)
   {
     buffer = std::make_unique<rclcpp::experimental::buffers::mock::IntraProcessBuffer<MessageT>>();
   }
@@ -278,8 +278,8 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBuffer<
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcess)
 
-  SubscriptionIntraProcess()
-  : SubscriptionIntraProcessBuffer<MessageT, Alloc, Deleter>()
+  explicit SubscriptionIntraProcess(rclcpp::QoS qos = rclcpp::QoS(10))
+  : SubscriptionIntraProcessBuffer<MessageT, Alloc, Deleter>(qos)
   {
   }
 };
@@ -359,15 +359,12 @@ TEST(TestIntraProcessManager, add_pub_sub) {
 
   auto ipm = std::make_shared<IntraProcessManagerT>();
 
-  auto p1 = std::make_shared<PublisherT>();
-  p1->qos.get_rmw_qos_profile().reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
+  auto p1 = std::make_shared<PublisherT>(rclcpp::QoS(10).best_effort());
 
-  auto p2 = std::make_shared<PublisherT>();
-  p2->qos.get_rmw_qos_profile().reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
+  auto p2 = std::make_shared<PublisherT>(rclcpp::QoS(10).best_effort());
   p2->topic_name = "different_topic_name";
 
-  auto s1 = std::make_shared<SubscriptionIntraProcessT>();
-  s1->qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
+  auto s1 = std::make_shared<SubscriptionIntraProcessT>(rclcpp::QoS(10).best_effort());
 
   auto p1_id = ipm->add_publisher(p1);
   auto p2_id = ipm->add_publisher(p2);
@@ -383,11 +380,9 @@ TEST(TestIntraProcessManager, add_pub_sub) {
   ASSERT_EQ(0u, p2_subs);
   ASSERT_EQ(0u, non_existing_pub_subs);
 
-  auto p3 = std::make_shared<PublisherT>();
-  p3->qos.get_rmw_qos_profile().reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  auto p3 = std::make_shared<PublisherT>(rclcpp::QoS(10).reliable());
 
-  auto s2 = std::make_shared<SubscriptionIntraProcessT>();
-  s2->qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  auto s2 = std::make_shared<SubscriptionIntraProcessT>(rclcpp::QoS(10).reliable());
 
   auto s2_id = ipm->add_subscription(s2);
   auto p3_id = ipm->add_publisher(p3);

rclcpp/test/rclcpp/test_node.cpp

@@ -335,9 +335,14 @@ TEST_F(TestNode, declare_parameter_with_no_initial_values) {
     rcl_interfaces::msg::ParameterDescriptor descriptor;
     descriptor.dynamic_typing = true;
     // no default, no initial
+    const std::string parameter_name = "parameter"_unq;
     rclcpp::ParameterValue value = node->declare_parameter(
-      "parameter"_unq, rclcpp::ParameterValue{}, descriptor);
+      parameter_name, rclcpp::ParameterValue{}, descriptor);
     EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_NOT_SET);
+    // Does not throw if unset before access
+    EXPECT_EQ(
+      rclcpp::PARAMETER_NOT_SET,
+      node->get_parameter(parameter_name).get_parameter_value().get_type());
   }
   {
     // int default, no initial
@@ -2798,9 +2803,20 @@ TEST_F(TestNode, static_and_dynamic_typing) {
     EXPECT_EQ("hello!", param);
   }
   {
+    auto param = node->declare_parameter("integer_override_not_given", rclcpp::PARAMETER_INTEGER);
+    EXPECT_EQ(rclcpp::PARAMETER_NOT_SET, param.get_type());
+    // Throws if not set before access
     EXPECT_THROW(
-      node->declare_parameter("integer_override_not_given", rclcpp::PARAMETER_INTEGER),
-      rclcpp::exceptions::NoParameterOverrideProvided);
+      node->get_parameter("integer_override_not_given"),
+      rclcpp::exceptions::ParameterUninitializedException);
+  }
+  {
+    auto param = node->declare_parameter("integer_set_after_declare", rclcpp::PARAMETER_INTEGER);
+    EXPECT_EQ(rclcpp::PARAMETER_NOT_SET, param.get_type());
+    auto result = node->set_parameter(rclcpp::Parameter{"integer_set_after_declare", 44});
+    ASSERT_TRUE(result.successful) << result.reason;
+    auto get_param = node->get_parameter("integer_set_after_declare");
+    EXPECT_EQ(44, get_param.as_int());
   }
   {
     EXPECT_THROW(

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)
+ */

rclcpp_action/CHANGELOG.rst

@@ -3,6 +3,11 @@ Changelog for package rclcpp_action
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+11.0.0 (2021-05-18)
+-------------------
+* Bump the benchmark timeout for benchmark_action_client (`#1671 <https://github.com/ros2/rclcpp/issues/1671>`_)
+* Contributors: Scott K Logan
+
 10.0.0 (2021-05-11)
 -------------------
 * Returns CancelResponse::REJECT while goal handle failed to transit to CANCELING state (`#1641 <https://github.com/ros2/rclcpp/issues/1641>`_)

rclcpp_action/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp_action</name>
-  <version>10.0.0</version>
+  <version>11.0.0</version>
   <description>Adds action APIs for C++.</description>
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
   <maintainer email="mabel@openrobotics.org">Mabel Zhang</maintainer>

rclcpp_action/src/server.cpp

@@ -500,9 +500,13 @@ ServerBase::execute_result_request_received(std::shared_ptr<void> & data)
     result_response = create_result_response(action_msgs::msg::GoalStatus::STATUS_UNKNOWN);
   } else {
     // Goal exists, check if a result is already available
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->unordered_map_mutex_);
     auto iter = pimpl_->goal_results_.find(uuid);
     if (iter != pimpl_->goal_results_.end()) {
       result_response = iter->second;
+    } else {
+      // Store the request so it can be responded to later
+      pimpl_->result_requests_[uuid].push_back(request_header);
     }
   }
 
@@ -514,10 +518,6 @@ ServerBase::execute_result_request_received(std::shared_ptr<void> & data)
     if (RCL_RET_OK != rcl_ret) {
       rclcpp::exceptions::throw_from_rcl_error(rcl_ret);
     }
-  } else {
-    // Store the request so it can be responded to later
-    std::lock_guard<std::recursive_mutex> lock(pimpl_->unordered_map_mutex_);
-    pimpl_->result_requests_[uuid].push_back(request_header);
   }
   data.reset();
 }
@@ -627,19 +627,30 @@ ServerBase::publish_result(const GoalUUID & uuid, std::shared_ptr<void> result_m
   }
 
   {
-    std::lock_guard<std::recursive_mutex> lock(pimpl_->unordered_map_mutex_);
+    /**
+    * NOTE: There is a potential deadlock issue if both unordered_map_mutex_ and
+    * action_server_reentrant_mutex_ locked in other block scopes. Unless using
+    * std::scoped_lock, locking order must be consistent with the current.
+    *
+    * Current locking order:
+    *
+    *   1. unordered_map_mutex_
+    *   2. action_server_reentrant_mutex_
+    *
+    */
+    std::lock_guard<std::recursive_mutex> unordered_map_lock(pimpl_->unordered_map_mutex_);
     pimpl_->goal_results_[uuid] = result_msg;
-  }
 
-  // if there are clients who already asked for the result, send it to them
-  auto iter = pimpl_->result_requests_.find(uuid);
-  if (iter != pimpl_->result_requests_.end()) {
-    for (auto & request_header : iter->second) {
+    // if there are clients who already asked for the result, send it to them
+    auto iter = pimpl_->result_requests_.find(uuid);
+    if (iter != pimpl_->result_requests_.end()) {
       std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
-      rcl_ret_t ret = rcl_action_send_result_response(
-        pimpl_->action_server_.get(), &request_header, result_msg.get());
-      if (RCL_RET_OK != ret) {
-        rclcpp::exceptions::throw_from_rcl_error(ret);
+      for (auto & request_header : iter->second) {
+        rcl_ret_t ret = rcl_action_send_result_response(
+          pimpl_->action_server_.get(), &request_header, result_msg.get());
+        if (RCL_RET_OK != ret) {
+          rclcpp::exceptions::throw_from_rcl_error(ret);
+        }
       }
     }
   }

rclcpp_action/test/benchmark/CMakeLists.txt

@@ -7,7 +7,7 @@ find_package(performance_test_fixture REQUIRED)
 add_performance_test(
   benchmark_action_client
   benchmark_action_client.cpp
-  TIMEOUT 120)
+  TIMEOUT 240)
 if(TARGET benchmark_action_client)
   target_link_libraries(benchmark_action_client ${PROJECT_NAME})
   ament_target_dependencies(benchmark_action_client rclcpp test_msgs)

rclcpp_components/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package rclcpp_components
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+11.0.0 (2021-05-18)
+-------------------
+
 10.0.0 (2021-05-11)
 -------------------
 

rclcpp_components/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp_components</name>
-  <version>10.0.0</version>
+  <version>11.0.0</version>
   <description>Package containing tools for dynamically loadable components</description>
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
   <maintainer email="mabel@openrobotics.org">Mabel Zhang</maintainer>

rclcpp_lifecycle/CHANGELOG.rst

@@ -3,6 +3,11 @@ Changelog for package rclcpp_lifecycle
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+11.0.0 (2021-05-18)
+-------------------
+* Fix destruction order in lifecycle benchmark (`#1675 <https://github.com/ros2/rclcpp/issues/1675>`_)
+* Contributors: Scott K Logan
+
 10.0.0 (2021-05-11)
 -------------------
 * [rclcpp] Type Adaptation feature (`#1557 <https://github.com/ros2/rclcpp/issues/1557>`_)

rclcpp_lifecycle/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp_lifecycle</name>
-  <version>10.0.0</version>
+  <version>11.0.0</version>
   <description>Package containing a prototype for lifecycle implementation</description>
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
   <maintainer email="mabel@openrobotics.org">Mabel Zhang</maintainer>

rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp

@@ -212,8 +212,9 @@ public:
     performance_test_fixture::PerformanceTest::TearDown(state);
     executor->cancel();
     spinner_.join();
-    lifecycle_node.reset();
+    executor.reset();
     lifecycle_client.reset();
+    lifecycle_node.reset();
     rclcpp::shutdown();
   }