WIP: Reorganize test CMakeLists and add benchmark dir

* Clear members for StaticExecutorEntitiesCollector to avoid shared_ptr dependency

Signed-off-by: Chen Lihui <Lihui.Chen@sony.com>
Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>
Co-authored-by: Chris Lalancette <clalancette@openrobotics.org>

rclcpp/CMakeLists.txt

@@ -222,6 +222,12 @@ if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
 
+  # For benchmark tests. Currently there is a bug where these targets can't be
+  # found if these calls are added to the CMakeLists.txt in test/rclcpp/benchmark
+  find_package(benchmark REQUIRED)
+  find_package(osrf_testing_tools_cpp REQUIRED)
+  find_package(performance_test_fixture REQUIRED)
+
   add_subdirectory(test)
 endif()
 

rclcpp/QUALITY_DECLARATION.md

@@ -123,6 +123,8 @@ Changes are required to make a best effort to keep or increase coverage before b
 
 Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_coverage/lastSuccessfulBuild/cobertura/src_ros2_rclcpp_rclcpp_src_rclcpp/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://index.ros.org/doc/ros2/Contributing/ROS-2-On-boarding-Guide/#note-on-coverage-runs).
 
+`rclcpp` has a line coverage `>= 95%`, which is calculated over all directories within `rclcpp` with the exception of the `experimental` directory.
+
 ### Performance [4.iv]
 
 It is not yet defined if this package requires performance testing and how addresses this topic.

rclcpp/include/rclcpp/callback_group.hpp

@@ -71,7 +71,7 @@ public:
    *   - will not be run at the same time as other callbacks in their group
    *   - but must run at the same time as callbacks in other groups
    *
-   * Additiionally, callback groups have a property which determines whether or
+   * Additionally, callback groups have a property which determines whether or
    * not they are added to an executor with their associated node automatically.
    * When creating a callback group the automatically_add_to_executor_with_node
    * argument determines this behavior, and if true it will cause the newly

rclcpp/include/rclcpp/create_subscription.hpp

@@ -117,8 +117,14 @@ create_subscription(
       rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>
       >(node_topics->get_node_base_interface()->get_name(), publisher);
 
-    auto sub_call_back = [subscription_topic_stats]() {
-        subscription_topic_stats->publish_message();
+    std::weak_ptr<
+      rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>
+    > weak_subscription_topic_stats(subscription_topic_stats);
+    auto sub_call_back = [weak_subscription_topic_stats]() {
+        auto subscription_topic_stats = weak_subscription_topic_stats.lock();
+        if (subscription_topic_stats) {
+          subscription_topic_stats->publish_message();
+        }
       };
 
     auto node_timer_interface = node_topics->get_node_timers_interface();

rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp

@@ -64,29 +64,18 @@ public:
   void
   init(
     rcl_wait_set_t * p_wait_set,
-    rclcpp::memory_strategy::MemoryStrategy::SharedPtr & memory_strategy,
+    rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy,
     rcl_guard_condition_t * executor_guard_condition);
 
+  /// Finalize StaticExecutorEntitiesCollector to clear resources
+  RCLCPP_PUBLIC
+  void
+  fini();
+
   RCLCPP_PUBLIC
   void
   execute() override;
 
-  RCLCPP_PUBLIC
-  void
-  fill_memory_strategy();
-
-  RCLCPP_PUBLIC
-  void
-  fill_executable_list();
-
-  /// Function to reallocate space for entities in the wait set.
-  /**
-   * \throws std::runtime_error if wait set couldn't be cleared or resized.
-   */
-  RCLCPP_PUBLIC
-  void
-  prepare_wait_set();
-
   /// Function to add_handles_to_wait_set and wait for work and
   /**
    * block until the wait set is ready or until the timeout has been exceeded.
@@ -282,6 +271,20 @@ public:
   rclcpp::Waitable::SharedPtr
   get_waitable(size_t i) {return exec_list_.waitable[i];}
 
+private:
+  /// Function to reallocate space for entities in the wait set.
+  /**
+   * \throws std::runtime_error if wait set couldn't be cleared or resized.
+   */
+  void
+  prepare_wait_set();
+
+  void
+  fill_executable_list();
+
+  void
+  fill_memory_strategy();
+
   /// Return true if the node belongs to the collector
   /**
    * \param[in] group_ptr a node base interface shared pointer
@@ -290,22 +293,19 @@ public:
   bool
   has_node(
     const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-    WeakCallbackGroupsToNodesMap weak_groups_to_nodes) const;
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) const;
 
-private:
   /// Add all callback groups that can be automatically added by any executor
   /// and is not already associated with an executor from nodes
   /// that are associated with executor
   /**
    * \see rclcpp::Executor::add_callback_groups_from_nodes_associated_to_executor()
    */
-  RCLCPP_PUBLIC
   void
   add_callback_groups_from_nodes_associated_to_executor();
 
-  RCLCPP_PUBLIC
   void
-  fill_executable_list_from_map(WeakCallbackGroupsToNodesMap weak_groups_to_nodes);
+  fill_executable_list_from_map(const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   /// Memory strategy: an interface for handling user-defined memory allocation strategies.
   rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy_;

rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp

@@ -193,6 +193,7 @@ public:
     std::chrono::nanoseconds timeout_left = timeout_ns;
 
     entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_);
+    RCLCPP_SCOPE_EXIT(entities_collector_->fini());
 
     while (rclcpp::ok(this->context_)) {
       // Do one set of work.

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -102,6 +102,16 @@ public:
 #endif
   }
 
+  ~SubscriptionIntraProcess()
+  {
+    if (rcl_guard_condition_fini(&gc_) != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "Failed to destroy guard condition: %s",
+        rcutils_get_error_string().str);
+    }
+  }
+
   bool
   is_ready(rcl_wait_set_t * wait_set)
   {

rclcpp/include/rclcpp/future_return_code.hpp

@@ -42,20 +42,6 @@ RCLCPP_PUBLIC
 std::string
 to_string(const FutureReturnCode & future_return_code);
 
-namespace executor
-{
-
-using FutureReturnCode [[deprecated("use rclcpp::FutureReturnCode instead")]] = FutureReturnCode;
-
-[[deprecated("use rclcpp::to_string(const rclcpp::FutureReturnCode &) instead")]]
-inline
-std::string
-to_string(const rclcpp::FutureReturnCode & future_return_code)
-{
-  return rclcpp::to_string(future_return_code);
-}
-
-}  // namespace executor
 }  // namespace rclcpp
 
 #endif  // RCLCPP__FUTURE_RETURN_CODE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp

@@ -24,6 +24,7 @@
 #include "rclcpp/node_interfaces/node_services_interface.hpp"
 #include "rclcpp/node_interfaces/node_time_source_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/time_source.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -46,7 +47,8 @@ public:
     rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
     rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
     rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
-    rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters
+    rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters,
+    const rclcpp::QoS & qos = rclcpp::RosoutQoS()
   );
 
   RCLCPP_PUBLIC

rclcpp/include/rclcpp/node_options.hpp

@@ -46,6 +46,8 @@ public:
    *   - enable_topic_statistics = false
    *   - start_parameter_services = true
    *   - start_parameter_event_publisher = true
+   *   - clock_qos = rclcpp::ClockQoS()
+   *   - rosout_qos = rclcpp::RosoutQoS()
    *   - parameter_event_qos = rclcpp::ParameterEventQoS
    *     - with history setting and depth from rmw_qos_profile_parameter_events
    *   - parameter_event_publisher_options = rclcpp::PublisherOptionsBase
@@ -243,6 +245,19 @@ public:
   NodeOptions &
   start_parameter_event_publisher(bool start_parameter_event_publisher);
 
+  /// Return a reference to the clock QoS.
+  RCLCPP_PUBLIC
+  const rclcpp::QoS &
+  clock_qos() const;
+
+  /// Set the clock QoS.
+  /**
+   * The QoS settings to be used for the publisher on /clock topic, if enabled.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  clock_qos(const rclcpp::QoS & clock_qos);
+
   /// Return a reference to the parameter_event_qos QoS.
   RCLCPP_PUBLIC
   const rclcpp::QoS &
@@ -256,6 +271,19 @@ public:
   NodeOptions &
   parameter_event_qos(const rclcpp::QoS & parameter_event_qos);
 
+  /// Return a reference to the rosout QoS.
+  RCLCPP_PUBLIC
+  const rclcpp::QoS &
+  rosout_qos() const;
+
+  /// Set the rosout QoS.
+  /**
+   * The QoS settings to be used for the publisher on /rosout topic, if enabled.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  rosout_qos(const rclcpp::QoS & rosout_qos);
+
   /// Return a reference to the parameter_event_publisher_options.
   RCLCPP_PUBLIC
   const rclcpp::PublisherOptionsBase &
@@ -354,10 +382,14 @@ private:
 
   bool start_parameter_event_publisher_ {true};
 
+  rclcpp::QoS clock_qos_ = rclcpp::ClockQoS();
+
   rclcpp::QoS parameter_event_qos_ = rclcpp::ParameterEventsQoS(
     rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)
   );
 
+  rclcpp::QoS rosout_qos_ = rclcpp::RosoutQoS();
+
   rclcpp::PublisherOptionsBase parameter_event_publisher_options_ = rclcpp::PublisherOptionsBase();
 
   bool allow_undeclared_parameters_ {false};

rclcpp/include/rclcpp/qos.hpp

@@ -19,6 +19,7 @@
 
 #include "rclcpp/duration.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rcl/logging_rosout.h"
 #include "rmw/incompatible_qos_events_statuses.h"
 #include "rmw/qos_profiles.h"
 #include "rmw/types.h"
@@ -26,6 +27,7 @@
 namespace rclcpp
 {
 
+RCLCPP_PUBLIC
 std::string qos_policy_name_from_kind(rmw_qos_policy_kind_t policy_kind);
 
 /// QoS initialization values, cannot be created directly, use KeepAll or KeepLast instead.
@@ -161,6 +163,26 @@ bool operator==(const QoS & left, const QoS & right);
 RCLCPP_PUBLIC
 bool operator!=(const QoS & left, const QoS & right);
 
+/**
+ * Clock QoS class
+ *    - History: Keep last,
+ *    - Depth: 1,
+ *    - Reliability: Best effort,
+ *    - Durability: Volatile,
+ *    - Deadline: Default,
+ *    - Lifespan: Default,
+ *    - Liveliness: System default,
+ *    - Liveliness lease duration: default,
+ *    - avoid ros namespace conventions: false
+ */
+class RCLCPP_PUBLIC ClockQoS : public QoS
+{
+public:
+  explicit
+  ClockQoS(
+    const QoSInitialization & qos_initialization = KeepLast(1));
+};
+
 /**
  * Sensor Data QoS class
  *    - History: Keep last,
@@ -249,6 +271,28 @@ public:
   ));
 };
 
+/**
+ * Rosout QoS class
+ *    - History: Keep last,
+ *    - Depth: 1000,
+ *    - Reliability: Reliable,
+ *    - Durability: TRANSIENT_LOCAL,
+ *    - Deadline: Default,
+ *    - Lifespan: {10, 0},
+ *    - Liveliness: System default,
+ *    - Liveliness lease duration: default,
+ *    - Avoid ros namespace conventions: false
+ */
+class RCLCPP_PUBLIC RosoutQoS : public QoS
+{
+public:
+  explicit
+  RosoutQoS(
+    const QoSInitialization & rosout_qos_initialization = (
+      QoSInitialization::from_rmw(rcl_qos_profile_rosout_default)
+  ));
+};
+
 /**
  * System defaults QoS class
  *    - History: System default,

rclcpp/include/rclcpp/time.hpp

@@ -218,6 +218,7 @@ private:
 /**
  * \throws std::overflow_error if addition leads to overflow
  */
+RCLCPP_PUBLIC
 Time
 operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs);
 

rclcpp/include/rclcpp/time_source.hpp

@@ -40,16 +40,19 @@ public:
    * The node will be attached to the time source.
    *
    * \param node std::shared pointer to a initialized node
+   * \param qos QoS that will be used when creating a `/clock` subscription.
    */
   RCLCPP_PUBLIC
-  explicit TimeSource(rclcpp::Node::SharedPtr node);
+  explicit TimeSource(rclcpp::Node::SharedPtr node, const rclcpp::QoS & qos = rclcpp::ClockQoS());
 
   /// Empty constructor
   /**
    * An Empty TimeSource class
+   *
+   * \param qos QoS that will be used when creating a `/clock` subscription.
    */
   RCLCPP_PUBLIC
-  TimeSource();
+  explicit TimeSource(const rclcpp::QoS & qos = rclcpp::ClockQoS());
 
   /// Attack node to the time source.
   /**
@@ -114,11 +117,14 @@ private:
   // Store (and update on node attach) logger for logging.
   Logger logger_;
 
+  // QoS of the clock subscription.
+  rclcpp::QoS qos_;
+
   // The subscription for the clock callback
   using MessageT = rosgraph_msgs::msg::Clock;
   using Alloc = std::allocator<void>;
   using SubscriptionT = rclcpp::Subscription<MessageT, Alloc>;
-  std::shared_ptr<SubscriptionT> clock_subscription_;
+  std::shared_ptr<SubscriptionT> clock_subscription_{nullptr};
   std::mutex clock_sub_lock_;
 
   // The clock callback itself
@@ -155,7 +161,7 @@ private:
 
   // Local storage of validity of ROS time
   // This is needed when new clocks are added.
-  bool ros_time_active_;
+  bool ros_time_active_{false};
   // Last set message to be passed to newly registered clocks
   rosgraph_msgs::msg::Clock::SharedPtr last_msg_set_;
 
@@ -164,7 +170,7 @@ private:
   // A vector to store references to associated clocks.
   std::vector<rclcpp::Clock::SharedPtr> associated_clocks_;
   // A handler for the use_sim_time parameter callback.
-  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr sim_time_cb_handler_ = nullptr;
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr sim_time_cb_handler_{nullptr};
 };
 
 }  // namespace rclcpp

rclcpp/package.xml

@@ -4,8 +4,11 @@
   <name>rclcpp</name>
   <version>5.0.0</version>
   <description>The ROS client library in C++.</description>
-  <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
+  <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
+  <maintainer email="mabel@openrobotics.org">Mabel Zhang</maintainer>
+  <maintainer email="william@openrobotics.org">William Woodall</maintainer>
   <license>Apache License 2.0</license>
+  <author email="dthomas@openrobotics.org">Dirk Thomas</author>
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
@@ -36,6 +39,7 @@
   <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>rmw</test_depend>
   <test_depend>rmw_implementation_cmake</test_depend>
   <test_depend>rosidl_default_generators</test_depend>

rclcpp/src/rclcpp/context.cpp

@@ -199,13 +199,17 @@ Context::init(
     throw rclcpp::ContextAlreadyInitialized();
   }
   this->clean_up();
-  rcl_context_.reset(new rcl_context_t, __delete_context);
-  *rcl_context_.get() = rcl_get_zero_initialized_context();
-  rcl_ret_t ret = rcl_init(argc, argv, init_options.get_rcl_init_options(), rcl_context_.get());
+  rcl_context_t * context = new rcl_context_t;
+  if (!context) {
+    throw std::runtime_error("failed to allocate memory for rcl context");
+  }
+  *context = rcl_get_zero_initialized_context();
+  rcl_ret_t ret = rcl_init(argc, argv, init_options.get_rcl_init_options(), context);
   if (RCL_RET_OK != ret) {
-    rcl_context_.reset();
+    delete context;
     rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialize rcl");
   }
+  rcl_context_.reset(context, __delete_context);
 
   if (init_options.auto_initialize_logging()) {
     logging_mutex_ = get_global_logging_mutex();

rclcpp/src/rclcpp/executor.cpp

@@ -79,13 +79,13 @@ Executor::Executor(const rclcpp::ExecutorOptions & options)
         "failed to destroy guard condition: %s", rcl_get_error_string().str);
       rcl_reset_error();
     }
-    throw std::runtime_error("Failed to create wait set in Executor constructor");
+    throw_from_rcl_error(ret, "Failed to create wait set in Executor constructor");
   }
 }
 
 Executor::~Executor()
 {
-  // Disassocate all callback groups.
+  // Disassociate all callback groups.
   for (auto & pair : weak_groups_to_nodes_) {
     auto group = pair.first.lock();
     if (group) {
@@ -93,7 +93,7 @@ Executor::~Executor()
       has_executor.store(false);
     }
   }
-  // Disassocate all nodes.
+  // Disassociate all nodes.
   std::for_each(
     weak_nodes_.begin(), weak_nodes_.end(), []
       (rclcpp::node_interfaces::NodeBaseInterface::WeakPtr weak_node_ptr) {
@@ -219,8 +219,9 @@ Executor::add_callback_group_to_map(
     weak_nodes_to_guard_conditions_[node_weak_ptr] = node_ptr->get_notify_guard_condition();
     if (notify) {
       // Interrupt waiting to handle new node
-      if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-        throw std::runtime_error(rcl_get_error_string().str);
+      rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_);
+      if (ret != RCL_RET_OK) {
+        throw_from_rcl_error(ret, "Failed to trigger guard condition on callback group add");
       }
     }
     // Add the node's notify condition to the guard condition handles
@@ -293,8 +294,9 @@ Executor::remove_callback_group_from_map(
     rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr);
     weak_nodes_to_guard_conditions_.erase(node_weak_ptr);
     if (notify) {
-      if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-        throw std::runtime_error(rcl_get_error_string().str);
+      rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_);
+      if (ret != RCL_RET_OK) {
+        throw_from_rcl_error(ret, "Failed to trigger guard condition on callback group remove");
       }
     }
     std::unique_lock<std::mutex> lock(memory_strategy_mutex_);
@@ -470,8 +472,9 @@ void
 Executor::cancel()
 {
   spinning.store(false);
-  if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-    throw std::runtime_error(rcl_get_error_string().str);
+  rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_);
+  if (ret != RCL_RET_OK) {
+    throw_from_rcl_error(ret, "Failed to trigger guard condition in cancel");
   }
 }
 
@@ -509,8 +512,9 @@ Executor::execute_any_executable(AnyExecutable & any_exec)
   any_exec.callback_group->can_be_taken_from().store(true);
   // Wake the wait, because it may need to be recalculated or work that
   // was previously blocked is now available.
-  if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-    throw std::runtime_error(rcl_get_error_string().str);
+  rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_);
+  if (ret != RCL_RET_OK) {
+    throw_from_rcl_error(ret, "Failed to trigger guard condition from execute_any_executable");
   }
 }
 
@@ -696,19 +700,19 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
     }
 
     // clear wait set
-    if (rcl_wait_set_clear(&wait_set_) != RCL_RET_OK) {
-      throw std::runtime_error("Couldn't clear wait set");
+    rcl_ret_t ret = rcl_wait_set_clear(&wait_set_);
+    if (ret != RCL_RET_OK) {
+      throw_from_rcl_error(ret, "Couldn't clear wait set");
     }
 
     // The size of waitables are accounted for in size of the other entities
-    rcl_ret_t ret = rcl_wait_set_resize(
+    ret = rcl_wait_set_resize(
       &wait_set_, memory_strategy_->number_of_ready_subscriptions(),
       memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(),
       memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services(),
       memory_strategy_->number_of_ready_events());
     if (RCL_RET_OK != ret) {
-      throw std::runtime_error(
-              std::string("Couldn't resize the wait set : ") + rcl_get_error_string().str);
+      throw_from_rcl_error(ret, "Couldn't resize the wait set");
     }
 
     if (!memory_strategy_->add_handles_to_wait_set(&wait_set_)) {

rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp

@@ -28,15 +28,15 @@ using rclcpp::executors::StaticExecutorEntitiesCollector;
 
 StaticExecutorEntitiesCollector::~StaticExecutorEntitiesCollector()
 {
-  // Disassocate all callback groups and thus nodes.
-  for (auto & pair : weak_groups_associated_with_executor_to_nodes_) {
+  // Disassociate all callback groups and thus nodes.
+  for (const auto & pair : weak_groups_associated_with_executor_to_nodes_) {
     auto group = pair.first.lock();
     if (group) {
       std::atomic_bool & has_executor = group->get_associated_with_executor_atomic();
       has_executor.store(false);
     }
   }
-  for (auto & pair : weak_groups_to_nodes_associated_with_executor_) {
+  for (const auto & pair : weak_groups_to_nodes_associated_with_executor_) {
     auto group = pair.first.lock();
     if (group) {
       std::atomic_bool & has_executor = group->get_associated_with_executor_atomic();
@@ -44,7 +44,7 @@ StaticExecutorEntitiesCollector::~StaticExecutorEntitiesCollector()
     }
   }
   // Disassociate all nodes
-  for (auto & weak_node : weak_nodes_) {
+  for (const auto & weak_node : weak_nodes_) {
     auto node = weak_node.lock();
     if (node) {
       std::atomic_bool & has_executor = node->get_associated_with_executor_atomic();
@@ -61,7 +61,7 @@ StaticExecutorEntitiesCollector::~StaticExecutorEntitiesCollector()
 void
 StaticExecutorEntitiesCollector::init(
   rcl_wait_set_t * p_wait_set,
-  rclcpp::memory_strategy::MemoryStrategy::SharedPtr & memory_strategy,
+  rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy,
   rcl_guard_condition_t * executor_guard_condition)
 {
   // Empty initialize executable list
@@ -80,6 +80,13 @@ StaticExecutorEntitiesCollector::init(
   execute();
 }
 
+void
+StaticExecutorEntitiesCollector::fini()
+{
+  memory_strategy_->clear_handles();
+  exec_list_.clear();
+}
+
 void
 StaticExecutorEntitiesCollector::execute()
 {
@@ -100,7 +107,7 @@ StaticExecutorEntitiesCollector::fill_memory_strategy()
   // Clean up any invalid nodes, if they were detected
   if (has_invalid_weak_groups_or_nodes) {
     std::vector<rclcpp::CallbackGroup::WeakPtr> invalid_group_ptrs;
-    for (auto & pair : weak_groups_to_nodes_associated_with_executor_) {
+    for (const auto & pair : weak_groups_to_nodes_associated_with_executor_) {
       auto & weak_group_ptr = pair.first;
       auto & weak_node_ptr = pair.second;
       if (weak_group_ptr.expired() || weak_node_ptr.expired()) {
@@ -118,9 +125,9 @@ StaticExecutorEntitiesCollector::fill_memory_strategy()
   // Clean up any invalid nodes, if they were detected
   if (has_invalid_weak_groups_or_nodes) {
     std::vector<rclcpp::CallbackGroup::WeakPtr> invalid_group_ptrs;
-    for (auto & pair : weak_groups_associated_with_executor_to_nodes_) {
+    for (const auto & pair : weak_groups_associated_with_executor_to_nodes_) {
       auto & weak_group_ptr = pair.first;
-      auto & weak_node_ptr = pair.second;
+      const auto & weak_node_ptr = pair.second;
       if (weak_group_ptr.expired() || weak_node_ptr.expired()) {
         invalid_group_ptrs.push_back(weak_group_ptr);
       }
@@ -148,7 +155,7 @@ StaticExecutorEntitiesCollector::fill_executable_list()
 }
 void
 StaticExecutorEntitiesCollector::fill_executable_list_from_map(
-  WeakCallbackGroupsToNodesMap weak_groups_to_nodes)
+  const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
 {
   for (const auto & pair : weak_groups_to_nodes) {
     auto group = pair.first.lock();
@@ -211,14 +218,14 @@ StaticExecutorEntitiesCollector::prepare_wait_set()
 
   if (RCL_RET_OK != ret) {
     throw std::runtime_error(
-            std::string("Couldn't resize the wait set : ") + rcl_get_error_string().str);
+            std::string("Couldn't resize the wait set: ") + rcl_get_error_string().str);
   }
 }
 
 void
 StaticExecutorEntitiesCollector::refresh_wait_set(std::chrono::nanoseconds timeout)
 {
-  // clear wait set (memeset to '0' all wait_set_ entities
+  // clear wait set (memset to '0' all wait_set_ entities
   // but keeps the wait_set_ number of entities)
   if (rcl_wait_set_clear(p_wait_set_) != RCL_RET_OK) {
     throw std::runtime_error("Couldn't clear wait set");
@@ -270,7 +277,7 @@ StaticExecutorEntitiesCollector::add_node(
   if (has_executor.exchange(true)) {
     throw std::runtime_error("Node has already been added to an executor.");
   }
-  for (auto & weak_group : node_ptr->get_callback_groups()) {
+  for (const auto & weak_group : node_ptr->get_callback_groups()) {
     auto group_ptr = weak_group.lock();
     if (group_ptr != nullptr && !group_ptr->get_associated_with_executor_atomic().load() &&
       group_ptr->automatically_add_to_executor_with_node())
@@ -297,6 +304,8 @@ StaticExecutorEntitiesCollector::add_callback_group(
   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_associated_with_executor_to_nodes_) &&
+    !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_);
   rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr;
   auto insert_info = weak_groups_to_nodes.insert(
     std::make_pair(weak_group_ptr, node_ptr));
@@ -304,8 +313,6 @@ StaticExecutorEntitiesCollector::add_callback_group(
   if (!was_inserted) {
     throw std::runtime_error("Callback group was already added to executor.");
   }
-  bool is_new_node = !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_) &&
-    !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_);
   if (is_new_node) {
     rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr);
     weak_nodes_to_guard_conditions_[node_weak_ptr] = node_ptr->get_notify_guard_condition();
@@ -430,7 +437,7 @@ StaticExecutorEntitiesCollector::is_ready(rcl_wait_set_t * p_wait_set)
 bool
 StaticExecutorEntitiesCollector::has_node(
   const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-  WeakCallbackGroupsToNodesMap weak_groups_to_nodes) const
+  const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) const
 {
   return std::find_if(
     weak_groups_to_nodes.begin(),
@@ -444,7 +451,7 @@ StaticExecutorEntitiesCollector::has_node(
 void
 StaticExecutorEntitiesCollector::add_callback_groups_from_nodes_associated_to_executor()
 {
-  for (auto & weak_node : weak_nodes_) {
+  for (const auto & weak_node : weak_nodes_) {
     auto node = weak_node.lock();
     if (node) {
       auto group_ptrs = node->get_callback_groups();
@@ -470,10 +477,10 @@ std::vector<rclcpp::CallbackGroup::WeakPtr>
 StaticExecutorEntitiesCollector::get_all_callback_groups()
 {
   std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
-  for (auto const & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) {
+  for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) {
     groups.push_back(group_node_ptr.first);
   }
-  for (auto const & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) {
+  for (const auto & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) {
     groups.push_back(group_node_ptr.first);
   }
   return groups;
@@ -483,7 +490,7 @@ std::vector<rclcpp::CallbackGroup::WeakPtr>
 StaticExecutorEntitiesCollector::get_manually_added_callback_groups()
 {
   std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
-  for (auto const & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) {
+  for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) {
     groups.push_back(group_node_ptr.first);
   }
   return groups;
@@ -493,7 +500,7 @@ std::vector<rclcpp::CallbackGroup::WeakPtr>
 StaticExecutorEntitiesCollector::get_automatically_added_callback_groups_from_nodes()
 {
   std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
-  for (auto const & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) {
+  for (const auto & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) {
     groups.push_back(group_node_ptr.first);
   }
   return groups;

rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp

@@ -42,6 +42,7 @@ StaticSingleThreadedExecutor::spin()
   // Set memory_strategy_ and exec_list_ based on weak_nodes_
   // Prepare wait_set_ based on memory_strategy_
   entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
 
   while (rclcpp::ok(this->context_) && spinning.load()) {
     // Refresh wait set and wait for work

rclcpp/src/rclcpp/init_options.cpp

@@ -26,7 +26,7 @@ InitOptions::InitOptions(rcl_allocator_t allocator)
   *init_options_ = rcl_get_zero_initialized_init_options();
   rcl_ret_t ret = rcl_init_options_init(init_options_.get(), allocator);
   if (RCL_RET_OK != ret) {
-    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialized rcl init options");
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialize rcl init options");
   }
 }
 

rclcpp/src/rclcpp/node.cpp

@@ -138,7 +138,8 @@ Node::Node(
       node_services_,
       node_logging_,
       node_clock_,
-      node_parameters_
+      node_parameters_,
+      options.clock_qos()
     )),
   node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())),
   node_options_(options),

rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp

@@ -26,14 +26,16 @@ NodeTimeSource::NodeTimeSource(
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
   rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
   rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
-  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters)
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters,
+  const rclcpp::QoS & qos)
 : node_base_(node_base),
   node_topics_(node_topics),
   node_graph_(node_graph),
   node_services_(node_services),
   node_logging_(node_logging),
   node_clock_(node_clock),
-  node_parameters_(node_parameters)
+  node_parameters_(node_parameters),
+  time_source_(qos)
 {
   time_source_.attachNode(
     node_base_,

rclcpp/src/rclcpp/node_options.cpp

@@ -67,6 +67,7 @@ NodeOptions &
 NodeOptions::operator=(const NodeOptions & other)
 {
   if (this != &other) {
+    this->node_options_.reset();
     this->context_ = other.context_;
     this->arguments_ = other.arguments_;
     this->parameter_overrides_ = other.parameter_overrides_;
@@ -75,11 +76,15 @@ NodeOptions::operator=(const NodeOptions & other)
     this->use_intra_process_comms_ = other.use_intra_process_comms_;
     this->enable_topic_statistics_ = other.enable_topic_statistics_;
     this->start_parameter_services_ = other.start_parameter_services_;
-    this->allocator_ = other.allocator_;
+    this->start_parameter_event_publisher_ = other.start_parameter_event_publisher_;
+    this->clock_qos_ = other.clock_qos_;
+    this->parameter_event_qos_ = other.parameter_event_qos_;
+    this->rosout_qos_ = other.rosout_qos_;
+    this->parameter_event_publisher_options_ = other.parameter_event_publisher_options_;
     this->allow_undeclared_parameters_ = other.allow_undeclared_parameters_;
     this->automatically_declare_parameters_from_overrides_ =
       other.automatically_declare_parameters_from_overrides_;
-    this->node_options_.reset();
+    this->allocator_ = other.allocator_;
   }
   return *this;
 }
@@ -94,6 +99,7 @@ NodeOptions::get_rcl_node_options() const
     node_options_->allocator = this->allocator_;
     node_options_->use_global_arguments = this->use_global_arguments_;
     node_options_->enable_rosout = this->enable_rosout_;
+    node_options_->rosout_qos = this->rosout_qos_.get_rmw_qos_profile();
 
     int c_argc = 0;
     std::unique_ptr<const char *[]> c_argv;
@@ -253,6 +259,19 @@ NodeOptions::start_parameter_event_publisher(bool start_parameter_event_publishe
   return *this;
 }
 
+const rclcpp::QoS &
+NodeOptions::clock_qos() const
+{
+  return this->clock_qos_;
+}
+
+NodeOptions &
+NodeOptions::clock_qos(const rclcpp::QoS & clock_qos)
+{
+  this->clock_qos_ = clock_qos;
+  return *this;
+}
+
 const rclcpp::QoS &
 NodeOptions::parameter_event_qos() const
 {
@@ -266,6 +285,20 @@ NodeOptions::parameter_event_qos(const rclcpp::QoS & parameter_event_qos)
   return *this;
 }
 
+const rclcpp::QoS &
+NodeOptions::rosout_qos() const
+{
+  return this->rosout_qos_;
+}
+
+NodeOptions &
+NodeOptions::rosout_qos(const rclcpp::QoS & rosout_qos)
+{
+  this->node_options_.reset();
+  this->rosout_qos_ = rosout_qos;
+  return *this;
+}
+
 const rclcpp::PublisherOptionsBase &
 NodeOptions::parameter_event_publisher_options() const
 {

rclcpp/src/rclcpp/parameter_client.cpp

@@ -15,7 +15,12 @@
 #include "rclcpp/parameter_client.hpp"
 
 #include <algorithm>
+#include <chrono>
+#include <functional>
+#include <future>
+#include <iterator>
 #include <memory>
+#include <stdexcept>
 #include <string>
 #include <vector>
 

rclcpp/src/rclcpp/parameter_service.cpp

@@ -119,7 +119,7 @@ ParameterService::ParameterService(
         RCLCPP_WARN(
           rclcpp::get_logger("rclcpp"), "Failed to set parameters atomically: %s", ex.what());
         response->result.successful = false;
-        response->result.reason = "One or more parameters wer not declared before setting";
+        response->result.reason = "One or more parameters were not declared before setting";
       }
     },
     qos_profile, nullptr);

rclcpp/src/rclcpp/publisher_base.cpp

@@ -262,7 +262,9 @@ PublisherBase::default_incompatible_qos_callback(
   std::string policy_name = qos_policy_name_from_kind(event.last_policy_kind);
   RCLCPP_WARN(
     rclcpp::get_logger(rcl_node_get_logger_name(rcl_node_handle_.get())),
-    "New subscription discovered on this topic, requesting incompatible QoS. "
+    "New subscription discovered on topic '%s', requesting incompatible QoS. "
     "No messages will be sent to it. "
-    "Last incompatible policy: %s", policy_name.c_str());
+    "Last incompatible policy: %s",
+    get_topic_name(),
+    policy_name.c_str());
 }

rclcpp/src/rclcpp/qos.cpp

@@ -235,6 +235,12 @@ bool operator!=(const QoS & left, const QoS & right)
   return !(left == right);
 }
 
+ClockQoS::ClockQoS(const QoSInitialization & qos_initialization)
+// Using `rmw_qos_profile_sensor_data` intentionally.
+// It's best effort and `qos_initialization` is overriding the depth to 1.
+: QoS(qos_initialization, rmw_qos_profile_sensor_data)
+{}
+
 SensorDataQoS::SensorDataQoS(const QoSInitialization & qos_initialization)
 : QoS(qos_initialization, rmw_qos_profile_sensor_data)
 {}
@@ -251,6 +257,10 @@ ParameterEventsQoS::ParameterEventsQoS(const QoSInitialization & qos_initializat
 : QoS(qos_initialization, rmw_qos_profile_parameter_events)
 {}
 
+RosoutQoS::RosoutQoS(const QoSInitialization & rosout_initialization)
+: QoS(rosout_initialization, rcl_qos_profile_rosout_default)
+{}
+
 SystemDefaultsQoS::SystemDefaultsQoS(const QoSInitialization & qos_initialization)
 : QoS(qos_initialization, rmw_qos_profile_system_default)
 {}

rclcpp/src/rclcpp/subscription_base.cpp

@@ -245,9 +245,11 @@ SubscriptionBase::default_incompatible_qos_callback(
   std::string policy_name = qos_policy_name_from_kind(event.last_policy_kind);
   RCLCPP_WARN(
     rclcpp::get_logger(rcl_node_get_logger_name(node_handle_.get())),
-    "New publisher discovered on this topic, offering incompatible QoS. "
+    "New publisher discovered on topic '%s', offering incompatible QoS. "
     "No messages will be sent to it. "
-    "Last incompatible policy: %s", policy_name.c_str());
+    "Last incompatible policy: %s",
+    get_topic_name(),
+    policy_name.c_str());
 }
 
 bool

rclcpp/src/rclcpp/time_source.cpp

@@ -33,17 +33,16 @@
 namespace rclcpp
 {
 
-TimeSource::TimeSource(std::shared_ptr<rclcpp::Node> node)
+TimeSource::TimeSource(std::shared_ptr<rclcpp::Node> node, const rclcpp::QoS & qos)
 : logger_(rclcpp::get_logger("rclcpp")),
-  clock_subscription_(nullptr),
-  ros_time_active_(false)
+  qos_(qos)
 {
   this->attachNode(node);
 }
 
-TimeSource::TimeSource()
+TimeSource::TimeSource(const rclcpp::QoS & qos)
 : logger_(rclcpp::get_logger("rclcpp")),
-  ros_time_active_(false)
+  qos_(qos)
 {
 }
 

rclcpp/src/rclcpp/timer.cpp

@@ -76,8 +76,9 @@ TimerBase::~TimerBase()
 void
 TimerBase::cancel()
 {
-  if (rcl_timer_cancel(timer_handle_.get()) != RCL_RET_OK) {
-    throw std::runtime_error(std::string("Couldn't cancel timer: ") + rcl_get_error_string().str);
+  rcl_ret_t ret = rcl_timer_cancel(timer_handle_.get());
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't cancel timer");
   }
 }
 
@@ -95,8 +96,9 @@ TimerBase::is_canceled()
 void
 TimerBase::reset()
 {
-  if (rcl_timer_reset(timer_handle_.get()) != RCL_RET_OK) {
-    throw std::runtime_error(std::string("Couldn't reset timer: ") + rcl_get_error_string().str);
+  rcl_ret_t ret = rcl_timer_reset(timer_handle_.get());
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't reset timer");
   }
 }
 
@@ -104,8 +106,9 @@ bool
 TimerBase::is_ready()
 {
   bool ready = false;
-  if (rcl_timer_is_ready(timer_handle_.get(), &ready) != RCL_RET_OK) {
-    throw std::runtime_error(std::string("Failed to check timer: ") + rcl_get_error_string().str);
+  rcl_ret_t ret = rcl_timer_is_ready(timer_handle_.get(), &ready);
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to check timer");
   }
   return ready;
 }
@@ -114,14 +117,10 @@ std::chrono::nanoseconds
 TimerBase::time_until_trigger()
 {
   int64_t time_until_next_call = 0;
-  if (
-    rcl_timer_get_time_until_next_call(
-      timer_handle_.get(),
-      &time_until_next_call) != RCL_RET_OK)
-  {
-    throw std::runtime_error(
-            std::string(
-              "Timer could not get time until next call: ") + rcl_get_error_string().str);
+  rcl_ret_t ret = rcl_timer_get_time_until_next_call(
+    timer_handle_.get(), &time_until_next_call);
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Timer could not get time until next call");
   }
   return std::chrono::nanoseconds(time_until_next_call);
 }

rclcpp/src/rclcpp/utilities.cpp

@@ -14,6 +14,8 @@
 
 #include "rclcpp/utilities.hpp"
 
+#include <chrono>
+#include <functional>
 #include <string>
 #include <vector>
 

rclcpp/test/CMakeLists.txt

@@ -1,14 +1,9 @@
-find_package(ament_cmake_gtest REQUIRED)
-
-find_package(rmw_implementation_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 
 find_package(test_msgs REQUIRED)
 
 include(cmake/rclcpp_add_build_failure_test.cmake)
 
-add_definitions(-DTEST_RESOURCES_DIRECTORY="${CMAKE_CURRENT_BINARY_DIR}/resources")
-
 rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs
   msg/Header.msg
   msg/MessageWithHeader.msg
@@ -17,585 +12,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs
   SKIP_INSTALL
 )
 
-# Increasing timeout because connext can take a long time to destroy nodes
-# TODO(brawner) remove when destroying Node for Connext is resolved. See:
-# https://github.com/ros2/rclcpp/issues/1250
-ament_add_gtest(
-  test_allocator_memory_strategy
-  rclcpp/strategies/test_allocator_memory_strategy.cpp
-  TIMEOUT 360)
-if(TARGET test_allocator_memory_strategy)
-  ament_target_dependencies(test_allocator_memory_strategy
-    "rcl"
-    "test_msgs"
-  )
-  target_link_libraries(test_allocator_memory_strategy ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_message_pool_memory_strategy rclcpp/strategies/test_message_pool_memory_strategy.cpp)
-if(TARGET test_message_pool_memory_strategy)
-  ament_target_dependencies(test_message_pool_memory_strategy
-    "rcl"
-    "test_msgs"
-  )
-  target_link_libraries(test_message_pool_memory_strategy ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_any_service_callback rclcpp/test_any_service_callback.cpp)
-if(TARGET test_any_service_callback)
-  ament_target_dependencies(test_any_service_callback
-    "test_msgs"
-  )
-  target_link_libraries(test_any_service_callback ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_any_subscription_callback rclcpp/test_any_subscription_callback.cpp)
-if(TARGET test_any_subscription_callback)
-  ament_target_dependencies(test_any_subscription_callback
-    "test_msgs"
-  )
-  target_link_libraries(test_any_subscription_callback ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_client rclcpp/test_client.cpp)
-if(TARGET test_client)
-  ament_target_dependencies(test_client
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_client ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_create_timer rclcpp/test_create_timer.cpp)
-if(TARGET test_create_timer)
-  ament_target_dependencies(test_create_timer
-    "rcl_interfaces"
-    "rmw"
-    "rcl"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_create_timer ${PROJECT_NAME})
-  target_include_directories(test_create_timer PRIVATE rclcpp/)
-endif()
-ament_add_gtest(test_create_subscription rclcpp/test_create_subscription.cpp)
-if(TARGET test_create_subscription)
-  target_link_libraries(test_create_subscription ${PROJECT_NAME})
-  ament_target_dependencies(test_create_subscription
-    "test_msgs"
-  )
-endif()
-ament_add_gtest(test_add_callback_groups_to_executor rclcpp/test_add_callback_groups_to_executor.cpp)
-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"
-  )
-endif()
-ament_add_gtest(test_expand_topic_or_service_name rclcpp/test_expand_topic_or_service_name.cpp)
-if(TARGET test_expand_topic_or_service_name)
-  ament_target_dependencies(test_expand_topic_or_service_name
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_function_traits rclcpp/test_function_traits.cpp)
-if(TARGET test_function_traits)
-  target_include_directories(test_function_traits PUBLIC ../include)
-  ament_target_dependencies(test_function_traits
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-endif()
-ament_add_gmock(test_intra_process_manager rclcpp/test_intra_process_manager.cpp)
-if(TARGET test_intra_process_manager)
-  ament_target_dependencies(test_intra_process_manager
-    "rcl"
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_intra_process_manager ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_ring_buffer_implementation rclcpp/test_ring_buffer_implementation.cpp)
-if(TARGET test_ring_buffer_implementation)
-  ament_target_dependencies(test_ring_buffer_implementation
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_ring_buffer_implementation ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_intra_process_buffer rclcpp/test_intra_process_buffer.cpp)
-if(TARGET test_intra_process_buffer)
-  ament_target_dependencies(test_intra_process_buffer
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_intra_process_buffer ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_loaned_message rclcpp/test_loaned_message.cpp)
-ament_target_dependencies(test_loaned_message
-  "test_msgs"
-)
-target_link_libraries(test_loaned_message ${PROJECT_NAME})
-
-ament_add_gtest(test_memory_strategy rclcpp/test_memory_strategy.cpp)
-ament_target_dependencies(test_memory_strategy
-  "test_msgs"
-)
-target_link_libraries(test_memory_strategy ${PROJECT_NAME})
-
-ament_add_gtest(test_message_memory_strategy rclcpp/test_message_memory_strategy.cpp)
-ament_target_dependencies(test_message_memory_strategy
-  "test_msgs"
-)
-target_link_libraries(test_message_memory_strategy ${PROJECT_NAME})
-
-ament_add_gtest(test_node rclcpp/test_node.cpp TIMEOUT 240)
-if(TARGET test_node)
-  ament_target_dependencies(test_node
-    "rcl_interfaces"
-    "rcpputils"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-    "test_msgs"
-  )
-  target_link_libraries(test_node ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_node_interfaces__get_node_interfaces
-  rclcpp/node_interfaces/test_get_node_interfaces.cpp)
-if(TARGET test_node_interfaces__get_node_interfaces)
-  target_link_libraries(test_node_interfaces__get_node_interfaces ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_node_interfaces__node_base
-  rclcpp/node_interfaces/test_node_base.cpp)
-if(TARGET test_node_interfaces__node_base)
-  target_link_libraries(test_node_interfaces__node_base ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_node_interfaces__node_clock
-  rclcpp/node_interfaces/test_node_clock.cpp)
-if(TARGET test_node_interfaces__node_clock)
-  target_link_libraries(test_node_interfaces__node_clock ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_node_interfaces__node_graph
-  rclcpp/node_interfaces/test_node_graph.cpp)
-if(TARGET test_node_interfaces__node_graph)
-  ament_target_dependencies(
-    test_node_interfaces__node_graph
-    "test_msgs")
-  target_link_libraries(test_node_interfaces__node_graph ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_node_interfaces__node_parameters
-  rclcpp/node_interfaces/test_node_parameters.cpp)
-if(TARGET test_node_interfaces__node_parameters)
-  target_link_libraries(test_node_interfaces__node_parameters ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_node_interfaces__node_services
-  rclcpp/node_interfaces/test_node_services.cpp)
-if(TARGET test_node_interfaces__node_services)
-  target_link_libraries(test_node_interfaces__node_services ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_node_interfaces__node_timers
-  rclcpp/node_interfaces/test_node_timers.cpp)
-if(TARGET test_node_interfaces__node_timers)
-  target_link_libraries(test_node_interfaces__node_timers ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_node_interfaces__node_topics
-  rclcpp/node_interfaces/test_node_topics.cpp)
-if(TARGET test_node_interfaces__node_topics)
-  ament_target_dependencies(
-    test_node_interfaces__node_topics
-    "test_msgs")
-  target_link_libraries(test_node_interfaces__node_topics ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_node_interfaces__node_waitables
-  rclcpp/node_interfaces/test_node_waitables.cpp)
-if(TARGET test_node_interfaces__node_waitables)
-  target_link_libraries(test_node_interfaces__node_waitables ${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/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp)
-# target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node
-#   ${PROJECT_NAME})
-
-# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_rclcpp_node
-#   rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp)
-# target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node
-#   ${PROJECT_NAME})
-
-# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_wrapped_node
-#   rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp)
-# target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node
-#   ${PROJECT_NAME})
-
-# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_wrapped_node
-#   rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp)
-# target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node
-#   ${PROJECT_NAME})
-
-ament_add_gtest(test_node_global_args rclcpp/test_node_global_args.cpp)
-if(TARGET test_node_global_args)
-  ament_target_dependencies(test_node_global_args
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_node_global_args ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_node_options rclcpp/test_node_options.cpp)
-if(TARGET test_node_options)
-  ament_target_dependencies(test_node_options "rcl")
-  target_link_libraries(test_node_options ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_init_options rclcpp/test_init_options.cpp)
-if(TARGET test_init_options)
-  ament_target_dependencies(test_init_options "rcl")
-  target_link_libraries(test_init_options ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_parameter_client rclcpp/test_parameter_client.cpp)
-if(TARGET test_parameter_client)
-  ament_target_dependencies(test_parameter_client
-    "rcl_interfaces"
-  )
-  target_link_libraries(test_parameter_client ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_parameter_events_filter rclcpp/test_parameter_events_filter.cpp)
-if(TARGET test_parameter_events_filter)
-  ament_target_dependencies(test_parameter_events_filter
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_parameter_events_filter ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_parameter rclcpp/test_parameter.cpp)
-if(TARGET test_parameter)
-  ament_target_dependencies(test_parameter
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_parameter ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_parameter_map rclcpp/test_parameter_map.cpp)
-if(TARGET test_parameter_map)
-  target_link_libraries(test_parameter_map ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_publisher rclcpp/test_publisher.cpp)
-if(TARGET test_publisher)
-  ament_target_dependencies(test_publisher
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-    "test_msgs"
-  )
-  target_link_libraries(test_publisher ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_publisher_subscription_count_api rclcpp/test_publisher_subscription_count_api.cpp)
-if(TARGET test_publisher_subscription_count_api)
-  ament_target_dependencies(test_publisher_subscription_count_api
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-    "test_msgs"
-  )
-  target_link_libraries(test_publisher_subscription_count_api ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_qos rclcpp/test_qos.cpp)
-if(TARGET test_qos)
-  ament_target_dependencies(test_qos
-    "rmw"
-  )
-  target_link_libraries(test_qos
-    ${PROJECT_NAME}
-  )
-endif()
-ament_add_gtest(test_qos_event rclcpp/test_qos_event.cpp)
-if(TARGET test_qos_event)
-  ament_target_dependencies(test_qos_event
-    "rmw"
-    "test_msgs"
-  )
-  target_link_libraries(test_qos_event
-    ${PROJECT_NAME}
-  )
-endif()
-ament_add_gtest(test_rate rclcpp/test_rate.cpp)
-if(TARGET test_rate)
-  ament_target_dependencies(test_rate
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-  )
-  target_link_libraries(test_rate
-    ${PROJECT_NAME}
-  )
-endif()
-ament_add_gtest(test_serialized_message_allocator rclcpp/test_serialized_message_allocator.cpp)
-if(TARGET test_serialized_message_allocator)
-  ament_target_dependencies(test_serialized_message_allocator
-    test_msgs
-  )
-  target_link_libraries(test_serialized_message_allocator
-    ${PROJECT_NAME}
-  )
-endif()
-ament_add_gtest(test_serialized_message rclcpp/test_serialized_message.cpp)
-if(TARGET test_serialized_message)
-  ament_target_dependencies(test_serialized_message
-    test_msgs
-  )
-  target_link_libraries(test_serialized_message
-    ${PROJECT_NAME}
-  )
-endif()
-ament_add_gtest(test_service rclcpp/test_service.cpp)
-if(TARGET test_service)
-  ament_target_dependencies(test_service
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-    "test_msgs"
-  )
-  target_link_libraries(test_service ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_subscription rclcpp/test_subscription.cpp)
-if(TARGET test_subscription)
-  ament_target_dependencies(test_subscription
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-    "test_msgs"
-  )
-  target_link_libraries(test_subscription ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_subscription_publisher_count_api rclcpp/test_subscription_publisher_count_api.cpp)
-if(TARGET test_subscription_publisher_count_api)
-  ament_target_dependencies(test_subscription_publisher_count_api
-    "rcl_interfaces"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-    "test_msgs"
-  )
-  target_link_libraries(test_subscription_publisher_count_api ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_subscription_traits rclcpp/test_subscription_traits.cpp)
-if(TARGET test_subscription_traits)
-  ament_target_dependencies(test_subscription_traits
-    "rcl"
-    "test_msgs"
-  )
-  target_link_libraries(test_subscription_traits ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_type_support rclcpp/test_type_support.cpp)
-if(TARGET test_type_support)
-  ament_target_dependencies(test_type_support
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-    "test_msgs"
-  )
-  target_link_libraries(test_type_support ${PROJECT_NAME})
-endif()
-ament_add_gtest(test_find_weak_nodes rclcpp/test_find_weak_nodes.cpp)
-if(TARGET test_find_weak_nodes)
-  ament_target_dependencies(test_find_weak_nodes
-    "rcl"
-  )
-  target_link_libraries(test_find_weak_nodes ${PROJECT_NAME})
-endif()
-
-set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}")
-if(WIN32)
-  set(append_library_dirs "${append_library_dirs}/$<CONFIG>")
-endif()
-
-ament_add_gtest(test_externally_defined_services rclcpp/test_externally_defined_services.cpp)
-ament_target_dependencies(test_externally_defined_services
-  "rcl"
-  "test_msgs"
-)
-target_link_libraries(test_externally_defined_services ${PROJECT_NAME})
-
-ament_add_gtest(test_duration rclcpp/test_duration.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_duration)
-  ament_target_dependencies(test_duration
-    "rcl")
-  target_link_libraries(test_duration ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_logger rclcpp/test_logger.cpp)
-target_link_libraries(test_logger ${PROJECT_NAME})
-
-ament_add_gmock(test_logging rclcpp/test_logging.cpp)
-target_link_libraries(test_logging ${PROJECT_NAME})
-
-ament_add_gtest(test_time rclcpp/test_time.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_time)
-  ament_target_dependencies(test_time
-    "rcl")
-  target_link_libraries(test_time ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_timer rclcpp/test_timer.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_timer)
-  ament_target_dependencies(test_timer
-    "rcl")
-  target_link_libraries(test_timer ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_time_source rclcpp/test_time_source.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_time_source)
-  ament_target_dependencies(test_time_source
-    "rcl")
-  target_link_libraries(test_time_source ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_utilities rclcpp/test_utilities.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_utilities)
-  ament_target_dependencies(test_utilities
-    "rcl")
-  target_link_libraries(test_utilities ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_init rclcpp/test_init.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_init)
-  ament_target_dependencies(test_init
-    "rcl")
-  target_link_libraries(test_init ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_interface_traits rclcpp/test_interface_traits.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_interface_traits)
-  ament_target_dependencies(test_interface_traits
-    "rcl")
-  target_link_libraries(test_interface_traits ${PROJECT_NAME})
-endif()
-
-# TODO(brawner) remove when destroying Node for Connext is resolved. See:
-# https://github.com/ros2/rclcpp/issues/1250
-ament_add_gtest(
-  test_executors
-  rclcpp/executors/test_executors.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}"
-  TIMEOUT 180)
-if(TARGET test_executors)
-  ament_target_dependencies(test_executors
-    "rcl"
-    "test_msgs")
-  target_link_libraries(test_executors ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_static_single_threaded_executor rclcpp/executors/test_static_single_threaded_executor.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_static_single_threaded_executor)
-  target_link_libraries(test_static_single_threaded_executor ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_multi_threaded_executor rclcpp/executors/test_multi_threaded_executor.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_multi_threaded_executor)
-  ament_target_dependencies(test_multi_threaded_executor
-    "rcl")
-  target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_static_executor_entities_collector rclcpp/executors/test_static_executor_entities_collector.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_static_executor_entities_collector)
-  ament_target_dependencies(test_static_executor_entities_collector
-    "rcl"
-    "test_msgs")
-  target_link_libraries(test_static_executor_entities_collector ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_guard_condition rclcpp/test_guard_condition.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_guard_condition)
-  target_link_libraries(test_guard_condition ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_wait_set rclcpp/test_wait_set.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}")
-if(TARGET test_wait_set)
-  ament_target_dependencies(test_wait_set "test_msgs")
-  target_link_libraries(test_wait_set ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_subscription_topic_statistics rclcpp/topic_statistics/test_subscription_topic_statistics.cpp
-  APPEND_LIBRARY_DIRS "${append_library_dirs}"
-)
-if(TARGET test_subscription_topic_statistics)
-  ament_target_dependencies(test_subscription_topic_statistics
-    "builtin_interfaces"
-    "libstatistics_collector"
-    "rcl_interfaces"
-    "rcutils"
-    "rmw"
-    "rosidl_runtime_cpp"
-    "rosidl_typesupport_cpp"
-    "statistics_msgs"
-    "test_msgs")
-  rosidl_target_interfaces(test_subscription_topic_statistics ${PROJECT_NAME}_test_msgs "rosidl_typesupport_cpp")
-  target_link_libraries(test_subscription_topic_statistics ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_subscription_options rclcpp/test_subscription_options.cpp)
-if(TARGET test_subscription_options)
-  ament_target_dependencies(test_subscription_options "rcl")
-  target_link_libraries(test_subscription_options ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_dynamic_storage rclcpp/wait_set_policies/test_dynamic_storage.cpp)
-if(TARGET test_dynamic_storage)
-  ament_target_dependencies(test_dynamic_storage "rcl" "test_msgs")
-  target_link_libraries(test_dynamic_storage ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_storage_policy_common rclcpp/wait_set_policies/test_storage_policy_common.cpp)
-if(TARGET test_storage_policy_common)
-  ament_target_dependencies(test_storage_policy_common "rcl" "test_msgs")
-  target_link_libraries(test_storage_policy_common ${PROJECT_NAME} mimick)
-endif()
-
-ament_add_gtest(test_static_storage rclcpp/wait_set_policies/test_static_storage.cpp)
-if(TARGET test_static_storage)
-  ament_target_dependencies(test_static_storage "rcl" "test_msgs")
-  target_link_libraries(test_static_storage ${PROJECT_NAME})
-endif()
-
-ament_add_gtest(test_thread_safe_synchronization rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp)
-if(TARGET test_thread_safe_synchronization)
-  ament_target_dependencies(test_thread_safe_synchronization "rcl" "test_msgs")
-  target_link_libraries(test_thread_safe_synchronization ${PROJECT_NAME})
-endif()
+add_subdirectory(rclcpp)
 
 ament_add_gtest(test_rclcpp_gtest_macros utils/test_rclcpp_gtest_macros.cpp)
 if(TARGET test_rclcpp_gtest_macros)

rclcpp/test/mocking_utils/patch.hpp

@@ -253,7 +253,7 @@ struct PatchTraits<ID, void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)>
     mock_type, void, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5);
 };
 
-/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)
+/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6)
 /// free functions.
 /**
  * \tparam ID Numerical identifier of the patch. Ought to be unique.
@@ -270,6 +270,62 @@ struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6)>
     mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6);
 };
 
+/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7)
+/// free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ReturnT,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3,
+  typename ArgT4, typename ArgT5,
+  typename ArgT6, typename ArgT7>
+struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7)>
+{
+  mmk_mock_define(
+    mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8)
+/// free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ReturnT,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3,
+  typename ArgT4, typename ArgT5,
+  typename ArgT6, typename ArgT7, typename ArgT8>
+struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8)>
+{
+  mmk_mock_define(
+    mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7,
+/// ArgT8, ArgT9) free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ReturnT,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3,
+  typename ArgT4, typename ArgT5,
+  typename ArgT6, typename ArgT7,
+  typename ArgT8, typename ArgT9>
+struct PatchTraits<ID, ReturnT(
+    ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8, ArgT9)>
+{
+  mmk_mock_define(
+    mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8, ArgT9);
+};
+
 /// Generic trampoline to wrap generalized callables in plain functions.
 /**
  * \tparam ID Numerical identifier of this trampoline. Ought to be unique.

rclcpp/test/rclcpp/CMakeLists.txt

@@ -0,0 +1,647 @@
+add_subdirectory(benchmark)
+
+find_package(ament_cmake_gtest REQUIRED)
+
+find_package(rmw_implementation_cmake REQUIRED)
+
+add_definitions(-DTEST_RESOURCES_DIRECTORY="${CMAKE_CURRENT_BINARY_DIR}/../resources")
+
+ament_add_gtest(
+  test_allocator_common
+  allocator/test_allocator_common.cpp)
+if(TARGET test_allocator_common)
+  target_link_libraries(test_allocator_common ${PROJECT_NAME})
+endif()
+ament_add_gtest(
+  test_allocator_deleter
+  allocator/test_allocator_deleter.cpp)
+if(TARGET test_allocator_deleter)
+  target_link_libraries(test_allocator_deleter ${PROJECT_NAME})
+endif()
+ament_add_gtest(
+  test_exceptions
+  exceptions/test_exceptions.cpp)
+if(TARGET test_exceptions)
+  target_link_libraries(test_exceptions ${PROJECT_NAME} mimick)
+endif()
+
+# Increasing timeout because connext can take a long time to destroy nodes
+# TODO(brawner) remove when destroying Node for Connext is resolved. See:
+# https://github.com/ros2/rclcpp/issues/1250
+ament_add_gtest(
+  test_allocator_memory_strategy
+  strategies/test_allocator_memory_strategy.cpp
+  TIMEOUT 360)
+if(TARGET test_allocator_memory_strategy)
+  ament_target_dependencies(test_allocator_memory_strategy
+    "rcl"
+    "test_msgs"
+  )
+  target_link_libraries(test_allocator_memory_strategy ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_message_pool_memory_strategy strategies/test_message_pool_memory_strategy.cpp)
+if(TARGET test_message_pool_memory_strategy)
+  ament_target_dependencies(test_message_pool_memory_strategy
+    "rcl"
+    "test_msgs"
+  )
+  target_link_libraries(test_message_pool_memory_strategy ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_any_service_callback test_any_service_callback.cpp)
+if(TARGET test_any_service_callback)
+  ament_target_dependencies(test_any_service_callback
+    "test_msgs"
+  )
+  target_link_libraries(test_any_service_callback ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_any_subscription_callback test_any_subscription_callback.cpp)
+if(TARGET test_any_subscription_callback)
+  ament_target_dependencies(test_any_subscription_callback
+    "test_msgs"
+  )
+  target_link_libraries(test_any_subscription_callback ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_client test_client.cpp)
+if(TARGET test_client)
+  ament_target_dependencies(test_client
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+    "test_msgs"
+  )
+  target_link_libraries(test_client ${PROJECT_NAME} mimick)
+endif()
+ament_add_gtest(test_create_timer test_create_timer.cpp)
+if(TARGET test_create_timer)
+  ament_target_dependencies(test_create_timer
+    "rcl_interfaces"
+    "rmw"
+    "rcl"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+  )
+  target_link_libraries(test_create_timer ${PROJECT_NAME})
+  target_include_directories(test_create_timer PRIVATE ./)
+endif()
+ament_add_gtest(test_create_subscription test_create_subscription.cpp)
+if(TARGET test_create_subscription)
+  target_link_libraries(test_create_subscription ${PROJECT_NAME})
+  ament_target_dependencies(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"
+  )
+endif()
+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
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+  )
+  target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME} mimick)
+endif()
+ament_add_gtest(test_function_traits test_function_traits.cpp)
+if(TARGET test_function_traits)
+  target_include_directories(test_function_traits PUBLIC ../../include)
+  ament_target_dependencies(test_function_traits
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+  )
+endif()
+ament_add_gtest(
+  test_future_return_code
+  test_future_return_code.cpp)
+if(TARGET test_future_return_code)
+  target_link_libraries(test_future_return_code ${PROJECT_NAME})
+endif()
+ament_add_gmock(test_intra_process_manager test_intra_process_manager.cpp)
+if(TARGET test_intra_process_manager)
+  ament_target_dependencies(test_intra_process_manager
+    "rcl"
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+  )
+  target_link_libraries(test_intra_process_manager ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_ring_buffer_implementation test_ring_buffer_implementation.cpp)
+if(TARGET test_ring_buffer_implementation)
+  ament_target_dependencies(test_ring_buffer_implementation
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+  )
+  target_link_libraries(test_ring_buffer_implementation ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_intra_process_buffer test_intra_process_buffer.cpp)
+if(TARGET test_intra_process_buffer)
+  ament_target_dependencies(test_intra_process_buffer
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+  )
+  target_link_libraries(test_intra_process_buffer ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_loaned_message test_loaned_message.cpp)
+ament_target_dependencies(test_loaned_message
+  "test_msgs"
+)
+target_link_libraries(test_loaned_message ${PROJECT_NAME} mimick)
+
+ament_add_gtest(test_memory_strategy test_memory_strategy.cpp)
+ament_target_dependencies(test_memory_strategy
+  "test_msgs"
+)
+target_link_libraries(test_memory_strategy ${PROJECT_NAME})
+
+ament_add_gtest(test_message_memory_strategy test_message_memory_strategy.cpp)
+ament_target_dependencies(test_message_memory_strategy
+  "test_msgs"
+)
+target_link_libraries(test_message_memory_strategy ${PROJECT_NAME})
+
+ament_add_gtest(test_node test_node.cpp TIMEOUT 240)
+if(TARGET test_node)
+  ament_target_dependencies(test_node
+    "rcl_interfaces"
+    "rcpputils"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+    "test_msgs"
+  )
+  target_link_libraries(test_node ${PROJECT_NAME} mimick)
+endif()
+
+ament_add_gtest(test_node_interfaces__get_node_interfaces
+  node_interfaces/test_get_node_interfaces.cpp)
+if(TARGET test_node_interfaces__get_node_interfaces)
+  target_link_libraries(test_node_interfaces__get_node_interfaces ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_node_interfaces__node_base
+  node_interfaces/test_node_base.cpp)
+if(TARGET test_node_interfaces__node_base)
+  target_link_libraries(test_node_interfaces__node_base ${PROJECT_NAME} mimick)
+endif()
+ament_add_gtest(test_node_interfaces__node_clock
+  node_interfaces/test_node_clock.cpp)
+if(TARGET test_node_interfaces__node_clock)
+  target_link_libraries(test_node_interfaces__node_clock ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_node_interfaces__node_graph
+  node_interfaces/test_node_graph.cpp
+  TIMEOUT 120)
+if(TARGET test_node_interfaces__node_graph)
+  ament_target_dependencies(
+    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_parameters
+  node_interfaces/test_node_parameters.cpp)
+if(TARGET test_node_interfaces__node_parameters)
+  target_link_libraries(test_node_interfaces__node_parameters ${PROJECT_NAME} mimick)
+endif()
+ament_add_gtest(test_node_interfaces__node_services
+  node_interfaces/test_node_services.cpp)
+if(TARGET test_node_interfaces__node_services)
+  target_link_libraries(test_node_interfaces__node_services ${PROJECT_NAME} mimick)
+endif()
+ament_add_gtest(test_node_interfaces__node_timers
+  node_interfaces/test_node_timers.cpp)
+if(TARGET test_node_interfaces__node_timers)
+  target_link_libraries(test_node_interfaces__node_timers ${PROJECT_NAME} mimick)
+endif()
+ament_add_gtest(test_node_interfaces__node_topics
+  node_interfaces/test_node_topics.cpp)
+if(TARGET test_node_interfaces__node_topics)
+  ament_target_dependencies(
+    test_node_interfaces__node_topics
+    "test_msgs")
+  target_link_libraries(test_node_interfaces__node_topics ${PROJECT_NAME} mimick)
+endif()
+ament_add_gtest(test_node_interfaces__node_waitables
+  node_interfaces/test_node_waitables.cpp)
+if(TARGET test_node_interfaces__node_waitables)
+  target_link_libraries(test_node_interfaces__node_waitables ${PROJECT_NAME} mimick)
+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
+#   node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp)
+# target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node
+#   ${PROJECT_NAME})
+
+# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_rclcpp_node
+#   node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp)
+# target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node
+#   ${PROJECT_NAME})
+
+# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_wrapped_node
+#   node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp)
+# target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node
+#   ${PROJECT_NAME})
+
+# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_wrapped_node
+#   node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp)
+# target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node
+#   ${PROJECT_NAME})
+
+ament_add_gtest(test_node_global_args test_node_global_args.cpp)
+if(TARGET test_node_global_args)
+  ament_target_dependencies(test_node_global_args
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+  )
+  target_link_libraries(test_node_global_args ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_node_options test_node_options.cpp)
+if(TARGET test_node_options)
+  ament_target_dependencies(test_node_options "rcl")
+  target_link_libraries(test_node_options ${PROJECT_NAME} mimick)
+endif()
+ament_add_gtest(test_init_options test_init_options.cpp)
+if(TARGET test_init_options)
+  ament_target_dependencies(test_init_options "rcl")
+  target_link_libraries(test_init_options ${PROJECT_NAME} mimick)
+endif()
+ament_add_gtest(test_parameter_client test_parameter_client.cpp)
+if(TARGET test_parameter_client)
+  ament_target_dependencies(test_parameter_client
+    "rcl_interfaces"
+  )
+  target_link_libraries(test_parameter_client ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_parameter_service test_parameter_service.cpp)
+if(TARGET test_parameter_service)
+  ament_target_dependencies(test_parameter_service
+    "rcl_interfaces"
+  )
+  target_link_libraries(test_parameter_service ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_parameter_events_filter test_parameter_events_filter.cpp)
+if(TARGET test_parameter_events_filter)
+  ament_target_dependencies(test_parameter_events_filter
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+  )
+  target_link_libraries(test_parameter_events_filter ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_parameter test_parameter.cpp)
+if(TARGET test_parameter)
+  ament_target_dependencies(test_parameter
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+  )
+  target_link_libraries(test_parameter ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_parameter_map test_parameter_map.cpp)
+if(TARGET test_parameter_map)
+  target_link_libraries(test_parameter_map ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_publisher test_publisher.cpp TIMEOUT 120)
+if(TARGET test_publisher)
+  ament_target_dependencies(test_publisher
+    "rcl"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+    "test_msgs"
+  )
+  target_link_libraries(test_publisher ${PROJECT_NAME} mimick)
+endif()
+ament_add_gtest(test_publisher_subscription_count_api test_publisher_subscription_count_api.cpp)
+if(TARGET test_publisher_subscription_count_api)
+  ament_target_dependencies(test_publisher_subscription_count_api
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+    "test_msgs"
+  )
+  target_link_libraries(test_publisher_subscription_count_api ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_qos test_qos.cpp)
+if(TARGET test_qos)
+  ament_target_dependencies(test_qos
+    "rmw"
+  )
+  target_link_libraries(test_qos
+    ${PROJECT_NAME}
+  )
+endif()
+ament_add_gtest(test_qos_event test_qos_event.cpp)
+if(TARGET test_qos_event)
+  ament_target_dependencies(test_qos_event
+    "rmw"
+    "test_msgs"
+  )
+  target_link_libraries(test_qos_event
+    ${PROJECT_NAME}
+    mimick
+  )
+endif()
+ament_add_gtest(test_rate test_rate.cpp)
+if(TARGET test_rate)
+  ament_target_dependencies(test_rate
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+  )
+  target_link_libraries(test_rate
+    ${PROJECT_NAME}
+  )
+endif()
+ament_add_gtest(test_serialized_message_allocator test_serialized_message_allocator.cpp)
+if(TARGET test_serialized_message_allocator)
+  ament_target_dependencies(test_serialized_message_allocator
+    test_msgs
+  )
+  target_link_libraries(test_serialized_message_allocator
+    ${PROJECT_NAME}
+  )
+endif()
+ament_add_gtest(test_serialized_message test_serialized_message.cpp)
+if(TARGET test_serialized_message)
+  ament_target_dependencies(test_serialized_message
+    test_msgs
+  )
+  target_link_libraries(test_serialized_message
+    ${PROJECT_NAME}
+  )
+endif()
+ament_add_gtest(test_service test_service.cpp)
+if(TARGET test_service)
+  ament_target_dependencies(test_service
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+    "test_msgs"
+  )
+  target_link_libraries(test_service ${PROJECT_NAME} mimick)
+endif()
+# Creating and destroying nodes is slow with Connext, so this needs larger timeout.
+ament_add_gtest(test_subscription test_subscription.cpp TIMEOUT 120)
+if(TARGET test_subscription)
+  ament_target_dependencies(test_subscription
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+    "test_msgs"
+  )
+  target_link_libraries(test_subscription ${PROJECT_NAME} mimick)
+endif()
+ament_add_gtest(test_subscription_publisher_count_api test_subscription_publisher_count_api.cpp)
+if(TARGET test_subscription_publisher_count_api)
+  ament_target_dependencies(test_subscription_publisher_count_api
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+    "test_msgs"
+  )
+  target_link_libraries(test_subscription_publisher_count_api ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_subscription_traits test_subscription_traits.cpp)
+if(TARGET test_subscription_traits)
+  ament_target_dependencies(test_subscription_traits
+    "rcl"
+    "test_msgs"
+  )
+  target_link_libraries(test_subscription_traits ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_type_support test_type_support.cpp)
+if(TARGET test_type_support)
+  ament_target_dependencies(test_type_support
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+    "test_msgs"
+  )
+  target_link_libraries(test_type_support ${PROJECT_NAME})
+endif()
+ament_add_gtest(test_find_weak_nodes test_find_weak_nodes.cpp)
+if(TARGET test_find_weak_nodes)
+  ament_target_dependencies(test_find_weak_nodes
+    "rcl"
+  )
+  target_link_libraries(test_find_weak_nodes ${PROJECT_NAME})
+endif()
+
+set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}")
+if(WIN32)
+  set(append_library_dirs "${append_library_dirs}/$<CONFIG>")
+endif()
+
+ament_add_gtest(test_externally_defined_services test_externally_defined_services.cpp)
+ament_target_dependencies(test_externally_defined_services
+  "rcl"
+  "test_msgs"
+)
+target_link_libraries(test_externally_defined_services ${PROJECT_NAME})
+
+ament_add_gtest(test_duration test_duration.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_duration)
+  ament_target_dependencies(test_duration
+    "rcl")
+  target_link_libraries(test_duration ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_logger test_logger.cpp)
+target_link_libraries(test_logger ${PROJECT_NAME})
+
+ament_add_gmock(test_logging test_logging.cpp)
+target_link_libraries(test_logging ${PROJECT_NAME})
+
+ament_add_gtest(test_time test_time.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_time)
+  ament_target_dependencies(test_time
+    "rcl")
+  target_link_libraries(test_time ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_timer test_timer.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_timer)
+  ament_target_dependencies(test_timer
+    "rcl")
+  target_link_libraries(test_timer ${PROJECT_NAME} mimick)
+endif()
+
+ament_add_gtest(test_time_source test_time_source.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_time_source)
+  ament_target_dependencies(test_time_source
+    "rcl")
+  target_link_libraries(test_time_source ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_utilities test_utilities.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_utilities)
+  ament_target_dependencies(test_utilities
+    "rcl")
+  target_link_libraries(test_utilities ${PROJECT_NAME} mimick)
+endif()
+
+ament_add_gtest(test_init test_init.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_init)
+  ament_target_dependencies(test_init
+    "rcl")
+  target_link_libraries(test_init ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_interface_traits test_interface_traits.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_interface_traits)
+  ament_target_dependencies(test_interface_traits
+    "rcl")
+  target_link_libraries(test_interface_traits ${PROJECT_NAME})
+endif()
+
+# TODO(brawner) remove when destroying Node for Connext is resolved. See:
+# https://github.com/ros2/rclcpp/issues/1250
+ament_add_gtest(
+  test_executors
+  executors/test_executors.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}"
+  TIMEOUT 180)
+if(TARGET test_executors)
+  ament_target_dependencies(test_executors
+    "rcl"
+    "test_msgs")
+  target_link_libraries(test_executors ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_static_single_threaded_executor executors/test_static_single_threaded_executor.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_static_single_threaded_executor)
+  ament_target_dependencies(test_static_single_threaded_executor
+    "test_msgs")
+  target_link_libraries(test_static_single_threaded_executor ${PROJECT_NAME} mimick)
+endif()
+
+ament_add_gtest(test_multi_threaded_executor executors/test_multi_threaded_executor.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_multi_threaded_executor)
+  ament_target_dependencies(test_multi_threaded_executor
+    "rcl")
+  target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_static_executor_entities_collector executors/test_static_executor_entities_collector.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120)
+if(TARGET test_static_executor_entities_collector)
+  ament_target_dependencies(test_static_executor_entities_collector
+    "rcl"
+    "test_msgs")
+  target_link_libraries(test_static_executor_entities_collector ${PROJECT_NAME} mimick)
+endif()
+
+ament_add_gtest(test_guard_condition test_guard_condition.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_guard_condition)
+  target_link_libraries(test_guard_condition ${PROJECT_NAME} mimick)
+endif()
+
+ament_add_gtest(test_wait_set test_wait_set.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}")
+if(TARGET test_wait_set)
+  ament_target_dependencies(test_wait_set "test_msgs")
+  target_link_libraries(test_wait_set ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_subscription_topic_statistics topic_statistics/test_subscription_topic_statistics.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}"
+)
+if(TARGET test_subscription_topic_statistics)
+  ament_target_dependencies(test_subscription_topic_statistics
+    "builtin_interfaces"
+    "libstatistics_collector"
+    "rcl_interfaces"
+    "rcutils"
+    "rmw"
+    "rosidl_runtime_cpp"
+    "rosidl_typesupport_cpp"
+    "statistics_msgs"
+    "test_msgs")
+  rosidl_target_interfaces(test_subscription_topic_statistics ${PROJECT_NAME}_test_msgs "rosidl_typesupport_cpp")
+  target_link_libraries(test_subscription_topic_statistics ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_subscription_options test_subscription_options.cpp)
+if(TARGET test_subscription_options)
+  ament_target_dependencies(test_subscription_options "rcl")
+  target_link_libraries(test_subscription_options ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_dynamic_storage wait_set_policies/test_dynamic_storage.cpp)
+if(TARGET test_dynamic_storage)
+  ament_target_dependencies(test_dynamic_storage "rcl" "test_msgs")
+  target_link_libraries(test_dynamic_storage ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_storage_policy_common wait_set_policies/test_storage_policy_common.cpp)
+if(TARGET test_storage_policy_common)
+  ament_target_dependencies(test_storage_policy_common "rcl" "test_msgs")
+  target_link_libraries(test_storage_policy_common ${PROJECT_NAME} mimick)
+endif()
+
+ament_add_gtest(test_static_storage wait_set_policies/test_static_storage.cpp)
+if(TARGET test_static_storage)
+  ament_target_dependencies(test_static_storage "rcl" "test_msgs")
+  target_link_libraries(test_static_storage ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_thread_safe_synchronization wait_set_policies/test_thread_safe_synchronization.cpp)
+if(TARGET test_thread_safe_synchronization)
+  ament_target_dependencies(test_thread_safe_synchronization "rcl" "test_msgs")
+  target_link_libraries(test_thread_safe_synchronization ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_rosout_qos test_rosout_qos.cpp)
+if(TARGET test_rosout_qos)
+  ament_target_dependencies(test_rosout_qos "rcl")
+  target_link_libraries(test_rosout_qos ${PROJECT_NAME})
+endif()
+
+ament_add_gtest(test_executor test_executor.cpp
+  APPEND_LIBRARY_DIRS "${append_library_dirs}"
+  TIMEOUT 120)
+if(TARGET test_executor)
+  ament_target_dependencies(test_executor "rcl")
+  target_link_libraries(test_executor ${PROJECT_NAME} mimick)
+endif()
+
+ament_add_gtest(test_graph_listener test_graph_listener.cpp)
+if(TARGET test_graph_listener)
+  target_link_libraries(test_graph_listener ${PROJECT_NAME} mimick)
+endif()

rclcpp/test/rclcpp/allocator/test_allocator_common.cpp

@@ -0,0 +1,67 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+
+#include "rclcpp/allocator/allocator_common.hpp"
+
+TEST(TestAllocatorCommon, retyped_allocate) {
+  std::allocator<int> allocator;
+  void * untyped_allocator = &allocator;
+  void * allocated_mem =
+    rclcpp::allocator::retyped_allocate<std::allocator<char>>(1u, untyped_allocator);
+  ASSERT_NE(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);
+  ASSERT_NE(nullptr, allocated_mem);
+  void * reallocated_mem =
+    rclcpp::allocator::retyped_reallocate<int, std::allocator<int>>(
+    allocated_mem, 2u, untyped_allocator);
+  ASSERT_NE(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);
+  EXPECT_NE(nullptr, rcl_allocator.allocate);
+  EXPECT_NE(nullptr, rcl_allocator.deallocate);
+  EXPECT_NE(nullptr, rcl_allocator.reallocate);
+  EXPECT_NE(nullptr, rcl_allocator.zero_allocate);
+  // Not testing state as that may or may not be null depending on platform
+}
+
+TEST(TestAllocatorCommon, get_void_rcl_allocator) {
+  std::allocator<void> allocator;
+  auto rcl_allocator =
+    rclcpp::allocator::get_rcl_allocator<void, std::allocator<void>>(allocator);
+  EXPECT_NE(nullptr, rcl_allocator.allocate);
+  EXPECT_NE(nullptr, rcl_allocator.deallocate);
+  EXPECT_NE(nullptr, rcl_allocator.reallocate);
+  EXPECT_NE(nullptr, rcl_allocator.zero_allocate);
+  // Not testing state as that may or may not be null depending on platform
+}

rclcpp/test/rclcpp/allocator/test_allocator_deleter.cpp

@@ -0,0 +1,101 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+
+#include "rclcpp/allocator/allocator_deleter.hpp"
+
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
+TEST(TestAllocatorDeleter, construct_destruct) {
+  std::allocator<int> allocator;
+
+  rclcpp::allocator::AllocatorDeleter<std::allocator<int>> deleter;
+  EXPECT_EQ(nullptr, deleter.get_allocator());
+  deleter.set_allocator(&allocator);
+  EXPECT_EQ(&allocator, deleter.get_allocator());
+
+  rclcpp::allocator::AllocatorDeleter<std::allocator<int>> deleter2(&allocator);
+  EXPECT_EQ(&allocator, deleter2.get_allocator());
+
+  rclcpp::allocator::AllocatorDeleter<std::allocator<int>> deleter3(deleter2);
+  EXPECT_EQ(&allocator, deleter3.get_allocator());
+}
+
+TEST(TestAllocatorDeleter, delete) {
+  std::allocator<int> allocator;
+  int * some_mem = allocator.allocate(1u);
+  ASSERT_NE(nullptr, some_mem);
+
+  rclcpp::allocator::AllocatorDeleter<std::allocator<int>> deleter(&allocator);
+  EXPECT_NO_THROW(deleter(some_mem));
+}
+
+TEST(TestAllocatorDeleter, set_allocator_for_deleter_AllocatorDeleter) {
+  using AllocatorT = std::allocator<int>;
+  using DeleterT = rclcpp::allocator::AllocatorDeleter<AllocatorT>;
+  AllocatorT allocator;
+  DeleterT deleter(&allocator);
+
+  std::allocator<int> allocator2;
+  rclcpp::allocator::set_allocator_for_deleter<AllocatorT, AllocatorT>(&deleter, &allocator2);
+  EXPECT_EQ(&allocator2, deleter.get_allocator());
+
+  auto throwing_statement = [&allocator]() {
+      DeleterT * null_del_ptr = nullptr;
+      rclcpp::allocator::set_allocator_for_deleter<AllocatorT, AllocatorT>(
+        null_del_ptr, &allocator);
+    };
+  RCLCPP_EXPECT_THROW_EQ(
+    throwing_statement(),
+    std::invalid_argument("Argument was NULL to set_allocator_for_deleter"));
+
+  auto throwing_statement2 = [&deleter]() {
+      AllocatorT * null_alloc_ptr = nullptr;
+      rclcpp::allocator::set_allocator_for_deleter<AllocatorT, AllocatorT>(
+        &deleter, null_alloc_ptr);
+    };
+
+  RCLCPP_EXPECT_THROW_EQ(
+    throwing_statement2(),
+    std::invalid_argument("Argument was NULL to set_allocator_for_deleter"));
+}
+
+TEST(TestAllocatorDeleter, set_allocator_for_deleter_std_default_delete) {
+  using AllocatorT = std::allocator<int>;
+  using DeleterT = std::default_delete<int>;
+  auto not_throwing_statement = []() {
+      AllocatorT allocator;
+      DeleterT deleter;
+      rclcpp::allocator::set_allocator_for_deleter<int, int>(&deleter, &allocator);
+    };
+  EXPECT_NO_THROW(not_throwing_statement());
+}
+
+TEST(TestAllocatorDeleter, set_allocator_for_deleter_unexpected_template) {
+  class SomeAllocatorClass {};
+  class SomeDeleterClass {};
+  using AllocatorT = SomeAllocatorClass;
+  using DeleterT = SomeDeleterClass;
+  auto throwing_statement = []() {
+      DeleterT deleter;
+      AllocatorT allocator;
+      rclcpp::allocator::set_allocator_for_deleter<AllocatorT, int, DeleterT>(&deleter, &allocator);
+    };
+  RCLCPP_EXPECT_THROW_EQ(
+    throwing_statement(),
+    std::runtime_error("Reached unexpected template specialization"));
+}

rclcpp/test/rclcpp/benchmark/CMakeLists.txt

@@ -0,0 +1,29 @@
+# Give cppcheck hints about macro definitions coming from outside this package
+get_target_property(
+  ament_cmake_cppcheck_ADDITIONAL_INCLUDE_DIRS
+  performance_test_fixture::performance_test_fixture
+  INTERFACE_INCLUDE_DIRECTORIES)
+
+function(add_rclcpp_benchmark NAME SOURCE)
+  cmake_parse_arguments(RCLCPP_BENCHMARK "" "LIBRARIES" "")
+  if(RCLCPP_UNPARSED_ARGUMENTS)
+    message(FATAL_ERROR "Unrecognized arguments for 'add_rclcpp_benchmark' (${RCLCPP_UNPARSED_ARGUMENTS})")
+    return()
+  endif()
+  find_package(${rmw_implementation} REQUIRED)
+  message(STATUS "Adding ${NAME} for '${rmw_implementation}'")
+  set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation})
+
+  set(full_benchmark_name ${NAME}${target_suffix})
+  add_performance_test(${full_benchmark_name} ${SOURCE})
+  if(TARGET ${full_benchmark_name})
+    target_link_libraries(${full_benchmark_name} ${PROJECT_NAME} ${LIBRARIES})
+  endif()
+endfunction()
+
+# Add new benchmarks inside this macro
+macro(rclcpp_benchmarks)
+  add_rclcpp_benchmark(benchmark_client benchmark_client.cpp)
+endmacro()
+
+call_for_each_rmw_implementation(rclcpp_benchmarks)

rclcpp/test/rclcpp/benchmark/benchmark_client.cpp

@@ -0,0 +1,13 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.

rclcpp/test/rclcpp/exceptions/test_exceptions.cpp

@@ -0,0 +1,58 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "rclcpp/exceptions/exceptions.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
+TEST(TestExceptions, throw_from_rcl_error) {
+  EXPECT_THROW(
+    rclcpp::exceptions::throw_from_rcl_error(RCL_RET_BAD_ALLOC, ""),
+    rclcpp::exceptions::RCLBadAlloc);
+
+  EXPECT_THROW(
+    rclcpp::exceptions::throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, ""),
+    rclcpp::exceptions::RCLInvalidArgument);
+
+  EXPECT_THROW(
+    rclcpp::exceptions::throw_from_rcl_error(RCL_RET_INVALID_ROS_ARGS, ""),
+    rclcpp::exceptions::RCLInvalidROSArgsError);
+
+  EXPECT_THROW(
+    rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, ""),
+    rclcpp::exceptions::RCLError);
+
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::exceptions::throw_from_rcl_error(RCL_RET_OK, ""),
+    std::invalid_argument("ret is RCL_RET_OK"));
+
+  {
+    auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_get_error_state, nullptr);
+    RCLCPP_EXPECT_THROW_EQ(
+      rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, ""),
+      std::runtime_error("rcl error state is not set"));
+  }
+}
+
+TEST(TestExceptions, basic_constructors) {
+  EXPECT_STREQ("node is invalid", rclcpp::exceptions::InvalidNodeError().what());
+  rcl_error_state_t error_state{"error", __FILE__, __LINE__};
+  EXPECT_STREQ(
+    "prefix: error not set",
+    rclcpp::exceptions::RCLInvalidROSArgsError(RCL_RET_ERROR, &error_state, "prefix: ").what());
+}

rclcpp/test/rclcpp/executors/test_executors.cpp

@@ -410,6 +410,14 @@ public:
     }
   }
 
+  ~TestWaitable()
+  {
+    rcl_ret_t ret = rcl_guard_condition_fini(&gc_);
+    if (RCL_RET_OK != ret) {
+      fprintf(stderr, "failed to call rcl_guard_condition_fini\n");
+    }
+  }
+
   bool
   add_to_wait_set(rcl_wait_set_t * wait_set) override
   {
@@ -526,3 +534,72 @@ TYPED_TEST(TestExecutorsStable, spinSome) {
 
   spinner.join();
 }
+
+// Check spin_node_until_future_complete with node base pointer
+TYPED_TEST(TestExecutorsStable, testSpinNodeUntilFutureCompleteNodeBasePtr) {
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+
+  std::promise<bool> promise;
+  std::future<bool> future = promise.get_future();
+  promise.set_value(true);
+
+  auto shared_future = future.share();
+  auto ret = rclcpp::executors::spin_node_until_future_complete(
+    executor, this->node->get_node_base_interface(), shared_future, 1s);
+  EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret);
+}
+
+// Check spin_node_until_future_complete with node pointer
+TYPED_TEST(TestExecutorsStable, testSpinNodeUntilFutureCompleteNodePtr) {
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+
+  std::promise<bool> promise;
+  std::future<bool> future = promise.get_future();
+  promise.set_value(true);
+
+  auto shared_future = future.share();
+  auto ret = rclcpp::executors::spin_node_until_future_complete(
+    executor, this->node, shared_future, 1s);
+  EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret);
+}
+
+// Check spin_until_future_complete with node base pointer (instantiates its own executor)
+TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr) {
+  rclcpp::init(0, nullptr);
+
+  {
+    auto node = std::make_shared<rclcpp::Node>("node");
+
+    std::promise<bool> promise;
+    std::future<bool> future = promise.get_future();
+    promise.set_value(true);
+
+    auto shared_future = future.share();
+    auto ret = rclcpp::spin_until_future_complete(
+      node->get_node_base_interface(), shared_future, 1s);
+    EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret);
+  }
+
+  rclcpp::shutdown();
+}
+
+// Check spin_until_future_complete with node pointer (instantiates its own executor)
+TEST(TestExecutors, testSpinUntilFutureCompleteNodePtr) {
+  rclcpp::init(0, nullptr);
+
+  {
+    auto node = std::make_shared<rclcpp::Node>("node");
+
+    std::promise<bool> promise;
+    std::future<bool> future = promise.get_future();
+    promise.set_value(true);
+
+    auto shared_future = future.share();
+    auto ret = rclcpp::spin_until_future_complete(node, shared_future, 1s);
+    EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret);
+  }
+
+  rclcpp::shutdown();
+}

rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp

@@ -14,13 +14,18 @@
 
 #include <gtest/gtest.h>
 
+#include <chrono>
 #include <memory>
+#include <stdexcept>
 
 #include "rclcpp/rclcpp.hpp"
 
 #include "test_msgs/msg/empty.hpp"
 #include "test_msgs/srv/empty.hpp"
 
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
 namespace
 {
 
@@ -105,7 +110,9 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node) {
   EXPECT_NO_THROW(entities_collector_->add_node(node1->get_node_base_interface()));
 
   // Check adding second time
-  EXPECT_THROW(entities_collector_->add_node(node1->get_node_base_interface()), std::runtime_error);
+  RCLCPP_EXPECT_THROW_EQ(
+    entities_collector_->add_node(node1->get_node_base_interface()),
+    std::runtime_error("Node has already been added to an executor."));
 
   auto node2 = std::make_shared<rclcpp::Node>("node2", "ns");
   EXPECT_FALSE(entities_collector_->remove_node(node2->get_node_base_interface()));
@@ -114,6 +121,10 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node) {
   EXPECT_TRUE(entities_collector_->remove_node(node1->get_node_base_interface()));
   EXPECT_FALSE(entities_collector_->remove_node(node1->get_node_base_interface()));
   EXPECT_TRUE(entities_collector_->remove_node(node2->get_node_base_interface()));
+
+  auto node3 = std::make_shared<rclcpp::Node>("node3", "ns");
+  node3->get_node_base_interface()->get_associated_with_executor_atomic().exchange(true);
+  EXPECT_FALSE(entities_collector_->remove_node(node3->get_node_base_interface()));
 }
 
 TEST_F(TestStaticExecutorEntitiesCollector, init_bad_arguments) {
@@ -159,6 +170,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_basic_node) {
   rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
 
   entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
   EXPECT_EQ(
     expected_number_of_entities->subscriptions,
     entities_collector_->get_number_of_subscriptions());
@@ -206,6 +218,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_out_of_scope) {
 
   // Expect weak_node pointers to be cleaned up and used
   entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
   EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions());
   EXPECT_EQ(0u, entities_collector_->get_number_of_timers());
   EXPECT_EQ(0u, entities_collector_->get_number_of_services());
@@ -267,6 +280,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) {
   rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
 
   entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
 
   EXPECT_EQ(
     1u + expected_number_of_entities->subscriptions,
@@ -293,3 +307,316 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) {
   // Still one for the executor
   EXPECT_EQ(1u, entities_collector_->get_number_of_waitables());
 }
+
+TEST_F(TestStaticExecutorEntitiesCollector, add_callback_group) {
+  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
+  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, add_callback_group_after_add_node) {
+  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  entities_collector_->add_node(node->get_node_base_interface());
+  RCLCPP_EXPECT_THROW_EQ(
+    entities_collector_->add_callback_group(cb_group, node->get_node_base_interface()),
+    std::runtime_error("Callback group has already been added to an executor."));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, add_callback_group_twice) {
+  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
+  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
+  cb_group->get_associated_with_executor_atomic().exchange(false);
+  RCLCPP_EXPECT_THROW_EQ(
+    entities_collector_->add_callback_group(cb_group, node->get_node_base_interface()),
+    std::runtime_error("Callback group was already added to executor."));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_clear_error) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+
+  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
+  rclcpp::GuardCondition guard_condition(shared_context);
+  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
+
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+
+  {
+    auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      entities_collector_->execute(),
+      std::runtime_error("Couldn't clear wait set"));
+  }
+
+  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_resize_error) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+
+  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
+  rclcpp::GuardCondition guard_condition(shared_context);
+  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
+
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+
+  {
+    auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_resize, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      entities_collector_->execute(),
+      std::runtime_error("Couldn't resize the wait set: error not set"));
+  }
+
+  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_not_initialized) {
+  RCLCPP_EXPECT_THROW_EQ(
+    entities_collector_->refresh_wait_set(std::chrono::nanoseconds(1000)),
+    std::runtime_error("Couldn't clear wait set"));
+  rcl_reset_error();
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_rcl_wait_failed) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+
+  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
+  rclcpp::GuardCondition guard_condition(shared_context);
+  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
+
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+
+  {
+    auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      entities_collector_->refresh_wait_set(std::chrono::nanoseconds(1000)),
+      std::runtime_error("rcl_wait() failed: error not set"));
+  }
+
+  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_add_handles_to_wait_set_failed) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+
+  // Create 1 of each entity type
+  auto subscription =
+    node->create_subscription<test_msgs::msg::Empty>(
+    "topic", rclcpp::QoS(10), [](test_msgs::msg::Empty::SharedPtr) {});
+  auto timer =
+    node->create_wall_timer(std::chrono::seconds(60), []() {});
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(
+    "service",
+    [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {});
+  auto client = node->create_client<test_msgs::srv::Empty>("service");
+  auto waitable = std::make_shared<TestWaitable>();
+
+  node->get_node_waitables_interface()->add_waitable(waitable, nullptr);
+
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+
+  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
+
+  rclcpp::GuardCondition guard_condition(shared_context);
+  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
+
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_wait_set_add_subscription,
+      RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      entities_collector_->refresh_wait_set(std::chrono::nanoseconds(1000)),
+      std::runtime_error("Couldn't fill wait set"));
+  }
+
+  entities_collector_->remove_node(node->get_node_base_interface());
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, add_to_wait_set_nullptr) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+
+  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
+  rclcpp::GuardCondition guard_condition(shared_context);
+  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
+
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+
+  RCLCPP_EXPECT_THROW_EQ(
+    entities_collector_->add_to_wait_set(nullptr),
+    std::runtime_error("Executor waitable: couldn't add guard condition to wait set"));
+  rcl_reset_error();
+
+  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, fill_memory_strategy_invalid_group) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+
+  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
+  rclcpp::GuardCondition guard_condition(shared_context);
+  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
+
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
+  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 2u);
+
+  cb_group.reset();
+
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
+
+  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, remove_callback_group_after_node) {
+  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
+  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
+
+  node.reset();
+
+  RCLCPP_EXPECT_THROW_EQ(
+    entities_collector_->remove_callback_group(cb_group),
+    std::runtime_error("Node must not be deleted before its callback group(s)."));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, remove_callback_group_twice) {
+  auto node = std::make_shared<rclcpp::Node>("node1", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
+  ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u);
+
+  entities_collector_->remove_callback_group(cb_group);
+
+  RCLCPP_EXPECT_THROW_EQ(
+    entities_collector_->remove_callback_group(cb_group),
+    std::runtime_error("Callback group needs to be associated with executor."));
+}
+
+TEST_F(TestStaticExecutorEntitiesCollector, remove_node_opposite_order) {
+  auto node1 = std::make_shared<rclcpp::Node>("node1", "ns");
+  EXPECT_NO_THROW(entities_collector_->add_node(node1->get_node_base_interface()));
+
+  auto node2 = std::make_shared<rclcpp::Node>("node2", "ns");
+  EXPECT_NO_THROW(entities_collector_->add_node(node2->get_node_base_interface()));
+
+  EXPECT_TRUE(entities_collector_->remove_node(node2->get_node_base_interface()));
+}
+
+TEST_F(
+  TestStaticExecutorEntitiesCollector,
+  add_callback_groups_from_nodes_associated_to_executor_add) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  entities_collector_->add_callback_group(cb_group, node->get_node_base_interface());
+  entities_collector_->add_node(node->get_node_base_interface());
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto shared_context = node->get_node_base_interface()->get_context();
+  rcl_context_t * context = shared_context->get_rcl_context().get();
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator));
+  RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));});
+
+  auto memory_strategy = rclcpp::memory_strategies::create_default_strategy();
+  rclcpp::GuardCondition guard_condition(shared_context);
+  rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition();
+
+  entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition);
+  RCLCPP_SCOPE_EXIT(entities_collector_->fini());
+
+  cb_group->get_associated_with_executor_atomic().exchange(false);
+  entities_collector_->execute();
+
+  EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface()));
+}

rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp

@@ -15,13 +15,20 @@
 #include <gtest/gtest.h>
 
 #include <chrono>
+#include <future>
 #include <memory>
+#include <stdexcept>
 
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp/executors.hpp"
 
+#include "test_msgs/srv/empty.hpp"
+
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
 using namespace std::chrono_literals;
 
 class TestStaticSingleThreadedExecutor : public ::testing::Test
@@ -47,3 +54,99 @@ TEST_F(TestStaticSingleThreadedExecutor, check_unimplemented) {
   EXPECT_THROW(executor.spin_all(0ns), rclcpp::exceptions::UnimplementedError);
   EXPECT_THROW(executor.spin_once(0ns), rclcpp::exceptions::UnimplementedError);
 }
+
+TEST_F(TestStaticSingleThreadedExecutor, add_callback_group_trigger_guard_failed) {
+  rclcpp::executors::StaticSingleThreadedExecutor executor;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      executor.add_callback_group(cb_group, node->get_node_base_interface(), true),
+      std::runtime_error("error not set"));
+  }
+}
+
+TEST_F(TestStaticSingleThreadedExecutor, add_node_trigger_guard_failed) {
+  rclcpp::executors::StaticSingleThreadedExecutor executor;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      executor.add_node(node),
+      std::runtime_error("error not set"));
+  }
+}
+
+TEST_F(TestStaticSingleThreadedExecutor, remove_callback_group_trigger_guard_failed) {
+  rclcpp::executors::StaticSingleThreadedExecutor executor;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  executor.add_callback_group(cb_group, node->get_node_base_interface(), true);
+
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      executor.remove_callback_group(cb_group, true),
+      std::runtime_error("error not set"));
+  }
+}
+
+TEST_F(TestStaticSingleThreadedExecutor, remove_node_failed) {
+  rclcpp::executors::StaticSingleThreadedExecutor executor;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      executor.remove_node(node, true),
+      std::runtime_error("Node needs to be associated with this executor."));
+  }
+}
+
+TEST_F(TestStaticSingleThreadedExecutor, remove_node_trigger_guard_failed) {
+  rclcpp::executors::StaticSingleThreadedExecutor executor;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+
+  executor.add_node(node);
+
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      executor.remove_node(node, true),
+      std::runtime_error("error not set"));
+  }
+}
+
+TEST_F(TestStaticSingleThreadedExecutor, execute_service) {
+  rclcpp::executors::StaticSingleThreadedExecutor executor;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  executor.add_node(node);
+
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(
+    "service",
+    [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {});
+  auto client = node->create_client<test_msgs::srv::Empty>("service");
+
+  std::promise<void> promise;
+  std::future<void> future = promise.get_future();
+  EXPECT_EQ(
+    rclcpp::FutureReturnCode::TIMEOUT,
+    executor.spin_until_future_complete(future, std::chrono::milliseconds(1)));
+
+  executor.remove_node(node);
+  executor.spin_until_future_complete(future, std::chrono::milliseconds(1));
+}

rclcpp/test/rclcpp/node_interfaces/test_node_base.cpp

@@ -21,6 +21,11 @@
 #include "rclcpp/node.hpp"
 #include "rclcpp/node_interfaces/node_base.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "rmw/validate_namespace.h"
+#include "rmw/validate_node_name.h"
+
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
 
 class TestNodeBase : public ::testing::Test
 {
@@ -36,6 +41,12 @@ public:
   }
 };
 
+// Required for mocking_utils below
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, ==)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, !=)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, <)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, >)
+
 TEST_F(TestNodeBase, construct_from_node)
 {
   std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node", "ns");
@@ -58,3 +69,125 @@ TEST_F(TestNodeBase, construct_from_node)
   EXPECT_NE(nullptr, const_node_base->get_rcl_node_handle());
   EXPECT_NE(nullptr, const_node_base->get_shared_rcl_node_handle());
 }
+
+TEST_F(TestNodeBase, construct_destruct_rcl_guard_condition_init_error) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_guard_condition_init, RCL_RET_ERROR);
+  EXPECT_THROW(
+    std::make_shared<rclcpp::Node>("node", "ns").reset(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_node_init_error) {
+  auto mock_node_init = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_node_init, RCL_RET_ERROR);
+
+  // This function is called only if rcl_node_init fails, so both mocked functions are required
+  // This just logs an error, so behavior shouldn't change
+  auto mock_guard_condition_fini = mocking_utils::inject_on_return(
+    "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR);
+
+  EXPECT_THROW(
+    std::make_shared<rclcpp::Node>("node", "ns").reset(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_node_name) {
+  auto mock_node_init = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAME);
+
+  // `rmw_validate_node_name` is only called if `rcl_node_init` returns INVALID_NAME
+  auto mock_validate_node_name = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_node_name, RMW_RET_ERROR);
+
+  EXPECT_THROW(
+    std::make_shared<rclcpp::Node>("node", "ns").reset(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_node_name_invalid_argument) {
+  auto mock_node_init = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAME);
+
+  // `rmw_validate_node_name` is only called if `rcl_node_init` returns INVALID_NAME
+  auto mock_validate_node_name = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_node_name, RMW_RET_INVALID_ARGUMENT);
+
+  EXPECT_THROW(
+    std::make_shared<rclcpp::Node>("node", "ns").reset(),
+    rclcpp::exceptions::RCLInvalidArgument);
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_node_name_valid_rmw_node_name) {
+  auto mock_node_init = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAME);
+
+  // `rmw_validate_node_name` is only called if `rcl_node_init` returns INVALID_NAME
+  auto mock = mocking_utils::patch(
+    "lib:rclcpp", rmw_validate_node_name, [](const char *, int * validation_result, size_t *)
+    {
+      *validation_result = RMW_NODE_NAME_VALID;
+      return RMW_RET_OK;
+    });
+
+  RCLCPP_EXPECT_THROW_EQ(
+    std::make_shared<rclcpp::Node>("node", "ns").reset(),
+    std::runtime_error("valid rmw node name but invalid rcl node name"));
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_namespace) {
+  auto mock_node_init = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAMESPACE);
+
+  // `rmw_validate_namespace` is only called if `rcl_node_init` returns INVALID_NAMESPACE
+  auto mock_validate_namespace = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_namespace, RMW_RET_ERROR);
+
+  EXPECT_THROW(
+    std::make_shared<rclcpp::Node>("node", "ns").reset(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_namespace_rmw_invalid_argument) {
+  auto mock_node_init = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAMESPACE);
+
+  // `rmw_validate_namespace` is only called if `rcl_node_init` returns INVALID_NAMESPACE
+  auto mock_validate_namespace = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_namespace, RMW_RET_INVALID_ARGUMENT);
+
+  EXPECT_THROW(
+    std::make_shared<rclcpp::Node>("node", "ns").reset(),
+    rclcpp::exceptions::RCLInvalidArgument);
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_namespace_valid_rmw_namespace) {
+  auto mock_node_init = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAMESPACE);
+
+  // `rmw_validate_namespace` is only called if `rcl_node_init` returns INVALID_NAMESPACE
+  auto mock = mocking_utils::patch(
+    "lib:rclcpp", rmw_validate_namespace, [](const char *, int * validation_result, size_t *)
+    {
+      *validation_result = RMW_NAMESPACE_VALID;
+      return RMW_RET_OK;
+    });
+
+  RCLCPP_EXPECT_THROW_EQ(
+    std::make_shared<rclcpp::Node>("node", "ns").reset(),
+    std::runtime_error("valid rmw node namespace but invalid rcl node namespace"));
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_node_init_fini_error) {
+  auto mock_node_fini = mocking_utils::inject_on_return(
+    "lib:rclcpp", rcl_node_fini, RCL_RET_ERROR);
+
+  EXPECT_NO_THROW(std::make_shared<rclcpp::Node>("node", "ns").reset());
+}
+
+TEST_F(TestNodeBase, construct_destruct_rcl_guard_condition_fini_error) {
+  auto mock_node_fini = mocking_utils::inject_on_return(
+    "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR);
+
+  EXPECT_NO_THROW(std::make_shared<rclcpp::Node>("node", "ns").reset());
+}

rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp

@@ -15,105 +15,172 @@
 #include <gtest/gtest.h>
 
 #include <memory>
+#include <string>
 #include <utility>
 
+#include "rcl/graph.h"
 #include "rcl/node_options.h"
+#include "rcl/remap.h"
 #include "rclcpp/node.hpp"
 #include "rclcpp/node_interfaces/node_base.hpp"
 #include "rclcpp/node_interfaces/node_graph.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "rcutils/strdup.h"
 #include "test_msgs/msg/empty.hpp"
 #include "test_msgs/srv/empty.hpp"
 
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
+namespace
+{
+
+constexpr char node_name[] = "node";
+constexpr char node_namespace[] = "ns";
+constexpr char absolute_namespace[] = "/ns";
+
+}  // namespace
+
 class TestNodeGraph : public ::testing::Test
 {
 public:
   void SetUp()
   {
     rclcpp::init(0, nullptr);
+    node_ = std::make_shared<rclcpp::Node>(node_name, node_namespace);
+
+    // This dynamic cast is not necessary for the unittests, but instead is used to ensure
+    // the proper type is being tested and covered.
+    node_graph_ =
+      dynamic_cast<rclcpp::node_interfaces::NodeGraph *>(node_->get_node_graph_interface().get());
+    ASSERT_NE(nullptr, node_graph_);
   }
 
   void TearDown()
   {
     rclcpp::shutdown();
   }
+
+protected:
+  std::shared_ptr<rclcpp::Node> node() {return node_;}
+
+  const rclcpp::node_interfaces::NodeGraph * node_graph() const {return node_graph_;}
+
+private:
+  std::shared_ptr<rclcpp::Node> node_;
+  rclcpp::node_interfaces::NodeGraph * node_graph_;
 };
 
 TEST_F(TestNodeGraph, construct_from_node)
 {
-  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node", "ns");
-
-  // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
-  // the proper type is being tested and covered.
-  const auto * node_graph =
-    dynamic_cast<rclcpp::node_interfaces::NodeGraph *>(node->get_node_graph_interface().get());
-  ASSERT_NE(nullptr, node_graph);
-
-  auto topic_names_and_types = node_graph->get_topic_names_and_types(false);
+  auto topic_names_and_types = node_graph()->get_topic_names_and_types(false);
   EXPECT_LT(0u, topic_names_and_types.size());
 
-  auto service_names_and_types = node_graph->get_service_names_and_types();
+  auto service_names_and_types = node_graph()->get_service_names_and_types();
   EXPECT_LT(0u, service_names_and_types.size());
 
-  auto names = node_graph->get_node_names();
+  auto names = node_graph()->get_node_names();
   EXPECT_EQ(1u, names.size());
 
-  auto names_and_namespaces = node_graph->get_node_names_and_namespaces();
+  auto names_and_namespaces = node_graph()->get_node_names_and_namespaces();
   EXPECT_EQ(1u, names_and_namespaces.size());
 
-  EXPECT_EQ(0u, node_graph->count_publishers("not_a_topic"));
-  EXPECT_EQ(0u, node_graph->count_subscribers("not_a_topic"));
+  EXPECT_EQ(0u, node_graph()->count_publishers("not_a_topic"));
+  EXPECT_EQ(0u, node_graph()->count_subscribers("not_a_topic"));
+  EXPECT_NE(nullptr, node_graph()->get_graph_guard_condition());
+
+  // get_graph_event is non-const
+  EXPECT_NE(nullptr, node()->get_node_graph_interface()->get_graph_event());
+  EXPECT_EQ(1u, node_graph()->count_graph_users());
 }
 
 TEST_F(TestNodeGraph, get_topic_names_and_types)
 {
-  auto node = std::make_shared<rclcpp::Node>("node2", "ns");
-  const auto * node_graph =
-    dynamic_cast<rclcpp::node_interfaces::NodeGraph *>(node->get_node_graph_interface().get());
-  ASSERT_NE(nullptr, node_graph);
-  auto topic_names_and_types = node_graph->get_topic_names_and_types();
+  auto topic_names_and_types = node_graph()->get_topic_names_and_types();
   EXPECT_LT(0u, topic_names_and_types.size());
 }
 
+TEST_F(TestNodeGraph, get_topic_names_and_types_rcl_error)
+{
+  auto mock_get_topic_names = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_topic_names_and_types, RCL_RET_ERROR);
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR);
+
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_topic_names_and_types(),
+    std::runtime_error(
+      "failed to get topic names and types: error not set, failed also to cleanup topic names and"
+      " types, leaking memory: error not set"));
+}
+
+TEST_F(TestNodeGraph, get_topic_names_and_types_rcl_names_and_types_fini_error)
+{
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR);
+
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_topic_names_and_types(),
+    std::runtime_error("could not destroy topic names and types: error not set"));
+}
+
 TEST_F(TestNodeGraph, get_service_names_and_types)
 {
-  auto node = std::make_shared<rclcpp::Node>("node2", "ns");
-  const auto * node_graph =
-    dynamic_cast<rclcpp::node_interfaces::NodeGraph *>(node->get_node_graph_interface().get());
-  ASSERT_NE(nullptr, node_graph);
-  auto service_names_and_types = node_graph->get_service_names_and_types();
+  auto service_names_and_types = node_graph()->get_service_names_and_types();
   EXPECT_LT(0u, service_names_and_types.size());
 }
 
+TEST_F(TestNodeGraph, get_service_names_and_types_rcl_error)
+{
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_service_names_and_types, RCL_RET_ERROR);
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_service_names_and_types(),
+    std::runtime_error(
+      "failed to get service names and types: error not set, failed also to cleanup service names"
+      " and types, leaking memory: error not set"));
+}
+
+TEST_F(TestNodeGraph, get_service_names_and_types_rcl_names_and_types_fini)
+{
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_service_names_and_types(),
+    std::runtime_error("could not destroy service names and types: error not set"));
+}
+
 TEST_F(TestNodeGraph, get_service_names_and_types_by_node)
 {
-  auto node1 = std::make_shared<rclcpp::Node>("node1", "ns");
   auto callback = [](
     const test_msgs::srv::Empty::Request::SharedPtr,
     test_msgs::srv::Empty::Response::SharedPtr) {};
   auto service =
-    node1->create_service<test_msgs::srv::Empty>("node1_service", std::move(callback));
-  auto node2 = std::make_shared<rclcpp::Node>("node2", "ns");
-  const auto * node_graph =
-    dynamic_cast<rclcpp::node_interfaces::NodeGraph *>(node1->get_node_graph_interface().get());
-  ASSERT_NE(nullptr, node_graph);
+    node()->create_service<test_msgs::srv::Empty>("node1_service", std::move(callback));
+
+  const std::string node2_name = "node2";
+  auto node2 = std::make_shared<rclcpp::Node>(node2_name, node_namespace);
 
   // rcl_get_service_names_and_types_by_node() expects the node to exist, otherwise it fails
   EXPECT_THROW(
-    node_graph->get_service_names_and_types_by_node("not_a_node", "not_absolute_namespace"),
+    node_graph()->get_service_names_and_types_by_node("not_a_node", "not_absolute_namespace"),
     std::runtime_error);
 
   // Check that node1_service exists for node1 but not node2. This shouldn't exercise graph
   // discovery as node_graph belongs to node1 anyway. This is just to test the API itself.
   auto services_of_node1 =
-    node_graph->get_service_names_and_types_by_node("node1", "/ns");
+    node_graph()->get_service_names_and_types_by_node(node_name, absolute_namespace);
   auto services_of_node2 =
-    node_graph->get_service_names_and_types_by_node("node2", "/ns");
+    node_graph()->get_service_names_and_types_by_node(node2_name, absolute_namespace);
 
   auto start = std::chrono::steady_clock::now();
   while (std::chrono::steady_clock::now() - start < std::chrono::seconds(1)) {
-    services_of_node1 = node_graph->get_service_names_and_types_by_node("node1", "/ns");
-    services_of_node2 = node_graph->get_service_names_and_types_by_node("node2", "/ns");
+    services_of_node1 =
+      node_graph()->get_service_names_and_types_by_node(node_name, absolute_namespace);
+    services_of_node2 =
+      node_graph()->get_service_names_and_types_by_node(node2_name, absolute_namespace);
     if (services_of_node1.find("/ns/node1_service") != services_of_node1.end()) {
       break;
     }
@@ -123,70 +190,134 @@ TEST_F(TestNodeGraph, get_service_names_and_types_by_node)
   EXPECT_FALSE(services_of_node2.find("/ns/node1_service") != services_of_node2.end());
 }
 
+TEST_F(TestNodeGraph, get_service_names_and_types_by_node_rcl_errors)
+{
+  auto callback = [](
+    const test_msgs::srv::Empty::Request::SharedPtr,
+    test_msgs::srv::Empty::Response::SharedPtr) {};
+  auto service =
+    node()->create_service<test_msgs::srv::Empty>("node1_service", std::move(callback));
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_service_names_and_types_by_node, RCL_RET_ERROR);
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_service_names_and_types_by_node(node_name, node_namespace),
+    std::runtime_error(
+      "failed to get service names and types by node: error not set, failed also to cleanup"
+      " service names and types, leaking memory: error not set"));
+}
+
+TEST_F(TestNodeGraph, get_service_names_and_types_by_node_names_and_types_fini_error)
+{
+  auto callback = [](
+    const test_msgs::srv::Empty::Request::SharedPtr,
+    test_msgs::srv::Empty::Response::SharedPtr) {};
+  auto service =
+    node()->create_service<test_msgs::srv::Empty>("node1_service", std::move(callback));
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR);
+
+  EXPECT_THROW(
+    node_graph()->get_service_names_and_types_by_node(node_name, absolute_namespace),
+    rclcpp::exceptions::RCLError);
+}
+
 TEST_F(TestNodeGraph, get_node_names_and_namespaces)
 {
-  auto node = std::make_shared<rclcpp::Node>("node", "ns");
-  const auto * node_graph =
-    dynamic_cast<rclcpp::node_interfaces::NodeGraph *>(node->get_node_graph_interface().get());
-  ASSERT_NE(nullptr, node_graph);
-
-  auto names_and_namespaces = node_graph->get_node_names_and_namespaces();
+  auto names_and_namespaces = node_graph()->get_node_names_and_namespaces();
   EXPECT_EQ(1u, names_and_namespaces.size());
 }
 
+TEST_F(TestNodeGraph, get_node_names_and_namespaces_rcl_errors)
+{
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_node_names, RCL_RET_ERROR);
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcutils_string_array_fini, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_node_names_and_namespaces(),
+    std::runtime_error(
+      "failed to get node names: error not set, failed also to cleanup node names, leaking memory:"
+      " error not set, failed also to cleanup node namespaces, leaking memory: error not set"));
+}
+
+TEST_F(TestNodeGraph, get_node_names_and_namespaces_fini_errors)
+{
+  auto mock_names_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcutils_string_array_fini, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_node_names_and_namespaces(),
+    std::runtime_error("could not destroy node names, could not destroy node namespaces"));
+}
+
+TEST_F(TestNodeGraph, count_publishers_rcl_error)
+{
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_count_publishers, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->count_publishers("topic"),
+    std::runtime_error("could not count publishers: error not set"));
+}
+
+TEST_F(TestNodeGraph, count_subscribers_rcl_error)
+{
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_count_subscribers, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->count_subscribers("topic"),
+    std::runtime_error("could not count subscribers: error not set"));
+}
+
 TEST_F(TestNodeGraph, notify_shutdown)
 {
-  auto node = std::make_shared<rclcpp::Node>("node", "ns");
-  auto * node_graph =
-    dynamic_cast<rclcpp::node_interfaces::NodeGraph *>(node->get_node_graph_interface().get());
-  ASSERT_NE(nullptr, node_graph);
-
-  EXPECT_NO_THROW(node_graph->notify_shutdown());
+  EXPECT_NO_THROW(node()->get_node_graph_interface()->notify_shutdown());
 }
 
 TEST_F(TestNodeGraph, wait_for_graph_change)
 {
-  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node", "ns");
-  auto * node_graph =
-    dynamic_cast<rclcpp::node_interfaces::NodeGraph *>(node->get_node_graph_interface().get());
-  ASSERT_NE(nullptr, node_graph);
-
-  EXPECT_NO_THROW(node_graph->notify_graph_change());
+  auto node_graph_interface = node()->get_node_graph_interface();
+  EXPECT_NO_THROW(node_graph_interface->notify_graph_change());
   EXPECT_THROW(
-    node_graph->wait_for_graph_change(nullptr, std::chrono::milliseconds(1)),
+    node_graph_interface->wait_for_graph_change(nullptr, std::chrono::milliseconds(1)),
     rclcpp::exceptions::InvalidEventError);
 
   auto event = std::make_shared<rclcpp::Event>();
   EXPECT_THROW(
-    node_graph->wait_for_graph_change(event, std::chrono::milliseconds(0)),
+    node_graph_interface->wait_for_graph_change(event, std::chrono::milliseconds(0)),
     rclcpp::exceptions::EventNotRegisteredError);
 }
 
+TEST_F(TestNodeGraph, notify_graph_change_rcl_error)
+{
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  EXPECT_THROW(
+    node()->get_node_graph_interface()->notify_graph_change(),
+    rclcpp::exceptions::RCLError);
+}
+
 TEST_F(TestNodeGraph, get_info_by_topic)
 {
-  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node", "ns");
   const rclcpp::QoS publisher_qos(1);
-  auto publisher = node->create_publisher<test_msgs::msg::Empty>("topic", publisher_qos);
+  auto publisher = node()->create_publisher<test_msgs::msg::Empty>("topic", publisher_qos);
   auto callback = [](const test_msgs::msg::Empty::SharedPtr) {};
 
   const rclcpp::QoS subscriber_qos(10);
   auto subscription =
-    node->create_subscription<test_msgs::msg::Empty>(
+    node()->create_subscription<test_msgs::msg::Empty>(
     "topic", subscriber_qos, std::move(callback));
 
-  const auto * node_graph =
-    dynamic_cast<rclcpp::node_interfaces::NodeGraph *>(node->get_node_graph_interface().get());
-  ASSERT_NE(nullptr, node_graph);
+  EXPECT_EQ(0u, node_graph()->get_publishers_info_by_topic("topic", true).size());
 
-  auto publishers = node_graph->get_publishers_info_by_topic("topic", false);
+  auto publishers = node_graph()->get_publishers_info_by_topic("topic", false);
   ASSERT_EQ(1u, publishers.size());
 
   auto publisher_endpoint_info = publishers[0];
   const auto const_publisher_endpoint_info = publisher_endpoint_info;
-  EXPECT_STREQ("node", publisher_endpoint_info.node_name().c_str());
-  EXPECT_STREQ("node", const_publisher_endpoint_info.node_name().c_str());
-  EXPECT_STREQ("/ns", publisher_endpoint_info.node_namespace().c_str());
-  EXPECT_STREQ("/ns", const_publisher_endpoint_info.node_namespace().c_str());
+  EXPECT_STREQ(node_name, publisher_endpoint_info.node_name().c_str());
+  EXPECT_STREQ(node_name, const_publisher_endpoint_info.node_name().c_str());
+  EXPECT_STREQ(absolute_namespace, publisher_endpoint_info.node_namespace().c_str());
+  EXPECT_STREQ(absolute_namespace, const_publisher_endpoint_info.node_namespace().c_str());
   EXPECT_STREQ("test_msgs/msg/Empty", publisher_endpoint_info.topic_type().c_str());
   EXPECT_STREQ("test_msgs/msg/Empty", const_publisher_endpoint_info.topic_type().c_str());
   EXPECT_EQ(rclcpp::EndpointType::Publisher, publisher_endpoint_info.endpoint_type());
@@ -225,3 +356,90 @@ TEST_F(TestNodeGraph, get_info_by_topic)
   }
   EXPECT_FALSE(endpoint_gid_is_all_zeros);
 }
+
+TEST_F(TestNodeGraph, get_info_by_topic_rcl_node_get_options_error)
+{
+  const rclcpp::QoS publisher_qos(1);
+  auto publisher = node()->create_publisher<test_msgs::msg::Empty>("topic", publisher_qos);
+
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_node_get_options, nullptr);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_publishers_info_by_topic("topic", false),
+    std::runtime_error("Need valid node options in get_info_by_topic()"));
+}
+
+// Required for mocking_utils below
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >)
+
+TEST_F(TestNodeGraph, get_info_by_topic_rcl_remap_topic_name_error)
+{
+  const rclcpp::QoS publisher_qos(1);
+  auto publisher = node()->create_publisher<test_msgs::msg::Empty>("topic", publisher_qos);
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_remap_topic_name, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_graph()->get_publishers_info_by_topic("topic", false),
+    std::runtime_error("Failed to remap topic name /ns/topic: error not set"));
+}
+
+TEST_F(TestNodeGraph, get_info_by_topic_rcl_remap_topic_name_nullptr)
+{
+  const rclcpp::QoS publisher_qos(1);
+  auto publisher = node()->create_publisher<test_msgs::msg::Empty>("topic", publisher_qos);
+
+  // Should be cleaned up by get_info_by_topic
+  char * some_string = rcutils_strdup("", rcl_get_default_allocator());
+  ASSERT_NE(nullptr, some_string);
+  auto mock =
+    mocking_utils::patch(
+    "lib:rclcpp", rcl_remap_topic_name, [&some_string](
+      const rcl_arguments_t *, const rcl_arguments_t *, const char *, const char *, const char *,
+      rcl_allocator_t, char ** output_name)
+    {
+      *output_name = some_string;
+      return RCL_RET_OK;
+    });
+  EXPECT_NO_THROW(node_graph()->get_publishers_info_by_topic("topic", false));
+}
+
+TEST_F(TestNodeGraph, get_info_by_topic_rcl_errors)
+{
+  const rclcpp::QoS publisher_qos(1);
+  auto publisher = node()->create_publisher<test_msgs::msg::Empty>("topic", publisher_qos);
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_publishers_info_by_topic, RCL_RET_ERROR);
+  auto mock_info_array_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_topic_endpoint_info_array_fini, RCL_RET_ERROR);
+  EXPECT_THROW(
+    node_graph()->get_publishers_info_by_topic("topic", false),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestNodeGraph, get_info_by_topic_unsupported)
+{
+  const rclcpp::QoS publisher_qos(1);
+  auto publisher = node()->create_publisher<test_msgs::msg::Empty>("topic", publisher_qos);
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_publishers_info_by_topic, RCL_RET_UNSUPPORTED);
+  EXPECT_THROW(
+    node_graph()->get_publishers_info_by_topic("topic", false),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestNodeGraph, get_info_by_topic_endpoint_info_array_fini_error)
+{
+  const rclcpp::QoS publisher_qos(1);
+  auto publisher = node()->create_publisher<test_msgs::msg::Empty>("topic", publisher_qos);
+
+  auto mock_info_array_fini = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_topic_endpoint_info_array_fini, RCL_RET_ERROR);
+  EXPECT_THROW(
+    node_graph()->get_publishers_info_by_topic("topic", false),
+    rclcpp::exceptions::RCLError);
+}

rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp

@@ -28,31 +28,47 @@
 #include "rclcpp/node.hpp"
 #include "rclcpp/node_interfaces/node_parameters.hpp"
 
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
 class TestNodeParameters : public ::testing::Test
 {
 public:
   void SetUp()
   {
     rclcpp::init(0, nullptr);
+    rclcpp::NodeOptions options;
+    options.allow_undeclared_parameters(true);
+    node = std::make_shared<rclcpp::Node>("node", "ns", options);
+
+    // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
+    // the proper type is being tested and covered.
+    node_parameters =
+      dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
+      node->get_node_parameters_interface().get());
+    ASSERT_NE(nullptr, node_parameters);
   }
 
   void TearDown()
   {
     rclcpp::shutdown();
   }
+
+protected:
+  std::shared_ptr<rclcpp::Node> node;
+  rclcpp::node_interfaces::NodeParameters * node_parameters;
 };
 
+TEST_F(TestNodeParameters, construct_destruct_rcl_errors) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_arguments_get_param_overrides, RCL_RET_ERROR);
+  EXPECT_THROW(
+    std::make_shared<rclcpp::Node>("node2", "ns").reset(),
+    rclcpp::exceptions::RCLError);
+}
+
 TEST_F(TestNodeParameters, list_parameters)
 {
-  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node", "ns");
-
-  // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
-  // the proper type is being tested and covered.
-  auto * node_parameters =
-    dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
-    node->get_node_parameters_interface().get());
-  ASSERT_NE(nullptr, node_parameters);
-
   std::vector<std::string> prefixes;
   const auto list_result = node_parameters->list_parameters(prefixes, 1u);
 
@@ -73,17 +89,113 @@ TEST_F(TestNodeParameters, list_parameters)
   EXPECT_NE(
     std::find(list_result2.names.begin(), list_result2.names.end(), parameter_name),
     list_result2.names.end());
+
+  // Check prefixes
+  const std::string parameter_name2 = "prefix.new_parameter";
+  const rclcpp::ParameterValue value2(true);
+  const rcl_interfaces::msg::ParameterDescriptor descriptor2;
+  const auto added_parameter_value2 =
+    node_parameters->declare_parameter(parameter_name2, value2, descriptor2, false);
+  EXPECT_EQ(value.get<bool>(), added_parameter_value.get<bool>());
+  prefixes = {"prefix"};
+  auto list_result3 = node_parameters->list_parameters(prefixes, 2u);
+  EXPECT_EQ(1u, list_result3.names.size());
+  EXPECT_NE(
+    std::find(list_result3.names.begin(), list_result3.names.end(), parameter_name2),
+    list_result3.names.end());
+
+  // Check if prefix equals parameter name
+  prefixes = {"new_parameter"};
+  auto list_result4 = node_parameters->list_parameters(prefixes, 2u);
+  EXPECT_EQ(1u, list_result4.names.size());
+  EXPECT_NE(
+    std::find(list_result4.names.begin(), list_result4.names.end(), parameter_name),
+    list_result4.names.end());
 }
 
 TEST_F(TestNodeParameters, parameter_overrides)
 {
-  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::NodeOptions node_options;
+  node_options.automatically_declare_parameters_from_overrides(true);
+  node_options.append_parameter_override("param1", true);
+  node_options.append_parameter_override("param2", 42);
 
-  auto * node_parameters =
+  std::shared_ptr<rclcpp::Node> node2 = std::make_shared<rclcpp::Node>("node2", "ns", node_options);
+
+  auto * node_parameters_interface =
     dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
-    node->get_node_parameters_interface().get());
+    node2->get_node_parameters_interface().get());
   ASSERT_NE(nullptr, node_parameters);
 
-  const auto & parameter_overrides = node_parameters->get_parameter_overrides();
-  EXPECT_EQ(0u, parameter_overrides.size());
+  const auto & parameter_overrides = node_parameters_interface->get_parameter_overrides();
+  EXPECT_EQ(2u, parameter_overrides.size());
+}
+
+TEST_F(TestNodeParameters, set_parameters) {
+  rclcpp::NodeOptions node_options;
+  node_options.allow_undeclared_parameters(true);
+
+  rcl_interfaces::msg::ParameterDescriptor bool_descriptor;
+  bool_descriptor.name = "bool_parameter";
+  bool_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
+  bool_descriptor.read_only = false;
+  node_parameters->declare_parameter(
+    "bool_parameter", rclcpp::ParameterValue(false), bool_descriptor, false);
+
+  rcl_interfaces::msg::ParameterDescriptor read_only_descriptor;
+  read_only_descriptor.name = "read_only_parameter";
+  read_only_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
+  read_only_descriptor.read_only = true;
+  node_parameters->declare_parameter(
+    "read_only_parameter", rclcpp::ParameterValue(42), read_only_descriptor, false);
+
+  const std::vector<rclcpp::Parameter> parameters = {
+    rclcpp::Parameter("bool_parameter", true),
+    rclcpp::Parameter("read_only_parameter", 42),
+  };
+  auto result = node_parameters->set_parameters(parameters);
+  ASSERT_EQ(parameters.size(), result.size());
+  EXPECT_TRUE(result[0].successful);
+  EXPECT_FALSE(result[1].successful);
+  EXPECT_STREQ(
+    "parameter 'read_only_parameter' cannot be set because it is read-only",
+    result[1].reason.c_str());
+
+  RCLCPP_EXPECT_THROW_EQ(
+    node_parameters->set_parameters({rclcpp::Parameter("", true)}),
+    rclcpp::exceptions::InvalidParametersException("parameter name must not be empty"));
+
+  result = node_parameters->set_parameters({rclcpp::Parameter("undeclared_parameter", 3.14159)});
+  ASSERT_EQ(1u, result.size());
+  EXPECT_TRUE(result[0].successful);
+}
+
+TEST_F(TestNodeParameters, add_remove_parameters_callback) {
+  rcl_interfaces::msg::ParameterDescriptor bool_descriptor;
+  bool_descriptor.name = "bool_parameter";
+  bool_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
+  bool_descriptor.read_only = false;
+  node_parameters->declare_parameter(
+    "bool_parameter", rclcpp::ParameterValue(false), bool_descriptor, false);
+  const std::vector<rclcpp::Parameter> parameters = {rclcpp::Parameter("bool_parameter", true)};
+
+  const std::string reason = "some totally not made up reason";
+  auto callback = [reason](const std::vector<rclcpp::Parameter> &) {
+      rcl_interfaces::msg::SetParametersResult result;
+      result.successful = false;
+      result.reason = reason;
+      return result;
+    };
+
+  auto handle = node_parameters->add_on_set_parameters_callback(callback);
+  auto result = node_parameters->set_parameters(parameters);
+  ASSERT_EQ(1u, result.size());
+  EXPECT_FALSE(result[0].successful);
+  EXPECT_EQ(reason, result[0].reason);
+
+  EXPECT_NO_THROW(node_parameters->remove_on_set_parameters_callback(handle.get()));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    node_parameters->remove_on_set_parameters_callback(handle.get()),
+    std::runtime_error("Callback doesn't exist"));
 }

rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp

@@ -22,6 +22,9 @@
 #include "rclcpp/node_interfaces/node_services.hpp"
 #include "rclcpp/rclcpp.hpp"
 
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
 class TestService : public rclcpp::ServiceBase
 {
 public:
@@ -51,25 +54,29 @@ public:
   void SetUp()
   {
     rclcpp::init(0, nullptr);
+
+    node = std::make_shared<rclcpp::Node>("node", "ns");
+
+    // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
+    // the proper type is being tested and covered.
+    node_services =
+      dynamic_cast<rclcpp::node_interfaces::NodeServices *>(
+      node->get_node_services_interface().get());
+    ASSERT_NE(nullptr, node_services);
   }
 
   void TearDown()
   {
     rclcpp::shutdown();
   }
+
+protected:
+  std::shared_ptr<rclcpp::Node> node;
+  rclcpp::node_interfaces::NodeServices * node_services;
 };
 
 TEST_F(TestNodeService, add_service)
 {
-  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node", "ns");
-
-  // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
-  // the proper type is being tested and covered.
-  auto * node_services =
-    dynamic_cast<rclcpp::node_interfaces::NodeServices *>(
-    node->get_node_services_interface().get());
-  ASSERT_NE(nullptr, node_services);
-
   auto service = std::make_shared<TestService>(node.get());
   auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
   EXPECT_NO_THROW(
@@ -80,22 +87,24 @@ TEST_F(TestNodeService, add_service)
 
   auto callback_group_in_different_node =
     different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-  EXPECT_THROW(
+  RCLCPP_EXPECT_THROW_EQ(
     node_services->add_service(service, callback_group_in_different_node),
-    std::runtime_error);
+    std::runtime_error("Cannot create service, group not in node."));
+}
+
+TEST_F(TestNodeService, add_service_rcl_trigger_guard_condition_error)
+{
+  auto service = std::make_shared<TestService>(node.get());
+  auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_services->add_service(service, callback_group),
+    std::runtime_error("Failed to notify wait set on service creation: error not set"));
 }
 
 TEST_F(TestNodeService, add_client)
 {
-  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node", "ns");
-
-  // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
-  // the proper type is being tested and covered.
-  auto * node_services =
-    dynamic_cast<rclcpp::node_interfaces::NodeServices *>(
-    node->get_node_services_interface().get());
-  ASSERT_NE(nullptr, node_services);
-
   auto client = std::make_shared<TestClient>(node.get());
   auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
   EXPECT_NO_THROW(node_services->add_client(client, callback_group));
@@ -105,7 +114,18 @@ TEST_F(TestNodeService, add_client)
 
   auto callback_group_in_different_node =
     different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-  EXPECT_THROW(
+  RCLCPP_EXPECT_THROW_EQ(
     node_services->add_client(client, callback_group_in_different_node),
-    std::runtime_error);
+    std::runtime_error("Cannot create client, group not in node."));
+}
+
+TEST_F(TestNodeService, add_client_rcl_trigger_guard_condition_error)
+{
+  auto client = std::make_shared<TestClient>(node.get());
+  auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_services->add_client(client, callback_group),
+    std::runtime_error("Failed to notify wait set on client creation: error not set"));
 }

rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp

@@ -22,6 +22,9 @@
 #include "rclcpp/node_interfaces/node_timers.hpp"
 #include "rclcpp/rclcpp.hpp"
 
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
 class TestTimer : public rclcpp::TimerBase
 {
 public:
@@ -39,23 +42,27 @@ public:
   void SetUp()
   {
     rclcpp::init(0, nullptr);
+    node = std::make_shared<rclcpp::Node>("node", "ns");
+
+    // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
+    // the proper type is being tested and covered.
+    node_timers =
+      dynamic_cast<rclcpp::node_interfaces::NodeTimers *>(node->get_node_timers_interface().get());
+    ASSERT_NE(nullptr, node_timers);
   }
 
   void TearDown()
   {
     rclcpp::shutdown();
   }
+
+protected:
+  std::shared_ptr<rclcpp::Node> node;
+  rclcpp::node_interfaces::NodeTimers * node_timers;
 };
 
 TEST_F(TestNodeTimers, add_timer)
 {
-  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node", "ns");
-
-  // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
-  // the proper type is being tested and covered.
-  auto node_timers =
-    dynamic_cast<rclcpp::node_interfaces::NodeTimers *>(node->get_node_timers_interface().get());
-  ASSERT_NE(nullptr, node_timers);
   auto timer = std::make_shared<TestTimer>(node.get());
   auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
   EXPECT_NO_THROW(node_timers->add_timer(timer, callback_group));
@@ -65,7 +72,19 @@ TEST_F(TestNodeTimers, add_timer)
 
   auto callback_group_in_different_node =
     different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-  EXPECT_THROW(
+  RCLCPP_EXPECT_THROW_EQ(
     node_timers->add_timer(timer, callback_group_in_different_node),
-    std::runtime_error);
+    std::runtime_error("Cannot create timer, group not in node."));
+}
+
+TEST_F(TestNodeTimers, add_timer_rcl_trigger_guard_condition_error)
+{
+  auto timer = std::make_shared<TestTimer>(node.get());
+  auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_timers->add_timer(timer, callback_group),
+    std::runtime_error("Failed to notify wait set on timer creation: error not set"));
 }

rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp

@@ -24,6 +24,9 @@
 #include "rclcpp/rclcpp.hpp"
 #include "test_msgs/msg/empty.hpp"
 
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
 namespace
 {
 
@@ -77,23 +80,27 @@ public:
   void SetUp()
   {
     rclcpp::init(0, nullptr);
+    node = std::make_shared<rclcpp::Node>("node", "ns");
+
+    // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
+    // the proper type is being tested and covered.
+    node_topics =
+      dynamic_cast<rclcpp::node_interfaces::NodeTopics *>(node->get_node_topics_interface().get());
+    ASSERT_NE(nullptr, node_topics);
   }
 
   void TearDown()
   {
     rclcpp::shutdown();
   }
+
+protected:
+  std::shared_ptr<rclcpp::Node> node;
+  rclcpp::node_interfaces::NodeTopics * node_topics;
 };
 
 TEST_F(TestNodeTopics, add_publisher)
 {
-  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node", "ns");
-
-  // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure
-  // the proper type is being tested and covered.
-  auto * node_topics =
-    dynamic_cast<rclcpp::node_interfaces::NodeTopics *>(node->get_node_topics_interface().get());
-  ASSERT_NE(nullptr, node_topics);
   auto publisher = std::make_shared<TestPublisher>(node.get());
   auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
   EXPECT_NO_THROW(node_topics->add_publisher(publisher, callback_group));
@@ -108,12 +115,20 @@ TEST_F(TestNodeTopics, add_publisher)
     std::runtime_error);
 }
 
+TEST_F(TestNodeTopics, add_publisher_rcl_trigger_guard_condition_error)
+{
+  auto publisher = std::make_shared<TestPublisher>(node.get());
+  auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_topics->add_publisher(publisher, callback_group),
+    std::runtime_error("Failed to notify wait set on publisher creation: error not set"));
+}
+
 TEST_F(TestNodeTopics, add_subscription)
 {
-  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node", "ns");
-  auto * node_topics =
-    dynamic_cast<rclcpp::node_interfaces::NodeTopics *>(node->get_node_topics_interface().get());
-  ASSERT_NE(nullptr, node_topics);
   auto subscription = std::make_shared<TestSubscription>(node.get());
   auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
   EXPECT_NO_THROW(node_topics->add_subscription(subscription, callback_group));
@@ -127,3 +142,15 @@ TEST_F(TestNodeTopics, add_subscription)
     node_topics->add_subscription(subscription, callback_group_in_different_node),
     std::runtime_error);
 }
+
+TEST_F(TestNodeTopics, add_subscription_rcl_trigger_guard_condition_error)
+{
+  auto subscription = std::make_shared<TestSubscription>(node.get());
+  auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_topics->add_subscription(subscription, callback_group),
+    std::runtime_error("failed to notify wait set on subscription creation: error not set"));
+}

rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp

@@ -22,6 +22,9 @@
 #include "rclcpp/node_interfaces/node_waitables.hpp"
 #include "rclcpp/rclcpp.hpp"
 
+#include "../../mocking_utils/patch.hpp"
+#include "../../utils/rclcpp_gtest_macros.hpp"
+
 class TestWaitable : public rclcpp::Waitable
 {
 public:
@@ -36,23 +39,26 @@ public:
   void SetUp()
   {
     rclcpp::init(0, nullptr);
+    node = std::make_shared<rclcpp::Node>("node", "ns");
+
+    node_waitables =
+      dynamic_cast<rclcpp::node_interfaces::NodeWaitables *>(
+      node->get_node_waitables_interface().get());
+    ASSERT_NE(nullptr, node_waitables);
   }
 
   void TearDown()
   {
     rclcpp::shutdown();
   }
+
+protected:
+  std::shared_ptr<rclcpp::Node> node;
+  rclcpp::node_interfaces::NodeWaitables * node_waitables;
 };
 
 TEST_F(TestNodeWaitables, add_remove_waitable)
 {
-  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node", "ns");
-
-  auto * node_waitables =
-    dynamic_cast<rclcpp::node_interfaces::NodeWaitables *>(
-    node->get_node_waitables_interface().get());
-  ASSERT_NE(nullptr, node_waitables);
-
   std::shared_ptr<rclcpp::Node> node2 = std::make_shared<rclcpp::Node>("node2", "ns");
 
   auto callback_group1 = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
@@ -60,9 +66,25 @@ TEST_F(TestNodeWaitables, add_remove_waitable)
   auto waitable = std::make_shared<TestWaitable>();
   EXPECT_NO_THROW(
     node_waitables->add_waitable(waitable, callback_group1));
-  EXPECT_THROW(
+  RCLCPP_EXPECT_THROW_EQ(
     node_waitables->add_waitable(waitable, callback_group2),
-    std::runtime_error);
+    std::runtime_error("Cannot create waitable, group not in node."));
   EXPECT_NO_THROW(node_waitables->remove_waitable(waitable, callback_group1));
   EXPECT_NO_THROW(node_waitables->remove_waitable(waitable, callback_group2));
+
+  auto waitable2 = std::make_shared<TestWaitable>();
+  EXPECT_NO_THROW(node_waitables->add_waitable(waitable2, nullptr));
+  EXPECT_NO_THROW(node_waitables->remove_waitable(waitable2, nullptr));
+}
+
+TEST_F(TestNodeWaitables, add_waitable_rcl_trigger_guard_condition_error)
+{
+  auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  auto waitable = std::make_shared<TestWaitable>();
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node_waitables->add_waitable(waitable, callback_group),
+    std::runtime_error("Failed to notify wait set on waitable creation: error not set"));
 }

rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp

@@ -86,6 +86,7 @@ public:
 
   void TearDown() override
   {
+    allocator_memory_strategy_.reset();
     rclcpp::shutdown();
   }
 

rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp

@@ -30,6 +30,10 @@
 #include "rclcpp/executor.hpp"
 #include "rclcpp/rclcpp.hpp"
 
+// Note: This is a long running test with rmw_connext_cpp, if you change this file, please check
+// that this test can complete fully, or adjust the timeout as necessary.
+// See https://github.com/ros2/rmw_connext/issues/325 for resolution]
+
 using namespace std::chrono_literals;
 
 template<typename T>

rclcpp/test/rclcpp/test_client.cpp

@@ -16,12 +16,17 @@
 
 #include <string>
 #include <memory>
+#include <utility>
 
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 #include "rcl_interfaces/srv/list_parameters.hpp"
 
+#include "../mocking_utils/patch.hpp"
+
+#include "test_msgs/srv/empty.hpp"
+
 class TestClient : public ::testing::Test
 {
 protected:
@@ -30,6 +35,11 @@ protected:
     rclcpp::init(0, nullptr);
   }
 
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
   void SetUp()
   {
     node = std::make_shared<rclcpp::Node>("my_node", "/ns");
@@ -48,6 +58,12 @@ class TestClientSub : public ::testing::Test
 protected:
   static void SetUpTestCase()
   {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
   }
 
   void SetUp()
@@ -106,6 +122,38 @@ TEST_F(TestClient, construction_with_free_function) {
   }
 }
 
+TEST_F(TestClient, construct_with_rcl_error) {
+  {
+    // reset() is not necessary for this exception, but handles unused return value warning
+    auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_client_init, RCL_RET_ERROR);
+    EXPECT_THROW(
+      node->create_client<test_msgs::srv::Empty>("service").reset(),
+      rclcpp::exceptions::RCLError);
+  }
+  {
+    // reset() is required for this one
+    auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_client_fini, RCL_RET_ERROR);
+    EXPECT_NO_THROW(node->create_client<test_msgs::srv::Empty>("service").reset());
+  }
+}
+
+TEST_F(TestClient, wait_for_service) {
+  const std::string service_name = "service";
+  auto client = node->create_client<test_msgs::srv::Empty>(service_name);
+  EXPECT_FALSE(client->wait_for_service(std::chrono::nanoseconds(0)));
+  EXPECT_FALSE(client->wait_for_service(std::chrono::milliseconds(10)));
+
+  auto callback = [](
+    const test_msgs::srv::Empty::Request::SharedPtr,
+    test_msgs::srv::Empty::Response::SharedPtr) {};
+
+  auto service =
+    node->create_service<test_msgs::srv::Empty>(service_name, std::move(callback));
+
+  EXPECT_TRUE(client->wait_for_service(std::chrono::nanoseconds(-1)));
+  EXPECT_TRUE(client->service_is_ready());
+}
+
 /*
    Testing client construction and destruction for subnodes.
  */
@@ -123,3 +171,160 @@ TEST_F(TestClientSub, construction_and_destruction) {
     }, rclcpp::exceptions::InvalidServiceNameError);
   }
 }
+
+class TestClientWithServer : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>("node", "ns");
+
+    auto callback = [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {};
+
+    service = node->create_service<test_msgs::srv::Empty>(service_name, std::move(callback));
+  }
+
+  ::testing::AssertionResult SendEmptyRequestAndWait(
+    std::chrono::milliseconds timeout = std::chrono::milliseconds(1000))
+  {
+    using SharedFuture = rclcpp::Client<test_msgs::srv::Empty>::SharedFuture;
+
+    auto client = node->create_client<test_msgs::srv::Empty>(service_name);
+    if (!client->wait_for_service()) {
+      return ::testing::AssertionFailure() << "Waiting for service failed";
+    }
+
+    auto request = std::make_shared<test_msgs::srv::Empty::Request>();
+    bool received_response = false;
+    ::testing::AssertionResult request_result = ::testing::AssertionSuccess();
+    auto callback = [&received_response, &request_result](SharedFuture future_response) {
+        if (nullptr == future_response.get()) {
+          request_result = ::testing::AssertionFailure() << "Future response was null";
+        }
+        received_response = true;
+      };
+
+    auto future = client->async_send_request(request, std::move(callback));
+
+    auto start = std::chrono::steady_clock::now();
+    while (!received_response &&
+      (std::chrono::steady_clock::now() - start) < timeout)
+    {
+      rclcpp::spin_some(node);
+    }
+
+    if (!received_response) {
+      return ::testing::AssertionFailure() << "Waiting for response timed out";
+    }
+
+    return request_result;
+  }
+
+  std::shared_ptr<rclcpp::Node> node;
+  std::shared_ptr<rclcpp::Service<test_msgs::srv::Empty>> service;
+  const std::string service_name{"empty_service"};
+};
+
+TEST_F(TestClientWithServer, async_send_request) {
+  EXPECT_TRUE(SendEmptyRequestAndWait());
+}
+
+TEST_F(TestClientWithServer, async_send_request_callback_with_request) {
+  using SharedFutureWithRequest =
+    rclcpp::Client<test_msgs::srv::Empty>::SharedFutureWithRequest;
+
+  auto client = node->create_client<test_msgs::srv::Empty>(service_name);
+  ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1)));
+
+  auto request = std::make_shared<test_msgs::srv::Empty::Request>();
+  bool received_response = false;
+  auto callback = [&request, &received_response](SharedFutureWithRequest future) {
+      auto request_response_pair = future.get();
+      EXPECT_EQ(request, request_response_pair.first);
+      EXPECT_NE(nullptr, request_response_pair.second);
+      received_response = true;
+    };
+  auto future = client->async_send_request(request, std::move(callback));
+
+  auto start = std::chrono::steady_clock::now();
+  while (!received_response &&
+    (std::chrono::steady_clock::now() - start) < std::chrono::seconds(1))
+  {
+    rclcpp::spin_some(node);
+  }
+  EXPECT_TRUE(received_response);
+}
+
+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);
+  EXPECT_THROW(SendEmptyRequestAndWait(), rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestClientWithServer, async_send_request_rcl_service_server_is_available_error) {
+  {
+    // Checking rcl_service_server_is_available in rclcpp::ClientBase::service_is_ready
+    auto client = node->create_client<test_msgs::srv::Empty>(service_name);
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_service_server_is_available, RCL_RET_NODE_INVALID);
+    EXPECT_THROW(client->service_is_ready(), rclcpp::exceptions::RCLError);
+  }
+  {
+    // Checking rcl_service_server_is_available exception in rclcpp::ClientBase::service_is_ready
+    auto client = node->create_client<test_msgs::srv::Empty>(service_name);
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_service_server_is_available, RCL_RET_ERROR);
+    EXPECT_THROW(client->service_is_ready(), rclcpp::exceptions::RCLError);
+  }
+  {
+    // Checking rcl_service_server_is_available exception in rclcpp::ClientBase::service_is_ready
+    auto client = node->create_client<test_msgs::srv::Empty>(service_name);
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_service_server_is_available, RCL_RET_ERROR);
+    EXPECT_THROW(client->service_is_ready(), rclcpp::exceptions::RCLError);
+  }
+}
+
+TEST_F(TestClientWithServer, take_response) {
+  auto client = node->create_client<test_msgs::srv::Empty>(service_name);
+  ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1)));
+  auto request = std::make_shared<test_msgs::srv::Empty::Request>();
+  auto request_header = client->create_request_header();
+  test_msgs::srv::Empty::Response response;
+
+  client->async_send_request(request);
+  EXPECT_FALSE(client->take_response(response, *request_header.get()));
+
+  {
+    // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_take_response, RCL_RET_OK);
+    EXPECT_TRUE(client->take_response(response, *request_header.get()));
+  }
+  {
+    // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_take_response, RCL_RET_CLIENT_TAKE_FAILED);
+    EXPECT_FALSE(client->take_response(response, *request_header.get()));
+  }
+  {
+    // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_take_response, RCL_RET_ERROR);
+    EXPECT_THROW(
+      client->take_response(response, *request_header.get()),
+      rclcpp::exceptions::RCLError);
+  }
+}

rclcpp/test/rclcpp/test_duration.cpp

@@ -25,6 +25,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp/duration.hpp"
 
+#include "../utils/rclcpp_gtest_macros.hpp"
 
 using namespace std::chrono_literals;
 
@@ -237,3 +238,26 @@ TEST_F(TestDuration, conversions) {
     EXPECT_EQ(chrono_duration.count(), -ONE_AND_HALF_SEC_IN_NS);
   }
 }
+
+TEST_F(TestDuration, test_some_constructors) {
+  builtin_interfaces::msg::Duration duration_msg;
+  duration_msg.sec = 1;
+  duration_msg.nanosec = 1000;
+  rclcpp::Duration duration_from_msg(duration_msg);
+  EXPECT_EQ(RCL_S_TO_NS(1) + 1000, duration_from_msg.nanoseconds());
+
+  rcl_duration_t duration_struct;
+  duration_struct.nanoseconds = 4000;
+  rclcpp::Duration duration_from_struct(duration_struct);
+  EXPECT_EQ(4000, duration_from_struct.nanoseconds());
+}
+
+TEST_F(TestDuration, test_some_exceptions) {
+  rclcpp::Duration test_duration(0u);
+  RCLCPP_EXPECT_THROW_EQ(
+    test_duration = rclcpp::Duration(INT64_MAX) - rclcpp::Duration(-1),
+    std::overflow_error("duration subtraction leads to int64_t overflow"));
+  RCLCPP_EXPECT_THROW_EQ(
+    test_duration = test_duration * (std::numeric_limits<double>::infinity()),
+    std::runtime_error("abnormal scale in rclcpp::Duration"));
+}

rclcpp/test/rclcpp/test_executor.cpp

@@ -0,0 +1,528 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <chrono>
+#include <memory>
+#include <stdexcept>
+#include <string>
+
+#include "rclcpp/executor.hpp"
+#include "rclcpp/memory_strategy.hpp"
+#include "rclcpp/strategies/allocator_memory_strategy.hpp"
+
+#include "../mocking_utils/patch.hpp"
+#include "../utils/rclcpp_gtest_macros.hpp"
+
+// This file tests the abstract rclcpp::Executor class.  For tests of the concrete classes
+// that implement this class, please see the test/rclcpp/executors subdirectory.
+
+class DummyExecutor : public rclcpp::Executor
+{
+public:
+  DummyExecutor()
+  : rclcpp::Executor()
+  {
+  }
+
+  void spin() override
+  {
+  }
+
+  void spin_nanoseconds(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node)
+  {
+    spin_node_once_nanoseconds(node, std::chrono::milliseconds(100));
+  }
+
+  rclcpp::memory_strategy::MemoryStrategy * memory_strategy_ptr()
+  {
+    return memory_strategy_.get();
+  }
+
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr local_get_node_by_group(
+    rclcpp::CallbackGroup::SharedPtr group)
+  {
+    return get_node_by_group(weak_groups_to_nodes_, group);
+  }
+
+  rclcpp::CallbackGroup::SharedPtr local_get_group_by_timer(rclcpp::TimerBase::SharedPtr timer)
+  {
+    return get_group_by_timer(timer);
+  }
+};
+
+class TestExecutor : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  void TearDown()
+  {
+    rclcpp::shutdown();
+  }
+};
+
+// Required for mocking_utils below
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, ==)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, !=)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, <)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, >)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >)
+
+TEST_F(TestExecutor, constructor_bad_guard_condition_init) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_guard_condition_init, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    new DummyExecutor(),
+    std::runtime_error(
+      "Failed to create interrupt guard condition in Executor constructor: error not set"));
+}
+
+TEST_F(TestExecutor, constructor_bad_wait_set_init) {
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_init, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    new DummyExecutor(),
+    std::runtime_error("Failed to create wait set in Executor constructor: error not set"));
+}
+
+TEST_F(TestExecutor, add_callback_group_twice) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  dummy.add_callback_group(cb_group, node->get_node_base_interface(), false);
+  cb_group->get_associated_with_executor_atomic().exchange(false);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.add_callback_group(cb_group, node->get_node_base_interface(), false),
+    std::runtime_error("Callback group was already added to executor."));
+}
+
+TEST_F(TestExecutor, add_callback_group_failed_trigger_guard_condition) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.add_callback_group(cb_group, node->get_node_base_interface(), true),
+    std::runtime_error("Failed to trigger guard condition on callback group add: error not set"));
+}
+
+TEST_F(TestExecutor, remove_callback_group_null_node) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  dummy.add_callback_group(cb_group, node->get_node_base_interface(), true);
+
+  node.reset();
+
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.remove_callback_group(cb_group, false),
+    std::runtime_error("Node must not be deleted before its callback group(s)."));
+}
+
+TEST_F(TestExecutor, remove_callback_group_failed_trigger_guard_condition) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  dummy.add_callback_group(cb_group, node->get_node_base_interface(), true);
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.remove_callback_group(cb_group, true),
+    std::runtime_error(
+      "Failed to trigger guard condition on callback group remove: error not set"));
+}
+
+TEST_F(TestExecutor, remove_node_not_associated) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.remove_node(node->get_node_base_interface(), false),
+    std::runtime_error("Node needs to be associated with an executor."));
+}
+
+TEST_F(TestExecutor, remove_node_associated_with_different_executor) {
+  DummyExecutor dummy1;
+  auto node1 = std::make_shared<rclcpp::Node>("node1", "ns");
+  dummy1.add_node(node1->get_node_base_interface(), false);
+
+  DummyExecutor dummy2;
+  auto node2 = std::make_shared<rclcpp::Node>("node2", "ns");
+  dummy2.add_node(node2->get_node_base_interface(), false);
+
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy2.remove_node(node1->get_node_base_interface(), false),
+    std::runtime_error("Node needs to be associated with this executor."));
+}
+
+TEST_F(TestExecutor, spin_node_once_nanoseconds) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  bool timer_fired = false;
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&timer_fired]() {timer_fired = true;});
+
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  EXPECT_FALSE(timer_fired);
+  dummy.spin_nanoseconds(node->get_node_base_interface());
+  EXPECT_TRUE(timer_fired);
+}
+
+TEST_F(TestExecutor, spin_node_some) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  bool timer_fired = false;
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&timer_fired]() {timer_fired = true;});
+
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  EXPECT_FALSE(timer_fired);
+  dummy.spin_node_some(node);
+  EXPECT_TRUE(timer_fired);
+}
+
+TEST_F(TestExecutor, spin_all_invalid_duration) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.spin_all(std::chrono::nanoseconds(-1)),
+    std::invalid_argument("max_duration must be positive"));
+}
+
+TEST_F(TestExecutor, spin_some_in_spin_some) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  bool spin_some_in_spin_some = false;
+  auto timer =
+    node->create_wall_timer(
+    std::chrono::milliseconds(1), [&]() {
+      try {
+        dummy.spin_some(std::chrono::milliseconds(1));
+      } catch (const std::runtime_error & err) {
+        if (err.what() == std::string("spin_some() called while already spinning")) {
+          spin_some_in_spin_some = true;
+        }
+      }
+    });
+
+  dummy.add_node(node);
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  EXPECT_FALSE(spin_some_in_spin_some);
+  dummy.spin_some(std::chrono::milliseconds(1));
+  EXPECT_TRUE(spin_some_in_spin_some);
+}
+
+TEST_F(TestExecutor, spin_some_elapsed) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  bool timer_called = false;
+  auto timer =
+    node->create_wall_timer(
+    std::chrono::milliseconds(1), [&]() {
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+      timer_called = true;
+    });
+
+  dummy.add_node(node);
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  dummy.spin_some(std::chrono::milliseconds(1));
+
+  ASSERT_TRUE(timer_called);
+}
+
+TEST_F(TestExecutor, spin_once_in_spin_once) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  bool spin_once_in_spin_once = false;
+  auto timer =
+    node->create_wall_timer(
+    std::chrono::milliseconds(1), [&]() {
+      try {
+        dummy.spin_once(std::chrono::milliseconds(1));
+      } catch (const std::runtime_error & err) {
+        if (err.what() == std::string("spin_once() called while already spinning")) {
+          spin_once_in_spin_once = true;
+        }
+      }
+    });
+
+  dummy.add_node(node);
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  EXPECT_FALSE(spin_once_in_spin_once);
+  dummy.spin_once(std::chrono::milliseconds(1));
+  EXPECT_TRUE(spin_once_in_spin_once);
+}
+
+TEST_F(TestExecutor, cancel_failed_trigger_guard_condition) {
+  DummyExecutor dummy;
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.cancel(),
+    std::runtime_error("Failed to trigger guard condition in cancel: error not set"));
+}
+
+TEST_F(TestExecutor, set_memory_strategy_nullptr) {
+  DummyExecutor dummy;
+
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.set_memory_strategy(nullptr),
+    std::runtime_error("Received NULL memory strategy in executor."));
+}
+
+TEST_F(TestExecutor, set_memory_strategy) {
+  DummyExecutor dummy;
+  rclcpp::memory_strategy::MemoryStrategy::SharedPtr strategy =
+    std::make_shared<
+    rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>();
+
+  dummy.set_memory_strategy(strategy);
+  EXPECT_EQ(dummy.memory_strategy_ptr(), strategy.get());
+}
+
+TEST_F(TestExecutor, spin_once_failed_trigger_guard_condition) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {});
+
+  dummy.add_node(node);
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.spin_once(std::chrono::milliseconds(1)),
+    std::runtime_error(
+      "Failed to trigger guard condition from execute_any_executable: error not set"));
+}
+
+TEST_F(TestExecutor, spin_some_fail_wait_set_clear) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {});
+
+  dummy.add_node(node);
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.spin_some(std::chrono::milliseconds(1)),
+    std::runtime_error("Couldn't clear wait set: error not set"));
+}
+
+TEST_F(TestExecutor, spin_some_fail_wait_set_resize) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {});
+
+  dummy.add_node(node);
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_resize, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.spin_some(std::chrono::milliseconds(1)),
+    std::runtime_error("Couldn't resize the wait set: error not set"));
+}
+
+TEST_F(TestExecutor, spin_some_fail_add_handles_to_wait_set) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {});
+
+  dummy.add_node(node);
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_wait_set_add_subscription,
+    RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.spin_some(std::chrono::milliseconds(1)),
+    std::runtime_error("Couldn't fill wait set"));
+}
+
+TEST_F(TestExecutor, spin_some_fail_wait) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {});
+
+  dummy.add_node(node);
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    dummy.spin_some(std::chrono::milliseconds(1)),
+    std::runtime_error("rcl_wait() failed: error not set"));
+}
+
+TEST_F(TestExecutor, get_node_by_group_null_group) {
+  DummyExecutor dummy;
+  ASSERT_EQ(nullptr, dummy.local_get_node_by_group(nullptr));
+}
+
+TEST_F(TestExecutor, get_node_by_group) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  dummy.add_callback_group(cb_group, node->get_node_base_interface(), false);
+  ASSERT_EQ(node->get_node_base_interface().get(), dummy.local_get_node_by_group(cb_group).get());
+}
+
+TEST_F(TestExecutor, get_node_by_group_not_found) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  ASSERT_EQ(nullptr, dummy.local_get_node_by_group(cb_group).get());
+}
+
+TEST_F(TestExecutor, get_group_by_timer_nullptr) {
+  DummyExecutor dummy;
+  ASSERT_EQ(nullptr, dummy.local_get_group_by_timer(nullptr));
+}
+
+TEST_F(TestExecutor, get_group_by_timer) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group);
+  dummy.add_node(node);
+
+  ASSERT_EQ(cb_group.get(), dummy.local_get_group_by_timer(timer).get());
+}
+
+TEST_F(TestExecutor, get_group_by_timer_with_deleted_group) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group);
+  dummy.add_node(node);
+
+  cb_group.reset();
+
+  ASSERT_EQ(nullptr, dummy.local_get_group_by_timer(timer).get());
+}
+
+TEST_F(TestExecutor, get_group_by_timer_add_callback_group) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive);
+  auto timer =
+    node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group);
+  dummy.add_callback_group(cb_group, node->get_node_base_interface(), false);
+
+  ASSERT_EQ(cb_group.get(), dummy.local_get_group_by_timer(timer).get());
+}
+
+TEST_F(TestExecutor, spin_until_future_complete_in_spin_until_future_complete) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  bool spin_until_future_complete_in_spin_until_future_complete = false;
+  auto timer =
+    node->create_wall_timer(
+    std::chrono::milliseconds(1), [&]() {
+      try {
+        std::promise<void> promise;
+        std::future<void> future = promise.get_future();
+        dummy.spin_until_future_complete(future, std::chrono::milliseconds(1));
+      } catch (const std::runtime_error & err) {
+        if (err.what() == std::string(
+          "spin_until_future_complete() called while already spinning"))
+        {
+          spin_until_future_complete_in_spin_until_future_complete = true;
+        }
+      }
+    });
+
+  dummy.add_node(node);
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  EXPECT_FALSE(spin_until_future_complete_in_spin_until_future_complete);
+  std::promise<void> promise;
+  std::future<void> future = promise.get_future();
+  dummy.spin_until_future_complete(future, std::chrono::milliseconds(1));
+  EXPECT_TRUE(spin_until_future_complete_in_spin_until_future_complete);
+}
+
+TEST_F(TestExecutor, spin_node_once_base_interface) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  bool spin_called = false;
+  auto timer =
+    node->create_wall_timer(
+    std::chrono::milliseconds(1), [&]() {
+      spin_called = true;
+    });
+
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  EXPECT_FALSE(spin_called);
+  dummy.spin_node_once(node->get_node_base_interface());
+  EXPECT_TRUE(spin_called);
+}
+
+TEST_F(TestExecutor, spin_node_once_node) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  bool spin_called = false;
+  auto timer =
+    node->create_wall_timer(
+    std::chrono::milliseconds(1), [&]() {
+      spin_called = true;
+    });
+
+  // Wait for the wall timer to have expired.
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  EXPECT_FALSE(spin_called);
+  dummy.spin_node_once(node);
+  EXPECT_TRUE(spin_called);
+}
+
+TEST_F(TestExecutor, spin_until_future_complete_future_already_complete) {
+  DummyExecutor dummy;
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  std::promise<void> promise;
+  std::future<void> future = promise.get_future();
+  promise.set_value();
+  EXPECT_EQ(
+    rclcpp::FutureReturnCode::SUCCESS,
+    dummy.spin_until_future_complete(future, std::chrono::milliseconds(1)));
+}

rclcpp/test/rclcpp/test_expand_topic_or_service_name.cpp

@@ -14,9 +14,20 @@
 
 #include <gtest/gtest.h>
 
+#include <stdexcept>
+
+#include "rcl/expand_topic_name.h"
+#include "rcl/validate_topic_name.h"
+#include "rmw/validate_full_topic_name.h"
+#include "rmw/validate_namespace.h"
+#include "rmw/validate_node_name.h"
+
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/expand_topic_or_service_name.hpp"
 
+#include "../mocking_utils/patch.hpp"
+#include "../utils/rclcpp_gtest_macros.hpp"
+
 /*
    Testing expand_topic_or_service_name.
  */
@@ -78,3 +89,156 @@ TEST(TestExpandTopicOrServiceName, exceptions) {
     }, rclcpp::exceptions::InvalidServiceNameError);
   }
 }
+
+// Required for mocking_utils below
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >)
+
+TEST(TestExpandTopicOrServiceName, rcutils_string_map_init_fail_bad_alloc) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcutils_string_map_init, RCUTILS_RET_BAD_ALLOC);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    std::bad_alloc());
+}
+
+TEST(TestExpandTopicOrServiceName, rcutils_string_map_init_fail_other) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcutils_string_map_init, RCUTILS_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    std::runtime_error("error not set"));
+}
+
+TEST(TestExpandTopicOrServiceName, rcl_get_default_topic_name_substitution_fail) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_default_topic_name_substitutions, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    std::runtime_error("error not set"));
+}
+
+TEST(TestExpandTopicOrServiceName, rcl_get_default_topic_name_substitution_and_map_fini_fail) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_default_topic_name_substitutions, RCL_RET_ERROR);
+  auto mock2 = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcutils_string_map_fini, RCUTILS_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    std::runtime_error("error not set"));
+}
+
+TEST(TestExpandTopicOrServiceName, rcutils_string_map_fini_fail_bad_alloc) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcutils_string_map_fini, RCUTILS_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    std::runtime_error("error not set"));
+}
+
+TEST(TestExpandTopicOrServiceName, rmw_valid_full_topic_name_fail_invalid_argument) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_full_topic_name, RMW_RET_INVALID_ARGUMENT);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    rclcpp::exceptions::RCLInvalidArgument(
+      RCL_RET_INVALID_ARGUMENT, rcl_get_error_state(), "failed to validate full topic name"));
+}
+
+TEST(TestExpandTopicOrServiceName, rcl_expand_topic_name_fail) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_expand_topic_name, RCL_RET_TOPIC_NAME_INVALID);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    std::runtime_error("topic name unexpectedly valid"));
+}
+
+TEST(TestExpandTopicOrServiceName, rcl_validate_topic_name_fail) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_expand_topic_name, RCL_RET_TOPIC_NAME_INVALID);
+  auto mock2 = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_validate_topic_name, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    rclcpp::exceptions::RCLError(
+      RCL_RET_ERROR, rcl_get_error_state(), "failed to validate full topic name"));
+}
+
+TEST(TestExpandTopicOrServiceName, rmw_validate_node_name_fail_invalid_argument) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAME);
+  auto mock2 = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_node_name, RMW_RET_INVALID_ARGUMENT);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    rclcpp::exceptions::RCLInvalidArgument(
+      RCL_RET_INVALID_ARGUMENT, rcl_get_error_state(), "failed to validate node name"));
+}
+
+TEST(TestExpandTopicOrServiceName, rmw_validate_node_name_fail_other) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAME);
+  auto mock2 = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_node_name, RMW_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    rclcpp::exceptions::RCLError(
+      RCL_RET_ERROR, rcl_get_error_state(), "failed to validate node name"));
+}
+
+TEST(TestExpandTopicOrServiceName, rmw_validate_namespace_fail_invalid_argument) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAMESPACE);
+  auto mock2 = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_namespace, RMW_RET_INVALID_ARGUMENT);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    rclcpp::exceptions::RCLInvalidArgument(
+      RCL_RET_INVALID_ARGUMENT, rcl_get_error_state(), "failed to validate namespace"));
+}
+
+TEST(TestExpandTopicOrServiceName, rmw_validate_namespace_fail_other) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAMESPACE);
+  auto mock2 = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_namespace, RMW_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    rclcpp::exceptions::RCLError(
+      RCL_RET_ERROR, rcl_get_error_state(), "failed to validate namespace"));
+}
+
+TEST(TestExpandTopicOrServiceName, rcl_expand_topic_name_fail_other) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_expand_topic_name, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    rclcpp::exceptions::RCLError(RCL_RET_ERROR, rcl_get_error_state(), "error not set"));
+}
+
+TEST(TestExpandTopicOrServiceName, rcl_expand_topic_name_fail_invalid_node_name) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAME);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    std::runtime_error("invalid rcl node name but valid rmw node name"));
+}
+
+TEST(TestExpandTopicOrServiceName, rcl_expand_topic_name_fail_invalid_node_namespace) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAMESPACE);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    std::runtime_error("invalid rcl namespace but valid rmw namespace"));
+}
+
+TEST(TestExpandTopicOrServiceName, rmw_validate_full_topic_name_fail_other) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_validate_full_topic_name, RMW_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"),
+    rclcpp::exceptions::RCLError(
+      RCL_RET_ERROR, rcl_get_error_state(), "failed to validate full topic name"));
+}

rclcpp/test/rclcpp/test_future_return_code.cpp

@@ -0,0 +1,42 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <sstream>
+#include <string>
+
+#include "rclcpp/future_return_code.hpp"
+
+TEST(TestFutureReturnCode, to_string) {
+  EXPECT_EQ(
+    "Unknown enum value (-1)", rclcpp::to_string(rclcpp::FutureReturnCode(-1)));
+  EXPECT_EQ(
+    "SUCCESS (0)", rclcpp::to_string(rclcpp::FutureReturnCode::SUCCESS));
+  EXPECT_EQ(
+    "INTERRUPTED (1)", rclcpp::to_string(rclcpp::FutureReturnCode::INTERRUPTED));
+  EXPECT_EQ(
+    "TIMEOUT (2)", rclcpp::to_string(rclcpp::FutureReturnCode::TIMEOUT));
+  EXPECT_EQ(
+    "Unknown enum value (3)", rclcpp::to_string(rclcpp::FutureReturnCode(3)));
+  EXPECT_EQ(
+    "Unknown enum value (100)", rclcpp::to_string(rclcpp::FutureReturnCode(100)));
+}
+
+TEST(FutureReturnCode, ostream) {
+  std::ostringstream ostream;
+
+  ostream << rclcpp::FutureReturnCode::SUCCESS;
+  ASSERT_EQ("SUCCESS (0)", ostream.str());
+}

rclcpp/test/rclcpp/test_graph_listener.cpp

@@ -0,0 +1,293 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include <memory>
+
+#include "rclcpp/contexts/default_context.hpp"
+#include "rclcpp/graph_listener.hpp"
+#include "rclcpp/node_interfaces/node_graph.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "../mocking_utils/patch.hpp"
+#include "../utils/rclcpp_gtest_macros.hpp"
+
+namespace
+{
+
+constexpr char node_name[] = "node";
+constexpr char node_namespace[] = "ns";
+constexpr char shutdown_error_str[] = "GraphListener already shutdown";
+
+}  // namespace
+
+class TestGraphListener : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    rclcpp::init(0, nullptr);
+    node_ = std::make_shared<rclcpp::Node>(node_name, node_namespace);
+
+    node_graph_ = node_->get_node_graph_interface();
+    ASSERT_NE(nullptr, node_graph_);
+
+    graph_listener_ =
+      std::make_shared<rclcpp::graph_listener::GraphListener>(
+      rclcpp::contexts::get_global_default_context());
+  }
+
+  void TearDown()
+  {
+    rclcpp::shutdown();
+  }
+
+protected:
+  std::shared_ptr<rclcpp::Node> node() {return node_;}
+  rclcpp::node_interfaces::NodeGraphInterface * node_graph() {return node_graph_.get();}
+  std::shared_ptr<rclcpp::graph_listener::GraphListener> graph_listener() {return graph_listener_;}
+
+private:
+  std::shared_ptr<rclcpp::Node> node_;
+  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
+  std::shared_ptr<rclcpp::graph_listener::GraphListener> graph_listener_;
+};
+
+/* Run base class init/shutdown */
+TEST_F(TestGraphListener, construction_and_destruction) {
+  EXPECT_FALSE(graph_listener()->has_node(node_graph()));
+  EXPECT_FALSE(graph_listener()->is_shutdown());
+}
+
+// Required for mocking_utils below
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, ==)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, !=)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, <)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, >)
+
+/* Error creating a new graph listener */
+TEST_F(TestGraphListener, error_construct_graph_listener) {
+  using rclcpp::contexts::get_global_default_context;
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_guard_condition_init, RCL_RET_ERROR);
+
+  RCLCPP_EXPECT_THROW_EQ(
+  {
+    auto graph_listener_error =
+    std::make_shared<rclcpp::graph_listener::GraphListener>(get_global_default_context());
+    graph_listener_error.reset();
+  }, std::runtime_error("failed to create interrupt guard condition: error not set"));
+}
+
+// Required for mocking_utils below
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >)
+
+/* Errors that occur when initializing the graph_listener */
+TEST_F(TestGraphListener, error_start_graph_listener) {
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_wait_set_init, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      graph_listener()->start_if_not_started(),
+      std::runtime_error("failed to initialize wait set: error not set"));
+  }
+  {
+    EXPECT_NO_THROW(graph_listener()->shutdown());
+    RCLCPP_EXPECT_THROW_EQ(
+      graph_listener()->start_if_not_started(),
+      std::runtime_error(shutdown_error_str));
+  }
+}
+
+class TestGraphListenerProtectedMethods : public rclcpp::graph_listener::GraphListener
+{
+public:
+  explicit TestGraphListenerProtectedMethods(std::shared_ptr<rclcpp::Context> parent_context)
+  : rclcpp::graph_listener::GraphListener{parent_context}
+  {}
+
+  void run_protected()
+  {
+    this->run();
+  }
+
+  void mock_start_thread()
+  {
+    // This function prepares the loop thread to be run, but leave
+    // early with the failure thrown. That way the graph_listener wait_set
+    // is init, without being started
+    auto mock_wait_set_init = mocking_utils::inject_on_return(
+      "lib:rclcpp", rcl_wait_set_init, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      this->start_if_not_started(),
+      std::runtime_error("failed to initialize wait set: error not set"));
+  }
+};
+
+/* Errors running graph protected methods */
+TEST_F(TestGraphListener, error_run_graph_listener_destroy_context) {
+  auto context_to_destroy = std::make_shared<rclcpp::contexts::DefaultContext>();
+  context_to_destroy->init(0, nullptr);
+  auto graph_listener_error =
+    std::make_shared<TestGraphListenerProtectedMethods>(context_to_destroy);
+  context_to_destroy.reset();
+  EXPECT_NO_THROW(graph_listener_error->run_protected());
+}
+
+TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_set_clear) {
+  auto global_context = rclcpp::contexts::get_global_default_context();
+  auto graph_listener_test =
+    std::make_shared<TestGraphListenerProtectedMethods>(global_context);
+  graph_listener_test->mock_start_thread();
+  auto mock_wait_set_clear = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    graph_listener_test->run_protected(),
+    std::runtime_error("failed to clear wait set: error not set"));
+}
+
+TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_set_add_guard_condition) {
+  auto global_context = rclcpp::contexts::get_global_default_context();
+  auto graph_listener_test =
+    std::make_shared<TestGraphListenerProtectedMethods>(global_context);
+  graph_listener_test->mock_start_thread();
+  auto mock_wait_set_clear = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_wait_set_add_guard_condition, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    graph_listener_test->run_protected(),
+    std::runtime_error("failed to add interrupt guard condition to wait set: error not set"));
+}
+
+TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_set_add_guard_condition_twice) {
+  auto global_context = rclcpp::contexts::get_global_default_context();
+  auto graph_listener_test =
+    std::make_shared<TestGraphListenerProtectedMethods>(global_context);
+  graph_listener_test->mock_start_thread();
+  auto mock = mocking_utils::patch(
+    "lib:rclcpp", rcl_wait_set_add_guard_condition, [](auto, ...) {
+      static int counter = 1;
+      if (counter == 1) {
+        counter++;
+        return RCL_RET_OK;
+      } else {
+        return RCL_RET_ERROR;
+      }
+    });
+  RCLCPP_EXPECT_THROW_EQ(
+    graph_listener_test->run_protected(),
+    std::runtime_error("failed to add shutdown guard condition to wait set: error not set"));
+}
+
+TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_error) {
+  auto global_context = rclcpp::contexts::get_global_default_context();
+  auto graph_listener_test =
+    std::make_shared<TestGraphListenerProtectedMethods>(global_context);
+  graph_listener_test->mock_start_thread();
+  auto mock_wait_set_clear = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_wait, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    graph_listener_test->run_protected(),
+    std::runtime_error("failed to wait on wait set: error not set"));
+}
+
+TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_timeout) {
+  auto global_context = rclcpp::contexts::get_global_default_context();
+  auto graph_listener_test =
+    std::make_shared<TestGraphListenerProtectedMethods>(global_context);
+  graph_listener_test->mock_start_thread();
+  auto mock_wait_set_clear = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_wait, RCL_RET_TIMEOUT);
+  RCLCPP_EXPECT_THROW_EQ(
+    graph_listener_test->run_protected(),
+    std::runtime_error("rcl_wait unexpectedly timed out"));
+}
+
+/* Add/Remove node usage */
+TEST_F(TestGraphListener, test_graph_listener_add_remove_node) {
+  EXPECT_FALSE(graph_listener()->has_node(node_graph()));
+
+  graph_listener()->add_node(node_graph());
+  EXPECT_TRUE(graph_listener()->has_node(node_graph()));
+
+  graph_listener()->remove_node(node_graph());
+  EXPECT_FALSE(graph_listener()->has_node(node_graph()));
+}
+
+/* Add/Remove node error usage */
+TEST_F(TestGraphListener, test_errors_graph_listener_add_remove_node) {
+  // nullptrs tests
+  EXPECT_FALSE(graph_listener()->has_node(nullptr));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    graph_listener()->add_node(nullptr),
+    std::invalid_argument("node is nullptr"));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    graph_listener()->remove_node(nullptr),
+    std::invalid_argument("node is nullptr"));
+
+  // Already added
+  graph_listener()->add_node(node_graph());
+  EXPECT_TRUE(graph_listener()->has_node(node_graph()));
+  RCLCPP_EXPECT_THROW_EQ(
+    graph_listener()->add_node(node_graph()),
+    std::runtime_error("node already added"));
+
+  // Remove node not found
+  graph_listener()->remove_node(node_graph());
+  EXPECT_FALSE(graph_listener()->has_node(node_graph()));
+  RCLCPP_EXPECT_THROW_EQ(
+    graph_listener()->remove_node(node_graph()),
+    std::runtime_error("node not found"));
+
+  // Add and remove after shutdown
+  EXPECT_NO_THROW(graph_listener()->shutdown());
+  RCLCPP_EXPECT_THROW_EQ(
+    graph_listener()->add_node(node_graph()),
+    std::runtime_error(shutdown_error_str));
+  // Remove works the same
+  RCLCPP_EXPECT_THROW_EQ(
+    graph_listener()->remove_node(node_graph()),
+    std::runtime_error("node not found"));
+}
+
+/* Shutdown errors */
+TEST_F(TestGraphListener, test_graph_listener_shutdown_wait_fini_error_nothrow) {
+  graph_listener()->start_if_not_started();
+  auto mock_wait_set_fini = mocking_utils::inject_on_return(
+    "lib:rclcpp", rcl_wait_set_fini, RCL_RET_ERROR);
+  // Exception is logged when using nothrow_t
+  EXPECT_NO_THROW(graph_listener()->shutdown(std::nothrow_t()));
+}
+
+TEST_F(TestGraphListener, test_graph_listener_shutdown_wait_fini_error_throw) {
+  graph_listener()->start_if_not_started();
+  auto mock_wait_set_fini = mocking_utils::inject_on_return(
+    "lib:rclcpp", rcl_wait_set_fini, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    graph_listener()->shutdown(),
+    std::runtime_error("failed to finalize wait set: error not set"));
+}
+
+TEST_F(TestGraphListener, test_graph_listener_shutdown_guard_fini_error_throw) {
+  graph_listener()->start_if_not_started();
+  auto mock_wait_set_fini = mocking_utils::inject_on_return(
+    "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    graph_listener()->shutdown(),
+    std::runtime_error("failed to finalize interrupt guard condition: error not set"));
+}

rclcpp/test/rclcpp/test_guard_condition.cpp

@@ -18,6 +18,8 @@
 
 #include "rclcpp/rclcpp.hpp"
 
+#include "../mocking_utils/patch.hpp"
+
 class TestGuardCondition : public ::testing::Test
 {
 protected:
@@ -54,6 +56,14 @@ TEST_F(TestGuardCondition, construction_and_destruction) {
       (void)gc;
     }, rclcpp::exceptions::RCLInvalidArgument);
   }
+
+  {
+    auto mock = mocking_utils::inject_on_return(
+      "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR);
+    auto gc = std::make_shared<rclcpp::GuardCondition>();
+    // This just logs an error on destruction
+    EXPECT_NO_THROW(gc.reset());
+  }
 }
 
 /*
@@ -82,6 +92,13 @@ TEST_F(TestGuardCondition, get_rcl_guard_condition) {
 TEST_F(TestGuardCondition, trigger) {
   {
     auto gc = std::make_shared<rclcpp::GuardCondition>();
-    gc->trigger();
+    EXPECT_NO_THROW(gc->trigger());
+
+    {
+      auto mock = mocking_utils::patch_and_return(
+        "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+      auto gc = std::make_shared<rclcpp::GuardCondition>();
+      EXPECT_THROW(gc->trigger(), rclcpp::exceptions::RCLError);
+    }
   }
 }

rclcpp/test/rclcpp/test_init_options.cpp

@@ -14,6 +14,7 @@
 
 #include <gtest/gtest.h>
 
+#include <stdexcept>
 #include <string>
 #include <vector>
 
@@ -22,6 +23,8 @@
 
 #include "rclcpp/init_options.hpp"
 
+#include "../mocking_utils/patch.hpp"
+#include "../utils/rclcpp_gtest_macros.hpp"
 
 TEST(TestInitOptions, test_construction) {
   rcl_allocator_t allocator = rcl_get_default_allocator();
@@ -75,3 +78,63 @@ TEST(TestInitOptions, test_domain_id) {
   options.use_default_domain_id();
   EXPECT_EQ(domain_id, options.get_domain_id());
 }
+
+// Required for mocking_utils below
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >)
+
+TEST(TestInitOptions, constructor_rcl_init_options_init_failed) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_init_options_init, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::InitOptions(),
+    std::runtime_error("failed to initialize rcl init options: error not set"));
+}
+
+TEST(TestInitOptions, constructor_rcl_init_options_copy_failed) {
+  rcl_init_options_t rcl_opts;
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_init_options_copy, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    new rclcpp::InitOptions(rcl_opts),
+    std::runtime_error("failed to copy rcl init options: error not set"));
+}
+
+TEST(TestInitOptions, copy_constructor_rcl_init_options_copy_failed) {
+  rclcpp::InitOptions options;
+  rclcpp::InitOptions options2;
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_init_options_copy, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    options2.operator=(options),
+    std::runtime_error("failed to copy rcl init options: error not set"));
+}
+
+TEST(TestInitOptions, use_default_domain_id_rcl_get_default_domain_id_failed) {
+  rclcpp::InitOptions options;
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_get_default_domain_id, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    options.use_default_domain_id(),
+    std::runtime_error("failed to get default domain id: error not set"));
+}
+
+TEST(TestInitOptions, set_domain_id_rcl_init_options_set_domain_id_failed) {
+  rclcpp::InitOptions options;
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_init_options_set_domain_id, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    options.set_domain_id(0),
+    std::runtime_error("failed to set domain id to rcl init options: error not set"));
+}
+
+TEST(TestInitOptions, get_domain_id_rcl_init_options_get_domain_id_failed) {
+  rclcpp::InitOptions options;
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_init_options_get_domain_id, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    options.get_domain_id(),
+    std::runtime_error("failed to get domain id from rcl init options: error not set"));
+}

rclcpp/test/rclcpp/test_loaned_message.cpp

@@ -20,6 +20,8 @@
 
 #include "test_msgs/msg/basic_types.hpp"
 
+#include "../mocking_utils/patch.hpp"
+
 using MessageT = test_msgs::msg::BasicTypes;
 using LoanedMessageT = rclcpp::LoanedMessage<MessageT>;
 
@@ -81,3 +83,51 @@ TEST_F(TestLoanedMessage, release) {
 
   SUCCEED();
 }
+
+TEST_F(TestLoanedMessage, construct_with_loaned_message_publisher) {
+  auto node = std::make_shared<rclcpp::Node>("loaned_message_test_node");
+  auto publisher = node->create_publisher<MessageT>("topic", 10);
+  std::allocator<MessageT> allocator;
+
+  auto mock_can_loan = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_publisher_can_loan_messages, true);
+
+  {
+    auto mock_borrow_loaned = mocking_utils::patch_and_return(
+      "self", rcl_borrow_loaned_message, RCL_RET_ERROR);
+
+    EXPECT_THROW(
+      std::make_shared<LoanedMessageT>(*publisher.get(), allocator).reset(),
+      rclcpp::exceptions::RCLError);
+  }
+
+  MessageT message;
+  auto borrow_loaned_message_callback =
+    [&message](
+    const rcl_publisher_t *, const rosidl_message_type_support_t *, void ** ros_message) {
+      *ros_message = &message;
+      return RCL_RET_OK;
+    };
+  auto mock_borrow_loaned = mocking_utils::patch(
+    "self", rcl_borrow_loaned_message, borrow_loaned_message_callback);
+
+  {
+    auto mock_return_loaned = mocking_utils::patch_and_return(
+      "self", rcl_return_loaned_message_from_publisher, RCL_RET_OK);
+
+    auto loaned_message = std::make_shared<LoanedMessageT>(*publisher.get(), allocator);
+    EXPECT_TRUE(loaned_message->is_valid());
+    EXPECT_NO_THROW(loaned_message.reset());
+  }
+
+  {
+    auto loaned_message = std::make_shared<LoanedMessageT>(*publisher.get(), allocator);
+    EXPECT_TRUE(loaned_message->is_valid());
+
+    auto mock_return_loaned = mocking_utils::patch_and_return(
+      "self", rcl_return_loaned_message_from_publisher, RCL_RET_ERROR);
+
+    // No exception, it just logs an error
+    EXPECT_NO_THROW(loaned_message.reset());
+  }
+}

rclcpp/test/rclcpp/test_memory_strategy.cpp

@@ -297,6 +297,12 @@ TEST_F(TestMemoryStrategy, get_node_by_group) {
 
     callback_group =
       node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+
+    // Nothing in the weak_groups_to_nodes, so find fails.
+    EXPECT_EQ(
+      nullptr,
+      memory_strategy()->get_node_by_group(callback_group, weak_groups_to_nodes));
+
     weak_groups_to_nodes.insert(
       std::pair<rclcpp::CallbackGroup::WeakPtr,
       rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>(
@@ -306,6 +312,7 @@ TEST_F(TestMemoryStrategy, get_node_by_group) {
       node_handle,
       memory_strategy()->get_node_by_group(callback_group, weak_groups_to_nodes));
   }  // Node goes out of scope
+  // Callback group still exists, so lookup returns nullptr because node is destroyed.
   EXPECT_EQ(
     nullptr,
     memory_strategy()->get_node_by_group(callback_group, weak_groups_to_nodes));

rclcpp/test/rclcpp/test_node.cpp

@@ -26,7 +26,14 @@
 #include "rclcpp/rclcpp.hpp"
 
 #include "rcpputils/filesystem_helper.hpp"
+
+#include "rmw/validate_namespace.h"
+
 #include "test_msgs/msg/basic_types.hpp"
+#include "test_msgs/msg/empty.hpp"
+#include "test_msgs/srv/empty.hpp"
+
+#include "../mocking_utils/patch.hpp"
 
 class TestNode : public ::testing::Test
 {
@@ -50,7 +57,19 @@ protected:
 TEST_F(TestNode, construction_and_destruction) {
   {
     auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
-    (void)node;
+    EXPECT_NE(nullptr, node->get_node_base_interface());
+    EXPECT_NE(nullptr, node->get_node_clock_interface());
+    EXPECT_NE(nullptr, node->get_node_graph_interface());
+    EXPECT_NE(nullptr, node->get_node_logging_interface());
+    EXPECT_NE(nullptr, node->get_node_time_source_interface());
+    EXPECT_NE(nullptr, node->get_node_timers_interface());
+    EXPECT_NE(nullptr, node->get_node_topics_interface());
+    EXPECT_NE(nullptr, node->get_node_services_interface());
+    EXPECT_NE(nullptr, node->get_node_parameters_interface());
+    EXPECT_NE(nullptr, node->get_node_waitables_interface());
+    EXPECT_NE(nullptr, node->get_node_options().get_rcl_node_options());
+    EXPECT_NE(nullptr, node->get_graph_event());
+    EXPECT_NE(nullptr, node->get_clock());
   }
 
   {
@@ -279,8 +298,11 @@ TEST_F(TestNode, get_logger) {
 TEST_F(TestNode, get_clock) {
   auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
   auto ros_clock = node->get_clock();
-  EXPECT_TRUE(ros_clock != nullptr);
+  EXPECT_NE(nullptr, ros_clock);
   EXPECT_EQ(ros_clock->get_clock_type(), RCL_ROS_TIME);
+
+  const rclcpp::Node & const_node = *node.get();
+  EXPECT_NE(nullptr, const_node.get_clock());
 }
 
 TEST_F(TestNode, now) {
@@ -785,15 +807,24 @@ TEST_F(TestNode, undeclare_parameter) {
 
 TEST_F(TestNode, has_parameter) {
   auto node = std::make_shared<rclcpp::Node>("test_has_parameter_node"_unq);
-  {
-    // normal use
-    auto name = "parameter"_unq;
-    EXPECT_FALSE(node->has_parameter(name));
-    node->declare_parameter(name);
-    EXPECT_TRUE(node->has_parameter(name));
-    node->undeclare_parameter(name);
-    EXPECT_FALSE(node->has_parameter(name));
-  }
+  // normal use
+  auto name = "parameter"_unq;
+  EXPECT_FALSE(node->has_parameter(name));
+  node->declare_parameter(name);
+  EXPECT_TRUE(node->has_parameter(name));
+  node->undeclare_parameter(name);
+  EXPECT_FALSE(node->has_parameter(name));
+}
+
+TEST_F(TestNode, list_parameters) {
+  auto node = std::make_shared<rclcpp::Node>("test_list_parameter_node"_unq);
+  // normal use
+  auto name = "parameter"_unq;
+  const size_t before_size = node->list_parameters({}, 1u).names.size();
+  node->declare_parameter(name);
+  EXPECT_EQ(1u + before_size, node->list_parameters({}, 1u).names.size());
+  node->undeclare_parameter(name);
+  EXPECT_EQ(before_size, node->list_parameters({}, 1u).names.size());
 }
 
 TEST_F(TestNode, set_parameter_undeclared_parameters_not_allowed) {
@@ -2455,3 +2486,89 @@ TEST_F(TestNode, get_publishers_subscriptions_info_by_topic) {
     subscription_list = node->get_subscriptions_info_by_topic("13");
   }, rclcpp::exceptions::InvalidTopicNameError);
 }
+
+TEST_F(TestNode, callback_groups) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  size_t num_callback_groups_in_basic_node = node->get_callback_groups().size();
+
+  auto group1 = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  EXPECT_EQ(1u + num_callback_groups_in_basic_node, node->get_callback_groups().size());
+
+  auto group2 = node->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
+  EXPECT_EQ(2u + num_callback_groups_in_basic_node, node->get_callback_groups().size());
+}
+
+// This is tested more thoroughly in node_interfaces/test_node_graph
+TEST_F(TestNode, get_entity_names) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  const auto node_names = node->get_node_names();
+  EXPECT_NE(
+    node_names.end(),
+    std::find(node_names.begin(), node_names.end(), node->get_fully_qualified_name()));
+
+  const auto topic_names_and_types = node->get_topic_names_and_types();
+  EXPECT_EQ(topic_names_and_types.end(), topic_names_and_types.find("topic"));
+
+  EXPECT_EQ(0u, node->count_publishers("topic"));
+  EXPECT_EQ(0u, node->count_subscribers("topic"));
+
+  const auto service_names_and_types = node->get_service_names_and_types();
+  EXPECT_EQ(service_names_and_types.end(), service_names_and_types.find("service"));
+
+  const auto service_names_and_types_by_node =
+    node->get_service_names_and_types_by_node("node", "/ns");
+  EXPECT_EQ(
+    service_names_and_types_by_node.end(),
+    service_names_and_types_by_node.find("service"));
+}
+
+TEST_F(TestNode, wait_for_graph_event) {
+  // Even though this node is only used in the std::thread below, it's here to ensure there is no
+  // race condition in its destruction and modification of the node_graph
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+
+  constexpr std::chrono::seconds timeout(10);
+  auto thread_start = std::chrono::steady_clock::now();
+  auto thread_completion = thread_start;
+
+  // This runs until the graph is updated
+  std::thread graph_event_wait_thread([&thread_completion, node, timeout]() {
+      auto event = node->get_graph_event();
+      EXPECT_NO_THROW(node->wait_for_graph_change(event, timeout));
+      thread_completion = std::chrono::steady_clock::now();
+    });
+
+  // Start creating nodes until at least one event triggers in graph_event_wait_thread or until 100
+  // nodes have been created (at which point this is a failure)
+  std::vector<std::shared_ptr<rclcpp::Node>> nodes;
+  while (thread_completion == thread_start && nodes.size() < 100) {
+    nodes.emplace_back(std::make_shared<rclcpp::Node>("node"_unq, "ns"));
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  }
+  graph_event_wait_thread.join();
+  // Nodes will probably only be of size 1
+  EXPECT_LT(0u, nodes.size());
+  EXPECT_GT(100u, nodes.size());
+  EXPECT_NE(thread_start, thread_completion);
+  EXPECT_GT(timeout, thread_completion - thread_start);
+}
+
+TEST_F(TestNode, create_sub_node_rmw_validate_namespace_error) {
+  auto node = std::make_shared<rclcpp::Node>("node", "ns");
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rmw_validate_namespace, RMW_RET_INVALID_ARGUMENT);
+
+    // reset() is not necessary for this exception, but it handles unused return value warning
+    EXPECT_THROW(
+      node->create_sub_node("ns").reset(),
+      rclcpp::exceptions::RCLInvalidArgument);
+  }
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rmw_validate_namespace, RMW_RET_ERROR);
+    EXPECT_THROW(
+      node->create_sub_node("ns").reset(),
+      rclcpp::exceptions::RCLError);
+  }
+}

rclcpp/test/rclcpp/test_node_options.cpp

@@ -14,6 +14,7 @@
 
 #include <gtest/gtest.h>
 
+#include <memory>
 #include <string>
 #include <vector>
 
@@ -23,6 +24,8 @@
 
 #include "rclcpp/node_options.hpp"
 
+#include "../mocking_utils/patch.hpp"
+
 
 TEST(TestNodeOptions, ros_args_only) {
   rcl_allocator_t allocator = rcl_get_default_allocator();
@@ -198,3 +201,118 @@ TEST(TestNodeOptions, copy) {
       rcl_arguments_get_count_unparsed(&rcl_options->arguments));
   }
 }
+
+TEST(TestNodeOptions, append_parameter_override) {
+  std::vector<std::string> expected_args{"--unknown-flag", "arg"};
+  auto options = rclcpp::NodeOptions().arguments(expected_args).use_global_arguments(false);
+  rclcpp::Parameter parameter("some_parameter", 10);
+  options.append_parameter_override("some_parameter", 10);
+  EXPECT_EQ(1u, options.parameter_overrides().size());
+  EXPECT_EQ(std::string("some_parameter"), options.parameter_overrides()[0].get_name());
+}
+
+TEST(TestNodeOptions, rcl_node_options_fini_error) {
+  auto mock = mocking_utils::inject_on_return("lib:rclcpp", rcl_node_options_fini, RCL_RET_ERROR);
+  auto options = std::make_shared<rclcpp::NodeOptions>();
+  // Necessary to setup internal pointer
+  ASSERT_NE(nullptr, options->get_rcl_node_options());
+  // If fini fails, this should just log an error and continue
+  EXPECT_NO_THROW(options.reset());
+}
+
+TEST(TestNodeOptions, bool_setters_and_getters) {
+  rclcpp::NodeOptions options;
+
+  options.use_global_arguments(false);
+  EXPECT_FALSE(options.use_global_arguments());
+  EXPECT_FALSE(options.get_rcl_node_options()->use_global_arguments);
+  options.use_global_arguments(true);
+  EXPECT_TRUE(options.use_global_arguments());
+  EXPECT_TRUE(options.get_rcl_node_options()->use_global_arguments);
+
+  options.enable_rosout(false);
+  EXPECT_FALSE(options.enable_rosout());
+  EXPECT_FALSE(options.get_rcl_node_options()->enable_rosout);
+  options.enable_rosout(true);
+  EXPECT_TRUE(options.enable_rosout());
+  EXPECT_TRUE(options.get_rcl_node_options()->enable_rosout);
+
+  options.use_intra_process_comms(false);
+  EXPECT_FALSE(options.use_intra_process_comms());
+  options.use_intra_process_comms(true);
+  EXPECT_TRUE(options.use_intra_process_comms());
+
+  options.enable_topic_statistics(false);
+  EXPECT_FALSE(options.enable_topic_statistics());
+  options.enable_topic_statistics(true);
+  EXPECT_TRUE(options.enable_topic_statistics());
+
+  options.start_parameter_services(false);
+  EXPECT_FALSE(options.start_parameter_services());
+  options.start_parameter_services(true);
+  EXPECT_TRUE(options.start_parameter_services());
+
+  options.allow_undeclared_parameters(false);
+  EXPECT_FALSE(options.allow_undeclared_parameters());
+  options.allow_undeclared_parameters(true);
+  EXPECT_TRUE(options.allow_undeclared_parameters());
+
+  options.start_parameter_event_publisher(false);
+  EXPECT_FALSE(options.start_parameter_event_publisher());
+  options.start_parameter_event_publisher(true);
+  EXPECT_TRUE(options.start_parameter_event_publisher());
+
+  options.automatically_declare_parameters_from_overrides(false);
+  EXPECT_FALSE(options.automatically_declare_parameters_from_overrides());
+  options.automatically_declare_parameters_from_overrides(true);
+  EXPECT_TRUE(options.automatically_declare_parameters_from_overrides());
+}
+
+TEST(TestNodeOptions, parameter_event_qos) {
+  rclcpp::NodeOptions options;
+  rclcpp::QoS qos1(1);
+  rclcpp::QoS qos2(2);
+  EXPECT_NE(qos1, options.parameter_event_qos());
+  EXPECT_NE(qos2, options.parameter_event_qos());
+  options.parameter_event_qos(qos1);
+  EXPECT_EQ(qos1, options.parameter_event_qos());
+  options.parameter_event_qos(qos2);
+  EXPECT_EQ(qos2, options.parameter_event_qos());
+}
+
+TEST(TestNodeOptions, parameter_event_publisher_options) {
+  rclcpp::NodeOptions options;
+  rclcpp::PublisherOptionsBase publisher_options;
+  publisher_options.use_default_callbacks = true;
+  options.parameter_event_publisher_options(publisher_options);
+  EXPECT_TRUE(options.parameter_event_publisher_options().use_default_callbacks);
+
+  publisher_options.use_default_callbacks = false;
+  options.parameter_event_publisher_options(publisher_options);
+  EXPECT_FALSE(options.parameter_event_publisher_options().use_default_callbacks);
+}
+
+TEST(TestNodeOptions, set_get_allocator) {
+  rclcpp::NodeOptions options;
+  EXPECT_NE(nullptr, options.allocator().allocate);
+  EXPECT_NE(nullptr, options.allocator().deallocate);
+  EXPECT_NE(nullptr, options.allocator().reallocate);
+  EXPECT_NE(nullptr, options.allocator().zero_allocate);
+
+  rcl_allocator_t fake_allocator;
+  fake_allocator.allocate = [](size_t, void *) -> void * {return nullptr;};
+  fake_allocator.deallocate = [](void *, void *) {};
+  fake_allocator.reallocate = [](void *, size_t, void *) -> void * {return nullptr;};
+  fake_allocator.zero_allocate = [](size_t, size_t, void *) -> void * {return nullptr;};
+  fake_allocator.state = rcl_get_default_allocator().state;
+
+  options.allocator(fake_allocator);
+  EXPECT_EQ(fake_allocator.allocate, options.allocator().allocate);
+  EXPECT_EQ(fake_allocator.deallocate, options.allocator().deallocate);
+  EXPECT_EQ(fake_allocator.reallocate, options.allocator().reallocate);
+  EXPECT_EQ(fake_allocator.zero_allocate, options.allocator().zero_allocate);
+  EXPECT_EQ(fake_allocator.state, options.allocator().state);
+
+  // Check invalid allocator
+  EXPECT_THROW(options.get_rcl_node_options(), std::bad_alloc);
+}

rclcpp/test/rclcpp/test_parameter.cpp

@@ -16,6 +16,7 @@
 
 #include <algorithm>
 #include <limits>
+#include <memory>
 #include <stdexcept>
 #include <string>
 #include <vector>
@@ -32,11 +33,20 @@ protected:
   }
 };
 
+TEST_F(TestParameter, construct_destruct) {
+  EXPECT_NO_THROW(std::make_shared<rclcpp::Parameter>().reset());
+  EXPECT_NO_THROW(std::make_shared<rclcpp::Parameter>("some_parameter").reset());
+  EXPECT_NO_THROW(std::make_shared<rclcpp::Parameter>("some_parameter", 10).reset());
+}
+
 TEST_F(TestParameter, not_set_variant) {
   // Direct instantiation
   rclcpp::Parameter not_set_variant;
   EXPECT_EQ(rclcpp::PARAMETER_NOT_SET, not_set_variant.get_type());
   EXPECT_EQ("not set", not_set_variant.get_type_name());
+  std::stringstream ss;
+  ss << not_set_variant;
+  EXPECT_EQ("{\"name\": \"\", \"type\": \"not set\", \"value\": \"not set\"}", ss.str());
 
   EXPECT_THROW(not_set_variant.as_bool(), rclcpp::ParameterTypeException);
   EXPECT_THROW(not_set_variant.as_int(), rclcpp::ParameterTypeException);
@@ -56,6 +66,17 @@ TEST_F(TestParameter, not_set_variant) {
   EXPECT_EQ(
     rclcpp::ParameterType::PARAMETER_NOT_SET,
     rclcpp::Parameter::from_parameter_msg(not_set_param).get_type());
+
+  EXPECT_THROW(
+    not_set_variant.get_value<int>(),
+    rclcpp::exceptions::InvalidParameterTypeException);
+
+  // Check == and != operators work as expected
+  EXPECT_EQ(not_set_variant, not_set_variant);
+  rclcpp::Parameter not_set_variant2;
+  EXPECT_EQ(not_set_variant, not_set_variant2);
+  rclcpp::Parameter bool_variant("bool_param", true);
+  EXPECT_NE(not_set_variant, bool_variant);
 }
 
 TEST_F(TestParameter, bool_variant) {
@@ -81,6 +102,10 @@ TEST_F(TestParameter, bool_variant) {
   EXPECT_THROW(bool_variant_true.as_string_array(), rclcpp::ParameterTypeException);
 
   EXPECT_EQ("true", bool_variant_true.value_to_string());
+  std::stringstream ss;
+  ss << bool_variant_true;
+  EXPECT_EQ("{\"name\": \"bool_param\", \"type\": \"bool\", \"value\": \"true\"}", ss.str());
+
 
   rclcpp::Parameter bool_variant_false("bool_param", false);
   EXPECT_FALSE(bool_variant_false.get_value<rclcpp::ParameterType::PARAMETER_BOOL>());
@@ -114,6 +139,11 @@ TEST_F(TestParameter, bool_variant) {
   EXPECT_EQ(
     rcl_interfaces::msg::ParameterType::PARAMETER_BOOL,
     bool_variant_false.get_value_message().type);
+
+  // Check == and != operators work as expected
+  EXPECT_EQ(bool_variant_true, bool_variant_true);
+  EXPECT_NE(bool_variant_false, bool_variant_true);
+  EXPECT_EQ(bool_variant_true, from_msg_true);
 }
 
 TEST_F(TestParameter, integer_variant) {
@@ -143,6 +173,9 @@ TEST_F(TestParameter, integer_variant) {
   EXPECT_THROW(integer_variant.as_string_array(), rclcpp::ParameterTypeException);
 
   EXPECT_EQ("42", integer_variant.value_to_string());
+  std::stringstream ss;
+  ss << integer_variant;
+  EXPECT_EQ("{\"name\": \"integer_param\", \"type\": \"integer\", \"value\": \"42\"}", ss.str());
 
   rcl_interfaces::msg::Parameter integer_param = integer_variant.to_parameter_msg();
   EXPECT_EQ("integer_param", integer_param.name);
@@ -162,6 +195,11 @@ TEST_F(TestParameter, integer_variant) {
   EXPECT_EQ(
     rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER,
     from_msg.get_value_message().type);
+
+  // Check == and != operators work as expected
+  EXPECT_EQ(integer_variant, integer_variant);
+  EXPECT_NE(integer_variant, rclcpp::Parameter("integer_param", 41));
+  EXPECT_EQ(integer_variant, from_msg);
 }
 
 TEST_F(TestParameter, long_integer_variant) {
@@ -191,6 +229,12 @@ TEST_F(TestParameter, long_integer_variant) {
   EXPECT_THROW(long_variant.as_string_array(), rclcpp::ParameterTypeException);
 
   EXPECT_EQ("9223372036854775807", long_variant.value_to_string());
+  std::stringstream ss;
+  ss << long_variant;
+  EXPECT_EQ(
+    "{\"name\": \"long_integer_param\", \"type\": \"integer\", \"value\": "
+    "\"9223372036854775807\"}",
+    ss.str());
 
   rcl_interfaces::msg::Parameter integer_param = long_variant.to_parameter_msg();
   EXPECT_EQ("long_integer_param", integer_param.name);
@@ -210,6 +254,12 @@ TEST_F(TestParameter, long_integer_variant) {
   EXPECT_EQ(
     rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER,
     from_msg.get_value_message().type);
+
+
+  // Check == and != operators work as expected
+  EXPECT_EQ(long_variant, long_variant);
+  EXPECT_NE(long_variant, rclcpp::Parameter("long_integer_param", 41));
+  EXPECT_EQ(long_variant, from_msg);
 }
 
 TEST_F(TestParameter, float_variant) {
@@ -239,6 +289,10 @@ TEST_F(TestParameter, float_variant) {
   EXPECT_THROW(float_variant.as_string_array(), rclcpp::ParameterTypeException);
 
   EXPECT_EQ("42.000000", float_variant.value_to_string());
+  std::stringstream ss;
+  ss << float_variant;
+  EXPECT_EQ(
+    "{\"name\": \"float_param\", \"type\": \"double\", \"value\": \"42.000000\"}", ss.str());
 
   rcl_interfaces::msg::Parameter float_param = float_variant.to_parameter_msg();
   EXPECT_EQ("float_param", float_param.name);
@@ -258,6 +312,11 @@ TEST_F(TestParameter, float_variant) {
   EXPECT_EQ(
     rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE,
     from_msg.get_value_message().type);
+
+  // Check == and != operators work as expected
+  EXPECT_EQ(float_variant, float_variant);
+  EXPECT_NE(float_variant, rclcpp::Parameter("float_param", 41.0));
+  EXPECT_EQ(float_variant, from_msg);
 }
 
 TEST_F(TestParameter, double_variant) {
@@ -287,6 +346,10 @@ TEST_F(TestParameter, double_variant) {
   EXPECT_THROW(double_variant.as_string_array(), rclcpp::ParameterTypeException);
 
   EXPECT_EQ("-42.100000", double_variant.value_to_string());
+  std::stringstream ss;
+  ss << double_variant;
+  EXPECT_EQ(
+    "{\"name\": \"double_param\", \"type\": \"double\", \"value\": \"-42.100000\"}", ss.str());
 
   rcl_interfaces::msg::Parameter double_param = double_variant.to_parameter_msg();
   EXPECT_EQ("double_param", double_param.name);
@@ -306,6 +369,11 @@ TEST_F(TestParameter, double_variant) {
   EXPECT_EQ(
     rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE,
     from_msg.get_value_message().type);
+
+  // Check == and != operators work as expected
+  EXPECT_EQ(double_variant, double_variant);
+  EXPECT_NE(double_variant, rclcpp::Parameter("double_param", -41.2));
+  EXPECT_EQ(double_variant, from_msg);
 }
 
 TEST_F(TestParameter, string_variant) {
@@ -335,6 +403,9 @@ TEST_F(TestParameter, string_variant) {
   EXPECT_THROW(string_variant.as_string_array(), rclcpp::ParameterTypeException);
 
   EXPECT_EQ(TEST_VALUE, string_variant.value_to_string());
+  std::stringstream ss;
+  ss << string_variant;
+  EXPECT_EQ("{\"name\": \"string_param\", \"type\": \"string\", \"value\": \"ROS2\"}", ss.str());
 
   rcl_interfaces::msg::Parameter string_param = string_variant.to_parameter_msg();
   EXPECT_EQ("string_param", string_param.name);
@@ -352,6 +423,11 @@ TEST_F(TestParameter, string_variant) {
   EXPECT_EQ(
     rcl_interfaces::msg::ParameterType::PARAMETER_STRING,
     from_msg.get_value_message().type);
+
+  // Check == and != operators work as expected
+  EXPECT_EQ(string_variant, string_variant);
+  EXPECT_NE(string_variant, rclcpp::Parameter("string_param", "ROS1"));
+  EXPECT_EQ(string_variant, from_msg);
 }
 
 TEST_F(TestParameter, byte_array_variant) {
@@ -381,6 +457,12 @@ TEST_F(TestParameter, byte_array_variant) {
   EXPECT_THROW(byte_array_variant.as_string_array(), rclcpp::ParameterTypeException);
 
   EXPECT_EQ("[0x52, 0x4f, 0x53, 0x32]", byte_array_variant.value_to_string());
+  std::stringstream ss;
+  ss << byte_array_variant;
+  EXPECT_EQ(
+    "{\"name\": \"byte_array_param\", \"type\": \"byte_array\", \"value\": "
+    "\"[0x52, 0x4f, 0x53, 0x32]\"}",
+    ss.str());
 
   rcl_interfaces::msg::Parameter byte_array_param = byte_array_variant.to_parameter_msg();
   EXPECT_EQ("byte_array_param", byte_array_param.name);
@@ -400,6 +482,12 @@ TEST_F(TestParameter, byte_array_variant) {
   EXPECT_EQ(
     rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY,
     from_msg.get_value_message().type);
+
+  // Check == and != operators work as expected
+  const std::vector<uint8_t> TEST_VALUE2 {0x1, 0x2, 0x3, 0x4};
+  EXPECT_EQ(byte_array_variant, byte_array_variant);
+  EXPECT_NE(byte_array_variant, rclcpp::Parameter("byte_array_param", TEST_VALUE2));
+  EXPECT_EQ(byte_array_variant, from_msg);
 }
 
 TEST_F(TestParameter, bool_array_variant) {
@@ -429,6 +517,12 @@ TEST_F(TestParameter, bool_array_variant) {
   EXPECT_THROW(bool_array_variant.as_string_array(), rclcpp::ParameterTypeException);
 
   EXPECT_EQ("[false, true, true, false, false, true]", bool_array_variant.value_to_string());
+  std::stringstream ss;
+  ss << bool_array_variant;
+  EXPECT_EQ(
+    "{\"name\": \"bool_array_param\", \"type\": \"bool_array\", \"value\": "
+    "\"[false, true, true, false, false, true]\"}",
+    ss.str());
 
   rcl_interfaces::msg::Parameter bool_array_param = bool_array_variant.to_parameter_msg();
   EXPECT_EQ("bool_array_param", bool_array_param.name);
@@ -448,6 +542,12 @@ TEST_F(TestParameter, bool_array_variant) {
   EXPECT_EQ(
     rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY,
     from_msg.get_value_message().type);
+
+  // Check == and != operators work as expected
+  const std::vector<bool> TEST_VALUE2 {true, true, true};
+  EXPECT_EQ(bool_array_variant, bool_array_variant);
+  EXPECT_NE(bool_array_variant, rclcpp::Parameter("bool_array_param", TEST_VALUE2));
+  EXPECT_EQ(bool_array_variant, from_msg);
 }
 
 TEST_F(TestParameter, integer_array_variant) {
@@ -495,6 +595,12 @@ TEST_F(TestParameter, integer_array_variant) {
   EXPECT_EQ(
     "[42, -99, 2147483647, -2147483648, 0]",
     integer_array_variant.value_to_string());
+  std::stringstream ss;
+  ss << integer_array_variant;
+  EXPECT_EQ(
+    "{\"name\": \"integer_array_param\", \"type\": \"integer_array\", \"value\": "
+    "\"[42, -99, 2147483647, -2147483648, 0]\"}",
+    ss.str());
 
   rcl_interfaces::msg::Parameter integer_array_param = integer_array_variant.to_parameter_msg();
   EXPECT_EQ("integer_array_param", integer_array_param.name);
@@ -527,6 +633,12 @@ TEST_F(TestParameter, integer_array_variant) {
   EXPECT_EQ(
     from_msg.get_value_message().type,
     rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY);
+
+  // Check == and != operators work as expected
+  const std::vector<int> TEST_VALUE2{1, 2, 3, 4, 5};
+  EXPECT_EQ(integer_array_variant, integer_array_variant);
+  EXPECT_NE(integer_array_variant, rclcpp::Parameter("integer_array_param", TEST_VALUE2));
+  EXPECT_EQ(integer_array_variant, from_msg);
 }
 
 TEST_F(TestParameter, long_integer_array_variant) {
@@ -560,6 +672,12 @@ TEST_F(TestParameter, long_integer_array_variant) {
   EXPECT_EQ(
     "[42, -99, 9223372036854775807, -9223372036854775808, 0]",
     long_array_variant.value_to_string());
+  std::stringstream ss;
+  ss << long_array_variant;
+  EXPECT_EQ(
+    "{\"name\": \"long_integer_array_param\", \"type\": \"integer_array\", \"value\": "
+    "\"[42, -99, 9223372036854775807, -9223372036854775808, 0]\"}",
+    ss.str());
 
   rcl_interfaces::msg::Parameter integer_array_param = long_array_variant.to_parameter_msg();
   EXPECT_EQ("long_integer_array_param", integer_array_param.name);
@@ -581,6 +699,12 @@ TEST_F(TestParameter, long_integer_array_variant) {
   EXPECT_EQ(
     rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY,
     from_msg.get_value_message().type);
+
+  // Check == and != operators work as expected
+  const std::vector<int64_t> TEST_VALUE2{1, 2, 3, 4, 5};
+  EXPECT_EQ(long_array_variant, long_array_variant);
+  EXPECT_NE(long_array_variant, rclcpp::Parameter("long_integer_array_param", TEST_VALUE2));
+  EXPECT_EQ(long_array_variant, from_msg);
 }
 
 TEST_F(TestParameter, float_array_variant) {
@@ -628,6 +752,12 @@ TEST_F(TestParameter, float_array_variant) {
   EXPECT_EQ(
     "[42.1, -99.1, 3.40282e+38, -3.40282e+38, 0.1]",
     float_array_variant.value_to_string());
+  std::stringstream ss;
+  ss << float_array_variant;
+  EXPECT_EQ(
+    "{\"name\": \"float_array_param\", \"type\": \"double_array\", \"value\": "
+    "\"[42.1, -99.1, 3.40282e+38, -3.40282e+38, 0.1]\"}",
+    ss.str());
 
   rcl_interfaces::msg::Parameter float_array_param = float_array_variant.to_parameter_msg();
   EXPECT_EQ("float_array_param", float_array_param.name);
@@ -660,6 +790,12 @@ TEST_F(TestParameter, float_array_variant) {
   EXPECT_EQ(
     rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY,
     from_msg.get_value_message().type);
+
+  // Check == and != operators work as expected
+  const std::vector<float> TEST_VALUE2{1.0, 2.0, 3.0, 4.0};
+  EXPECT_EQ(float_array_variant, float_array_variant);
+  EXPECT_NE(float_array_variant, rclcpp::Parameter("float_array_param", TEST_VALUE2));
+  EXPECT_EQ(float_array_variant, from_msg);
 }
 
 TEST_F(TestParameter, double_array_variant) {
@@ -693,6 +829,12 @@ TEST_F(TestParameter, double_array_variant) {
   EXPECT_EQ(
     "[42.1, -99.1, 1.79769e+308, -1.79769e+308, 0.1]",
     double_array_variant.value_to_string());
+  std::stringstream ss;
+  ss << double_array_variant;
+  EXPECT_EQ(
+    "{\"name\": \"double_array_param\", \"type\": \"double_array\", \"value\": "
+    "\"[42.1, -99.1, 1.79769e+308, -1.79769e+308, 0.1]\"}",
+    ss.str());
 
   rcl_interfaces::msg::Parameter double_array_param = double_array_variant.to_parameter_msg();
   EXPECT_EQ("double_array_param", double_array_param.name);
@@ -714,6 +856,12 @@ TEST_F(TestParameter, double_array_variant) {
   EXPECT_EQ(
     rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY,
     from_msg.get_value_message().type);
+
+  // Check == and != operators work as expected
+  const std::vector<double> TEST_VALUE2{1.0, 2.0, 3.0, 4.0};
+  EXPECT_EQ(double_array_variant, double_array_variant);
+  EXPECT_NE(double_array_variant, rclcpp::Parameter("double_array_param", TEST_VALUE2));
+  EXPECT_EQ(double_array_variant, from_msg);
 }
 
 TEST_F(TestParameter, string_array_variant) {
@@ -745,6 +893,11 @@ TEST_F(TestParameter, string_array_variant) {
   EXPECT_THROW(string_array_variant.as_double_array(), rclcpp::ParameterTypeException);
 
   EXPECT_EQ("[R, O, S2]", string_array_variant.value_to_string());
+  std::stringstream ss;
+  ss << string_array_variant;
+  EXPECT_EQ(
+    "{\"name\": \"string_array_param\", \"type\": \"string_array\", \"value\": \"[R, O, S2]\"}",
+    ss.str());
 
   rcl_interfaces::msg::Parameter string_array_param = string_array_variant.to_parameter_msg();
   EXPECT_EQ("string_array_param", string_array_param.name);
@@ -766,4 +919,31 @@ TEST_F(TestParameter, string_array_variant) {
   EXPECT_EQ(
     rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY,
     from_msg.get_value_message().type);
+
+  // Check == and != operators work as expected
+  const std::vector<std::string> TEST_VALUE2{"R", "O", "S1"};
+  EXPECT_EQ(string_array_variant, string_array_variant);
+  EXPECT_NE(string_array_variant, rclcpp::Parameter("string_array_param", TEST_VALUE2));
+  EXPECT_EQ(string_array_variant, from_msg);
+}
+
+TEST_F(TestParameter, parameter_vector_stringification) {
+  const std::vector<rclcpp::Parameter> parameters = {
+    rclcpp::Parameter(),
+    rclcpp::Parameter("bool_param", true),
+    rclcpp::Parameter("integer_param", 42),
+    rclcpp::Parameter("double_param", 3.14159),
+    rclcpp::Parameter("string_param", "I'm a string"),
+  };
+
+  std::stringstream ss;
+  ss << parameters;
+  EXPECT_EQ(
+    "{"
+    "\"\": {\"type\": \"not set\", \"value\": \"not set\"}, "
+    "\"bool_param\": {\"type\": \"bool\", \"value\": \"true\"}, "
+    "\"integer_param\": {\"type\": \"integer\", \"value\": \"42\"}, "
+    "\"double_param\": {\"type\": \"double\", \"value\": \"3.141590\"}, "
+    "\"string_param\": {\"type\": \"string\", \"value\": \"I'm a string\"}}",
+    ss.str());
 }

rclcpp/test/rclcpp/test_parameter_client.cpp

@@ -14,11 +14,18 @@
 
 #include <gtest/gtest.h>
 
-#include <string>
+#include <chrono>
+#include <functional>
+#include <future>
 #include <memory>
+#include <stdexcept>
+#include <string>
+#include <vector>
 
 #include "rclcpp/rclcpp.hpp"
 
+#include "../utils/rclcpp_gtest_macros.hpp"
+
 #include "rcl_interfaces/msg/parameter_event.hpp"
 
 class TestParameterClient : public ::testing::Test
@@ -159,3 +166,128 @@ TEST_F(TestParameterClient, sync_parameter_event_subscription) {
     (void)event_sub;
   }
 }
+
+/*
+   Coverage for simple get_parameter methods
+ */
+TEST_F(TestParameterClient, sync_parameter_get_parameter) {
+  rclcpp::SyncParametersClient client(node);
+  EXPECT_EQ(10, client.get_parameter("not_a_parameter", 10));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    client.get_parameter<int>("not_a_parameter"),
+    std::runtime_error("Parameter 'not_a_parameter' is not set"));
+}
+
+/*
+   Coverage for async waiting/is_ready
+ */
+TEST_F(TestParameterClient, sync_parameter_is_ready) {
+  rclcpp::SyncParametersClient client(node);
+  EXPECT_TRUE(client.wait_for_service());
+  EXPECT_TRUE(client.service_is_ready());
+}
+
+/*
+  Coverage for async get_parameter_types
+ */
+TEST_F(TestParameterClient, async_parameter_get_parameter_types) {
+  auto asynchronous_client = std::make_shared<rclcpp::AsyncParametersClient>(node);
+  bool callback_called = false;
+  auto callback = [&callback_called](std::shared_future<std::vector<rclcpp::ParameterType>> result)
+    {
+      // We expect the result to be empty since we tried to get a parameter that didn't exist.
+      if (result.valid() && result.get().size() == 0) {
+        callback_called = true;
+      }
+    };
+  std::vector<std::string> names{"foo"};
+  std::shared_future<std::vector<rclcpp::ParameterType>> future =
+    asynchronous_client->get_parameter_types(names, callback);
+  auto return_code = rclcpp::spin_until_future_complete(
+    node, future, std::chrono::milliseconds(100));
+  ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code);
+  ASSERT_TRUE(callback_called);
+}
+
+/*
+  Coverage for async get_parameters
+ */
+TEST_F(TestParameterClient, async_parameter_get_parameters) {
+  auto asynchronous_client = std::make_shared<rclcpp::AsyncParametersClient>(node);
+  bool callback_called = false;
+  auto callback = [&callback_called](std::shared_future<std::vector<rclcpp::Parameter>> result)
+    {
+      if (result.valid() && result.get().size() == 1 && result.get()[0].get_name() == "foo") {
+        callback_called = true;
+      }
+    };
+  std::vector<std::string> names{"foo"};
+  std::shared_future<std::vector<rclcpp::Parameter>> future = asynchronous_client->get_parameters(
+    names, callback);
+  auto return_code = rclcpp::spin_until_future_complete(
+    node, future, std::chrono::milliseconds(100));
+  ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code);
+  ASSERT_TRUE(callback_called);
+}
+
+/*
+  Coverage for async set_parameters_atomically
+ */
+TEST_F(TestParameterClient, async_parameter_set_parameters_atomically) {
+  auto asynchronous_client = std::make_shared<rclcpp::AsyncParametersClient>(node);
+  bool callback_called = false;
+  auto callback =
+    [&callback_called](std::shared_future<rcl_interfaces::msg::SetParametersResult> result)
+    {
+      // We expect this to fail since we didn't declare the parameter first.
+      if (result.valid() && !result.get().successful &&
+        result.get().reason == "One or more parameters were not declared before setting")
+      {
+        callback_called = true;
+      }
+    };
+  std::vector<rclcpp::Parameter> parameters;
+  parameters.emplace_back(rclcpp::Parameter("foo"));
+  std::shared_future<rcl_interfaces::msg::SetParametersResult> future =
+    asynchronous_client->set_parameters_atomically(parameters, callback);
+  auto return_code = rclcpp::spin_until_future_complete(
+    node, future, std::chrono::milliseconds(100));
+  ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code);
+  ASSERT_TRUE(callback_called);
+}
+
+/*
+  Coverage for async list_parameters
+ */
+TEST_F(TestParameterClient, async_parameter_list_parameters) {
+  auto asynchronous_client = std::make_shared<rclcpp::AsyncParametersClient>(node);
+  bool callback_called = false;
+  auto callback =
+    [&callback_called](std::shared_future<rcl_interfaces::msg::ListParametersResult> result)
+    {
+      if (result.valid() && result.get().names.size() == 0 && result.get().prefixes.size() == 0) {
+        callback_called = true;
+      }
+    };
+  std::vector<std::string> prefixes{"foo"};
+  std::shared_future<rcl_interfaces::msg::ListParametersResult> future =
+    asynchronous_client->list_parameters(prefixes, 0, callback);
+  auto return_code = rclcpp::spin_until_future_complete(
+    node, future, std::chrono::milliseconds(100));
+  ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code);
+  ASSERT_TRUE(callback_called);
+}
+
+/*
+  Coverage for sync get_parameter_types
+ */
+TEST_F(TestParameterClient, sync_parameter_get_parameter_types) {
+  node->declare_parameter("foo", 4);
+  auto synchronous_client = std::make_shared<rclcpp::SyncParametersClient>(node);
+  std::vector<std::string> names{"foo"};
+  std::vector<rclcpp::ParameterType> parameter_types =
+    synchronous_client->get_parameter_types(names);
+  ASSERT_EQ(1u, parameter_types.size());
+  ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_types[0]);
+}

rclcpp/test/rclcpp/test_parameter_service.cpp

@@ -0,0 +1,139 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "../../src/rclcpp/parameter_service_names.hpp"
+
+// This tests the ParameterService as it is included in an rclcpp::Node. Creating a separate
+// ParameterService would interfere with the built-in one
+class TestParameterService : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>("test_parameter_service", "/ns");
+    client = std::make_shared<rclcpp::SyncParametersClient>(node);
+    ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1)));
+  }
+
+  rclcpp::Node::SharedPtr node;
+  rclcpp::SyncParametersClient::SharedPtr client;
+};
+
+TEST_F(TestParameterService, get_parameters) {
+  node->declare_parameter("parameter1", rclcpp::ParameterValue(42));
+  EXPECT_EQ(42, client->get_parameter("parameter1", 0));
+
+  EXPECT_EQ(-42, client->get_parameter("undeclared_parameter", -42));
+}
+
+TEST_F(TestParameterService, get_parameter_types) {
+  node->declare_parameter("parameter1", rclcpp::ParameterValue(42));
+
+  const std::vector<std::string> declared_parameters = {"parameter1"};
+  const auto parameter_types = client->get_parameter_types(declared_parameters);
+  ASSERT_EQ(1u, parameter_types.size());
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_types[0]);
+
+  const std::vector<std::string> undeclared_parameters = {"parameter2"};
+  const auto undeclared_parameter_types = client->get_parameter_types(undeclared_parameters);
+  EXPECT_EQ(0u, undeclared_parameter_types.size());
+}
+
+TEST_F(TestParameterService, set_parameters) {
+  node->declare_parameter("parameter1", rclcpp::ParameterValue(42));
+  ASSERT_EQ(42, client->get_parameter("parameter1", 0));
+
+  const std::vector<rclcpp::Parameter> parameters = {
+    rclcpp::Parameter("parameter1", 0),
+  };
+  client->set_parameters(parameters);
+  EXPECT_EQ(0, client->get_parameter("parameter1", 100));
+}
+
+TEST_F(TestParameterService, set_parameters_atomically) {
+  node->declare_parameter("parameter1", rclcpp::ParameterValue(42));
+  ASSERT_EQ(42, client->get_parameter("parameter1", 0));
+
+  const std::vector<rclcpp::Parameter> parameters = {
+    rclcpp::Parameter("parameter1", 0),
+  };
+  client->set_parameters_atomically(parameters);
+  EXPECT_EQ(0, client->get_parameter("parameter1", 100));
+}
+
+TEST_F(TestParameterService, list_parameters) {
+  const size_t number_parameters_in_basic_node = client->list_parameters({}, 1).names.size();
+  node->declare_parameter("parameter1", rclcpp::ParameterValue(42));
+
+  const auto list_result = client->list_parameters({}, 1);
+  EXPECT_EQ(1u + number_parameters_in_basic_node, list_result.names.size());
+}
+
+TEST_F(TestParameterService, describe_parameters) {
+  // There is no current API in ParameterClient for calling the describe_parameters service
+  // Update this test when https://github.com/ros2/rclcpp/issues/1354 is resolved
+
+  const std::string describe_parameters_service_name =
+    node->get_fully_qualified_name() + std::string("/") +
+    rclcpp::parameter_service_names::describe_parameters;
+  using ServiceT = rcl_interfaces::srv::DescribeParameters;
+  auto client =
+    node->create_client<ServiceT>(describe_parameters_service_name);
+
+  ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1)));
+  node->declare_parameter("parameter1", rclcpp::ParameterValue(42));
+
+  {
+    auto request = std::make_shared<rcl_interfaces::srv::DescribeParameters::Request>();
+    request->names.push_back("parameter1");
+    auto future = client->async_send_request(request);
+
+    rclcpp::spin_until_future_complete(node, future, std::chrono::seconds(1));
+    auto response = future.get();
+    ASSERT_NE(nullptr, response);
+    EXPECT_EQ(1u, response->descriptors.size());
+
+    auto descriptor = response->descriptors[0];
+    EXPECT_EQ("parameter1", descriptor.name);
+    EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, descriptor.type);
+  }
+  {
+    auto request = std::make_shared<rcl_interfaces::srv::DescribeParameters::Request>();
+    request->names.push_back("undeclared_parameter");
+    auto future = client->async_send_request(request);
+
+    rclcpp::spin_until_future_complete(node, future, std::chrono::seconds(1));
+    auto response = future.get();
+    ASSERT_NE(nullptr, response);
+    EXPECT_EQ(0u, response->descriptors.size());
+  }
+}

rclcpp/test/rclcpp/test_publisher.cpp

@@ -16,13 +16,23 @@
 
 #include <string>
 #include <memory>
+#include <utility>
 #include <vector>
 
+#include "rcl/publisher.h"
+
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/rclcpp.hpp"
 
+#include "../mocking_utils/patch.hpp"
+#include "../utils/rclcpp_gtest_macros.hpp"
+
 #include "test_msgs/msg/empty.hpp"
 
+// Note: This is a long running test with rmw_connext_cpp, if you change this file, please check
+// that this test can complete fully, or adjust the timeout as necessary.
+// See https://github.com/ros2/rmw_connext/issues/325 for resolution
+
 class TestPublisher : public ::testing::Test
 {
 public:
@@ -259,3 +269,227 @@ TEST_F(TestPublisher, basic_getters) {
     EXPECT_EQ(publisher, publisher_rmw_gid);
   }
 }
+
+TEST_F(TestPublisher, serialized_message_publish) {
+  initialize();
+  rclcpp::PublisherOptionsWithAllocator<std::allocator<void>> options;
+  // This is the default, but it's also important for this test to succeed.
+  options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable;
+  auto publisher = node->create_publisher<test_msgs::msg::Empty>("topic", 10, options);
+
+  rclcpp::SerializedMessage serialized_msg;
+  // Mock successful rcl publish because the serialized_msg above is poorly formed
+  auto mock = mocking_utils::patch_and_return(
+    "self", rcl_publish_serialized_message, RCL_RET_OK);
+  EXPECT_NO_THROW(publisher->publish(serialized_msg));
+
+  EXPECT_NO_THROW(publisher->publish(serialized_msg.get_rcl_serialized_message()));
+}
+
+TEST_F(TestPublisher, rcl_publisher_init_error) {
+  initialize();
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_publisher_init, RCL_RET_ERROR);
+  EXPECT_THROW(
+    node->create_publisher<test_msgs::msg::Empty>("topic", 10).reset(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestPublisher, rcl_publisher_get_rmw_handle_error) {
+  initialize();
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_publisher_get_rmw_handle, nullptr);
+  RCLCPP_EXPECT_THROW_EQ(
+    node->create_publisher<test_msgs::msg::Empty>("topic", 10),
+    std::runtime_error("failed to get rmw handle: error not set"));
+}
+
+TEST_F(TestPublisher, rcl_publisher_get_gid_for_publisher_error) {
+  initialize();
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_get_gid_for_publisher, RMW_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    node->create_publisher<test_msgs::msg::Empty>("topic", 10),
+    std::runtime_error("failed to get publisher gid: error not set"));
+}
+
+TEST_F(TestPublisher, rcl_publisher_fini_error) {
+  initialize();
+  auto mock = mocking_utils::inject_on_return("lib:rclcpp", rcl_publisher_fini, RCL_RET_ERROR);
+  auto publisher = node->create_publisher<test_msgs::msg::Empty>("topic", 10);
+  ASSERT_EQ(1, publisher.use_count());
+  // Failure in rcl_publisher_fini should just log error
+  EXPECT_NO_THROW(publisher.reset());
+}
+
+TEST_F(TestPublisher, rcl_publisher_get_options_error) {
+  initialize();
+  auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_publisher_get_options, nullptr);
+  auto publisher = node->create_publisher<test_msgs::msg::Empty>("topic", 10);
+  RCLCPP_EXPECT_THROW_EQ(
+    publisher->get_queue_size(),
+    std::runtime_error("failed to get publisher options: error not set"));
+}
+
+TEST_F(TestPublisher, rcl_publisher_get_subscription_count_publisher_invalid) {
+  initialize();
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_publisher_get_subscription_count, RCL_RET_PUBLISHER_INVALID);
+  auto publisher = node->create_publisher<test_msgs::msg::Empty>("topic", 10);
+  EXPECT_THROW(
+    publisher->get_subscription_count(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestPublisher, rcl_publisher_get_actual_qos_error) {
+  initialize();
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_publisher_get_actual_qos, nullptr);
+  auto publisher = node->create_publisher<test_msgs::msg::Empty>("topic", 10);
+  RCLCPP_EXPECT_THROW_EQ(
+    publisher->get_actual_qos(),
+    std::runtime_error("failed to get qos settings: error not set"));
+}
+
+TEST_F(TestPublisher, publishers_equal_rmw_compare_gids_error) {
+  initialize();
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rmw_compare_gids_equal, RMW_RET_ERROR);
+  const auto publisher = node->create_publisher<test_msgs::msg::Empty>("topic", 10);
+  const rmw_gid_t * gid = nullptr;
+  auto throwing_fn = [publisher, gid]()
+    {
+      // The == operator is expected to throw here, but this lambda avoids unused result warning
+      return (*publisher.get() == gid) ? true : false;
+    };
+
+  RCLCPP_EXPECT_THROW_EQ(
+    throwing_fn(),
+    std::runtime_error("failed to compare gids: error not set"));
+}
+
+TEST_F(TestPublisher, intra_process_publish_failures) {
+  initialize();
+  rclcpp::PublisherOptionsWithAllocator<std::allocator<void>> options;
+  options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable;
+  auto publisher = node->create_publisher<test_msgs::msg::Empty>("topic", 10, options);
+
+  auto msg_unique = std::make_unique<test_msgs::msg::Empty>();
+  EXPECT_NO_THROW(publisher->publish(std::move(msg_unique)));
+
+  rclcpp::SerializedMessage serialized_msg;
+  RCLCPP_EXPECT_THROW_EQ(
+    publisher->publish(serialized_msg),
+    std::runtime_error("storing serialized messages in intra process is not supported yet"));
+
+  std::allocator<void> allocator;
+  {
+    rclcpp::LoanedMessage<test_msgs::msg::Empty> loaned_msg(*publisher, allocator);
+    RCLCPP_EXPECT_THROW_EQ(
+      publisher->publish(std::move(loaned_msg)),
+      std::runtime_error("storing loaned messages in intra process is not supported yet"));
+  }
+
+  {
+    rclcpp::LoanedMessage<test_msgs::msg::Empty> loaned_msg(*publisher, allocator);
+    loaned_msg.release();
+    RCLCPP_EXPECT_THROW_EQ(
+      publisher->publish(std::move(loaned_msg)),
+      std::runtime_error("loaned message is not valid"));
+  }
+  RCLCPP_EXPECT_THROW_EQ(
+    node->create_publisher<test_msgs::msg::Empty>(
+      "topic", rclcpp::QoS(0), options),
+    std::invalid_argument(
+      "intraprocess communication is not allowed with a zero qos history depth value"));
+}
+
+TEST_F(TestPublisher, inter_process_publish_failures) {
+  initialize();
+  rclcpp::PublisherOptionsWithAllocator<std::allocator<void>> options;
+  options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable;
+  auto publisher = node->create_publisher<test_msgs::msg::Empty>("topic", 10, options);
+
+  auto msg_unique = std::make_unique<test_msgs::msg::Empty>();
+  EXPECT_NO_THROW(publisher->publish(std::move(msg_unique)));
+
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_publish, RCL_RET_PUBLISHER_INVALID);
+    test_msgs::msg::Empty msg;
+    EXPECT_THROW(publisher->publish(msg), rclcpp::exceptions::RCLError);
+  }
+
+  {
+    // Using 'self' instead of 'lib:rclcpp' because `rcl_publish_serialized_message` is entirely
+    // defined in a header. Also, this one requires mocking because the serialized_msg is poorly
+    // formed and this just tests rclcpp functionality.
+    auto mock = mocking_utils::patch_and_return(
+      "self", rcl_publish_serialized_message, RCL_RET_OK);
+    rclcpp::SerializedMessage serialized_msg;
+    EXPECT_NO_THROW(publisher->publish(serialized_msg));
+  }
+
+  {
+    // Using 'self' instead of 'lib:rclcpp' because `rcl_publish_serialized_message` is entirely
+    // defined in a header
+    auto mock = mocking_utils::patch_and_return(
+      "self", rcl_publish_serialized_message, RCL_RET_ERROR);
+    rclcpp::SerializedMessage serialized_msg;
+    EXPECT_THROW(publisher->publish(serialized_msg), rclcpp::exceptions::RCLError);
+  }
+
+  std::allocator<void> allocator;
+  rclcpp::LoanedMessage<test_msgs::msg::Empty> loaned_msg(*publisher, allocator);
+  EXPECT_NO_THROW(publisher->publish(std::move(loaned_msg)));
+}
+
+template<typename MessageT, typename AllocatorT = std::allocator<void>>
+class TestPublisherProtectedMethods : public rclcpp::Publisher<MessageT, AllocatorT>
+{
+public:
+  using rclcpp::Publisher<MessageT, AllocatorT>::Publisher;
+
+  void publish_loaned_message(MessageT * msg)
+  {
+    this->do_loaned_message_publish(msg);
+  }
+
+  void call_default_incompatible_qos_callback(rclcpp::QOSOfferedIncompatibleQoSInfo & event) const
+  {
+    this->default_incompatible_qos_callback(event);
+  }
+};
+
+TEST_F(TestPublisher, do_loaned_message_publish_error) {
+  initialize();
+  using PublisherT = TestPublisherProtectedMethods<test_msgs::msg::Empty, std::allocator<void>>;
+  auto publisher =
+    node->create_publisher<test_msgs::msg::Empty, std::allocator<void>, PublisherT>("topic", 10);
+
+  auto msg = std::make_shared<test_msgs::msg::Empty>();
+  {
+    // Using 'self' instead of 'lib:rclcpp' because `rcl_publish_loaned_message` is entirely
+    // defined in a header
+    auto mock = mocking_utils::patch_and_return(
+      "self", rcl_publish_loaned_message, RCL_RET_PUBLISHER_INVALID);
+    EXPECT_THROW(publisher->publish_loaned_message(msg.get()), rclcpp::exceptions::RCLError);
+  }
+}
+
+TEST_F(TestPublisher, default_incompatible_qos_callback) {
+  initialize();
+  using PublisherT = TestPublisherProtectedMethods<test_msgs::msg::Empty, std::allocator<void>>;
+  auto publisher =
+    node->create_publisher<test_msgs::msg::Empty, std::allocator<void>, PublisherT>("topic", 10);
+  rclcpp::QOSOfferedIncompatibleQoSInfo event;
+  event.last_policy_kind = RMW_QOS_POLICY_INVALID;
+  // This message just logs an error message
+  EXPECT_NO_THROW(publisher->call_default_incompatible_qos_callback(event));
+}
+
+TEST_F(TestPublisher, run_event_handlers) {
+  initialize();
+  auto publisher = node->create_publisher<test_msgs::msg::Empty>("topic", 10);
+  for (const auto & handler : publisher->get_event_handlers()) {
+    EXPECT_NO_THROW(handler->execute());
+  }
+}

rclcpp/test/rclcpp/test_qos.cpp

@@ -16,6 +16,8 @@
 
 #include "rclcpp/qos.hpp"
 
+#include "rmw/types.h"
+
 TEST(TestQoS, equality_history) {
   rclcpp::QoS a(10);
   rclcpp::QoS b(10);
@@ -75,3 +77,125 @@ TEST(TestQoS, equality_namespace) {
   a.avoid_ros_namespace_conventions(true);
   EXPECT_NE(a, b);
 }
+
+TEST(TestQoS, setters) {
+  rclcpp::QoS qos(10);
+
+  qos.keep_all();
+  EXPECT_EQ(RMW_QOS_POLICY_HISTORY_KEEP_ALL, qos.get_rmw_qos_profile().history);
+
+  qos.keep_last(20);
+  EXPECT_EQ(RMW_QOS_POLICY_HISTORY_KEEP_LAST, qos.get_rmw_qos_profile().history);
+  EXPECT_EQ(20u, qos.get_rmw_qos_profile().depth);
+
+  qos.reliable();
+  EXPECT_EQ(RMW_QOS_POLICY_RELIABILITY_RELIABLE, qos.get_rmw_qos_profile().reliability);
+
+  qos.durability_volatile();
+  EXPECT_EQ(RMW_QOS_POLICY_DURABILITY_VOLATILE, qos.get_rmw_qos_profile().durability);
+
+  qos.transient_local();
+  EXPECT_EQ(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, qos.get_rmw_qos_profile().durability);
+
+  qos.history(RMW_QOS_POLICY_HISTORY_KEEP_ALL);
+  EXPECT_EQ(RMW_QOS_POLICY_HISTORY_KEEP_ALL, qos.get_rmw_qos_profile().history);
+
+  constexpr size_t duration_ns = 12345;
+  constexpr std::chrono::nanoseconds duration(duration_ns);
+  qos.deadline(duration);
+  EXPECT_EQ(0u, qos.get_rmw_qos_profile().deadline.sec);
+  EXPECT_EQ(duration_ns, qos.get_rmw_qos_profile().deadline.nsec);
+
+  const rmw_time_t rmw_time {0, 54321};
+  qos.deadline(rmw_time);
+  EXPECT_EQ(rmw_time.sec, qos.get_rmw_qos_profile().deadline.sec);
+  EXPECT_EQ(rmw_time.nsec, qos.get_rmw_qos_profile().deadline.nsec);
+
+  qos.lifespan(duration);
+  EXPECT_EQ(0u, qos.get_rmw_qos_profile().lifespan.sec);
+  EXPECT_EQ(duration_ns, qos.get_rmw_qos_profile().lifespan.nsec);
+
+  qos.lifespan(rmw_time);
+  EXPECT_EQ(rmw_time.sec, qos.get_rmw_qos_profile().lifespan.sec);
+  EXPECT_EQ(rmw_time.nsec, qos.get_rmw_qos_profile().lifespan.nsec);
+
+  qos.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC);
+  EXPECT_EQ(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC, qos.get_rmw_qos_profile().liveliness);
+
+  qos.liveliness_lease_duration(duration);
+  EXPECT_EQ(0u, qos.get_rmw_qos_profile().liveliness_lease_duration.sec);
+  EXPECT_EQ(duration_ns, qos.get_rmw_qos_profile().liveliness_lease_duration.nsec);
+
+  qos.liveliness_lease_duration(rmw_time);
+  EXPECT_EQ(rmw_time.sec, qos.get_rmw_qos_profile().liveliness_lease_duration.sec);
+  EXPECT_EQ(rmw_time.nsec, qos.get_rmw_qos_profile().liveliness_lease_duration.nsec);
+
+  qos.avoid_ros_namespace_conventions(true);
+  EXPECT_TRUE(qos.get_rmw_qos_profile().avoid_ros_namespace_conventions);
+  qos.avoid_ros_namespace_conventions(false);
+  EXPECT_FALSE(qos.get_rmw_qos_profile().avoid_ros_namespace_conventions);
+}
+
+bool operator==(const rmw_qos_profile_t & lhs, const rmw_qos_profile_t & rhs)
+{
+  if (lhs.history != rhs.history) {
+    return false;
+  }
+  switch (lhs.history) {
+    case RMW_QOS_POLICY_HISTORY_KEEP_ALL:
+      return true;
+    case RMW_QOS_POLICY_HISTORY_KEEP_LAST:
+    case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT:
+    case RMW_QOS_POLICY_HISTORY_UNKNOWN:
+      return lhs.depth == rhs.depth;
+  }
+  throw std::runtime_error("This line shouldn't be reached");
+}
+
+TEST(TestQoS, DerivedTypes) {
+  rclcpp::SensorDataQoS sensor_data_qos;
+  EXPECT_EQ(rmw_qos_profile_sensor_data, sensor_data_qos.get_rmw_qos_profile());
+
+  rclcpp::ParametersQoS parameter_qos;
+  EXPECT_EQ(rmw_qos_profile_parameters, parameter_qos.get_rmw_qos_profile());
+
+  rclcpp::ServicesQoS services_qos;
+  EXPECT_EQ(rmw_qos_profile_services_default, services_qos.get_rmw_qos_profile());
+
+  rclcpp::ParameterEventsQoS parameter_events_qos;
+  EXPECT_EQ(rmw_qos_profile_parameter_events, parameter_events_qos.get_rmw_qos_profile());
+
+  rclcpp::RosoutQoS rosout_qos;
+  EXPECT_EQ(rcl_qos_profile_rosout_default, rosout_qos.get_rmw_qos_profile());
+
+  rclcpp::SystemDefaultsQoS system_default_qos;
+  const rclcpp::KeepLast expected_initialization(RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT);
+  const rclcpp::QoS expected_default(expected_initialization);
+  EXPECT_EQ(expected_default.get_rmw_qos_profile(), system_default_qos.get_rmw_qos_profile());
+}
+
+TEST(TestQoS, policy_name_from_kind) {
+  EXPECT_EQ(
+    "DURABILITY_QOS_POLICY",
+    rclcpp::qos_policy_name_from_kind(RMW_QOS_POLICY_DURABILITY));
+
+  EXPECT_EQ(
+    "DEADLINE_QOS_POLICY",
+    rclcpp::qos_policy_name_from_kind(RMW_QOS_POLICY_DEADLINE));
+
+  EXPECT_EQ(
+    "LIVELINESS_QOS_POLICY",
+    rclcpp::qos_policy_name_from_kind(RMW_QOS_POLICY_LIVELINESS));
+
+  EXPECT_EQ(
+    "RELIABILITY_QOS_POLICY",
+    rclcpp::qos_policy_name_from_kind(RMW_QOS_POLICY_RELIABILITY));
+
+  EXPECT_EQ(
+    "HISTORY_QOS_POLICY",
+    rclcpp::qos_policy_name_from_kind(RMW_QOS_POLICY_HISTORY));
+
+  EXPECT_EQ(
+    "LIFESPAN_QOS_POLICY",
+    rclcpp::qos_policy_name_from_kind(RMW_QOS_POLICY_LIFESPAN));
+}

rclcpp/test/rclcpp/test_qos_event.cpp

@@ -25,6 +25,8 @@
 #include "rmw/rmw.h"
 #include "test_msgs/msg/empty.hpp"
 
+#include "../mocking_utils/patch.hpp"
+
 class TestQosEvent : public ::testing::Test
 {
 protected:
@@ -207,18 +209,117 @@ TEST_F(TestQosEvent, test_default_incompatible_qos_callbacks)
   rclcpp::executors::SingleThreadedExecutor ex;
   ex.add_node(node->get_node_base_interface());
 
-  ex.spin_until_future_complete(log_msgs_future, std::chrono::seconds(10));
+  // This future won't complete on fastrtps, so just timeout immediately
+  const auto timeout = (is_fastrtps) ? std::chrono::milliseconds(5) : std::chrono::seconds(10);
+  ex.spin_until_future_complete(log_msgs_future, timeout);
 
-  if (!is_fastrtps) {
+  if (is_fastrtps) {
+    EXPECT_EQ("", pub_log_msg);
+    EXPECT_EQ("", sub_log_msg);
+  } else {
     EXPECT_EQ(
-      "New subscription discovered on this topic, requesting incompatible QoS. "
+      "New subscription discovered on topic '/ns/test_topic', requesting incompatible QoS. "
       "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY",
       pub_log_msg);
     EXPECT_EQ(
-      "New publisher discovered on this topic, offering incompatible QoS. "
+      "New publisher discovered on topic '/ns/test_topic', offering incompatible QoS. "
       "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY",
       sub_log_msg);
   }
 
   rcutils_logging_set_output_handler(original_output_handler);
 }
+
+TEST_F(TestQosEvent, construct_destruct_rcl_error) {
+  auto publisher = node->create_publisher<test_msgs::msg::Empty>(topic_name, 10);
+  auto rcl_handle = publisher->get_publisher_handle();
+  ASSERT_NE(nullptr, rcl_handle);
+
+  // This callback requires some type of parameter, but it could be anything
+  auto callback = [](int) {};
+  const rcl_publisher_event_type_t event_type = RCL_PUBLISHER_OFFERED_DEADLINE_MISSED;
+
+  {
+    // Logs error and returns
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_publisher_event_init, RCL_RET_ERROR);
+
+    auto throwing_statement = [callback, rcl_handle, event_type]() {
+        // reset() is not needed for the exception, but it handles unused return value warning
+        std::make_shared<
+          rclcpp::QOSEventHandler<decltype(callback), std::shared_ptr<rcl_publisher_t>>>(
+          callback, rcl_publisher_event_init, rcl_handle, event_type).reset();
+      };
+    // This is done through a lambda because the compiler is having trouble parsing the templated
+    // function inside a macro.
+    EXPECT_THROW(throwing_statement(), rclcpp::exceptions::RCLError);
+  }
+
+  {
+    // Logs error and returns
+    auto mock = mocking_utils::inject_on_return(
+      "lib:rclcpp", rcl_event_fini, RCL_RET_ERROR);
+
+    auto throwing_statement = [callback, rcl_handle, event_type]() {
+        // reset() is needed for this exception
+        std::make_shared<
+          rclcpp::QOSEventHandler<decltype(callback), std::shared_ptr<rcl_publisher_t>>>(
+          callback, rcl_publisher_event_init, rcl_handle, event_type).reset();
+      };
+
+    // This is done through a lambda because the compiler is having trouble parsing the templated
+    // function inside a macro.
+    EXPECT_NO_THROW(throwing_statement());
+  }
+}
+
+TEST_F(TestQosEvent, execute) {
+  auto publisher = node->create_publisher<test_msgs::msg::Empty>(topic_name, 10);
+  auto rcl_handle = publisher->get_publisher_handle();
+
+  bool handler_callback_executed = false;
+  // This callback requires some type of parameter, but it could be anything
+  auto callback = [&handler_callback_executed](int) {handler_callback_executed = true;};
+  rcl_publisher_event_type_t event_type = RCL_PUBLISHER_OFFERED_DEADLINE_MISSED;
+
+  rclcpp::QOSEventHandler<decltype(callback), decltype(rcl_handle)> handler(
+    callback, rcl_publisher_event_init, rcl_handle, event_type);
+
+  EXPECT_NO_THROW(handler.execute());
+  EXPECT_TRUE(handler_callback_executed);
+
+  {
+    handler_callback_executed = false;
+    // Logs error and returns early
+    auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_take_event, RCL_RET_ERROR);
+    EXPECT_NO_THROW(handler.execute());
+    EXPECT_FALSE(handler_callback_executed);
+  }
+}
+
+TEST_F(TestQosEvent, add_to_wait_set) {
+  auto publisher = node->create_publisher<test_msgs::msg::Empty>(topic_name, 10);
+  auto rcl_handle = publisher->get_publisher_handle();
+
+  // This callback requires some type of parameter, but it could be anything
+  auto callback = [](int) {};
+
+  rcl_publisher_event_type_t event_type = RCL_PUBLISHER_OFFERED_DEADLINE_MISSED;
+  rclcpp::QOSEventHandler<decltype(callback), decltype(rcl_handle)> handler(
+    callback, rcl_publisher_event_init, rcl_handle, event_type);
+
+  EXPECT_EQ(1u, handler.get_number_of_ready_events());
+
+  {
+    rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+    auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_add_event, RCL_RET_OK);
+    EXPECT_TRUE(handler.add_to_wait_set(&wait_set));
+  }
+
+  {
+    rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_wait_set_add_event, RCL_RET_ERROR);
+    EXPECT_THROW(handler.add_to_wait_set(&wait_set), rclcpp::exceptions::RCLError);
+  }
+}

rclcpp/test/rclcpp/test_rate.cpp

@@ -29,6 +29,7 @@ TEST(TestRate, rate_basics) {
 
   auto start = std::chrono::system_clock::now();
   rclcpp::Rate r(period);
+  EXPECT_EQ(period, r.period());
   ASSERT_FALSE(r.is_steady());
   ASSERT_TRUE(r.sleep());
   auto one = std::chrono::system_clock::now();
@@ -68,6 +69,7 @@ TEST(TestRate, wall_rate_basics) {
 
   auto start = std::chrono::steady_clock::now();
   rclcpp::WallRate r(period);
+  EXPECT_EQ(period, r.period());
   ASSERT_TRUE(r.is_steady());
   ASSERT_TRUE(r.sleep());
   auto one = std::chrono::steady_clock::now();
@@ -98,3 +100,22 @@ TEST(TestRate, wall_rate_basics) {
   delta = five - four;
   EXPECT_GT(epsilon, delta);
 }
+
+TEST(TestRate, from_double) {
+  {
+    rclcpp::WallRate rate(1.0);
+    EXPECT_EQ(std::chrono::seconds(1), rate.period());
+  }
+  {
+    rclcpp::WallRate rate(2.0);
+    EXPECT_EQ(std::chrono::milliseconds(500), rate.period());
+  }
+  {
+    rclcpp::WallRate rate(0.5);
+    EXPECT_EQ(std::chrono::seconds(2), rate.period());
+  }
+  {
+    rclcpp::WallRate rate(4.0);
+    EXPECT_EQ(std::chrono::milliseconds(250), rate.period());
+  }
+}

rclcpp/test/rclcpp/test_rosout_qos.cpp

@@ -0,0 +1,79 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include <memory>
+
+#include "rclcpp/node_options.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp/duration.hpp"
+#include "rcl/logging_rosout.h"
+#include "rcl/init.h"
+#include "rmw/types.h"
+
+bool operator==(
+  const rmw_time_t & lhs,
+  const rmw_time_t & rhs)
+{
+  return lhs.sec == rhs.sec && lhs.nsec == rhs.nsec;
+}
+
+bool operator==(
+  const rmw_qos_profile_t & lhs,
+  const rmw_qos_profile_t & rhs)
+{
+  return lhs.history == rhs.history &&
+         lhs.depth == rhs.depth &&
+         lhs.reliability == rhs.reliability &&
+         lhs.durability == rhs.durability &&
+         lhs.deadline == rhs.deadline &&
+         lhs.lifespan == rhs.lifespan &&
+         lhs.liveliness == rhs.liveliness &&
+         lhs.liveliness_lease_duration == rhs.liveliness_lease_duration &&
+         lhs.avoid_ros_namespace_conventions == rhs.avoid_ros_namespace_conventions;
+}
+
+bool operator!=(
+  const rmw_qos_profile_t & lhs,
+  const rmw_qos_profile_t & rhs)
+{
+  return !(lhs == rhs);
+}
+
+/*
+   Test rosout_qos function with default value.
+ */
+TEST(TestRosoutQoS, test_rosout_qos_with_default_value) {
+  rclcpp::NodeOptions node_options;
+  rclcpp::QoS rosout_qos_profile = node_options.rosout_qos();
+  rmw_qos_profile_t rmw_qos_profile = rosout_qos_profile.get_rmw_qos_profile();
+  EXPECT_EQ(rcl_qos_profile_rosout_default, rmw_qos_profile);
+  EXPECT_EQ(rcl_qos_profile_rosout_default, node_options.get_rcl_node_options()->rosout_qos);
+}
+
+/*
+   Test `rosout_qos` function with custom value.
+ */
+TEST(TestRosoutQoS, test_rosout_qos_with_custom_value) {
+  rmw_time_t life_span;
+  life_span.sec = 10;
+  life_span.nsec = 0;
+  auto qos_profile = rclcpp::QoS(rclcpp::KeepLast(1000)).transient_local().lifespan(life_span);
+  auto options = rclcpp::NodeOptions().rosout_qos(qos_profile);
+  rclcpp::QoS rosout_qos = options.rosout_qos();
+  rmw_qos_profile_t rmw_qos_profile = rosout_qos.get_rmw_qos_profile();
+
+  EXPECT_EQ(rmw_qos_profile, qos_profile.get_rmw_qos_profile());
+  EXPECT_EQ(rmw_qos_profile, options.get_rcl_node_options()->rosout_qos);
+}

rclcpp/test/rclcpp/test_service.cpp

@@ -16,10 +16,13 @@
 
 #include <string>
 #include <memory>
+#include <utility>
 
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/rclcpp.hpp"
 
+#include "../mocking_utils/patch.hpp"
+
 #include "rcl_interfaces/srv/list_parameters.hpp"
 #include "test_msgs/srv/empty.hpp"
 #include "test_msgs/srv/empty.h"
@@ -32,6 +35,11 @@ protected:
     rclcpp::init(0, nullptr);
   }
 
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
   void SetUp()
   {
     node = std::make_shared<rclcpp::Node>("my_node", "/ns");
@@ -50,6 +58,12 @@ class TestServiceSub : public ::testing::Test
 protected:
   static void SetUpTestCase()
   {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
   }
 
   void SetUp()
@@ -77,6 +91,9 @@ TEST_F(TestService, construction_and_destruction) {
     };
   {
     auto service = node->create_service<ListParameters>("service", callback);
+    EXPECT_NE(nullptr, service->get_service_handle());
+    const rclcpp::ServiceBase * const_service_base = service.get();
+    EXPECT_NE(nullptr, const_service_base->get_service_handle());
   }
 
   {
@@ -108,6 +125,25 @@ TEST_F(TestServiceSub, construction_and_destruction) {
   }
 }
 
+TEST_F(TestService, construction_and_destruction_rcl_errors) {
+  auto callback =
+    [](const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {};
+
+  {
+    auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_service_init, RCL_RET_ERROR);
+    // reset() isn't necessary for this exception, it just avoids unused return value warning
+    EXPECT_THROW(
+      node->create_service<test_msgs::srv::Empty>("service", callback).reset(),
+      rclcpp::exceptions::RCLError);
+  }
+  {
+    // reset() is required for this one
+    auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_service_fini, RCL_RET_ERROR);
+    EXPECT_NO_THROW(node->create_service<test_msgs::srv::Empty>("service", callback).reset());
+  }
+}
+
 /* Testing basic getters */
 TEST_F(TestService, basic_public_getters) {
   using rcl_interfaces::srv::ListParameters;
@@ -148,3 +184,54 @@ TEST_F(TestService, basic_public_getters) {
         node_handle_int->get_node_base_interface()->get_rcl_node_handle()));
   }
 }
+
+TEST_F(TestService, take_request) {
+  auto callback =
+    [](const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {};
+  auto server = node->create_service<test_msgs::srv::Empty>("service", callback);
+  {
+    auto request_id = server->create_request_header();
+    test_msgs::srv::Empty::Request request;
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_take_request, RCL_RET_OK);
+    EXPECT_TRUE(server->take_request(request, *request_id.get()));
+  }
+  {
+    auto request_id = server->create_request_header();
+    test_msgs::srv::Empty::Request request;
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_take_request, RCL_RET_SERVICE_TAKE_FAILED);
+    EXPECT_FALSE(server->take_request(request, *request_id.get()));
+  }
+  {
+    auto request_id = server->create_request_header();
+    test_msgs::srv::Empty::Request request;
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_take_request, RCL_RET_ERROR);
+    EXPECT_THROW(server->take_request(request, *request_id.get()), rclcpp::exceptions::RCLError);
+  }
+}
+
+TEST_F(TestService, send_response) {
+  auto callback =
+    [](const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {};
+  auto server = node->create_service<test_msgs::srv::Empty>("service", callback);
+
+  {
+    auto request_id = server->create_request_header();
+    test_msgs::srv::Empty::Response response;
+    auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_send_response, RCL_RET_OK);
+    EXPECT_NO_THROW(server->send_response(*request_id.get(), response));
+  }
+
+  {
+    auto request_id = server->create_request_header();
+    test_msgs::srv::Empty::Response response;
+    auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_send_response, RCL_RET_ERROR);
+    EXPECT_THROW(
+      server->send_response(*request_id.get(), response),
+      rclcpp::exceptions::RCLError);
+  }
+}

rclcpp/test/rclcpp/test_subscription.cpp

@@ -15,6 +15,7 @@
 #include <gtest/gtest.h>
 
 #include <chrono>
+#include <functional>
 #include <memory>
 #include <string>
 #include <thread>
@@ -23,8 +24,15 @@
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/rclcpp.hpp"
 
+#include "../mocking_utils/patch.hpp"
+#include "../utils/rclcpp_gtest_macros.hpp"
+
 #include "test_msgs/msg/empty.hpp"
 
+// Note: This is a long running test with rmw_connext_cpp, if you change this file, please check
+// that this test can complete fully, or adjust the timeout as necessary.
+// See https://github.com/ros2/rmw_connext/issues/325 for resolution
+
 using namespace std::chrono_literals;
 
 class TestSubscription : public ::testing::Test
@@ -150,7 +158,19 @@ TEST_F(TestSubscription, construction_and_destruction) {
       (void)msg;
     };
   {
-    auto sub = node->create_subscription<Empty>("topic", 10, callback);
+    constexpr size_t depth = 10u;
+    auto sub = node->create_subscription<Empty>("topic", depth, callback);
+
+    EXPECT_NE(nullptr, sub->get_subscription_handle());
+    // Converting to base class was necessary for the compiler to choose the const version of
+    // get_subscription_handle()
+    const rclcpp::SubscriptionBase * const_sub = sub.get();
+    EXPECT_NE(nullptr, const_sub->get_subscription_handle());
+    EXPECT_FALSE(sub->use_take_shared_method());
+
+    EXPECT_NE(nullptr, sub->get_message_type_support_handle().typesupport_identifier);
+    EXPECT_NE(nullptr, sub->get_message_type_support_handle().data);
+    EXPECT_EQ(depth, sub->get_actual_qos().get_rmw_qos_profile().depth);
   }
 
   {
@@ -229,6 +249,21 @@ TEST_F(TestSubscription, various_creation_signatures) {
       node, "topic", 42, cb, rclcpp::SubscriptionOptions());
     (void)sub;
   }
+  {
+    rclcpp::SubscriptionOptionsWithAllocator<std::allocator<void>> options;
+    options.allocator = std::make_shared<std::allocator<void>>();
+    EXPECT_NE(nullptr, options.get_allocator());
+    auto sub = rclcpp::create_subscription<Empty>(
+      node, "topic", 42, cb, options);
+    (void)sub;
+  }
+  {
+    rclcpp::SubscriptionOptionsBase options_base;
+    rclcpp::SubscriptionOptionsWithAllocator<std::allocator<void>> options(options_base);
+    auto sub = rclcpp::create_subscription<Empty>(
+      node, "topic", 42, cb, options);
+    (void)sub;
+  }
 }
 
 /*
@@ -329,6 +364,86 @@ TEST_F(TestSubscription, take_serialized) {
   }
 }
 
+TEST_F(TestSubscription, rcl_subscription_init_error) {
+  initialize();
+  auto callback = [](std::shared_ptr<const test_msgs::msg::Empty>) {};
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_subscription_init, RCL_RET_TOPIC_NAME_INVALID);
+
+  // reset() is not needed for triggering exception, just to avoid an unused return value warning
+  EXPECT_THROW(
+    node->create_subscription<test_msgs::msg::Empty>("topic", 10, callback).reset(),
+    rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestSubscription, rcl_subscription_fini_error) {
+  initialize();
+  auto callback = [](std::shared_ptr<const test_msgs::msg::Empty>) {};
+  auto mock = mocking_utils::inject_on_return(
+    "lib:rclcpp", rcl_subscription_fini, RCL_RET_ERROR);
+
+  // Cleanup just fails, no exception expected
+  EXPECT_NO_THROW(
+    node->create_subscription<test_msgs::msg::Empty>("topic", 10, callback).reset());
+}
+
+TEST_F(TestSubscription, rcl_subscription_get_actual_qos_error) {
+  initialize();
+  auto callback = [](std::shared_ptr<const test_msgs::msg::Empty>) {};
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_subscription_get_actual_qos, nullptr);
+
+  auto sub = node->create_subscription<test_msgs::msg::Empty>("topic", 10, callback);
+  RCLCPP_EXPECT_THROW_EQ(
+    sub->get_actual_qos(), std::runtime_error("failed to get qos settings: error not set"));
+}
+
+TEST_F(TestSubscription, rcl_take_type_erased_error) {
+  initialize();
+  auto callback = [](std::shared_ptr<const test_msgs::msg::Empty>) {};
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_take, RCL_RET_ERROR);
+
+  auto sub = node->create_subscription<test_msgs::msg::Empty>("topic", 10, callback);
+  test_msgs::msg::Empty msg;
+  rclcpp::MessageInfo message_info;
+
+  EXPECT_THROW(sub->take_type_erased(&msg, message_info), rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestSubscription, rcl_take_serialized_message_error) {
+  initialize();
+  auto callback = [](std::shared_ptr<const test_msgs::msg::Empty>) {};
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_take_serialized_message, RCL_RET_ERROR);
+
+  auto sub = node->create_subscription<test_msgs::msg::Empty>("topic", 10, callback);
+  rclcpp::SerializedMessage msg;
+  rclcpp::MessageInfo message_info;
+
+  EXPECT_THROW(sub->take_serialized(msg, message_info), rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestSubscription, rcl_subscription_get_publisher_count_error) {
+  initialize();
+  auto callback = [](std::shared_ptr<const test_msgs::msg::Empty>) {};
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp", rcl_subscription_get_publisher_count, RCL_RET_ERROR);
+
+  auto sub = node->create_subscription<test_msgs::msg::Empty>("topic", 10, callback);
+  EXPECT_THROW(sub->get_publisher_count(), rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestSubscription, handle_loaned_message) {
+  initialize();
+  auto callback = [](std::shared_ptr<const test_msgs::msg::Empty>) {};
+  auto sub = node->create_subscription<test_msgs::msg::Empty>("topic", 10, callback);
+
+  test_msgs::msg::Empty msg;
+  rclcpp::MessageInfo message_info;
+  EXPECT_NO_THROW(sub->handle_loaned_message(&msg, message_info));
+}
+
 /*
    Testing subscription with intraprocess enabled and invalid QoS
  */
@@ -351,6 +466,29 @@ TEST_P(TestSubscriptionInvalidIntraprocessQos, test_subscription_throws) {
   }
 }
 
+/*
+   Testing subscription with invalid use_intra_process_comm
+ */
+TEST_P(TestSubscriptionInvalidIntraprocessQos, test_subscription_throws_intraprocess) {
+  rclcpp::SubscriptionOptionsWithAllocator<std::allocator<void>> options;
+  options.use_intra_process_comm = static_cast<rclcpp::IntraProcessSetting>(5);
+
+  initialize();
+  rclcpp::QoS qos = GetParam().qos;
+  auto callback = std::bind(
+    &TestSubscriptionInvalidIntraprocessQos::OnMessage,
+    this,
+    std::placeholders::_1);
+
+  RCLCPP_EXPECT_THROW_EQ(
+    {auto subscription = node->create_subscription<test_msgs::msg::Empty>(
+        "topic",
+        qos,
+        callback,
+        options);},
+    std::runtime_error("Unrecognized IntraProcessSetting value"));
+}
+
 static std::vector<TestParameters> invalid_qos_profiles()
 {
   std::vector<TestParameters> parameters;

rclcpp/test/rclcpp/test_subscription_options.cpp

@@ -23,6 +23,8 @@
 #include "rclcpp/node_options.hpp"
 #include "rclcpp/subscription_options.hpp"
 
+#include "../utils/rclcpp_gtest_macros.hpp"
+
 using namespace std::chrono_literals;
 
 namespace
@@ -85,4 +87,17 @@ TEST_F(TestSubscriptionOptions, topic_statistics_options_node_default_mode) {
     rclcpp::detail::resolve_enable_topic_statistics(
       subscription_options,
       *(node->get_node_base_interface())));
+
+  subscription_options.topic_stats_options.state = rclcpp::TopicStatisticsState::Disable;
+  EXPECT_FALSE(
+    rclcpp::detail::resolve_enable_topic_statistics(
+      subscription_options,
+      *(node->get_node_base_interface())));
+
+  subscription_options.topic_stats_options.state = static_cast<rclcpp::TopicStatisticsState>(5);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::detail::resolve_enable_topic_statistics(
+      subscription_options,
+      *(node->get_node_base_interface())),
+    std::runtime_error("Unrecognized EnableTopicStatistics value"));
 }

rclcpp/test/rclcpp/test_time.cpp

@@ -25,6 +25,8 @@
 #include "rclcpp/time.hpp"
 #include "rclcpp/utilities.hpp"
 
+#include "../utils/rclcpp_gtest_macros.hpp"
+
 namespace
 {
 
@@ -352,3 +354,93 @@ TEST_F(TestTime, seconds) {
   EXPECT_DOUBLE_EQ(4.5, rclcpp::Time(4, 500000000).seconds());
   EXPECT_DOUBLE_EQ(2.5, rclcpp::Time(0, 2500000000).seconds());
 }
+
+TEST_F(TestTime, test_max) {
+  const rclcpp::Time time_max = rclcpp::Time::max();
+  const rclcpp::Time max_time(std::numeric_limits<int32_t>::max(), 999999999);
+  EXPECT_DOUBLE_EQ(max_time.seconds(), time_max.seconds());
+  EXPECT_EQ(max_time.nanoseconds(), time_max.nanoseconds());
+}
+
+TEST_F(TestTime, test_constructor_from_rcl_time_point) {
+  const rcl_time_point_value_t test_nano_seconds = 555;
+  const rcl_clock_type_t test_clock_type = RCL_ROS_TIME;
+  rcl_time_point_t test_time_point;
+  test_time_point.nanoseconds = test_nano_seconds;
+  test_time_point.clock_type = test_clock_type;
+
+  const rclcpp::Time time_max = rclcpp::Time(test_time_point);
+
+  EXPECT_EQ(test_nano_seconds, time_max.nanoseconds());
+  EXPECT_EQ(test_nano_seconds, test_time_point.nanoseconds);
+  EXPECT_EQ(test_clock_type, time_max.get_clock_type());
+  EXPECT_EQ(test_clock_type, test_time_point.clock_type);
+}
+
+TEST_F(TestTime, test_assignment_operator_from_builtin_msg_time) {
+  rclcpp::Clock ros_clock(RCL_ROS_TIME);
+  const builtin_interfaces::msg::Time ros_now = ros_clock.now();
+  EXPECT_NE(0, ros_now.sec);
+  EXPECT_NE(0u, ros_now.nanosec);
+
+  rclcpp::Time test_time(0u, RCL_CLOCK_UNINITIALIZED);
+  EXPECT_EQ(0u, test_time.nanoseconds());
+  EXPECT_EQ(RCL_CLOCK_UNINITIALIZED, test_time.get_clock_type());
+
+  test_time = ros_now;
+  EXPECT_NE(0, test_time.nanoseconds());
+  // The clock type is hardcoded internally
+  EXPECT_EQ(RCL_ROS_TIME, test_time.get_clock_type());
+}
+
+TEST_F(TestTime, test_sum_operator) {
+  const rclcpp::Duration one(1);
+  const rclcpp::Time test_time(0u);
+  EXPECT_EQ(0u, test_time.nanoseconds());
+
+  const rclcpp::Time new_time = one + test_time;
+  EXPECT_EQ(1, new_time.nanoseconds());
+}
+
+TEST_F(TestTime, test_overflow_underflow_throws) {
+  rclcpp::Time test_time(0u);
+
+  RCLCPP_EXPECT_THROW_EQ(
+    test_time = rclcpp::Time(INT64_MAX) + rclcpp::Duration(1),
+    std::overflow_error("addition leads to int64_t overflow"));
+  RCLCPP_EXPECT_THROW_EQ(
+    test_time = rclcpp::Time(INT64_MIN) + rclcpp::Duration(-1),
+    std::underflow_error("addition leads to int64_t underflow"));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    test_time = rclcpp::Time(INT64_MAX) - rclcpp::Duration(-1),
+    std::overflow_error("time subtraction leads to int64_t overflow"));
+  RCLCPP_EXPECT_THROW_EQ(
+    test_time = rclcpp::Time(INT64_MIN) - rclcpp::Duration(1),
+    std::underflow_error("time subtraction leads to int64_t underflow"));
+
+  test_time = rclcpp::Time(INT64_MAX);
+  RCLCPP_EXPECT_THROW_EQ(
+    test_time += rclcpp::Duration(1),
+    std::overflow_error("addition leads to int64_t overflow"));
+  test_time = rclcpp::Time(INT64_MIN);
+  RCLCPP_EXPECT_THROW_EQ(
+    test_time += rclcpp::Duration(-1),
+    std::underflow_error("addition leads to int64_t underflow"));
+
+  test_time = rclcpp::Time(INT64_MAX);
+  RCLCPP_EXPECT_THROW_EQ(
+    test_time -= rclcpp::Duration(-1),
+    std::overflow_error("time subtraction leads to int64_t overflow"));
+  test_time = rclcpp::Time(INT64_MIN);
+  RCLCPP_EXPECT_THROW_EQ(
+    test_time -= rclcpp::Duration(1),
+    std::underflow_error("time subtraction leads to int64_t underflow"));
+
+  RCLCPP_EXPECT_THROW_EQ(
+    test_time = rclcpp::Duration(INT64_MAX) + rclcpp::Time(1),
+    std::overflow_error("addition leads to int64_t overflow"));
+  RCLCPP_EXPECT_THROW_EQ(
+    test_time = rclcpp::Duration(INT64_MIN) + rclcpp::Time(-1),
+    std::underflow_error("addition leads to int64_t underflow"));
+}

rclcpp/test/rclcpp/test_time_source.cpp

@@ -209,6 +209,9 @@ TEST_F(TestTimeSource, ROS_time_valid_attach_detach) {
 
   ts.attachNode(node);
   EXPECT_FALSE(ros_clock->ros_time_is_active());
+
+  ts.detachClock(ros_clock);
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
 }
 
 TEST_F(TestTimeSource, ROS_time_valid_wall_time) {

rclcpp/test/rclcpp/test_timer.cpp

@@ -25,6 +25,9 @@
 #include "rclcpp/executors/single_threaded_executor.hpp"
 #include "rclcpp/rclcpp.hpp"
 
+#include "../mocking_utils/patch.hpp"
+#include "../utils/rclcpp_gtest_macros.hpp"
+
 using namespace std::chrono_literals;
 
 /// Timer testing bring up and teardown
@@ -54,6 +57,7 @@ protected:
         this->executor->cancel();
       }
     );
+    EXPECT_TRUE(timer->is_steady());
 
     executor->add_node(test_node);
     // don't start spinning, let the test dictate when
@@ -191,3 +195,89 @@ TEST_F(TestTimer, test_bad_arguments) {
     rclcpp::GenericTimer<void (*)()>(unitialized_clock, 1us, []() {}, context),
     rclcpp::exceptions::RCLError);
 }
+
+TEST_F(TestTimer, callback_with_timer) {
+  rclcpp::TimerBase * timer_ptr = nullptr;
+  timer = test_node->create_wall_timer(
+    std::chrono::milliseconds(1),
+    [&timer_ptr](rclcpp::TimerBase & timer) {
+      timer_ptr = &timer;
+    });
+  auto start = std::chrono::steady_clock::now();
+  while (nullptr == timer_ptr &&
+    (std::chrono::steady_clock::now() - start) < std::chrono::milliseconds(100))
+  {
+    executor->spin_once(std::chrono::milliseconds(10));
+  }
+  EXPECT_EQ(timer.get(), timer_ptr);
+  EXPECT_LE(std::chrono::nanoseconds(0).count(), timer_ptr->time_until_trigger().count());
+  EXPECT_FALSE(timer_ptr->is_ready());
+}
+
+TEST_F(TestTimer, callback_with_period_zero) {
+  rclcpp::TimerBase * timer_ptr = nullptr;
+  timer = test_node->create_wall_timer(
+    std::chrono::milliseconds(0),
+    [&timer_ptr](rclcpp::TimerBase & timer) {
+      timer_ptr = &timer;
+    });
+  auto start = std::chrono::steady_clock::now();
+  while (nullptr == timer_ptr &&
+    (std::chrono::steady_clock::now() - start) < std::chrono::milliseconds(100))
+  {
+    executor->spin_once(std::chrono::milliseconds(10));
+  }
+  ASSERT_EQ(timer.get(), timer_ptr);
+  EXPECT_GE(std::chrono::nanoseconds(0).count(), timer_ptr->time_until_trigger().count());
+  EXPECT_TRUE(timer_ptr->is_ready());
+}
+
+/// Test internal failures using mocks
+TEST_F(TestTimer, test_failures_with_exceptions)
+{
+  // expect clean state, don't run otherwise
+  test_initial_conditions(timer, has_timer_run);
+  {
+    std::shared_ptr<rclcpp::TimerBase> timer_to_test_destructor;
+    // Test destructor failure, just logs a msg
+    auto mock = mocking_utils::inject_on_return("lib:rclcpp", rcl_timer_fini, RCL_RET_ERROR);
+    EXPECT_NO_THROW(
+    {
+      timer_to_test_destructor =
+      test_node->create_wall_timer(std::chrono::milliseconds(0), [](void) {});
+      timer_to_test_destructor.reset();
+    });
+  }
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_timer_cancel, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      timer->cancel(), std::runtime_error("Couldn't cancel timer: error not set"));
+  }
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_timer_is_canceled, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      timer->is_canceled(),
+      std::runtime_error("Couldn't get timer cancelled state: error not set"));
+  }
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_timer_reset, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      timer->reset(), std::runtime_error("Couldn't reset timer: error not set"));
+  }
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_timer_is_ready, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      timer->is_ready(), std::runtime_error("Failed to check timer: error not set"));
+  }
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_timer_get_time_until_next_call, RCL_RET_ERROR);
+    RCLCPP_EXPECT_THROW_EQ(
+      timer->time_until_trigger(),
+      std::runtime_error("Timer could not get time until next call: error not set"));
+  }
+}

rclcpp/test/rclcpp/test_utilities.cpp

@@ -14,12 +14,20 @@
 
 #include <gtest/gtest.h>
 
-#include <string>
 #include <memory>
+#include <string>
+#include <vector>
+
+#include "rcl/init.h"
+#include "rcl/logging.h"
 
 #include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/utilities.hpp"
+#include "rclcpp/scope_exit.hpp"
+
+#include "../mocking_utils/patch.hpp"
+#include "../utils/rclcpp_gtest_macros.hpp"
 
 TEST(TestUtilities, remove_ros_arguments) {
   const char * const argv[] = {
@@ -40,12 +48,14 @@ TEST(TestUtilities, remove_ros_arguments) {
 }
 
 TEST(TestUtilities, init_with_args) {
+  EXPECT_FALSE(rclcpp::signal_handlers_installed());
   const char * const argv[] = {"process_name"};
   int argc = sizeof(argv) / sizeof(const char *);
   auto other_args = rclcpp::init_and_remove_ros_arguments(argc, argv);
 
   ASSERT_EQ(1u, other_args.size());
   ASSERT_EQ(std::string{"process_name"}, other_args[0]);
+  EXPECT_TRUE(rclcpp::signal_handlers_installed());
 
   EXPECT_TRUE(rclcpp::ok());
   rclcpp::shutdown();
@@ -78,3 +88,207 @@ TEST(TestUtilities, multi_init) {
   EXPECT_FALSE(rclcpp::ok(context1));
   EXPECT_FALSE(rclcpp::ok(context2));
 }
+
+TEST(TestUtilities, test_context_basic_access) {
+  auto context1 = std::make_shared<rclcpp::contexts::DefaultContext>();
+  EXPECT_NE(nullptr, context1->get_init_options().get_rcl_init_options());
+  EXPECT_EQ(0u, context1->get_on_shutdown_callbacks().size());
+  EXPECT_EQ(std::string{""}, context1->shutdown_reason());
+}
+
+TEST(TestUtilities, test_context_basic_access_const_methods) {
+  auto context1 = std::make_shared<const rclcpp::contexts::DefaultContext>();
+
+  EXPECT_NE(nullptr, context1->get_init_options().get_rcl_init_options());
+  EXPECT_EQ(0u, context1->get_on_shutdown_callbacks().size());
+}
+
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, ==)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, !=)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, >)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, <)
+
+TEST(TestUtilities, test_context_release_interrupt_guard_condition) {
+  auto context1 = std::make_shared<rclcpp::contexts::DefaultContext>();
+  context1->init(0, nullptr);
+  RCLCPP_SCOPE_EXIT(rclcpp::shutdown(context1););
+
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcl_ret_t ret = rcl_wait_set_init(
+    &wait_set, 0, 2, 0, 0, 0, 0, context1->get_rcl_context().get(),
+    rcl_get_default_allocator());
+  ASSERT_EQ(RCL_RET_OK, ret);
+
+  // Expected usage
+  rcl_guard_condition_t * interrupt_guard_condition =
+    context1->get_interrupt_guard_condition(&wait_set);
+  EXPECT_NE(nullptr, interrupt_guard_condition);
+  EXPECT_NO_THROW(context1->release_interrupt_guard_condition(&wait_set));
+
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_guard_condition_init, RCL_RET_ERROR);
+    EXPECT_THROW(
+      {interrupt_guard_condition = context1->get_interrupt_guard_condition(&wait_set);},
+      rclcpp::exceptions::RCLError);
+  }
+
+  {
+    interrupt_guard_condition = context1->get_interrupt_guard_condition(&wait_set);
+    auto mock = mocking_utils::inject_on_return(
+      "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR);
+    EXPECT_THROW(
+      {context1->release_interrupt_guard_condition(&wait_set);},
+      rclcpp::exceptions::RCLError);
+  }
+
+  {
+    interrupt_guard_condition = context1->get_interrupt_guard_condition(&wait_set);
+    auto mock = mocking_utils::inject_on_return(
+      "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR);
+    EXPECT_NO_THROW({context1->release_interrupt_guard_condition(&wait_set, std::nothrow);});
+  }
+
+  {
+    EXPECT_THROW(
+      context1->release_interrupt_guard_condition(nullptr),
+      std::runtime_error);
+  }
+
+  // Test it works after restore mocks
+  interrupt_guard_condition = context1->get_interrupt_guard_condition(&wait_set);
+  EXPECT_NE(nullptr, interrupt_guard_condition);
+  EXPECT_NO_THROW(context1->release_interrupt_guard_condition(&wait_set));
+}
+
+TEST(TestUtilities, test_context_init_shutdown_fails) {
+  {
+    auto context = std::make_shared<rclcpp::contexts::DefaultContext>();
+    auto context_fail_init = std::make_shared<rclcpp::contexts::DefaultContext>();
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_init, RCL_RET_ERROR);
+    EXPECT_THROW(context_fail_init->init(0, nullptr), rclcpp::exceptions::RCLError);
+  }
+
+  {
+    auto context_fail_init = std::make_shared<rclcpp::contexts::DefaultContext>();
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_logging_configure_with_output_handler, RCL_RET_ERROR);
+    EXPECT_THROW(context_fail_init->init(0, nullptr), rclcpp::exceptions::RCLError);
+  }
+
+  {
+    auto context = std::make_shared<rclcpp::contexts::DefaultContext>();
+    context->init(0, nullptr);
+    auto mock = mocking_utils::inject_on_return(
+      "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+    // This will log a message, no throw expected
+    EXPECT_NO_THROW(context->shutdown(""));
+  }
+
+  {
+    auto context = std::make_shared<rclcpp::contexts::DefaultContext>();
+    context->init(0, nullptr);
+    auto mock = mocking_utils::inject_on_return(
+      "lib:rclcpp", rcl_shutdown, RCL_RET_ERROR);
+    EXPECT_THROW(context->shutdown(""), rclcpp::exceptions::RCLError);
+  }
+
+  {
+    auto context = std::make_shared<rclcpp::contexts::DefaultContext>();
+    context->init(0, nullptr);
+    auto mock = mocking_utils::inject_on_return(
+      "lib:rclcpp", rcl_logging_fini, RCL_RET_ERROR);
+    // This will log a message, no throw expected
+    EXPECT_NO_THROW(context->shutdown(""));
+  }
+
+  {
+    auto context_to_destroy = std::make_shared<rclcpp::contexts::DefaultContext>();
+    auto mock = mocking_utils::inject_on_return(
+      "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR);
+    // This will log a message, no throw expected
+    EXPECT_NO_THROW({context_to_destroy.reset();});
+  }
+}
+
+// Required for mocking_utils below
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >)
+
+TEST(TestUtilities, remove_ros_arguments_rcl_parse_arguments_failed) {
+  const char * const argv[] = {
+    "process_name",
+    "-d", "--ros-args",
+    "-r", "__ns:=/foo/bar",
+    "-r", "__ns:=/fiz/buz",
+    "--", "--foo=bar", "--baz"
+  };
+  int argc = sizeof(argv) / sizeof(const char *);
+
+  auto mock = mocking_utils::inject_on_return(
+    "lib:rclcpp", rcl_parse_arguments, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::remove_ros_arguments(argc, argv),
+    rclcpp::exceptions::RCLError(
+      RCL_RET_ERROR, rcl_get_error_state(), "failed to parse arguments"));
+}
+
+TEST(TestUtilities, remove_ros_arguments_rcl_remove_ros_arguments_failed) {
+  const char * const argv[] = {
+    "process_name",
+    "-d", "--ros-args",
+    "-r", "__ns:=/foo/bar",
+    "-r", "__ns:=/fiz/buz",
+    "--", "--foo=bar", "--baz"
+  };
+  int argc = sizeof(argv) / sizeof(const char *);
+
+  auto mock = mocking_utils::inject_on_return(
+    "lib:rclcpp", rcl_remove_ros_arguments, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::remove_ros_arguments(argc, argv),
+    rclcpp::exceptions::RCLError(
+      RCL_RET_ERROR, rcl_get_error_state(), ""));
+}
+
+TEST(TestUtilities, remove_ros_arguments_rcl_remove_ros_arguments_failed_and_fini) {
+  const char * const argv[] = {
+    "process_name",
+    "-d", "--ros-args",
+    "-r", "__ns:=/foo/bar",
+    "-r", "__ns:=/fiz/buz",
+    "--", "--foo=bar", "--baz"
+  };
+  int argc = sizeof(argv) / sizeof(const char *);
+
+  auto mock = mocking_utils::inject_on_return(
+    "lib:rclcpp", rcl_remove_ros_arguments, RCL_RET_ERROR);
+  auto mock2 = mocking_utils::inject_on_return(
+    "lib:rclcpp", rcl_arguments_fini, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::remove_ros_arguments(argc, argv),
+    rclcpp::exceptions::RCLError(
+      RCL_RET_ERROR, rcl_get_error_state(),
+      ", failed also to cleanup parsed arguments, leaking memory: "));
+}
+
+TEST(TestUtilities, remove_ros_arguments_rcl_arguments_fini_failed) {
+  const char * const argv[] = {
+    "process_name",
+    "-d", "--ros-args",
+    "-r", "__ns:=/foo/bar",
+    "-r", "__ns:=/fiz/buz",
+    "--", "--foo=bar", "--baz"
+  };
+  int argc = sizeof(argv) / sizeof(const char *);
+
+  auto mock = mocking_utils::inject_on_return(
+    "lib:rclcpp", rcl_arguments_fini, RCL_RET_ERROR);
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::remove_ros_arguments(argc, argv),
+    rclcpp::exceptions::RCLError(
+      RCL_RET_ERROR, rcl_get_error_state(), "failed to cleanup parsed arguments, leaking memory"));
+}

rclcpp/test/rclcpp/test_wait_set.cpp

@@ -266,6 +266,64 @@ TEST_F(TestWaitSet, add_guard_condition_to_two_different_wait_set) {
   }
 }
 
+
+/*
+ * Testing adding each entity and waiting, and removing each entity and waiting
+ */
+TEST_F(TestWaitSet, add_remove_wait) {
+  rclcpp::WaitSet wait_set;
+  auto node = std::make_shared<rclcpp::Node>("add_remove_wait");
+
+  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+  guard_condition->trigger();
+
+  // For coverage reasons, this subscription should have event handlers
+  rclcpp::SubscriptionOptions subscription_options;
+  subscription_options.event_callbacks.deadline_callback = [](auto) {};
+  subscription_options.event_callbacks.liveliness_callback = [](auto) {};
+  auto do_nothing = [](const std::shared_ptr<test_msgs::msg::BasicTypes>) {};
+  auto sub =
+    node->create_subscription<test_msgs::msg::BasicTypes>(
+    "~/test", 1, do_nothing, subscription_options);
+
+  auto timer = node->create_wall_timer(std::chrono::milliseconds(1), []() {});
+
+  auto client = node->create_client<rcl_interfaces::srv::ListParameters>("~/test");
+
+  auto srv_do_nothing = [](
+    const std::shared_ptr<rcl_interfaces::srv::ListParameters::Request>,
+    std::shared_ptr<rcl_interfaces::srv::ListParameters::Response>) {};
+  auto service =
+    node->create_service<rcl_interfaces::srv::ListParameters>("~/test", srv_do_nothing);
+
+  rclcpp::PublisherOptions publisher_options;
+  publisher_options.event_callbacks.deadline_callback =
+    [](rclcpp::QOSDeadlineOfferedInfo &) {};
+  auto pub = node->create_publisher<test_msgs::msg::BasicTypes>(
+    "~/test", 1, publisher_options);
+  auto qos_event = pub->get_event_handlers()[0];
+
+  // Subscription mask is required here for coverage.
+  wait_set.add_subscription(sub, {true, true, true});
+  wait_set.add_guard_condition(guard_condition);
+  wait_set.add_timer(timer);
+  wait_set.add_client(client);
+  wait_set.add_service(service);
+  wait_set.add_waitable(qos_event, pub);
+
+  // At least timer or guard_condition should trigger
+  EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_set.wait(std::chrono::seconds(1)).kind());
+
+  wait_set.remove_subscription(sub, {true, true, true});
+  wait_set.remove_guard_condition(guard_condition);
+  wait_set.remove_timer(timer);
+  wait_set.remove_client(client);
+  wait_set.remove_service(service);
+  wait_set.remove_waitable(qos_event);
+
+  EXPECT_EQ(rclcpp::WaitResultKind::Empty, wait_set.wait(std::chrono::seconds(1)).kind());
+}
+
 /*
  * Get wait_set from result.
  */

rclcpp_action/CMakeLists.txt

@@ -77,20 +77,36 @@ if(BUILD_TESTING)
   ament_add_gtest(test_client test/test_client.cpp TIMEOUT 180)
   if(TARGET test_client)
     ament_target_dependencies(test_client
+      "rcutils"
       "test_msgs"
     )
     target_link_libraries(test_client
       ${PROJECT_NAME}
+      mimick
     )
   endif()
 
-  ament_add_gtest(test_server test/test_server.cpp)
+  ament_add_gtest(test_server test/test_server.cpp TIMEOUT 180)
   if(TARGET test_server)
     ament_target_dependencies(test_server
+      "rcutils"
       "test_msgs"
     )
     target_link_libraries(test_server
       ${PROJECT_NAME}
+      mimick
+    )
+  endif()
+
+  ament_add_gtest(test_server_goal_handle test/test_server_goal_handle.cpp)
+  if(TARGET test_server_goal_handle)
+    ament_target_dependencies(test_server_goal_handle
+      "rcutils"
+      "test_msgs"
+    )
+    target_link_libraries(test_server_goal_handle
+      ${PROJECT_NAME}
+      mimick
     )
   endif()
 

rclcpp_action/package.xml

@@ -4,8 +4,11 @@
   <name>rclcpp_action</name>
   <version>5.0.0</version>
   <description>Adds action APIs for C++.</description>
-  <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
+  <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
+  <maintainer email="mabel@openrobotics.org">Mabel Zhang</maintainer>
+  <maintainer email="william@openrobotics.org">William Woodall</maintainer>
   <license>Apache License 2.0</license>
+  <author email="dthomas@openrobotics.org">Dirk Thomas</author>
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
@@ -22,6 +25,7 @@
   <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>test_msgs</test_depend>
 
   <export>

rclcpp_action/test/mocking_utils/patch.hpp

@@ -0,0 +1,393 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// Original file taken from:
+// https://github.com/ros2/rcutils/blob/master/test/mocking_utils/patch.hpp
+
+#ifndef MOCKING_UTILS__PATCH_HPP_
+#define MOCKING_UTILS__PATCH_HPP_
+
+#define MOCKING_UTILS_SUPPORT_VA_LIST
+#if (defined(__aarch64__) || defined(__arm__) || defined(_M_ARM) || defined(__thumb__))
+// In ARM machines, va_list does not define comparison operators
+// nor the compiler allows defining them via operator overloads.
+// Thus, Mimick argument matching code will not compile.
+#undef MOCKING_UTILS_SUPPORT_VA_LIST
+#endif
+
+#ifdef MOCKING_UTILS_SUPPORT_VA_LIST
+#include <cstdarg>
+#endif
+
+#include <functional>
+#include <string>
+#include <type_traits>
+#include <utility>
+
+#include "mimick/mimick.h"
+#include "rcutils/macros.h"
+
+namespace mocking_utils
+{
+
+/// Mimick specific traits for each mocking_utils::Patch instance.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam SignatureT Type of the symbol to be patched.
+*/
+template<size_t ID, typename SignatureT>
+struct PatchTraits;
+
+/// Traits specialization for ReturnT(void) free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ */
+template<size_t ID, typename ReturnT>
+struct PatchTraits<ID, ReturnT(void)>
+{
+  mmk_mock_define(mock_type, ReturnT);
+};
+
+/// Traits specialization for ReturnT(ArgT0) free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgT0 Argument type.
+ */
+template<size_t ID, typename ReturnT, typename ArgT0>
+struct PatchTraits<ID, ReturnT(ArgT0)>
+{
+  mmk_mock_define(mock_type, ReturnT, ArgT0);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1) free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ReturnT,
+  typename ArgT0, typename ArgT1>
+struct PatchTraits<ID, ReturnT(ArgT0, ArgT1)>
+{
+  mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2) free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ReturnT,
+  typename ArgT0, typename ArgT1, typename ArgT2>
+struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2)>
+{
+  mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1, ArgT2);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3) free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ReturnT,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3>
+struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3)>
+{
+  mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4)
+/// free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ReturnT,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3, typename ArgT4>
+struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4)>
+{
+  mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)
+/// free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ReturnT,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3,
+  typename ArgT4, typename ArgT5>
+struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5)>
+{
+  mmk_mock_define(
+    mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5);
+};
+
+/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6)
+/// free functions.
+/**
+ * \tparam ID Numerical identifier of the patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTx Argument types.
+ */
+template<size_t ID, typename ReturnT,
+  typename ArgT0, typename ArgT1,
+  typename ArgT2, typename ArgT3,
+  typename ArgT4, typename ArgT5, typename ArgT6>
+struct PatchTraits<ID, ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6)>
+{
+  mmk_mock_define(
+    mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6);
+};
+
+/// Generic trampoline to wrap generalized callables in plain functions.
+/**
+ * \tparam ID Numerical identifier of this trampoline. Ought to be unique.
+ * \tparam SignatureT Type of the symbol this trampoline replaces.
+ */
+template<size_t ID, typename SignatureT>
+struct Trampoline;
+
+/// Trampoline specialization for free functions.
+template<size_t ID, typename ReturnT, typename ... ArgTs>
+struct Trampoline<ID, ReturnT(ArgTs...)>
+{
+  static ReturnT base(ArgTs... args)
+  {
+    return target(std::forward<ArgTs>(args)...);
+  }
+
+  static std::function<ReturnT(ArgTs...)> target;
+};
+
+template<size_t ID, typename ReturnT, typename ... ArgTs>
+std::function<ReturnT(ArgTs...)>
+Trampoline<ID, ReturnT(ArgTs...)>::target;
+
+/// Setup trampoline with the given @p target.
+/**
+ * \param[in] target Callable that this trampoline will target.
+ * \return the plain base function of this trampoline.
+ *
+ * \tparam ID Numerical identifier of this trampoline. Ought to be unique.
+ * \tparam SignatureT Type of the symbol this trampoline replaces.
+ */
+template<size_t ID, typename SignatureT>
+auto prepare_trampoline(std::function<SignatureT> target)
+{
+  Trampoline<ID, SignatureT>::target = target;
+  return Trampoline<ID, SignatureT>::base;
+}
+
+/// Patch class for binary API mocking
+/**
+ * Built on top of Mimick, to enable symbol mocking on a per dynamically
+ * linked binary object basis.
+ *
+ * \tparam ID Numerical identifier for this patch. Ought to be unique.
+ * \tparam SignatureT Type of the symbol to be patched.
+ */
+template<size_t ID, typename SignatureT>
+class Patch;
+
+/// Patch specialization for ReturnT(ArgTs...) free functions.
+/**
+ * \tparam ID Numerical identifier for this patch. Ought to be unique.
+ * \tparam ReturnT Return value type.
+ * \tparam ArgTs Argument types.
+ */
+template<size_t ID, typename ReturnT, typename ... ArgTs>
+class Patch<ID, ReturnT(ArgTs...)>
+{
+public:
+  using mock_type = typename PatchTraits<ID, ReturnT(ArgTs...)>::mock_type;
+
+  /// Construct a patch.
+  /**
+   * \param[in] target Symbol target string, using Mimick syntax
+   *   i.e. "symbol(@scope)?", where scope may be "self" to target the current
+   *   binary, "lib:library_name" to target a given library, "file:path/to/library"
+   *   to target a given file, or "sym:other_symbol" to target the first library
+   *   that defines said symbol.
+   * \param[in] proxy An indirection to call the target function.
+   *   This indirection must ensure this call goes through the function's
+   *   trampoline, as setup by the dynamic linker.
+   * \return a mocking_utils::Patch instance.
+   */
+  explicit Patch(const std::string & target, std::function<ReturnT(ArgTs...)> proxy)
+  : target_(target), proxy_(proxy)
+  {
+  }
+
+  // Copy construction and assignment are disabled.
+  Patch(const Patch &) = delete;
+  Patch & operator=(const Patch &) = delete;
+
+  Patch(Patch && other)
+  {
+    mock_ = other.mock_;
+    other.mock_ = nullptr;
+  }
+
+  Patch & operator=(Patch && other)
+  {
+    if (mock_) {
+      mmk_reset(mock_);
+    }
+    mock_ = other.mock_;
+    other.mock_ = nullptr;
+  }
+
+  ~Patch()
+  {
+    if (mock_) {
+      mmk_reset(mock_);
+    }
+  }
+
+  /// Inject a @p replacement for the patched function.
+  Patch & then_call(std::function<ReturnT(ArgTs...)> replacement) &
+  {
+    replace_with(replacement);
+    return *this;
+  }
+
+  /// Inject a @p replacement for the patched function.
+  Patch && then_call(std::function<ReturnT(ArgTs...)> replacement) &&
+  {
+    replace_with(replacement);
+    return std::move(*this);
+  }
+
+private:
+  // Helper for template parameter pack expansion using `mmk_any`
+  // macro as pattern.
+  template<typename T>
+  T any() {return mmk_any(T);}
+
+  void replace_with(std::function<ReturnT(ArgTs...)> replacement)
+  {
+    if (mock_) {
+      throw std::logic_error("Cannot configure patch more than once");
+    }
+    auto type_erased_trampoline =
+      reinterpret_cast<mmk_fn>(prepare_trampoline<ID>(replacement));
+    auto MMK_MANGLE(mock_type, create) =
+      PatchTraits<ID, ReturnT(ArgTs...)>::MMK_MANGLE(mock_type, create);
+    mock_ = mmk_mock(target_.c_str(), mock_type);
+    mmk_when(proxy_(any<ArgTs>()...), .then_call = type_erased_trampoline);
+  }
+
+  mock_type mock_{nullptr};
+  std::string target_;
+  std::function<ReturnT(ArgTs...)> proxy_;
+};
+
+/// Make a patch for a `target` function.
+/**
+ * Useful for type deduction during \ref mocking_utils::Patch construction.
+ *
+ * \param[in] target Symbol target string, using Mimick syntax.
+ * \param[in] proxy An indirection to call the target function.
+ * \return a mocking_utils::Patch instance.
+ *
+ * \tparam ID Numerical identifier for this patch. Ought to be unique.
+ * \tparam SignatureT Type of the function to be patched.
+ *
+ * \sa mocking_utils::Patch for further reference.
+ */
+template<size_t ID, typename SignatureT>
+auto make_patch(const std::string & target, std::function<SignatureT> proxy)
+{
+  return Patch<ID, SignatureT>(target, proxy);
+}
+
+/// Define a dummy operator `op` for a given `type`.
+/**
+ * Useful to enable patching functions that take arguments whose types
+ * do not define basic comparison operators, as required by Mimick.
+*/
+#define MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(type_, op) \
+  template<typename T> \
+  typename std::enable_if<std::is_same<T, type_>::value, bool>::type \
+  operator op(const T &, const T &) { \
+    return false; \
+  }
+
+/// Get the exact \ref mocking_utils::Patch type for a given `id` and `function`.
+/**
+ * Useful to avoid ignored attribute warnings when using the \b decltype operator.
+ */
+#define MOCKING_UTILS_PATCH_TYPE(id, function) \
+  decltype(mocking_utils::make_patch<id, decltype(function)>("", nullptr))
+
+/// A transparent forwarding proxy to a given `function`.
+/**
+ * Useful to ensure a call to `function` goes through its trampoline.
+ */
+#define MOCKING_UTILS_PATCH_PROXY(function) \
+  [] (auto && ... args)->decltype(auto) { \
+    return function(std::forward<decltype(args)>(args)...); \
+  }
+
+/// Compute a Mimick symbol target string based on which `function` is to be patched
+/// in which `scope`.
+#define MOCKING_UTILS_PATCH_TARGET(scope, function) \
+  (std::string(RCUTILS_STRINGIFY(function)) + "@" + (scope))
+
+/// Prepare a mocking_utils::Patch for patching a `function` in a given `scope`
+/// but defer applying any changes.
+#define prepare_patch(scope, function) \
+  make_patch<__COUNTER__, decltype(function)>( \
+    MOCKING_UTILS_PATCH_TARGET(scope, function), MOCKING_UTILS_PATCH_PROXY(function) \
+  )
+
+/// Patch a `function` with a used-provided `replacement` in a given `scope`.
+#define patch(scope, function, replacement) \
+  prepare_patch(scope, function).then_call(replacement)
+
+/// Patch a `function` to always yield a given `return_code` in a given `scope`.
+#define patch_and_return(scope, function, return_code) \
+  patch(scope, function, [&](auto && ...) {return return_code;})
+
+/// Patch a `function` to execute normally but always yield a given `return_code`
+/// in a given `scope`.
+#define inject_on_return(scope, function, return_code) \
+  patch( \
+    scope, function, ([&, base = function](auto && ... __args) { \
+      static_cast<void>(base(std::forward<decltype(__args)>(__args)...)); \
+      return return_code; \
+    }))
+
+}  // namespace mocking_utils
+
+#ifdef MOCKING_UTILS_SUPPORT_VA_LIST
+// Define dummy comparison operators for C standard va_list type
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, ==)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, !=)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, <)
+MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, >)
+#endif
+
+#endif  // MOCKING_UTILS__PATCH_HPP_

rclcpp_action/test/test_client.cpp

@@ -20,6 +20,7 @@
 
 #include <rcl_action/names.h>
 #include <rcl_action/default_qos.h>
+#include <rcl_action/wait.h>
 
 #include <rclcpp/clock.hpp>
 #include <rclcpp/exceptions.hpp>
@@ -46,6 +47,8 @@
 #include "rclcpp_action/qos.hpp"
 #include "rclcpp_action/types.hpp"
 
+#include "./mocking_utils/patch.hpp"
+
 using namespace std::chrono_literals;
 
 const auto WAIT_FOR_SERVER_TIMEOUT = 10s;
@@ -326,23 +329,85 @@ TEST_F(TestClient, construction_and_destruction_callback_group)
     ).reset());
 }
 
+TEST_F(TestClient, construction_and_destruction_rcl_errors)
+{
+  {
+    auto mock = mocking_utils::inject_on_return(
+      "lib:rclcpp_action", rcl_action_client_fini, RCL_RET_ERROR);
+    // It just logs an error message and continues
+    EXPECT_NO_THROW(
+      rclcpp_action::create_client<ActionType>(client_node, action_name).reset());
+  }
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp_action", rcl_action_client_init, RCL_RET_ERROR);
+    EXPECT_THROW(
+      rclcpp_action::create_client<ActionType>(client_node, action_name).reset(),
+      rclcpp::exceptions::RCLError);
+  }
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp_action", rcl_action_client_wait_set_get_num_entities, RCL_RET_ERROR);
+    EXPECT_THROW(
+      rclcpp_action::create_client<ActionType>(client_node, action_name),
+      rclcpp::exceptions::RCLError);
+  }
+}
+
 TEST_F(TestClient, wait_for_action_server)
 {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
   EXPECT_FALSE(action_client->wait_for_action_server(0ms));
-  EXPECT_FALSE(action_client->wait_for_action_server(100ms));
-  auto future = std::async(
-    std::launch::async, [&action_client]() {
-      return action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT);
-    });
+  EXPECT_FALSE(action_client->wait_for_action_server(10ms));
   SetUpServer();
-  EXPECT_TRUE(future.get());
+  ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
   TearDownServer();
 
   client_node.reset();  // Drop node before action client
   EXPECT_THROW(action_client->wait_for_action_server(0ms), rclcpp::exceptions::InvalidNodeError);
 }
 
+TEST_F(TestClient, wait_for_action_server_rcl_errors)
+{
+  auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+  SetUpServer();
+  ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp_action", rcl_action_server_is_available, RCL_RET_NODE_INVALID);
+    EXPECT_THROW(action_client->action_server_is_ready(), rclcpp::exceptions::RCLError);
+
+    auto mock_context_is_valid = mocking_utils::patch_and_return(
+      "lib:rclcpp_action", rcl_context_is_valid, false);
+    EXPECT_FALSE(action_client->action_server_is_ready());
+  }
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp_action", rcl_action_server_is_available, RCL_RET_ERROR);
+    EXPECT_THROW(action_client->action_server_is_ready(), rclcpp::exceptions::RCLError);
+  }
+  TearDownServer();
+}
+
+TEST_F(TestClient, is_ready) {
+  auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcutils_allocator_t allocator = rcutils_get_default_allocator();
+  auto rcl_context = client_node->get_node_base_interface()->get_context()->get_rcl_context().get();
+  ASSERT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 10, 10, 10, 10, 10, 10, rcl_context, allocator));
+  ASSERT_TRUE(action_client->add_to_wait_set(&wait_set));
+  EXPECT_TRUE(action_client->is_ready(&wait_set));
+
+  {
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp_action", rcl_action_client_wait_set_get_entities_ready, RCL_RET_ERROR);
+    EXPECT_THROW(action_client->is_ready(&wait_set), rclcpp::exceptions::RCLError);
+  }
+  client_node.reset();  // Drop node before action client
+}
+
 TEST_F(TestClientAgainstServer, async_send_goal_no_callbacks)
 {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
@@ -435,7 +500,7 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_goal_response_callback_wait
   auto send_goal_ops = rclcpp_action::Client<ActionType>::SendGoalOptions();
   send_goal_ops.goal_response_callback =
     [&goal_response_received]
-      (typename ActionGoalHandle::SharedPtr goal_handle) mutable
+      (typename ActionGoalHandle::SharedPtr goal_handle)
     {
       if (goal_handle) {
         goal_response_received = true;
@@ -483,7 +548,7 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_feedback_callback_wait_for_
   send_goal_ops.feedback_callback =
     [&feedback_count](
     typename ActionGoalHandle::SharedPtr goal_handle,
-    const std::shared_ptr<const ActionFeedback> feedback) mutable
+    const std::shared_ptr<const ActionFeedback> feedback)
     {
       (void)goal_handle;
       (void)feedback;
@@ -516,7 +581,7 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_result_callback_wait_for_re
   auto send_goal_ops = rclcpp_action::Client<ActionType>::SendGoalOptions();
   send_goal_ops.result_callback =
     [&result_callback_received](
-    const typename ActionGoalHandle::WrappedResult & result) mutable
+    const typename ActionGoalHandle::WrappedResult & result)
     {
       if (
         rclcpp_action::ResultCode::SUCCEEDED == result.code &&
@@ -558,7 +623,7 @@ TEST_F(TestClientAgainstServer, async_get_result_with_callback)
   auto future_result = action_client->async_get_result(
     goal_handle,
     [&result_callback_received](
-      const typename ActionGoalHandle::WrappedResult & result) mutable
+      const typename ActionGoalHandle::WrappedResult & result)
     {
       if (
         rclcpp_action::ResultCode::SUCCEEDED == result.code &&
@@ -609,7 +674,7 @@ TEST_F(TestClientAgainstServer, async_cancel_one_goal_with_callback)
   auto future_cancel = action_client->async_cancel_goal(
     goal_handle,
     [&cancel_response_received, goal_handle](
-      ActionCancelGoalResponse::SharedPtr response) mutable
+      ActionCancelGoalResponse::SharedPtr response)
     {
       if (
         ActionCancelGoalResponse::ERROR_NONE == response->return_code &&
@@ -786,3 +851,113 @@ TEST_F(TestClientAgainstServer, async_cancel_some_goals_with_callback)
   EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid);
   EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status());
 }
+
+TEST_F(TestClientAgainstServer, send_rcl_errors)
+{
+  auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+  ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
+
+  auto send_goal_ops = rclcpp_action::Client<ActionType>::SendGoalOptions();
+  send_goal_ops.result_callback =
+    [](const typename ActionGoalHandle::WrappedResult &) {};
+  send_goal_ops.feedback_callback =
+    [](typename ActionGoalHandle::SharedPtr,
+      const std::shared_ptr<const ActionFeedback>) {};
+
+  {
+    ActionGoal goal;
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp_action", rcl_action_send_goal_request, RCL_RET_ERROR);
+    EXPECT_THROW(
+      action_client->async_send_goal(goal, send_goal_ops),
+      rclcpp::exceptions::RCLError);
+  }
+  {
+    ActionGoal goal;
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp_action", rcl_action_send_result_request, RCL_RET_ERROR);
+    auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops);
+    dual_spin_until_future_complete(future_goal_handle);
+    auto goal_handle = future_goal_handle.get();
+    EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_UNKNOWN, goal_handle->get_status());
+  }
+  {
+    ActionGoal goal;
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp_action", rcl_action_send_cancel_request, RCL_RET_ERROR);
+    auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops);
+    dual_spin_until_future_complete(future_goal_handle);
+    auto goal_handle = future_goal_handle.get();
+    EXPECT_THROW(
+      action_client->async_cancel_goals_before(goal_handle->get_goal_stamp()),
+      rclcpp::exceptions::RCLError);
+  }
+}
+
+TEST_F(TestClientAgainstServer, execute_rcl_errors)
+{
+  auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+  ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
+
+  auto send_goal_ops = rclcpp_action::Client<ActionType>::SendGoalOptions();
+  send_goal_ops.result_callback =
+    [](const typename ActionGoalHandle::WrappedResult &) {};
+  send_goal_ops.feedback_callback =
+    [](typename ActionGoalHandle::SharedPtr,
+      const std::shared_ptr<const ActionFeedback>) {};
+
+  {
+    ActionGoal goal;
+    goal.order = 5;
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp_action", rcl_action_take_feedback, RCL_RET_ERROR);
+
+    auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops);
+    dual_spin_until_future_complete(future_goal_handle);
+    auto goal_handle = future_goal_handle.get();
+    auto future_result = action_client->async_get_result(goal_handle);
+    EXPECT_THROW(
+      dual_spin_until_future_complete(future_result),
+      rclcpp::exceptions::RCLError);
+  }
+  {
+    ActionGoal goal;
+    goal.order = 5;
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp_action", rcl_action_take_goal_response, RCL_RET_ERROR);
+
+    auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops);
+    EXPECT_THROW(
+      dual_spin_until_future_complete(future_goal_handle),
+      rclcpp::exceptions::RCLError);
+  }
+  {
+    ActionGoal goal;
+    goal.order = 5;
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp_action", rcl_action_take_result_response, RCL_RET_ERROR);
+
+    auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops);
+    dual_spin_until_future_complete(future_goal_handle);
+    auto goal_handle = future_goal_handle.get();
+    auto future_result = action_client->async_get_result(goal_handle);
+    EXPECT_THROW(
+      dual_spin_until_future_complete(future_result),
+      rclcpp::exceptions::RCLError);
+  }
+  {
+    ActionGoal goal;
+    goal.order = 5;
+    auto mock = mocking_utils::patch_and_return(
+      "lib:rclcpp_action", rcl_action_take_cancel_response, RCL_RET_ERROR);
+
+    auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops);
+    dual_spin_until_future_complete(future_goal_handle);
+    auto goal_handle = future_goal_handle.get();
+    auto future_cancel_some =
+      action_client->async_cancel_goals_before(goal_handle->get_goal_stamp());
+    EXPECT_THROW(
+      dual_spin_until_future_complete(future_cancel_some),
+      rclcpp::exceptions::RCLError);
+  }
+}

rclcpp_action/test/test_server.cpp

@@ -15,6 +15,7 @@
 #include <rclcpp/exceptions.hpp>
 #include <rclcpp/node.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <rclcpp/scope_exit.hpp>
 #include <test_msgs/action/fibonacci.hpp>
 
 #include <gtest/gtest.h>
@@ -22,8 +23,15 @@
 #include <memory>
 #include <vector>
 
+#include "rcl_action/action_server.h"
+#include "rcl_action/wait.h"
 #include "rclcpp_action/create_server.hpp"
 #include "rclcpp_action/server.hpp"
+#include "./mocking_utils/patch.hpp"
+
+// Note: This is a long running test with rmw_connext_cpp, if you change this file, please check
+// that this test can complete fully, or adjust the timeout as necessary.
+// See https://github.com/ros2/rmw_connext/issues/325 for resolution
 
 using Fibonacci = test_msgs::action::Fibonacci;
 using CancelResponse = typename Fibonacci::Impl::CancelGoalService::Response;
@@ -37,8 +45,15 @@ protected:
     rclcpp::init(0, nullptr);
   }
 
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
   std::shared_ptr<Fibonacci::Impl::SendGoalService::Request>
-  send_goal_request(rclcpp::Node::SharedPtr node, GoalUUID uuid)
+  send_goal_request(
+    rclcpp::Node::SharedPtr node, GoalUUID uuid,
+    std::chrono::milliseconds timeout = std::chrono::milliseconds(-1))
   {
     auto client = node->create_client<Fibonacci::Impl::SendGoalService>(
       "fibonacci/_action/send_goal");
@@ -48,17 +63,20 @@ protected:
     auto request = std::make_shared<Fibonacci::Impl::SendGoalService::Request>();
     request->goal_id.uuid = uuid;
     auto future = client->async_send_request(request);
-    if (
-      rclcpp::FutureReturnCode::SUCCESS !=
-      rclcpp::spin_until_future_complete(node, future))
-    {
+    auto return_code = rclcpp::spin_until_future_complete(node, future, timeout);
+    if (rclcpp::FutureReturnCode::SUCCESS == return_code) {
+      return request;
+    } else if (rclcpp::FutureReturnCode::TIMEOUT == return_code) {
+      throw std::runtime_error("send goal future timed out");
+    } else {
       throw std::runtime_error("send goal future didn't complete succesfully");
     }
-    return request;
   }
 
   CancelResponse::SharedPtr
-  send_cancel_request(rclcpp::Node::SharedPtr node, GoalUUID uuid)
+  send_cancel_request(
+    rclcpp::Node::SharedPtr node, GoalUUID uuid,
+    std::chrono::milliseconds timeout = std::chrono::milliseconds(-1))
   {
     auto cancel_client = node->create_client<Fibonacci::Impl::CancelGoalService>(
       "fibonacci/_action/cancel_goal");
@@ -68,13 +86,14 @@ protected:
     auto request = std::make_shared<Fibonacci::Impl::CancelGoalService::Request>();
     request->goal_info.goal_id.uuid = uuid;
     auto future = cancel_client->async_send_request(request);
-    if (
-      rclcpp::FutureReturnCode::SUCCESS !=
-      rclcpp::spin_until_future_complete(node, future))
-    {
-      throw std::runtime_error("cancel goal future didn't complete succesfully");
+    auto return_code = rclcpp::spin_until_future_complete(node, future, timeout);
+    if (rclcpp::FutureReturnCode::SUCCESS == return_code) {
+      return future.get();
+    } else if (rclcpp::FutureReturnCode::TIMEOUT == return_code) {
+      throw std::runtime_error("cancel request future timed out");
+    } else {
+      throw std::runtime_error("cancel request future didn't complete succesfully");
     }
-    return future.get();
   }
 };
 
@@ -146,6 +165,50 @@ TEST_F(TestServer, construction_and_destruction_callback_group)
       group));
 }
 
+TEST_F(TestServer, construction_and_destruction_server_init_error)
+{
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp_action", rcl_action_server_init, RCL_RET_ERROR);
+  auto node = std::make_shared<rclcpp::Node>("construct_node", "/rclcpp_action/construct");
+
+  EXPECT_THROW(
+  {
+    using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+    auto as = rclcpp_action::create_server<Fibonacci>(
+      node, "fibonacci",
+      [](const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>) {
+        return rclcpp_action::GoalResponse::REJECT;
+      },
+      [](std::shared_ptr<GoalHandle>) {
+        return rclcpp_action::CancelResponse::REJECT;
+      },
+      [](std::shared_ptr<GoalHandle>) {});
+    (void)as;
+  }, rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestServer, construction_and_destruction_wait_set_error)
+{
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp_action", rcl_action_server_wait_set_get_num_entities, RCL_RET_ERROR);
+  auto node = std::make_shared<rclcpp::Node>("construct_node", "/rclcpp_action/construct");
+
+  EXPECT_THROW(
+  {
+    using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+    auto as = rclcpp_action::create_server<Fibonacci>(
+      node, "fibonacci",
+      [](const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>) {
+        return rclcpp_action::GoalResponse::REJECT;
+      },
+      [](std::shared_ptr<GoalHandle>) {
+        return rclcpp_action::CancelResponse::REJECT;
+      },
+      [](std::shared_ptr<GoalHandle>) {});
+    (void)as;
+  }, rclcpp::exceptions::RCLError);
+}
+
 TEST_F(TestServer, handle_goal_called)
 {
   auto node = std::make_shared<rclcpp::Node>("handle_goal_node", "/rclcpp_action/handle_goal");
@@ -923,3 +986,238 @@ TEST_F(TestServer, deferred_execution)
   received_handle->execute();
   EXPECT_TRUE(received_handle->is_executing());
 }
+
+class TestBasicServer : public TestServer
+{
+public:
+  void SetUp()
+  {
+    node_ = std::make_shared<rclcpp::Node>("goal_request", "/rclcpp_action/goal_request");
+    uuid_ = {{1, 2, 3, 4, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}};
+    action_server_ = rclcpp_action::create_server<Fibonacci>(
+      node_, "fibonacci",
+      [](const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>) {
+        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+      },
+      [](std::shared_ptr<GoalHandle>) {
+        return rclcpp_action::CancelResponse::ACCEPT;
+      },
+      [this](std::shared_ptr<GoalHandle> handle) {
+        goal_handle_ = handle;
+      });
+  }
+
+  void SendClientGoalRequest(
+    std::chrono::milliseconds timeout = std::chrono::milliseconds(-1))
+  {
+    send_goal_request(node_, uuid_, timeout);
+    auto result_client = node_->create_client<Fibonacci::Impl::GetResultService>(
+      "fibonacci/_action/get_result");
+    if (!result_client->wait_for_service(std::chrono::seconds(20))) {
+      throw std::runtime_error("get result service didn't become available");
+    }
+    auto request = std::make_shared<Fibonacci::Impl::GetResultService::Request>();
+    request->goal_id.uuid = uuid_;
+    auto future = result_client->async_send_request(request);
+
+    // Send a result
+    auto result = std::make_shared<Fibonacci::Result>();
+    result->sequence = {5, 8, 13, 21};
+    goal_handle_->succeed(result);
+
+    // Wait for the result request to be received
+    ASSERT_EQ(
+      rclcpp::FutureReturnCode::SUCCESS,
+      rclcpp::spin_until_future_complete(node_, future));
+
+    auto response = future.get();
+    EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->status);
+    EXPECT_EQ(result->sequence, response->result.sequence);
+
+    // Wait for goal expiration
+    rclcpp::sleep_for(std::chrono::milliseconds(100));
+
+    // Allow for expiration to take place
+    rclcpp::spin_some(node_);
+
+    // Send and wait for another result request
+    future = result_client->async_send_request(request);
+    ASSERT_EQ(
+      rclcpp::FutureReturnCode::SUCCESS,
+      rclcpp::spin_until_future_complete(node_, future));
+  }
+
+protected:
+  GoalUUID uuid_;
+  std::shared_ptr<rclcpp::Node> node_;
+  std::shared_ptr<rclcpp_action::Server<Fibonacci>> action_server_;
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+  std::shared_ptr<GoalHandle> goal_handle_;
+};
+
+class TestGoalRequestServer : public TestBasicServer {};
+
+TEST_F(TestGoalRequestServer, execute_goal_request_received_take_goal)
+{
+  EXPECT_NO_THROW(SendClientGoalRequest());
+}
+
+class TestCancelRequestServer : public TestBasicServer
+{
+public:
+  void SendClientCancelRequest(
+    std::chrono::milliseconds timeout = std::chrono::milliseconds(-1))
+  {
+    send_goal_request(node_, uuid_, timeout);
+    send_cancel_request(node_, uuid_, timeout);
+  }
+};
+
+TEST_F(TestCancelRequestServer, execute_goal_request_received_take_goal)
+{
+  EXPECT_NO_THROW(SendClientCancelRequest());
+}
+
+TEST_F(TestGoalRequestServer, is_ready_rcl_error) {
+  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
+  rcutils_allocator_t allocator = rcutils_get_default_allocator();
+  auto rcl_context = node_->get_node_base_interface()->get_context()->get_rcl_context().get();
+  ASSERT_EQ(
+    RCL_RET_OK,
+    rcl_wait_set_init(&wait_set, 10, 10, 10, 10, 10, 10, rcl_context, allocator));
+  RCLCPP_SCOPE_EXIT(
+  {
+    EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));
+  });
+  ASSERT_TRUE(action_server_->add_to_wait_set(&wait_set));
+
+  EXPECT_TRUE(action_server_->is_ready(&wait_set));
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp_action", rcl_action_server_wait_set_get_entities_ready, RCL_RET_ERROR);
+  EXPECT_THROW(action_server_->is_ready(&wait_set), rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestGoalRequestServer, execute_goal_request_received_take_goal_request_errors)
+{
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp_action", rcl_action_take_goal_request, RCL_RET_ERROR);
+
+  EXPECT_THROW(SendClientGoalRequest(), rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestGoalRequestServer, execute_goal_request_received_send_goal_response_errors)
+{
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp_action", rcl_action_send_goal_response, RCL_RET_ERROR);
+
+  EXPECT_THROW(SendClientGoalRequest(), rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestGoalRequestServer, execute_goal_request_received_accept_new_goal_errors)
+{
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp_action", rcl_action_accept_new_goal, nullptr);
+
+  EXPECT_THROW(SendClientGoalRequest(), std::runtime_error);
+}
+
+TEST_F(TestGoalRequestServer, execute_goal_request_received_update_goal_state_errors)
+{
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp_action", rcl_action_update_goal_state, RCL_RET_ERROR);
+
+  EXPECT_THROW(SendClientGoalRequest(), rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestGoalRequestServer, publish_status_server_get_goal_handles_errors)
+{
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp_action", rcl_action_server_get_goal_handles, RCL_RET_ERROR);
+
+  EXPECT_THROW(SendClientGoalRequest(), rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestGoalRequestServer, publish_status_get_goal_status_array_errors)
+{
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp_action", rcl_action_get_goal_status_array, RCL_RET_ERROR);
+
+  EXPECT_THROW(SendClientGoalRequest(), rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestGoalRequestServer, publish_status_publish_status_errors)
+{
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp_action", rcl_action_publish_status, RCL_RET_ERROR);
+
+  EXPECT_THROW(SendClientGoalRequest(), std::runtime_error);
+}
+
+TEST_F(TestGoalRequestServer, execute_goal_request_received_take_failed)
+{
+  auto mock = mocking_utils::inject_on_return(
+    "lib:rclcpp_action", rcl_action_take_goal_request, RCL_RET_ACTION_SERVER_TAKE_FAILED);
+  try {
+    SendClientGoalRequest(std::chrono::milliseconds(100));
+    ADD_FAILURE() << "SetupActionServerAndSpin did not throw, but was expected to";
+  } catch (const std::runtime_error & e) {
+    EXPECT_STREQ("send goal future timed out", e.what());
+  } catch (...) {
+    ADD_FAILURE() << "SetupActionServerAndSpin threw, but not the expected std::runtime_error";
+  }
+}
+
+TEST_F(TestGoalRequestServer, get_result_rcl_errors)
+{
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp_action", rcl_action_take_result_request, RCL_RET_ERROR);
+
+  EXPECT_THROW(SendClientGoalRequest(), rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestGoalRequestServer, send_result_rcl_errors)
+{
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp_action", rcl_action_send_result_response, RCL_RET_ERROR);
+
+  EXPECT_THROW(SendClientGoalRequest(), rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestCancelRequestServer, execute_cancel_request_received_take_cancel_request_errors)
+{
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp_action", rcl_action_take_cancel_request, RCL_RET_ERROR);
+
+  EXPECT_THROW(SendClientCancelRequest(), rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestCancelRequestServer, execute_cancel_request_received_take_failed)
+{
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp_action", rcl_action_take_cancel_request, RCL_RET_ACTION_SERVER_TAKE_FAILED);
+  try {
+    SendClientCancelRequest(std::chrono::milliseconds(100));
+    ADD_FAILURE() << "SetupActionServerAndSpin did not throw, but it was expected to";
+  } catch (const std::runtime_error & e) {
+    EXPECT_STREQ("cancel request future timed out", e.what());
+  } catch (...) {
+    ADD_FAILURE() << "SetupActionServerAndSpin threw, but not the expected std::runtime_error";
+  }
+}
+
+TEST_F(TestCancelRequestServer, publish_status_rcl_errors)
+{
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp_action", rcl_action_process_cancel_request, RCL_RET_ERROR);
+
+  EXPECT_THROW(SendClientCancelRequest(), rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestCancelRequestServer, publish_status_send_cancel_response_errors)
+{
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp_action", rcl_action_send_cancel_response, RCL_RET_ERROR);
+
+  EXPECT_THROW(SendClientCancelRequest(), std::runtime_error);
+}

rclcpp_action/test/test_server_goal_handle.cpp

@@ -0,0 +1,159 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <rclcpp/exceptions.hpp>
+#include <rclcpp/node.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <test_msgs/action/fibonacci.hpp>
+
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <vector>
+
+#include "action_msgs/msg/goal_info.h"
+#include "rclcpp_action/server_goal_handle.hpp"
+#include "./mocking_utils/patch.hpp"
+
+class FibonacciServerGoalHandle
+  : public rclcpp_action::ServerGoalHandle<test_msgs::action::Fibonacci>
+{
+public:
+  FibonacciServerGoalHandle(
+    std::shared_ptr<rcl_action_goal_handle_t> rcl_handle,
+    rclcpp_action::GoalUUID uuid,
+    std::shared_ptr<const test_msgs::action::Fibonacci::Goal> goal,
+    std::function<void(
+      const rclcpp_action::GoalUUID &, std::shared_ptr<void>)> on_terminal_state,
+    std::function<void(const rclcpp_action::GoalUUID &)> on_executing,
+    std::function<void(
+      std::shared_ptr<test_msgs::action::Fibonacci::Impl::FeedbackMessage>)> publish_feedback)
+  : rclcpp_action::ServerGoalHandle<test_msgs::action::Fibonacci>(
+      rcl_handle, uuid, goal, on_terminal_state, on_executing, publish_feedback) {}
+
+  bool try_cancel() {return try_canceling();}
+
+  void cancel_goal() {_cancel_goal();}
+};
+
+class TestServerGoalHandle : public ::testing::Test
+{
+public:
+  TestServerGoalHandle()
+  : handle_(nullptr) {}
+
+  void SetUp()
+  {
+    std::shared_ptr<rcl_action_goal_handle_t> rcl_handle =
+      std::make_shared<rcl_action_goal_handle_t>();
+    *rcl_handle.get() = rcl_action_get_zero_initialized_goal_handle();
+    rcutils_allocator_t allocator = rcutils_get_default_allocator();
+    rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info();
+    ASSERT_EQ(RCL_RET_OK, rcl_action_goal_handle_init(rcl_handle.get(), &goal_info, allocator));
+    rclcpp_action::GoalUUID uuid;
+    std::shared_ptr<const test_msgs::action::Fibonacci::Goal> goal =
+      std::make_shared<const test_msgs::action::Fibonacci::Goal>();
+    auto on_terminal_state = [](const rclcpp_action::GoalUUID &, std::shared_ptr<void>) {};
+    auto on_executing = [](const rclcpp_action::GoalUUID &) {};
+    auto publish_feedback =
+      [](std::shared_ptr<test_msgs::action::Fibonacci::Impl::FeedbackMessage>) {};
+    handle_ = std::make_unique<FibonacciServerGoalHandle>(
+      rcl_handle, uuid, goal, on_terminal_state, on_executing, publish_feedback);
+  }
+
+protected:
+  std::unique_ptr<FibonacciServerGoalHandle> handle_;
+};
+
+TEST_F(TestServerGoalHandle, construct_destruct) {
+  EXPECT_FALSE(handle_->is_canceling());
+  EXPECT_TRUE(handle_->is_active());
+  EXPECT_FALSE(handle_->is_executing());
+}
+
+TEST_F(TestServerGoalHandle, cancel) {
+  handle_->execute();
+  EXPECT_TRUE(handle_->try_cancel());
+  EXPECT_FALSE(handle_->is_canceling());
+  EXPECT_FALSE(handle_->is_active());
+  EXPECT_FALSE(handle_->is_executing());
+
+  {
+    auto mock_get_status = mocking_utils::patch_and_return(
+      "lib:rclcpp_action", rcl_action_goal_handle_get_status, RCL_RET_ERROR);
+    EXPECT_FALSE(handle_->try_cancel());
+  }
+
+  {
+    auto mock_update_status = mocking_utils::patch_and_return(
+      "lib:rclcpp_action", rcl_action_update_goal_state, RCL_RET_ERROR);
+    auto mock_is_cancelable = mocking_utils::patch_and_return(
+      "lib:rclcpp_action", rcl_action_goal_handle_is_cancelable, true);
+    EXPECT_FALSE(handle_->try_cancel());
+    EXPECT_THROW(handle_->cancel_goal(), rclcpp::exceptions::RCLError);
+
+    test_msgs::action::Fibonacci::Result::SharedPtr result =
+      std::make_shared<test_msgs::action::Fibonacci::Result>();
+    EXPECT_THROW(handle_->canceled(result), rclcpp::exceptions::RCLError);
+  }
+}
+
+TEST_F(TestServerGoalHandle, abort) {
+  handle_->execute();
+  test_msgs::action::Fibonacci::Result::SharedPtr result =
+    std::make_shared<test_msgs::action::Fibonacci::Result>();
+  handle_->abort(result);
+  EXPECT_FALSE(handle_->is_canceling());
+  EXPECT_FALSE(handle_->is_active());
+  EXPECT_FALSE(handle_->is_executing());
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp_action", rcl_action_update_goal_state, RCL_RET_ERROR);
+  EXPECT_THROW(handle_->abort(result), rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestServerGoalHandle, succeed) {
+  handle_->execute();
+  test_msgs::action::Fibonacci::Result::SharedPtr result =
+    std::make_shared<test_msgs::action::Fibonacci::Result>();
+  handle_->succeed(result);
+  EXPECT_FALSE(handle_->is_canceling());
+  EXPECT_FALSE(handle_->is_active());
+  EXPECT_FALSE(handle_->is_executing());
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp_action", rcl_action_update_goal_state, RCL_RET_ERROR);
+  EXPECT_THROW(handle_->succeed(result), rclcpp::exceptions::RCLError);
+}
+
+
+TEST_F(TestServerGoalHandle, execute) {
+  handle_->execute();
+  EXPECT_FALSE(handle_->is_canceling());
+  EXPECT_TRUE(handle_->is_active());
+  EXPECT_TRUE(handle_->is_executing());
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp_action", rcl_action_update_goal_state, RCL_RET_ERROR);
+  EXPECT_THROW(handle_->execute(), rclcpp::exceptions::RCLError);
+}
+
+TEST_F(TestServerGoalHandle, rcl_action_goal_handle_get_status_error) {
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp_action", rcl_action_goal_handle_get_status, RCL_RET_ERROR);
+
+  EXPECT_THROW(handle_->is_canceling(), rclcpp::exceptions::RCLError);
+  EXPECT_NO_THROW(handle_->is_active());
+  EXPECT_THROW(handle_->is_executing(), rclcpp::exceptions::RCLError);
+}

rclcpp_components/include/rclcpp_components/component_manager.hpp

@@ -101,7 +101,9 @@ public:
   ComponentManager(
     std::weak_ptr<rclcpp::Executor> executor,
     std::string node_name = "ComponentManager",
-    const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions());
+    const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()
+    .start_parameter_services(false)
+    .start_parameter_event_publisher(false));
 
   RCLCPP_COMPONENTS_PUBLIC
   virtual ~ComponentManager();

rclcpp_components/package.xml

@@ -4,8 +4,11 @@
   <name>rclcpp_components</name>
   <version>5.0.0</version>
   <description>Package containing tools for dynamically loadable components</description>
-  <maintainer email="michael@openrobotics.org">Michael Carroll</maintainer>
+  <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
+  <maintainer email="mabel@openrobotics.org">Mabel Zhang</maintainer>
+  <maintainer email="william@openrobotics.org">William Woodall</maintainer>
   <license>Apache License 2.0</license>
+  <author email="michael@openrobotics.org">Michael Carroll</author>
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 

rclcpp_lifecycle/CMakeLists.txt

@@ -51,7 +51,7 @@ if(BUILD_TESTING)
   set(ament_cmake_cppcheck_ADDITIONAL_INCLUDE_DIRS ${rclcpp_INCLUDE_DIRS})
   ament_lint_auto_find_test_dependencies()
 
-  ament_add_gtest(test_lifecycle_node test/test_lifecycle_node.cpp)
+  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"
@@ -69,7 +69,7 @@ if(BUILD_TESTING)
     )
     target_link_libraries(test_lifecycle_publisher ${PROJECT_NAME})
   endif()
-  ament_add_gtest(test_lifecycle_service_client test/test_lifecycle_service_client.cpp)
+  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_lifecycle/package.xml

@@ -4,8 +4,11 @@
   <name>rclcpp_lifecycle</name>
   <version>5.0.0</version>
   <description>Package containing a prototype for lifecycle implementation</description>
-  <maintainer email="karsten@osrfoundation.org">Karsten Knese</maintainer>
+  <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
+  <maintainer email="mabel@openrobotics.org">Mabel Zhang</maintainer>
+  <maintainer email="william@openrobotics.org">William Woodall</maintainer>
   <license>Apache License 2.0</license>
+  <author email="karsten@openrobotics.org">Karsten Knese</author>
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 

rclcpp_lifecycle/src/lifecycle_node.cpp

@@ -38,6 +38,7 @@
 #include "rclcpp/node_interfaces/node_topics.hpp"
 #include "rclcpp/node_interfaces/node_waitables.hpp"
 #include "rclcpp/parameter_service.hpp"
+#include "rclcpp/qos.hpp"
 
 #include "lifecycle_node_interface_impl.hpp"  // implementation
 
@@ -97,7 +98,8 @@ LifecycleNode::LifecycleNode(
       node_services_,
       node_logging_,
       node_clock_,
-      node_parameters_
+      node_parameters_,
+      options.clock_qos()
     )),
   node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())),
   node_options_(options),

rclcpp_lifecycle/test/test_lifecycle_node.cpp

@@ -31,6 +31,10 @@
 
 #include "./mocking_utils/patch.hpp"
 
+// Note: This is a long running test with rmw_connext_cpp, if you change this file, please check
+// that this test can complete fully, or adjust the timeout as necessary.
+// See https://github.com/ros2/rmw_connext/issues/325 for resolution
+
 using lifecycle_msgs::msg::State;
 using lifecycle_msgs::msg::Transition;
 

rclcpp_lifecycle/test/test_lifecycle_service_client.cpp

@@ -52,6 +52,10 @@ constexpr char const * node_get_transition_graph_topic =
   "/lc_talker/get_transition_graph";
 const lifecycle_msgs::msg::State unknown_state = lifecycle_msgs::msg::State();
 
+// Note: This is a long running test with rmw_connext_cpp, if you change this file, please check
+// that this test can complete fully, or adjust the timeout as necessary.
+// See https://github.com/ros2/rmw_connext/issues/325 for resolution
+
 class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode
 {
 public:
@@ -372,6 +376,14 @@ TEST_F(TestLifecycleServiceClient, get_service_names_and_types_by_node)
     std::runtime_error);
   auto service_names_and_types1 = node_graph->get_service_names_and_types_by_node("client1", "/");
   auto service_names_and_types2 = node_graph->get_service_names_and_types_by_node("client2", "/");
+  auto start = std::chrono::steady_clock::now();
+  while (0 == service_names_and_types1.size() ||
+    service_names_and_types1.size() != service_names_and_types2.size() ||
+    (std::chrono::steady_clock::now() - start) < std::chrono::seconds(1))
+  {
+    service_names_and_types1 = node_graph->get_service_names_and_types_by_node("client1", "/");
+    service_names_and_types2 = node_graph->get_service_names_and_types_by_node("client2", "/");
+  }
   EXPECT_EQ(service_names_and_types1.size(), service_names_and_types2.size());
 }