Fix Windows min/max macro problem.

Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>

rclcpp/CHANGELOG.rst

@@ -2,6 +2,72 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+5.1.0 (2020-11-02)
+------------------
+* Deprecate Duration(rcl_duration_value_t) in favor of static Duration::from_nanoseconds(rcl_duration_value_t) (`#1432 <https://github.com/ros2/rclcpp/issues/1432>`_)
+* Avoid parsing arguments twice in `rclcpp::init_and_remove_ros_arguments` (`#1415 <https://github.com/ros2/rclcpp/issues/1415>`_)
+* Add service and client benchmarks (`#1425 <https://github.com/ros2/rclcpp/issues/1425>`_)
+* Set CMakeLists to only use default rmw for benchmarks (`#1427 <https://github.com/ros2/rclcpp/issues/1427>`_)
+* Update tracetools' QL in rclcpp's QD (`#1428 <https://github.com/ros2/rclcpp/issues/1428>`_)
+* Add missing locking to the rclcpp_action::ServerBase. (`#1421 <https://github.com/ros2/rclcpp/issues/1421>`_)
+* Initial benchmark tests for rclcpp::init/shutdown create/destroy node (`#1411 <https://github.com/ros2/rclcpp/issues/1411>`_)
+* Refactor test CMakeLists in prep for benchmarks (`#1422 <https://github.com/ros2/rclcpp/issues/1422>`_)
+* Add methods in topic and service interface to resolve a name (`#1410 <https://github.com/ros2/rclcpp/issues/1410>`_)
+* Update deprecated gtest macros (`#1370 <https://github.com/ros2/rclcpp/issues/1370>`_)
+* Clear members for StaticExecutorEntitiesCollector to avoid shared_ptr dependency (`#1303 <https://github.com/ros2/rclcpp/issues/1303>`_)
+* Increase test timeouts of slow running tests with rmw_connext_cpp (`#1400 <https://github.com/ros2/rclcpp/issues/1400>`_)
+* Avoid self dependency that not destoryed (`#1301 <https://github.com/ros2/rclcpp/issues/1301>`_)
+* Update maintainers (`#1384 <https://github.com/ros2/rclcpp/issues/1384>`_)
+* Add clock qos to node options (`#1375 <https://github.com/ros2/rclcpp/issues/1375>`_)
+* Fix NodeOptions copy constructor (`#1376 <https://github.com/ros2/rclcpp/issues/1376>`_)
+* Make sure to clean the external client/service handle. (`#1296 <https://github.com/ros2/rclcpp/issues/1296>`_)
+* Increase coverage of WaitSetTemplate (`#1368 <https://github.com/ros2/rclcpp/issues/1368>`_)
+* Increase coverage of guard_condition.cpp to 100% (`#1369 <https://github.com/ros2/rclcpp/issues/1369>`_)
+* Add coverage statement (`#1367 <https://github.com/ros2/rclcpp/issues/1367>`_)
+* Tests for LoanedMessage with mocked loaned message publisher (`#1366 <https://github.com/ros2/rclcpp/issues/1366>`_)
+* Add unit tests for qos and qos_event files (`#1352 <https://github.com/ros2/rclcpp/issues/1352>`_)
+* Finish coverage of publisher API (`#1365 <https://github.com/ros2/rclcpp/issues/1365>`_)
+* Finish API coverage on executors. (`#1364 <https://github.com/ros2/rclcpp/issues/1364>`_)
+* Add test for ParameterService (`#1355 <https://github.com/ros2/rclcpp/issues/1355>`_)
+* Add time API coverage tests (`#1347 <https://github.com/ros2/rclcpp/issues/1347>`_)
+* Add timer coverage tests (`#1363 <https://github.com/ros2/rclcpp/issues/1363>`_)
+* Add in additional tests for parameter_client.cpp coverage.
+* Minor fixes to the parameter_service.cpp file.
+* reset rcl_context shared_ptr after calling rcl_init sucessfully (`#1357 <https://github.com/ros2/rclcpp/issues/1357>`_)
+* Improved test publisher - zero qos history depth value exception (`#1360 <https://github.com/ros2/rclcpp/issues/1360>`_)
+* Covered resolve_use_intra_process (`#1359 <https://github.com/ros2/rclcpp/issues/1359>`_)
+* Improve test_subscription_options (`#1358 <https://github.com/ros2/rclcpp/issues/1358>`_)
+* Add in more tests for init_options coverage. (`#1353 <https://github.com/ros2/rclcpp/issues/1353>`_)
+* Test the remaining node public API (`#1342 <https://github.com/ros2/rclcpp/issues/1342>`_)
+* Complete coverage of Parameter and ParameterValue API (`#1344 <https://github.com/ros2/rclcpp/issues/1344>`_)
+* Add in more tests for the utilities. (`#1349 <https://github.com/ros2/rclcpp/issues/1349>`_)
+* Add in two more tests for expand_topic_or_service_name. (`#1350 <https://github.com/ros2/rclcpp/issues/1350>`_)
+* Add tests for node_options API (`#1343 <https://github.com/ros2/rclcpp/issues/1343>`_)
+* Add in more coverage for expand_topic_or_service_name. (`#1346 <https://github.com/ros2/rclcpp/issues/1346>`_)
+* Test exception in spin_until_future_complete. (`#1345 <https://github.com/ros2/rclcpp/issues/1345>`_)
+* Add coverage tests graph_listener (`#1330 <https://github.com/ros2/rclcpp/issues/1330>`_)
+* Add in unit tests for the Executor class.
+* Allow mimick patching of methods with up to 9 arguments.
+* Improve the error messages in the Executor class.
+* Add coverage for client API (`#1329 <https://github.com/ros2/rclcpp/issues/1329>`_)
+* Increase service coverage (`#1332 <https://github.com/ros2/rclcpp/issues/1332>`_)
+* Make more of the static entity collector API private.
+* Const-ify more of the static executor.
+* Add more tests for the static single threaded executor.
+* Many more tests for the static_executor_entities_collector.
+* Get one more line of code coverage in memory_strategy.cpp
+* Bugfix when adding callback group.
+* Fix typos in comments.
+* Remove deprecated executor::FutureReturnCode APIs. (`#1327 <https://github.com/ros2/rclcpp/issues/1327>`_)
+* Increase coverage of publisher/subscription API (`#1325 <https://github.com/ros2/rclcpp/issues/1325>`_)
+* Not finalize guard condition while destructing SubscriptionIntraProcess (`#1307 <https://github.com/ros2/rclcpp/issues/1307>`_)
+* Expose qos setting for /rosout (`#1247 <https://github.com/ros2/rclcpp/issues/1247>`_)
+* Add coverage for missing API (except executors) (`#1326 <https://github.com/ros2/rclcpp/issues/1326>`_)
+* Include topic name in QoS mismatch warning messages (`#1286 <https://github.com/ros2/rclcpp/issues/1286>`_)
+* Add coverage tests context functions (`#1321 <https://github.com/ros2/rclcpp/issues/1321>`_)
+* Increase coverage of node_interfaces, including with mocking rcl errors (`#1322 <https://github.com/ros2/rclcpp/issues/1322>`_)
+* Contributors: Ada-King, Alejandro Hernández Cordero, Audrow Nash, Barry Xu, Chen Lihui, Chris Lalancette, Christophe Bedard, Ivan Santiago Paunovic, Jorge Perez, Morgan Quigley, brawner
+
 5.0.0 (2020-09-18)
 ------------------
 * Make node_graph::count_graph_users() const (`#1320 <https://github.com/ros2/rclcpp/issues/1320>`_)

rclcpp/QUALITY_DECLARATION.md

@@ -2,7 +2,7 @@ This document is a declaration of software quality for the `rclcpp` package, bas
 
 # rclcpp Quality Declaration
 
-The package `rclcpp` claims to be in the **Quality Level 3** category.
+The package `rclcpp` claims to be in the **Quality Level 2** category.
 
 Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-quality-categories) of the ROS2 developer guide.
 
@@ -155,19 +155,19 @@ It also has several test dependencies, which do not affect the resulting quality
 
 The `libstatistics_collector` package provides lightweight aggregation utilities to collect statistics and measure message metrics.
 
-It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros-tooling/libstatistics_collector/tree/master/QUALITY_DECLARATION.md).
+It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros-tooling/libstatistics_collector/tree/master/QUALITY_DECLARATION.md).
 
 #### `rcl`
 
 `rcl` a library to support implementation of language specific ROS 2 Client Libraries.
 
-It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl/QUALITY_DECLARATION.md).
+It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl/QUALITY_DECLARATION.md).
 
 #### `rcl_yaml_param_parser`
 
 The `rcl_yaml_param_parser` package provides an API that is used to parse YAML configuration files which may be used to configure ROS and specific nodes.
 
-It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros2/rcl/tree/master/rcl_yaml_param_parser/QUALITY_DECLARATION.md).
+It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros2/rcl/tree/master/rcl_yaml_param_parser/QUALITY_DECLARATION.md).
 
 #### `rcpputils`
 
@@ -191,13 +191,13 @@ It is **Quality Level 2**, see its [Quality Declaration document](https://github
 
 The `statistics_msgs` package contains ROS 2 message definitions for reporting statistics for topics and system resources.
 
-It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/statistics_msgs/QUALITY_DECLARATION.md).
+It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/statistics_msgs/QUALITY_DECLARATION.md).
 
 #### `tracetools`
 
 The `tracetools` package provides utilities for instrumenting the code in `rclcpp` so that it may be traced for debugging and performance analysis.
 
-It is **Quality Level 2**, see its [Quality Declaration document](https://gitlab.com/micro-ROS/ros_tracing/ros2_tracing/-/blob/master/tracetools/QUALITY_DECLARATION.md).
+It is **Quality Level 1**, see its [Quality Declaration document](https://gitlab.com/ros-tracing/ros2_tracing/-/blob/master/tracetools/QUALITY_DECLARATION.md).
 
 ### Direct Runtime non-ROS Dependency [5.iii]
 

rclcpp/README.md

@@ -6,4 +6,4 @@ Visit the [rclcpp API documentation](http://docs.ros2.org/latest/api/rclcpp/) fo
 
 ## Quality Declaration
 
-This package claims to be in the **Quality Level 3** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details.
+This package claims to be in the **Quality Level 2** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details.

rclcpp/include/rclcpp/duration.hpp

@@ -21,6 +21,13 @@
 #include "rcl/time.h"
 #include "rclcpp/visibility_control.hpp"
 
+// Windows has preprocessor defines for "min" and "max", which can cause
+// problems when things like std::numeric_limits<int>::max are used.  This
+// macro is used to defeat the macro matching logic and hence workaround the
+// issue.  The idea comes from:
+// https://stackoverflow.com/questions/11544073/how-do-i-deal-with-the-max-macro-in-windows-h-colliding-with-max-in-std/11550864#11550864
+#define PREVENT_WINDOWS_MIN_MAX
+
 namespace rclcpp
 {
 class RCLCPP_PUBLIC Duration
@@ -38,10 +45,14 @@ public:
    */
   Duration(int32_t seconds, uint32_t nanoseconds);
 
-  // This constructor matches any numeric value - ints or floats.
+  /// Construct duration from the specified nanoseconds.
+  [[deprecated(
+    "Use Duration::from_nanoseconds instead or std::chrono_literals. For example:"
+    "rclcpp::Duration::from_nanoseconds(int64_variable);"
+    "rclcpp::Duration(0ns);")]]
   explicit Duration(rcl_duration_value_t nanoseconds);
 
-  // This constructor matches std::chrono::nanoseconds.
+  /// Construct duration from the specified std::chrono::nanoseconds.
   explicit Duration(std::chrono::nanoseconds nanoseconds);
 
   // This constructor matches any std::chrono value other than nanoseconds
@@ -104,7 +115,7 @@ public:
    */
   static
   Duration
-  max();
+  max PREVENT_WINDOWS_MIN_MAX();
 
   Duration
   operator*(double scale) const;
@@ -129,6 +140,10 @@ public:
   static Duration
   from_seconds(double seconds);
 
+  /// Create a duration object from an integer number representing nanoseconds
+  static Duration
+  from_nanoseconds(rcl_duration_value_t nanoseconds);
+
   /// Convert Duration into a std::chrono::Duration.
   template<class DurationT>
   DurationT
@@ -143,6 +158,8 @@ public:
 
 private:
   rcl_duration_t rcl_duration_;
+
+  Duration() = default;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

@@ -207,7 +207,7 @@ public:
       sub_ids.take_shared_subscriptions.size() <= 1)
     {
       // There is at maximum 1 buffer that does not require ownership.
-      // So we this case is equivalent to all the buffers requiring ownership
+      // So this case is equivalent to all the buffers requiring ownership
 
       // Merge the two vector of ids into a unique one
       std::vector<uint64_t> concatenated_vector(sub_ids.take_shared_subscriptions);

rclcpp/include/rclcpp/graph_listener.hpp

@@ -160,6 +160,12 @@ protected:
   void
   run_loop();
 
+  RCLCPP_PUBLIC
+  void init_wait_set();
+
+  RCLCPP_PUBLIC
+  void cleanup_wait_set();
+
 private:
   RCLCPP_DISABLE_COPY(GraphListener)
 

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -118,6 +118,10 @@ public:
   bool
   get_enable_topic_statistics_default() const override;
 
+  std::string
+  resolve_topic_or_service_name(
+    const std::string & name, bool is_service, bool only_expand = false) const override;
+
 private:
   RCLCPP_DISABLE_COPY(NodeBase)
 

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -163,6 +163,13 @@ public:
   virtual
   bool
   get_enable_topic_statistics_default() const = 0;
+
+  /// Expand and remap a given topic or service name.
+  RCLCPP_PUBLIC
+  virtual
+  std::string
+  resolve_topic_or_service_name(
+    const std::string & name, bool is_service, bool only_expand = false) const = 0;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_services.hpp

@@ -15,6 +15,8 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_SERVICES_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_SERVICES_HPP_
 
+#include <string>
+
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/client.hpp"
 #include "rclcpp/macros.hpp"
@@ -53,6 +55,10 @@ public:
     rclcpp::ServiceBase::SharedPtr service_base_ptr,
     rclcpp::CallbackGroup::SharedPtr group) override;
 
+  RCLCPP_PUBLIC
+  std::string
+  resolve_service_name(const std::string & name, bool only_expand = false) const override;
+
 private:
   RCLCPP_DISABLE_COPY(NodeServices)
 

rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp

@@ -15,6 +15,8 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_
 
+#include <string>
+
 #include "rclcpp/callback_group.hpp"
 #include "rclcpp/client.hpp"
 #include "rclcpp/macros.hpp"
@@ -49,6 +51,12 @@ public:
   add_service(
     rclcpp::ServiceBase::SharedPtr service_base_ptr,
     rclcpp::CallbackGroup::SharedPtr group) = 0;
+
+  /// Get the remapped and expanded service name given a input name.
+  RCLCPP_PUBLIC
+  virtual
+  std::string
+  resolve_service_name(const std::string & name, bool only_expand = false) const = 0;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_topics.hpp

@@ -81,6 +81,10 @@ public:
   rclcpp::node_interfaces::NodeTimersInterface *
   get_node_timers_interface() const override;
 
+  RCLCPP_PUBLIC
+  std::string
+  resolve_topic_name(const std::string & name, bool only_expand = false) const override;
+
 private:
   RCLCPP_DISABLE_COPY(NodeTopics)
 

rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp

@@ -86,6 +86,12 @@ public:
   virtual
   rclcpp::node_interfaces::NodeTimersInterface *
   get_node_timers_interface() const = 0;
+
+  /// Get a remapped and expanded topic name given an input name.
+  RCLCPP_PUBLIC
+  virtual
+  std::string
+  resolve_topic_name(const std::string & name, bool only_expand = false) const = 0;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/time.hpp

@@ -23,6 +23,13 @@
 
 #include "rclcpp/duration.hpp"
 
+// Windows has preprocessor defines for "min" and "max", which can cause
+// problems when things like std::numeric_limits<int>::max are used.  This
+// macro is used to defeat the macro matching logic and hence workaround the
+// issue.  The idea comes from:
+// https://stackoverflow.com/questions/11544073/how-do-i-deal-with-the-max-macro-in-windows-h-colliding-with-max-in-std/11550864#11550864
+#define PREVENT_WINDOWS_MIN_MAX
+
 namespace rclcpp
 {
 
@@ -189,7 +196,7 @@ public:
    */
   RCLCPP_PUBLIC
   static Time
-  max();
+  max PREVENT_WINDOWS_MIN_MAX();
 
   /// Get the seconds since epoch
   /**

rclcpp/package.xml

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

@@ -37,7 +37,7 @@ Duration::Duration(int32_t seconds, uint32_t nanoseconds)
   rcl_duration_.nanoseconds += nanoseconds;
 }
 
-Duration::Duration(int64_t nanoseconds)
+Duration::Duration(rcl_duration_value_t nanoseconds)
 {
   rcl_duration_.nanoseconds = nanoseconds;
 }
@@ -148,7 +148,7 @@ Duration::operator+(const rclcpp::Duration & rhs) const
     this->rcl_duration_.nanoseconds,
     rhs.rcl_duration_.nanoseconds,
     std::numeric_limits<rcl_duration_value_t>::max());
-  return Duration(
+  return Duration::from_nanoseconds(
     rcl_duration_.nanoseconds + rhs.rcl_duration_.nanoseconds);
 }
 
@@ -177,7 +177,7 @@ Duration::operator-(const rclcpp::Duration & rhs) const
     rhs.rcl_duration_.nanoseconds,
     std::numeric_limits<rcl_duration_value_t>::max());
 
-  return Duration(
+  return Duration::from_nanoseconds(
     rcl_duration_.nanoseconds - rhs.rcl_duration_.nanoseconds);
 }
 
@@ -208,7 +208,7 @@ Duration::operator*(double scale) const
     scale,
     std::numeric_limits<rcl_duration_value_t>::max());
   long double scale_ld = static_cast<long double>(scale);
-  return Duration(
+  return Duration::from_nanoseconds(
     static_cast<rcl_duration_value_t>(
       static_cast<long double>(rcl_duration_.nanoseconds) * scale_ld));
 }
@@ -249,7 +249,17 @@ Duration::to_rmw_time() const
 Duration
 Duration::from_seconds(double seconds)
 {
-  return Duration(static_cast<int64_t>(RCL_S_TO_NS(seconds)));
+  Duration ret;
+  ret.rcl_duration_.nanoseconds = static_cast<int64_t>(RCL_S_TO_NS(seconds));
+  return ret;
+}
+
+Duration
+Duration::from_nanoseconds(rcl_duration_value_t nanoseconds)
+{
+  Duration ret;
+  ret.rcl_duration_.nanoseconds = nanoseconds;
+  return ret;
 }
 
 }  // namespace rclcpp

rclcpp/src/rclcpp/executor.cpp

@@ -25,6 +25,7 @@
 
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/executor.hpp"
+#include "rclcpp/memory_strategy.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/scope_exit.hpp"
 #include "rclcpp/utilities.hpp"
@@ -195,7 +196,7 @@ void
 Executor::add_callback_group_to_map(
   rclcpp::CallbackGroup::SharedPtr group_ptr,
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-  WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
+  rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
   bool notify)
 {
   // If the callback_group already has an executor
@@ -269,7 +270,7 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt
 void
 Executor::remove_callback_group_from_map(
   rclcpp::CallbackGroup::SharedPtr group_ptr,
-  WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
+  rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
   bool notify)
 {
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr;
@@ -342,27 +343,24 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node
   if (!found_node) {
     throw std::runtime_error("Node needs to be associated with this executor.");
   }
-  std::vector<rclcpp::CallbackGroup::SharedPtr> found_group_ptrs;
-  std::for_each(
-    weak_groups_to_nodes_associated_with_executor_.begin(),
-    weak_groups_to_nodes_associated_with_executor_.end(),
-    [&found_group_ptrs, node_ptr](std::pair<rclcpp::CallbackGroup::WeakPtr,
-    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> key_value_pair) {
-      auto weak_node_ptr = key_value_pair.second;
-      auto shared_node_ptr = weak_node_ptr.lock();
-      auto group_ptr = key_value_pair.first.lock();
-      if (shared_node_ptr == node_ptr) {
-        found_group_ptrs.push_back(group_ptr);
-      }
-    });
-  std::for_each(
-    found_group_ptrs.begin(), found_group_ptrs.end(), [this, notify]
-      (rclcpp::CallbackGroup::SharedPtr group_ptr) {
+
+  for (auto it = weak_groups_to_nodes_associated_with_executor_.begin();
+    it != weak_groups_to_nodes_associated_with_executor_.end(); )
+  {
+    auto weak_node_ptr = it->second;
+    auto shared_node_ptr = weak_node_ptr.lock();
+    auto group_ptr = it->first.lock();
+
+    // Increment iterator before removing in case it's invalidated
+    it++;
+    if (shared_node_ptr == node_ptr) {
       remove_callback_group_from_map(
         group_ptr,
         weak_groups_to_nodes_associated_with_executor_,
         notify);
-    });
+    }
+  }
+
   std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
   has_executor.store(false);
 }
@@ -738,7 +736,7 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
 
 rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
 Executor::get_node_by_group(
-  WeakCallbackGroupsToNodesMap weak_groups_to_nodes,
+  rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap weak_groups_to_nodes,
   rclcpp::CallbackGroup::SharedPtr group)
 {
   if (!group) {
@@ -796,7 +794,7 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable)
 bool
 Executor::get_next_ready_executable_from_map(
   AnyExecutable & any_executable,
-  WeakCallbackGroupsToNodesMap weak_groups_to_nodes)
+  rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap weak_groups_to_nodes)
 {
   bool success = false;
   // Check the timers to see if there are any that are ready
@@ -884,7 +882,7 @@ Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanos
 bool
 Executor::has_node(
   const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-  WeakCallbackGroupsToNodesMap weak_groups_to_nodes) const
+  rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap weak_groups_to_nodes) const
 {
   return std::find_if(
     weak_groups_to_nodes.begin(),

rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp

@@ -155,7 +155,8 @@ StaticExecutorEntitiesCollector::fill_executable_list()
 }
 void
 StaticExecutorEntitiesCollector::fill_executable_list_from_map(
-  const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
+  const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap &
+  weak_groups_to_nodes)
 {
   for (const auto & pair : weak_groups_to_nodes) {
     auto group = pair.first.lock();
@@ -297,7 +298,7 @@ bool
 StaticExecutorEntitiesCollector::add_callback_group(
   rclcpp::CallbackGroup::SharedPtr group_ptr,
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-  WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
+  rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
 {
   // If the callback_group already has an executor
   std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic();
@@ -341,7 +342,7 @@ StaticExecutorEntitiesCollector::remove_callback_group(
 bool
 StaticExecutorEntitiesCollector::remove_callback_group_from_map(
   rclcpp::CallbackGroup::SharedPtr group_ptr,
-  WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
+  rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
 {
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr;
   rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr;
@@ -437,7 +438,8 @@ StaticExecutorEntitiesCollector::is_ready(rcl_wait_set_t * p_wait_set)
 bool
 StaticExecutorEntitiesCollector::has_node(
   const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-  const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) const
+  const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap &
+  weak_groups_to_nodes) const
 {
   return std::find_if(
     weak_groups_to_nodes.begin(),

rclcpp/src/rclcpp/graph_listener.cpp

@@ -62,6 +62,27 @@ GraphListener::~GraphListener()
   this->shutdown(std::nothrow);
 }
 
+void GraphListener::init_wait_set()
+{
+  auto parent_context = parent_context_.lock();
+  if (!parent_context) {
+    throw std::runtime_error("parent context was destroyed");
+  }
+  rcl_ret_t ret = rcl_wait_set_init(
+    &wait_set_,
+    0,  // number_of_subscriptions
+    2,  // number_of_guard_conditions
+    0,  // number_of_timers
+    0,  // number_of_clients
+    0,  // number_of_services
+    0,  // number_of_events
+    parent_context->get_rcl_context().get(),
+    rcl_get_default_allocator());
+  if (RCL_RET_OK != ret) {
+    throw_from_rcl_error(ret, "failed to initialize wait set");
+  }
+}
+
 void
 GraphListener::start_if_not_started()
 {
@@ -71,24 +92,8 @@ GraphListener::start_if_not_started()
   }
   if (!is_started_) {
     // Initialize the wait set before starting.
-    auto parent_context = parent_context_.lock();
-    if (!parent_context) {
-      throw std::runtime_error("parent context was destroyed");
-    }
-    rcl_ret_t ret = rcl_wait_set_init(
-      &wait_set_,
-      0,  // number_of_subscriptions
-      2,  // number_of_guard_conditions
-      0,  // number_of_timers
-      0,  // number_of_clients
-      0,  // number_of_services
-      0,  // number_of_events
-      parent_context->get_rcl_context().get(),
-      rcl_get_default_allocator());
-    if (RCL_RET_OK != ret) {
-      throw_from_rcl_error(ret, "failed to initialize wait set");
-    }
-    // Register an on_shutdown hook to shtudown the graph listener.
+    init_wait_set();
+    // Register an on_shutdown hook to shutdown the graph listener.
     // This is important to ensure that the wait set is finalized before
     // destruction of static objects occurs.
     std::weak_ptr<GraphListener> weak_this = shared_from_this();
@@ -350,6 +355,15 @@ GraphListener::remove_node(rclcpp::node_interfaces::NodeGraphInterface * node_gr
   remove_node_(&node_graph_interfaces_, node_graph);
 }
 
+void
+GraphListener::cleanup_wait_set()
+{
+  rcl_ret_t ret = rcl_wait_set_fini(&wait_set_);
+  if (RCL_RET_OK != ret) {
+    throw_from_rcl_error(ret, "failed to finalize wait set");
+  }
+}
+
 void
 GraphListener::__shutdown(bool should_throw)
 {
@@ -386,10 +400,7 @@ GraphListener::__shutdown(bool should_throw)
       shutdown_guard_condition_ = nullptr;
     }
     if (is_started_) {
-      ret = rcl_wait_set_fini(&wait_set_);
-      if (RCL_RET_OK != ret) {
-        throw_from_rcl_error(ret, "failed to finalize wait set");
-      }
+      cleanup_wait_set();
     }
   }
 }

rclcpp/src/rclcpp/node_interfaces/node_base.cpp

@@ -288,3 +288,24 @@ NodeBase::get_enable_topic_statistics_default() const
 {
   return enable_topic_statistics_default_;
 }
+
+std::string
+NodeBase::resolve_topic_or_service_name(
+  const std::string & name, bool is_service, bool only_expand) const
+{
+  char * output_cstr = NULL;
+  auto allocator = rcl_get_default_allocator();
+  rcl_ret_t ret = rcl_node_resolve_name(
+    node_handle_.get(),
+    name.c_str(),
+    allocator,
+    is_service,
+    only_expand,
+    &output_cstr);
+  if (RCL_RET_OK != ret) {
+    throw_from_rcl_error(ret, "failed to resolve name", rcl_get_error_state());
+  }
+  std::string output{output_cstr};
+  allocator.deallocate(output_cstr, allocator.state);
+  return output;
+}

rclcpp/src/rclcpp/node_interfaces/node_services.cpp

@@ -78,3 +78,9 @@ NodeServices::add_client(
     }
   }
 }
+
+std::string
+NodeServices::resolve_service_name(const std::string & name, bool only_expand) const
+{
+  return node_base_->resolve_topic_or_service_name(name, true, only_expand);
+}

rclcpp/src/rclcpp/node_interfaces/node_topics.cpp

@@ -129,3 +129,9 @@ NodeTopics::get_node_timers_interface() const
 {
   return node_timers_;
 }
+
+std::string
+NodeTopics::resolve_topic_name(const std::string & name, bool only_expand) const
+{
+  return node_base_->resolve_topic_or_service_name(name, false, only_expand);
+}

rclcpp/src/rclcpp/time.cpp

@@ -199,7 +199,7 @@ Time::operator-(const rclcpp::Time & rhs) const
     throw std::underflow_error("time subtraction leads to int64_t underflow");
   }
 
-  return Duration(rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds);
+  return Duration::from_nanoseconds(rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds);
 }
 
 Time

rclcpp/src/rclcpp/utilities.cpp

@@ -21,6 +21,7 @@
 
 #include "./signal_handler.hpp"
 #include "rclcpp/contexts/default_context.hpp"
+#include "rclcpp/detail/utilities.hpp"
 #include "rclcpp/exceptions.hpp"
 
 #include "rcl/error_handling.h"
@@ -56,6 +57,47 @@ uninstall_signal_handlers()
   return SignalHandler::get_global_signal_handler().uninstall();
 }
 
+static
+std::vector<std::string>
+_remove_ros_arguments(
+  char const * const argv[],
+  const rcl_arguments_t * args,
+  rcl_allocator_t alloc)
+{
+  rcl_ret_t ret;
+  int nonros_argc = 0;
+  const char ** nonros_argv = NULL;
+
+  ret = rcl_remove_ros_arguments(
+    argv,
+    args,
+    alloc,
+    &nonros_argc,
+    &nonros_argv);
+
+  if (RCL_RET_OK != ret || nonros_argc < 0) {
+    // Not using throw_from_rcl_error, because we may need to append deallocation failures.
+    exceptions::RCLError exc(ret, rcl_get_error_state(), "");
+    rcl_reset_error();
+    if (NULL != nonros_argv) {
+      alloc.deallocate(nonros_argv, alloc.state);
+    }
+    throw exc;
+  }
+
+  std::vector<std::string> return_arguments(static_cast<size_t>(nonros_argc));
+
+  for (size_t ii = 0; ii < static_cast<size_t>(nonros_argc); ++ii) {
+    return_arguments[ii] = std::string(nonros_argv[ii]);
+  }
+
+  if (NULL != nonros_argv) {
+    alloc.deallocate(nonros_argv, alloc.state);
+  }
+
+  return return_arguments;
+}
+
 std::vector<std::string>
 init_and_remove_ros_arguments(
   int argc,
@@ -63,7 +105,10 @@ init_and_remove_ros_arguments(
   const InitOptions & init_options)
 {
   init(argc, argv, init_options);
-  return remove_ros_arguments(argc, argv);
+
+  using rclcpp::contexts::get_global_default_context;
+  auto rcl_context = get_global_default_context()->get_rcl_context();
+  return _remove_ros_arguments(argv, &(rcl_context->global_arguments), rcl_get_default_allocator());
 }
 
 std::vector<std::string>
@@ -79,40 +124,17 @@ remove_ros_arguments(int argc, char const * const argv[])
     exceptions::throw_from_rcl_error(ret, "failed to parse arguments");
   }
 
-  int nonros_argc = 0;
-  const char ** nonros_argv = NULL;
-
-  ret = rcl_remove_ros_arguments(
-    argv,
-    &parsed_args,
-    alloc,
-    &nonros_argc,
-    &nonros_argv);
-
-  if (RCL_RET_OK != ret || nonros_argc < 0) {
-    // Not using throw_from_rcl_error, because we may need to append deallocation failures.
-    exceptions::RCLErrorBase base_exc(ret, rcl_get_error_state());
-    rcl_reset_error();
-    if (NULL != nonros_argv) {
-      alloc.deallocate(nonros_argv, alloc.state);
-    }
+  std::vector<std::string> return_arguments;
+  try {
+    return_arguments = _remove_ros_arguments(argv, &parsed_args, alloc);
+  } catch (exceptions::RCLError & exc) {
     if (RCL_RET_OK != rcl_arguments_fini(&parsed_args)) {
-      base_exc.formatted_message += std::string(
+      exc.formatted_message += std::string(
         ", failed also to cleanup parsed arguments, leaking memory: ") +
         rcl_get_error_string().str;
       rcl_reset_error();
     }
-    throw exceptions::RCLError(base_exc, "");
-  }
-
-  std::vector<std::string> return_arguments(static_cast<size_t>(nonros_argc));
-
-  for (size_t ii = 0; ii < static_cast<size_t>(nonros_argc); ++ii) {
-    return_arguments[ii] = std::string(nonros_argv[ii]);
-  }
-
-  if (NULL != nonros_argv) {
-    alloc.deallocate(nonros_argv, alloc.state);
+    throw exc;
   }
 
   ret = rcl_arguments_fini(&parsed_args);

rclcpp/test/CMakeLists.txt

@@ -1,667 +1,17 @@
-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
-  DEPENDENCIES builtin_interfaces
-  LIBRARY_NAME ${PROJECT_NAME}
-  SKIP_INSTALL
-)
-
-ament_add_gtest(
-  test_allocator_common
-  rclcpp/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
-  rclcpp/allocator/test_allocator_deleter.cpp)
-if(TARGET test_allocator_deleter)
-  target_link_libraries(test_allocator_deleter ${PROJECT_NAME})
-endif()
-ament_add_gtest(
-  test_exceptions
-  rclcpp/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
-  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"
-    "test_msgs"
-  )
-  target_link_libraries(test_client ${PROJECT_NAME} mimick)
-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
-  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 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} mimick)
-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_gtest(
-  test_future_return_code
-  rclcpp/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 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} mimick)
-
-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} mimick)
-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} mimick)
-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
-  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
-  rclcpp/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
-  rclcpp/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
-  rclcpp/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
-  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} mimick)
-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} 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
-#   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} mimick)
-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} mimick)
-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_service rclcpp/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 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 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 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}
-    mimick
-  )
-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} mimick)
-endif()
-# Creating and destroying nodes is slow with Connext, so this needs larger timeout.
-ament_add_gtest(test_subscription rclcpp/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 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} mimick)
-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} mimick)
-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)
-  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 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}" 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 rclcpp/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 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(benchmark)
+add_subdirectory(rclcpp)
 
 ament_add_gtest(test_rclcpp_gtest_macros utils/test_rclcpp_gtest_macros.cpp)
 if(TARGET test_rclcpp_gtest_macros)
   target_link_libraries(test_rclcpp_gtest_macros ${PROJECT_NAME})
 endif()
 
-ament_add_gtest(test_rosout_qos rclcpp/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 rclcpp/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 rclcpp/test_graph_listener.cpp)
-if(TARGET test_graph_listener)
-  target_link_libraries(test_graph_listener ${PROJECT_NAME} mimick)
-endif()
-
 # Install test resources
 install(
   DIRECTORY resources

rclcpp/test/benchmark/CMakeLists.txt

@@ -0,0 +1,38 @@
+find_package(performance_test_fixture REQUIRED)
+
+# These benchmarks are only being created and run for the default RMW
+# implementation. We are looking to test the performance of the ROS 2 code, not
+# the underlying middleware.
+
+add_performance_test(benchmark_client benchmark_client.cpp)
+if(TARGET benchmark_client)
+  target_link_libraries(benchmark_client ${PROJECT_NAME})
+  ament_target_dependencies(benchmark_client test_msgs rcl_interfaces)
+endif()
+
+add_performance_test(benchmark_executor benchmark_executor.cpp)
+if(TARGET benchmark_executor)
+  target_link_libraries(benchmark_executor ${PROJECT_NAME})
+  ament_target_dependencies(benchmark_executor test_msgs)
+endif()
+
+add_performance_test(benchmark_init_shutdown benchmark_init_shutdown.cpp)
+if(TARGET benchmark_init_shutdown)
+  target_link_libraries(benchmark_init_shutdown ${PROJECT_NAME})
+endif()
+
+add_performance_test(benchmark_node benchmark_node.cpp)
+if(TARGET benchmark_node)
+  target_link_libraries(benchmark_node ${PROJECT_NAME})
+endif()
+
+add_performance_test(benchmark_node_parameters_interface benchmark_node_parameters_interface.cpp)
+if(TARGET benchmark_node_parameters_interface)
+  target_link_libraries(benchmark_node_parameters_interface ${PROJECT_NAME})
+endif()
+
+add_performance_test(benchmark_service benchmark_service.cpp)
+if(TARGET benchmark_service)
+  target_link_libraries(benchmark_service ${PROJECT_NAME})
+  ament_target_dependencies(benchmark_service test_msgs rcl_interfaces)
+endif()

rclcpp/test/benchmark/benchmark_client.cpp

@@ -0,0 +1,153 @@
+// 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 <memory>
+#include <string>
+
+#include "performance_test_fixture/performance_test_fixture.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "test_msgs/srv/empty.hpp"
+
+using performance_test_fixture::PerformanceTest;
+
+constexpr char empty_service_name[] = "empty_service";
+
+class ClientPerformanceTest : public PerformanceTest
+{
+public:
+  explicit ClientPerformanceTest(rclcpp::NodeOptions = rclcpp::NodeOptions()) {}
+  void SetUp(benchmark::State & state)
+  {
+    rclcpp::init(0, nullptr);
+    node = std::make_unique<rclcpp::Node>("node", "ns");
+
+    auto empty_service_callback =
+      [](
+      const test_msgs::srv::Empty::Request::SharedPtr,
+      test_msgs::srv::Empty::Response::SharedPtr) {};
+    empty_service =
+      node->create_service<test_msgs::srv::Empty>(empty_service_name, empty_service_callback);
+
+    performance_test_fixture::PerformanceTest::SetUp(state);
+  }
+
+  void TearDown(benchmark::State & state)
+  {
+    performance_test_fixture::PerformanceTest::TearDown(state);
+    empty_service.reset();
+    node.reset();
+    rclcpp::shutdown();
+  }
+
+protected:
+  std::unique_ptr<rclcpp::Node> node;
+  std::shared_ptr<rclcpp::Service<test_msgs::srv::Empty>> empty_service;
+};
+
+BENCHMARK_F(ClientPerformanceTest, construct_client_no_service)(benchmark::State & state) {
+  // Prime cache
+  auto outer_client = node->create_client<test_msgs::srv::Empty>("not_an_existing_service");
+  outer_client.reset();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    auto client = node->create_client<test_msgs::srv::Empty>("not_an_existing_service");
+    benchmark::DoNotOptimize(client);
+    benchmark::ClobberMemory();
+
+    state.PauseTiming();
+    client.reset();
+    state.ResumeTiming();
+  }
+}
+
+BENCHMARK_F(ClientPerformanceTest, construct_client_empty_srv)(benchmark::State & state) {
+  // Prime cache
+  auto outer_client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
+  outer_client.reset();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    auto client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
+    benchmark::DoNotOptimize(client);
+    benchmark::ClobberMemory();
+
+    state.PauseTiming();
+    client.reset();
+    state.ResumeTiming();
+  }
+}
+
+BENCHMARK_F(ClientPerformanceTest, destroy_client_empty_srv)(benchmark::State & state) {
+  // Prime cache
+  auto outer_client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
+  outer_client.reset();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    state.PauseTiming();
+    auto client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
+    state.ResumeTiming();
+    benchmark::DoNotOptimize(client);
+    benchmark::ClobberMemory();
+
+    client.reset();
+  }
+}
+
+BENCHMARK_F(ClientPerformanceTest, wait_for_service)(benchmark::State & state) {
+  int count = 0;
+  for (auto _ : state) {
+    state.PauseTiming();
+    const std::string service_name = std::string("service_") + std::to_string(count++);
+    // Create client before service so it has to 'discover' the service after construction
+    auto client = node->create_client<test_msgs::srv::Empty>(service_name);
+    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, callback);
+    state.ResumeTiming();
+
+    client->wait_for_service(std::chrono::seconds(1));
+    benchmark::ClobberMemory();
+  }
+}
+
+BENCHMARK_F(ClientPerformanceTest, async_send_request_only)(benchmark::State & state) {
+  auto client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
+  auto shared_request = std::make_shared<test_msgs::srv::Empty::Request>();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    auto future = client->async_send_request(shared_request);
+    benchmark::DoNotOptimize(future);
+    benchmark::ClobberMemory();
+  }
+}
+
+BENCHMARK_F(ClientPerformanceTest, async_send_request_and_response)(benchmark::State & state) {
+  auto client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
+  auto shared_request = std::make_shared<test_msgs::srv::Empty::Request>();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    auto future = client->async_send_request(shared_request);
+    rclcpp::spin_until_future_complete(
+      node->get_node_base_interface(), future, std::chrono::seconds(1));
+    benchmark::DoNotOptimize(future);
+    benchmark::ClobberMemory();
+  }
+}

rclcpp/test/benchmark/benchmark_executor.cpp

@@ -0,0 +1,389 @@
+// 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 <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "performance_test_fixture/performance_test_fixture.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/scope_exit.hpp"
+#include "test_msgs/msg/empty.hpp"
+
+using namespace std::chrono_literals;
+using performance_test_fixture::PerformanceTest;
+
+constexpr unsigned int kNumberOfNodes = 10;
+
+class PerformanceTestExecutor : public PerformanceTest
+{
+public:
+  void SetUp(benchmark::State & st)
+  {
+    rclcpp::init(0, nullptr);
+    callback_count = 0;
+    for (unsigned int i = 0u; i < kNumberOfNodes; i++) {
+      nodes.push_back(std::make_shared<rclcpp::Node>("my_node_" + std::to_string(i)));
+
+      publishers.push_back(
+        nodes[i]->create_publisher<test_msgs::msg::Empty>(
+          "/empty_msgs_" + std::to_string(i), rclcpp::QoS(10)));
+
+      auto callback = [this](test_msgs::msg::Empty::SharedPtr) {this->callback_count++;};
+      subscriptions.push_back(
+        nodes[i]->create_subscription<test_msgs::msg::Empty>(
+          "/empty_msgs_" + std::to_string(i), rclcpp::QoS(10), std::move(callback)));
+    }
+    PerformanceTest::SetUp(st);
+  }
+  void TearDown(benchmark::State & st)
+  {
+    PerformanceTest::TearDown(st);
+    subscriptions.clear();
+    publishers.clear();
+    nodes.clear();
+    rclcpp::shutdown();
+  }
+
+  test_msgs::msg::Empty empty_msgs;
+  std::vector<rclcpp::Node::SharedPtr> nodes;
+  std::vector<rclcpp::Publisher<test_msgs::msg::Empty>::SharedPtr> publishers;
+  std::vector<rclcpp::Subscription<test_msgs::msg::Empty>::SharedPtr> subscriptions;
+  int callback_count;
+};
+
+BENCHMARK_F(PerformanceTestExecutor, single_thread_executor_spin_some)(benchmark::State & st)
+{
+  rclcpp::executors::SingleThreadedExecutor executor;
+  for (unsigned int i = 0u; i < kNumberOfNodes; i++) {
+    executor.add_node(nodes[i]);
+    publishers[i]->publish(empty_msgs);
+    executor.spin_some(100ms);
+  }
+
+  callback_count = 0;
+  reset_heap_counters();
+
+  for (auto _ : st) {
+    st.PauseTiming();
+    for (unsigned int i = 0u; i < kNumberOfNodes; i++) {
+      publishers[i]->publish(empty_msgs);
+    }
+    st.ResumeTiming();
+
+    executor.spin_some(100ms);
+  }
+  if (callback_count == 0) {
+    st.SkipWithError("No message was received");
+  }
+}
+
+BENCHMARK_F(PerformanceTestExecutor, multi_thread_executor_spin_some)(benchmark::State & st)
+{
+  rclcpp::executors::MultiThreadedExecutor executor;
+  for (unsigned int i = 0u; i < kNumberOfNodes; i++) {
+    executor.add_node(nodes[i]);
+    publishers[i]->publish(empty_msgs);
+    executor.spin_some(100ms);
+  }
+
+  callback_count = 0;
+  reset_heap_counters();
+
+  for (auto _ : st) {
+    st.PauseTiming();
+    for (unsigned int i = 0u; i < kNumberOfNodes; i++) {
+      publishers[i]->publish(empty_msgs);
+    }
+    st.ResumeTiming();
+
+    executor.spin_some(100ms);
+  }
+  if (callback_count == 0) {
+    st.SkipWithError("No message was received");
+  }
+}
+
+class PerformanceTestExecutorSimple : public PerformanceTest
+{
+public:
+  void SetUp(benchmark::State & st)
+  {
+    rclcpp::init(0, nullptr);
+    node = std::make_shared<rclcpp::Node>("my_node");
+
+    PerformanceTest::SetUp(st);
+  }
+  void TearDown(benchmark::State & st)
+  {
+    PerformanceTest::TearDown(st);
+    node.reset();
+    rclcpp::shutdown();
+  }
+
+  rclcpp::Node::SharedPtr node;
+};
+
+
+BENCHMARK_F(PerformanceTestExecutorSimple, single_thread_executor_add_node)(benchmark::State & st)
+{
+  rclcpp::executors::SingleThreadedExecutor executor;
+  for (auto _ : st) {
+    executor.add_node(node);
+    st.PauseTiming();
+    executor.remove_node(node);
+    st.ResumeTiming();
+  }
+}
+
+BENCHMARK_F(
+  PerformanceTestExecutorSimple, single_thread_executor_remove_node)(benchmark::State & st)
+{
+  rclcpp::executors::SingleThreadedExecutor executor;
+  for (auto _ : st) {
+    st.PauseTiming();
+    executor.add_node(node);
+    st.ResumeTiming();
+    executor.remove_node(node);
+  }
+}
+
+BENCHMARK_F(PerformanceTestExecutorSimple, multi_thread_executor_add_node)(benchmark::State & st)
+{
+  rclcpp::executors::MultiThreadedExecutor executor;
+  for (auto _ : st) {
+    executor.add_node(node);
+    st.PauseTiming();
+    executor.remove_node(node);
+    st.ResumeTiming();
+  }
+}
+
+BENCHMARK_F(PerformanceTestExecutorSimple, multi_thread_executor_remove_node)(benchmark::State & st)
+{
+  rclcpp::executors::MultiThreadedExecutor executor;
+  for (auto _ : st) {
+    st.PauseTiming();
+    executor.add_node(node);
+    st.ResumeTiming();
+    executor.remove_node(node);
+  }
+}
+
+BENCHMARK_F(
+  PerformanceTestExecutorSimple,
+  static_single_thread_executor_add_node)(benchmark::State & st)
+{
+  rclcpp::executors::StaticSingleThreadedExecutor executor;
+  for (auto _ : st) {
+    executor.add_node(node);
+    st.PauseTiming();
+    executor.remove_node(node);
+    st.ResumeTiming();
+  }
+}
+
+BENCHMARK_F(
+  PerformanceTestExecutorSimple,
+  static_single_thread_executor_remove_node)(benchmark::State & st)
+{
+  rclcpp::executors::StaticSingleThreadedExecutor executor;
+  for (auto _ : st) {
+    st.PauseTiming();
+    executor.add_node(node);
+    st.ResumeTiming();
+    executor.remove_node(node);
+  }
+}
+
+BENCHMARK_F(
+  PerformanceTestExecutorSimple,
+  static_single_thread_executor_spin_until_future_complete)(benchmark::State & st)
+{
+  rclcpp::executors::StaticSingleThreadedExecutor executor;
+  // test success of an immediately finishing future
+  std::promise<bool> promise;
+  std::future<bool> future = promise.get_future();
+  promise.set_value(true);
+  auto shared_future = future.share();
+
+  auto ret = executor.spin_until_future_complete(shared_future, 100ms);
+  if (ret != rclcpp::FutureReturnCode::SUCCESS) {
+    st.SkipWithError(rcutils_get_error_string().str);
+  }
+
+  reset_heap_counters();
+
+  for (auto _ : st) {
+    // static_single_thread_executor has a special design. We need to add/remove the node each
+    // time you call spin
+    st.PauseTiming();
+    executor.add_node(node);
+    st.ResumeTiming();
+
+    ret = executor.spin_until_future_complete(shared_future, 100ms);
+    if (ret != rclcpp::FutureReturnCode::SUCCESS) {
+      st.SkipWithError(rcutils_get_error_string().str);
+      break;
+    }
+    st.PauseTiming();
+    executor.remove_node(node);
+    st.ResumeTiming();
+  }
+}
+
+BENCHMARK_F(
+  PerformanceTestExecutorSimple,
+  single_thread_executor_spin_node_until_future_complete)(benchmark::State & st)
+{
+  rclcpp::executors::SingleThreadedExecutor executor;
+  // test success of an immediately finishing future
+  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, node, shared_future, 1s);
+  if (ret != rclcpp::FutureReturnCode::SUCCESS) {
+    st.SkipWithError(rcutils_get_error_string().str);
+  }
+
+  reset_heap_counters();
+
+  for (auto _ : st) {
+    ret = rclcpp::executors::spin_node_until_future_complete(
+      executor, node, shared_future, 1s);
+    if (ret != rclcpp::FutureReturnCode::SUCCESS) {
+      st.SkipWithError(rcutils_get_error_string().str);
+      break;
+    }
+  }
+}
+
+BENCHMARK_F(
+  PerformanceTestExecutorSimple,
+  multi_thread_executor_spin_node_until_future_complete)(benchmark::State & st)
+{
+  rclcpp::executors::MultiThreadedExecutor executor;
+  // test success of an immediately finishing future
+  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, node, shared_future, 1s);
+  if (ret != rclcpp::FutureReturnCode::SUCCESS) {
+    st.SkipWithError(rcutils_get_error_string().str);
+  }
+
+  reset_heap_counters();
+
+  for (auto _ : st) {
+    ret = rclcpp::executors::spin_node_until_future_complete(
+      executor, node, shared_future, 1s);
+    if (ret != rclcpp::FutureReturnCode::SUCCESS) {
+      st.SkipWithError(rcutils_get_error_string().str);
+      break;
+    }
+  }
+}
+
+BENCHMARK_F(
+  PerformanceTestExecutorSimple,
+  static_single_thread_executor_spin_node_until_future_complete)(benchmark::State & st)
+{
+  rclcpp::executors::StaticSingleThreadedExecutor executor;
+  // test success of an immediately finishing future
+  std::promise<bool> promise;
+  std::future<bool> future = promise.get_future();
+  promise.set_value(true);
+  auto shared_future = future.share();
+
+  reset_heap_counters();
+
+  for (auto _ : st) {
+    auto ret = rclcpp::executors::spin_node_until_future_complete(
+      executor, node, shared_future, 1s);
+    if (ret != rclcpp::FutureReturnCode::SUCCESS) {
+      st.SkipWithError(rcutils_get_error_string().str);
+      break;
+    }
+  }
+}
+
+BENCHMARK_F(PerformanceTestExecutorSimple, spin_until_future_complete)(benchmark::State & st)
+{
+  // test success of an immediately finishing future
+  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);
+  if (ret != rclcpp::FutureReturnCode::SUCCESS) {
+    st.SkipWithError(rcutils_get_error_string().str);
+  }
+
+  reset_heap_counters();
+
+  for (auto _ : st) {
+    ret = rclcpp::spin_until_future_complete(node, shared_future, 1s);
+    if (ret != rclcpp::FutureReturnCode::SUCCESS) {
+      st.SkipWithError(rcutils_get_error_string().str);
+      break;
+    }
+  }
+}
+
+BENCHMARK_F(
+  PerformanceTestExecutorSimple,
+  static_executor_entities_collector_execute)(benchmark::State & st)
+{
+  rclcpp::executors::StaticExecutorEntitiesCollector::SharedPtr entities_collector_ =
+    std::make_shared<rclcpp::executors::StaticExecutorEntitiesCollector>();
+  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();
+  rcl_ret_t ret = rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator);
+  if (ret != RCL_RET_OK) {
+    st.SkipWithError(rcutils_get_error_string().str);
+  }
+  RCLCPP_SCOPE_EXIT(
+  {
+    rcl_ret_t ret = rcl_wait_set_fini(&wait_set);
+    if (ret != RCL_RET_OK) {
+      st.SkipWithError(rcutils_get_error_string().str);
+    }
+  });
+
+  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());
+
+  reset_heap_counters();
+
+  for (auto _ : st) {
+    entities_collector_->execute();
+  }
+}

rclcpp/test/benchmark/benchmark_init_shutdown.cpp

@@ -0,0 +1,53 @@
+// 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 "performance_test_fixture/performance_test_fixture.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+
+using performance_test_fixture::PerformanceTest;
+
+BENCHMARK_F(PerformanceTest, rclcpp_init)(benchmark::State & state)
+{
+  // Warmup and prime caches
+  rclcpp::init(0, nullptr);
+  rclcpp::shutdown();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    rclcpp::init(0, nullptr);
+
+    state.PauseTiming();
+    rclcpp::shutdown();
+    state.ResumeTiming();
+    benchmark::ClobberMemory();
+  }
+}
+
+BENCHMARK_F(PerformanceTest, rclcpp_shutdown)(benchmark::State & state)
+{
+  // Warmup and prime caches
+  rclcpp::init(0, nullptr);
+  rclcpp::shutdown();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    state.PauseTiming();
+    rclcpp::init(0, nullptr);
+    state.ResumeTiming();
+
+    rclcpp::shutdown();
+    benchmark::ClobberMemory();
+  }
+}

rclcpp/test/benchmark/benchmark_node.cpp

@@ -0,0 +1,77 @@
+// 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 <memory>
+#include <string>
+
+#include "performance_test_fixture/performance_test_fixture.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+using performance_test_fixture::PerformanceTest;
+
+class NodePerformanceTest : public PerformanceTest
+{
+public:
+  void SetUp(benchmark::State & state)
+  {
+    rclcpp::init(0, nullptr);
+    performance_test_fixture::PerformanceTest::SetUp(state);
+  }
+
+  void TearDown(benchmark::State & state)
+  {
+    performance_test_fixture::PerformanceTest::TearDown(state);
+    rclcpp::shutdown();
+  }
+};
+
+BENCHMARK_F(NodePerformanceTest, create_node)(benchmark::State & state)
+{
+  // Warmup and prime caches
+  auto outer_node = std::make_shared<rclcpp::Node>("node");
+  outer_node.reset();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    // Using pointer to separate construction and destruction in timing
+    auto node = std::make_shared<rclcpp::Node>("node");
+    benchmark::DoNotOptimize(node);
+    benchmark::ClobberMemory();
+
+    // Ensure destruction of node is not counted toward timing
+    state.PauseTiming();
+    node.reset();
+    state.ResumeTiming();
+  }
+}
+
+BENCHMARK_F(NodePerformanceTest, destroy_node)(benchmark::State & state)
+{
+  // Warmup and prime caches
+  auto outer_node = std::make_shared<rclcpp::Node>("node");
+  outer_node.reset();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    // Using pointer to separate construction and destruction in timing
+    state.PauseTiming();
+    auto node = std::make_shared<rclcpp::Node>("node");
+    state.ResumeTiming();
+
+    benchmark::DoNotOptimize(node);
+    benchmark::ClobberMemory();
+
+    node.reset();
+  }
+}

rclcpp/test/benchmark/benchmark_node_parameters_interface.cpp

@@ -0,0 +1,262 @@
+// 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 <memory>
+#include <string>
+#include <vector>
+
+#include "performance_test_fixture/performance_test_fixture.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+
+class NodeParametersInterfaceTest : public performance_test_fixture::PerformanceTest
+{
+public:
+  NodeParametersInterfaceTest()
+  : node_name("my_node"),
+    param_prefix("my_prefix"),
+    param1_name(param_prefix + ".my_param_1"),
+    param2_name(param_prefix + ".my_param_2"),
+    param3_name(param_prefix + ".my_param_3")
+  {
+  }
+
+  void SetUp(benchmark::State & state)
+  {
+    rclcpp::init(0, nullptr);
+    node = std::make_shared<rclcpp::Node>(node_name);
+
+    node->declare_parameter(param1_name);
+    node->declare_parameter(param2_name);
+    node->declare_parameter(param3_name);
+    node->undeclare_parameter(param3_name);
+
+    performance_test_fixture::PerformanceTest::SetUp(state);
+  }
+
+  void TearDown(benchmark::State & state)
+  {
+    performance_test_fixture::PerformanceTest::TearDown(state);
+
+    node.reset();
+    rclcpp::shutdown();
+  }
+
+  const std::string node_name;
+  const std::string param_prefix;
+  const std::string param1_name;
+  const std::string param2_name;
+  const std::string param3_name;
+
+protected:
+  rclcpp::Node::SharedPtr node;
+};
+
+BENCHMARK_F(NodeParametersInterfaceTest, declare_undeclare)(benchmark::State & state)
+{
+  for (auto _ : state) {
+    node->declare_parameter(param3_name);
+    node->undeclare_parameter(param3_name);
+  }
+}
+
+BENCHMARK_F(NodeParametersInterfaceTest, has_parameter_hit)(benchmark::State & state)
+{
+  for (auto _ : state) {
+    if (!node->has_parameter(param1_name)) {
+      state.SkipWithError("Parameter was expected");
+      break;
+    }
+  }
+}
+
+BENCHMARK_F(NodeParametersInterfaceTest, has_parameter_miss)(benchmark::State & state)
+{
+  for (auto _ : state) {
+    if (node->has_parameter(param3_name)) {
+      state.SkipWithError("Parameter was not expected");
+      break;
+    }
+  }
+}
+
+BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_bool)(benchmark::State & state)
+{
+  const std::vector<rclcpp::Parameter> param_values1
+  {
+    rclcpp::Parameter(param1_name, true),
+    rclcpp::Parameter(param2_name, false),
+  };
+  const std::vector<rclcpp::Parameter> param_values2
+  {
+    rclcpp::Parameter(param1_name, false),
+    rclcpp::Parameter(param2_name, true),
+  };
+
+  reset_heap_counters();
+
+  for (auto _ : state) {
+    node->set_parameters(param_values2);
+    node->set_parameters(param_values1);
+  }
+}
+
+BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_atomically_bool)(benchmark::State & state)
+{
+  const std::vector<rclcpp::Parameter> param_values1
+  {
+    rclcpp::Parameter(param1_name, true),
+    rclcpp::Parameter(param2_name, false),
+  };
+  const std::vector<rclcpp::Parameter> param_values2
+  {
+    rclcpp::Parameter(param1_name, false),
+    rclcpp::Parameter(param2_name, true),
+  };
+
+  reset_heap_counters();
+
+  for (auto _ : state) {
+    node->set_parameters_atomically(param_values2);
+    node->set_parameters_atomically(param_values1);
+  }
+}
+
+BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_callback_bool)(benchmark::State & state)
+{
+  const std::vector<rclcpp::Parameter> param_values1
+  {
+    rclcpp::Parameter(param1_name, true),
+    rclcpp::Parameter(param2_name, false),
+  };
+  const std::vector<rclcpp::Parameter> param_values2
+  {
+    rclcpp::Parameter(param1_name, false),
+    rclcpp::Parameter(param2_name, true),
+  };
+
+  rcl_interfaces::msg::SetParametersResult callback_result;
+  bool callback_received = false;
+  callback_result.successful = true;
+  auto callback =
+    [&callback_result, &callback_received](const std::vector<rclcpp::Parameter> &) {
+      callback_received = true;
+      return callback_result;
+    };
+  auto handle = node->add_on_set_parameters_callback(callback);
+
+  reset_heap_counters();
+
+  for (auto _ : state) {
+    node->set_parameters(param_values2);
+    node->set_parameters(param_values1);
+  }
+
+  if (!callback_received) {
+    state.SkipWithError("Callback is not functioning");
+  }
+
+  node->remove_on_set_parameters_callback(handle.get());
+}
+
+BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_string)(benchmark::State & state)
+{
+  const std::vector<rclcpp::Parameter> param_values1
+  {
+    rclcpp::Parameter(param1_name, "param 1 value A"),
+    rclcpp::Parameter(param2_name, "param 2 value B"),
+  };
+  const std::vector<rclcpp::Parameter> param_values2
+  {
+    rclcpp::Parameter(param1_name, "param 1 value B"),
+    rclcpp::Parameter(param2_name, "param 2 value A"),
+  };
+
+  reset_heap_counters();
+
+  for (auto _ : state) {
+    node->set_parameters(param_values2);
+    node->set_parameters(param_values1);
+  }
+}
+
+BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_array)(benchmark::State & state)
+{
+  const std::vector<rclcpp::Parameter> param_values1
+  {
+    rclcpp::Parameter(param1_name, std::vector<int> {0, 1, 2}),
+    rclcpp::Parameter(param2_name, std::vector<int> {3, 4, 5}),
+  };
+  const std::vector<rclcpp::Parameter> param_values2
+  {
+    rclcpp::Parameter(param1_name, std::vector<int> {4, 5, 6}),
+    rclcpp::Parameter(param2_name, std::vector<int> {7, 8, 9}),
+  };
+
+  reset_heap_counters();
+
+  for (auto _ : state) {
+    node->set_parameters(param_values2);
+    node->set_parameters(param_values1);
+  }
+}
+
+BENCHMARK_F(NodeParametersInterfaceTest, get_parameter)(benchmark::State & state)
+{
+  rclcpp::Parameter param1_value;
+
+  reset_heap_counters();
+
+  for (auto _ : state) {
+    node->get_parameter(param1_name, param1_value);
+  }
+}
+
+BENCHMARK_F(NodeParametersInterfaceTest, list_parameters_hit)(benchmark::State & state)
+{
+  rcl_interfaces::msg::ListParametersResult param_list;
+  const std::vector<std::string> prefixes
+  {
+    param_prefix,
+  };
+
+  reset_heap_counters();
+
+  for (auto _ : state) {
+    param_list = node->list_parameters(prefixes, 10);
+    if (param_list.names.size() != 2) {
+      state.SkipWithError("Expected node names");
+      break;
+    }
+  }
+}
+
+BENCHMARK_F(NodeParametersInterfaceTest, list_parameters_miss)(benchmark::State & state)
+{
+  rcl_interfaces::msg::ListParametersResult param_list;
+  const std::vector<std::string> prefixes
+  {
+    "your_param",
+  };
+
+  reset_heap_counters();
+
+  for (auto _ : state) {
+    param_list = node->list_parameters(prefixes, 10);
+    if (param_list.names.size() != 0) {
+      state.SkipWithError("Expected no node names");
+      break;
+    }
+  }
+}

rclcpp/test/benchmark/benchmark_service.cpp

@@ -0,0 +1,141 @@
+// 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 <memory>
+
+#include "performance_test_fixture/performance_test_fixture.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "test_msgs/srv/empty.hpp"
+
+using performance_test_fixture::PerformanceTest;
+
+constexpr char empty_service_name[] = "empty_service";
+
+class ServicePerformanceTest : public PerformanceTest
+{
+public:
+  ServicePerformanceTest()
+  : callback_count(0) {}
+
+  void SetUp(benchmark::State & state)
+  {
+    rclcpp::init(0, nullptr);
+    node = std::make_unique<rclcpp::Node>("node", "ns");
+    empty_client = node->create_client<test_msgs::srv::Empty>(empty_service_name);
+    callback_count = 0;
+
+    performance_test_fixture::PerformanceTest::SetUp(state);
+  }
+
+  void TearDown(benchmark::State & state)
+  {
+    performance_test_fixture::PerformanceTest::TearDown(state);
+
+    empty_client.reset();
+    node.reset();
+    rclcpp::shutdown();
+  }
+
+  void ServiceCallback(
+    const test_msgs::srv::Empty::Request::SharedPtr,
+    test_msgs::srv::Empty::Response::SharedPtr)
+  {
+    callback_count++;
+  }
+
+protected:
+  std::unique_ptr<rclcpp::Node> node;
+  std::shared_ptr<rclcpp::Client<test_msgs::srv::Empty>> empty_client;
+  int callback_count;
+};
+
+BENCHMARK_F(ServicePerformanceTest, construct_service_no_client)(benchmark::State & state) {
+  auto callback = std::bind(
+    &ServicePerformanceTest::ServiceCallback,
+    this, std::placeholders::_1, std::placeholders::_2);
+
+  auto outer_service = node->create_service<test_msgs::srv::Empty>("not_a_service", callback);
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    auto service = node->create_service<test_msgs::srv::Empty>("not_a_service", callback);
+    benchmark::DoNotOptimize(service);
+    benchmark::ClobberMemory();
+
+    state.PauseTiming();
+    service.reset();
+    state.ResumeTiming();
+  }
+}
+
+BENCHMARK_F(ServicePerformanceTest, construct_service_empty_srv)(benchmark::State & state) {
+  auto callback = std::bind(
+    &ServicePerformanceTest::ServiceCallback,
+    this, std::placeholders::_1, std::placeholders::_2);
+  auto outer_service = node->create_service<test_msgs::srv::Empty>(empty_service_name, callback);
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    auto service = node->create_service<test_msgs::srv::Empty>(empty_service_name, callback);
+    benchmark::DoNotOptimize(service);
+    benchmark::ClobberMemory();
+
+    state.PauseTiming();
+    service.reset();
+    state.ResumeTiming();
+  }
+}
+
+BENCHMARK_F(ServicePerformanceTest, destroy_service_empty_srv)(benchmark::State & state) {
+  auto callback = std::bind(
+    &ServicePerformanceTest::ServiceCallback,
+    this, std::placeholders::_1, std::placeholders::_2);
+  auto outer_service = node->create_service<test_msgs::srv::Empty>(empty_service_name, callback);
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    state.PauseTiming();
+    auto service = node->create_service<test_msgs::srv::Empty>(empty_service_name, callback);
+    state.ResumeTiming();
+    benchmark::DoNotOptimize(service);
+    benchmark::ClobberMemory();
+
+    service.reset();
+  }
+}
+
+BENCHMARK_F(ServicePerformanceTest, async_send_response)(benchmark::State & state) {
+  auto callback = std::bind(
+    &ServicePerformanceTest::ServiceCallback,
+    this, std::placeholders::_1, std::placeholders::_2);
+  auto service = node->create_service<test_msgs::srv::Empty>(empty_service_name, callback);
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    state.PauseTiming();
+    // Clear executor queue
+    rclcpp::spin_some(node->get_node_base_interface());
+
+    auto request = std::make_shared<test_msgs::srv::Empty::Request>();
+    auto future = empty_client->async_send_request(request);
+    state.ResumeTiming();
+    benchmark::DoNotOptimize(service);
+    benchmark::ClobberMemory();
+
+    rclcpp::spin_until_future_complete(node->get_node_base_interface(), future);
+  }
+  if (callback_count == 0) {
+    state.SkipWithError("Service callback was not called");
+  }
+}

rclcpp/test/rclcpp/CMakeLists.txt

@@ -0,0 +1,653 @@
+find_package(ament_cmake_gtest REQUIRED)
+
+find_package(rmw_implementation_cmake REQUIRED)
+
+add_definitions(-DTEST_RESOURCES_DIRECTORY="${CMAKE_CURRENT_BINARY_DIR}/../resources")
+
+rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs
+  ../msg/Header.msg
+  ../msg/MessageWithHeader.msg
+  DEPENDENCIES builtin_interfaces
+  LIBRARY_NAME ${PROJECT_NAME}
+  SKIP_INSTALL
+)
+
+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/executors/test_executors.cpp

@@ -115,9 +115,9 @@ public:
   }
 };
 
-// TYPED_TEST_CASE is deprecated as of gtest 1.9, use TYPED_TEST_SUITE when gtest dependency
+// TYPED_TEST_SUITE is deprecated as of gtest 1.9, use TYPED_TEST_SUITE when gtest dependency
 // is updated.
-TYPED_TEST_CASE(TestExecutors, ExecutorTypes, ExecutorTypeNames);
+TYPED_TEST_SUITE(TestExecutors, ExecutorTypes, ExecutorTypeNames);
 
 // StaticSingleThreadedExecutor is not included in these tests for now, due to:
 // https://github.com/ros2/rclcpp/issues/1219
@@ -125,7 +125,7 @@ using StandardExecutors =
   ::testing::Types<
   rclcpp::executors::SingleThreadedExecutor,
   rclcpp::executors::MultiThreadedExecutor>;
-TYPED_TEST_CASE(TestExecutorsStable, StandardExecutors, ExecutorTypeNames);
+TYPED_TEST_SUITE(TestExecutorsStable, StandardExecutors, ExecutorTypeNames);
 
 // Make sure that executors detach from nodes when destructing
 TYPED_TEST(TestExecutors, detachOnDestruction) {

rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp

@@ -16,6 +16,7 @@
 
 #include <memory>
 #include <string>
+#include <vector>
 
 #include "rcl/node_options.h"
 #include "rclcpp/node.hpp"
@@ -55,7 +56,9 @@ public:
   {
     rclcpp::init(0, nullptr);
 
-    node = std::make_shared<rclcpp::Node>("node", "ns");
+    rclcpp::NodeOptions options{};
+    options.arguments(std::vector<std::string>{"-r", "foo:=bar"});
+    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.
@@ -129,3 +132,13 @@ TEST_F(TestNodeService, add_client_rcl_trigger_guard_condition_error)
     node_services->add_client(client, callback_group),
     std::runtime_error("Failed to notify wait set on client creation: error not set"));
 }
+
+TEST_F(TestNodeService, resolve_service_name)
+{
+  EXPECT_EQ("/ns/bar", node_services->resolve_service_name("foo", false));
+  EXPECT_EQ("/ns/foo", node_services->resolve_service_name("foo", true));
+  EXPECT_EQ("/foo", node_services->resolve_service_name("/foo", true));
+  EXPECT_THROW(
+    node_services->resolve_service_name("this is not a valid name!~>", true),
+    rclcpp::exceptions::RCLError);
+}

rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp

@@ -17,6 +17,7 @@
 #include <memory>
 #include <string>
 #include <type_traits>
+#include <vector>
 
 #include "rcl/node_options.h"
 #include "rclcpp/node.hpp"
@@ -80,7 +81,9 @@ public:
   void SetUp()
   {
     rclcpp::init(0, nullptr);
-    node = std::make_shared<rclcpp::Node>("node", "ns");
+    rclcpp::NodeOptions options{};
+    options.arguments(std::vector<std::string>{"-r", "foo:=bar"});
+    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.
@@ -154,3 +157,13 @@ TEST_F(TestNodeTopics, add_subscription_rcl_trigger_guard_condition_error)
     node_topics->add_subscription(subscription, callback_group),
     std::runtime_error("failed to notify wait set on subscription creation: error not set"));
 }
+
+TEST_F(TestNodeTopics, resolve_topic_name)
+{
+  EXPECT_EQ("/ns/bar", node_topics->resolve_topic_name("foo", false));
+  EXPECT_EQ("/ns/foo", node_topics->resolve_topic_name("foo", true));
+  EXPECT_EQ("/foo", node_topics->resolve_topic_name("/foo", true));
+  EXPECT_THROW(
+    node_topics->resolve_topic_name("this is not a valid name!~>", true),
+    rclcpp::exceptions::RCLError);
+}

rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp

@@ -79,7 +79,7 @@ public:
   }
 };
 
-TYPED_TEST_CASE(TestAddCallbackGroupsToExecutor, ExecutorTypes, ExecutorTypeNames);
+TYPED_TEST_SUITE(TestAddCallbackGroupsToExecutor, ExecutorTypes, ExecutorTypeNames);
 
 /*
  * Test adding callback groups.

rclcpp/test/rclcpp/test_duration.cpp

@@ -68,7 +68,7 @@ TEST_F(TestDuration, operators) {
 TEST_F(TestDuration, chrono_overloads) {
   int64_t ns = 123456789l;
   auto chrono_ns = std::chrono::nanoseconds(ns);
-  auto d1 = rclcpp::Duration(ns);
+  auto d1 = rclcpp::Duration::from_nanoseconds(ns);
   auto d2 = rclcpp::Duration(chrono_ns);
   auto d3 = rclcpp::Duration(123456789ns);
   EXPECT_EQ(d1, d2);
@@ -85,11 +85,11 @@ TEST_F(TestDuration, chrono_overloads) {
 }
 
 TEST_F(TestDuration, overflows) {
-  rclcpp::Duration max(std::numeric_limits<rcl_duration_value_t>::max());
-  rclcpp::Duration min(std::numeric_limits<rcl_duration_value_t>::min());
+  auto max = rclcpp::Duration::from_nanoseconds(std::numeric_limits<rcl_duration_value_t>::max());
+  auto min = rclcpp::Duration::from_nanoseconds(std::numeric_limits<rcl_duration_value_t>::min());
 
-  rclcpp::Duration one(1);
-  rclcpp::Duration negative_one(-1);
+  rclcpp::Duration one(1ns);
+  rclcpp::Duration negative_one(-1ns);
 
   EXPECT_THROW(max + one, std::overflow_error);
   EXPECT_THROW(min - one, std::underflow_error);
@@ -106,7 +106,7 @@ TEST_F(TestDuration, overflows) {
 }
 
 TEST_F(TestDuration, negative_duration) {
-  rclcpp::Duration assignable_duration = rclcpp::Duration(0) - rclcpp::Duration(5, 0);
+  rclcpp::Duration assignable_duration = rclcpp::Duration(0ns) - rclcpp::Duration(5, 0);
 
   {
     // avoid windows converting a literal number less than -INT_MAX to unsigned int C4146
@@ -140,22 +140,24 @@ static const int64_t ONE_SEC_IN_NS = 1000 * 1000 * 1000;
 static const int64_t ONE_AND_HALF_SEC_IN_NS = 3 * HALF_SEC_IN_NS;
 
 TEST_F(TestDuration, from_seconds) {
-  EXPECT_EQ(rclcpp::Duration(0), rclcpp::Duration::from_seconds(0.0));
-  EXPECT_EQ(rclcpp::Duration(0), rclcpp::Duration::from_seconds(0));
+  EXPECT_EQ(rclcpp::Duration(0ns), rclcpp::Duration::from_seconds(0.0));
+  EXPECT_EQ(rclcpp::Duration(0ns), rclcpp::Duration::from_seconds(0));
   EXPECT_EQ(rclcpp::Duration(1, HALF_SEC_IN_NS), rclcpp::Duration::from_seconds(1.5));
-  EXPECT_EQ(rclcpp::Duration(-ONE_AND_HALF_SEC_IN_NS), rclcpp::Duration::from_seconds(-1.5));
+  EXPECT_EQ(
+    rclcpp::Duration::from_nanoseconds(-ONE_AND_HALF_SEC_IN_NS),
+    rclcpp::Duration::from_seconds(-1.5));
 }
 
 TEST_F(TestDuration, std_chrono_constructors) {
-  EXPECT_EQ(rclcpp::Duration(0), rclcpp::Duration(0.0s));
-  EXPECT_EQ(rclcpp::Duration(0), rclcpp::Duration(0s));
+  EXPECT_EQ(rclcpp::Duration(0ns), rclcpp::Duration(0.0s));
+  EXPECT_EQ(rclcpp::Duration(0ns), rclcpp::Duration(0s));
   EXPECT_EQ(rclcpp::Duration(1, HALF_SEC_IN_NS), rclcpp::Duration(1.5s));
   EXPECT_EQ(rclcpp::Duration(-1, 0), rclcpp::Duration(-1s));
 }
 
 TEST_F(TestDuration, conversions) {
   {
-    const rclcpp::Duration duration(HALF_SEC_IN_NS);
+    auto duration = rclcpp::Duration::from_nanoseconds(HALF_SEC_IN_NS);
     const auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(duration);
     EXPECT_EQ(duration_msg.sec, 0);
     EXPECT_EQ(duration_msg.nanosec, HALF_SEC_IN_NS);
@@ -170,7 +172,7 @@ TEST_F(TestDuration, conversions) {
   }
 
   {
-    const rclcpp::Duration duration(ONE_SEC_IN_NS);
+    auto duration = rclcpp::Duration::from_nanoseconds(ONE_SEC_IN_NS);
     const auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(duration);
     EXPECT_EQ(duration_msg.sec, 1);
     EXPECT_EQ(duration_msg.nanosec, 0u);
@@ -185,7 +187,7 @@ TEST_F(TestDuration, conversions) {
   }
 
   {
-    const rclcpp::Duration duration(ONE_AND_HALF_SEC_IN_NS);
+    auto duration = rclcpp::Duration::from_nanoseconds(ONE_AND_HALF_SEC_IN_NS);
     auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(duration);
     EXPECT_EQ(duration_msg.sec, 1);
     EXPECT_EQ(duration_msg.nanosec, HALF_SEC_IN_NS);
@@ -200,7 +202,7 @@ TEST_F(TestDuration, conversions) {
   }
 
   {
-    rclcpp::Duration duration(-HALF_SEC_IN_NS);
+    auto duration = rclcpp::Duration::from_nanoseconds(-HALF_SEC_IN_NS);
     auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(duration);
     EXPECT_EQ(duration_msg.sec, -1);
     EXPECT_EQ(duration_msg.nanosec, HALF_SEC_IN_NS);
@@ -213,7 +215,7 @@ TEST_F(TestDuration, conversions) {
   }
 
   {
-    rclcpp::Duration duration(-ONE_SEC_IN_NS);
+    auto duration = rclcpp::Duration::from_nanoseconds(-ONE_SEC_IN_NS);
     auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(duration);
     EXPECT_EQ(duration_msg.sec, -1);
     EXPECT_EQ(duration_msg.nanosec, 0u);
@@ -226,7 +228,7 @@ TEST_F(TestDuration, conversions) {
   }
 
   {
-    rclcpp::Duration duration(-ONE_AND_HALF_SEC_IN_NS);
+    auto duration = rclcpp::Duration::from_nanoseconds(-ONE_AND_HALF_SEC_IN_NS);
     auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(duration);
     EXPECT_EQ(duration_msg.sec, -2);
     EXPECT_EQ(duration_msg.nanosec, HALF_SEC_IN_NS);
@@ -253,9 +255,10 @@ TEST_F(TestDuration, test_some_constructors) {
 }
 
 TEST_F(TestDuration, test_some_exceptions) {
-  rclcpp::Duration test_duration(0u);
+  rclcpp::Duration test_duration(0ns);
   RCLCPP_EXPECT_THROW_EQ(
-    test_duration = rclcpp::Duration(INT64_MAX) - rclcpp::Duration(-1),
+    test_duration =
+    rclcpp::Duration::from_nanoseconds(INT64_MAX) - rclcpp::Duration(-1ns),
     std::overflow_error("duration subtraction leads to int64_t overflow"));
   RCLCPP_EXPECT_THROW_EQ(
     test_duration = test_duration * (std::numeric_limits<double>::infinity()),

rclcpp/test/rclcpp/test_graph_listener.cpp

@@ -125,16 +125,14 @@ public:
     this->run();
   }
 
-  void mock_start_thread()
+  void mock_init_wait_set()
   {
-    // 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"));
+    this->init_wait_set();
+  }
+
+  void mock_cleanup_wait_set()
+  {
+    this->cleanup_wait_set();
   }
 };
 
@@ -152,7 +150,7 @@ 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();
+  graph_listener_test->mock_init_wait_set();
   auto mock_wait_set_clear = mocking_utils::patch_and_return(
     "lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR);
   RCLCPP_EXPECT_THROW_EQ(
@@ -164,7 +162,7 @@ TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_set_add_guard_condi
   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();
+  graph_listener_test->mock_init_wait_set();
   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(
@@ -176,7 +174,7 @@ TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_set_add_guard_condi
   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();
+  graph_listener_test->mock_init_wait_set();
   auto mock = mocking_utils::patch(
     "lib:rclcpp", rcl_wait_set_add_guard_condition, [](auto, ...) {
       static int counter = 1;
@@ -196,7 +194,7 @@ 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();
+  graph_listener_test->mock_init_wait_set();
   auto mock_wait_set_clear = mocking_utils::patch_and_return(
     "lib:rclcpp", rcl_wait, RCL_RET_ERROR);
   RCLCPP_EXPECT_THROW_EQ(
@@ -208,7 +206,7 @@ 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();
+  graph_listener_test->mock_init_wait_set();
   auto mock_wait_set_clear = mocking_utils::patch_and_return(
     "lib:rclcpp", rcl_wait, RCL_RET_TIMEOUT);
   RCLCPP_EXPECT_THROW_EQ(
@@ -267,27 +265,54 @@ TEST_F(TestGraphListener, test_errors_graph_listener_add_remove_node) {
 
 /* 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()));
+  auto global_context = rclcpp::contexts::get_global_default_context();
+  auto graph_listener_test =
+    std::make_shared<TestGraphListenerProtectedMethods>(global_context);
+
+  graph_listener_test->start_if_not_started();
+
+  {
+    auto mock_wait_set_fini = mocking_utils::patch_and_return(
+      "lib:rclcpp", rcl_wait_set_fini, RCL_RET_ERROR);
+    // Exception is logged when using nothrow_t
+    EXPECT_NO_THROW(graph_listener_test->shutdown(std::nothrow_t()));
+  }
+
+  graph_listener_test->mock_cleanup_wait_set();
 }
 
 TEST_F(TestGraphListener, test_graph_listener_shutdown_wait_fini_error_throw) {
+  auto global_context = rclcpp::contexts::get_global_default_context();
+  auto graph_listener_test =
+    std::make_shared<TestGraphListenerProtectedMethods>(global_context);
+
   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"));
+
+  {
+    auto mock_wait_set_fini = mocking_utils::patch_and_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"));
+  }
+
+  graph_listener_test->mock_cleanup_wait_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(
+  auto global_context = rclcpp::contexts::get_global_default_context();
+  auto graph_listener_test =
+    std::make_shared<TestGraphListenerProtectedMethods>(global_context);
+
+  graph_listener_test->start_if_not_started();
+
+  auto mock_wait_set_fini = mocking_utils::patch_and_return(
     "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR);
+
   RCLCPP_EXPECT_THROW_EQ(
-    graph_listener()->shutdown(),
+    graph_listener_test->shutdown(),
     std::runtime_error("failed to finalize interrupt guard condition: error not set"));
+
+  graph_listener_test->mock_cleanup_wait_set();
 }

rclcpp/test/rclcpp/test_publisher.cpp

@@ -184,7 +184,7 @@ static std::vector<TestParameters> invalid_qos_profiles()
   return parameters;
 }
 
-INSTANTIATE_TEST_CASE_P(
+INSTANTIATE_TEST_SUITE_P(
   TestPublisherThrows, TestPublisherInvalidIntraprocessQos,
   ::testing::ValuesIn(invalid_qos_profiles()),
   ::testing::PrintToStringParamName());

rclcpp/test/rclcpp/test_publisher_subscription_count_api.cpp

@@ -192,7 +192,7 @@ using AllTestDescriptions = ::testing::Types<
   TwoSubscriptionsInTwoContextsWithIntraprocessComm,
   TwoSubscriptionsInTwoContextsWithoutIntraprocessComm
 >;
-TYPED_TEST_CASE(TestPublisherSubscriptionCount, AllTestDescriptions, PrintTestDescription);
+TYPED_TEST_SUITE(TestPublisherSubscriptionCount, AllTestDescriptions, PrintTestDescription);
 
 
 using test_msgs::msg::Empty;

rclcpp/test/rclcpp/test_subscription.cpp

@@ -506,7 +506,7 @@ static std::vector<TestParameters> invalid_qos_profiles()
   return parameters;
 }
 
-INSTANTIATE_TEST_CASE_P(
+INSTANTIATE_TEST_SUITE_P(
   TestSubscriptionThrows, TestSubscriptionInvalidIntraprocessQos,
   ::testing::ValuesIn(invalid_qos_profiles()),
   ::testing::PrintToStringParamName());

rclcpp/test/rclcpp/test_subscription_publisher_count_api.cpp

@@ -129,7 +129,7 @@ struct TwoContextsPerTest
 };
 
 using AllTestDescriptions = ::testing::Types<OneContextPerTest, TwoContextsPerTest>;
-TYPED_TEST_CASE(TestSubscriptionPublisherCount, AllTestDescriptions, PrintTestDescription);
+TYPED_TEST_SUITE(TestSubscriptionPublisherCount, AllTestDescriptions, PrintTestDescription);
 
 
 using test_msgs::msg::Empty;

rclcpp/test/rclcpp/test_time.cpp

@@ -15,6 +15,7 @@
 #include <gtest/gtest.h>
 
 #include <algorithm>
+#include <chrono>
 #include <limits>
 #include <string>
 
@@ -30,6 +31,8 @@
 namespace
 {
 
+using namespace std::chrono_literals;
+
 bool logical_eq(const bool a, const bool b)
 {
   return (a && b) || ((!a) && !(b));
@@ -221,7 +224,7 @@ TEST_F(TestTime, operators) {
   EXPECT_EQ(sub, young - old);
 
   rclcpp::Time young_changed(young);
-  young_changed -= rclcpp::Duration(old.nanoseconds());
+  young_changed -= rclcpp::Duration::from_nanoseconds(old.nanoseconds());
   EXPECT_EQ(sub.nanoseconds(), young_changed.nanoseconds());
 
   rclcpp::Time system_time(0, 0, RCL_SYSTEM_TIME);
@@ -320,8 +323,8 @@ TEST_F(TestTime, overflow_detectors) {
 TEST_F(TestTime, overflows) {
   rclcpp::Time max_time(std::numeric_limits<rcl_time_point_value_t>::max());
   rclcpp::Time min_time(std::numeric_limits<rcl_time_point_value_t>::min());
-  rclcpp::Duration one(1);
-  rclcpp::Duration two(2);
+  rclcpp::Duration one(1ns);
+  rclcpp::Duration two(2ns);
 
   // Cross min/max
   EXPECT_THROW(max_time + one, std::overflow_error);
@@ -394,7 +397,7 @@ TEST_F(TestTime, test_assignment_operator_from_builtin_msg_time) {
 }
 
 TEST_F(TestTime, test_sum_operator) {
-  const rclcpp::Duration one(1);
+  const rclcpp::Duration one(1ns);
   const rclcpp::Time test_time(0u);
   EXPECT_EQ(0u, test_time.nanoseconds());
 
@@ -406,41 +409,41 @@ 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),
+    test_time = rclcpp::Time(INT64_MAX) + rclcpp::Duration(1ns),
     std::overflow_error("addition leads to int64_t overflow"));
   RCLCPP_EXPECT_THROW_EQ(
-    test_time = rclcpp::Time(INT64_MIN) + rclcpp::Duration(-1),
+    test_time = rclcpp::Time(INT64_MIN) + rclcpp::Duration(-1ns),
     std::underflow_error("addition leads to int64_t underflow"));
 
   RCLCPP_EXPECT_THROW_EQ(
-    test_time = rclcpp::Time(INT64_MAX) - rclcpp::Duration(-1),
+    test_time = rclcpp::Time(INT64_MAX) - rclcpp::Duration(-1ns),
     std::overflow_error("time subtraction leads to int64_t overflow"));
   RCLCPP_EXPECT_THROW_EQ(
-    test_time = rclcpp::Time(INT64_MIN) - rclcpp::Duration(1),
+    test_time = rclcpp::Time(INT64_MIN) - rclcpp::Duration(1ns),
     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),
+    test_time += rclcpp::Duration(1ns),
     std::overflow_error("addition leads to int64_t overflow"));
   test_time = rclcpp::Time(INT64_MIN);
   RCLCPP_EXPECT_THROW_EQ(
-    test_time += rclcpp::Duration(-1),
+    test_time += rclcpp::Duration(-1ns),
     std::underflow_error("addition leads to int64_t underflow"));
 
   test_time = rclcpp::Time(INT64_MAX);
   RCLCPP_EXPECT_THROW_EQ(
-    test_time -= rclcpp::Duration(-1),
+    test_time -= rclcpp::Duration(-1ns),
     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),
+    test_time -= rclcpp::Duration(1ns),
     std::underflow_error("time subtraction leads to int64_t underflow"));
 
   RCLCPP_EXPECT_THROW_EQ(
-    test_time = rclcpp::Duration(INT64_MAX) + rclcpp::Time(1),
+    test_time = rclcpp::Duration::from_nanoseconds(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),
+    test_time = rclcpp::Duration::from_nanoseconds(INT64_MIN) + rclcpp::Time(-1),
     std::underflow_error("addition leads to int64_t underflow"));
 }

rclcpp/test/rclcpp/test_utilities.cpp

@@ -61,6 +61,29 @@ TEST(TestUtilities, init_with_args) {
   rclcpp::shutdown();
 }
 
+TEST(TestUtilities, init_with_args_contains_ros) {
+  EXPECT_FALSE(rclcpp::signal_handlers_installed());
+  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 args = rclcpp::init_and_remove_ros_arguments(argc, argv);
+
+  ASSERT_EQ(4u, args.size());
+  ASSERT_EQ(std::string{"process_name"}, args[0]);
+  ASSERT_EQ(std::string{"-d"}, args[1]);
+  ASSERT_EQ(std::string{"--foo=bar"}, args[2]);
+  ASSERT_EQ(std::string{"--baz"}, args[3]);
+  EXPECT_TRUE(rclcpp::signal_handlers_installed());
+
+  EXPECT_TRUE(rclcpp::ok());
+  rclcpp::shutdown();
+}
+
 TEST(TestUtilities, multi_init) {
   auto context1 = std::make_shared<rclcpp::contexts::DefaultContext>();
   auto context2 = std::make_shared<rclcpp::contexts::DefaultContext>();

rclcpp_action/CHANGELOG.rst

@@ -3,6 +3,15 @@ Changelog for package rclcpp_action
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+5.1.0 (2020-11-02)
+------------------
+* Benchmark rclcpp_action action_client (`#1429 <https://github.com/ros2/rclcpp/issues/1429>`_)
+* Add missing locking to the rclcpp_action::ServerBase. (`#1421 <https://github.com/ros2/rclcpp/issues/1421>`_)
+* Increase test timeouts of slow running tests with rmw_connext_cpp (`#1400 <https://github.com/ros2/rclcpp/issues/1400>`_)
+* Update maintainers (`#1384 <https://github.com/ros2/rclcpp/issues/1384>`_)
+* Increase coverage rclcpp_action to 95% (`#1290 <https://github.com/ros2/rclcpp/issues/1290>`_)
+* Contributors: Chris Lalancette, Ivan Santiago Paunovic, brawner
+
 5.0.0 (2020-09-18)
 ------------------
 * Pass goal handle to goal response callback instead of a future (`#1311 <https://github.com/ros2/rclcpp/issues/1311>`_)

rclcpp_action/CMakeLists.txt

@@ -74,6 +74,8 @@ if(BUILD_TESTING)
   set(ament_cmake_cppcheck_ADDITIONAL_INCLUDE_DIRS ${rclcpp_INCLUDE_DIRS})
   ament_lint_auto_find_test_dependencies()
 
+  add_subdirectory(test/benchmark)
+
   ament_add_gtest(test_client test/test_client.cpp TIMEOUT 180)
   if(TARGET test_client)
     ament_target_dependencies(test_client

rclcpp_action/QUALITY_DECLARATION.md

@@ -2,7 +2,7 @@ This document is a declaration of software quality for the `rclcpp_action` packa
 
 # rclcpp_action Quality Declaration
 
-The package `rclcpp_action` claims to be in the **Quality Level 3** category.
+The package `rclcpp_action` claims to be in the **Quality Level 2** category.
 
 Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-quality-categories) of the ROS2 developer guide.
 
@@ -151,19 +151,19 @@ It also has several test dependencies, which do not affect the resulting quality
 
 `action_msgs` provides messages and services for ROS 2 actions.
 
-It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/action_msgs/QUALITY_DECLARATION.md).
+It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/action_msgs/QUALITY_DECLARATION.md).
 
 #### `rclcpp`
 
 The `rclcpp` package provides the ROS client library in C++.
 
-It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/master/rclcpp/QUALITY_DECLARATION.md).
+It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/master/rclcpp/QUALITY_DECLARATION.md).
 
 #### `rcl_action`
 
 The `rcl_action` package provides C-based ROS action implementation.
 
-It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl_action/QUALITY_DECLARATION.md).
+It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl_action/QUALITY_DECLARATION.md).
 
 ### Direct Runtime non-ROS Dependency [5.iii]
 

rclcpp_action/README.md

@@ -6,4 +6,4 @@ Visit the [rclcpp_action API documentation](http://docs.ros2.org/latest/api/rclc
 
 ## Quality Declaration
 
-This package claims to be in the **Quality Level 3** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details.
+This package claims to be in the **Quality Level 2** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details.

rclcpp_action/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp_action</name>
-  <version>5.0.0</version>
+  <version>5.1.0</version>
   <description>Adds action APIs for C++.</description>
   <maintainer email="ivanpauno@ekumenlabs.com">Ivan Paunovic</maintainer>
   <maintainer email="mabel@openrobotics.org">Mabel Zhang</maintainer>
@@ -26,6 +26,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>test_msgs</test_depend>
 
   <export>

rclcpp_action/src/server.cpp

@@ -44,7 +44,7 @@ public:
   }
 
   // Lock everything except user callbacks
-  std::recursive_mutex reentrant_mutex_;
+  std::recursive_mutex action_server_reentrant_mutex_;
 
   std::shared_ptr<rcl_action_server_t> action_server_;
 
@@ -88,12 +88,13 @@ ServerBase::ServerBase(
       if (nullptr != ptr) {
         rcl_node_t * rcl_node = node_base->get_rcl_node_handle();
         rcl_ret_t ret = rcl_action_server_fini(ptr, rcl_node);
-        (void)ret;
-        RCLCPP_DEBUG(
-          rclcpp::get_logger("rclcpp_action"),
-          "failed to fini rcl_action_server_t in deleter");
+        if (RCL_RET_OK != ret) {
+          RCLCPP_DEBUG(
+            rclcpp::get_logger("rclcpp_action"),
+            "failed to fini rcl_action_server_t in deleter");
+        }
+        delete ptr;
       }
-      delete ptr;
     };
 
   pimpl_->action_server_.reset(new rcl_action_server_t, deleter);
@@ -159,7 +160,7 @@ ServerBase::get_number_of_ready_guard_conditions()
 bool
 ServerBase::add_to_wait_set(rcl_wait_set_t * wait_set)
 {
-  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
   rcl_ret_t ret = rcl_action_wait_set_add_action_server(
     wait_set, pimpl_->action_server_.get(), NULL);
   return RCL_RET_OK == ret;
@@ -168,7 +169,7 @@ ServerBase::add_to_wait_set(rcl_wait_set_t * wait_set)
 bool
 ServerBase::is_ready(rcl_wait_set_t * wait_set)
 {
-  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
   rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready(
     wait_set,
     pimpl_->action_server_.get(),
@@ -210,7 +211,7 @@ ServerBase::execute_goal_request_received()
   rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info();
   rmw_request_id_t request_header;
 
-  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
 
   std::shared_ptr<void> message = create_goal_request();
   ret = rcl_action_take_goal_request(
@@ -254,10 +255,11 @@ ServerBase::execute_goal_request_received()
       {
         if (nullptr != ptr) {
           rcl_ret_t fail_ret = rcl_action_goal_handle_fini(ptr);
-          (void)fail_ret;
-          RCLCPP_DEBUG(
-            rclcpp::get_logger("rclcpp_action"),
-            "failed to fini rcl_action_goal_handle_t in deleter");
+          if (RCL_RET_OK != fail_ret) {
+            RCLCPP_DEBUG(
+              rclcpp::get_logger("rclcpp_action"),
+              "failed to fini rcl_action_goal_handle_t in deleter");
+          }
           delete ptr;
         }
       };
@@ -297,7 +299,7 @@ ServerBase::execute_cancel_request_received()
   // Initialize cancel request
   auto request = std::make_shared<action_msgs::srv::CancelGoal::Request>();
 
-  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
   ret = rcl_action_take_cancel_request(
     pimpl_->action_server_.get(),
     &request_header,
@@ -383,7 +385,7 @@ ServerBase::execute_result_request_received()
   // Get the result request message
   rmw_request_id_t request_header;
   std::shared_ptr<void> result_request = create_result_request();
-  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
   ret = rcl_action_take_result_request(
     pimpl_->action_server_.get(), &request_header, result_request.get());
 
@@ -439,7 +441,7 @@ ServerBase::execute_check_expired_goals()
 
   // Loop in case more than 1 goal expired
   while (num_expired > 0u) {
-    std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
     rcl_ret_t ret;
     ret = rcl_action_expire_goals(pimpl_->action_server_.get(), expired_goals, 1, &num_expired);
     if (RCL_RET_OK != ret) {
@@ -461,6 +463,11 @@ ServerBase::publish_status()
 {
   rcl_ret_t ret;
 
+  // We need to hold the lock across this entire method because
+  // rcl_action_server_get_goal_handles() returns an internal pointer to the
+  // goal data.
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+
   // Get all goal handles known to C action server
   rcl_action_goal_handle_t ** goal_handles = NULL;
   size_t num_goals = 0;
@@ -516,7 +523,7 @@ ServerBase::publish_result(const GoalUUID & uuid, std::shared_ptr<void> result_m
   // Check that the goal exists
   rcl_action_goal_info_t goal_info;
   convert(uuid, &goal_info);
-  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
   bool goal_exists;
   goal_exists = rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info);
 
@@ -542,7 +549,7 @@ ServerBase::publish_result(const GoalUUID & uuid, std::shared_ptr<void> result_m
 void
 ServerBase::notify_goal_terminal_state()
 {
-  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
   rcl_ret_t ret = rcl_action_notify_goal_done(pimpl_->action_server_.get());
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
@@ -552,7 +559,7 @@ ServerBase::notify_goal_terminal_state()
 void
 ServerBase::publish_feedback(std::shared_ptr<void> feedback_msg)
 {
-  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
   rcl_ret_t ret = rcl_action_publish_feedback(pimpl_->action_server_.get(), feedback_msg.get());
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to publish feedback");

rclcpp_action/test/benchmark/CMakeLists.txt

@@ -0,0 +1,23 @@
+find_package(performance_test_fixture REQUIRED)
+
+# These benchmarks are only being created and run for the default RMW
+# implementation. We are looking to test the performance of the ROS 2 code, not
+# the underlying middleware.
+
+add_performance_test(
+  benchmark_action_client
+  benchmark_action_client.cpp
+  TIMEOUT 120)
+if(TARGET benchmark_action_client)
+  target_link_libraries(benchmark_action_client ${PROJECT_NAME})
+  ament_target_dependencies(benchmark_action_client rclcpp test_msgs)
+endif()
+
+add_performance_test(
+  benchmark_action_server
+  benchmark_action_server.cpp
+  TIMEOUT 120)
+if(TARGET benchmark_action_server)
+  target_link_libraries(benchmark_action_server ${PROJECT_NAME})
+  ament_target_dependencies(benchmark_action_server rclcpp test_msgs)
+endif()

rclcpp_action/test/benchmark/benchmark_action_client.cpp

@@ -0,0 +1,354 @@
+// 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 <memory>
+#include <string>
+
+#include "performance_test_fixture/performance_test_fixture.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "test_msgs/action/fibonacci.hpp"
+
+using performance_test_fixture::PerformanceTest;
+
+using Fibonacci = test_msgs::action::Fibonacci;
+using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+using CancelResponse = typename Fibonacci::Impl::CancelGoalService::Response;
+using GoalUUID = rclcpp_action::GoalUUID;
+
+constexpr char fibonacci_action_name[] = "fibonacci";
+
+namespace
+{
+
+test_msgs::action::Fibonacci::Goal GetGoalOfOrder(int order)
+{
+  test_msgs::action::Fibonacci::Goal goal;
+  goal.order = order;
+  return goal;
+}
+
+}  // namespace
+
+class ActionClientPerformanceTest : public PerformanceTest
+{
+public:
+  void SetUp(benchmark::State & state)
+  {
+    rclcpp::init(0, nullptr);
+    // Use same node for server and client to avoid interprocess communication
+    node = std::make_shared<rclcpp::Node>("node", "ns");
+    performance_test_fixture::PerformanceTest::SetUp(state);
+  }
+
+  void SetUpServer(const std::string & action_name)
+  {
+    // This action server accepts and defers so that execution can be timed separately from
+    // accepting the goal
+    action_server = rclcpp_action::create_server<Fibonacci>(
+      node, action_name,
+      [](const GoalUUID &, std::shared_ptr<const Fibonacci::Goal> goal) {
+        if (goal->order <= 0) {
+          return rclcpp_action::GoalResponse::REJECT;
+        }
+        return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
+      },
+      [](std::shared_ptr<GoalHandle>) {
+        return rclcpp_action::CancelResponse::ACCEPT;
+      },
+      [this](std::shared_ptr<GoalHandle> goal_handle) {
+        current_goal_handle = goal_handle;
+      });
+  }
+
+  void ComputeFibonacciAndSetSuccess()
+  {
+    // This method is suprisingly slow, primarily due to the goal_handle->execute/succeed calls.
+    current_goal_handle->execute();
+    const auto goal = current_goal_handle->get_goal();
+    auto result = std::make_shared<Fibonacci::Result>();
+
+    // Should be checked by the server above
+    assert(goal->order > 0);
+    result->sequence.resize(goal->order);
+    result->sequence[0] = 0;
+    if (goal->order == 1) {
+      current_goal_handle->succeed(result);
+      return;
+    }
+    result->sequence[1] = 1;
+    if (goal->order == 2) {
+      current_goal_handle->succeed(result);
+      return;
+    }
+    for (int i = 2; i < goal->order; ++i) {
+      result->sequence[i] =
+        result->sequence[i - 1] + result->sequence[i - 2];
+    }
+    current_goal_handle->succeed(result);
+  }
+
+  void TearDown(benchmark::State & state)
+  {
+    performance_test_fixture::PerformanceTest::TearDown(state);
+    // Ensure proper sequencing of destruction
+    current_goal_handle.reset();
+    action_server.reset();
+    node.reset();
+    rclcpp::shutdown();
+  }
+
+protected:
+  std::shared_ptr<rclcpp::Node> node;
+  std::shared_ptr<rclcpp_action::Server<test_msgs::action::Fibonacci>> action_server;
+
+  // Goal handle needs to be kept alive by the server in order for client request specific to the
+  // goal to succeed.
+  std::shared_ptr<GoalHandle> current_goal_handle;
+};
+
+BENCHMARK_F(ActionClientPerformanceTest, construct_client_without_server)(benchmark::State & state)
+{
+  constexpr char action_name[] = "no_corresponding_server";
+  for (auto _ : state) {
+    auto client = rclcpp_action::create_client<Fibonacci>(node, action_name);
+
+    // Only timing construction, so destruction needs to happen explicitly while timing is paused
+    state.PauseTiming();
+    client.reset();
+    state.ResumeTiming();
+  }
+}
+
+BENCHMARK_F(ActionClientPerformanceTest, construct_client_with_server)(benchmark::State & state)
+{
+  SetUpServer(fibonacci_action_name);
+  reset_heap_counters();
+  for (auto _ : state) {
+    auto client = rclcpp_action::create_client<Fibonacci>(node, fibonacci_action_name);
+
+    // Only timing construction, so destruction needs to happen explicitly while timing is paused
+    state.PauseTiming();
+    client.reset();
+    state.ResumeTiming();
+  }
+}
+
+BENCHMARK_F(ActionClientPerformanceTest, destroy_client)(benchmark::State & state)
+{
+  for (auto _ : state) {
+    // This client does not have a corresponding server
+    state.PauseTiming();
+    auto client = rclcpp_action::create_client<Fibonacci>(node, fibonacci_action_name);
+    state.ResumeTiming();
+
+    client.reset();
+  }
+}
+
+BENCHMARK_F(ActionClientPerformanceTest, async_send_goal_only)(benchmark::State & state)
+{
+  auto client = rclcpp_action::create_client<Fibonacci>(node, fibonacci_action_name);
+  SetUpServer(fibonacci_action_name);
+  if (!client->wait_for_action_server(std::chrono::seconds(1))) {
+    state.SkipWithError("Waiting for server timed out");
+    return;
+  }
+
+  const auto goal = GetGoalOfOrder(5);
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    auto future_goal_handle = client->async_send_goal(goal);
+  }
+}
+
+BENCHMARK_F(ActionClientPerformanceTest, async_send_goal_rejected)(benchmark::State & state)
+{
+  auto client = rclcpp_action::create_client<Fibonacci>(node, fibonacci_action_name);
+  SetUpServer(fibonacci_action_name);
+  if (!client->wait_for_action_server(std::chrono::seconds(1))) {
+    state.SkipWithError("Waiting for server timed out");
+    return;
+  }
+
+  // Order of 0 is invalid
+  const auto goal = GetGoalOfOrder(0);
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    auto future_goal_handle = client->async_send_goal(goal);
+    rclcpp::spin_until_future_complete(node, future_goal_handle, std::chrono::seconds(1));
+    if (!future_goal_handle.valid()) {
+      state.SkipWithError("Shared future was invalid");
+      return;
+    }
+    if (nullptr != future_goal_handle.get()) {
+      state.SkipWithError("Invalid goal was not rejected");
+      return;
+    }
+  }
+}
+
+BENCHMARK_F(ActionClientPerformanceTest, async_send_goal_get_accepted_response)(
+  benchmark::State & state)
+{
+  auto client = rclcpp_action::create_client<Fibonacci>(node, fibonacci_action_name);
+  SetUpServer(fibonacci_action_name);
+  if (!client->wait_for_action_server(std::chrono::seconds(1))) {
+    state.SkipWithError("Waiting for server timed out");
+    return;
+  }
+
+  const auto goal = GetGoalOfOrder(10);
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    // This server's execution is deferred
+    auto future_goal_handle = client->async_send_goal(goal);
+    rclcpp::spin_until_future_complete(node, future_goal_handle, std::chrono::seconds(1));
+
+    if (!future_goal_handle.valid()) {
+      state.SkipWithError("Shared future was invalid");
+      return;
+    }
+
+    auto goal_handle = future_goal_handle.get();
+    if (rclcpp_action::GoalStatus::STATUS_ACCEPTED != goal_handle->get_status()) {
+      state.SkipWithError("Valid goal was not accepted");
+      return;
+    }
+  }
+}
+
+BENCHMARK_F(ActionClientPerformanceTest, async_get_result)(benchmark::State & state)
+{
+  auto client = rclcpp_action::create_client<Fibonacci>(node, fibonacci_action_name);
+  SetUpServer(fibonacci_action_name);
+  if (!client->wait_for_action_server(std::chrono::seconds(1))) {
+    state.SkipWithError("Waiting for server timed out");
+    return;
+  }
+
+  constexpr int expected_order = 5;
+  const auto goal = GetGoalOfOrder(expected_order);
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    // Send goal, accept and execute while timing is paused
+    state.PauseTiming();
+    auto future_goal_handle = client->async_send_goal(goal);
+
+    // Action server accepts and defers, so this spin doesn't include result
+    rclcpp::spin_until_future_complete(node, future_goal_handle, std::chrono::seconds(1));
+
+    if (!future_goal_handle.valid()) {
+      state.SkipWithError("Shared future was invalid");
+      return;
+    }
+    auto goal_handle = future_goal_handle.get();
+    if (nullptr == goal_handle) {
+      state.SkipWithError("Goal handle was a nullptr");
+      break;
+    }
+
+    // Perform actual execution and set success
+    ComputeFibonacciAndSetSuccess();
+    state.ResumeTiming();
+
+    // Measure how long it takes client to receive the succeeded result
+    auto future_result = client->async_get_result(goal_handle);
+    rclcpp::spin_until_future_complete(node, future_result, std::chrono::seconds(1));
+    const auto & wrapped_result = future_result.get();
+    if (rclcpp_action::ResultCode::SUCCEEDED != wrapped_result.code) {
+      state.SkipWithError("Fibonacci action did not succeed");
+      break;
+    }
+
+    const auto & sequence = wrapped_result.result->sequence;
+    if (sequence.size() != expected_order || sequence.back() != 3) {
+      state.SkipWithError("Fibonacci result was not correct");
+      break;
+    }
+  }
+}
+
+BENCHMARK_F(ActionClientPerformanceTest, async_cancel_goal)(benchmark::State & state)
+{
+  auto client = rclcpp_action::create_client<Fibonacci>(node, fibonacci_action_name);
+  SetUpServer(fibonacci_action_name);
+  if (!client->wait_for_action_server(std::chrono::seconds(1))) {
+    state.SkipWithError("Waiting for server timed out");
+    return;
+  }
+
+  constexpr int expected_order = 5;
+  const auto goal = GetGoalOfOrder(expected_order);
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    state.PauseTiming();
+    auto future_goal_handle = client->async_send_goal(goal);
+
+    // Action server accepts and defers, so action can be canceled
+    rclcpp::spin_until_future_complete(node, future_goal_handle, std::chrono::seconds(1));
+    auto goal_handle = future_goal_handle.get();
+    state.ResumeTiming();
+
+    auto future_cancel = client->async_cancel_goal(goal_handle);
+    rclcpp::spin_until_future_complete(node, future_cancel, std::chrono::seconds(1));
+    auto cancel_response = future_cancel.get();
+
+    using CancelResponse = test_msgs::action::Fibonacci::Impl::CancelGoalService::Response;
+    if (CancelResponse::ERROR_NONE != cancel_response->return_code) {
+      state.SkipWithError("Cancel request did not succeed");
+      break;
+    }
+  }
+}
+
+BENCHMARK_F(ActionClientPerformanceTest, async_cancel_all_goals)(benchmark::State & state)
+{
+  auto client = rclcpp_action::create_client<Fibonacci>(node, fibonacci_action_name);
+  SetUpServer(fibonacci_action_name);
+  if (!client->wait_for_action_server(std::chrono::seconds(1))) {
+    state.SkipWithError("Waiting for server timed out");
+    return;
+  }
+
+  constexpr int expected_order = 5;
+  const auto goal = GetGoalOfOrder(expected_order);
+  constexpr int num_concurrently_inflight_goals = 10u;
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    state.PauseTiming();
+    for (int i = 0; i < num_concurrently_inflight_goals; ++i) {
+      auto future_goal_handle = client->async_send_goal(goal);
+      rclcpp::spin_until_future_complete(node, future_goal_handle, std::chrono::seconds(1));
+    }
+    // Action server accepts and defers, so action can be canceled
+    state.ResumeTiming();
+
+    auto future_cancel_all = client->async_cancel_all_goals();
+    rclcpp::spin_until_future_complete(node, future_cancel_all, std::chrono::seconds(1));
+    auto cancel_response = future_cancel_all.get();
+
+    using CancelResponse = test_msgs::action::Fibonacci::Impl::CancelGoalService::Response;
+    if (CancelResponse::ERROR_NONE != cancel_response->return_code) {
+      state.SkipWithError("Cancel request did not succeed");
+      break;
+    }
+  }
+}

rclcpp_action/test/benchmark/benchmark_action_server.cpp

@@ -0,0 +1,318 @@
+// 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 <memory>
+#include <string>
+
+#include "performance_test_fixture/performance_test_fixture.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "test_msgs/action/fibonacci.hpp"
+
+using performance_test_fixture::PerformanceTest;
+
+using Fibonacci = test_msgs::action::Fibonacci;
+using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+using CancelResponse = typename Fibonacci::Impl::CancelGoalService::Response;
+using GoalUUID = rclcpp_action::GoalUUID;
+
+constexpr char fibonacci_action_name[] = "fibonacci";
+
+class ActionServerPerformanceTest : public PerformanceTest
+{
+public:
+  void SetUp(benchmark::State & state)
+  {
+    rclcpp::init(0, nullptr);
+    node = std::make_shared<rclcpp::Node>("node", "ns");
+    action_client =
+      rclcpp_action::create_client<Fibonacci>(node, fibonacci_action_name);
+    performance_test_fixture::PerformanceTest::SetUp(state);
+  }
+
+  void TearDown(benchmark::State & state)
+  {
+    performance_test_fixture::PerformanceTest::TearDown(state);
+
+    action_client.reset();
+    node.reset();
+    rclcpp::shutdown();
+  }
+
+  auto AsyncSendGoalOfOrder(const int order)
+  {
+    test_msgs::action::Fibonacci::Goal goal;
+    goal.order = order;
+
+    return action_client->async_send_goal(goal);
+  }
+
+protected:
+  std::shared_ptr<rclcpp::Node> node;
+  std::shared_ptr<rclcpp_action::Client<Fibonacci>> action_client;
+};
+
+BENCHMARK_F(ActionServerPerformanceTest, construct_server_without_client)(benchmark::State & state)
+{
+  constexpr char action_name[] = "no_corresponding_client";
+  for (auto _ : state) {
+    auto action_server = rclcpp_action::create_server<Fibonacci>(
+      node, action_name,
+      [](const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>) {
+        return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
+      },
+      [](std::shared_ptr<GoalHandle>) {
+        return rclcpp_action::CancelResponse::ACCEPT;
+      },
+      [](std::shared_ptr<GoalHandle>) {});
+    benchmark::DoNotOptimize(action_server);
+    benchmark::ClobberMemory();
+
+    state.PauseTiming();
+    action_server.reset();
+    state.ResumeTiming();
+  }
+}
+
+BENCHMARK_F(ActionServerPerformanceTest, construct_server_with_client)(benchmark::State & state)
+{
+  for (auto _ : state) {
+    auto action_server = rclcpp_action::create_server<Fibonacci>(
+      node, fibonacci_action_name,
+      [](const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>) {
+        return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
+      },
+      [](std::shared_ptr<GoalHandle>) {
+        return rclcpp_action::CancelResponse::ACCEPT;
+      },
+      [](std::shared_ptr<GoalHandle>) {});
+    benchmark::DoNotOptimize(action_server);
+    benchmark::ClobberMemory();
+
+    state.PauseTiming();
+    action_server.reset();
+    state.ResumeTiming();
+  }
+}
+
+BENCHMARK_F(ActionServerPerformanceTest, destroy_server)(benchmark::State & state)
+{
+  for (auto _ : state) {
+    state.PauseTiming();
+    auto action_server = rclcpp_action::create_server<Fibonacci>(
+      node, fibonacci_action_name,
+      [](const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>) {
+        return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
+      },
+      [](std::shared_ptr<GoalHandle>) {
+        return rclcpp_action::CancelResponse::ACCEPT;
+      },
+      [](std::shared_ptr<GoalHandle>) {});
+    state.ResumeTiming();
+    benchmark::DoNotOptimize(action_server);
+    benchmark::ClobberMemory();
+
+    action_server.reset();
+  }
+}
+
+BENCHMARK_F(ActionServerPerformanceTest, action_server_accept_goal)(benchmark::State & state)
+{
+  std::shared_ptr<GoalHandle> current_goal_handle = nullptr;
+  auto action_server = rclcpp_action::create_server<Fibonacci>(
+    node, fibonacci_action_name,
+    [](const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>) {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
+    },
+    [](std::shared_ptr<GoalHandle>) {
+      return rclcpp_action::CancelResponse::ACCEPT;
+    },
+    [&current_goal_handle](std::shared_ptr<GoalHandle> goal_handle) {
+      current_goal_handle = goal_handle;
+    });
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    state.PauseTiming();
+    auto client_goal_handle_future = AsyncSendGoalOfOrder(1);
+    state.ResumeTiming();
+
+    rclcpp::spin_until_future_complete(node, client_goal_handle_future);
+    auto goal_handle = client_goal_handle_future.get();
+    if (rclcpp_action::GoalStatus::STATUS_ACCEPTED != goal_handle->get_status()) {
+      state.SkipWithError("Valid goal was not accepted");
+      return;
+    }
+  }
+}
+
+BENCHMARK_F(ActionServerPerformanceTest, action_server_cancel_goal)(benchmark::State & state)
+{
+  // The goal handle needs to be assigned to a variable for the lifetime of the goal so that it is
+  // not cleaned up before the cancel request is received and processed.
+  std::shared_ptr<GoalHandle> server_goal_handle = nullptr;
+  auto action_server = rclcpp_action::create_server<Fibonacci>(
+    node, fibonacci_action_name,
+    [](const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>) {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
+    },
+    [](std::shared_ptr<GoalHandle>) {
+      return rclcpp_action::CancelResponse::ACCEPT;
+    },
+    [&server_goal_handle](std::shared_ptr<GoalHandle> goal_handle) {
+      server_goal_handle = goal_handle;
+    });
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    state.PauseTiming();
+    auto client_goal_handle_future = AsyncSendGoalOfOrder(1);
+    // This spin completes when the goal has been accepted, but not executed because server
+    // responds with ACCEPT_AND_DEFER
+    rclcpp::spin_until_future_complete(node, client_goal_handle_future, std::chrono::seconds(1));
+    auto client_goal_handle = client_goal_handle_future.get();
+    auto future_cancel = action_client->async_cancel_goal(client_goal_handle);
+    state.ResumeTiming();
+
+    rclcpp::spin_until_future_complete(node, future_cancel, std::chrono::seconds(1));
+    auto cancel_response = future_cancel.get();
+    using CancelResponse = test_msgs::action::Fibonacci::Impl::CancelGoalService::Response;
+    if (CancelResponse::ERROR_NONE != cancel_response->return_code) {
+      state.SkipWithError("Cancel request did not succeed");
+      break;
+    }
+  }
+}
+
+BENCHMARK_F(ActionServerPerformanceTest, action_server_execute_goal)(benchmark::State & state)
+{
+  std::shared_ptr<GoalHandle> server_goal_handle = nullptr;
+  auto action_server = rclcpp_action::create_server<Fibonacci>(
+    node, fibonacci_action_name,
+    [](const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>) {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
+    },
+    [](std::shared_ptr<GoalHandle>) {
+      return rclcpp_action::CancelResponse::ACCEPT;
+    },
+    [&server_goal_handle](std::shared_ptr<GoalHandle> goal_handle) {
+      server_goal_handle = goal_handle;
+    });
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    state.PauseTiming();
+    auto client_goal_handle_future = AsyncSendGoalOfOrder(1);
+
+    rclcpp::spin_until_future_complete(node, client_goal_handle_future);
+    auto goal_handle = client_goal_handle_future.get();
+    if (rclcpp_action::GoalStatus::STATUS_ACCEPTED != goal_handle->get_status()) {
+      state.SkipWithError("Valid goal was not accepted");
+      return;
+    }
+    state.ResumeTiming();
+
+    server_goal_handle->execute();
+  }
+}
+
+BENCHMARK_F(ActionServerPerformanceTest, action_server_set_success)(benchmark::State & state)
+{
+  constexpr int goal_order = 1;
+  std::shared_ptr<GoalHandle> server_goal_handle = nullptr;
+  auto action_server = rclcpp_action::create_server<Fibonacci>(
+    node, fibonacci_action_name,
+    [](const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>) {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
+    },
+    [](std::shared_ptr<GoalHandle>) {
+      return rclcpp_action::CancelResponse::ACCEPT;
+    },
+    [&server_goal_handle](std::shared_ptr<GoalHandle> goal_handle) {
+      server_goal_handle = goal_handle;
+    });
+
+  // MSVC and Clang disagree how goal_order should be captured here. Though this capture is a bit
+  // too wide, they at least could agree it was fine. In my testing MSVC errored if goal_order was
+  // not captured, but clang would warn if it was explicitly captured.
+  const auto result = [&]() {
+      auto result = std::make_shared<Fibonacci::Result>();
+      for (int i = 0; i < goal_order; ++i) {
+        // Not the fibonacci sequence, but that's not important to this benchmark
+        result->sequence.push_back(i);
+      }
+      return result;
+    } ();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    state.PauseTiming();
+    auto client_goal_handle_future = AsyncSendGoalOfOrder(goal_order);
+
+    rclcpp::spin_until_future_complete(node, client_goal_handle_future);
+    auto goal_handle = client_goal_handle_future.get();
+    if (rclcpp_action::GoalStatus::STATUS_ACCEPTED != goal_handle->get_status()) {
+      state.SkipWithError("Valid goal was not accepted");
+      return;
+    }
+    server_goal_handle->execute();
+    state.ResumeTiming();
+
+    server_goal_handle->succeed(result);
+  }
+}
+
+BENCHMARK_F(ActionServerPerformanceTest, action_server_abort)(benchmark::State & state)
+{
+  constexpr int goal_order = 1;
+  std::shared_ptr<GoalHandle> server_goal_handle = nullptr;
+  auto action_server = rclcpp_action::create_server<Fibonacci>(
+    node, fibonacci_action_name,
+    [](const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>) {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
+    },
+    [](std::shared_ptr<GoalHandle>) {
+      return rclcpp_action::CancelResponse::ACCEPT;
+    },
+    [&server_goal_handle](std::shared_ptr<GoalHandle> goal_handle) {
+      server_goal_handle = goal_handle;
+    });
+
+  // Capturing with & because MSVC and Clang disagree about how to capture goal_order
+  const auto result = [&]() {
+      auto result = std::make_shared<Fibonacci::Result>();
+      for (int i = 0; i < goal_order; ++i) {
+        // Not the fibonacci sequence, but that's not important to this benchmark
+        result->sequence.push_back(i);
+      }
+      return result;
+    } ();
+
+  reset_heap_counters();
+  for (auto _ : state) {
+    state.PauseTiming();
+    auto client_goal_handle_future = AsyncSendGoalOfOrder(goal_order);
+
+    rclcpp::spin_until_future_complete(node, client_goal_handle_future);
+    auto goal_handle = client_goal_handle_future.get();
+    if (rclcpp_action::GoalStatus::STATUS_ACCEPTED != goal_handle->get_status()) {
+      state.SkipWithError("Valid goal was not accepted");
+      return;
+    }
+    server_goal_handle->execute();
+    state.ResumeTiming();
+
+    server_goal_handle->abort(result);
+  }
+}

rclcpp_action/test/test_client.cpp

@@ -332,7 +332,7 @@ TEST_F(TestClient, construction_and_destruction_callback_group)
 TEST_F(TestClient, construction_and_destruction_rcl_errors)
 {
   {
-    auto mock = mocking_utils::inject_on_return(
+    auto mock = mocking_utils::patch_and_return(
       "lib:rclcpp_action", rcl_action_client_fini, RCL_RET_ERROR);
     // It just logs an error message and continues
     EXPECT_NO_THROW(

rclcpp_action/test/test_server.cpp

@@ -1156,7 +1156,7 @@ TEST_F(TestGoalRequestServer, publish_status_publish_status_errors)
 
 TEST_F(TestGoalRequestServer, execute_goal_request_received_take_failed)
 {
-  auto mock = mocking_utils::inject_on_return(
+  auto mock = mocking_utils::patch_and_return(
     "lib:rclcpp_action", rcl_action_take_goal_request, RCL_RET_ACTION_SERVER_TAKE_FAILED);
   try {
     SendClientGoalRequest(std::chrono::milliseconds(100));

rclcpp_components/CHANGELOG.rst

@@ -2,6 +2,12 @@
 Changelog for package rclcpp_components
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+5.1.0 (2020-11-02)
+------------------
+* Update maintainers (`#1384 <https://github.com/ros2/rclcpp/issues/1384>`_)
+* ComponentManager: switch off parameter services and event publisher (`#1333 <https://github.com/ros2/rclcpp/issues/1333>`_)
+* Contributors: Ivan Santiago Paunovic, Martijn Buijs
+
 5.0.0 (2020-09-18)
 ------------------
 

rclcpp_components/QUALITY_DECLARATION.md

@@ -2,7 +2,7 @@ This document is a declaration of software quality for the `rclcpp_components` p
 
 # rclcpp_components Quality Declaration
 
-The package `rclcpp_components` claims to be in the **Quality Level 3** category.
+The package `rclcpp_components` claims to be in the **Quality Level 2** category.
 
 Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-quality-categories) of the ROS2 developer guide.
 
@@ -151,25 +151,25 @@ It also has several test dependencies, which do not affect the resulting quality
 
 The `ament_index_cpp` package provides a C++ API to access the ament resource index.
 
-It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ament/ament_index/blob/master/ament_index_cpp/QUALITY_DECLARATION.md).
+It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ament/ament_index/blob/master/ament_index_cpp/QUALITY_DECLARATION.md).
 
 #### `class_loader`
 
 The `class_loader` package provides a ROS-independent package for loading plugins during runtime
 
-It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros/class_loader/blob/ros2/QUALITY_DECLARATION.md).
+It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros/class_loader/blob/ros2/QUALITY_DECLARATION.md).
 
 #### `composition_interfaces`
 
 The `composition_interfaces` package contains message and service definitions for managing composable nodes in a container process.
 
-It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/composition_interfaces/QUALITY_DECLARATION.md).
+It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/composition_interfaces/QUALITY_DECLARATION.md).
 
 #### `rclcpp`
 
 The `rclcpp` package provides the ROS client library in C++.
 
-It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/master/rclcpp/QUALITY_DECLARATION.md).
+It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/master/rclcpp/QUALITY_DECLARATION.md).
 
 #### `rcpputils`
 

rclcpp_components/README.md

@@ -6,4 +6,4 @@ Visit the [rclcpp_components API documentation](http://docs.ros2.org/latest/api/
 
 ## Quality Declaration
 
-This package claims to be in the **Quality Level 3** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details.
+This package claims to be in the **Quality Level 2** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details.

rclcpp_components/package.xml

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

rclcpp_lifecycle/CHANGELOG.rst

@@ -3,6 +3,13 @@ Changelog for package rclcpp_lifecycle
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+5.1.0 (2020-11-02)
+------------------
+* Increase test timeouts of slow running tests with rmw_connext_cpp (`#1400 <https://github.com/ros2/rclcpp/issues/1400>`_)
+* Update maintainers (`#1384 <https://github.com/ros2/rclcpp/issues/1384>`_)
+* Add clock qos to node options (`#1375 <https://github.com/ros2/rclcpp/issues/1375>`_)
+* Contributors: Ivan Santiago Paunovic, brawner
+
 5.0.0 (2020-09-18)
 ------------------
 * Increase test coverage of rclcpp_lifecycle to 96% (`#1298 <https://github.com/ros2/rclcpp/issues/1298>`_)

rclcpp_lifecycle/QUALITY_DECLARATION.md

@@ -2,7 +2,7 @@ This document is a declaration of software quality for the `rclcpp_lifecycle` pa
 
 # rclcpp_lifecycle Quality Declaration
 
-The package `rclcpp_lifecycle` claims to be in the **Quality Level 3** category.
+The package `rclcpp_lifecycle` claims to be in the **Quality Level 2** category.
 
 Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-quality-categories) of the ROS2 developer guide.
 
@@ -151,25 +151,25 @@ It also has several test dependencies, which do not affect the resulting quality
 
 The `lifecycle_msgs` package contains message and service definitions for managing lifecycle nodes.
 
-It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/lifecycle_msgs/QUALITY_DECLARATION.md).
+It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/lifecycle_msgs/QUALITY_DECLARATION.md).
 
 #### `rclcpp`
 
 The `rclcpp` package provides the ROS client library in C++.
 
-It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/master/rclcpp/QUALITY_DECLARATION.md).
+It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/master/rclcpp/QUALITY_DECLARATION.md).
 
 #### `rcl_lifecycle`
 
 The `rcl_lifecycle` package provides functionality for ROS 2 lifecycle nodes in C.
 
-It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl_lifecycle/QUALITY_DECLARATION.md).
+It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl_lifecycle/QUALITY_DECLARATION.md).
 
 #### `rosidl_typesupport_cpp`
 
 The `rosidl_typesupport_cpp` package generates the type support for C++ messages.
 
-It is **Quality Level 3**, see its [Quality Declaration document](https://github.com/ros2/rosidl_typesupport/blob/master/rosidl_typesupport_cpp/QUALITY_DECLARATION.md).
+It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros2/rosidl_typesupport/blob/master/rosidl_typesupport_cpp/QUALITY_DECLARATION.md).
 
 #### `rmw`
 

rclcpp_lifecycle/README.md

@@ -6,4 +6,4 @@ Visit the [rclcpp_lifecycle API documentation](http://docs.ros2.org/latest/api/r
 
 ## Quality Declaration
 
-This package claims to be in the **Quality Level 3** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details.
+This package claims to be in the **Quality Level 2** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details.

rclcpp_lifecycle/package.xml

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