18.0.0

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

CODEOWNERS

@@ -0,0 +1,2 @@
+# This file was generated by https://github.com/audrow/update-ros2-repos
+* @ivanpauno @hidmic @wjwwood

README.md

@@ -8,7 +8,8 @@ rclcpp provides the standard C++ API for interacting with ROS 2.
 
 `#include "rclcpp/rclcpp.hpp"` allows use of the most common elements of the ROS 2 system.
 
-Visit the [rclcpp API documentation](http://docs.ros2.org/latest/api/rclcpp/) for a complete list of its main components.
+The link to the latest API documentation can be found on the rclcpp package info page, at the [ROS Index](https://index.ros.org/p/rclcpp/).
+
 
 ### Examples
 

rclcpp/CHANGELOG.rst

@@ -2,6 +2,36 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+18.0.0 (2022-12-29)
+-------------------
+* Implement Unified Node Interface (NodeInterfaces class) (`#2041 <https://github.com/ros2/rclcpp/issues/2041>`_)
+* Do not throw exception if trying to dequeue an empty intra-process buffer (`#2061 <https://github.com/ros2/rclcpp/issues/2061>`_)
+* Move event callback binding to PublisherBase and SubscriptionBase (`#2066 <https://github.com/ros2/rclcpp/issues/2066>`_)
+* Implement validity checks for rclcpp::Clock (`#2040 <https://github.com/ros2/rclcpp/issues/2040>`_)
+* Explicitly set callback type (`#2059 <https://github.com/ros2/rclcpp/issues/2059>`_)
+* Fix logging macros to build with msvc and cpp20 (`#2063 <https://github.com/ros2/rclcpp/issues/2063>`_)
+* Add clock type to node_options (`#1982 <https://github.com/ros2/rclcpp/issues/1982>`_)
+* Fix nullptr dereference in prune_requests_older_than (`#2008 <https://github.com/ros2/rclcpp/issues/2008>`_)
+* Remove templating on to_rcl_subscription_options (`#2056 <https://github.com/ros2/rclcpp/issues/2056>`_)
+* Fix SharedFuture from async_send_request never becoming valid (`#2044 <https://github.com/ros2/rclcpp/issues/2044>`_)
+* Add in a warning for a KeepLast depth of 0. (`#2048 <https://github.com/ros2/rclcpp/issues/2048>`_)
+* Mark rclcpp::Clock::now() as const (`#2050 <https://github.com/ros2/rclcpp/issues/2050>`_)
+* Fix a case that did not throw ParameterUninitializedException (`#2036 <https://github.com/ros2/rclcpp/issues/2036>`_)
+* Update maintainers (`#2043 <https://github.com/ros2/rclcpp/issues/2043>`_)
+* Contributors: Alberto Soragna, Audrow Nash, Chen Lihui, Chris Lalancette, Jeffery Hsu, Lei Liu, Mateusz Szczygielski, Shane Loretz, andrei, mauropasse, methylDragon
+
+17.1.0 (2022-11-02)
+-------------------
+* MultiThreadExecutor number of threads is at least 2+ in default. (`#2032 <https://github.com/ros2/rclcpp/issues/2032>`_)
+* Fix bug that a callback not reached (`#1640 <https://github.com/ros2/rclcpp/issues/1640>`_)
+* Set the minimum number of threads of the Multithreaded executor to 2 (`#2030 <https://github.com/ros2/rclcpp/issues/2030>`_)
+* check thread whether joinable before join (`#2019 <https://github.com/ros2/rclcpp/issues/2019>`_)
+* Set cpplint test timeout to 3 minutes (`#2022 <https://github.com/ros2/rclcpp/issues/2022>`_)
+* Make sure to include-what-you-use in the node_interfaces. (`#2018 <https://github.com/ros2/rclcpp/issues/2018>`_)
+* Do not clear entities callbacks on destruction (`#2002 <https://github.com/ros2/rclcpp/issues/2002>`_)
+* fix mismatched issue if using zero_allocate (`#1995 <https://github.com/ros2/rclcpp/issues/1995>`_)
+* Contributors: Alexis Paques, Chen Lihui, Chris Lalancette, Cristóbal Arroyo, Tomoya Fujita, mauropasse, uupks
+
 17.0.0 (2022-09-13)
 -------------------
 * Make ParameterService and Sync/AsyncParameterClient accept rclcpp::QoS (`#1978 <https://github.com/ros2/rclcpp/issues/1978>`_)

rclcpp/CMakeLists.txt

@@ -259,4 +259,8 @@ if(TEST cppcheck)
   set_tests_properties(cppcheck PROPERTIES TIMEOUT 500)
 endif()
 
+if(TEST cpplint)
+  set_tests_properties(cpplint PROPERTIES TIMEOUT 180)
+endif()
+
 ament_generate_version_header(${PROJECT_NAME})

rclcpp/include/rclcpp/allocator/allocator_common.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_
 #define RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_
 
+#include <cstring>
 #include <memory>
 
 #include "rcl/allocator.h"
@@ -39,6 +40,22 @@ void * retyped_allocate(size_t size, void * untyped_allocator)
   return std::allocator_traits<Alloc>::allocate(*typed_allocator, size);
 }
 
+template<typename Alloc>
+void * retyped_zero_allocate(size_t number_of_elem, size_t size_of_elem, void * untyped_allocator)
+{
+  auto typed_allocator = static_cast<Alloc *>(untyped_allocator);
+  if (!typed_allocator) {
+    throw std::runtime_error("Received incorrect allocator type");
+  }
+  size_t size = number_of_elem * size_of_elem;
+  void * allocated_memory =
+    std::allocator_traits<Alloc>::allocate(*typed_allocator, size);
+  if (allocated_memory) {
+    std::memset(allocated_memory, 0, size);
+  }
+  return allocated_memory;
+}
+
 template<typename T, typename Alloc>
 void retyped_deallocate(void * untyped_pointer, void * untyped_allocator)
 {
@@ -73,6 +90,7 @@ rcl_allocator_t get_rcl_allocator(Alloc & allocator)
   rcl_allocator_t rcl_allocator = rcl_get_default_allocator();
 #ifndef _WIN32
   rcl_allocator.allocate = &retyped_allocate<Alloc>;
+  rcl_allocator.zero_allocate = &retyped_zero_allocate<Alloc>;
   rcl_allocator.deallocate = &retyped_deallocate<T, Alloc>;
   rcl_allocator.reallocate = &retyped_reallocate<T, Alloc>;
   rcl_allocator.state = &allocator;

rclcpp/include/rclcpp/callback_group.hpp

@@ -16,11 +16,14 @@
 #define RCLCPP__CALLBACK_GROUP_HPP_
 
 #include <atomic>
+#include <functional>
+#include <memory>
 #include <mutex>
-#include <string>
 #include <vector>
 
 #include "rclcpp/client.hpp"
+#include "rclcpp/context.hpp"
+#include "rclcpp/guard_condition.hpp"
 #include "rclcpp/publisher_base.hpp"
 #include "rclcpp/service.hpp"
 #include "rclcpp/subscription_base.hpp"
@@ -95,6 +98,10 @@ public:
     CallbackGroupType group_type,
     bool automatically_add_to_executor_with_node = true);
 
+  /// Default destructor.
+  RCLCPP_PUBLIC
+  ~CallbackGroup();
+
   template<typename Function>
   rclcpp::SubscriptionBase::SharedPtr
   find_subscription_ptrs_if(Function func) const
@@ -171,6 +178,16 @@ public:
   bool
   automatically_add_to_executor_with_node() const;
 
+  /// Defer creating the notify guard condition and return it.
+  RCLCPP_PUBLIC
+  rclcpp::GuardCondition::SharedPtr
+  get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr);
+
+  /// Trigger the notify guard condition.
+  RCLCPP_PUBLIC
+  void
+  trigger_notify_guard_condition();
+
 protected:
   RCLCPP_DISABLE_COPY(CallbackGroup)
 
@@ -213,6 +230,9 @@ protected:
   std::vector<rclcpp::Waitable::WeakPtr> waitable_ptrs_;
   std::atomic_bool can_be_taken_from_;
   const bool automatically_add_to_executor_with_node_;
+  // defer the creation of the guard condition
+  std::shared_ptr<rclcpp::GuardCondition> notify_guard_condition_ = nullptr;
+  std::recursive_mutex notify_guard_condition_mutex_;
 
 private:
   template<typename TypeT, typename Function>

rclcpp/include/rclcpp/client.hpp

@@ -130,7 +130,7 @@ public:
     rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph);
 
   RCLCPP_PUBLIC
-  virtual ~ClientBase();
+  virtual ~ClientBase() = default;
 
   /// Take the next response for this client as a type erased pointer.
   /**
@@ -312,7 +312,7 @@ public:
     // This two-step setting, prevents a gap where the old std::function has
     // been replaced but the middleware hasn't been told about the new one yet.
     set_on_new_response_callback(
-      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      rclcpp::detail::cpp_callback_trampoline<decltype(new_callback), const void *, size_t>,
       static_cast<const void *>(&new_callback));
 
     // Store the std::function to keep it in scope, also overwrites the existing one.
@@ -320,7 +320,8 @@ public:
 
     // Set it again, now using the permanent storage.
     set_on_new_response_callback(
-      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      rclcpp::detail::cpp_callback_trampoline<
+        decltype(on_new_response_callback_), const void *, size_t>,
       static_cast<const void *>(&on_new_response_callback_));
   }
 
@@ -769,7 +770,9 @@ public:
     auto old_size = pending_requests_.size();
     for (auto it = pending_requests_.begin(), last = pending_requests_.end(); it != last; ) {
       if (it->second.first < time_point) {
-        pruned_requests->push_back(it->first);
+        if (pruned_requests) {
+          pruned_requests->push_back(it->first);
+        }
         it = pending_requests_.erase(it);
       } else {
         ++it;
@@ -792,16 +795,14 @@ protected:
   async_send_request_impl(const Request & request, CallbackInfoVariant value)
   {
     int64_t sequence_number;
+    std::lock_guard<std::mutex> lock(pending_requests_mutex_);
     rcl_ret_t ret = rcl_send_request(get_client_handle().get(), &request, &sequence_number);
     if (RCL_RET_OK != ret) {
       rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request");
     }
-    {
-      std::lock_guard<std::mutex> lock(pending_requests_mutex_);
-      pending_requests_.try_emplace(
-        sequence_number,
-        std::make_pair(std::chrono::system_clock::now(), std::move(value)));
-    }
+    pending_requests_.try_emplace(
+      sequence_number,
+      std::make_pair(std::chrono::system_clock::now(), std::move(value)));
     return sequence_number;
   }
 

rclcpp/include/rclcpp/clock.hpp

@@ -77,7 +77,7 @@ public:
    */
   RCLCPP_PUBLIC
   Time
-  now();
+  now() const;
 
   /**
    * Sleep until a specified Time, according to clock type.
@@ -137,6 +137,51 @@ public:
     Duration rel_time,
     Context::SharedPtr context = contexts::get_global_default_context());
 
+  /**
+   * Check if the clock is started.
+   *
+   * A started clock is a clock that reflects non-zero time.
+   * Typically a clock will be unstarted if it is using RCL_ROS_TIME with ROS time and
+   * nothing has been published on the clock topic yet.
+   *
+   * \return true if clock is started
+   * \throws std::runtime_error if the clock is not rcl_clock_valid
+   */
+  RCLCPP_PUBLIC
+  bool
+  started();
+
+  /**
+   * Wait until clock to start.
+   *
+   * \rclcpp::Clock::started
+   * \param context the context to wait in
+   * \return true if clock was already started or became started
+   * \throws std::runtime_error if the context is invalid or clock is not rcl_clock_valid
+   */
+  RCLCPP_PUBLIC
+  bool
+  wait_until_started(Context::SharedPtr context = contexts::get_global_default_context());
+
+  /**
+   * Wait for clock to start, with timeout.
+   *
+   * The timeout is waited in steady time.
+   *
+   * \rclcpp::Clock::started
+   * \param timeout the maximum time to wait for.
+   * \param context the context to wait in.
+   * \param wait_tick_ns the time to wait between each iteration of the wait loop (in nanoseconds).
+   * \return true if clock was or became valid
+   * \throws std::runtime_error if the context is invalid or clock is not rcl_clock_valid
+   */
+  RCLCPP_PUBLIC
+  bool
+  wait_until_started(
+    const rclcpp::Duration & timeout,
+    Context::SharedPtr context = contexts::get_global_default_context(),
+    const rclcpp::Duration & wait_tick_ns = rclcpp::Duration(0, static_cast<uint32_t>(1e7)));
+
   /**
    * Returns the clock of the type `RCL_ROS_TIME` is active.
    *

rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp

@@ -41,6 +41,7 @@ namespace detail
  * so no exceptions should be thrown at this point, and doing so results in
  * undefined behavior.
  *
+ * \tparam UserDataRealT Declared type of the passed function
  * \tparam UserDataT Deduced type based on what is passed for user data,
  *   usually this type is either `void *` or `const void *`.
  * \tparam Args the arguments being passed to the callback
@@ -50,6 +51,7 @@ namespace detail
  * \returns whatever the callback returns, if anything
  */
 template<
+  typename UserDataRealT,
   typename UserDataT,
   typename ... Args,
   typename ReturnT = void
@@ -57,7 +59,7 @@ template<
 ReturnT
 cpp_callback_trampoline(UserDataT user_data, Args ... args) noexcept
 {
-  auto & actual_callback = *reinterpret_cast<const std::function<ReturnT(Args...)> *>(user_data);
+  auto & actual_callback = *static_cast<const UserDataRealT *>(user_data);
   return actual_callback(args ...);
 }
 

rclcpp/include/rclcpp/detail/template_contains.hpp

@@ -0,0 +1,47 @@
+// Copyright 2022 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__DETAIL__TEMPLATE_CONTAINS_HPP_
+#define RCLCPP__DETAIL__TEMPLATE_CONTAINS_HPP_
+
+#include <type_traits>
+
+namespace rclcpp
+{
+namespace detail
+{
+
+/// Template meta-function that checks if a given T is contained in the list Us.
+template<typename T, typename ... Us>
+struct template_contains;
+
+template<typename ... Args>
+inline constexpr bool template_contains_v = template_contains<Args ...>::value;
+
+template<typename T, typename NextT, typename ... Us>
+struct template_contains<T, NextT, Us ...>
+{
+  enum { value = (std::is_same_v<T, NextT>|| template_contains_v<T, Us ...>)};
+};
+
+template<typename T>
+struct template_contains<T>
+{
+  enum { value = false };
+};
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__TEMPLATE_CONTAINS_HPP_

rclcpp/include/rclcpp/detail/template_unique.hpp

@@ -0,0 +1,49 @@
+// Copyright 2022 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__DETAIL__TEMPLATE_UNIQUE_HPP_
+#define RCLCPP__DETAIL__TEMPLATE_UNIQUE_HPP_
+
+#include <type_traits>
+
+#include "rclcpp/detail/template_contains.hpp"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+/// Template meta-function that checks if a given list Ts contains unique types.
+template<typename ... Ts>
+struct template_unique;
+
+template<typename ... Args>
+inline constexpr bool template_unique_v = template_unique<Args ...>::value;
+
+template<typename NextT, typename ... Ts>
+struct template_unique<NextT, Ts ...>
+{
+  enum { value = !template_contains_v<NextT, Ts ...>&& template_unique_v<Ts ...>};
+};
+
+template<typename T>
+struct template_unique<T>
+{
+  enum { value = true };
+};
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__TEMPLATE_UNIQUE_HPP_

rclcpp/include/rclcpp/executor.hpp

@@ -560,14 +560,14 @@ protected:
   virtual void
   spin_once_impl(std::chrono::nanoseconds timeout);
 
-  typedef std::map<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
+  typedef std::map<rclcpp::CallbackGroup::WeakPtr,
       const rclcpp::GuardCondition *,
-      std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>
-    WeakNodesToGuardConditionsMap;
+      std::owner_less<rclcpp::CallbackGroup::WeakPtr>>
+    WeakCallbackGroupsToGuardConditionsMap;
 
-  /// maps nodes to guard conditions
-  WeakNodesToGuardConditionsMap
-  weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+  /// maps callback groups to guard conditions
+  WeakCallbackGroupsToGuardConditionsMap
+  weak_groups_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
 
   /// maps callback groups associated to nodes
   WeakCallbackGroupsToNodesMap

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -47,7 +47,7 @@ public:
    *
    * \param options common options for all executors
    * \param number_of_threads number of threads to have in the thread pool,
-   *   the default 0 will use the number of cpu cores found instead
+   *   the default 0 will use the number of cpu cores found (minimum of 2)
    * \param yield_before_execute if true std::this_thread::yield() is called
    * \param timeout maximum time to wait
    */

rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp

@@ -86,8 +86,7 @@ public:
     std::lock_guard<std::mutex> lock(mutex_);
 
     if (!has_data_()) {
-      RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Calling dequeue on empty intra-process buffer");
-      throw std::runtime_error("Calling dequeue on empty intra-process buffer");
+      return BufferT();
     }
 
     auto request = std::move(ring_buffer_[read_index_]);

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -109,8 +109,14 @@ public:
 
     if (any_callback_.use_take_shared_method()) {
       shared_msg = this->buffer_->consume_shared();
+      if (!shared_msg) {
+        return nullptr;
+      }
     } else {
       unique_msg = this->buffer_->consume_unique();
+      if (!unique_msg) {
+        return nullptr;
+      }
     }
     return std::static_pointer_cast<void>(
       std::make_shared<std::pair<ConstMessageSharedPtr, MessageUniquePtr>>(
@@ -138,7 +144,7 @@ protected:
   execute_impl(std::shared_ptr<void> & data)
   {
     if (!data) {
-      throw std::runtime_error("'data' is empty");
+      return;
     }
 
     rmw_message_info_t msg_info;

rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp

@@ -52,7 +52,7 @@ public:
   {}
 
   RCLCPP_PUBLIC
-  virtual ~SubscriptionIntraProcessBase();
+  virtual ~SubscriptionIntraProcessBase() = default;
 
   RCLCPP_PUBLIC
   size_t

rclcpp/include/rclcpp/generic_publisher.hpp

@@ -78,38 +78,12 @@ public:
       node_base,
       topic_name,
       *rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib),
-      options.template to_rcl_publisher_options<rclcpp::SerializedMessage>(qos)),
+      options.template to_rcl_publisher_options<rclcpp::SerializedMessage>(qos),
+      // NOTE(methylDragon): Passing these args separately is necessary for event binding
+      options.event_callbacks,
+      options.use_default_callbacks),
     ts_lib_(ts_lib)
-  {
-    // This is unfortunately duplicated with the code in publisher.hpp.
-    // TODO(nnmm): Deduplicate by moving this into PublisherBase.
-    if (options.event_callbacks.deadline_callback) {
-      this->add_event_handler(
-        options.event_callbacks.deadline_callback,
-        RCL_PUBLISHER_OFFERED_DEADLINE_MISSED);
-    }
-    if (options.event_callbacks.liveliness_callback) {
-      this->add_event_handler(
-        options.event_callbacks.liveliness_callback,
-        RCL_PUBLISHER_LIVELINESS_LOST);
-    }
-    if (options.event_callbacks.incompatible_qos_callback) {
-      this->add_event_handler(
-        options.event_callbacks.incompatible_qos_callback,
-        RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
-    } else if (options.use_default_callbacks) {
-      // Register default callback when not specified
-      try {
-        this->add_event_handler(
-          [this](QOSOfferedIncompatibleQoSInfo & info) {
-            this->default_incompatible_qos_callback(info);
-          },
-          RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
-      } catch (UnsupportedEventTypeException & /*exc*/) {
-        // pass
-      }
-    }
-  }
+  {}
 
   RCLCPP_PUBLIC
   virtual ~GenericPublisher() = default;

rclcpp/include/rclcpp/generic_subscription.hpp

@@ -81,45 +81,13 @@ public:
       node_base,
       *rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib),
       topic_name,
-      options.template to_rcl_subscription_options<rclcpp::SerializedMessage>(qos),
+      options.to_rcl_subscription_options(qos),
+      options.event_callbacks,
+      options.use_default_callbacks,
       true),
     callback_(callback),
     ts_lib_(ts_lib)
-  {
-    // This is unfortunately duplicated with the code in subscription.hpp.
-    // TODO(nnmm): Deduplicate by moving this into SubscriptionBase.
-    if (options.event_callbacks.deadline_callback) {
-      this->add_event_handler(
-        options.event_callbacks.deadline_callback,
-        RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED);
-    }
-    if (options.event_callbacks.liveliness_callback) {
-      this->add_event_handler(
-        options.event_callbacks.liveliness_callback,
-        RCL_SUBSCRIPTION_LIVELINESS_CHANGED);
-    }
-    if (options.event_callbacks.incompatible_qos_callback) {
-      this->add_event_handler(
-        options.event_callbacks.incompatible_qos_callback,
-        RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
-    } else if (options.use_default_callbacks) {
-      // Register default callback when not specified
-      try {
-        this->add_event_handler(
-          [this](QOSRequestedIncompatibleQoSInfo & info) {
-            this->default_incompatible_qos_callback(info);
-          },
-          RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
-      } catch (UnsupportedEventTypeException & /*exc*/) {
-        // pass
-      }
-    }
-    if (options.event_callbacks.message_lost_callback) {
-      this->add_event_handler(
-        options.event_callbacks.message_lost_callback,
-        RCL_SUBSCRIPTION_MESSAGE_LOST);
-    }
-  }
+  {}
 
   RCLCPP_PUBLIC
   virtual ~GenericSubscription() = default;

rclcpp/include/rclcpp/node.hpp

@@ -740,17 +740,21 @@ public:
   /**
    * If the parameter has not been declared, then this method may throw the
    * rclcpp::exceptions::ParameterNotDeclaredException exception.
+   * If the parameter has not been initialized, then this method may throw the
+   * rclcpp::exceptions::ParameterUninitializedException exception.
    *
    * If undeclared parameters are allowed, see the node option
    * rclcpp::NodeOptions::allow_undeclared_parameters, then this method will
-   * not throw an exception, and instead return a default initialized
-   * rclcpp::Parameter, which has a type of
+   * not throw the rclcpp::exceptions::ParameterNotDeclaredException exception,
+   * and instead return a default initialized rclcpp::Parameter, which has a type of
    * rclcpp::ParameterType::PARAMETER_NOT_SET.
    *
    * \param[in] name The name of the parameter to get.
    * \return The requested parameter inside of a rclcpp parameter object.
    * \throws rclcpp::exceptions::ParameterNotDeclaredException if the parameter
    *   has not been declared and undeclared parameters are not allowed.
+   * \throws rclcpp::exceptions::ParameterUninitializedException if the parameter
+   *   has not been initialized.
    */
   RCLCPP_PUBLIC
   rclcpp::Parameter
@@ -834,12 +838,12 @@ public:
 
   /// Return the parameters by the given parameter names.
   /**
-   * Like get_parameters(), this method may throw the
+   * Like get_parameter(const std::string &), this method may throw the
    * rclcpp::exceptions::ParameterNotDeclaredException exception if the
    * requested parameter has not been declared and undeclared parameters are
-   * not allowed.
+   * not allowed, and may throw the rclcpp::exceptions::ParameterUninitializedException exception.
    *
-   * Also like get_parameters(), if undeclared parameters are allowed and the
+   * Also like get_parameter(const std::string &), if undeclared parameters are allowed and the
    * parameter has not been declared, then the corresponding rclcpp::Parameter
    * will be default initialized and therefore have the type
    * rclcpp::ParameterType::PARAMETER_NOT_SET.
@@ -849,6 +853,8 @@ public:
    * \throws rclcpp::exceptions::ParameterNotDeclaredException if any of the
    *   parameters have not been declared and undeclared parameters are not
    *   allowed.
+   * \throws rclcpp::exceptions::ParameterUninitializedException if any of the
+   *   parameters have not been initialized.
    */
   RCLCPP_PUBLIC
   std::vector<rclcpp::Parameter>

rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp

@@ -0,0 +1,204 @@
+// Copyright 2022 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__NODE_INTERFACES__DETAIL__NODE_INTERFACES_HELPERS_HPP_
+#define RCLCPP__NODE_INTERFACES__DETAIL__NODE_INTERFACES_HELPERS_HPP_
+
+#include <memory>
+#include <tuple>
+#include <type_traits>
+#include <utility>
+
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+namespace detail
+{
+
+// Support and Helper template classes for the NodeInterfaces class.
+
+template<typename NodeT, typename ... Ts>
+std::tuple<std::shared_ptr<Ts>...>
+init_tuple(NodeT & n);
+
+/// Stores the interfaces in a tuple, provides constructors, and getters.
+template<typename ... InterfaceTs>
+struct NodeInterfacesStorage
+{
+  template<typename NodeT>
+  NodeInterfacesStorage(NodeT & node)  // NOLINT(runtime/explicit)
+  : interfaces_(init_tuple<decltype(node), InterfaceTs ...>(node))
+  {}
+
+  explicit NodeInterfacesStorage(std::shared_ptr<InterfaceTs>... args)
+  : interfaces_(args ...)
+  {}
+
+  /// Individual Node Interface non-const getter.
+  template<typename NodeInterfaceT>
+  std::shared_ptr<NodeInterfaceT>
+  get()
+  {
+    static_assert(
+      (std::is_same_v<NodeInterfaceT, InterfaceTs>|| ...),
+      "NodeInterfaces class does not contain given NodeInterfaceT");
+    return std::get<std::shared_ptr<NodeInterfaceT>>(interfaces_);
+  }
+
+  /// Individual Node Interface const getter.
+  template<typename NodeInterfaceT>
+  std::shared_ptr<const NodeInterfaceT>
+  get() const
+  {
+    static_assert(
+      (std::is_same_v<NodeInterfaceT, InterfaceTs>|| ...),
+      "NodeInterfaces class does not contain given NodeInterfaceT");
+    return std::get<std::shared_ptr<NodeInterfaceT>>(interfaces_);
+  }
+
+protected:
+  std::tuple<std::shared_ptr<InterfaceTs>...> interfaces_;
+};
+
+/// Prototype of NodeInterfacesSupports.
+/**
+ * Should read NodeInterfacesSupports<..., T, ...> as "NodeInterfaces supports T", and
+ * if NodeInterfacesSupport is specialized for T, the is_supported should be
+ * set to std::true_type, but by default it is std::false_type, which will
+ * lead to a compiler error when trying to use T with NodeInterfaces.
+ */
+template<typename StorageClassT, typename ... Ts>
+struct NodeInterfacesSupports;
+
+/// Prototype of NodeInterfacesSupportCheck template meta-function.
+/**
+ * This meta-function checks that all the types given are supported,
+ * throwing a more human-readable error if an unsupported type is used.
+ */
+template<typename StorageClassT, typename ... InterfaceTs>
+struct NodeInterfacesSupportCheck;
+
+/// Iterating specialization that ensures classes are supported and inherited.
+template<typename StorageClassT, typename NextInterfaceT, typename ... RemainingInterfaceTs>
+struct NodeInterfacesSupportCheck<StorageClassT, NextInterfaceT, RemainingInterfaceTs ...>
+  : public NodeInterfacesSupportCheck<StorageClassT, RemainingInterfaceTs ...>
+{
+  static_assert(
+    NodeInterfacesSupports<StorageClassT, NextInterfaceT>::is_supported::value,
+    "given NodeInterfaceT is not supported by rclcpp::node_interfaces::NodeInterfaces");
+};
+
+/// Terminating case when there are no more "RemainingInterfaceTs".
+template<typename StorageClassT>
+struct NodeInterfacesSupportCheck<StorageClassT>
+{};
+
+/// Default specialization, needs to be specialized for each supported interface.
+template<typename StorageClassT, typename ... RemainingInterfaceTs>
+struct NodeInterfacesSupports
+{
+  // Specializations need to set this to std::true_type in addition to other interfaces.
+  using is_supported = std::false_type;
+};
+
+/// Terminating specialization of NodeInterfacesSupports.
+template<typename StorageClassT>
+struct NodeInterfacesSupports<StorageClassT>
+  : public StorageClassT
+{
+  /// Perfect forwarding constructor to get arguments down to StorageClassT.
+  template<typename ... ArgsT>
+  explicit NodeInterfacesSupports(ArgsT && ... args)
+  : StorageClassT(std::forward<ArgsT>(args) ...)
+  {}
+};
+
+// Helper functions to initialize the tuple in NodeInterfaces.
+
+template<typename StorageClassT, typename ElementT, typename TupleT, typename NodeT>
+void
+init_element(TupleT & t, NodeT & n)
+{
+  std::get<std::shared_ptr<ElementT>>(t) =
+    NodeInterfacesSupports<StorageClassT, ElementT>::get_from_node_like(n);
+}
+
+template<typename NodeT, typename ... Ts>
+std::tuple<std::shared_ptr<Ts>...>
+init_tuple(NodeT & n)
+{
+  using StorageClassT = NodeInterfacesStorage<Ts ...>;
+  std::tuple<std::shared_ptr<Ts>...> t;
+  (init_element<StorageClassT, Ts>(t, n), ...);
+  return t;
+}
+
+/// Macro for creating specializations with less boilerplate.
+/**
+ * You can use this macro to add support for your interface class if:
+ *
+ * - The standard getter is get_node_{NodeInterfaceName}_interface(), and
+ * - the getter returns a non-const shared_ptr<{NodeInterfaceType}>
+ *
+ * Examples of using this can be seen in the standard node interface headers
+ * in rclcpp, e.g. rclcpp/node_interfaces/node_base_interface.hpp has:
+ *
+ *   RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base)
+ *
+ * If your interface has a non-standard getter, or you want to instrument it or
+ * something like that, then you'll need to create your own specialization of
+ * the NodeInterfacesSupports struct without this macro.
+ */
+#define RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(NodeInterfaceType, NodeInterfaceName) \
+  namespace rclcpp::node_interfaces::detail { \
+  template<typename StorageClassT, typename ... RemainingInterfaceTs> \
+  struct NodeInterfacesSupports< \
+    StorageClassT, \
+    NodeInterfaceType, \
+    RemainingInterfaceTs ...> \
+    : public NodeInterfacesSupports<StorageClassT, RemainingInterfaceTs ...> \
+  { \
+    using is_supported = std::true_type; \
+ \
+    template<typename NodeT> \
+    static \
+    std::shared_ptr<NodeInterfaceType> \
+    get_from_node_like(NodeT & node_like) \
+    { \
+      return node_like.get_node_ ## NodeInterfaceName ## _interface(); \
+    } \
+ \
+    /* Perfect forwarding constructor to get arguments down to StorageClassT (eventually). */ \
+    template<typename ... ArgsT> \
+    explicit NodeInterfacesSupports(ArgsT && ... args) \
+      : NodeInterfacesSupports<StorageClassT, RemainingInterfaceTs ...>( \
+        std::forward<ArgsT>(args) ...) \
+    {} \
+ \
+    std::shared_ptr<NodeInterfaceType> \
+    get_node_ ## NodeInterfaceName ## _interface() \
+    { \
+      return StorageClassT::template get<NodeInterfaceType>(); \
+    } \
+  }; \
+  }  // namespace rclcpp::node_interfaces::detail
+
+}  // namespace detail
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__DETAIL__NODE_INTERFACES_HELPERS_HPP_

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -15,7 +15,7 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_
 
-#include <functional>
+#include <atomic>
 #include <memory>
 #include <mutex>
 #include <string>

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -15,10 +15,10 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_
 
+#include <atomic>
+#include <functional>
 #include <memory>
-#include <mutex>
 #include <string>
-#include <vector>
 
 #include "rcl/node.h"
 
@@ -26,6 +26,7 @@
 #include "rclcpp/context.hpp"
 #include "rclcpp/guard_condition.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -177,4 +178,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_clock.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_
 
+#include "rcl/time.h"
 #include "rclcpp/clock.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
@@ -42,7 +43,8 @@ public:
     rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
     rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
     rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
-    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging);
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+    rcl_clock_type_t clock_type);
 
   RCLCPP_PUBLIC
   virtual
@@ -67,7 +69,7 @@ private:
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
   rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
 
-  rclcpp::Clock::SharedPtr ros_clock_;
+  rclcpp::Clock::SharedPtr clock_;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp

@@ -17,6 +17,7 @@
 
 #include "rclcpp/clock.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -50,4 +51,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeClockInterface, clock)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_CLOCK_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_graph.hpp

@@ -15,7 +15,9 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_
 
+#include <atomic>
 #include <chrono>
+#include <condition_variable>
 #include <map>
 #include <memory>
 #include <mutex>

rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp

@@ -29,6 +29,7 @@
 
 #include "rclcpp/event.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -382,4 +383,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeGraphInterface, graph)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp

@@ -0,0 +1,165 @@
+// Copyright 2022 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__NODE_INTERFACES__NODE_INTERFACES_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_INTERFACES_HPP_
+
+#include <memory>
+
+#include "rclcpp/detail/template_unique.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
+
+#define ALL_RCLCPP_NODE_INTERFACES \
+  rclcpp::node_interfaces::NodeBaseInterface, \
+  rclcpp::node_interfaces::NodeClockInterface, \
+  rclcpp::node_interfaces::NodeGraphInterface, \
+  rclcpp::node_interfaces::NodeLoggingInterface, \
+  rclcpp::node_interfaces::NodeParametersInterface, \
+  rclcpp::node_interfaces::NodeServicesInterface, \
+  rclcpp::node_interfaces::NodeTimeSourceInterface, \
+  rclcpp::node_interfaces::NodeTimersInterface, \
+  rclcpp::node_interfaces::NodeTopicsInterface, \
+  rclcpp::node_interfaces::NodeWaitablesInterface
+
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+
+/// A helper class for aggregating node interfaces.
+template<typename ... InterfaceTs>
+class NodeInterfaces
+  : public detail::NodeInterfacesSupportCheck<
+    detail::NodeInterfacesStorage<InterfaceTs ...>,
+    InterfaceTs ...
+  >,
+  public detail::NodeInterfacesSupports<
+    detail::NodeInterfacesStorage<InterfaceTs ...>,
+    InterfaceTs ...
+  >
+{
+  static_assert(
+    0 != sizeof ...(InterfaceTs),
+    "must provide at least one interface as a template argument");
+  static_assert(
+    rclcpp::detail::template_unique_v<InterfaceTs ...>,
+    "must provide unique template parameters");
+
+  using NodeInterfacesSupportsT = detail::NodeInterfacesSupports<
+    detail::NodeInterfacesStorage<InterfaceTs ...>,
+    InterfaceTs ...
+  >;
+
+public:
+  /// Create a new NodeInterfaces object using the given node-like object's interfaces.
+  /**
+   * Specify which interfaces you need by passing them as template parameters.
+   *
+   * This allows you to aggregate interfaces from different sources together to pass as a single
+   * aggregate object to any functions that take node interfaces or node-likes, without needing to
+   * templatize that function.
+   *
+   * You may also use this constructor to create a NodeInterfaces that contains a subset of
+   * another NodeInterfaces' interfaces.
+   *
+   * Finally, this class supports implicit conversion from node-like objects, allowing you to
+   * directly pass a node-like to a function that takes a NodeInterfaces object.
+   *
+   * Usage examples:
+   *   ```cpp
+   *   // Suppose we have some function:
+   *   void fn(NodeInterfaces<NodeBaseInterface, NodeClockInterface> interfaces);
+   *
+   *   // Then we can, explicitly:
+   *   rclcpp::Node node("some_node");
+   *   auto ni = NodeInterfaces<NodeBaseInterface, NodeClockInterface>(node);
+   *   fn(ni);
+   *
+   *   // But also:
+   *   fn(node);
+   *
+   *   // Subsetting a NodeInterfaces object also works!
+   *   auto ni_base = NodeInterfaces<NodeBaseInterface>(ni);
+   *
+   *   // Or aggregate them (you could aggregate interfaces from disparate node-likes)
+   *   auto ni_aggregated = NodeInterfaces<NodeBaseInterface, NodeClockInterface>(
+   *     node->get_node_base_interface(),
+   *     node->get_node_clock_interface()
+   *   )
+   *
+   *   // And then to access the interfaces:
+   *   // Get with get<>
+   *   auto base = ni.get<NodeBaseInterface>();
+   *
+   *   // Or the appropriate getter
+   *   auto clock = ni.get_clock_interface();
+   *   ```
+   *
+   * You may use any of the standard node interfaces that come with rclcpp:
+   *   - rclcpp::node_interfaces::NodeBaseInterface
+   *   - rclcpp::node_interfaces::NodeClockInterface
+   *   - rclcpp::node_interfaces::NodeGraphInterface
+   *   - rclcpp::node_interfaces::NodeLoggingInterface
+   *   - rclcpp::node_interfaces::NodeParametersInterface
+   *   - rclcpp::node_interfaces::NodeServicesInterface
+   *   - rclcpp::node_interfaces::NodeTimeSourceInterface
+   *   - rclcpp::node_interfaces::NodeTimersInterface
+   *   - rclcpp::node_interfaces::NodeTopicsInterface
+   *   - rclcpp::node_interfaces::NodeWaitablesInterface
+   *
+   * Or you use custom interfaces as long as you make a template specialization
+   * of the rclcpp::node_interfaces::detail::NodeInterfacesSupport struct using
+   * the RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT macro.
+   *
+   * Usage example:
+   *   ```RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base)```
+   *
+   * If you choose not to use the helper macro, then you can specialize the
+   * template yourself, but you must:
+   *
+   *   - Provide a template specialization of the get_from_node_like method that gets the interface
+   *     from any node-like that stores the interface, using the node-like's getter
+   *   - Designate the is_supported type as std::true_type using a using directive
+   *   - Provide any number of getter methods to be used to obtain the interface with the
+   *     NodeInterface object, noting that the getters of the storage class will apply to all
+   *     supported interfaces.
+   *     - The getter method names should not clash in name with any other interface getter
+   *       specializations if those other interfaces are meant to be aggregated in the same
+   *       NodeInterfaces object.
+   *
+   * \param[in] node Node-like object from which to get the node interfaces
+   */
+  template<typename NodeT>
+  NodeInterfaces(NodeT & node)  // NOLINT(runtime/explicit)
+  : NodeInterfacesSupportsT(node)
+  {}
+
+  /// NodeT::SharedPtr Constructor
+  template<typename NodeT>
+  NodeInterfaces(std::shared_ptr<NodeT> node)  // NOLINT(runtime/explicit)
+  : NodeInterfaces(node ? *node : throw std::invalid_argument("given node pointer is nullptr"))
+  {}
+
+  explicit NodeInterfaces(std::shared_ptr<InterfaceTs>... args)
+  : NodeInterfacesSupportsT(args ...)
+  {}
+};
+
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_INTERFACES_HPP_

rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp

@@ -19,6 +19,7 @@
 
 #include "rclcpp/logger.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -58,4 +59,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeLoggingInterface, logging)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_LOGGING_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -15,9 +15,10 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_
 
+#include <list>
 #include <map>
 #include <memory>
-#include <list>
+#include <mutex>
 #include <string>
 #include <vector>
 

rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp

@@ -15,8 +15,8 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_
 
+#include <functional>
 #include <map>
-#include <memory>
 #include <string>
 #include <vector>
 
@@ -25,6 +25,7 @@
 #include "rcl_interfaces/msg/set_parameters_result.hpp"
 
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/parameter.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -276,4 +277,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeParametersInterface, parameters)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp

@@ -20,6 +20,7 @@
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/client.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/service.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -62,4 +63,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeServicesInterface, services)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp

@@ -16,6 +16,7 @@
 #define RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_
 
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -37,4 +38,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeTimeSourceInterface, time_source)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp

@@ -17,6 +17,7 @@
 
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -47,4 +48,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeTimersInterface, timers)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_TIMERS_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp

@@ -15,8 +15,6 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_
 
-#include <functional>
-#include <memory>
 #include <string>
 
 #include "rcl/publisher.h"
@@ -24,6 +22,7 @@
 
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/node_interfaces/node_timers_interface.hpp"
 #include "rclcpp/publisher.hpp"
@@ -97,4 +96,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeTopicsInterface, topics)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp

@@ -17,6 +17,7 @@
 
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rclcpp/waitable.hpp"
 
@@ -54,4 +55,6 @@ public:
 }  // namespace node_interfaces
 }  // namespace rclcpp
 
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeWaitablesInterface, waitables)
+
 #endif  // RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_

rclcpp/include/rclcpp/node_options.hpp

@@ -19,6 +19,7 @@
 #include <string>
 #include <vector>
 
+#include "rcl/time.h"
 #include "rcl/node_options.h"
 #include "rclcpp/context.hpp"
 #include "rclcpp/contexts/default_context.hpp"
@@ -46,6 +47,7 @@ public:
    *   - enable_topic_statistics = false
    *   - start_parameter_services = true
    *   - start_parameter_event_publisher = true
+   *   - clock_type = RCL_ROS_TIME
    *   - clock_qos = rclcpp::ClockQoS()
    *   - use_clock_thread = true
    *   - rosout_qos = rclcpp::RosoutQoS()
@@ -246,6 +248,19 @@ public:
   NodeOptions &
   start_parameter_event_publisher(bool start_parameter_event_publisher);
 
+  /// Return a reference to the clock type.
+  RCLCPP_PUBLIC
+  const rcl_clock_type_t &
+  clock_type() const;
+
+  /// Set the clock type.
+  /**
+   * The clock type to be used by the node.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  clock_type(const rcl_clock_type_t & clock_type);
+
   /// Return a reference to the clock QoS.
   RCLCPP_PUBLIC
   const rclcpp::QoS &
@@ -400,6 +415,8 @@ private:
 
   bool start_parameter_event_publisher_ {true};
 
+  rcl_clock_type_t clock_type_ {RCL_ROS_TIME};
+
   rclcpp::QoS clock_qos_ = rclcpp::ClockQoS();
 
   bool use_clock_thread_ {true};

rclcpp/include/rclcpp/publisher.hpp

@@ -131,40 +131,16 @@ public:
       node_base,
       topic,
       rclcpp::get_message_type_support_handle<MessageT>(),
-      options.template to_rcl_publisher_options<MessageT>(qos)),
+      options.template to_rcl_publisher_options<MessageT>(qos),
+      // NOTE(methylDragon): Passing these args separately is necessary for event binding
+      options.event_callbacks,
+      options.use_default_callbacks),
     options_(options),
     published_type_allocator_(*options.get_allocator()),
     ros_message_type_allocator_(*options.get_allocator())
   {
     allocator::set_allocator_for_deleter(&published_type_deleter_, &published_type_allocator_);
     allocator::set_allocator_for_deleter(&ros_message_type_deleter_, &ros_message_type_allocator_);
-
-    if (options_.event_callbacks.deadline_callback) {
-      this->add_event_handler(
-        options_.event_callbacks.deadline_callback,
-        RCL_PUBLISHER_OFFERED_DEADLINE_MISSED);
-    }
-    if (options_.event_callbacks.liveliness_callback) {
-      this->add_event_handler(
-        options_.event_callbacks.liveliness_callback,
-        RCL_PUBLISHER_LIVELINESS_LOST);
-    }
-    if (options_.event_callbacks.incompatible_qos_callback) {
-      this->add_event_handler(
-        options_.event_callbacks.incompatible_qos_callback,
-        RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
-    } else if (options_.use_default_callbacks) {
-      // Register default callback when not specified
-      try {
-        this->add_event_handler(
-          [this](QOSOfferedIncompatibleQoSInfo & info) {
-            this->default_incompatible_qos_callback(info);
-          },
-          RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
-      } catch (UnsupportedEventTypeException & /*exc*/) {
-        // pass
-      }
-    }
     // Setup continues in the post construction method, post_init_setup().
   }
 

rclcpp/include/rclcpp/publisher_base.hpp

@@ -78,11 +78,18 @@ public:
     rclcpp::node_interfaces::NodeBaseInterface * node_base,
     const std::string & topic,
     const rosidl_message_type_support_t & type_support,
-    const rcl_publisher_options_t & publisher_options);
+    const rcl_publisher_options_t & publisher_options,
+    const PublisherEventCallbacks & event_callbacks,
+    bool use_default_callbacks);
 
   RCLCPP_PUBLIC
   virtual ~PublisherBase();
 
+  /// Add event handlers for passed in event_callbacks.
+  RCLCPP_PUBLIC
+  void
+  bind_event_callbacks(const PublisherEventCallbacks & event_callbacks, bool use_default_callbacks);
+
   /// Get the topic that this publisher publishes on.
   /** \return The topic name. */
   RCLCPP_PUBLIC
@@ -348,6 +355,8 @@ protected:
   rmw_gid_t rmw_gid_;
 
   const rosidl_message_type_support_t type_support_;
+
+  const PublisherEventCallbacks event_callbacks_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/qos_event.hpp

@@ -185,7 +185,7 @@ public:
     // This two-step setting, prevents a gap where the old std::function has
     // been replaced but the middleware hasn't been told about the new one yet.
     set_on_new_event_callback(
-      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      rclcpp::detail::cpp_callback_trampoline<decltype(new_callback), const void *, size_t>,
       static_cast<const void *>(&new_callback));
 
     // Store the std::function to keep it in scope, also overwrites the existing one.
@@ -193,7 +193,8 @@ public:
 
     // Set it again, now using the permanent storage.
     set_on_new_event_callback(
-      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      rclcpp::detail::cpp_callback_trampoline<
+        decltype(on_new_event_callback_), const void *, size_t>,
       static_cast<const void *>(&on_new_event_callback_));
   }
 

rclcpp/include/rclcpp/service.hpp

@@ -55,7 +55,7 @@ public:
   explicit ServiceBase(std::shared_ptr<rcl_node_t> node_handle);
 
   RCLCPP_PUBLIC
-  virtual ~ServiceBase();
+  virtual ~ServiceBase() = default;
 
   /// Return the name of the service.
   /** \return The name of the service. */
@@ -222,7 +222,7 @@ public:
     // This two-step setting, prevents a gap where the old std::function has
     // been replaced but the middleware hasn't been told about the new one yet.
     set_on_new_request_callback(
-      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      rclcpp::detail::cpp_callback_trampoline<decltype(new_callback), const void *, size_t>,
       static_cast<const void *>(&new_callback));
 
     // Store the std::function to keep it in scope, also overwrites the existing one.
@@ -230,7 +230,8 @@ public:
 
     // Set it again, now using the permanent storage.
     set_on_new_request_callback(
-      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      rclcpp::detail::cpp_callback_trampoline<
+        decltype(on_new_request_callback_), const void *, size_t>,
       static_cast<const void *>(&on_new_request_callback_));
   }
 

rclcpp/include/rclcpp/subscription.hpp

@@ -140,44 +140,15 @@ public:
       node_base,
       type_support_handle,
       topic_name,
-      options.template to_rcl_subscription_options<ROSMessageType>(qos),
+      options.to_rcl_subscription_options(qos),
+      // NOTE(methylDragon): Passing these args separately is necessary for event binding
+      options.event_callbacks,
+      options.use_default_callbacks,
       callback.is_serialized_message_callback()),
     any_callback_(callback),
     options_(options),
     message_memory_strategy_(message_memory_strategy)
   {
-    if (options_.event_callbacks.deadline_callback) {
-      this->add_event_handler(
-        options_.event_callbacks.deadline_callback,
-        RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED);
-    }
-    if (options_.event_callbacks.liveliness_callback) {
-      this->add_event_handler(
-        options_.event_callbacks.liveliness_callback,
-        RCL_SUBSCRIPTION_LIVELINESS_CHANGED);
-    }
-    if (options_.event_callbacks.incompatible_qos_callback) {
-      this->add_event_handler(
-        options_.event_callbacks.incompatible_qos_callback,
-        RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
-    } else if (options_.use_default_callbacks) {
-      // Register default callback when not specified
-      try {
-        this->add_event_handler(
-          [this](QOSRequestedIncompatibleQoSInfo & info) {
-            this->default_incompatible_qos_callback(info);
-          },
-          RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
-      } catch (UnsupportedEventTypeException & /*exc*/) {
-        // pass
-      }
-    }
-    if (options_.event_callbacks.message_lost_callback) {
-      this->add_event_handler(
-        options_.event_callbacks.message_lost_callback,
-        RCL_SUBSCRIPTION_MESSAGE_LOST);
-    }
-
     // Setup intra process publishing if requested.
     if (rclcpp::detail::resolve_use_intra_process(options_, *node_base)) {
       using rclcpp::detail::resolve_intra_process_buffer_type;

rclcpp/include/rclcpp/subscription_base.hpp

@@ -84,12 +84,20 @@ public:
     const rosidl_message_type_support_t & type_support_handle,
     const std::string & topic_name,
     const rcl_subscription_options_t & subscription_options,
+    const SubscriptionEventCallbacks & event_callbacks,
+    bool use_default_callbacks,
     bool is_serialized = false);
 
   /// Destructor.
   RCLCPP_PUBLIC
   virtual ~SubscriptionBase();
 
+  /// Add event handlers for passed in event_callbacks.
+  RCLCPP_PUBLIC
+  void
+  bind_event_callbacks(
+    const SubscriptionEventCallbacks & event_callbacks, bool use_default_callbacks);
+
   /// Get the topic that this subscription is subscribed on.
   RCLCPP_PUBLIC
   const char *
@@ -348,7 +356,7 @@ public:
     // This two-step setting, prevents a gap where the old std::function has
     // been replaced but the middleware hasn't been told about the new one yet.
     set_on_new_message_callback(
-      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      rclcpp::detail::cpp_callback_trampoline<decltype(new_callback), const void *, size_t>,
       static_cast<const void *>(&new_callback));
 
     // Store the std::function to keep it in scope, also overwrites the existing one.
@@ -356,7 +364,8 @@ public:
 
     // Set it again, now using the permanent storage.
     set_on_new_message_callback(
-      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      rclcpp::detail::cpp_callback_trampoline<
+        decltype(on_new_message_callback_), const void *, size_t>,
       static_cast<const void *>(&on_new_message_callback_));
   }
 
@@ -569,6 +578,8 @@ protected:
   uint64_t intra_process_subscription_id_;
   std::shared_ptr<rclcpp::experimental::SubscriptionIntraProcessBase> subscription_intra_process_;
 
+  const SubscriptionEventCallbacks event_callbacks_;
+
 private:
   RCLCPP_DISABLE_COPY(SubscriptionBase)
 

rclcpp/include/rclcpp/subscription_options.hpp

@@ -110,7 +110,6 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
    * \param qos QoS profile for subcription.
    * \return rcl_subscription_options_t structure based on the rclcpp::QoS
    */
-  template<typename MessageT>
   rcl_subscription_options_t
   to_rcl_subscription_options(const rclcpp::QoS & qos) const
   {

rclcpp/package.xml

@@ -2,13 +2,17 @@
 <?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>17.0.0</version>
+  <version>18.0.0</version>
   <description>The ROS client library in C++.</description>
+
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
-  <maintainer email="jacob@openrobotics.org">Jacob Perron</maintainer>
+  <maintainer email="michel@ekumenlabs.com">Michel Hidalgo</maintainer>
   <maintainer email="william@openrobotics.org">William Woodall</maintainer>
+
   <license>Apache License 2.0</license>
+
   <author email="dthomas@openrobotics.org">Dirk Thomas</author>
+  <author email="jacob@openrobotics.org">Jacob Perron</author>
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
   <buildtool_depend>ament_cmake_gen_version_h</buildtool_depend>

rclcpp/resource/logging.hpp.em

@@ -124,7 +124,7 @@ def get_rclcpp_suffix_from_features(features):
 ) \
   do { \
     static_assert( \
-      ::std::is_same<typename std::remove_cv<typename std::remove_reference<decltype(logger)>::type>::type, \
+      ::std::is_same<typename std::remove_cv_t<typename std::remove_reference_t<decltype(logger)>>, \
       typename ::rclcpp::Logger>::value, \
       "First argument to logging macros must be an rclcpp::Logger"); \
 @[ if 'throttle' in feature_combination]@ \

rclcpp/src/rclcpp/callback_group.cpp

@@ -12,9 +12,19 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "rclcpp/callback_group.hpp"
+#include <algorithm>
+#include <atomic>
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <stdexcept>
 
-#include <vector>
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/client.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/subscription_base.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/waitable.hpp"
 
 using rclcpp::CallbackGroup;
 using rclcpp::CallbackGroupType;
@@ -27,6 +37,10 @@ CallbackGroup::CallbackGroup(
   automatically_add_to_executor_with_node_(automatically_add_to_executor_with_node)
 {}
 
+CallbackGroup::~CallbackGroup()
+{
+  trigger_notify_guard_condition();
+}
 
 std::atomic_bool &
 CallbackGroup::can_be_taken_from()
@@ -97,6 +111,33 @@ CallbackGroup::automatically_add_to_executor_with_node() const
   return automatically_add_to_executor_with_node_;
 }
 
+rclcpp::GuardCondition::SharedPtr
+CallbackGroup::get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr)
+{
+  std::lock_guard<std::recursive_mutex> lock(notify_guard_condition_mutex_);
+  if (notify_guard_condition_ && context_ptr != notify_guard_condition_->get_context()) {
+    if (associated_with_executor_) {
+      trigger_notify_guard_condition();
+    }
+    notify_guard_condition_ = nullptr;
+  }
+
+  if (!notify_guard_condition_) {
+    notify_guard_condition_ = std::make_shared<rclcpp::GuardCondition>(context_ptr);
+  }
+
+  return notify_guard_condition_;
+}
+
+void
+CallbackGroup::trigger_notify_guard_condition()
+{
+  std::lock_guard<std::recursive_mutex> lock(notify_guard_condition_mutex_);
+  if (notify_guard_condition_) {
+    notify_guard_condition_->trigger();
+  }
+}
+
 void
 CallbackGroup::add_subscription(
   const rclcpp::SubscriptionBase::SharedPtr subscription_ptr)

rclcpp/src/rclcpp/client.cpp

@@ -65,13 +65,6 @@ ClientBase::ClientBase(
     });
 }
 
-ClientBase::~ClientBase()
-{
-  clear_on_new_response_callback();
-  // Make sure the client handle is destructed as early as possible and before the node handle
-  client_handle_.reset();
-}
-
 bool
 ClientBase::take_type_erased_response(void * response_out, rmw_request_id_t & request_header_out)
 {

rclcpp/src/rclcpp/clock.cpp

@@ -67,7 +67,7 @@ Clock::Clock(rcl_clock_type_t clock_type)
 Clock::~Clock() {}
 
 Time
-Clock::now()
+Clock::now() const
 {
   Time now(0, 0, impl_->rcl_clock_.type);
 
@@ -182,6 +182,71 @@ Clock::sleep_for(Duration rel_time, Context::SharedPtr context)
   return sleep_until(now() + rel_time, context);
 }
 
+bool
+Clock::started()
+{
+  if (!rcl_clock_valid(get_clock_handle())) {
+    throw std::runtime_error("clock is not rcl_clock_valid");
+  }
+  return rcl_clock_time_started(get_clock_handle());
+}
+
+bool
+Clock::wait_until_started(Context::SharedPtr context)
+{
+  if (!context || !context->is_valid()) {
+    throw std::runtime_error("context cannot be slept with because it's invalid");
+  }
+  if (!rcl_clock_valid(get_clock_handle())) {
+    throw std::runtime_error("clock cannot be waited on as it is not rcl_clock_valid");
+  }
+
+  if (started()) {
+    return true;
+  } else {
+    // Wait until the first non-zero time
+    return sleep_until(rclcpp::Time(0, 1, get_clock_type()), context);
+  }
+}
+
+bool
+Clock::wait_until_started(
+  const Duration & timeout,
+  Context::SharedPtr context,
+  const Duration & wait_tick_ns)
+{
+  if (!context || !context->is_valid()) {
+    throw std::runtime_error("context cannot be slept with because it's invalid");
+  }
+  if (!rcl_clock_valid(get_clock_handle())) {
+    throw std::runtime_error("clock cannot be waited on as it is not rcl_clock_valid");
+  }
+
+  Clock timeout_clock = Clock(RCL_STEADY_TIME);
+  Time start = timeout_clock.now();
+
+  // Check if the clock has started every wait_tick_ns nanoseconds
+  // Context check checks for rclcpp::shutdown()
+  while (!started() && context->is_valid()) {
+    if (timeout < wait_tick_ns) {
+      timeout_clock.sleep_for(timeout);
+    } else {
+      Duration time_left = start + timeout - timeout_clock.now();
+      if (time_left > wait_tick_ns) {
+        timeout_clock.sleep_for(Duration(wait_tick_ns));
+      } else {
+        timeout_clock.sleep_for(time_left);
+      }
+    }
+
+    if (timeout_clock.now() - start > timeout) {
+      return started();
+    }
+  }
+  return started();
+}
+
+
 bool
 Clock::ros_time_is_active()
 {

rclcpp/src/rclcpp/executor.cpp

@@ -106,11 +106,11 @@ Executor::~Executor()
   weak_groups_associated_with_executor_to_nodes_.clear();
   weak_groups_to_nodes_associated_with_executor_.clear();
   weak_groups_to_nodes_.clear();
-  for (const auto & pair : weak_nodes_to_guard_conditions_) {
+  for (const auto & pair : weak_groups_to_guard_conditions_) {
     auto guard_condition = pair.second;
     memory_strategy_->remove_guard_condition(guard_condition);
   }
-  weak_nodes_to_guard_conditions_.clear();
+  weak_groups_to_guard_conditions_.clear();
 
   // Finalize the wait set.
   if (rcl_wait_set_fini(&wait_set_) != RCL_RET_OK) {
@@ -204,8 +204,7 @@ Executor::add_callback_group_to_map(
   if (has_executor.exchange(true)) {
     throw std::runtime_error("Callback group has already been added to an executor.");
   }
-  bool is_new_node = !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) &&
-    !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_);
+
   rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr;
   auto insert_info =
     weak_groups_to_nodes.insert(std::make_pair(weak_group_ptr, node_ptr));
@@ -215,21 +214,24 @@ Executor::add_callback_group_to_map(
   }
   // Also add to the map that contains all callback groups
   weak_groups_to_nodes_.insert(std::make_pair(weak_group_ptr, node_ptr));
-  if (is_new_node) {
-    const auto & gc = node_ptr->get_notify_guard_condition();
-    weak_nodes_to_guard_conditions_[node_ptr] = &gc;
-    if (notify) {
-      // Interrupt waiting to handle new node
-      try {
-        interrupt_guard_condition_.trigger();
-      } catch (const rclcpp::exceptions::RCLError & ex) {
-        throw std::runtime_error(
-                std::string(
-                  "Failed to trigger guard condition on callback group add: ") + ex.what());
-      }
+
+  if (node_ptr->get_context()->is_valid()) {
+    auto callback_group_guard_condition =
+      group_ptr->get_notify_guard_condition(node_ptr->get_context());
+    weak_groups_to_guard_conditions_[weak_group_ptr] = callback_group_guard_condition.get();
+    // Add the callback_group's notify condition to the guard condition handles
+    memory_strategy_->add_guard_condition(*callback_group_guard_condition);
+  }
+
+  if (notify) {
+    // Interrupt waiting to handle new node
+    try {
+      interrupt_guard_condition_.trigger();
+    } catch (const rclcpp::exceptions::RCLError & ex) {
+      throw std::runtime_error(
+              std::string(
+                "Failed to trigger guard condition on callback group add: ") + ex.what());
     }
-    // Add the node's notify condition to the guard condition handles
-    memory_strategy_->add_guard_condition(gc);
   }
 }
 
@@ -300,7 +302,12 @@ Executor::remove_callback_group_from_map(
   if (!has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) &&
     !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_))
   {
-    weak_nodes_to_guard_conditions_.erase(node_ptr);
+    auto iter = weak_groups_to_guard_conditions_.find(weak_group_ptr);
+    if (iter != weak_groups_to_guard_conditions_.end()) {
+      memory_strategy_->remove_guard_condition(iter->second);
+    }
+    weak_groups_to_guard_conditions_.erase(weak_group_ptr);
+
     if (notify) {
       try {
         interrupt_guard_condition_.trigger();
@@ -310,7 +317,6 @@ Executor::remove_callback_group_from_map(
                   "Failed to trigger guard condition on callback group remove: ") + ex.what());
       }
     }
-    memory_strategy_->remove_guard_condition(&node_ptr->get_notify_guard_condition());
   }
 }
 
@@ -700,12 +706,6 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
         auto weak_node_ptr = pair.second;
         if (weak_group_ptr.expired() || weak_node_ptr.expired()) {
           invalid_group_ptrs.push_back(weak_group_ptr);
-          auto node_guard_pair = weak_nodes_to_guard_conditions_.find(weak_node_ptr);
-          if (node_guard_pair != weak_nodes_to_guard_conditions_.end()) {
-            auto guard_condition = node_guard_pair->second;
-            weak_nodes_to_guard_conditions_.erase(weak_node_ptr);
-            memory_strategy_->remove_guard_condition(guard_condition);
-          }
         }
       }
       std::for_each(
@@ -721,6 +721,12 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
           {
             weak_groups_associated_with_executor_to_nodes_.erase(group_ptr);
           }
+          auto callback_guard_pair = weak_groups_to_guard_conditions_.find(group_ptr);
+          if (callback_guard_pair != weak_groups_to_guard_conditions_.end()) {
+            auto guard_condition = callback_guard_pair->second;
+            weak_groups_to_guard_conditions_.erase(group_ptr);
+            memory_strategy_->remove_guard_condition(guard_condition);
+          }
           weak_groups_to_nodes_.erase(group_ptr);
         });
     }

rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp

@@ -21,6 +21,7 @@
 
 #include "rcpputils/scope_exit.hpp"
 
+#include "rclcpp/logging.hpp"
 #include "rclcpp/utilities.hpp"
 
 using rclcpp::executors::MultiThreadedExecutor;
@@ -34,9 +35,15 @@ MultiThreadedExecutor::MultiThreadedExecutor(
   yield_before_execute_(yield_before_execute),
   next_exec_timeout_(next_exec_timeout)
 {
-  number_of_threads_ = number_of_threads ? number_of_threads : std::thread::hardware_concurrency();
-  if (number_of_threads_ == 0) {
-    number_of_threads_ = 1;
+  number_of_threads_ = number_of_threads > 0 ?
+    number_of_threads :
+    std::max(std::thread::hardware_concurrency(), 2U);
+
+  if (number_of_threads_ == 1) {
+    RCLCPP_WARN(
+      rclcpp::get_logger("rclcpp"),
+      "MultiThreadedExecutor is used with a single thread.\n"
+      "Use the SingleThreadedExecutor instead.");
   }
 }
 

rclcpp/src/rclcpp/node.cpp

@@ -176,7 +176,8 @@ Node::Node(
       node_topics_,
       node_graph_,
       node_services_,
-      node_logging_
+      node_logging_,
+      options.clock_type()
     )),
   node_parameters_(new rclcpp::node_interfaces::NodeParameters(
       node_base_,

rclcpp/src/rclcpp/node_interfaces/node_clock.cpp

@@ -24,13 +24,14 @@ NodeClock::NodeClock(
   rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
   rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
-  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging)
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+  rcl_clock_type_t clock_type)
 : node_base_(node_base),
   node_topics_(node_topics),
   node_graph_(node_graph),
   node_services_(node_services),
   node_logging_(node_logging),
-  ros_clock_(std::make_shared<rclcpp::Clock>(RCL_ROS_TIME))
+  clock_(std::make_shared<rclcpp::Clock>(clock_type))
 {}
 
 NodeClock::~NodeClock()
@@ -39,11 +40,11 @@ NodeClock::~NodeClock()
 rclcpp::Clock::SharedPtr
 NodeClock::get_clock()
 {
-  return ros_clock_;
+  return clock_;
 }
 
 rclcpp::Clock::ConstSharedPtr
 NodeClock::get_clock() const
 {
-  return ros_clock_;
+  return clock_;
 }

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -904,22 +904,12 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
 std::vector<rclcpp::Parameter>
 NodeParameters::get_parameters(const std::vector<std::string> & names) const
 {
-  std::lock_guard<std::recursive_mutex> lock(mutex_);
   std::vector<rclcpp::Parameter> results;
   results.reserve(names.size());
 
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
   for (auto & name : names) {
-    auto found_parameter = parameters_.find(name);
-    if (found_parameter != parameters_.cend()) {
-      // found
-      results.emplace_back(name, found_parameter->second.value);
-    } else if (this->allow_undeclared_) {
-      // not found, but undeclared allowed
-      results.emplace_back(name, rclcpp::ParameterValue());
-    } else {
-      // not found, and undeclared are not allowed
-      throw rclcpp::exceptions::ParameterNotDeclaredException(name);
-    }
+    results.emplace_back(this->get_parameter(name));
   }
   return results;
 }
@@ -930,18 +920,18 @@ NodeParameters::get_parameter(const std::string & name) const
   std::lock_guard<std::recursive_mutex> lock(mutex_);
 
   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 rclcpp::Parameter{};
-  } else if (parameters_.end() == param_iter) {
-    throw rclcpp::exceptions::ParameterNotDeclaredException(name);
-  } else {
+  if (parameters_.end() != param_iter) {
+    if (
+      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};
+    }
     throw rclcpp::exceptions::ParameterUninitializedException(name);
+  } else if (this->allow_undeclared_) {
+    return rclcpp::Parameter{name};
+  } else {
+    throw rclcpp::exceptions::ParameterNotDeclaredException(name);
   }
 }
 

rclcpp/src/rclcpp/node_interfaces/node_services.cpp

@@ -35,15 +35,17 @@ NodeServices::add_service(
       // TODO(jacquelinekay): use custom exception
       throw std::runtime_error("Cannot create service, group not in node.");
     }
-    group->add_service(service_base_ptr);
   } else {
-    node_base_->get_default_callback_group()->add_service(service_base_ptr);
+    group = node_base_->get_default_callback_group();
   }
 
+  group->add_service(service_base_ptr);
+
   // Notify the executor that a new service was created using the parent Node.
   auto & node_gc = node_base_->get_notify_guard_condition();
   try {
     node_gc.trigger();
+    group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string("failed to notify wait set on service creation: ") + ex.what());
@@ -60,15 +62,17 @@ NodeServices::add_client(
       // TODO(jacquelinekay): use custom exception
       throw std::runtime_error("Cannot create client, group not in node.");
     }
-    group->add_client(client_base_ptr);
   } else {
-    node_base_->get_default_callback_group()->add_client(client_base_ptr);
+    group = node_base_->get_default_callback_group();
   }
 
+  group->add_client(client_base_ptr);
+
   // Notify the executor that a new client was created using the parent Node.
   auto & node_gc = node_base_->get_notify_guard_condition();
   try {
     node_gc.trigger();
+    group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string("failed to notify wait set on client creation: ") + ex.what());

rclcpp/src/rclcpp/node_interfaces/node_timers.cpp

@@ -37,14 +37,15 @@ NodeTimers::add_timer(
       // TODO(jacquelinekay): use custom exception
       throw std::runtime_error("Cannot create timer, group not in node.");
     }
-    callback_group->add_timer(timer);
   } else {
-    node_base_->get_default_callback_group()->add_timer(timer);
+    callback_group = node_base_->get_default_callback_group();
   }
+  callback_group->add_timer(timer);
 
   auto & node_gc = node_base_->get_notify_guard_condition();
   try {
     node_gc.trigger();
+    callback_group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string("failed to notify wait set on timer creation: ") + ex.what());

rclcpp/src/rclcpp/node_interfaces/node_topics.cpp

@@ -73,6 +73,7 @@ NodeTopics::add_publisher(
   auto & node_gc = node_base_->get_notify_guard_condition();
   try {
     node_gc.trigger();
+    callback_group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string("failed to notify wait set on publisher creation: ") + ex.what());
@@ -121,6 +122,7 @@ NodeTopics::add_subscription(
   auto & node_gc = node_base_->get_notify_guard_condition();
   try {
     node_gc.trigger();
+    callback_group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string("failed to notify wait set on subscription creation: ") + ex.what());

rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp

@@ -35,15 +35,17 @@ NodeWaitables::add_waitable(
       // TODO(jacobperron): use custom exception
       throw std::runtime_error("Cannot create waitable, group not in node.");
     }
-    group->add_waitable(waitable_ptr);
   } else {
-    node_base_->get_default_callback_group()->add_waitable(waitable_ptr);
+    group = node_base_->get_default_callback_group();
   }
 
+  group->add_waitable(waitable_ptr);
+
   // Notify the executor that a new waitable was created using the parent Node.
   auto & node_gc = node_base_->get_notify_guard_condition();
   try {
     node_gc.trigger();
+    group->trigger_notify_guard_condition();
   } catch (const rclcpp::exceptions::RCLError & ex) {
     throw std::runtime_error(
             std::string("failed to notify wait set on waitable creation: ") + ex.what());

rclcpp/src/rclcpp/node_options.cpp

@@ -77,6 +77,7 @@ NodeOptions::operator=(const NodeOptions & other)
     this->enable_topic_statistics_ = other.enable_topic_statistics_;
     this->start_parameter_services_ = other.start_parameter_services_;
     this->start_parameter_event_publisher_ = other.start_parameter_event_publisher_;
+    this->clock_type_ = other.clock_type_;
     this->clock_qos_ = other.clock_qos_;
     this->use_clock_thread_ = other.use_clock_thread_;
     this->parameter_event_qos_ = other.parameter_event_qos_;
@@ -260,6 +261,19 @@ NodeOptions::start_parameter_event_publisher(bool start_parameter_event_publishe
   return *this;
 }
 
+const rcl_clock_type_t &
+NodeOptions::clock_type() const
+{
+  return this->clock_type_;
+}
+
+NodeOptions &
+NodeOptions::clock_type(const rcl_clock_type_t & clock_type)
+{
+  this->clock_type_ = clock_type;
+  return *this;
+}
+
 const rclcpp::QoS &
 NodeOptions::clock_qos() const
 {

rclcpp/src/rclcpp/parameter_service.cpp

@@ -48,6 +48,8 @@ ParameterService::ParameterService(
         }
       } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) {
         RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to get parameters: %s", ex.what());
+      } catch (const rclcpp::exceptions::ParameterUninitializedException & ex) {
+        RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to get parameters: %s", ex.what());
       }
     },
     qos_profile, nullptr);

rclcpp/src/rclcpp/publisher_base.cpp

@@ -45,11 +45,14 @@ PublisherBase::PublisherBase(
   rclcpp::node_interfaces::NodeBaseInterface * node_base,
   const std::string & topic,
   const rosidl_message_type_support_t & type_support,
-  const rcl_publisher_options_t & publisher_options)
+  const rcl_publisher_options_t & publisher_options,
+  const PublisherEventCallbacks & event_callbacks,
+  bool use_default_callbacks)
 : rcl_node_handle_(node_base->get_shared_rcl_node_handle()),
   intra_process_is_enabled_(false),
   intra_process_publisher_id_(0),
-  type_support_(type_support)
+  type_support_(type_support),
+  event_callbacks_(event_callbacks)
 {
   auto custom_deleter = [node_handle = this->rcl_node_handle_](rcl_publisher_t * rcl_pub)
     {
@@ -98,15 +101,12 @@ PublisherBase::PublisherBase(
     rmw_reset_error();
     throw std::runtime_error(msg);
   }
+
+  bind_event_callbacks(event_callbacks_, use_default_callbacks);
 }
 
 PublisherBase::~PublisherBase()
 {
-  for (const auto & pair : event_handlers_) {
-    rcl_publisher_event_type_t event_type = pair.first;
-    clear_on_new_qos_event_callback(event_type);
-  }
-
   // must fini the events before fini-ing the publisher
   event_handlers_.clear();
 
@@ -131,6 +131,38 @@ PublisherBase::get_topic_name() const
   return rcl_publisher_get_topic_name(publisher_handle_.get());
 }
 
+void
+PublisherBase::bind_event_callbacks(
+  const PublisherEventCallbacks & event_callbacks, bool use_default_callbacks)
+{
+  if (event_callbacks.deadline_callback) {
+    this->add_event_handler(
+      event_callbacks.deadline_callback,
+      RCL_PUBLISHER_OFFERED_DEADLINE_MISSED);
+  }
+  if (event_callbacks.liveliness_callback) {
+    this->add_event_handler(
+      event_callbacks.liveliness_callback,
+      RCL_PUBLISHER_LIVELINESS_LOST);
+  }
+  if (event_callbacks.incompatible_qos_callback) {
+    this->add_event_handler(
+      event_callbacks.incompatible_qos_callback,
+      RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
+  } else if (use_default_callbacks) {
+    // Register default callback when not specified
+    try {
+      this->add_event_handler(
+        [this](QOSOfferedIncompatibleQoSInfo & info) {
+          this->default_incompatible_qos_callback(info);
+        },
+        RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
+    } catch (UnsupportedEventTypeException & /*exc*/) {
+      // pass
+    }
+  }
+}
+
 size_t
 PublisherBase::get_queue_size() const
 {

rclcpp/src/rclcpp/qos.cpp

@@ -16,6 +16,8 @@
 
 #include <string>
 
+#include "rclcpp/logging.hpp"
+
 #include "rmw/error_handling.h"
 #include "rmw/types.h"
 #include "rmw/qos_profiles.h"
@@ -67,13 +69,30 @@ KeepAll::KeepAll()
 
 KeepLast::KeepLast(size_t depth)
 : QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth)
-{}
+{
+  if (depth == 0) {
+    RCLCPP_WARN_ONCE(
+      rclcpp::get_logger(
+        "rclcpp"),
+      "A zero depth with KEEP_LAST doesn't make sense; no data could be stored."
+      "This will be interpreted as SYSTEM_DEFAULT");
+  }
+}
 
 QoS::QoS(
   const QoSInitialization & qos_initialization,
   const rmw_qos_profile_t & initial_profile)
 : rmw_qos_profile_(initial_profile)
 {
+  if (qos_initialization.history_policy == RMW_QOS_POLICY_HISTORY_KEEP_LAST &&
+    qos_initialization.depth == 0)
+  {
+    RCLCPP_WARN_ONCE(
+      rclcpp::get_logger(
+        "rclcpp"),
+      "A zero depth with KEEP_LAST doesn't make sense; no data could be stored."
+      "This will be interpreted as SYSTEM_DEFAULT");
+  }
   rmw_qos_profile_.history = qos_initialization.history_policy;
   rmw_qos_profile_.depth = qos_initialization.depth;
 }
@@ -111,6 +130,14 @@ QoS::history(HistoryPolicy history)
 QoS &
 QoS::keep_last(size_t depth)
 {
+  if (depth == 0) {
+    RCLCPP_WARN_ONCE(
+      rclcpp::get_logger(
+        "rclcpp"),
+      "A zero depth with KEEP_LAST doesn't make sense; no data could be stored."
+      "This will be interpreted as SYSTEM_DEFAULT");
+  }
+
   rmw_qos_profile_.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST;
   rmw_qos_profile_.depth = depth;
   return *this;

rclcpp/src/rclcpp/qos_event.cpp

@@ -35,6 +35,11 @@ UnsupportedEventTypeException::UnsupportedEventTypeException(
 
 QOSEventHandlerBase::~QOSEventHandlerBase()
 {
+  // Since the rmw event listener holds a reference to
+  // this callback, we need to clear it on destruction of this class.
+  // This clearing is not needed for other rclcpp entities like pub/subs, since
+  // they do own the underlying rmw entities, which are destroyed
+  // on their rclcpp destructors, thus no risk of dangling pointers.
   if (on_new_event_callback_) {
     clear_on_ready_callback();
   }

rclcpp/src/rclcpp/service.cpp

@@ -32,10 +32,6 @@ ServiceBase::ServiceBase(std::shared_ptr<rcl_node_t> node_handle)
   node_logger_(rclcpp::get_node_logger(node_handle_.get()))
 {}
 
-ServiceBase::~ServiceBase()
-{
-  clear_on_new_request_callback();
-}
 
 bool
 ServiceBase::take_type_erased_request(void * request_out, rmw_request_id_t & request_id_out)

rclcpp/src/rclcpp/signal_handler.cpp

@@ -191,7 +191,9 @@ SignalHandler::uninstall()
     signal_handlers_options_ = SignalHandlerOptions::None;
     RCLCPP_DEBUG(get_logger(), "SignalHandler::uninstall(): notifying deferred signal handler");
     notify_signal_handler();
-    signal_handler_thread_.join();
+    if (signal_handler_thread_.joinable()) {
+      signal_handler_thread_.join();
+    }
     teardown_wait_for_signal();
   } catch (...) {
     installed_.exchange(true);

rclcpp/src/rclcpp/subscription_base.cpp

@@ -39,12 +39,15 @@ SubscriptionBase::SubscriptionBase(
   const rosidl_message_type_support_t & type_support_handle,
   const std::string & topic_name,
   const rcl_subscription_options_t & subscription_options,
+  const SubscriptionEventCallbacks & event_callbacks,
+  bool use_default_callbacks,
   bool is_serialized)
 : node_base_(node_base),
   node_handle_(node_base_->get_shared_rcl_node_handle()),
   node_logger_(rclcpp::get_node_logger(node_handle_.get())),
   use_intra_process_(false),
   intra_process_subscription_id_(0),
+  event_callbacks_(event_callbacks),
   type_support_(type_support_handle),
   is_serialized_(is_serialized)
 {
@@ -80,20 +83,14 @@ SubscriptionBase::SubscriptionBase(
         rcl_node_get_name(rcl_node_handle),
         rcl_node_get_namespace(rcl_node_handle));
     }
-
     rclcpp::exceptions::throw_from_rcl_error(ret, "could not create subscription");
   }
+
+  bind_event_callbacks(event_callbacks_, use_default_callbacks);
 }
 
 SubscriptionBase::~SubscriptionBase()
 {
-  clear_on_new_message_callback();
-
-  for (const auto & pair : event_handlers_) {
-    rcl_subscription_event_type_t event_type = pair.first;
-    clear_on_new_qos_event_callback(event_type);
-  }
-
   if (!use_intra_process_) {
     return;
   }
@@ -108,6 +105,43 @@ SubscriptionBase::~SubscriptionBase()
   ipm->remove_subscription(intra_process_subscription_id_);
 }
 
+void
+SubscriptionBase::bind_event_callbacks(
+  const SubscriptionEventCallbacks & event_callbacks, bool use_default_callbacks)
+{
+  if (event_callbacks.deadline_callback) {
+    this->add_event_handler(
+      event_callbacks.deadline_callback,
+      RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED);
+  }
+  if (event_callbacks.liveliness_callback) {
+    this->add_event_handler(
+      event_callbacks.liveliness_callback,
+      RCL_SUBSCRIPTION_LIVELINESS_CHANGED);
+  }
+  if (event_callbacks.incompatible_qos_callback) {
+    this->add_event_handler(
+      event_callbacks.incompatible_qos_callback,
+      RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
+  } else if (use_default_callbacks) {
+    // Register default callback when not specified
+    try {
+      this->add_event_handler(
+        [this](QOSRequestedIncompatibleQoSInfo & info) {
+          this->default_incompatible_qos_callback(info);
+        },
+        RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS);
+    } catch (UnsupportedEventTypeException & /*exc*/) {
+      // pass
+    }
+  }
+  if (event_callbacks.message_lost_callback) {
+    this->add_event_handler(
+      event_callbacks.message_lost_callback,
+      RCL_SUBSCRIPTION_MESSAGE_LOST);
+  }
+}
+
 const char *
 SubscriptionBase::get_topic_name() const
 {

rclcpp/src/rclcpp/subscription_intra_process_base.cpp

@@ -17,11 +17,6 @@
 
 using rclcpp::experimental::SubscriptionIntraProcessBase;
 
-SubscriptionIntraProcessBase::~SubscriptionIntraProcessBase()
-{
-  clear_on_ready_callback();
-}
-
 void
 SubscriptionIntraProcessBase::add_to_wait_set(rcl_wait_set_t * wait_set)
 {

rclcpp/src/rclcpp/time_source.cpp

@@ -91,10 +91,13 @@ public:
   // Attach a clock
   void attachClock(rclcpp::Clock::SharedPtr clock)
   {
-    if (clock->get_clock_type() != RCL_ROS_TIME) {
-      throw std::invalid_argument("Cannot attach clock to a time source that's not a ROS clock");
+    {
+      std::lock_guard<std::mutex> clock_guard(clock->get_clock_mutex());
+      if (clock->get_clock_type() != RCL_ROS_TIME && ros_time_active_) {
+        throw std::invalid_argument(
+                "ros_time_active_ can't be true while clock is not of RCL_ROS_TIME type");
+      }
     }
-
     std::lock_guard<std::mutex> guard(clock_list_lock_);
     associated_clocks_.push_back(clock);
     // Set the clock to zero unless there's a recently received message
@@ -125,27 +128,32 @@ public:
   {
     std::lock_guard<std::mutex> clock_guard(clock->get_clock_mutex());
 
-    // Do change
-    if (!set_ros_time_enabled && clock->ros_time_is_active()) {
-      auto ret = rcl_disable_ros_time_override(clock->get_clock_handle());
-      if (ret != RCL_RET_OK) {
-        rclcpp::exceptions::throw_from_rcl_error(
-          ret, "Failed to disable ros_time_override_status");
+    if (clock->get_clock_type() == RCL_ROS_TIME) {
+      // Do change
+      if (!set_ros_time_enabled && clock->ros_time_is_active()) {
+        auto ret = rcl_disable_ros_time_override(clock->get_clock_handle());
+        if (ret != RCL_RET_OK) {
+          rclcpp::exceptions::throw_from_rcl_error(
+            ret, "Failed to disable ros_time_override_status");
+        }
+      } else if (set_ros_time_enabled && !clock->ros_time_is_active()) {
+        auto ret = rcl_enable_ros_time_override(clock->get_clock_handle());
+        if (ret != RCL_RET_OK) {
+          rclcpp::exceptions::throw_from_rcl_error(
+            ret, "Failed to enable ros_time_override_status");
+        }
       }
-    } else if (set_ros_time_enabled && !clock->ros_time_is_active()) {
-      auto ret = rcl_enable_ros_time_override(clock->get_clock_handle());
-      if (ret != RCL_RET_OK) {
-        rclcpp::exceptions::throw_from_rcl_error(
-          ret, "Failed to enable ros_time_override_status");
-      }
-    }
 
-    auto ret = rcl_set_ros_time_override(
-      clock->get_clock_handle(),
-      rclcpp::Time(*msg).nanoseconds());
-    if (ret != RCL_RET_OK) {
-      rclcpp::exceptions::throw_from_rcl_error(
-        ret, "Failed to set ros_time_override_status");
+      auto ret = rcl_set_ros_time_override(
+        clock->get_clock_handle(),
+        rclcpp::Time(*msg).nanoseconds());
+      if (ret != RCL_RET_OK) {
+        rclcpp::exceptions::throw_from_rcl_error(
+          ret, "Failed to set ros_time_override_status");
+      }
+    } else if (set_ros_time_enabled) {
+      throw std::invalid_argument(
+              "set_ros_time_enabled can't be true while clock is not of RCL_ROS_TIME type");
     }
   }
 
@@ -166,6 +174,18 @@ public:
     last_msg_set_ = msg;
   }
 
+  bool are_all_clocks_rcl_ros_time()
+  {
+    std::lock_guard<std::mutex> guard(clock_list_lock_);
+    for (auto & clock : associated_clocks_) {
+      std::lock_guard<std::mutex> clock_guard(clock->get_clock_mutex());
+      if (clock->get_clock_type() != RCL_ROS_TIME) {
+        return false;
+      }
+    }
+    return true;
+  }
+
 private:
   // Store (and update on node attach) logger for logging.
   Logger logger_;
@@ -266,6 +286,10 @@ public:
       throw std::invalid_argument("Invalid type for parameter 'use_sim_time', should be 'bool'");
     }
 
+    on_set_parameters_callback_ = node_parameters_->add_on_set_parameters_callback(
+      std::bind(&TimeSource::NodeState::on_set_parameters, this, std::placeholders::_1));
+
+
     // TODO(tfoote) use parameters interface not subscribe to events via topic ticketed #609
     parameter_subscription_ = rclcpp::AsyncParametersClient::on_parameter_event(
       node_topics_,
@@ -285,6 +309,10 @@ public:
     // can't possibly call any of the callbacks as we are cleaning up.
     destroy_clock_sub();
     clocks_state_.disable_ros_time();
+    if (on_set_parameters_callback_) {
+      node_parameters_->remove_on_set_parameters_callback(on_set_parameters_callback_.get());
+    }
+    on_set_parameters_callback_.reset();
     parameter_subscription_.reset();
     node_base_.reset();
     node_topics_.reset();
@@ -419,10 +447,34 @@ private:
     clock_subscription_.reset();
   }
 
+  // On set Parameters callback handle
+  node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_{nullptr};
+
   // Parameter Event subscription
   using ParamSubscriptionT = rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>;
   std::shared_ptr<ParamSubscriptionT> parameter_subscription_;
 
+  // Callback for parameter settings
+  rcl_interfaces::msg::SetParametersResult on_set_parameters(
+    const std::vector<rclcpp::Parameter> & parameters)
+  {
+    rcl_interfaces::msg::SetParametersResult result;
+    result.successful = true;
+    for (const auto & param : parameters) {
+      if (param.get_name() == "use_sim_time" && param.get_type() == rclcpp::PARAMETER_BOOL) {
+        if (param.as_bool() && !(clocks_state_.are_all_clocks_rcl_ros_time())) {
+          result.successful = false;
+          result.reason =
+            "use_sim_time parameter can't be true while clocks are not all of RCL_ROS_TIME type";
+          RCLCPP_ERROR(
+            logger_,
+            "use_sim_time parameter can't be true while clocks are not all of RCL_ROS_TIME type");
+        }
+      }
+    }
+    return result;
+  }
+
   // Callback for parameter updates
   void on_parameter_event(std::shared_ptr<const rcl_interfaces::msg::ParameterEvent> event)
   {

rclcpp/test/rclcpp/CMakeLists.txt

@@ -100,15 +100,20 @@ if(TARGET test_create_subscription)
     "test_msgs"
   )
 endif()
-ament_add_gtest(test_add_callback_groups_to_executor
-  test_add_callback_groups_to_executor.cpp
-  TIMEOUT 120)
-if(TARGET test_add_callback_groups_to_executor)
-  target_link_libraries(test_add_callback_groups_to_executor ${PROJECT_NAME})
-  ament_target_dependencies(test_add_callback_groups_to_executor
-    "test_msgs"
+function(test_add_callback_groups_to_executor_for_rmw_implementation)
+  set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation})
+  ament_add_gmock(test_add_callback_groups_to_executor${target_suffix} test_add_callback_groups_to_executor.cpp
+    ENV ${rmw_implementation_env_var}
+    TIMEOUT 120
   )
-endif()
+  if(TARGET test_add_callback_groups_to_executor${target_suffix})
+    target_link_libraries(test_add_callback_groups_to_executor${target_suffix} ${PROJECT_NAME})
+    ament_target_dependencies(test_add_callback_groups_to_executor${target_suffix}
+      "test_msgs"
+    )
+  endif()
+endfunction()
+call_for_each_rmw_implementation(test_add_callback_groups_to_executor_for_rmw_implementation)
 ament_add_gtest(test_expand_topic_or_service_name test_expand_topic_or_service_name.cpp)
 if(TARGET test_expand_topic_or_service_name)
   ament_target_dependencies(test_expand_topic_or_service_name
@@ -229,6 +234,11 @@ if(TARGET test_node_interfaces__node_graph)
     "test_msgs")
   target_link_libraries(test_node_interfaces__node_graph ${PROJECT_NAME} mimick)
 endif()
+ament_add_gtest(test_node_interfaces__node_interfaces
+  node_interfaces/test_node_interfaces.cpp)
+if(TARGET test_node_interfaces__node_interfaces)
+  target_link_libraries(test_node_interfaces__node_interfaces ${PROJECT_NAME} mimick)
+endif()
 ament_add_gtest(test_node_interfaces__node_parameters
   node_interfaces/test_node_parameters.cpp)
 if(TARGET test_node_interfaces__node_parameters)
@@ -257,6 +267,11 @@ ament_add_gtest(test_node_interfaces__node_waitables
 if(TARGET test_node_interfaces__node_waitables)
   target_link_libraries(test_node_interfaces__node_waitables ${PROJECT_NAME} mimick)
 endif()
+ament_add_gtest(test_node_interfaces__test_template_utils  # Compile time test
+  node_interfaces/detail/test_template_utils.cpp)
+if(TARGET test_node_interfaces__test_template_utils)
+  target_link_libraries(test_node_interfaces__test_template_utils ${PROJECT_NAME})
+endif()
 
 # TODO(wjwwood): reenable these build failure tests when I can get Jenkins to ignore their output
 # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_rclcpp_node

rclcpp/test/rclcpp/allocator/test_allocator_common.cpp

@@ -51,6 +51,53 @@ TEST(TestAllocatorCommon, retyped_allocate) {
   EXPECT_NO_THROW(code2());
 }
 
+TEST(TestAllocatorCommon, retyped_zero_allocate_basic) {
+  std::allocator<int> allocator;
+  void * untyped_allocator = &allocator;
+  void * allocated_mem =
+    rclcpp::allocator::retyped_zero_allocate<std::allocator<char>>(20u, 1u, untyped_allocator);
+  ASSERT_TRUE(nullptr != allocated_mem);
+
+  auto code = [&untyped_allocator, allocated_mem]() {
+      rclcpp::allocator::retyped_deallocate<char, std::allocator<char>>(
+        allocated_mem, untyped_allocator);
+    };
+  EXPECT_NO_THROW(code());
+}
+
+TEST(TestAllocatorCommon, retyped_zero_allocate) {
+  std::allocator<int> allocator;
+  void * untyped_allocator = &allocator;
+  void * allocated_mem =
+    rclcpp::allocator::retyped_zero_allocate<std::allocator<char>>(20u, 1u, untyped_allocator);
+  // The more natural check here is ASSERT_NE(nullptr, ptr), but clang static
+  // analysis throws a false-positive memory leak warning.  Use ASSERT_TRUE instead.
+  ASSERT_TRUE(nullptr != allocated_mem);
+
+  auto code = [&untyped_allocator, allocated_mem]() {
+      rclcpp::allocator::retyped_deallocate<int, std::allocator<int>>(
+        allocated_mem, untyped_allocator);
+    };
+  EXPECT_NO_THROW(code());
+
+  allocated_mem = allocator.allocate(1);
+  // The more natural check here is ASSERT_NE(nullptr, ptr), but clang static
+  // analysis throws a false-positive memory leak warning.  Use ASSERT_TRUE instead.
+  ASSERT_TRUE(nullptr != allocated_mem);
+  void * reallocated_mem =
+    rclcpp::allocator::retyped_reallocate<int, std::allocator<int>>(
+    allocated_mem, 2u, untyped_allocator);
+  // The more natural check here is ASSERT_NE(nullptr, ptr), but clang static
+  // analysis throws a false-positive memory leak warning.  Use ASSERT_TRUE instead.
+  ASSERT_TRUE(nullptr != reallocated_mem);
+
+  auto code2 = [&untyped_allocator, reallocated_mem]() {
+      rclcpp::allocator::retyped_deallocate<int, std::allocator<int>>(
+        reallocated_mem, untyped_allocator);
+    };
+  EXPECT_NO_THROW(code2());
+}
+
 TEST(TestAllocatorCommon, get_rcl_allocator) {
   std::allocator<int> allocator;
   auto rcl_allocator = rclcpp::allocator::get_rcl_allocator<int>(allocator);

rclcpp/test/rclcpp/node_interfaces/detail/test_template_utils.cpp

@@ -0,0 +1,56 @@
+// Copyright 2022 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "rclcpp/detail/template_contains.hpp"
+#include "rclcpp/detail/template_unique.hpp"
+
+
+TEST(NoOpTests, test_node_interfaces_template_utils) {
+}                                                        // This is just to let gtest work
+
+namespace rclcpp
+{
+namespace detail
+{
+
+// This tests template logic at compile time
+namespace
+{
+
+struct test_template_unique
+{
+  static_assert(template_unique_v<int>, "failed");
+  static_assert(template_unique_v<int, double>, "failed");
+  static_assert(!template_unique_v<int, int>, "failed");
+  static_assert(!template_unique_v<int, double, int>, "failed");
+  static_assert(!template_unique_v<int, int, double>, "failed");
+};
+
+
+struct test_template_contains
+{
+  static_assert(template_contains_v<int, int, double>, "failed");
+  static_assert(template_contains_v<double, int, double>, "failed");
+  static_assert(template_contains_v<int, int>, "failed");
+  static_assert(!template_contains_v<int>, "failed");
+  static_assert(!template_contains_v<int, float>, "failed");
+  static_assert(!template_contains_v<int, float, double>, "failed");
+};
+
+}   // namespace
+
+}  // namespace detail
+}  // namespace rclcpp

rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp

@@ -0,0 +1,245 @@
+// Copyright 2022 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "rclcpp/node.hpp"
+#include "rclcpp/node_interfaces/node_interfaces.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_graph_interface.hpp"
+
+class TestNodeInterfaces : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+};
+
+/*
+   Testing NodeInterfaces construction from nodes.
+ */
+TEST_F(TestNodeInterfaces, node_interfaces_nominal) {
+  auto node = std::make_shared<rclcpp::Node>("my_node");
+
+  // Create a NodeInterfaces for base and graph using a rclcpp::Node.
+  {
+    using rclcpp::node_interfaces::NodeInterfaces;
+    using rclcpp::node_interfaces::NodeBaseInterface;
+    using rclcpp::node_interfaces::NodeGraphInterface;
+    auto node_interfaces = NodeInterfaces<NodeBaseInterface, NodeGraphInterface>(node);
+  }
+
+  // Implicit conversion of rclcpp::Node into function that uses NodeInterfaces of base.
+  {
+    using rclcpp::node_interfaces::NodeInterfaces;
+    using rclcpp::node_interfaces::NodeBaseInterface;
+    auto some_func = [](NodeInterfaces<NodeBaseInterface> ni) {
+        auto base_interface = ni.get<NodeBaseInterface>();
+      };
+
+    some_func(node);
+  }
+
+  // Implicit narrowing of NodeInterfaces into a new interface NodeInterfaces with fewer interfaces.
+  {
+    using rclcpp::node_interfaces::NodeInterfaces;
+    using rclcpp::node_interfaces::NodeBaseInterface;
+    using rclcpp::node_interfaces::NodeGraphInterface;
+    auto some_func = [](NodeInterfaces<NodeBaseInterface> ni_with_one) {
+        auto base_interface = ni_with_one.get<NodeBaseInterface>();
+      };
+
+    NodeInterfaces<NodeBaseInterface, NodeGraphInterface> ni_with_two(node);
+
+    some_func(ni_with_two);
+  }
+
+  // Create a NodeInterfaces via aggregation of interfaces in constructor.
+  {
+    using rclcpp::node_interfaces::NodeInterfaces;
+    using rclcpp::node_interfaces::NodeBaseInterface;
+    using rclcpp::node_interfaces::NodeGraphInterface;
+    auto loose_node_base = node->get_node_base_interface();
+    auto loose_node_graph = node->get_node_graph_interface();
+    auto ni = NodeInterfaces<NodeBaseInterface, NodeGraphInterface>(
+      loose_node_base,
+      loose_node_graph);
+  }
+}
+
+/*
+   Test construction with all standard rclcpp::node_interfaces::Node*Interfaces.
+ */
+TEST_F(TestNodeInterfaces, node_interfaces_standard_interfaces) {
+  auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+
+  auto ni = rclcpp::node_interfaces::NodeInterfaces<
+    rclcpp::node_interfaces::NodeBaseInterface,
+    rclcpp::node_interfaces::NodeClockInterface,
+    rclcpp::node_interfaces::NodeGraphInterface,
+    rclcpp::node_interfaces::NodeLoggingInterface,
+    rclcpp::node_interfaces::NodeTimersInterface,
+    rclcpp::node_interfaces::NodeTopicsInterface,
+    rclcpp::node_interfaces::NodeServicesInterface,
+    rclcpp::node_interfaces::NodeWaitablesInterface,
+    rclcpp::node_interfaces::NodeParametersInterface,
+    rclcpp::node_interfaces::NodeTimeSourceInterface
+    >(node);
+}
+
+/*
+   Testing getters.
+ */
+TEST_F(TestNodeInterfaces, ni_init) {
+  auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+
+  using rclcpp::node_interfaces::NodeInterfaces;
+  using rclcpp::node_interfaces::NodeBaseInterface;
+  using rclcpp::node_interfaces::NodeClockInterface;
+  using rclcpp::node_interfaces::NodeGraphInterface;
+  using rclcpp::node_interfaces::NodeLoggingInterface;
+  using rclcpp::node_interfaces::NodeTimersInterface;
+  using rclcpp::node_interfaces::NodeTopicsInterface;
+  using rclcpp::node_interfaces::NodeServicesInterface;
+  using rclcpp::node_interfaces::NodeWaitablesInterface;
+  using rclcpp::node_interfaces::NodeParametersInterface;
+  using rclcpp::node_interfaces::NodeTimeSourceInterface;
+
+  auto ni = NodeInterfaces<
+    NodeBaseInterface,
+    NodeClockInterface,
+    NodeGraphInterface,
+    NodeLoggingInterface,
+    NodeTimersInterface,
+    NodeTopicsInterface,
+    NodeServicesInterface,
+    NodeWaitablesInterface,
+    NodeParametersInterface,
+    NodeTimeSourceInterface
+    >(node);
+
+  {
+    auto base = ni.get<NodeBaseInterface>();
+    base = ni.get_node_base_interface();
+    EXPECT_STREQ(base->get_name(), "my_node");  // Test for functionality
+  }
+  {
+    auto clock = ni.get<NodeClockInterface>();
+    clock = ni.get_node_clock_interface();
+    clock->get_clock();
+  }
+  {
+    auto graph = ni.get<NodeGraphInterface>();
+    graph = ni.get_node_graph_interface();
+  }
+  {
+    auto logging = ni.get<NodeLoggingInterface>();
+    logging = ni.get_node_logging_interface();
+  }
+  {
+    auto timers = ni.get<NodeTimersInterface>();
+    timers = ni.get_node_timers_interface();
+  }
+  {
+    auto topics = ni.get<NodeTopicsInterface>();
+    topics = ni.get_node_topics_interface();
+  }
+  {
+    auto services = ni.get<NodeServicesInterface>();
+    services = ni.get_node_services_interface();
+  }
+  {
+    auto waitables = ni.get<NodeWaitablesInterface>();
+    waitables = ni.get_node_waitables_interface();
+  }
+  {
+    auto parameters = ni.get<NodeParametersInterface>();
+    parameters = ni.get_node_parameters_interface();
+  }
+  {
+    auto time_source = ni.get<NodeTimeSourceInterface>();
+    time_source = ni.get_node_time_source_interface();
+  }
+}
+
+/*
+   Testing macro'ed getters.
+ */
+TEST_F(TestNodeInterfaces, ni_all_init) {
+  auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+
+  using rclcpp::node_interfaces::NodeInterfaces;
+  using rclcpp::node_interfaces::NodeBaseInterface;
+  using rclcpp::node_interfaces::NodeClockInterface;
+  using rclcpp::node_interfaces::NodeGraphInterface;
+  using rclcpp::node_interfaces::NodeLoggingInterface;
+  using rclcpp::node_interfaces::NodeTimersInterface;
+  using rclcpp::node_interfaces::NodeTopicsInterface;
+  using rclcpp::node_interfaces::NodeServicesInterface;
+  using rclcpp::node_interfaces::NodeWaitablesInterface;
+  using rclcpp::node_interfaces::NodeParametersInterface;
+  using rclcpp::node_interfaces::NodeTimeSourceInterface;
+
+  auto ni = rclcpp::node_interfaces::NodeInterfaces<ALL_RCLCPP_NODE_INTERFACES>(node);
+
+  {
+    auto base = ni.get<NodeBaseInterface>();
+    base = ni.get_node_base_interface();
+    EXPECT_STREQ(base->get_name(), "my_node");  // Test for functionality
+  }
+  {
+    auto clock = ni.get<NodeClockInterface>();
+    clock = ni.get_node_clock_interface();
+    clock->get_clock();
+  }
+  {
+    auto graph = ni.get<NodeGraphInterface>();
+    graph = ni.get_node_graph_interface();
+  }
+  {
+    auto logging = ni.get<NodeLoggingInterface>();
+    logging = ni.get_node_logging_interface();
+  }
+  {
+    auto timers = ni.get<NodeTimersInterface>();
+    timers = ni.get_node_timers_interface();
+  }
+  {
+    auto topics = ni.get<NodeTopicsInterface>();
+    topics = ni.get_node_topics_interface();
+  }
+  {
+    auto services = ni.get<NodeServicesInterface>();
+    services = ni.get_node_services_interface();
+  }
+  {
+    auto waitables = ni.get<NodeWaitablesInterface>();
+    waitables = ni.get_node_waitables_interface();
+  }
+  {
+    auto parameters = ni.get<NodeParametersInterface>();
+    parameters = ni.get_node_parameters_interface();
+  }
+  {
+    auto time_source = ni.get<NodeTimeSourceInterface>();
+    time_source = ni.get_node_time_source_interface();
+  }
+}

rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp

@@ -36,16 +36,14 @@ const rosidl_message_type_support_t EmptyTypeSupport()
   return *rosidl_typesupport_cpp::get_message_type_support_handle<test_msgs::msg::Empty>();
 }
 
-const rcl_publisher_options_t PublisherOptions()
+const rclcpp::PublisherOptionsWithAllocator<std::allocator<void>> PublisherOptions()
 {
-  return rclcpp::PublisherOptionsWithAllocator<std::allocator<void>>().template
-         to_rcl_publisher_options<test_msgs::msg::Empty>(rclcpp::QoS(10));
+  return rclcpp::PublisherOptionsWithAllocator<std::allocator<void>>();
 }
 
-const rcl_subscription_options_t SubscriptionOptions()
+const rclcpp::SubscriptionOptionsWithAllocator<std::allocator<void>> SubscriptionOptions()
 {
-  return rclcpp::SubscriptionOptionsWithAllocator<std::allocator<void>>().template
-         to_rcl_subscription_options<test_msgs::msg::Empty>(rclcpp::QoS(10));
+  return rclcpp::SubscriptionOptionsWithAllocator<std::allocator<void>>();
 }
 
 }  // namespace
@@ -55,7 +53,9 @@ class TestPublisher : public rclcpp::PublisherBase
 public:
   explicit TestPublisher(rclcpp::Node * node)
   : rclcpp::PublisherBase(
-      node->get_node_base_interface().get(), "topic", EmptyTypeSupport(), PublisherOptions()) {}
+      node->get_node_base_interface().get(), "topic", EmptyTypeSupport(),
+      PublisherOptions().to_rcl_publisher_options<test_msgs::msg::Empty>(rclcpp::QoS(10)),
+      PublisherOptions().event_callbacks, PublisherOptions().use_default_callbacks) {}
 };
 
 class TestSubscription : public rclcpp::SubscriptionBase
@@ -63,7 +63,9 @@ class TestSubscription : public rclcpp::SubscriptionBase
 public:
   explicit TestSubscription(rclcpp::Node * node)
   : rclcpp::SubscriptionBase(
-      node->get_node_base_interface().get(), EmptyTypeSupport(), "topic", SubscriptionOptions()) {}
+      node->get_node_base_interface().get(), EmptyTypeSupport(), "topic",
+      SubscriptionOptions().to_rcl_subscription_options(rclcpp::QoS(10)),
+      SubscriptionOptions().event_callbacks, SubscriptionOptions().use_default_callbacks) {}
   std::shared_ptr<void> create_message() override {return nullptr;}
 
   std::shared_ptr<rclcpp::SerializedMessage>

rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp

@@ -276,6 +276,70 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, one_node_many_callback_groups_many_e
   ASSERT_EQ(timer_executor.get_all_callback_groups().size(), 2u);
 }
 
+/*
+ * Test callback groups from one node to many executors.
+ * A subscriber on a new executor with a callback group not received a message
+ * because the executor can't be triggered while a subscriber created, see
+ * https://github.com/ros2/rclcpp/issues/1611
+*/
+TYPED_TEST(TestAddCallbackGroupsToExecutor, subscriber_triggered_to_receive_message)
+{
+  auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+
+  // create a thread running an executor with a new callback group for a coming subscriber
+  rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive, false);
+  rclcpp::executors::SingleThreadedExecutor cb_grp_executor;
+
+  std::promise<bool> received_message_promise;
+  auto received_message_future = received_message_promise.get_future();
+  rclcpp::FutureReturnCode return_code = rclcpp::FutureReturnCode::TIMEOUT;
+  std::thread cb_grp_thread = std::thread(
+    [&cb_grp, &node, &cb_grp_executor, &received_message_future, &return_code]() {
+      cb_grp_executor.add_callback_group(cb_grp, node->get_node_base_interface());
+      return_code = cb_grp_executor.spin_until_future_complete(received_message_future, 10s);
+    });
+
+  // expect the subscriber to receive a message
+  auto sub_callback = [&received_message_promise](test_msgs::msg::Empty::ConstSharedPtr) {
+      received_message_promise.set_value(true);
+    };
+
+  rclcpp::Subscription<test_msgs::msg::Empty>::SharedPtr subscription;
+  rclcpp::Publisher<test_msgs::msg::Empty>::SharedPtr publisher;
+  // to create a timer with a callback run on another executor
+  rclcpp::TimerBase::SharedPtr timer = nullptr;
+  std::promise<void> timer_promise;
+  auto timer_callback =
+    [&subscription, &publisher, &timer, &cb_grp, &node, &sub_callback, &timer_promise]() {
+      if (timer) {
+        timer.reset();
+      }
+
+      // create a subscription using the `cb_grp` callback group
+      rclcpp::QoS qos = rclcpp::QoS(1).reliable();
+      auto options = rclcpp::SubscriptionOptions();
+      options.callback_group = cb_grp;
+      subscription =
+        node->create_subscription<test_msgs::msg::Empty>("topic_name", qos, sub_callback, options);
+      // create a publisher to send data
+      publisher =
+        node->create_publisher<test_msgs::msg::Empty>("topic_name", qos);
+      publisher->publish(test_msgs::msg::Empty());
+      timer_promise.set_value();
+    };
+
+  rclcpp::executors::SingleThreadedExecutor timer_executor;
+  timer = node->create_wall_timer(100ms, timer_callback);
+  timer_executor.add_node(node);
+  auto future = timer_promise.get_future();
+  timer_executor.spin_until_future_complete(future);
+  cb_grp_thread.join();
+
+  ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code);
+  EXPECT_TRUE(received_message_future.get());
+}
+
 /*
  * Test removing callback group from executor that its not associated with.
  */

rclcpp/test/rclcpp/test_client.cpp

@@ -327,6 +327,27 @@ TEST_F(TestClientWithServer, test_client_remove_pending_request) {
   EXPECT_TRUE(client->remove_pending_request(future));
 }
 
+TEST_F(TestClientWithServer, prune_requests_older_than_no_pruned) {
+  auto client = node->create_client<test_msgs::srv::Empty>(service_name);
+  auto request = std::make_shared<test_msgs::srv::Empty::Request>();
+  auto future = client->async_send_request(request);
+  auto time = std::chrono::system_clock::now() + 1s;
+
+  EXPECT_EQ(1u, client->prune_requests_older_than(time));
+}
+
+TEST_F(TestClientWithServer, prune_requests_older_than_with_pruned) {
+  auto client = node->create_client<test_msgs::srv::Empty>(service_name);
+  auto request = std::make_shared<test_msgs::srv::Empty::Request>();
+  auto future = client->async_send_request(request);
+  auto time = std::chrono::system_clock::now() + 1s;
+
+  std::vector<int64_t> pruned_requests;
+  EXPECT_EQ(1u, client->prune_requests_older_than(time, &pruned_requests));
+  ASSERT_EQ(1u, pruned_requests.size());
+  EXPECT_EQ(future.request_id, pruned_requests[0]);
+}
+
 TEST_F(TestClientWithServer, async_send_request_rcl_send_request_error) {
   // Checking rcl_send_request in rclcpp::Client::async_send_request()
   auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_send_request, RCL_RET_ERROR);

rclcpp/test/rclcpp/test_node.cpp

@@ -95,6 +95,74 @@ TEST_F(TestNode, construction_and_destruction) {
       (void)node;
     }, rclcpp::exceptions::InvalidNamespaceError);
   }
+
+  {
+    rclcpp::NodeOptions options;
+    ASSERT_NO_THROW(
+    {
+      const auto node = std::make_shared<rclcpp::Node>("my_node", "/ns", options);
+      EXPECT_EQ(RCL_ROS_TIME, node->get_clock()->get_clock_type());
+    });
+  }
+
+  {
+    rclcpp::NodeOptions options;
+    options.parameter_overrides(
+    {
+      {"use_sim_time", true},
+    });
+    ASSERT_NO_THROW(
+    {
+      const auto node = std::make_shared<rclcpp::Node>("my_node", "/ns", options);
+      EXPECT_EQ(RCL_ROS_TIME, node->get_clock()->get_clock_type());
+    });
+  }
+
+  {
+    rclcpp::NodeOptions options;
+    options.clock_type(RCL_SYSTEM_TIME);
+    ASSERT_NO_THROW(
+    {
+      const auto node = std::make_shared<rclcpp::Node>("my_node", "/ns", options);
+      EXPECT_EQ(RCL_SYSTEM_TIME, node->get_clock()->get_clock_type());
+    });
+  }
+
+  {
+    rclcpp::NodeOptions options;
+    options.parameter_overrides(
+    {
+      {"use_sim_time", true},
+    });
+    options.clock_type(RCL_SYSTEM_TIME);
+    ASSERT_THROW(
+      const auto node = std::make_shared<rclcpp::Node>(
+        "my_node", "/ns",
+        options), std::invalid_argument);
+  }
+
+  {
+    rclcpp::NodeOptions options;
+    options.clock_type(RCL_STEADY_TIME);
+    ASSERT_NO_THROW(
+    {
+      const auto node = std::make_shared<rclcpp::Node>("my_node", "/ns", options);
+      EXPECT_EQ(RCL_STEADY_TIME, node->get_clock()->get_clock_type());
+    });
+  }
+
+  {
+    rclcpp::NodeOptions options;
+    options.parameter_overrides(
+    {
+      {"use_sim_time", true},
+    });
+    options.clock_type(RCL_STEADY_TIME);
+    ASSERT_THROW(
+      const auto node = std::make_shared<rclcpp::Node>(
+        "my_node", "/ns",
+        options), std::invalid_argument);
+  }
 }
 
 /*
@@ -3346,6 +3414,9 @@ TEST_F(TestNode, static_and_dynamic_typing) {
     EXPECT_THROW(
       node->get_parameter("integer_override_not_given"),
       rclcpp::exceptions::ParameterUninitializedException);
+    EXPECT_THROW(
+      node->get_parameters({"integer_override_not_given"}),
+      rclcpp::exceptions::ParameterUninitializedException);
   }
   {
     auto param = node->declare_parameter("integer_set_after_declare", rclcpp::PARAMETER_INTEGER);
@@ -3367,3 +3438,34 @@ TEST_F(TestNode, static_and_dynamic_typing) {
       rclcpp::exceptions::InvalidParameterTypeException);
   }
 }
+
+TEST_F(TestNode, parameter_uninitialized_exception_even_if_allow_undeclared) {
+  rclcpp::NodeOptions no;
+  no.allow_undeclared_parameters(true);
+  auto node = std::make_shared<rclcpp::Node>("node", "ns", no);
+  {
+    const std::string param_name = "integer_override_not_given";
+    auto param_value = node->declare_parameter(param_name, rclcpp::PARAMETER_INTEGER);
+    EXPECT_EQ(rclcpp::PARAMETER_NOT_SET, param_value.get_type());
+    // Throws if not set before access
+    EXPECT_THROW(
+      node->get_parameter(param_name),
+      rclcpp::exceptions::ParameterUninitializedException);
+    EXPECT_THROW(
+      node->get_parameters({param_name}),
+      rclcpp::exceptions::ParameterUninitializedException);
+  }
+}
+
+TEST_F(TestNode, get_parameter_with_node_allow_undeclared) {
+  rclcpp::NodeOptions no;
+  no.allow_undeclared_parameters(true);
+  auto node = std::make_shared<rclcpp::Node>("node", "ns", no);
+  {
+    const std::string param_name = "allow_undeclared_param";
+    auto param = node->get_parameter(param_name);
+    EXPECT_EQ(param_name, param.get_name());
+    EXPECT_EQ(rclcpp::PARAMETER_NOT_SET, param.get_type());
+    EXPECT_EQ(rclcpp::ParameterValue{}, param.get_parameter_value());
+  }
+}

rclcpp/test/rclcpp/test_node_options.cpp

@@ -316,3 +316,12 @@ TEST(TestNodeOptions, set_get_allocator) {
   // Check invalid allocator
   EXPECT_THROW(options.get_rcl_node_options(), std::bad_alloc);
 }
+
+TEST(TestNodeOptions, clock_type) {
+  rclcpp::NodeOptions options;
+  EXPECT_EQ(RCL_ROS_TIME, options.clock_type());
+  options.clock_type(RCL_SYSTEM_TIME);
+  EXPECT_EQ(RCL_SYSTEM_TIME, options.clock_type());
+  options.clock_type(RCL_STEADY_TIME);
+  EXPECT_EQ(RCL_STEADY_TIME, options.clock_type());
+}

rclcpp/test/rclcpp/test_parameter_client.cpp

@@ -440,6 +440,7 @@ TEST_F(TestParameterClient, sync_parameter_get_parameter_types_allow_undeclared)
 TEST_F(TestParameterClient, sync_parameter_get_parameters) {
   node->declare_parameter("foo", 4);
   node->declare_parameter("bar", "this is bar");
+  node->declare_parameter("baz", rclcpp::PARAMETER_INTEGER);
   auto synchronous_client = std::make_shared<rclcpp::SyncParametersClient>(node);
 
   {
@@ -448,6 +449,14 @@ TEST_F(TestParameterClient, sync_parameter_get_parameters) {
     ASSERT_EQ(0u, parameters.size());
   }
 
+  {
+    // not throw ParameterUninitializedException while getting parameter from service
+    // even if the parameter is not initialized in the node
+    std::vector<std::string> names{"baz"};
+    std::vector<rclcpp::Parameter> parameters = synchronous_client->get_parameters(names, 10s);
+    ASSERT_EQ(0u, parameters.size());
+  }
+
   {
     std::vector<std::string> names{"none", "foo", "bar"};
     std::vector<rclcpp::Parameter> parameters = synchronous_client->get_parameters(names, 10s);
@@ -487,6 +496,7 @@ TEST_F(TestParameterClient, sync_parameter_get_parameters) {
 TEST_F(TestParameterClient, sync_parameter_get_parameters_allow_undeclared) {
   node_with_option->declare_parameter("foo", 4);
   node_with_option->declare_parameter("bar", "this is bar");
+  node_with_option->declare_parameter("baz", rclcpp::PARAMETER_INTEGER);
   auto synchronous_client = std::make_shared<rclcpp::SyncParametersClient>(node_with_option);
 
   {
@@ -495,6 +505,14 @@ TEST_F(TestParameterClient, sync_parameter_get_parameters_allow_undeclared) {
     ASSERT_EQ(1u, parameters.size());
   }
 
+  {
+    // not throw ParameterUninitializedException while getting parameter from service
+    // even if the parameter is not initialized in the node
+    std::vector<std::string> names{"baz"};
+    std::vector<rclcpp::Parameter> parameters = synchronous_client->get_parameters(names, 10s);
+    ASSERT_EQ(0u, parameters.size());
+  }
+
   {
     std::vector<std::string> names{"none", "foo", "bar"};
     std::vector<rclcpp::Parameter> parameters = synchronous_client->get_parameters(names, 10s);

rclcpp/test/rclcpp/test_publisher.cpp

@@ -245,10 +245,9 @@ const rosidl_message_type_support_t EmptyTypeSupport()
   return *rosidl_typesupport_cpp::get_message_type_support_handle<test_msgs::msg::Empty>();
 }
 
-const rcl_publisher_options_t PublisherOptions()
+const rclcpp::PublisherOptionsWithAllocator<std::allocator<void>> PublisherOptions()
 {
-  return rclcpp::PublisherOptionsWithAllocator<std::allocator<void>>().template
-         to_rcl_publisher_options<test_msgs::msg::Empty>(rclcpp::QoS(10));
+  return rclcpp::PublisherOptionsWithAllocator<std::allocator<void>>();
 }
 
 class TestPublisherBase : public rclcpp::PublisherBase
@@ -256,7 +255,9 @@ class TestPublisherBase : public rclcpp::PublisherBase
 public:
   explicit TestPublisherBase(rclcpp::Node * node)
   : rclcpp::PublisherBase(
-      node->get_node_base_interface().get(), "topic", EmptyTypeSupport(), PublisherOptions()) {}
+      node->get_node_base_interface().get(), "topic", EmptyTypeSupport(),
+      PublisherOptions().to_rcl_publisher_options<test_msgs::msg::Empty>(rclcpp::QoS(10)),
+      PublisherOptions().event_callbacks, PublisherOptions().use_default_callbacks) {}
 };
 
 /*

rclcpp/test/rclcpp/test_qos_parameters.cpp

@@ -255,3 +255,9 @@ TEST(TestQosParameters, internal_functions_failure_modes) {
       nullptr, rclcpp::QosPolicyKind::Reliability),
     std::invalid_argument);
 }
+
+TEST(TestQosParameters, keep_last_zero) {
+  rclcpp::KeepLast keep_last(0);
+
+  EXPECT_EQ(keep_last.depth, RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT);
+}

rclcpp/test/rclcpp/test_time.cpp

@@ -19,6 +19,7 @@
 #include <limits>
 #include <memory>
 #include <string>
+#include <thread>
 
 #include "rcl/error_handling.h"
 #include "rcl/time.h"
@@ -848,3 +849,72 @@ TEST_F(TestClockSleep, sleep_for_basic_ros) {
   sleep_thread.join();
   EXPECT_TRUE(sleep_succeeded);
 }
+
+class TestClockStarted : public ::testing::Test
+{
+protected:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  void TearDown()
+  {
+    rclcpp::shutdown();
+  }
+};
+
+TEST_F(TestClockStarted, started) {
+  // rclcpp::Clock ros_clock(RCL_ROS_TIME);
+  // auto ros_clock_handle = ros_clock.get_clock_handle();
+  //
+  // // At this point, the ROS clock is reading system time since the ROS time override isn't on
+  // // So we expect it to be started (it's extremely unlikely that system time is at epoch start)
+  // EXPECT_TRUE(ros_clock.started());
+  // EXPECT_TRUE(ros_clock.wait_until_started());
+  // EXPECT_TRUE(ros_clock.wait_until_started(rclcpp::Duration(0, static_cast<uint32_t>(1e7))));
+  // EXPECT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(ros_clock_handle));
+  // EXPECT_TRUE(ros_clock.ros_time_is_active());
+  // EXPECT_FALSE(ros_clock.started());
+  // EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(ros_clock_handle, 1));
+  // EXPECT_TRUE(ros_clock.started());
+  //
+  // rclcpp::Clock system_clock(RCL_SYSTEM_TIME);
+  // EXPECT_TRUE(system_clock.started());
+  // EXPECT_TRUE(system_clock.wait_until_started());
+  // EXPECT_TRUE(system_clock.wait_until_started(rclcpp::Duration(0, static_cast<uint32_t>(1e7))));
+  //
+  // rclcpp::Clock steady_clock(RCL_STEADY_TIME);
+  // EXPECT_TRUE(steady_clock.started());
+  // EXPECT_TRUE(steady_clock.wait_until_started());
+  // EXPECT_TRUE(steady_clock.wait_until_started(rclcpp::Duration(0, static_cast<uint32_t>(1e7))));
+  //
+  // rclcpp::Clock uninit_clock(RCL_CLOCK_UNINITIALIZED);
+  // RCLCPP_EXPECT_THROW_EQ(
+  //   uninit_clock.started(), std::runtime_error("clock is not rcl_clock_valid"));
+  // RCLCPP_EXPECT_THROW_EQ(
+  //   uninit_clock.wait_until_started(rclcpp::Duration(0, static_cast<uint32_t>(1e7))),
+  //   std::runtime_error("clock cannot be waited on as it is not rcl_clock_valid"));
+}
+
+TEST_F(TestClockStarted, started_timeout) {
+  rclcpp::Clock ros_clock(RCL_ROS_TIME);
+  auto ros_clock_handle = ros_clock.get_clock_handle();
+
+  EXPECT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(ros_clock_handle));
+  EXPECT_TRUE(ros_clock.ros_time_is_active());
+
+  EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(ros_clock_handle, 0));
+
+  EXPECT_FALSE(ros_clock.started());
+  EXPECT_FALSE(ros_clock.wait_until_started(rclcpp::Duration(0, static_cast<uint32_t>(1e7))));
+
+  std::thread t([]() {
+      std::this_thread::sleep_for(std::chrono::seconds(1));
+      rclcpp::shutdown();
+    });
+
+  // Test rclcpp shutdown escape hatch (otherwise this waits indefinitely)
+  EXPECT_FALSE(ros_clock.wait_until_started());
+  t.join();
+}

rclcpp/test/rclcpp/test_time_source.cpp

@@ -267,6 +267,35 @@ TEST(TimeSource, invalid_sim_time_parameter_override)
   rclcpp::shutdown();
 }
 
+TEST(TimeSource, valid_clock_type_for_sim_time)
+{
+  rclcpp::init(0, nullptr);
+
+  rclcpp::NodeOptions options;
+  auto node = std::make_shared<rclcpp::Node>("my_node", options);
+  EXPECT_TRUE(
+    node->set_parameter(
+      rclcpp::Parameter(
+        "use_sim_time", rclcpp::ParameterValue(
+          true))).successful);
+  rclcpp::shutdown();
+}
+
+TEST(TimeSource, invalid_clock_type_for_sim_time)
+{
+  rclcpp::init(0, nullptr);
+
+  rclcpp::NodeOptions options;
+  options.clock_type(RCL_STEADY_TIME);
+  auto node = std::make_shared<rclcpp::Node>("my_node", options);
+  EXPECT_FALSE(
+    node->set_parameter(
+      rclcpp::Parameter(
+        "use_sim_time", rclcpp::ParameterValue(
+          true))).successful);
+  rclcpp::shutdown();
+}
+
 TEST_F(TestTimeSource, clock) {
   rclcpp::TimeSource ts(node);
   auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);

rclcpp/test/rclcpp/test_type_support.cpp

@@ -58,10 +58,9 @@ protected:
   rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("my_node", "/ns");
 };
 
-const rcl_publisher_options_t PublisherOptions()
+const rclcpp::PublisherOptionsWithAllocator<std::allocator<void>> PublisherOptions()
 {
-  return rclcpp::PublisherOptionsWithAllocator<std::allocator<void>>().template
-         to_rcl_publisher_options<test_msgs::msg::Empty>(rclcpp::QoS(10));
+  return rclcpp::PublisherOptionsWithAllocator<std::allocator<void>>();
 }
 
 // Auxiliary classes used to test rosidl_message_type_support_t getters
@@ -77,7 +76,8 @@ public:
       node->get_node_base_interface().get(),
       "topicTSParameterEvent",
       *ts_parameter_event,
-      PublisherOptions()) {}
+      PublisherOptions().to_rcl_publisher_options<test_msgs::msg::Empty>(rclcpp::QoS(10)),
+      PublisherOptions().event_callbacks, PublisherOptions().use_default_callbacks) {}
 };
 
 const rosidl_message_type_support_t * ts_set_parameter_result =
@@ -91,7 +91,8 @@ public:
       node->get_node_base_interface().get(),
       "topicTSSetParameterResult",
       *ts_set_parameter_result,
-      PublisherOptions()) {}
+      PublisherOptions().to_rcl_publisher_options<test_msgs::msg::Empty>(rclcpp::QoS(10)),
+      PublisherOptions().event_callbacks, PublisherOptions().use_default_callbacks) {}
 };
 
 const rosidl_message_type_support_t * ts_parameter_descriptor =
@@ -105,7 +106,8 @@ public:
       node->get_node_base_interface().get(),
       "topicTSParameterDescriptor",
       *ts_parameter_descriptor,
-      PublisherOptions()) {}
+      PublisherOptions().to_rcl_publisher_options<test_msgs::msg::Empty>(rclcpp::QoS(10)),
+      PublisherOptions().event_callbacks, PublisherOptions().use_default_callbacks) {}
 };
 
 const rosidl_message_type_support_t * ts_list_parameter_result =
@@ -119,7 +121,8 @@ public:
       node->get_node_base_interface().get(),
       "topicTSListParametersResult",
       *ts_list_parameter_result,
-      PublisherOptions()) {}
+      PublisherOptions().to_rcl_publisher_options<test_msgs::msg::Empty>(rclcpp::QoS(10)),
+      PublisherOptions().event_callbacks, PublisherOptions().use_default_callbacks) {}
 };
 
 /*

rclcpp_action/CHANGELOG.rst

@@ -3,6 +3,17 @@ Changelog for package rclcpp_action
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+18.0.0 (2022-12-29)
+-------------------
+* Explicitly set callback type (`#2059 <https://github.com/ros2/rclcpp/issues/2059>`_)
+* Update maintainers (`#2043 <https://github.com/ros2/rclcpp/issues/2043>`_)
+* Contributors: Audrow Nash, mauropasse
+
+17.1.0 (2022-11-02)
+-------------------
+* Do not clear entities callbacks on destruction (`#2002 <https://github.com/ros2/rclcpp/issues/2002>`_)
+* Contributors: mauropasse
+
 17.0.0 (2022-09-13)
 -------------------
 * Revert "Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 <https://github.com/ros2/rclcpp/issues/1821>`_) (`#1874 <https://github.com/ros2/rclcpp/issues/1874>`_)" (`#1956 <https://github.com/ros2/rclcpp/issues/1956>`_)

rclcpp_action/package.xml

@@ -2,13 +2,17 @@
 <?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>17.0.0</version>
+  <version>18.0.0</version>
   <description>Adds action APIs for C++.</description>
+
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
-  <maintainer email="jacob@openrobotics.org">Jacob Perron</maintainer>
+  <maintainer email="michel@ekumenlabs.com">Michel Hidalgo</maintainer>
   <maintainer email="william@openrobotics.org">William Woodall</maintainer>
+
   <license>Apache License 2.0</license>
+
   <author email="dthomas@openrobotics.org">Dirk Thomas</author>
+  <author email="jacob@openrobotics.org">Jacob Perron</author>
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 

rclcpp_action/src/client.cpp

@@ -136,7 +136,6 @@ ClientBase::ClientBase(
 
 ClientBase::~ClientBase()
 {
-  clear_on_ready_callback();
 }
 
 bool
@@ -434,7 +433,7 @@ ClientBase::set_callback_to_entity(
   // been replaced but the middleware hasn't been told about the new one yet.
   set_on_ready_callback(
     entity_type,
-    rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+    rclcpp::detail::cpp_callback_trampoline<decltype(new_callback), const void *, size_t>,
     static_cast<const void *>(&new_callback));
 
   std::lock_guard<std::recursive_mutex> lock(listener_mutex_);
@@ -454,7 +453,7 @@ ClientBase::set_callback_to_entity(
     auto & cb = it->second;
     set_on_ready_callback(
       entity_type,
-      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      rclcpp::detail::cpp_callback_trampoline<decltype(it->second), const void *, size_t>,
       static_cast<const void *>(&cb));
   }
 

rclcpp_action/src/server.cpp

@@ -132,7 +132,6 @@ ServerBase::ServerBase(
 
 ServerBase::~ServerBase()
 {
-  clear_on_ready_callback();
 }
 
 size_t
@@ -746,7 +745,7 @@ ServerBase::set_callback_to_entity(
   // been replaced but the middleware hasn't been told about the new one yet.
   set_on_ready_callback(
     entity_type,
-    rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+    rclcpp::detail::cpp_callback_trampoline<decltype(new_callback), const void *, size_t>,
     static_cast<const void *>(&new_callback));
 
   std::lock_guard<std::recursive_mutex> lock(listener_mutex_);
@@ -766,7 +765,7 @@ ServerBase::set_callback_to_entity(
     auto & cb = it->second;
     set_on_ready_callback(
       entity_type,
-      rclcpp::detail::cpp_callback_trampoline<const void *, size_t>,
+      rclcpp::detail::cpp_callback_trampoline<decltype(it->second), const void *, size_t>,
       static_cast<const void *>(&cb));
   }
 

rclcpp_components/CHANGELOG.rst

@@ -2,6 +2,16 @@
 Changelog for package rclcpp_components
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+18.0.0 (2022-12-29)
+-------------------
+* Update maintainers (`#2043 <https://github.com/ros2/rclcpp/issues/2043>`_)
+* Contributors: Audrow Nash
+
+17.1.0 (2022-11-02)
+-------------------
+* use unique ptr and remove unuseful container (`#2013 <https://github.com/ros2/rclcpp/issues/2013>`_)
+* Contributors: Chen Lihui
+
 17.0.0 (2022-09-13)
 -------------------
 * Revert "Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 <https://github.com/ros2/rclcpp/issues/1821>`_) (`#1874 <https://github.com/ros2/rclcpp/issues/1874>`_)" (`#1956 <https://github.com/ros2/rclcpp/issues/1956>`_)

rclcpp_components/package.xml

@@ -2,12 +2,16 @@
 <?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>17.0.0</version>
+  <version>18.0.0</version>
   <description>Package containing tools for dynamically loadable components</description>
+
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
-  <maintainer email="jacob@openrobotics.org">Jacob Perron</maintainer>
+  <maintainer email="michel@ekumenlabs.com">Michel Hidalgo</maintainer>
   <maintainer email="william@openrobotics.org">William Woodall</maintainer>
+
   <license>Apache License 2.0</license>
+
+  <author email="jacob@openrobotics.org">Jacob Perron</author>
   <author email="michael@openrobotics.org">Michael Carroll</author>
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>

rclcpp_components/src/node_main.cpp.in

@@ -30,14 +30,13 @@ int main(int argc, char * argv[])
   rclcpp::executors::@executor@ exec;
   rclcpp::NodeOptions options;
   options.arguments(args);
-  std::vector<class_loader::ClassLoader * > loaders;
   std::vector<rclcpp_components::NodeInstanceWrapper> node_wrappers;
 
   std::string library_name = "@library_name@";
   std::string class_name = "rclcpp_components::NodeFactoryTemplate<@component@>";
 
   RCLCPP_DEBUG(logger, "Load library %s", library_name.c_str());
-  auto loader = new class_loader::ClassLoader(library_name);
+  auto loader = std::make_unique<class_loader::ClassLoader>(library_name);
   auto classes = loader->getAvailableClasses<rclcpp_components::NodeFactory>();
   for (const auto & clazz : classes) {
     std::string name = clazz.c_str();
@@ -59,8 +58,6 @@ int main(int argc, char * argv[])
       exec.add_node(node);
     }
   }
-  loaders.push_back(loader);
-
 
   exec.spin();
 

rclcpp_lifecycle/CHANGELOG.rst

@@ -3,6 +3,22 @@ Changelog for package rclcpp_lifecycle
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+18.0.0 (2022-12-29)
+-------------------
+* Implement Unified Node Interface (NodeInterfaces class) (`#2041 <https://github.com/ros2/rclcpp/issues/2041>`_)
+* Add clock type to node_options (`#1982 <https://github.com/ros2/rclcpp/issues/1982>`_)
+* Update maintainers (`#2043 <https://github.com/ros2/rclcpp/issues/2043>`_)
+* Contributors: Audrow Nash, Jeffery Hsu, methylDragon
+
+17.1.0 (2022-11-02)
+-------------------
+* LifecycleNode on_configure doc fix. (`#2034 <https://github.com/ros2/rclcpp/issues/2034>`_)
+* Bugfix 20210810 get current state (`#1756 <https://github.com/ros2/rclcpp/issues/1756>`_)
+* Make lifecycle impl get_current_state() const. (`#2031 <https://github.com/ros2/rclcpp/issues/2031>`_)
+* Cleanup the lifecycle implementation (`#2027 <https://github.com/ros2/rclcpp/issues/2027>`_)
+* Cleanup the rclcpp_lifecycle dependencies. (`#2021 <https://github.com/ros2/rclcpp/issues/2021>`_)
+* Contributors: Chris Lalancette, Tomoya Fujita
+
 17.0.0 (2022-09-13)
 -------------------
 * Revert "Revert "Add a create_timer method to Node and `LifecycleNode` classes (`#1975 <https://github.com/ros2/rclcpp/issues/1975>`_)" (`#2009 <https://github.com/ros2/rclcpp/issues/2009>`_) (`#2010 <https://github.com/ros2/rclcpp/issues/2010>`_)

rclcpp_lifecycle/CMakeLists.txt

@@ -11,14 +11,18 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 find_package(ament_cmake_ros REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(rcl_lifecycle REQUIRED)
-find_package(rosidl_typesupport_cpp REQUIRED)
 find_package(lifecycle_msgs REQUIRED)
+find_package(rcl REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rcl_interfaces REQUIRED)
+find_package(rcl_lifecycle REQUIRED)
+find_package(rcutils REQUIRED)
+find_package(rosidl_typesupport_cpp REQUIRED)
 
 ### CPP High level library
 add_library(rclcpp_lifecycle
   src/lifecycle_node.cpp
+  src/lifecycle_node_interface_impl.cpp
   src/managed_entity.cpp
   src/node_interfaces/lifecycle_node_interface.cpp
   src/state.cpp
@@ -28,12 +32,14 @@ target_include_directories(${PROJECT_NAME}
   PUBLIC
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
   "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
-# specific order: dependents before dependencies
-ament_target_dependencies(rclcpp_lifecycle
-  "rclcpp"
-  "rcl_lifecycle"
-  "lifecycle_msgs"
-  "rosidl_typesupport_cpp"
+target_link_libraries(${PROJECT_NAME}
+  ${lifecycle_msgs_TARGETS}
+  rcl::rcl
+  rclcpp::rclcpp
+  ${rcl_interfaces_TARGETS}
+  rcl_lifecycle::rcl_lifecycle
+  rcutils::rcutils
+  rosidl_typesupport_cpp::rosidl_typesupport_cpp
 )
 
 # Causes the visibility macros to use dllexport rather than dllimport,
@@ -46,6 +52,9 @@ install(TARGETS
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin)
 
+install(DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME})
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # Give cppcheck hints about macro definitions coming from outside this package
@@ -58,15 +67,13 @@ if(BUILD_TESTING)
     benchmark_lifecycle_client
     test/benchmark/benchmark_lifecycle_client.cpp)
   if(TARGET benchmark_lifecycle_client)
-    target_link_libraries(benchmark_lifecycle_client ${PROJECT_NAME})
-    ament_target_dependencies(benchmark_lifecycle_client rclcpp)
+    target_link_libraries(benchmark_lifecycle_client ${PROJECT_NAME} rclcpp::rclcpp)
   endif()
   add_performance_test(
     benchmark_lifecycle_node
     test/benchmark/benchmark_lifecycle_node.cpp)
   if(TARGET benchmark_lifecycle_node)
-    target_link_libraries(benchmark_lifecycle_node ${PROJECT_NAME})
-    ament_target_dependencies(benchmark_lifecycle_node rclcpp)
+    target_link_libraries(benchmark_lifecycle_node ${PROJECT_NAME} rclcpp::rclcpp)
   endif()
   add_performance_test(
     benchmark_state
@@ -83,31 +90,21 @@ if(BUILD_TESTING)
 
   ament_add_gtest(test_lifecycle_node test/test_lifecycle_node.cpp TIMEOUT 120)
   if(TARGET test_lifecycle_node)
-    ament_target_dependencies(test_lifecycle_node
-      "rcl_lifecycle"
-      "rclcpp"
-      "rcutils"
-    )
-    target_link_libraries(test_lifecycle_node ${PROJECT_NAME} mimick)
+    target_link_libraries(test_lifecycle_node ${PROJECT_NAME} mimick rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp rcutils::rcutils)
   endif()
   ament_add_gtest(test_lifecycle_publisher test/test_lifecycle_publisher.cpp)
   if(TARGET test_lifecycle_publisher)
-    ament_target_dependencies(test_lifecycle_publisher
-      "rcl_lifecycle"
-      "rclcpp"
-      "test_msgs"
-    )
-    target_link_libraries(test_lifecycle_publisher ${PROJECT_NAME})
+    target_link_libraries(test_lifecycle_publisher ${PROJECT_NAME} rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp ${test_msgs_TARGETS})
   endif()
   ament_add_gtest(test_lifecycle_service_client test/test_lifecycle_service_client.cpp TIMEOUT 120)
   if(TARGET test_lifecycle_service_client)
-    ament_target_dependencies(test_lifecycle_service_client
-      "rcl_lifecycle"
-      "rclcpp"
-      "rcpputils"
-      "rcutils"
-    )
-    target_link_libraries(test_lifecycle_service_client ${PROJECT_NAME} mimick)
+    target_link_libraries(test_lifecycle_service_client
+      ${PROJECT_NAME}
+      mimick
+      rcl_lifecycle::rcl_lifecycle
+      rclcpp::rclcpp
+      rcpputils::rcpputils
+      rcutils::rcutils)
   endif()
   ament_add_gtest(test_client test/test_client.cpp TIMEOUT 120)
   if(TARGET test_client)
@@ -127,44 +124,23 @@ if(BUILD_TESTING)
   endif()
   ament_add_gtest(test_state_machine_info test/test_state_machine_info.cpp)
   if(TARGET test_state_machine_info)
-    ament_target_dependencies(test_state_machine_info
-      "rcl_lifecycle"
-      "rclcpp"
-    )
-    target_link_libraries(test_state_machine_info ${PROJECT_NAME})
+    target_link_libraries(test_state_machine_info ${PROJECT_NAME} rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp)
   endif()
   ament_add_gtest(test_register_custom_callbacks test/test_register_custom_callbacks.cpp)
   if(TARGET test_register_custom_callbacks)
-    ament_target_dependencies(test_register_custom_callbacks
-      "rcl_lifecycle"
-      "rclcpp"
-    )
-    target_link_libraries(test_register_custom_callbacks ${PROJECT_NAME})
+    target_link_libraries(test_register_custom_callbacks ${PROJECT_NAME} rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp)
   endif()
   ament_add_gtest(test_callback_exceptions test/test_callback_exceptions.cpp)
   if(TARGET test_callback_exceptions)
-    ament_target_dependencies(test_callback_exceptions
-      "rcl_lifecycle"
-      "rclcpp"
-    )
-    target_link_libraries(test_callback_exceptions ${PROJECT_NAME})
+    target_link_libraries(test_callback_exceptions ${PROJECT_NAME} rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp)
   endif()
   ament_add_gtest(test_state_wrapper test/test_state_wrapper.cpp)
   if(TARGET test_state_wrapper)
-    ament_target_dependencies(test_state_wrapper
-      "rcl_lifecycle"
-      "rclcpp"
-    )
-    target_link_libraries(test_state_wrapper ${PROJECT_NAME})
+    target_link_libraries(test_state_wrapper ${PROJECT_NAME} rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp)
   endif()
   ament_add_gtest(test_transition_wrapper test/test_transition_wrapper.cpp)
   if(TARGET test_transition_wrapper)
-    ament_target_dependencies(test_transition_wrapper
-      "rcl_lifecycle"
-      "rclcpp"
-      "rcutils"
-    )
-    target_link_libraries(test_transition_wrapper ${PROJECT_NAME} mimick)
+    target_link_libraries(test_transition_wrapper ${PROJECT_NAME} mimick rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp rcutils::rcutils)
     target_compile_definitions(test_transition_wrapper
       PUBLIC RCUTILS_ENABLE_FAULT_INJECTION
     )
@@ -178,11 +154,7 @@ ament_export_libraries(${PROJECT_NAME})
 # Export modern CMake targets
 ament_export_targets(${PROJECT_NAME})
 
-# specific order: dependents before dependencies
-ament_export_dependencies(rclcpp)
-ament_export_dependencies(rcl_lifecycle)
-ament_export_dependencies(lifecycle_msgs)
-ament_package()
+# Export dependencies
+ament_export_dependencies(lifecycle_msgs rcl rclcpp rcl_interfaces rcl_lifecycle rcutils rosidl_typesupport_cpp)
 
-install(DIRECTORY include/
-  DESTINATION include/${PROJECT_NAME})
+ament_package()

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp

@@ -36,6 +36,8 @@
 #ifndef RCLCPP_LIFECYCLE__LIFECYCLE_NODE_HPP_
 #define RCLCPP_LIFECYCLE__LIFECYCLE_NODE_HPP_
 
+#include <chrono>
+#include <functional>
 #include <map>
 #include <memory>
 #include <string>
@@ -49,13 +51,11 @@
 
 #include "rcl_interfaces/msg/list_parameters_result.hpp"
 #include "rcl_interfaces/msg/parameter_descriptor.hpp"
-#include "rcl_interfaces/msg/parameter_event.hpp"
 #include "rcl_interfaces/msg/set_parameters_result.hpp"
 
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/client.hpp"
 #include "rclcpp/clock.hpp"
-#include "rclcpp/context.hpp"
 #include "rclcpp/event.hpp"
 #include "rclcpp/generic_publisher.hpp"
 #include "rclcpp/generic_subscription.hpp"
@@ -83,6 +83,8 @@
 #include "rclcpp/time.hpp"
 #include "rclcpp/timer.hpp"
 
+#include "rmw/types.h"
+
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "rclcpp_lifecycle/lifecycle_publisher.hpp"
 #include "rclcpp_lifecycle/state.hpp"
@@ -137,7 +139,7 @@ public:
     const rclcpp::NodeOptions & options = rclcpp::NodeOptions(),
     bool enable_communication_interface = true);
 
-  /// Create a node based on the node name and a rclcpp::Context.
+  /// Create a node based on the node name
   /**
    * \param[in] node_name Name of the node.
    * \param[in] namespace_ Namespace of the node.
@@ -837,7 +839,7 @@ public:
    */
   RCLCPP_LIFECYCLE_PUBLIC
   const State &
-  get_current_state();
+  get_current_state() const;
 
   /// Return a list with the available states.
   /**
@@ -845,7 +847,7 @@ public:
    */
   RCLCPP_LIFECYCLE_PUBLIC
   std::vector<State>
-  get_available_states();
+  get_available_states() const;
 
   /// Return a list with the current available transitions.
   /**
@@ -853,7 +855,7 @@ public:
    */
   RCLCPP_LIFECYCLE_PUBLIC
   std::vector<Transition>
-  get_available_transitions();
+  get_available_transitions() const;
 
   /// Return a list with all the transitions.
   /**
@@ -861,7 +863,7 @@ public:
    */
   RCLCPP_LIFECYCLE_PUBLIC
   std::vector<Transition>
-  get_transition_graph();
+  get_transition_graph() const;
 
   /// Trigger the specified transition.
   /*

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp

@@ -15,29 +15,35 @@
 #ifndef RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_
 #define RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_
 
+#include <algorithm>
+#include <chrono>
+#include <functional>
 #include <map>
 #include <memory>
 #include <string>
 #include <utility>
 #include <vector>
 
-#include "rclcpp/contexts/default_context.hpp"
+#include "rcl_interfaces/msg/parameter_descriptor.hpp"
+
+#include "rclcpp/callback_group.hpp"
 #include "rclcpp/create_client.hpp"
 #include "rclcpp/create_generic_publisher.hpp"
 #include "rclcpp/create_generic_subscription.hpp"
 #include "rclcpp/create_publisher.hpp"
 #include "rclcpp/create_service.hpp"
 #include "rclcpp/create_subscription.hpp"
-#include "rclcpp/event.hpp"
-#include "rclcpp/experimental/intra_process_manager.hpp"
 #include "rclcpp/parameter.hpp"
+#include "rclcpp/publisher_options.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/subscription_options.hpp"
 #include "rclcpp/type_support_decl.hpp"
 
-#include "lifecycle_publisher.hpp"
-#include "rclcpp_lifecycle/visibility_control.h"
+#include "rmw/types.h"
 
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
+#include "rclcpp_lifecycle/visibility_control.h"
 
 namespace rclcpp_lifecycle
 {

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp

@@ -20,7 +20,10 @@
 #include <utility>
 
 #include "rclcpp/logging.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/publisher.hpp"
+#include "rclcpp/publisher_options.hpp"
+#include "rclcpp/qos.hpp"
 
 #include "rclcpp_lifecycle/managed_entity.hpp"
 

rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp

@@ -17,10 +17,9 @@
 
 #include "lifecycle_msgs/msg/transition.hpp"
 
-#include "rcl_lifecycle/rcl_lifecycle.h"
-
 #include "rclcpp_lifecycle/state.hpp"
 #include "rclcpp_lifecycle/visibility_control.h"
+#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp"
 
 namespace rclcpp_lifecycle
 {
@@ -55,7 +54,7 @@ public:
 
   /// Callback function for configure transition
   /*
-   * \return true by default
+   * \return SUCCESS by default
    */
   RCLCPP_LIFECYCLE_PUBLIC
   virtual CallbackReturn
@@ -63,7 +62,7 @@ public:
 
   /// Callback function for cleanup transition
   /*
-   * \return true by default
+   * \return SUCCESS by default
    */
   RCLCPP_LIFECYCLE_PUBLIC
   virtual CallbackReturn
@@ -71,7 +70,7 @@ public:
 
   /// Callback function for shutdown transition
   /*
-   * \return true by default
+   * \return SUCCESS by default
    */
   RCLCPP_LIFECYCLE_PUBLIC
   virtual CallbackReturn
@@ -79,7 +78,7 @@ public:
 
   /// Callback function for activate transition
   /*
-   * \return true by default
+   * \return SUCCESS by default
    */
   RCLCPP_LIFECYCLE_PUBLIC
   virtual CallbackReturn
@@ -87,7 +86,7 @@ public:
 
   /// Callback function for deactivate transition
   /*
-   * \return true by default
+   * \return SUCCESS by default
    */
   RCLCPP_LIFECYCLE_PUBLIC
   virtual CallbackReturn
@@ -95,7 +94,7 @@ public:
 
   /// Callback function for errorneous transition
   /*
-   * \return false by default
+   * \return SUCCESS by default
    */
   RCLCPP_LIFECYCLE_PUBLIC
   virtual CallbackReturn
@@ -108,4 +107,8 @@ public:
 
 }  // namespace node_interfaces
 }  // namespace rclcpp_lifecycle
+
+RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface, lifecycle_node)
+
 #endif  // RCLCPP_LIFECYCLE__NODE_INTERFACES__LIFECYCLE_NODE_INTERFACE_HPP_

rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp

@@ -15,9 +15,11 @@
 #ifndef RCLCPP_LIFECYCLE__STATE_HPP_
 #define RCLCPP_LIFECYCLE__STATE_HPP_
 
+#include <mutex>
 #include <string>
 
 #include "rcl_lifecycle/data_types.h"
+
 #include "rclcpp_lifecycle/visibility_control.h"
 
 #include "rcutils/allocator.h"
@@ -91,6 +93,7 @@ protected:
 
   bool owns_rcl_state_handle_;
 
+  mutable std::recursive_mutex state_handle_mutex_;
   rcl_lifecycle_state_t * state_handle_;
 };
 

rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp

@@ -18,6 +18,7 @@
 #include <string>
 
 #include "rcl_lifecycle/data_types.h"
+
 #include "rclcpp_lifecycle/state.hpp"
 #include "rclcpp_lifecycle/visibility_control.h"
 

rclcpp_lifecycle/package.xml

@@ -2,35 +2,44 @@
 <?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>17.0.0</version>
+  <version>18.0.0</version>
   <description>Package containing a prototype for lifecycle implementation</description>
+
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
-  <maintainer email="jacob@openrobotics.org">Jacob Perron</maintainer>
+  <maintainer email="michel@ekumenlabs.com">Michel Hidalgo</maintainer>
   <maintainer email="william@openrobotics.org">William Woodall</maintainer>
+
   <license>Apache License 2.0</license>
+
+  <author email="jacob@openrobotics.org">Jacob Perron</author>
   <author email="karsten@openrobotics.org">Karsten Knese</author>
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
   <build_depend>lifecycle_msgs</build_depend>
+  <build_depend>rcl</build_depend>
   <build_depend>rclcpp</build_depend>
+  <build_depend>rcl_interfaces</build_depend>
   <build_depend>rcl_lifecycle</build_depend>
+  <build_depend>rcutils</build_depend>
+  <build_depend>rmw</build_depend>
   <build_depend>rosidl_typesupport_cpp</build_depend>
 
   <exec_depend>lifecycle_msgs</exec_depend>
+  <exec_depend>rcl</exec_depend>
   <exec_depend>rclcpp</exec_depend>
+  <exec_depend>rcl_interfaces</exec_depend>
   <exec_depend>rcl_lifecycle</exec_depend>
+  <exec_depend>rcutils</exec_depend>
+  <exec_depend>rmw</exec_depend>
   <exec_depend>rosidl_typesupport_cpp</exec_depend>
 
-  <depend>rmw</depend>
-
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <test_depend>mimick_vendor</test_depend>
   <test_depend>performance_test_fixture</test_depend>
   <test_depend>rcpputils</test_depend>
-  <test_depend>rcutils</test_depend>
   <test_depend>test_msgs</test_depend>
 
   <export>

rclcpp_lifecycle/src/lifecycle_node.cpp

@@ -14,15 +14,22 @@
 
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 
-#include <string>
+#include <chrono>
+#include <functional>
 #include <map>
 #include <memory>
-#include <vector>
+#include <stdexcept>
+#include <string>
 #include <utility>
+#include <vector>
 
 #include "lifecycle_msgs/msg/state.hpp"
 #include "lifecycle_msgs/msg/transition.hpp"
 
+#include "rcl_interfaces/msg/list_parameters_result.hpp"
+#include "rcl_interfaces/msg/parameter_descriptor.hpp"
+#include "rcl_interfaces/msg/set_parameters_result.hpp"
+
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/graph_listener.hpp"
 #include "rclcpp/logger.hpp"
@@ -78,7 +85,8 @@ LifecycleNode::LifecycleNode(
       node_topics_,
       node_graph_,
       node_services_,
-      node_logging_
+      node_logging_,
+      options.clock_type()
     )),
   node_parameters_(new rclcpp::node_interfaces::NodeParameters(
       node_base_,
@@ -528,25 +536,25 @@ LifecycleNode::register_on_error(
 }
 
 const State &
-LifecycleNode::get_current_state()
+LifecycleNode::get_current_state() const
 {
   return impl_->get_current_state();
 }
 
 std::vector<State>
-LifecycleNode::get_available_states()
+LifecycleNode::get_available_states() const
 {
   return impl_->get_available_states();
 }
 
 std::vector<Transition>
-LifecycleNode::get_available_transitions()
+LifecycleNode::get_available_transitions() const
 {
   return impl_->get_available_transitions();
 }
 
 std::vector<Transition>
-LifecycleNode::get_transition_graph()
+LifecycleNode::get_transition_graph() const
 {
   return impl_->get_transition_graph();
 }

rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp

@@ -0,0 +1,584 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <stdexcept>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "lifecycle_msgs/msg/transition_description.hpp"
+#include "lifecycle_msgs/msg/transition_event.h"  // for getting the c-typesupport
+#include "lifecycle_msgs/msg/transition_event.hpp"
+#include "lifecycle_msgs/srv/change_state.hpp"
+#include "lifecycle_msgs/srv/get_state.hpp"
+#include "lifecycle_msgs/srv/get_available_states.hpp"
+#include "lifecycle_msgs/srv/get_available_transitions.hpp"
+
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_services_interface.hpp"
+
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+
+#include "rcl/error_handling.h"
+#include "rcl/node.h"
+
+#include "rcl_lifecycle/rcl_lifecycle.h"
+#include "rcl_lifecycle/transition_map.h"
+
+#include "rcutils/logging_macros.h"
+
+#include "rmw/types.h"
+
+#include "lifecycle_node_interface_impl.hpp"
+
+namespace rclcpp_lifecycle
+{
+
+LifecycleNode::LifecycleNodeInterfaceImpl::LifecycleNodeInterfaceImpl(
+  std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> node_base_interface,
+  std::shared_ptr<rclcpp::node_interfaces::NodeServicesInterface> node_services_interface)
+: node_base_interface_(node_base_interface),
+  node_services_interface_(node_services_interface)
+{
+}
+
+LifecycleNode::LifecycleNodeInterfaceImpl::~LifecycleNodeInterfaceImpl()
+{
+  rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle();
+  rcl_ret_t ret;
+  {
+    std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
+    ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_handle);
+  }
+  if (ret != RCL_RET_OK) {
+    RCUTILS_LOG_FATAL_NAMED(
+      "rclcpp_lifecycle",
+      "failed to destroy rcl_state_machine");
+  }
+}
+
+void
+LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interface)
+{
+  rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle();
+  const rcl_node_options_t * node_options =
+    rcl_node_get_options(node_base_interface_->get_rcl_node_handle());
+  auto state_machine_options = rcl_lifecycle_get_default_state_machine_options();
+  state_machine_options.enable_com_interface = enable_communication_interface;
+  state_machine_options.allocator = node_options->allocator;
+
+  // The call to initialize the state machine takes
+  // currently five different typesupports for all publishers/services
+  // created within the RCL_LIFECYCLE structure.
+  // The publisher takes a C-Typesupport since the publishing (i.e. creating
+  // the message) is done fully in RCL.
+  // Services are handled in C++, so that it needs a C++ typesupport structure.
+  std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
+  state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine();
+  rcl_ret_t ret = rcl_lifecycle_state_machine_init(
+    &state_machine_,
+    node_handle,
+    ROSIDL_GET_MSG_TYPE_SUPPORT(lifecycle_msgs, msg, TransitionEvent),
+    rosidl_typesupport_cpp::get_service_type_support_handle<ChangeStateSrv>(),
+    rosidl_typesupport_cpp::get_service_type_support_handle<GetStateSrv>(),
+    rosidl_typesupport_cpp::get_service_type_support_handle<GetAvailableStatesSrv>(),
+    rosidl_typesupport_cpp::get_service_type_support_handle<GetAvailableTransitionsSrv>(),
+    rosidl_typesupport_cpp::get_service_type_support_handle<GetAvailableTransitionsSrv>(),
+    &state_machine_options);
+  if (ret != RCL_RET_OK) {
+    throw std::runtime_error(
+            std::string("Couldn't initialize state machine for node ") +
+            node_base_interface_->get_name());
+  }
+
+  current_state_ = State(state_machine_.current_state);
+
+  if (enable_communication_interface) {
+    { // change_state
+      auto cb = std::bind(
+        &LifecycleNode::LifecycleNodeInterfaceImpl::on_change_state, this,
+        std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
+      rclcpp::AnyServiceCallback<ChangeStateSrv> any_cb;
+      any_cb.set(std::move(cb));
+
+      srv_change_state_ = std::make_shared<rclcpp::Service<ChangeStateSrv>>(
+        node_base_interface_->get_shared_rcl_node_handle(),
+        &state_machine_.com_interface.srv_change_state,
+        any_cb);
+      node_services_interface_->add_service(
+        std::dynamic_pointer_cast<rclcpp::ServiceBase>(srv_change_state_),
+        nullptr);
+    }
+
+    { // get_state
+      auto cb = std::bind(
+        &LifecycleNode::LifecycleNodeInterfaceImpl::on_get_state, this,
+        std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
+      rclcpp::AnyServiceCallback<GetStateSrv> any_cb;
+      any_cb.set(std::move(cb));
+
+      srv_get_state_ = std::make_shared<rclcpp::Service<GetStateSrv>>(
+        node_base_interface_->get_shared_rcl_node_handle(),
+        &state_machine_.com_interface.srv_get_state,
+        any_cb);
+      node_services_interface_->add_service(
+        std::dynamic_pointer_cast<rclcpp::ServiceBase>(srv_get_state_),
+        nullptr);
+    }
+
+    { // get_available_states
+      auto cb = std::bind(
+        &LifecycleNode::LifecycleNodeInterfaceImpl::on_get_available_states, this,
+        std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
+      rclcpp::AnyServiceCallback<GetAvailableStatesSrv> any_cb;
+      any_cb.set(std::move(cb));
+
+      srv_get_available_states_ = std::make_shared<rclcpp::Service<GetAvailableStatesSrv>>(
+        node_base_interface_->get_shared_rcl_node_handle(),
+        &state_machine_.com_interface.srv_get_available_states,
+        any_cb);
+      node_services_interface_->add_service(
+        std::dynamic_pointer_cast<rclcpp::ServiceBase>(srv_get_available_states_),
+        nullptr);
+    }
+
+    { // get_available_transitions
+      auto cb = std::bind(
+        &LifecycleNode::LifecycleNodeInterfaceImpl::on_get_available_transitions, this,
+        std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
+      rclcpp::AnyServiceCallback<GetAvailableTransitionsSrv> any_cb;
+      any_cb.set(std::move(cb));
+
+      srv_get_available_transitions_ =
+        std::make_shared<rclcpp::Service<GetAvailableTransitionsSrv>>(
+        node_base_interface_->get_shared_rcl_node_handle(),
+        &state_machine_.com_interface.srv_get_available_transitions,
+        any_cb);
+      node_services_interface_->add_service(
+        std::dynamic_pointer_cast<rclcpp::ServiceBase>(srv_get_available_transitions_),
+        nullptr);
+    }
+
+    { // get_transition_graph
+      auto cb = std::bind(
+        &LifecycleNode::LifecycleNodeInterfaceImpl::on_get_transition_graph, this,
+        std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
+      rclcpp::AnyServiceCallback<GetAvailableTransitionsSrv> any_cb;
+      any_cb.set(std::move(cb));
+
+      srv_get_transition_graph_ =
+        std::make_shared<rclcpp::Service<GetAvailableTransitionsSrv>>(
+        node_base_interface_->get_shared_rcl_node_handle(),
+        &state_machine_.com_interface.srv_get_transition_graph,
+        any_cb);
+      node_services_interface_->add_service(
+        std::dynamic_pointer_cast<rclcpp::ServiceBase>(srv_get_transition_graph_),
+        nullptr);
+    }
+  }
+}
+
+bool
+LifecycleNode::LifecycleNodeInterfaceImpl::register_callback(
+  std::uint8_t lifecycle_transition,
+  std::function<node_interfaces::LifecycleNodeInterface::CallbackReturn(const State &)> & cb)
+{
+  cb_map_[lifecycle_transition] = cb;
+  return true;
+}
+
+void
+LifecycleNode::LifecycleNodeInterfaceImpl::on_change_state(
+  const std::shared_ptr<rmw_request_id_t> header,
+  const std::shared_ptr<ChangeStateSrv::Request> req,
+  std::shared_ptr<ChangeStateSrv::Response> resp)
+{
+  (void)header;
+  std::uint8_t transition_id;
+  {
+    std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
+    if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
+      throw std::runtime_error("Can't get state. State machine is not initialized.");
+    }
+
+    transition_id = req->transition.id;
+    // if there's a label attached to the request,
+    // we check the transition attached to this label.
+    // we further can't compare the id of the looked up transition
+    // because ros2 service call defaults all intergers to zero.
+    // that means if we call ros2 service call ... {transition: {label: shutdown}}
+    // the id of the request is 0 (zero) whereas the id from the lookup up transition
+    // can be different.
+    // the result of this is that the label takes presedence of the id.
+    if (req->transition.label.size() != 0) {
+      auto rcl_transition = rcl_lifecycle_get_transition_by_label(
+        state_machine_.current_state, req->transition.label.c_str());
+      if (rcl_transition == nullptr) {
+        resp->success = false;
+        return;
+      }
+      transition_id = static_cast<std::uint8_t>(rcl_transition->id);
+    }
+  }
+
+  node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code;
+  auto ret = change_state(transition_id, cb_return_code);
+  (void) ret;
+  // TODO(karsten1987): Lifecycle msgs have to be extended to keep both returns
+  // 1. return is the actual transition
+  // 2. return is whether an error occurred or not
+  resp->success =
+    (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS);
+}
+
+void
+LifecycleNode::LifecycleNodeInterfaceImpl::on_get_state(
+  const std::shared_ptr<rmw_request_id_t> header,
+  const std::shared_ptr<GetStateSrv::Request> req,
+  std::shared_ptr<GetStateSrv::Response> resp) const
+{
+  (void)header;
+  (void)req;
+  std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
+  if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
+    throw std::runtime_error(
+            "Can't get state. State machine is not initialized.");
+  }
+  resp->current_state.id = static_cast<uint8_t>(state_machine_.current_state->id);
+  resp->current_state.label = state_machine_.current_state->label;
+}
+
+void
+LifecycleNode::LifecycleNodeInterfaceImpl::on_get_available_states(
+  const std::shared_ptr<rmw_request_id_t> header,
+  const std::shared_ptr<GetAvailableStatesSrv::Request> req,
+  std::shared_ptr<GetAvailableStatesSrv::Response> resp) const
+{
+  (void)header;
+  (void)req;
+  std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
+  if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
+    throw std::runtime_error(
+            "Can't get available states. State machine is not initialized.");
+  }
+
+  resp->available_states.resize(state_machine_.transition_map.states_size);
+  for (unsigned int i = 0; i < state_machine_.transition_map.states_size; ++i) {
+    resp->available_states[i].id =
+      static_cast<uint8_t>(state_machine_.transition_map.states[i].id);
+    resp->available_states[i].label =
+      static_cast<std::string>(state_machine_.transition_map.states[i].label);
+  }
+}
+
+void
+LifecycleNode::LifecycleNodeInterfaceImpl::on_get_available_transitions(
+  const std::shared_ptr<rmw_request_id_t> header,
+  const std::shared_ptr<GetAvailableTransitionsSrv::Request> req,
+  std::shared_ptr<GetAvailableTransitionsSrv::Response> resp) const
+{
+  (void)header;
+  (void)req;
+  std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
+  if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
+    throw std::runtime_error(
+            "Can't get available transitions. State machine is not initialized.");
+  }
+
+  resp->available_transitions.resize(state_machine_.current_state->valid_transition_size);
+  for (unsigned int i = 0; i < state_machine_.current_state->valid_transition_size; ++i) {
+    lifecycle_msgs::msg::TransitionDescription & trans_desc = resp->available_transitions[i];
+
+    auto rcl_transition = state_machine_.current_state->valid_transitions[i];
+    trans_desc.transition.id = static_cast<uint8_t>(rcl_transition.id);
+    trans_desc.transition.label = rcl_transition.label;
+    trans_desc.start_state.id = static_cast<uint8_t>(rcl_transition.start->id);
+    trans_desc.start_state.label = rcl_transition.start->label;
+    trans_desc.goal_state.id = static_cast<uint8_t>(rcl_transition.goal->id);
+    trans_desc.goal_state.label = rcl_transition.goal->label;
+  }
+}
+
+void
+LifecycleNode::LifecycleNodeInterfaceImpl::on_get_transition_graph(
+  const std::shared_ptr<rmw_request_id_t> header,
+  const std::shared_ptr<GetAvailableTransitionsSrv::Request> req,
+  std::shared_ptr<GetAvailableTransitionsSrv::Response> resp) const
+{
+  (void)header;
+  (void)req;
+  std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
+  if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
+    throw std::runtime_error(
+            "Can't get available transitions. State machine is not initialized.");
+  }
+
+  resp->available_transitions.resize(state_machine_.transition_map.transitions_size);
+  for (unsigned int i = 0; i < state_machine_.transition_map.transitions_size; ++i) {
+    lifecycle_msgs::msg::TransitionDescription & trans_desc = resp->available_transitions[i];
+
+    auto rcl_transition = state_machine_.transition_map.transitions[i];
+    trans_desc.transition.id = static_cast<uint8_t>(rcl_transition.id);
+    trans_desc.transition.label = rcl_transition.label;
+    trans_desc.start_state.id = static_cast<uint8_t>(rcl_transition.start->id);
+    trans_desc.start_state.label = rcl_transition.start->label;
+    trans_desc.goal_state.id = static_cast<uint8_t>(rcl_transition.goal->id);
+    trans_desc.goal_state.label = rcl_transition.goal->label;
+  }
+}
+
+const State &
+LifecycleNode::LifecycleNodeInterfaceImpl::get_current_state() const
+{
+  return current_state_;
+}
+
+std::vector<State>
+LifecycleNode::LifecycleNodeInterfaceImpl::get_available_states() const
+{
+  std::vector<State> states;
+  std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
+  states.reserve(state_machine_.transition_map.states_size);
+
+  for (unsigned int i = 0; i < state_machine_.transition_map.states_size; ++i) {
+    states.emplace_back(&state_machine_.transition_map.states[i]);
+  }
+  return states;
+}
+
+std::vector<Transition>
+LifecycleNode::LifecycleNodeInterfaceImpl::get_available_transitions() const
+{
+  std::vector<Transition> transitions;
+  std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
+  transitions.reserve(state_machine_.current_state->valid_transition_size);
+
+  for (unsigned int i = 0; i < state_machine_.current_state->valid_transition_size; ++i) {
+    transitions.emplace_back(&state_machine_.current_state->valid_transitions[i]);
+  }
+  return transitions;
+}
+
+std::vector<Transition>
+LifecycleNode::LifecycleNodeInterfaceImpl::get_transition_graph() const
+{
+  std::vector<Transition> transitions;
+  std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
+  transitions.reserve(state_machine_.transition_map.transitions_size);
+
+  for (unsigned int i = 0; i < state_machine_.transition_map.transitions_size; ++i) {
+    transitions.emplace_back(&state_machine_.transition_map.transitions[i]);
+  }
+  return transitions;
+}
+
+rcl_ret_t
+LifecycleNode::LifecycleNodeInterfaceImpl::change_state(
+  std::uint8_t transition_id,
+  node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code)
+{
+  constexpr bool publish_update = true;
+  State initial_state;
+  unsigned int current_state_id;
+
+  {
+    std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
+    if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR(
+        "Unable to change state for state machine for %s: %s",
+        node_base_interface_->get_name(), rcl_get_error_string().str);
+      return RCL_RET_ERROR;
+    }
+
+    // keep the initial state to pass to a transition callback
+    initial_state = State(state_machine_.current_state);
+
+    if (
+      rcl_lifecycle_trigger_transition_by_id(
+        &state_machine_, transition_id, publish_update) != RCL_RET_OK)
+    {
+      RCUTILS_LOG_ERROR(
+        "Unable to start transition %u from current state %s: %s",
+        transition_id, state_machine_.current_state->label, rcl_get_error_string().str);
+      rcutils_reset_error();
+      return RCL_RET_ERROR;
+    }
+    current_state_id = state_machine_.current_state->id;
+  }
+
+  // Update the internal current_state_
+  current_state_ = State(state_machine_.current_state);
+
+  auto get_label_for_return_code =
+    [](node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code) -> const char *{
+      auto cb_id = static_cast<uint8_t>(cb_return_code);
+      if (cb_id == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS) {
+        return rcl_lifecycle_transition_success_label;
+      } else if (cb_id == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE) {
+        return rcl_lifecycle_transition_failure_label;
+      }
+      return rcl_lifecycle_transition_error_label;
+    };
+
+  cb_return_code = execute_callback(current_state_id, initial_state);
+  auto transition_label = get_label_for_return_code(cb_return_code);
+
+  {
+    std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
+    if (
+      rcl_lifecycle_trigger_transition_by_label(
+        &state_machine_, transition_label, publish_update) != RCL_RET_OK)
+    {
+      RCUTILS_LOG_ERROR(
+        "Failed to finish transition %u. Current state is now: %s (%s)",
+        transition_id, state_machine_.current_state->label, rcl_get_error_string().str);
+      rcutils_reset_error();
+      return RCL_RET_ERROR;
+    }
+    current_state_id = state_machine_.current_state->id;
+  }
+
+  // Update the internal current_state_
+  current_state_ = State(state_machine_.current_state);
+
+  // error handling ?!
+  // TODO(karsten1987): iterate over possible ret value
+  if (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) {
+    RCUTILS_LOG_WARN("Error occurred while doing error handling.");
+
+    auto error_cb_code = execute_callback(current_state_id, initial_state);
+    auto error_cb_label = get_label_for_return_code(error_cb_code);
+    std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
+    if (
+      rcl_lifecycle_trigger_transition_by_label(
+        &state_machine_, error_cb_label, publish_update) != RCL_RET_OK)
+    {
+      RCUTILS_LOG_ERROR("Failed to call cleanup on error state: %s", rcl_get_error_string().str);
+      rcutils_reset_error();
+      return RCL_RET_ERROR;
+    }
+  }
+
+  // Update the internal current_state_
+  current_state_ = State(state_machine_.current_state);
+
+  // This true holds in both cases where the actual callback
+  // was successful or not, since at this point we have a valid transistion
+  // to either a new primary state or error state
+  return RCL_RET_OK;
+}
+
+node_interfaces::LifecycleNodeInterface::CallbackReturn
+LifecycleNode::LifecycleNodeInterfaceImpl::execute_callback(
+  unsigned int cb_id, const State & previous_state) const
+{
+  // in case no callback was attached, we forward directly
+  auto cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+
+  auto it = cb_map_.find(static_cast<uint8_t>(cb_id));
+  if (it != cb_map_.end()) {
+    auto callback = it->second;
+    try {
+      cb_success = callback(State(previous_state));
+    } catch (const std::exception & e) {
+      RCUTILS_LOG_ERROR("Caught exception in callback for transition %d", it->first);
+      RCUTILS_LOG_ERROR("Original error: %s", e.what());
+      cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
+    }
+  }
+  return cb_success;
+}
+
+const State & LifecycleNode::LifecycleNodeInterfaceImpl::trigger_transition(
+  const char * transition_label)
+{
+  node_interfaces::LifecycleNodeInterface::CallbackReturn error;
+  return trigger_transition(transition_label, error);
+}
+
+const State & LifecycleNode::LifecycleNodeInterfaceImpl::trigger_transition(
+  const char * transition_label,
+  node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code)
+{
+  const rcl_lifecycle_transition_t * transition;
+  {
+    std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
+
+    transition =
+      rcl_lifecycle_get_transition_by_label(state_machine_.current_state, transition_label);
+  }
+  if (transition) {
+    change_state(static_cast<uint8_t>(transition->id), cb_return_code);
+  }
+  return get_current_state();
+}
+
+const State &
+LifecycleNode::LifecycleNodeInterfaceImpl::trigger_transition(uint8_t transition_id)
+{
+  node_interfaces::LifecycleNodeInterface::CallbackReturn error;
+  change_state(transition_id, error);
+  (void) error;
+  return get_current_state();
+}
+
+const State &
+LifecycleNode::LifecycleNodeInterfaceImpl::trigger_transition(
+  uint8_t transition_id,
+  node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code)
+{
+  change_state(transition_id, cb_return_code);
+  return get_current_state();
+}
+
+void
+LifecycleNode::LifecycleNodeInterfaceImpl::add_managed_entity(
+  std::weak_ptr<rclcpp_lifecycle::ManagedEntityInterface> managed_entity)
+{
+  weak_managed_entities_.push_back(managed_entity);
+}
+
+void
+LifecycleNode::LifecycleNodeInterfaceImpl::add_timer_handle(
+  std::shared_ptr<rclcpp::TimerBase> timer)
+{
+  weak_timers_.push_back(timer);
+}
+
+void
+LifecycleNode::LifecycleNodeInterfaceImpl::on_activate() const
+{
+  for (const auto & weak_entity : weak_managed_entities_) {
+    auto entity = weak_entity.lock();
+    if (entity) {
+      entity->on_activate();
+    }
+  }
+}
+
+void
+LifecycleNode::LifecycleNodeInterfaceImpl::on_deactivate() const
+{
+  for (const auto & weak_entity : weak_managed_entities_) {
+    auto entity = weak_entity.lock();
+    if (entity) {
+      entity->on_deactivate();
+    }
+  }
+}
+
+}  // namespace rclcpp_lifecycle