8.1.0

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

rclcpp/CHANGELOG.rst

@@ -2,6 +2,95 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+8.1.0 (2021-03-25)
+------------------
+* Remove rmw_connext_cpp references. (`#1595 <https://github.com/ros2/rclcpp/issues/1595>`_)
+* Add API for checking QoS profile compatibility (`#1554 <https://github.com/ros2/rclcpp/issues/1554>`_)
+* Document misuse of parameters callback (`#1590 <https://github.com/ros2/rclcpp/issues/1590>`_)
+* use const auto & to iterate over parameters (`#1593 <https://github.com/ros2/rclcpp/issues/1593>`_)
+* Contributors: Chris Lalancette, Jacob Perron, Karsten Knese
+
+8.0.0 (2021-03-23)
+------------------
+* Guard against integer overflow in duration conversion (`#1584 <https://github.com/ros2/rclcpp/issues/1584>`_)
+* Contributors: Jacob Perron
+
+7.0.1 (2021-03-22)
+------------------
+* get_parameters service should return empty if undeclared parameters are allowed (`#1514 <https://github.com/ros2/rclcpp/issues/1514>`_)
+* Made 'Context::shutdown_reason' function a const function (`#1578 <https://github.com/ros2/rclcpp/issues/1578>`_)
+* Contributors: Tomoya Fujita, suab321321
+
+7.0.0 (2021-03-18)
+------------------
+* Document design decisions that were made for statically typed parameters (`#1568 <https://github.com/ros2/rclcpp/issues/1568>`_)
+* Fix doc typo in CallbackGroup constructor (`#1582 <https://github.com/ros2/rclcpp/issues/1582>`_)
+* Enable qos parameter overrides for the /parameter_events topic  (`#1532 <https://github.com/ros2/rclcpp/issues/1532>`_)
+* Add support for rmw_connextdds (`#1574 <https://github.com/ros2/rclcpp/issues/1574>`_)
+* Remove 'struct' from the rcl_time_jump_t. (`#1577 <https://github.com/ros2/rclcpp/issues/1577>`_)
+* Add tests for declaring statically typed parameters when undeclared parameters are allowed (`#1575 <https://github.com/ros2/rclcpp/issues/1575>`_)
+* Quiet clang memory leak warning on "DoNotOptimize". (`#1571 <https://github.com/ros2/rclcpp/issues/1571>`_)
+* Add ParameterEventsSubscriber class (`#829 <https://github.com/ros2/rclcpp/issues/829>`_)
+* When a parameter change is rejected, the parameters map shouldn't be updated. (`#1567 <https://github.com/ros2/rclcpp/pull/1567>`_)
+* Fix when to throw the NoParameterOverrideProvided exception. (`#1567 <https://github.com/ros2/rclcpp/pull/1567>`_)
+* Fix SEGV caused by order of destruction of Node sub-interfaces (`#1469 <https://github.com/ros2/rclcpp/issues/1469>`_)
+* Fix benchmark test failure introduced in `#1522 <https://github.com/ros2/rclcpp/issues/1522>`_ (`#1564 <https://github.com/ros2/rclcpp/issues/1564>`_)
+* Fix documented example in create_publisher (`#1558 <https://github.com/ros2/rclcpp/issues/1558>`_)
+* Enforce static parameter types (`#1522 <https://github.com/ros2/rclcpp/issues/1522>`_)
+* Allow timers to keep up the intended rate in MultiThreadedExecutor (`#1516 <https://github.com/ros2/rclcpp/issues/1516>`_)
+* Fix UBSAN warnings in any_subscription_callback. (`#1551 <https://github.com/ros2/rclcpp/issues/1551>`_)
+* Fix runtime error: reference binding to null pointer of type (`#1547 <https://github.com/ros2/rclcpp/issues/1547>`_)
+* Contributors: Andrea Sorbini, Chris Lalancette, Colin MacKenzie, Ivan Santiago Paunovic, Jacob Perron, Steven! Ragnarök, bpwilcox, tomoya
+
+6.3.1 (2021-02-08)
+------------------
+* Reference test resources directly from source tree (`#1543 <https://github.com/ros2/rclcpp/issues/1543>`_)
+* clear statistics after window reset (`#1531 <https://github.com/ros2/rclcpp/issues/1531>`_) (`#1535 <https://github.com/ros2/rclcpp/issues/1535>`_)
+* Fix a minor string error in the topic_statistics test. (`#1541 <https://github.com/ros2/rclcpp/issues/1541>`_)
+* Avoid `Resource deadlock avoided` if use intra_process_comms (`#1530 <https://github.com/ros2/rclcpp/issues/1530>`_)
+* Avoid an object copy in parameter_value.cpp. (`#1538 <https://github.com/ros2/rclcpp/issues/1538>`_)
+* Assert that the publisher_list size is 1. (`#1537 <https://github.com/ros2/rclcpp/issues/1537>`_)
+* Don't access objects after they have been std::move (`#1536 <https://github.com/ros2/rclcpp/issues/1536>`_)
+* Update for checking correct variable (`#1534 <https://github.com/ros2/rclcpp/issues/1534>`_)
+* Destroy msg extracted from LoanedMessage. (`#1305 <https://github.com/ros2/rclcpp/issues/1305>`_)
+* Contributors: Chen Lihui, Chris Lalancette, Ivan Santiago Paunovic, Miaofei Mei, Scott K Logan, William Woodall, hsgwa
+
+6.3.0 (2021-01-25)
+------------------
+* Add instrumentation for linking a timer to a node (`#1500 <https://github.com/ros2/rclcpp/issues/1500>`_)
+* Fix error when using IPC with StaticSingleThreadExecutor (`#1520 <https://github.com/ros2/rclcpp/issues/1520>`_)
+* Change to using unique_ptrs for DummyExecutor. (`#1517 <https://github.com/ros2/rclcpp/issues/1517>`_)
+* Allow reconfiguring 'clock' topic qos (`#1512 <https://github.com/ros2/rclcpp/issues/1512>`_)
+* Allow to add/remove nodes thread safely in rclcpp::Executor  (`#1505 <https://github.com/ros2/rclcpp/issues/1505>`_)
+* Call rclcpp::shutdown in test_node for clean shutdown on Windows (`#1515 <https://github.com/ros2/rclcpp/issues/1515>`_)
+* Reapply "Add get_logging_directory method to rclcpp::Logger (`#1509 <https://github.com/ros2/rclcpp/issues/1509>`_)" (`#1513 <https://github.com/ros2/rclcpp/issues/1513>`_)
+* use describe_parameters of parameter client for test (`#1499 <https://github.com/ros2/rclcpp/issues/1499>`_)
+* Revert "Add get_logging_directory method to rclcpp::Logger (`#1509 <https://github.com/ros2/rclcpp/issues/1509>`_)" (`#1511 <https://github.com/ros2/rclcpp/issues/1511>`_)
+* Add get_logging_directory method to rclcpp::Logger (`#1509 <https://github.com/ros2/rclcpp/issues/1509>`_)
+* Contributors: Chris Lalancette, Christophe Bedard, Ivan Santiago Paunovic, eboasson, mauropasse, tomoya
+
+6.2.0 (2021-01-08)
+------------------
+* Better documentation for the QoS class (`#1508 <https://github.com/ros2/rclcpp/issues/1508>`_)
+* Modify excluding callback duration from topic statistics (`#1492 <https://github.com/ros2/rclcpp/issues/1492>`_)
+* Make the test of graph users more robust. (`#1504 <https://github.com/ros2/rclcpp/issues/1504>`_)
+* Make sure to wait for graph change events in test_node_graph. (`#1503 <https://github.com/ros2/rclcpp/issues/1503>`_)
+* add timeout to SyncParametersClient methods (`#1493 <https://github.com/ros2/rclcpp/issues/1493>`_)
+* Fix wrong test expectations (`#1497 <https://github.com/ros2/rclcpp/issues/1497>`_)
+* Update create_publisher/subscription documentation, clarifying when a parameters interface is required (`#1494 <https://github.com/ros2/rclcpp/issues/1494>`_)
+* Fix string literal warnings (`#1442 <https://github.com/ros2/rclcpp/issues/1442>`_)
+* support describe_parameters methods to parameter client. (`#1453 <https://github.com/ros2/rclcpp/issues/1453>`_)
+* Contributors: Audrow Nash, Chris Lalancette, Ivan Santiago Paunovic, Nikolai Morin, hsgwa, tomoya
+
+6.1.0 (2020-12-10)
+------------------
+* Add getters to rclcpp::qos and rclcpp::Policy enum classes (`#1467 <https://github.com/ros2/rclcpp/issues/1467>`_)
+* Change nullptr checks to use ASSERT_TRUE. (`#1486 <https://github.com/ros2/rclcpp/issues/1486>`_)
+* Adjust logic around finding and erasing guard_condition (`#1474 <https://github.com/ros2/rclcpp/issues/1474>`_)
+* Update QDs to QL 1 (`#1477 <https://github.com/ros2/rclcpp/issues/1477>`_)
+* Add performance tests for parameter transport (`#1463 <https://github.com/ros2/rclcpp/issues/1463>`_)
+* Contributors: Chris Lalancette, Ivan Santiago Paunovic, Scott K Logan, Stephen Brawner
+
 6.0.0 (2020-11-18)
 ------------------
 * Move ownership of shutdown_guard_condition to executors/graph_listener (`#1404 <https://github.com/ros2/rclcpp/issues/1404>`_)

rclcpp/CMakeLists.txt

@@ -39,6 +39,8 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/clock.cpp
   src/rclcpp/context.cpp
   src/rclcpp/contexts/default_context.cpp
+  src/rclcpp/detail/mutex_two_priorities.cpp
+  src/rclcpp/detail/resolve_parameter_overrides.cpp
   src/rclcpp/detail/rmw_implementation_specific_payload.cpp
   src/rclcpp/detail/rmw_implementation_specific_publisher_payload.cpp
   src/rclcpp/detail/rmw_implementation_specific_subscription_payload.cpp
@@ -79,6 +81,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/parameter.cpp
   src/rclcpp/parameter_value.cpp
   src/rclcpp/parameter_client.cpp
+  src/rclcpp/parameter_event_handler.cpp
   src/rclcpp/parameter_events_filter.cpp
   src/rclcpp/parameter_map.cpp
   src/rclcpp/parameter_service.cpp

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 2** category.
+The package `rclcpp` claims to be in the **Quality Level 1** category when it is used with a **Quality Level 1** middleware.
 
 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.
 
@@ -82,13 +82,13 @@ The API is publicly available in its [ROS 2 API documentation](http://docs.ros2.
 
 The license for `rclcpp` is Apache 2.0, and a summary is in each source file, the type is declared in the [`package.xml`](./package.xml) manifest file, and a full copy of the license is in the [`LICENSE`](../LICENSE) file.
 
-There is an automated test which runs a linter that ensures each file has a license statement. [Here](http://build.ros2.org/view/Fpr/job/Fpr__rclcpp__ubuntu_focal_amd64/lastSuccessfulBuild/testReport/rclcpp/) can be found a list with the latest results of the various linters being run on the package.
+There is an automated test which runs a linter that ensures each file has a license statement. [Here](http://build.ros2.org/view/Rpr/job/Rpr__rclcpp__ubuntu_focal_amd64/lastCompletedBuild/testReport/rclcpp/) can be found a list with the latest results of the various linters being run on the package.
 
 ### Copyright Statements [3.iv]
 
 The copyright holders each provide a statement of copyright in each source code file in `rclcpp`.
 
-There is an automated test which runs a linter that ensures each file has at least one copyright statement. Latest linter result report can be seen [here](http://build.ros2.org/view/Fpr/job/Fpr__rclcpp__ubuntu_focal_amd64/lastSuccessfulBuild/testReport/rclcpp/copyright/).
+There is an automated test which runs a linter that ensures each file has at least one copyright statement. Latest linter result report can be seen [here](http://build.ros2.org/view/Rpr/job/Rpr__rclcpp__ubuntu_focal_amd64/lastCompletedBuild/testReport/rclcpp/copyright/).
 
 ## Testing [4]
 
@@ -121,13 +121,21 @@ This includes:
 
 Changes are required to make a best effort to keep or increase coverage before being accepted, but decreases are allowed if properly justified and accepted by reviewers.
 
-Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_coverage/lastSuccessfulBuild/cobertura/src_ros2_rclcpp_rclcpp_src_rclcpp/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://index.ros.org/doc/ros2/Contributing/ROS-2-On-boarding-Guide/#note-on-coverage-runs).
+Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_coverage/lastCompletedBuild/cobertura/src_ros2_rclcpp_rclcpp_src_rclcpp/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://index.ros.org/doc/ros2/Contributing/ROS-2-On-boarding-Guide/#note-on-coverage-runs).
 
 `rclcpp` has a line coverage `>= 95%`, which is calculated over all directories within `rclcpp` with the exception of the `experimental` directory.
 
 ### Performance [4.iv]
 
-It is not yet defined if this package requires performance testing and how addresses this topic.
+`rclcpp` follows the recommendations for performance testing of C/C++ code in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#performance), and opts to do performance analysis on each release rather than each change.
+
+The performance tests of `rclcpp` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/master/rclcpp/test/benchmark).
+
+System level performance benchmarks that cover features of `rclcpp` can be found at:
+* [Benchmarks](http://build.ros2.org/view/Rci/job/Rci__benchmark_ubuntu_focal_amd64/BenchmarkTable/)
+* [Performance](http://build.ros2.org/view/Rci/job/Rci__nightly-performance_ubuntu_focal_amd64/lastCompletedBuild/)
+
+Changes that introduce regressions in performance must be adequately justified in order to be accepted and merged.
 
 ### Linters and Static Analysis [4.v]
 
@@ -155,43 +163,43 @@ 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 2**, see its [Quality Declaration document](https://github.com/ros-tooling/libstatistics_collector/tree/master/QUALITY_DECLARATION.md).
+It is **Quality Level 1**, 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 2**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl/QUALITY_DECLARATION.md).
+It is **Quality Level 1**, 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 2**, see its [Quality Declaration document](https://github.com/ros2/rcl/tree/master/rcl_yaml_param_parser/QUALITY_DECLARATION.md).
+It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl/tree/master/rcl_yaml_param_parser/QUALITY_DECLARATION.md).
 
 #### `rcpputils`
 
 The `rcpputils` package provides an API which contains common utilities and data structures useful when programming in C++.
 
-It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros2/rcpputils/blob/master/QUALITY_DECLARATION.md).
+It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcpputils/blob/master/QUALITY_DECLARATION.md).
 
 #### `rcutils`
 
 The `rcutils` package provides an API which contains common utilities and data structures useful when programming in C.
 
-It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros2/rcutils/blob/master/QUALITY_DECLARATION.md).
+It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcutils/blob/master/QUALITY_DECLARATION.md).
 
 #### `rmw`
 
 `rmw` is the ROS 2 middleware library.
 
-It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros2/rmw/blob/master/rmw/QUALITY_DECLARATION.md).
+It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rmw/blob/master/rmw/QUALITY_DECLARATION.md).
 
 #### `statistics_msgs`
 
 The `statistics_msgs` package contains ROS 2 message definitions for reporting statistics for topics and system resources.
 
-It is **Quality Level 2**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/statistics_msgs/QUALITY_DECLARATION.md).
+It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/statistics_msgs/QUALITY_DECLARATION.md).
 
 #### `tracetools`
 

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 2** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details.
+This package claims to be in the **Quality Level 1** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details.

rclcpp/doc/notes_on_statically_typed_parameters.md

@@ -0,0 +1,141 @@
+# Notes on statically typed parameters
+
+## Introduction
+
+Until ROS 2 Foxy, all parameters could change type anytime, except if the user installed a parameter callback to reject a change.
+This could generate confusing errors, for example:
+
+```
+$ ros2 run demo_nodes_py listener &
+$ ros2 param set /listener use_sim_time not_a_boolean
+[ERROR] [1614712713.233733147] [listener]: use_sim_time parameter set to something besides a bool
+Set parameter successful
+$ ros2 param get /listener use_sim_time
+String value is: not_a_boolean
+```
+
+For most use cases, having static parameter types is enough.
+This article documents some of the decisions that were made when implementing static parameter types enforcement in:
+
+* https://github.com/ros2/rclcpp/pull/1522
+* https://github.com/ros2/rclpy/pull/683
+
+## Allowing dynamically typed parameters
+
+There might be some scenarios where dynamic typing is desired, so a `dynamic_typing` field was added to the [parameter descriptor](https://github.com/ros2/rcl_interfaces/blob/09b5ed93a733adb9deddc47f9a4a8c6e9f584667/rcl_interfaces/msg/ParameterDescriptor.msg#L25).
+It defaults to `false`.
+
+For example:
+
+```cpp
+rcl_interfaces::msg::ParameterDescriptor descriptor;
+descriptor.dynamic_typing = true;
+
+node->declare_parameter("dynamically_typed_parameter", rclcpp::ParameterValue{}, descriptor);
+```
+
+```py
+rcl_interfaces.msg.ParameterDescriptor descriptor;
+descriptor.dynamic_typing = True;
+
+node.declare_parameter("dynamically_typed_parameter", None, descriptor);
+```
+
+## How is the parameter type specified?
+
+The parameter type will be inferred from the default value provided when declaring it.
+
+## Statically typed parameters when allowing undeclared parameters
+
+When undeclared parameters are allowed and a parameter is set without a previous declaration, the parameter will be dynamically typed.
+This is consistent with other allowed behaviors when undeclared parameters are allowed, e.g. trying to get an undeclared parameter returns "NOT_SET".
+Parameter declarations will remain special and dynamic or static typing will be used based on the parameter descriptor (default to static).
+
+## Declaring a parameter without a default value
+
+There might be cases were a default value does not make sense and the user must always provide an override.
+In those cases, the parameter type can be specified explicitly:
+
+```cpp
+// method signature
+template<typename T>
+Node::declare_parameter<T>(std::string name, rcl_interfaces::msg::ParameterDescriptor = rcl_interfaces::msg::ParameterDescriptor{});
+// or alternatively
+Node::declare_parameter(std::string name, rclcpp::ParameterType type, rcl_interfaces::msg::ParameterDescriptor = rcl_interfaces::msg::ParameterDescriptor{});
+
+// examples
+node->declare_parameter<int64_t>("my_integer_parameter");  // declare an integer parameter
+node->declare_parameter("another_integer_parameter", rclcpp::ParameterType::PARAMETER_INTEGER);  // another way to do the same
+```
+
+```py
+# method signature
+Node.declare_parameter(name: str, param_type: rclpy.Parameter.Type, descriptor: rcl_interfaces.msg.ParameterDescriptor = rcl_interfaces.msg.ParameterDescriptor())
+
+# example
+node.declare_parameter('my_integer_parameter', rclpy.Parameter.Type.INTEGER);  # declare an integer parameter
+```
+
+If the parameter may be unused, but when used requires a parameter override, then you could conditionally declare it:
+
+```cpp
+auto mode = node->declare_parameter("mode", "modeA");  // "mode" parameter is an string
+if (mode == "modeB") {
+    node->declare_parameter<int64_t>("param_needed_when_mode_b");  // when "modeB", the user must provide "param_needed_when_mode_b"
+}
+```
+
+## Other migration notes
+
+Declaring a parameter with only a name is deprecated:
+
+```cpp
+node->declare_parameter("my_param");  // this generates a build warning
+```
+
+```py
+node.declare_parameter("my_param");  # this generates a python user warning
+```
+
+Before, you could initialize a parameter with the "NOT SET" value (in cpp `rclcpp::ParameterValue{}`, in python `None`).
+Now this will throw an exception in both cases:
+
+```cpp
+node->declare_parameter("my_param", rclcpp::ParameterValue{});  // not valid, will throw exception
+```
+
+```py
+node.declare_parameter("my_param", None);  # not valid, will raise error
+```
+
+## Possible improvements
+
+### Easier way to declare dynamically typed parameters
+
+Declaring a dynamically typed parameter in `rclcpp` could be considered to be a bit verbose:
+
+```cpp
+rcl_interfaces::msg::ParameterDescriptor descriptor;
+descriptor.dynamic_typing = true;
+
+node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor);
+```
+
+the following ways could be supported to make it simpler:
+
+```cpp
+node->declare_parameter(name, rclcpp::PARAMETER_DYNAMIC);
+node->declare_parameter(name, default_value, rclcpp::PARAMETER_DYNAMIC);
+```
+
+or alternatively:
+
+```cpp
+node->declare_parameter(name, default_value, rclcpp::ParameterDescriptor{}.dynamic_typing());
+```
+
+For `rclpy`, there's already a short way to do it:
+
+```py
+node.declare_parameter(name, default_value, rclpy.ParameterDescriptor(dynamic_typing=true));
+```

rclcpp/include/rclcpp/any_subscription_callback.hpp

@@ -65,6 +65,9 @@ public:
     const_shared_ptr_callback_(nullptr), const_shared_ptr_with_info_callback_(nullptr),
     unique_ptr_callback_(nullptr), unique_ptr_with_info_callback_(nullptr)
   {
+    if (allocator == nullptr) {
+      throw std::runtime_error("invalid allocator");
+    }
     message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
     allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get());
   }

rclcpp/include/rclcpp/callback_group.hpp

@@ -85,8 +85,8 @@ public:
    * group, or after, is irrelevant; the callback group will be automatically
    * added to the executor in either case.
    *
-   * \param[in] group_type They type of the callback group.
-   * \param[in] automatically_add_to_executor_with_node a boolean which
+   * \param[in] group_type The type of the callback group.
+   * \param[in] automatically_add_to_executor_with_node A boolean that
    *   determines whether a callback group is automatically added to an executor
    *   with the node with which it is associated.
    */

rclcpp/include/rclcpp/clock.hpp

@@ -136,7 +136,7 @@ private:
   RCLCPP_PUBLIC
   static void
   on_time_jump(
-    const struct rcl_time_jump_t * time_jump,
+    const rcl_time_jump_t * time_jump,
     bool before_jump,
     void * user_data);
 

rclcpp/include/rclcpp/context.hpp

@@ -150,7 +150,7 @@ public:
    */
   RCLCPP_PUBLIC
   std::string
-  shutdown_reason();
+  shutdown_reason() const;
 
   /// Shutdown the context, making it uninitialized and therefore invalid for derived entities.
   /**
@@ -286,7 +286,7 @@ private:
 
   // This mutex is recursive so that the destructor can ensure atomicity
   // between is_initialized and shutdown.
-  std::recursive_mutex init_mutex_;
+  mutable std::recursive_mutex init_mutex_;
   std::shared_ptr<rcl_context_t> rcl_context_;
   rclcpp::InitOptions init_options_;
   std::string shutdown_reason_;

rclcpp/include/rclcpp/create_publisher.hpp

@@ -80,6 +80,10 @@ create_publisher(
 /**
  * The NodeT type only needs to have a method called get_node_topics_interface()
  * which returns a shared_ptr to a NodeTopicsInterface.
+ *
+ * In case `options.qos_overriding_options` is enabling qos parameter overrides,
+ * NodeT must also have a method called get_node_parameters_interface()
+ * which returns a shared_ptr to a NodeParametersInterface.
  */
 template<
   typename MessageT,

rclcpp/include/rclcpp/create_subscription.hpp

@@ -101,7 +101,7 @@ create_subscription(
     auto sub_call_back = [weak_subscription_topic_stats]() {
         auto subscription_topic_stats = weak_subscription_topic_stats.lock();
         if (subscription_topic_stats) {
-          subscription_topic_stats->publish_message();
+          subscription_topic_stats->publish_message_and_reset_measurements();
         }
       };
 
@@ -146,6 +146,10 @@ create_subscription(
  * which returns a shared_ptr to a NodeTopicsInterface, or be a
  * NodeTopicsInterface pointer itself.
  *
+ * In case `options.qos_overriding_options` is enabling qos parameter overrides,
+ * NodeT must also have a method called get_node_parameters_interface()
+ * which returns a shared_ptr to a NodeParametersInterface.
+ *
  * \tparam MessageT
  * \tparam CallbackT
  * \tparam AllocatorT

rclcpp/include/rclcpp/detail/mutex_two_priorities.hpp

@@ -0,0 +1,75 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__DETAIL__MUTEX_TWO_PRIORITIES_HPP_
+#define RCLCPP__DETAIL__MUTEX_TWO_PRIORITIES_HPP_
+
+#include <mutex>
+
+namespace rclcpp
+{
+namespace detail
+{
+/// \internal A mutex that has two locking mechanism, one with higher priority than the other.
+/**
+ * After the current mutex owner release the lock, a thread that used the high
+ * priority mechanism will have priority over threads that used the low priority mechanism.
+ */
+class MutexTwoPriorities
+{
+public:
+  class HighPriorityLockable
+  {
+public:
+    explicit HighPriorityLockable(MutexTwoPriorities & parent);
+
+    void lock();
+
+    void unlock();
+
+private:
+    MutexTwoPriorities & parent_;
+  };
+
+  class LowPriorityLockable
+  {
+public:
+    explicit LowPriorityLockable(MutexTwoPriorities & parent);
+
+    void lock();
+
+    void unlock();
+
+private:
+    MutexTwoPriorities & parent_;
+  };
+
+  HighPriorityLockable
+  get_high_priority_lockable();
+
+  LowPriorityLockable
+  get_low_priority_lockable();
+
+private:
+  // Implementation detail: the idea here is that only one low priority thread can be
+  // trying to take the data_ mutex while the others are excluded by the barrier_ mutex.
+  // All high priority threads are already waiting for the data_ mutex.
+  std::mutex barrier_;
+  std::mutex data_;
+};
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__MUTEX_TWO_PRIORITIES_HPP_

rclcpp/include/rclcpp/duration.hpp

@@ -137,6 +137,9 @@ public:
   static Duration
   from_nanoseconds(rcl_duration_value_t nanoseconds);
 
+  static Duration
+  from_rmw_time(rmw_time_t duration);
+
   /// Convert Duration into a std::chrono::Duration.
   template<class DurationT>
   DurationT

rclcpp/include/rclcpp/exceptions/exceptions.hpp

@@ -282,6 +282,20 @@ class ParameterModifiedInCallbackException : public std::runtime_error
   using std::runtime_error::runtime_error;
 };
 
+/// Thrown when a parameter override wasn't provided and one was required.
+class NoParameterOverrideProvided : public std::runtime_error
+{
+public:
+  /// Construct an instance.
+  /**
+   * \param[in] name the name of the parameter.
+   * \param[in] message custom exception message.
+   */
+  explicit NoParameterOverrideProvided(const std::string & name)
+  : std::runtime_error("parameter '" + name + "' requires an user provided parameter override")
+  {}
+};
+
 /// Thrown if the QoS overrides provided aren't valid.
 class InvalidQosOverridesException : public std::runtime_error
 {
@@ -289,6 +303,13 @@ class InvalidQosOverridesException : public std::runtime_error
   using std::runtime_error::runtime_error;
 };
 
+/// Thrown if a QoS compatibility check fails.
+class QoSCheckCompatibleException : public std::runtime_error
+{
+  // Inherit constructors from runtime_error.
+  using std::runtime_error::runtime_error;
+};
+
 }  // namespace exceptions
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/executor.hpp

@@ -448,7 +448,7 @@ protected:
   RCLCPP_PUBLIC
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
   get_node_by_group(
-    WeakCallbackGroupsToNodesMap weak_groups_to_nodes,
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
     rclcpp::CallbackGroup::SharedPtr group);
 
   /// Return true if the node has been added to this executor.
@@ -460,7 +460,7 @@ protected:
   bool
   has_node(
     const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-    WeakCallbackGroupsToNodesMap weak_groups_to_nodes) const;
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) const;
 
   RCLCPP_PUBLIC
   rclcpp::CallbackGroup::SharedPtr
@@ -476,7 +476,7 @@ protected:
     rclcpp::CallbackGroup::SharedPtr group_ptr,
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
     WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
-    bool notify = true);
+    bool notify = true) RCPPUTILS_TSA_REQUIRES(mutex_);
 
   /// Remove a callback group from the executor.
   /**
@@ -487,7 +487,7 @@ protected:
   remove_callback_group_from_map(
     rclcpp::CallbackGroup::SharedPtr group_ptr,
     WeakCallbackGroupsToNodesMap & weak_groups_to_nodes,
-    bool notify = true);
+    bool notify = true) RCPPUTILS_TSA_REQUIRES(mutex_);
 
   RCLCPP_PUBLIC
   bool
@@ -497,7 +497,7 @@ protected:
   bool
   get_next_ready_executable_from_map(
     AnyExecutable & any_executable,
-    WeakCallbackGroupsToNodesMap weak_groups_to_nodes);
+    const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes);
 
   RCLCPP_PUBLIC
   bool
@@ -518,7 +518,7 @@ protected:
    */
   RCLCPP_PUBLIC
   virtual void
-  add_callback_groups_from_nodes_associated_to_executor();
+  add_callback_groups_from_nodes_associated_to_executor() RCPPUTILS_TSA_REQUIRES(mutex_);
 
   /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins.
   std::atomic_bool spinning;
@@ -532,10 +532,11 @@ protected:
   rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set();
 
   // Mutex to protect the subsequent memory_strategy_.
-  std::mutex memory_strategy_mutex_;
+  mutable std::mutex mutex_;
 
   /// The memory strategy: an interface for handling user-defined memory allocation strategies.
-  memory_strategy::MemoryStrategy::SharedPtr memory_strategy_;
+  memory_strategy::MemoryStrategy::SharedPtr
+  memory_strategy_ RCPPUTILS_TSA_PT_GUARDED_BY(mutex_);
 
   /// The context associated with this executor.
   std::shared_ptr<rclcpp::Context> context_;
@@ -552,19 +553,24 @@ protected:
     WeakNodesToGuardConditionsMap;
 
   /// maps nodes to guard conditions
-  WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_;
+  WeakNodesToGuardConditionsMap
+  weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
 
   /// maps callback groups associated to nodes
-  WeakCallbackGroupsToNodesMap weak_groups_associated_with_executor_to_nodes_;
+  WeakCallbackGroupsToNodesMap
+  weak_groups_associated_with_executor_to_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
 
   /// maps callback groups to nodes associated with executor
-  WeakCallbackGroupsToNodesMap weak_groups_to_nodes_associated_with_executor_;
+  WeakCallbackGroupsToNodesMap
+  weak_groups_to_nodes_associated_with_executor_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
 
   /// maps all callback groups to nodes
-  WeakCallbackGroupsToNodesMap weak_groups_to_nodes_;
+  WeakCallbackGroupsToNodesMap
+  weak_groups_to_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
 
   /// nodes that are associated with the executor
-  std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
+  std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>
+  weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
 };
 
 namespace executor

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -22,6 +22,7 @@
 #include <thread>
 #include <unordered_map>
 
+#include "rclcpp/detail/mutex_two_priorities.hpp"
 #include "rclcpp/executor.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/memory_strategies.hpp"
@@ -81,7 +82,7 @@ protected:
 private:
   RCLCPP_DISABLE_COPY(MultiThreadedExecutor)
 
-  std::mutex wait_mutex_;
+  detail::MutexTwoPriorities wait_mutex_;
   size_t number_of_threads_;
   bool yield_before_execute_;
   std::chrono::nanoseconds next_exec_timeout_;

rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

@@ -307,7 +307,7 @@ private:
   {
     SubscriptionInfo() = default;
 
-    rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription;
+    rclcpp::experimental::SubscriptionIntraProcessBase::WeakPtr subscription;
     rmw_qos_profile_t qos;
     const char * topic_name;
     bool use_take_shared_method;
@@ -361,13 +361,16 @@ private:
       if (subscription_it == subscriptions_.end()) {
         throw std::runtime_error("subscription has unexpectedly gone out of scope");
       }
-      auto subscription_base = subscription_it->second.subscription;
+      auto subscription_base = subscription_it->second.subscription.lock();
+      if (subscription_base) {
+        auto subscription = std::static_pointer_cast<
+          rclcpp::experimental::SubscriptionIntraProcess<MessageT>
+          >(subscription_base);
 
-      auto subscription = std::static_pointer_cast<
-        rclcpp::experimental::SubscriptionIntraProcess<MessageT>
-        >(subscription_base);
-
-      subscription->provide_intra_process_message(message);
+        subscription->provide_intra_process_message(message);
+      } else {
+        subscriptions_.erase(id);
+      }
     }
   }
 
@@ -389,24 +392,27 @@ private:
       if (subscription_it == subscriptions_.end()) {
         throw std::runtime_error("subscription has unexpectedly gone out of scope");
       }
-      auto subscription_base = subscription_it->second.subscription;
+      auto subscription_base = subscription_it->second.subscription.lock();
+      if (subscription_base) {
+        auto subscription = std::static_pointer_cast<
+          rclcpp::experimental::SubscriptionIntraProcess<MessageT>
+          >(subscription_base);
 
-      auto subscription = std::static_pointer_cast<
-        rclcpp::experimental::SubscriptionIntraProcess<MessageT>
-        >(subscription_base);
+        if (std::next(it) == subscription_ids.end()) {
+          // If this is the last subscription, give up ownership
+          subscription->provide_intra_process_message(std::move(message));
+        } else {
+          // Copy the message since we have additional subscriptions to serve
+          MessageUniquePtr copy_message;
+          Deleter deleter = message.get_deleter();
+          auto ptr = MessageAllocTraits::allocate(*allocator.get(), 1);
+          MessageAllocTraits::construct(*allocator.get(), ptr, *message);
+          copy_message = MessageUniquePtr(ptr, deleter);
 
-      if (std::next(it) == subscription_ids.end()) {
-        // If this is the last subscription, give up ownership
-        subscription->provide_intra_process_message(std::move(message));
+          subscription->provide_intra_process_message(std::move(copy_message));
+        }
       } else {
-        // Copy the message since we have additional subscriptions to serve
-        MessageUniquePtr copy_message;
-        Deleter deleter = message.get_deleter();
-        auto ptr = MessageAllocTraits::allocate(*allocator.get(), 1);
-        MessageAllocTraits::construct(*allocator.get(), ptr, *message);
-        copy_message = MessageUniquePtr(ptr, deleter);
-
-        subscription->provide_intra_process_message(std::move(copy_message));
+        subscriptions_.erase(subscription_it);
       }
     }
   }

rclcpp/include/rclcpp/loaned_message.hpp

@@ -183,14 +183,30 @@ public:
   /**
    * A call to `release()` will unmanage the memory for the ROS message.
    * That means that the destructor of this class will not free the memory on scope exit.
+   * If the message is loaned from the middleware but not be published, the user needs to call
+   * `rcl_return_loaned_message_from_publisher` manually.
+   * If the memory is from the local allocator, the memory is freed when the unique pointer
+   * goes out instead.
    *
-   * \return Raw pointer to the message instance.
+   * \return std::unique_ptr to the message instance.
    */
-  MessageT * release()
+  std::unique_ptr<MessageT, std::function<void(MessageT *)>>
+  release()
   {
     auto msg = message_;
     message_ = nullptr;
-    return msg;
+
+    if (pub_.can_loan_messages()) {
+      return std::unique_ptr<MessageT, std::function<void(MessageT *)>>(msg, [](MessageT *) {});
+    }
+
+    return std::unique_ptr<MessageT, std::function<void(MessageT *)>>(
+      msg,
+      [allocator = message_allocator_](MessageT * msg_ptr) mutable {
+        // call destructor before deallocating
+        msg_ptr->~MessageT();
+        allocator.deallocate(msg_ptr, 1);
+      });
   }
 
 protected:

rclcpp/include/rclcpp/logger.hpp

@@ -22,6 +22,7 @@
 
 #include "rcl/node.h"
 #include "rcutils/logging.h"
+#include "rcpputils/filesystem_helper.hpp"
 
 /**
  * \def RCLCPP_LOGGING_ENABLED
@@ -75,6 +76,18 @@ RCLCPP_PUBLIC
 Logger
 get_node_logger(const rcl_node_t * node);
 
+/// Get the current logging directory.
+/**
+ * For more details of how the logging directory is determined,
+ * see \ref rcl_logging_get_logging_directory.
+ *
+ * \returns the logging directory being used.
+ * \throws rclcpp::exceptions::RCLError if an unexpected error occurs.
+ */
+RCLCPP_PUBLIC
+rcpputils::fs::path
+get_logging_directory();
+
 class Logger
 {
 public:

rclcpp/include/rclcpp/node.hpp

@@ -163,7 +163,7 @@ public:
    * pub = node->create_publisher<MsgT>("chatter", QoS(10));  // implicitly KeepLast
    * pub = node->create_publisher<MsgT>("chatter", QoS(KeepLast(10)));
    * pub = node->create_publisher<MsgT>("chatter", QoS(KeepAll()));
-   * pub = node->create_publisher<MsgT>("chatter", QoS(1).best_effort().volatile());
+   * pub = node->create_publisher<MsgT>("chatter", QoS(1).best_effort().durability_volatile());
    * {
    *   rclcpp::QoS custom_qos(KeepLast(10), rmw_qos_profile_sensor_data);
    *   pub = node->create_publisher<MsgT>("chatter", custom_qos);
@@ -305,16 +305,59 @@ public:
    *   name is invalid.
    * \throws rclcpp::exceptions::InvalidParameterValueException if initial
    *   value fails to be set.
+   * \throws rclcpp::exceptions::InvalidParameterTypeException
+   *   if the type of the default value or override is wrong.
    */
   RCLCPP_PUBLIC
   const rclcpp::ParameterValue &
   declare_parameter(
     const std::string & name,
-    const rclcpp::ParameterValue & default_value = rclcpp::ParameterValue(),
+    const rclcpp::ParameterValue & default_value,
     const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
     rcl_interfaces::msg::ParameterDescriptor(),
     bool ignore_override = false);
 
+  /// Declare and initialize a parameter, return the effective value.
+  /**
+   * Same as the previous one, but a default value is not provided and the user
+   * must provide a parameter override of the correct type.
+   *
+   * \param[in] name The name of the parameter.
+   * \param[in] type Desired type of the parameter, which will enforced at runtime.
+   * \param[in] parameter_descriptor An optional, custom description for
+   *   the parameter.
+   * \param[in] ignore_override When `true`, the parameter override is ignored.
+   *    Default to `false`.
+   * \return A const reference to the value of the parameter.
+   * \throws Same as the previous overload taking a default value.
+   * \throws rclcpp::exceptions::InvalidParameterTypeException
+   *   if an override is not provided or the provided override is of the wrong type.
+   */
+  RCLCPP_PUBLIC
+  const rclcpp::ParameterValue &
+  declare_parameter(
+    const std::string & name,
+    rclcpp::ParameterType type,
+    const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
+    rcl_interfaces::msg::ParameterDescriptor{},
+    bool ignore_override = false);
+
+  /// Declare a parameter
+  [[deprecated(
+    "declare_parameter() with only a name is deprecated and will be deleted in the future.\n" \
+    "If you want to declare a parameter that won't change type without a default value use:\n" \
+    "`node->declare_parameter<ParameterT>(name)`, where e.g. ParameterT=int64_t.\n\n" \
+    "If you want to declare a parameter that can dynamically change type use:\n" \
+    "```\n" \
+    "rcl_interfaces::msg::ParameterDescriptor descriptor;\n" \
+    "descriptor.dynamic_typing = true;\n" \
+    "node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor);\n" \
+    "```"
+  )]]
+  RCLCPP_PUBLIC
+  const rclcpp::ParameterValue &
+  declare_parameter(const std::string & name);
+
   /// Declare and initialize a parameter with a type.
   /**
    * See the non-templated declare_parameter() on this class for details.
@@ -345,6 +388,18 @@ public:
     rcl_interfaces::msg::ParameterDescriptor(),
     bool ignore_override = false);
 
+  /// Declare and initialize a parameter with a type.
+  /**
+   * See the non-templated declare_parameter() on this class for details.
+   */
+  template<typename ParameterT>
+  auto
+  declare_parameter(
+    const std::string & name,
+    const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
+    rcl_interfaces::msg::ParameterDescriptor(),
+    bool ignore_override = false);
+
   /// Declare and initialize several parameters with the same namespace and type.
   /**
    * For each key in the map, a parameter with a name of "namespace.key"
@@ -785,6 +840,9 @@ public:
    *
    * This allows the node developer to control which parameters may be changed.
    *
+   * It is considered bad practice to reject changes for "unknown" parameters as this prevents
+   * other parts of the node (that may be aware of these parameters) from handling them.
+   *
    * Note that the callback is called when declare_parameter() and its variants
    * are called, and so you cannot assume the parameter has been set before
    * this callback, so when checking a new value against the existing one, you

rclcpp/include/rclcpp/node_impl.hpp

@@ -172,6 +172,24 @@ Node::declare_parameter(
   }
 }
 
+template<typename ParameterT>
+auto
+Node::declare_parameter(
+  const std::string & name,
+  const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor,
+  bool ignore_override)
+{
+  // get advantage of parameter value template magic to get
+  // the correct rclcpp::ParameterType from ParameterT
+  rclcpp::ParameterValue value{ParameterT{}};
+  return this->declare_parameter(
+    name,
+    value.get_type(),
+    parameter_descriptor,
+    ignore_override
+  ).get<ParameterT>();
+}
+
 template<typename ParameterT>
 std::vector<ParameterT>
 Node::declare_parameters(

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -103,13 +103,42 @@ public:
   virtual
   ~NodeParameters();
 
+// This is overriding a deprecated method, so we need to ignore the deprecation warning here.
+// Users of the method will still get a warning!
+#ifndef _WIN32
+# pragma GCC diagnostic push
+# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#else
+# pragma warning(push)
+# pragma warning(disable: 4996)
+#endif
+  [[deprecated(RCLCPP_INTERNAL_NODE_PARAMETERS_INTERFACE_DEPRECATE_DECLARE)]]
+  RCLCPP_PUBLIC
+  const rclcpp::ParameterValue &
+  declare_parameter(const std::string & name) override;
+#ifndef _WIN32
+# pragma GCC diagnostic pop
+#else
+# pragma warning(pop)
+#endif
+
   RCLCPP_PUBLIC
   const rclcpp::ParameterValue &
   declare_parameter(
     const std::string & name,
     const rclcpp::ParameterValue & default_value,
-    const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor,
-    bool ignore_override) override;
+    const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
+    rcl_interfaces::msg::ParameterDescriptor{},
+    bool ignore_override = false) override;
+
+  RCLCPP_PUBLIC
+  const rclcpp::ParameterValue &
+  declare_parameter(
+    const std::string & name,
+    rclcpp::ParameterType type,
+    const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
+    rcl_interfaces::msg::ParameterDescriptor(),
+    bool ignore_override = false) override;
 
   RCLCPP_PUBLIC
   void

rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp

@@ -45,6 +45,17 @@ struct OnSetParametersCallbackHandle
   OnParametersSetCallbackType callback;
 };
 
+#define RCLCPP_INTERNAL_NODE_PARAMETERS_INTERFACE_DEPRECATE_DECLARE \
+  "declare_parameter() with only a name is deprecated and will be deleted in the future.\n" \
+  "If you want to declare a parameter that won't change type without a default value use:\n" \
+  "`node_params->declare_parameter(name, type)`, with e.g. type=rclcpp::PARAMETER_INTEGER.\n\n" \
+  "If you want to declare a parameter that can dynamically change type use:\n" \
+  "```\n" \
+  "rcl_interfaces::msg::ParameterDescriptor descriptor;\n" \
+  "descriptor.dynamic_typing = true;\n" \
+  "node_params->declare_parameter(name, rclcpp::ParameterValue{}, descriptor);\n" \
+  "```"
+
 /// Pure virtual interface class for the NodeParameters part of the Node API.
 class NodeParametersInterface
 {
@@ -55,6 +66,15 @@ public:
   virtual
   ~NodeParametersInterface() = default;
 
+  /// Declare a parameter.
+  /**
+   * \sa rclcpp::Node::declare_parameter
+   */
+  [[deprecated(RCLCPP_INTERNAL_NODE_PARAMETERS_INTERFACE_DEPRECATE_DECLARE)]]
+  virtual
+  const rclcpp::ParameterValue &
+  declare_parameter(const std::string & name) = 0;
+
   /// Declare and initialize a parameter.
   /**
    * \sa rclcpp::Node::declare_parameter
@@ -64,7 +84,21 @@ public:
   const rclcpp::ParameterValue &
   declare_parameter(
     const std::string & name,
-    const rclcpp::ParameterValue & default_value = rclcpp::ParameterValue(),
+    const rclcpp::ParameterValue & default_value,
+    const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
+    rcl_interfaces::msg::ParameterDescriptor(),
+    bool ignore_override = false) = 0;
+
+  /// Declare a parameter.
+  /**
+   * \sa rclcpp::Node::declare_parameter
+   */
+  RCLCPP_PUBLIC
+  virtual
+  const rclcpp::ParameterValue &
+  declare_parameter(
+    const std::string & name,
+    rclcpp::ParameterType type,
     const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
     rcl_interfaces::msg::ParameterDescriptor(),
     bool ignore_override = false) = 0;

rclcpp/include/rclcpp/parameter_client.hpp

@@ -15,6 +15,8 @@
 #ifndef RCLCPP__PARAMETER_CLIENT_HPP_
 #define RCLCPP__PARAMETER_CLIENT_HPP_
 
+#include <functional>
+#include <future>
 #include <memory>
 #include <string>
 #include <utility>
@@ -120,6 +122,14 @@ public:
       void(std::shared_future<std::vector<rclcpp::Parameter>>)
     > callback = nullptr);
 
+  RCLCPP_PUBLIC
+  std::shared_future<std::vector<rcl_interfaces::msg::ParameterDescriptor>>
+  describe_parameters(
+    const std::vector<std::string> & names,
+    std::function<
+      void(std::shared_future<std::vector<rcl_interfaces::msg::ParameterDescriptor>>)
+    > callback = nullptr);
+
   RCLCPP_PUBLIC
   std::shared_future<std::vector<rclcpp::ParameterType>>
   get_parameter_types(
@@ -332,9 +342,17 @@ public:
       qos_profile);
   }
 
-  RCLCPP_PUBLIC
+  template<typename RepT = int64_t, typename RatioT = std::milli>
   std::vector<rclcpp::Parameter>
-  get_parameters(const std::vector<std::string> & parameter_names);
+  get_parameters(
+    const std::vector<std::string> & parameter_names,
+    std::chrono::duration<RepT, RatioT> timeout = std::chrono::duration<RepT, RatioT>(-1))
+  {
+    return get_parameters(
+      parameter_names,
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
+    );
+  }
 
   RCLCPP_PUBLIC
   bool
@@ -378,23 +396,67 @@ public:
     );
   }
 
-  RCLCPP_PUBLIC
+  template<typename RepT = int64_t, typename RatioT = std::milli>
+  std::vector<rcl_interfaces::msg::ParameterDescriptor>
+  describe_parameters(
+    const std::vector<std::string> & parameter_names,
+    std::chrono::duration<RepT, RatioT> timeout = std::chrono::duration<RepT, RatioT>(-1))
+  {
+    return describe_parameters(
+      parameter_names,
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
+    );
+  }
+
+  template<typename RepT = int64_t, typename RatioT = std::milli>
   std::vector<rclcpp::ParameterType>
-  get_parameter_types(const std::vector<std::string> & parameter_names);
+  get_parameter_types(
+    const std::vector<std::string> & parameter_names,
+    std::chrono::duration<RepT, RatioT> timeout = std::chrono::duration<RepT, RatioT>(-1))
+  {
+    return get_parameter_types(
+      parameter_names,
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
+    );
+  }
 
-  RCLCPP_PUBLIC
+  template<typename RepT = int64_t, typename RatioT = std::milli>
   std::vector<rcl_interfaces::msg::SetParametersResult>
-  set_parameters(const std::vector<rclcpp::Parameter> & parameters);
+  set_parameters(
+    const std::vector<rclcpp::Parameter> & parameters,
+    std::chrono::duration<RepT, RatioT> timeout = std::chrono::duration<RepT, RatioT>(-1))
+  {
+    return set_parameters(
+      parameters,
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
+    );
+  }
 
-  RCLCPP_PUBLIC
+  template<typename RepT = int64_t, typename RatioT = std::milli>
   rcl_interfaces::msg::SetParametersResult
-  set_parameters_atomically(const std::vector<rclcpp::Parameter> & parameters);
+  set_parameters_atomically(
+    const std::vector<rclcpp::Parameter> & parameters,
+    std::chrono::duration<RepT, RatioT> timeout = std::chrono::duration<RepT, RatioT>(-1))
+  {
+    return set_parameters_atomically(
+      parameters,
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
+    );
+  }
 
-  RCLCPP_PUBLIC
+  template<typename RepT = int64_t, typename RatioT = std::milli>
   rcl_interfaces::msg::ListParametersResult
   list_parameters(
     const std::vector<std::string> & parameter_prefixes,
-    uint64_t depth);
+    uint64_t depth,
+    std::chrono::duration<RepT, RatioT> timeout = std::chrono::duration<RepT, RatioT>(-1))
+  {
+    return list_parameters(
+      parameter_prefixes,
+      depth,
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
+    );
+  }
 
   template<typename CallbackT>
   typename rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
@@ -437,6 +499,44 @@ public:
     return async_parameters_client_->wait_for_service(timeout);
   }
 
+protected:
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::Parameter>
+  get_parameters(
+    const std::vector<std::string> & parameter_names,
+    std::chrono::nanoseconds timeout);
+
+  RCLCPP_PUBLIC
+  std::vector<rcl_interfaces::msg::ParameterDescriptor>
+  describe_parameters(
+    const std::vector<std::string> & parameter_names,
+    std::chrono::nanoseconds timeout);
+
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::ParameterType>
+  get_parameter_types(
+    const std::vector<std::string> & parameter_names,
+    std::chrono::nanoseconds timeout);
+
+  RCLCPP_PUBLIC
+  std::vector<rcl_interfaces::msg::SetParametersResult>
+  set_parameters(
+    const std::vector<rclcpp::Parameter> & parameters,
+    std::chrono::nanoseconds timeout);
+
+  RCLCPP_PUBLIC
+  rcl_interfaces::msg::SetParametersResult
+  set_parameters_atomically(
+    const std::vector<rclcpp::Parameter> & parameters,
+    std::chrono::nanoseconds timeout);
+
+  RCLCPP_PUBLIC
+  rcl_interfaces::msg::ListParametersResult
+  list_parameters(
+    const std::vector<std::string> & parameter_prefixes,
+    uint64_t depth,
+    std::chrono::nanoseconds timeout);
+
 private:
   rclcpp::Executor::SharedPtr executor_;
   const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_;

rclcpp/include/rclcpp/parameter_event_handler.hpp

@@ -0,0 +1,338 @@
+// Copyright 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__PARAMETER_EVENT_HANDLER_HPP_
+#define RCLCPP__PARAMETER_EVENT_HANDLER_HPP_
+
+#include <list>
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include "rclcpp/create_subscription.hpp"
+#include "rclcpp/node_interfaces/get_node_base_interface.hpp"
+#include "rclcpp/node_interfaces/get_node_topics_interface.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp/subscription.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rcl_interfaces/msg/parameter_event.hpp"
+
+namespace rclcpp
+{
+
+struct ParameterCallbackHandle
+{
+  RCLCPP_SMART_PTR_DEFINITIONS(ParameterCallbackHandle)
+
+  using ParameterCallbackType = std::function<void (const rclcpp::Parameter &)>;
+
+  std::string parameter_name;
+  std::string node_name;
+  ParameterCallbackType callback;
+};
+
+struct ParameterEventCallbackHandle
+{
+  RCLCPP_SMART_PTR_DEFINITIONS(ParameterEventCallbackHandle)
+
+  using ParameterEventCallbackType =
+    std::function<void (const rcl_interfaces::msg::ParameterEvent::SharedPtr &)>;
+
+  ParameterEventCallbackType callback;
+};
+
+/// A class used to "handle" (monitor and respond to) changes to parameters.
+/**
+ * The ParameterEventHandler class allows for the monitoring of changes to node parameters,
+ * either a node's own parameters or parameters owned by other nodes in the system.
+ * Multiple parameter callbacks can be set and will be invoked when the specified parameter
+ * changes.
+ *
+ * The first step is to instantiate a ParameterEventHandler, providing a ROS node to use
+ * to create any required subscriptions:
+ *
+ *   auto param_handler = std::make_shared<rclcpp::ParameterEventHandler>(node);
+ *
+ * Next, you can supply a callback to the add_parameter_callback method, as follows:
+ *
+ *   auto cb1 = [&node](const rclcpp::Parameter & p) {
+ *      RCLCPP_INFO(
+ *        node->get_logger(),
+ *        "cb1: Received an update to parameter \"%s\" of type %s: \"%ld\"",
+ *        p.get_name().c_str(),
+ *        p.get_type_name().c_str(),
+ *        p.as_int());
+ *   };
+ *   auto handle1 = param_handler->add_parameter_callback("an_int_param", cb1);
+ *
+ * In this case, we didn't supply a node name (the third, optional, parameter) so the
+ * default will be to monitor for changes to the "an_int_param" parameter associated with
+ * the ROS node supplied in the ParameterEventHandler constructor.
+ * The callback, a lambda function in this case, simply prints out the value of the parameter.
+ *
+ * You may also monitor for changes to parameters in other nodes by supplying the node
+ * name to add_parameter_callback:
+ *
+ *   auto cb2 = [&node](const rclcpp::Parameter & p) {
+ *       RCLCPP_INFO(
+ *         node->get_logger(),
+ *         "cb2: Received an update to parameter \"%s\" of type: %s: \"%s\"",
+ *         p.get_name().c_str(),
+ *         p.get_type_name().c_str(),
+ *         p.as_string().c_str());
+ *     };
+ *   auto handle2 = param_handler->add_parameter_callback(
+ *     "some_remote_param_name", cb2, "some_remote_node_name");
+ *
+ * In this case, the callback will be invoked whenever "some_remote_param_name" changes
+ * on remote node "some_remote_node_name".
+ *
+ * To remove a parameter callback, call remove_parameter_callback, passing the handle returned
+ * from add_parameter_callback:
+ *
+ *   param_handler->remove_parameter_callback(handle2);
+ *
+ * You can also monitor for *all* parameter changes, using add_parameter_event_callback.
+ * In this case, the callback will be invoked whenever any parameter changes in the system.
+ * You are likely interested in a subset of these parameter changes, so in the callback it
+ * is convenient to use a regular expression on the node names or namespaces of interest.
+ * For example:
+ *
+ *   auto cb3 =
+ *     [fqn, remote_param_name, &node](const rcl_interfaces::msg::ParameterEvent::SharedPtr & event) {
+ *       // Look for any updates to parameters in "/a_namespace" as well as any parameter changes
+ *       // to our own node ("this_node")
+ *       std::regex re("(/a_namespace/.*)|(/this_node)");
+ *       if (regex_match(event->node, re)) {
+ *         // Now that we know the event matches the regular expression we scanned for, we can
+ *         // use 'get_parameter_from_event' to get a specific parameter name that we're looking for
+ *         rclcpp::Parameter p;
+ *         if (rclcpp::ParameterEventsSubscriber::get_parameter_from_event(
+ *             *event, p, remote_param_name, fqn))
+ *         {
+ *           RCLCPP_INFO(
+ *             node->get_logger(),
+ *             "cb3: Received an update to parameter \"%s\" of type: %s: \"%s\"",
+ *             p.get_name().c_str(),
+ *             p.get_type_name().c_str(),
+ *             p.as_string().c_str());
+ *         }
+ *
+ *         // You can also use 'get_parameter*s*_from_event' to enumerate all changes that came
+ *         // in on this event
+ *         auto params = rclcpp::ParameterEventsSubscriber::get_parameters_from_event(*event);
+ *         for (auto & p : params) {
+ *           RCLCPP_INFO(
+ *             node->get_logger(),
+ *             "cb3: Received an update to parameter \"%s\" of type: %s: \"%s\"",
+ *             p.get_name().c_str(),
+ *             p.get_type_name().c_str(),
+ *             p.value_to_string().c_str());
+ *         }
+ *       }
+ *     };
+ *   auto handle3 = param_handler->add_parameter_event_callback(cb3);
+ *
+ * For both parameter callbacks and parameter event callbacks, when multiple callbacks are added,
+ * the callbacks are invoked last-in, first-called order (LIFO).
+ *
+ * To remove a parameter event callback, use:
+ *
+ *   param_handler->remove_event_parameter_callback(handle);
+ */
+class ParameterEventHandler
+{
+public:
+  /// Construct a parameter events monitor.
+  /**
+   * \param[in] node The node to use to create any required subscribers.
+   * \param[in] qos The QoS settings to use for any subscriptions.
+   */
+  template<typename NodeT>
+  ParameterEventHandler(
+    NodeT node,
+    const rclcpp::QoS & qos =
+    rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)))
+  {
+    node_base_ = rclcpp::node_interfaces::get_node_base_interface(node);
+    auto node_topics = rclcpp::node_interfaces::get_node_topics_interface(node);
+
+    event_subscription_ = rclcpp::create_subscription<rcl_interfaces::msg::ParameterEvent>(
+      node_topics, "/parameter_events", qos,
+      std::bind(&ParameterEventHandler::event_callback, this, std::placeholders::_1));
+  }
+
+  using ParameterEventCallbackType =
+    ParameterEventCallbackHandle::ParameterEventCallbackType;
+
+  /// Set a callback for all parameter events.
+  /**
+   * This function may be called multiple times to set multiple parameter event callbacks.
+   *
+   * \param[in] callback Function callback to be invoked on parameter updates.
+   * \returns A handle used to refer to the callback.
+   */
+  RCLCPP_PUBLIC
+  ParameterEventCallbackHandle::SharedPtr
+  add_parameter_event_callback(
+    ParameterEventCallbackType callback);
+
+  /// Remove parameter event callback registered with add_parameter_event_callback.
+  /**
+   * \param[in] callback_handle Handle of the callback to remove.
+   */
+  RCLCPP_PUBLIC
+  void
+  remove_parameter_event_callback(
+    ParameterEventCallbackHandle::SharedPtr callback_handle);
+
+  using ParameterCallbackType = ParameterCallbackHandle::ParameterCallbackType;
+
+  /// Add a callback for a specified parameter.
+  /**
+   * If a node_name is not provided, defaults to the current node.
+   *
+   * \param[in] parameter_name Name of parameter to monitor.
+   * \param[in] callback Function callback to be invoked upon parameter update.
+   * \param[in] node_name Name of node which hosts the parameter.
+   * \returns A handle used to refer to the callback.
+   */
+  RCLCPP_PUBLIC
+  ParameterCallbackHandle::SharedPtr
+  add_parameter_callback(
+    const std::string & parameter_name,
+    ParameterCallbackType callback,
+    const std::string & node_name = "");
+
+  /// Remove a parameter callback registered with add_parameter_callback.
+  /**
+   * The parameter name and node name are inspected from the callback handle. The callback handle
+   * is erased from the list of callback handles on the {parameter_name, node_name} in the map.
+   * An error is thrown if the handle does not exist and/or was already removed.
+   *
+   * \param[in] callback_handle Handle of the callback to remove.
+   */
+  RCLCPP_PUBLIC
+  void
+  remove_parameter_callback(
+    ParameterCallbackHandle::SharedPtr callback_handle);
+
+  /// Get an rclcpp::Parameter from a parameter event.
+  /**
+   * If a node_name is not provided, defaults to the current node.
+   *
+   * \param[in] event Event msg to be inspected.
+   * \param[out] parameter Reference to rclcpp::Parameter to be assigned.
+   * \param[in] parameter_name Name of parameter.
+   * \param[in] node_name Name of node which hosts the parameter.
+   * \returns Output parameter is set with requested parameter info and returns true if
+   * requested parameter name and node is in event. Otherwise, returns false.
+   */
+  RCLCPP_PUBLIC
+  static bool
+  get_parameter_from_event(
+    const rcl_interfaces::msg::ParameterEvent & event,
+    rclcpp::Parameter & parameter,
+    const std::string parameter_name,
+    const std::string node_name = "");
+
+  /// Get an rclcpp::Parameter from parameter event
+  /**
+   * If a node_name is not provided, defaults to the current node.
+   *
+   * The user is responsible to check if the returned parameter has been properly assigned.
+   * By default, if the requested parameter is not found in the event, the returned parameter
+   * has parameter value of type rclcpp::PARAMETER_NOT_SET.
+   *
+   * \param[in] event Event msg to be inspected.
+   * \param[in] parameter_name Name of parameter.
+   * \param[in] node_name Name of node which hosts the parameter.
+   * \returns The resultant rclcpp::Parameter from the event.
+   */
+  RCLCPP_PUBLIC
+  static rclcpp::Parameter
+  get_parameter_from_event(
+    const rcl_interfaces::msg::ParameterEvent & event,
+    const std::string parameter_name,
+    const std::string node_name = "");
+
+  /// Get all rclcpp::Parameter values from a parameter event
+  /**
+   * \param[in] event Event msg to be inspected.
+   * \returns A vector rclcpp::Parameter values from the event.
+   */
+  RCLCPP_PUBLIC
+  static std::vector<rclcpp::Parameter>
+  get_parameters_from_event(
+    const rcl_interfaces::msg::ParameterEvent & event);
+
+  using CallbacksContainerType = std::list<ParameterCallbackHandle::WeakPtr>;
+
+protected:
+  /// Callback for parameter events subscriptions.
+  RCLCPP_PUBLIC
+  void
+  event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event);
+
+  // Utility function for resolving node path.
+  std::string resolve_path(const std::string & path);
+
+  // Node interface used for base functionality
+  std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> node_base_;
+
+  // *INDENT-OFF* Uncrustify doesn't handle indented public/private labels
+  // Hash function for string pair required in std::unordered_map
+  // See: https://stackoverflow.com/questions/35985960/c-why-is-boosthash-combine-the-best-way-to-combine-hash-values
+  class StringPairHash
+  {
+  public:
+    template<typename T>
+    inline void hash_combine(std::size_t & seed, const T & v) const
+    {
+      std::hash<T> hasher;
+      seed ^= hasher(v) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
+    }
+
+    inline size_t operator()(const std::pair<std::string, std::string> & s) const
+    {
+      size_t seed = 0;
+      hash_combine(seed, s.first);
+      hash_combine(seed, s.second);
+      return seed;
+    }
+  };
+  // *INDENT-ON*
+
+  // Map container for registered parameters
+  std::unordered_map<
+    std::pair<std::string, std::string>,
+    CallbacksContainerType,
+    StringPairHash
+  > parameter_callbacks_;
+
+  rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr event_subscription_;
+
+  std::list<ParameterEventCallbackHandle::WeakPtr> event_callbacks_;
+
+  std::recursive_mutex mutex_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__PARAMETER_EVENT_HANDLER_HPP_

rclcpp/include/rclcpp/publisher.hpp

@@ -261,7 +261,7 @@ public:
     if (this->can_loan_messages()) {
       // we release the ownership from the rclpp::LoanedMessage instance
       // and let the middleware clean up the memory.
-      this->do_loaned_message_publish(loaned_msg.release());
+      this->do_loaned_message_publish(std::move(loaned_msg.release()));
     } else {
       // we don't release the ownership, let the middleware copy the ros message
       // and thus the destructor of rclcpp::LoanedMessage cleans up the memory.
@@ -310,9 +310,9 @@ protected:
   }
 
   void
-  do_loaned_message_publish(MessageT * msg)
+  do_loaned_message_publish(std::unique_ptr<MessageT, std::function<void(MessageT *)>> msg)
   {
-    auto status = rcl_publish_loaned_message(publisher_handle_.get(), msg, nullptr);
+    auto status = rcl_publish_loaned_message(publisher_handle_.get(), msg.get(), nullptr);
 
     if (RCL_RET_PUBLISHER_INVALID == status) {
       rcl_reset_error();  // next call will reset error message if not context

rclcpp/include/rclcpp/publisher_options.hpp

@@ -95,12 +95,7 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase
   get_allocator() const
   {
     if (!this->allocator) {
-      // TODO(wjwwood): I would like to use the commented line instead, but
-      //   cppcheck 1.89 fails with:
-      //     Syntax Error: AST broken, binary operator '>' doesn't have two operands.
-      // return std::make_shared<Allocator>();
-      std::shared_ptr<Allocator> tmp(new Allocator());
-      return tmp;
+      return std::make_shared<Allocator>();
     }
     return this->allocator;
   }

rclcpp/include/rclcpp/qos.hpp

@@ -18,6 +18,7 @@
 #include <string>
 
 #include "rclcpp/duration.hpp"
+#include "rclcpp/exceptions.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rcl/logging_rosout.h"
 #include "rmw/incompatible_qos_events_statuses.h"
@@ -30,6 +31,45 @@ namespace rclcpp
 RCLCPP_PUBLIC
 std::string qos_policy_name_from_kind(rmw_qos_policy_kind_t policy_kind);
 
+enum class HistoryPolicy
+{
+  KeepLast = RMW_QOS_POLICY_HISTORY_KEEP_LAST,
+  KeepAll = RMW_QOS_POLICY_HISTORY_KEEP_ALL,
+  SystemDefault = RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT,
+  Unknown = RMW_QOS_POLICY_HISTORY_UNKNOWN,
+};
+
+enum class ReliabilityPolicy
+{
+  BestEffort = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
+  Reliable = RMW_QOS_POLICY_RELIABILITY_RELIABLE,
+  SystemDefault = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT,
+  Unknown = RMW_QOS_POLICY_RELIABILITY_UNKNOWN,
+};
+
+enum class DurabilityPolicy
+{
+  Volatile = RMW_QOS_POLICY_DURABILITY_VOLATILE,
+  TransientLocal = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL,
+  SystemDefault = RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT,
+  Unknown = RMW_QOS_POLICY_DURABILITY_UNKNOWN,
+};
+
+enum class LivelinessPolicy
+{
+  Automatic = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC,
+  ManualByTopic = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC,
+  SystemDefault = RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
+  Unknown = RMW_QOS_POLICY_LIVELINESS_UNKNOWN,
+};
+
+enum class QoSCompatibility
+{
+  Ok = RMW_QOS_COMPATIBILITY_OK,
+  Warning = RMW_QOS_COMPATIBILITY_WARNING,
+  Error = RMW_QOS_COMPATIBILITY_ERROR,
+};
+
 /// QoS initialization values, cannot be created directly, use KeepAll or KeepLast instead.
 struct RCLCPP_PUBLIC QoSInitialization
 {
@@ -58,10 +98,32 @@ struct RCLCPP_PUBLIC KeepLast : public rclcpp::QoSInitialization
 };
 
 /// Encapsulation of Quality of Service settings.
+/**
+ * Quality of Service settings control the behavior of publishers, subscriptions,
+ * and other entities, and includes things like how data is sent or resent,
+ * how data is buffered on the publishing and subscribing side, and other things.
+ * See:
+ *   <a href="https://index.ros.org/doc/ros2/Concepts/About-Quality-of-Service-Settings">
+ *     https://index.ros.org/doc/ros2/Concepts/About-Quality-of-Service-Settings
+ *   </a>
+ */
 class RCLCPP_PUBLIC QoS
 {
 public:
-  /// Constructor which allows you to construct a QoS by giving the only required settings.
+  /// Create a QoS by specifying only the history policy and history depth.
+  /**
+   * When using the default initial profile, the defaults will include:
+   *
+   *   - \link rclcpp::ReliabilityPolicy::Reliable ReliabilityPolicy::Reliable\endlink
+   *   - \link rclcpp::DurabilityPolicy::Volatile DurabilityPolicy::Volatile\endlink
+   *
+   * See rmw_qos_profile_default for a full list of default settings.
+   * If some other rmw_qos_profile_t is passed to initial_profile, then the defaults will derive from
+   * that profile instead.
+   *
+   * \param[in] qos_initialization Specifies history policy and history depth.
+   * \param[in] initial_profile The rmw_qos_profile_t instance on which to base the default settings.
+   */
   explicit
   QoS(
     const QoSInitialization & qos_initialization,
@@ -69,7 +131,11 @@ public:
 
   /// Conversion constructor to ease construction in the common case of just specifying depth.
   /**
-   * Convenience constructor, equivalent to QoS(KeepLast(history_depth)).
+   * This is a convenience constructor that calls QoS(KeepLast(history_depth)).
+   *
+   * \param[in] history_depth How many messages can be queued when publishing
+   *   with a Publisher, or how many messages can be queued before being replaced
+   *   by a Subscription.
    */
   // cppcheck-suppress noExplicitConstructor
   QoS(size_t history_depth);  // NOLINT(runtime/explicit): conversion constructor
@@ -82,6 +148,10 @@ public:
   const rmw_qos_profile_t &
   get_rmw_qos_profile() const;
 
+  /// Set the history policy.
+  QoS &
+  history(HistoryPolicy history);
+
   /// Set the history policy.
   QoS &
   history(rmw_qos_history_policy_t history);
@@ -98,6 +168,10 @@ public:
   QoS &
   reliability(rmw_qos_reliability_policy_t reliability);
 
+  /// Set the reliability setting.
+  QoS &
+  reliability(ReliabilityPolicy reliability);
+
   /// Set the reliability setting to reliable.
   QoS &
   reliable();
@@ -110,6 +184,10 @@ public:
   QoS &
   durability(rmw_qos_durability_policy_t durability);
 
+  /// Set the durability setting.
+  QoS &
+  durability(DurabilityPolicy durability);
+
   /// Set the durability setting to volatile.
   /**
     * Note that this cannot be named `volatile` because it is a C++ keyword.
@@ -141,6 +219,10 @@ public:
   QoS &
   liveliness(rmw_qos_liveliness_policy_t liveliness);
 
+  /// Set the liveliness setting.
+  QoS &
+  liveliness(LivelinessPolicy liveliness);
+
   /// Set the liveliness_lease_duration setting.
   QoS &
   liveliness_lease_duration(rmw_time_t liveliness_lease_duration);
@@ -153,6 +235,42 @@ public:
   QoS &
   avoid_ros_namespace_conventions(bool avoid_ros_namespace_conventions);
 
+  /// Get the history qos policy.
+  HistoryPolicy
+  history() const;
+
+  /// Get the history depth.
+  size_t
+  depth() const;
+
+  /// Get the reliability policy.
+  ReliabilityPolicy
+  reliability() const;
+
+  /// Get the durability policy.
+  DurabilityPolicy
+  durability() const;
+
+  /// Get the deadline duration setting.
+  rclcpp::Duration
+  deadline() const;
+
+  /// Get the lifespan duration setting.
+  rclcpp::Duration
+  lifespan() const;
+
+  /// Get the liveliness policy.
+  LivelinessPolicy
+  liveliness() const;
+
+  /// Get the liveliness lease duration setting.
+  rclcpp::Duration
+  liveliness_lease_duration() const;
+
+  /// Get the `avoid ros namespace convention` setting.
+  bool
+  avoid_ros_namespace_conventions() const;
+
 private:
   rmw_qos_profile_t rmw_qos_profile_;
 };
@@ -163,6 +281,61 @@ bool operator==(const QoS & left, const QoS & right);
 RCLCPP_PUBLIC
 bool operator!=(const QoS & left, const QoS & right);
 
+/// Result type for checking QoS compatibility
+/**
+ * \see rclcpp::qos_check_compatible()
+ */
+struct QoSCheckCompatibleResult
+{
+  /// Compatibility result.
+  QoSCompatibility compatibility;
+
+  /// Reason for a (possible) incompatibility.
+  /**
+   * Set if compatiblity is QoSCompatibility::Warning or QoSCompatiblity::Error.
+   * Not set if the QoS profiles are compatible.
+   */
+  std::string reason;
+};
+
+/// Check if two QoS profiles are compatible.
+/**
+ * Two QoS profiles are compatible if a publisher and subcription
+ * using the QoS policies can communicate with each other.
+ *
+ * If any policies have value "system default" or "unknown" then it is possible that
+ * compatiblity cannot be determined.
+ * In this case, the value QoSCompatility::Warning is set as part of
+ * the returned structure.
+ *
+ * Example usage:
+ *
+ * ```cpp
+ * rclcpp::QoSCheckCompatibleResult result = rclcpp::qos_check_compatible(
+ *   publisher_qos, subscription_qos);
+ * if (rclcpp::QoSCompatibility::Error != result.compatibility) {
+ *   // QoS not compatible ...
+ *   // result.reason contains info about the incompatibility
+ * } else if (rclcpp::QoSCompatibility::Warning != result.compatibility) {
+ *   // QoS may not be compatible ...
+ *   // result.reason contains info about the possible incompatibility
+ * }
+ * ```
+ *
+ * \param[in] publisher_qos: The QoS profile for a publisher.
+ * \param[in] subscription_qos: The QoS profile for a subscription.
+ * \return Struct with compatiblity set to QoSCompatibility::Ok if the QoS profiles are
+ *   compatible, or
+ * \return Struct with compatibility set to QoSCompatibility::Warning if there is a chance
+ *   the QoS profiles are not compatible, or
+ * \return Struct with compatibility set to QoSCompatibility::Error if the QoS profiles are
+ *   not compatible.
+ * \throws rclcpp::exceptions::QoSCheckCompatibilityException if an unexpected error occurs.
+ */
+RCLCPP_PUBLIC
+QoSCheckCompatibleResult
+qos_check_compatible(const QoS & publisher_qos, const QoS & subscription_qos);
+
 /**
  * Clock QoS class
  *    - History: Keep last,

rclcpp/include/rclcpp/rclcpp.hpp

@@ -147,14 +147,15 @@
 #include "rclcpp/guard_condition.hpp"
 #include "rclcpp/logging.hpp"
 #include "rclcpp/node.hpp"
-#include "rclcpp/parameter.hpp"
 #include "rclcpp/parameter_client.hpp"
+#include "rclcpp/parameter_event_handler.hpp"
+#include "rclcpp/parameter.hpp"
 #include "rclcpp/parameter_service.hpp"
 #include "rclcpp/rate.hpp"
 #include "rclcpp/time.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
-#include "rclcpp/wait_set.hpp"
 #include "rclcpp/waitable.hpp"
+#include "rclcpp/wait_set.hpp"
 
 #endif  // RCLCPP__RCLCPP_HPP_

rclcpp/include/rclcpp/service.hpp

@@ -180,7 +180,7 @@ public:
     std::weak_ptr<rcl_node_t> weak_node_handle(node_handle_);
     // rcl does the static memory allocation here
     service_handle_ = std::shared_ptr<rcl_service_t>(
-      new rcl_service_t, [weak_node_handle](rcl_service_t * service)
+      new rcl_service_t, [weak_node_handle, service_name](rcl_service_t * service)
       {
         auto handle = weak_node_handle.lock();
         if (handle) {
@@ -192,10 +192,10 @@ public:
             rcl_reset_error();
           }
         } else {
-          RCLCPP_ERROR(
+          RCLCPP_ERROR_STREAM(
             rclcpp::get_logger("rclcpp"),
-            "Error in destruction of rcl service handle: "
-            "the Node Handle was destructed too early. You will leak memory");
+            "Error in destruction of rcl service handle " << service_name <<
+              ": the Node Handle was destructed too early. You will leak memory");
         }
         delete service;
       });

rclcpp/include/rclcpp/subscription.hpp

@@ -171,11 +171,7 @@ public:
 
       // First create a SubscriptionIntraProcess which will be given to the intra-process manager.
       auto context = node_base->get_context();
-      using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess<
-        CallbackMessageT,
-        AllocatorT,
-        typename MessageUniquePtr::deleter_type>;
-      auto subscription_intra_process = std::make_shared<SubscriptionIntraProcessT>(
+      subscription_intra_process_ = std::make_shared<SubscriptionIntraProcessT>(
         callback,
         options.get_allocator(),
         context,
@@ -185,12 +181,12 @@ public:
       TRACEPOINT(
         rclcpp_subscription_init,
         static_cast<const void *>(get_subscription_handle().get()),
-        static_cast<const void *>(subscription_intra_process.get()));
+        static_cast<const void *>(subscription_intra_process_.get()));
 
       // Add it to the intra process manager.
       using rclcpp::experimental::IntraProcessManager;
       auto ipm = context->get_sub_context<IntraProcessManager>();
-      uint64_t intra_process_subscription_id = ipm->add_subscription(subscription_intra_process);
+      uint64_t intra_process_subscription_id = ipm->add_subscription(subscription_intra_process_);
       this->setup_intra_process(intra_process_subscription_id, ipm);
     }
 
@@ -277,11 +273,18 @@ public:
       return;
     }
     auto typed_message = std::static_pointer_cast<CallbackMessageT>(message);
+
+    std::chrono::time_point<std::chrono::system_clock> now;
+    if (subscription_topic_statistics_) {
+      // get current time before executing callback to
+      // exclude callback duration from topic statistics result.
+      now = std::chrono::system_clock::now();
+    }
+
     any_callback_.dispatch(typed_message, message_info);
 
     if (subscription_topic_statistics_) {
-      const auto nanos = std::chrono::time_point_cast<std::chrono::nanoseconds>(
-        std::chrono::system_clock::now());
+      const auto nanos = std::chrono::time_point_cast<std::chrono::nanoseconds>(now);
       const auto time = rclcpp::Time(nanos.time_since_epoch().count());
       subscription_topic_statistics_->handle_message(*typed_message, time);
     }
@@ -340,6 +343,11 @@ private:
     message_memory_strategy_;
   /// Component which computes and publishes topic statistics for this subscriber
   SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics_{nullptr};
+  using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess<
+    CallbackMessageT,
+    AllocatorT,
+    typename MessageUniquePtr::deleter_type>;
+  std::shared_ptr<SubscriptionIntraProcessT> subscription_intra_process_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/subscription_options.hpp

@@ -104,7 +104,7 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
     rcl_subscription_options_t result = rcl_subscription_get_default_options();
     using AllocatorTraits = std::allocator_traits<Allocator>;
     using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc<MessageT>;
-    auto message_alloc = std::make_shared<MessageAllocatorT>(*allocator.get());
+    auto message_alloc = std::make_shared<MessageAllocatorT>(*this->get_allocator().get());
     result.allocator = allocator::get_rcl_allocator<MessageT>(*message_alloc);
     result.qos = qos.get_rmw_qos_profile();
     result.rmw_subscription_options.ignore_local_publications = this->ignore_local_publications;

rclcpp/include/rclcpp/time_source.hpp

@@ -32,6 +32,20 @@ namespace rclcpp
 {
 class Clock;
 
+/**
+ * Time source that will drive the attached clocks.
+ *
+ * If the attached node `use_sim_time` parameter is `true`, the attached clocks will
+ * be updated based on messages received.
+ *
+ * The subscription to the clock topic created by the time source can have it's qos reconfigured
+ * using parameter overrides, particularly the following ones are accepted:
+ *
+ * - qos_overrides./clock.depth
+ * - qos_overrides./clock.durability
+ * - qos_overrides./clock.history
+ * - qos_overrides./clock.reliability
+ */
 class TimeSource
 {
 public:

rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp

@@ -127,7 +127,7 @@ public:
   /**
    * This method acquires a lock to prevent race conditions to collectors list.
    */
-  virtual void publish_message()
+  virtual void publish_message_and_reset_measurements()
   {
     std::vector<MetricsMessage> msgs;
     rclcpp::Time window_end{get_current_nanoseconds_since_epoch()};
@@ -136,6 +136,7 @@ public:
       std::lock_guard<std::mutex> lock(mutex_);
       for (auto & collector : subscriber_statistics_collectors_) {
         const auto collected_stats = collector->GetStatisticsResults();
+        collector->ClearCurrentMeasurements();
 
         auto message = libstatistics_collector::collector::GenerateStatisticMessage(
           node_name_,

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>6.0.0</version>
+  <version>8.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>

rclcpp/resource/logging.hpp.em

@@ -151,10 +151,9 @@ def get_rclcpp_suffix_from_features(features):
 @[ end if]@
       logger.get_name(), \
 @[ if 'stream' not in feature_combination]@
-      rclcpp::get_c_string(RCLCPP_FIRST_ARG(__VA_ARGS__, "")), \
-        RCLCPP_ALL_BUT_FIRST_ARGS(__VA_ARGS__,"")); \
+      __VA_ARGS__); \
 @[ else]@
-      "%s", rclcpp::get_c_string(ss.str())); \
+      "%s", ss.str().c_str()); \
 @[ end if]@
   } while (0)
 

rclcpp/src/rclcpp/clock.cpp

@@ -113,7 +113,7 @@ Clock::get_clock_mutex() noexcept
 
 void
 Clock::on_time_jump(
-  const struct rcl_time_jump_t * time_jump,
+  const rcl_time_jump_t * time_jump,
   bool before_jump,
   void * user_data)
 {

rclcpp/src/rclcpp/context.cpp

@@ -291,7 +291,7 @@ Context::get_domain_id() const
 }
 
 std::string
-Context::shutdown_reason()
+Context::shutdown_reason() const
 {
   std::lock_guard<std::recursive_mutex> lock(init_mutex_);
   return shutdown_reason_;

rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp

@@ -0,0 +1,75 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/detail/mutex_two_priorities.hpp"
+
+#include <mutex>
+
+namespace rclcpp
+{
+namespace detail
+{
+
+using LowPriorityLockable = MutexTwoPriorities::LowPriorityLockable;
+using HighPriorityLockable = MutexTwoPriorities::HighPriorityLockable;
+
+HighPriorityLockable::HighPriorityLockable(MutexTwoPriorities & parent)
+: parent_(parent)
+{}
+
+void
+HighPriorityLockable::lock()
+{
+  parent_.data_.lock();
+}
+
+void
+HighPriorityLockable::unlock()
+{
+  parent_.data_.unlock();
+}
+
+LowPriorityLockable::LowPriorityLockable(MutexTwoPriorities & parent)
+: parent_(parent)
+{}
+
+void
+LowPriorityLockable::lock()
+{
+  std::unique_lock<std::mutex> barrier_guard{parent_.barrier_};
+  parent_.data_.lock();
+  barrier_guard.release();
+}
+
+void
+LowPriorityLockable::unlock()
+{
+  std::lock_guard<std::mutex> barrier_guard{parent_.barrier_, std::adopt_lock};
+  parent_.data_.unlock();
+}
+
+HighPriorityLockable
+MutexTwoPriorities::get_high_priority_lockable()
+{
+  return HighPriorityLockable{*this};
+}
+
+LowPriorityLockable
+MutexTwoPriorities::get_low_priority_lockable()
+{
+  return LowPriorityLockable{*this};
+}
+
+}  // namespace detail
+}  // namespace rclcpp

rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp

@@ -0,0 +1,77 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "./resolve_parameter_overrides.hpp"
+
+#include <string>
+#include <map>
+#include <vector>
+
+#include "rcl_yaml_param_parser/parser.h"
+
+#include "rclcpp/scope_exit.hpp"
+#include "rclcpp/parameter_map.hpp"
+
+std::map<std::string, rclcpp::ParameterValue>
+rclcpp::detail::resolve_parameter_overrides(
+  const std::string & node_fqn,
+  const std::vector<rclcpp::Parameter> & parameter_overrides,
+  const rcl_arguments_t * local_args,
+  const rcl_arguments_t * global_args)
+{
+  std::map<std::string, rclcpp::ParameterValue> result;
+
+  // global before local so that local overwrites global
+  std::array<const rcl_arguments_t *, 2> argument_sources = {global_args, local_args};
+
+  // Get fully qualified node name post-remapping to use to find node's params in yaml files
+
+  for (const rcl_arguments_t * source : argument_sources) {
+    if (!source) {
+      continue;
+    }
+    rcl_params_t * params = NULL;
+    rcl_ret_t ret = rcl_arguments_get_param_overrides(source, &params);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret);
+    }
+    if (params) {
+      auto cleanup_params = make_scope_exit(
+        [params]() {
+          rcl_yaml_node_struct_fini(params);
+        });
+      rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params);
+
+      // Enforce wildcard matching precedence
+      // TODO(cottsay) implement further wildcard matching
+      const std::array<std::string, 2> node_matching_names{"/**", node_fqn};
+      for (const auto & node_name : node_matching_names) {
+        if (initial_map.count(node_name) > 0) {
+          // Combine parameter yaml files, overwriting values in older ones
+          for (const rclcpp::Parameter & param : initial_map.at(node_name)) {
+            result[param.get_name()] =
+              rclcpp::ParameterValue(param.get_value_message());
+          }
+        }
+      }
+    }
+  }
+
+  // parameter overrides passed to constructor will overwrite overrides from yaml file sources
+  for (auto & param : parameter_overrides) {
+    result[param.get_name()] =
+      rclcpp::ParameterValue(param.get_value_message());
+  }
+  return result;
+}

rclcpp/src/rclcpp/detail/resolve_parameter_overrides.hpp

@@ -0,0 +1,44 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__DETAIL__RESOLVE_PARAMETER_OVERRIDES_HPP_
+#define RCLCPP__DETAIL__RESOLVE_PARAMETER_OVERRIDES_HPP_
+
+#include <string>
+#include <map>
+#include <vector>
+
+#include "rcl/arguments.h"
+
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/parameter_value.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace detail
+{
+/// \internal Get the parameter overrides from the arguments.
+RCLCPP_LOCAL
+std::map<std::string, rclcpp::ParameterValue>
+resolve_parameter_overrides(
+  const std::string & node_name,
+  const std::vector<rclcpp::Parameter> & parameter_overrides,
+  const rcl_arguments_t * local_args,
+  const rcl_arguments_t * global_args);
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__RESOLVE_PARAMETER_OVERRIDES_HPP_

rclcpp/src/rclcpp/duration.cpp

@@ -67,13 +67,27 @@ Duration::operator builtin_interfaces::msg::Duration() const
 {
   builtin_interfaces::msg::Duration msg_duration;
   constexpr rcl_duration_value_t kDivisor = RCL_S_TO_NS(1);
+  constexpr int32_t max_s = std::numeric_limits<int32_t>::max();
+  constexpr int32_t min_s = std::numeric_limits<int32_t>::min();
+  constexpr uint32_t max_ns = std::numeric_limits<uint32_t>::max();
   const auto result = std::div(rcl_duration_.nanoseconds, kDivisor);
   if (result.rem >= 0) {
-    msg_duration.sec = static_cast<std::int32_t>(result.quot);
-    msg_duration.nanosec = static_cast<std::uint32_t>(result.rem);
+    // saturate if we will overflow
+    if (result.quot > max_s) {
+      msg_duration.sec = max_s;
+      msg_duration.nanosec = max_ns;
+    } else {
+      msg_duration.sec = static_cast<int32_t>(result.quot);
+      msg_duration.nanosec = static_cast<uint32_t>(result.rem);
+    }
   } else {
-    msg_duration.sec = static_cast<std::int32_t>(result.quot - 1);
-    msg_duration.nanosec = static_cast<std::uint32_t>(kDivisor + result.rem);
+    if (result.quot <= min_s) {
+      msg_duration.sec = min_s;
+      msg_duration.nanosec = 0u;
+    } else {
+      msg_duration.sec = static_cast<int32_t>(result.quot - 1);
+      msg_duration.nanosec = static_cast<uint32_t>(kDivisor + result.rem);
+    }
   }
   return msg_duration;
 }
@@ -238,14 +252,38 @@ Duration::to_rmw_time() const
     throw std::runtime_error("rmw_time_t cannot be negative");
   }
 
-  // reuse conversion logic from msg creation
-  builtin_interfaces::msg::Duration msg = *this;
+  // Purposefully avoid creating from builtin_interfaces::msg::Duration
+  // to avoid possible overflow converting from int64_t to int32_t, then back to uint64_t
   rmw_time_t result;
-  result.sec = static_cast<uint64_t>(msg.sec);
-  result.nsec = static_cast<uint64_t>(msg.nanosec);
+  constexpr rcl_duration_value_t kDivisor = RCL_S_TO_NS(1);
+  const auto div_result = std::div(rcl_duration_.nanoseconds, kDivisor);
+  result.sec = static_cast<uint64_t>(div_result.quot);
+  result.nsec = static_cast<uint64_t>(div_result.rem);
+
   return result;
 }
 
+Duration
+Duration::from_rmw_time(rmw_time_t duration)
+{
+  Duration ret;
+  constexpr rcl_duration_value_t limit_ns = std::numeric_limits<rcl_duration_value_t>::max();
+  constexpr rcl_duration_value_t limit_sec = RCL_NS_TO_S(limit_ns);
+  if (duration.sec > limit_sec || duration.nsec > limit_ns) {
+    // saturate if will overflow
+    ret.rcl_duration_.nanoseconds = limit_ns;
+    return ret;
+  }
+  uint64_t total_ns = RCL_S_TO_NS(duration.sec) + duration.nsec;
+  if (total_ns > limit_ns) {
+    // saturate if will overflow
+    ret.rcl_duration_.nanoseconds = limit_ns;
+    return ret;
+  }
+  ret.rcl_duration_.nanoseconds = static_cast<rcl_duration_value_t>(total_ns);
+  return ret;
+}
+
 Duration
 Duration::from_seconds(double seconds)
 {

rclcpp/src/rclcpp/executor.cpp

@@ -144,6 +144,7 @@ std::vector<rclcpp::CallbackGroup::WeakPtr>
 Executor::get_all_callback_groups()
 {
   std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
+  std::lock_guard<std::mutex> guard{mutex_};
   for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) {
     groups.push_back(group_node_ptr.first);
   }
@@ -157,6 +158,7 @@ std::vector<rclcpp::CallbackGroup::WeakPtr>
 Executor::get_manually_added_callback_groups()
 {
   std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
+  std::lock_guard<std::mutex> guard{mutex_};
   for (auto const & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) {
     groups.push_back(group_node_ptr.first);
   }
@@ -167,6 +169,7 @@ std::vector<rclcpp::CallbackGroup::WeakPtr>
 Executor::get_automatically_added_callback_groups_from_nodes()
 {
   std::vector<rclcpp::CallbackGroup::WeakPtr> groups;
+  std::lock_guard<std::mutex> guard{mutex_};
   for (auto const & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) {
     groups.push_back(group_node_ptr.first);
   }
@@ -233,7 +236,6 @@ Executor::add_callback_group_to_map(
       }
     }
     // Add the node's notify condition to the guard condition handles
-    std::unique_lock<std::mutex> lock(memory_strategy_mutex_);
     memory_strategy_->add_guard_condition(node_ptr->get_notify_guard_condition());
   }
 }
@@ -244,6 +246,7 @@ Executor::add_callback_group(
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
   bool notify)
 {
+  std::lock_guard<std::mutex> guard{mutex_};
   this->add_callback_group_to_map(
     group_ptr,
     node_ptr,
@@ -259,6 +262,7 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt
   if (has_executor.exchange(true)) {
     throw std::runtime_error("Node has already been added to an executor.");
   }
+  std::lock_guard<std::mutex> guard{mutex_};
   for (auto & weak_group : node_ptr->get_callback_groups()) {
     auto group_ptr = weak_group.lock();
     if (group_ptr != nullptr && !group_ptr->get_associated_with_executor_atomic().load() &&
@@ -307,7 +311,6 @@ Executor::remove_callback_group_from_map(
         throw_from_rcl_error(ret, "Failed to trigger guard condition on callback group remove");
       }
     }
-    std::unique_lock<std::mutex> lock(memory_strategy_mutex_);
     memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition());
   }
 }
@@ -317,6 +320,7 @@ Executor::remove_callback_group(
   rclcpp::CallbackGroup::SharedPtr group_ptr,
   bool notify)
 {
+  std::lock_guard<std::mutex> guard{mutex_};
   this->remove_callback_group_from_map(
     group_ptr,
     weak_groups_associated_with_executor_to_nodes_,
@@ -336,6 +340,7 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node
     throw std::runtime_error("Node needs to be associated with an executor.");
   }
 
+  std::lock_guard<std::mutex> guard{mutex_};
   bool found_node = false;
   auto node_it = weak_nodes_.begin();
   while (node_it != weak_nodes_.end()) {
@@ -489,6 +494,7 @@ Executor::set_memory_strategy(rclcpp::memory_strategy::MemoryStrategy::SharedPtr
   if (memory_strategy == nullptr) {
     throw std::runtime_error("Received NULL memory strategy in executor.");
   }
+  std::lock_guard<std::mutex> guard{mutex_};
   memory_strategy_ = memory_strategy;
 }
 
@@ -662,7 +668,7 @@ void
 Executor::wait_for_work(std::chrono::nanoseconds timeout)
 {
   {
-    std::unique_lock<std::mutex> lock(memory_strategy_mutex_);
+    std::lock_guard<std::mutex> guard(mutex_);
 
     // Check weak_nodes_ to find any callback group that is not owned
     // by an executor and add it to the list of callbackgroups for
@@ -683,8 +689,11 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
         if (weak_group_ptr.expired() || weak_node_ptr.expired()) {
           invalid_group_ptrs.push_back(weak_group_ptr);
           auto node_guard_pair = weak_nodes_to_guard_conditions_.find(weak_node_ptr);
-          weak_nodes_to_guard_conditions_.erase(weak_node_ptr);
-          memory_strategy_->remove_guard_condition(node_guard_pair->second);
+          if (node_guard_pair != weak_nodes_to_guard_conditions_.end()) {
+            auto guard_condition = node_guard_pair->second;
+            weak_nodes_to_guard_conditions_.erase(weak_node_ptr);
+            memory_strategy_->remove_guard_condition(guard_condition);
+          }
         }
       }
       std::for_each(
@@ -738,12 +747,14 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
 
   // check the null handles in the wait set and remove them from the handles in memory strategy
   // for callback-based entities
+  std::lock_guard<std::mutex> guard(mutex_);
   memory_strategy_->remove_null_handles(&wait_set_);
 }
 
 rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
 Executor::get_node_by_group(
-  rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap weak_groups_to_nodes,
+  const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap &
+  weak_groups_to_nodes,
   rclcpp::CallbackGroup::SharedPtr group)
 {
   if (!group) {
@@ -761,6 +772,7 @@ Executor::get_node_by_group(
 rclcpp::CallbackGroup::SharedPtr
 Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer)
 {
+  std::lock_guard<std::mutex> guard{mutex_};
   for (const auto & pair : weak_groups_associated_with_executor_to_nodes_) {
     auto group = pair.first.lock();
     if (!group) {
@@ -801,9 +813,11 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable)
 bool
 Executor::get_next_ready_executable_from_map(
   AnyExecutable & any_executable,
-  rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap weak_groups_to_nodes)
+  const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap &
+  weak_groups_to_nodes)
 {
   bool success = false;
+  std::lock_guard<std::mutex> guard{mutex_};
   // Check the timers to see if there are any that are ready
   memory_strategy_->get_next_timer(any_executable, weak_groups_to_nodes);
   if (any_executable.timer) {
@@ -890,7 +904,8 @@ Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanos
 bool
 Executor::has_node(
   const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
-  rclcpp::memory_strategy::MemoryStrategy::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/executors/multi_threaded_executor.cpp

@@ -22,6 +22,7 @@
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/scope_exit.hpp"
 
+using rclcpp::detail::MutexTwoPriorities;
 using rclcpp::executors::MultiThreadedExecutor;
 
 MultiThreadedExecutor::MultiThreadedExecutor(
@@ -51,7 +52,8 @@ MultiThreadedExecutor::spin()
   std::vector<std::thread> threads;
   size_t thread_id = 0;
   {
-    std::lock_guard<std::mutex> wait_lock(wait_mutex_);
+    auto low_priority_wait_mutex = wait_mutex_.get_low_priority_lockable();
+    std::lock_guard<MutexTwoPriorities::LowPriorityLockable> wait_lock(low_priority_wait_mutex);
     for (; thread_id < number_of_threads_ - 1; ++thread_id) {
       auto func = std::bind(&MultiThreadedExecutor::run, this, thread_id);
       threads.emplace_back(func);
@@ -76,7 +78,8 @@ MultiThreadedExecutor::run(size_t)
   while (rclcpp::ok(this->context_) && spinning.load()) {
     rclcpp::AnyExecutable any_exec;
     {
-      std::lock_guard<std::mutex> wait_lock(wait_mutex_);
+      auto low_priority_wait_mutex = wait_mutex_.get_low_priority_lockable();
+      std::lock_guard<MutexTwoPriorities::LowPriorityLockable> wait_lock(low_priority_wait_mutex);
       if (!rclcpp::ok(this->context_) || !spinning.load()) {
         return;
       }
@@ -103,7 +106,8 @@ MultiThreadedExecutor::run(size_t)
     execute_any_executable(any_exec);
 
     if (any_exec.timer) {
-      std::lock_guard<std::mutex> wait_lock(wait_mutex_);
+      auto high_priority_wait_mutex = wait_mutex_.get_high_priority_lockable();
+      std::lock_guard<MutexTwoPriorities::HighPriorityLockable> wait_lock(high_priority_wait_mutex);
       auto it = scheduled_timers_.find(any_exec.timer);
       if (it != scheduled_timers_.end()) {
         scheduled_timers_.erase(it);

rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp

@@ -323,8 +323,7 @@ StaticExecutorEntitiesCollector::add_callback_group(
     throw std::runtime_error("Callback group was already added to executor.");
   }
   if (is_new_node) {
-    rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr);
-    weak_nodes_to_guard_conditions_[node_weak_ptr] = node_ptr->get_notify_guard_condition();
+    weak_nodes_to_guard_conditions_[node_ptr] = node_ptr->get_notify_guard_condition();
     return true;
   }
   return false;

rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp

@@ -175,9 +175,10 @@ StaticSingleThreadedExecutor::execute_ready_executables()
   }
   // Execute all the ready waitables
   for (size_t i = 0; i < entities_collector_->get_number_of_waitables(); ++i) {
-    if (entities_collector_->get_waitable(i)->is_ready(&wait_set_)) {
-      std::shared_ptr<void> shared_ptr;
-      entities_collector_->get_waitable(i)->execute(shared_ptr);
+    auto waitable = entities_collector_->get_waitable(i);
+    if (waitable->is_ready(&wait_set_)) {
+      auto data = waitable->take_data();
+      waitable->execute(data);
     }
   }
 }

rclcpp/src/rclcpp/intra_process_manager.cpp

@@ -157,7 +157,13 @@ IntraProcessManager::get_subscription_intra_process(uint64_t intra_process_subsc
   if (subscription_it == subscriptions_.end()) {
     return nullptr;
   } else {
-    return subscription_it->second.subscription;
+    auto subscription = subscription_it->second.subscription.lock();
+    if (subscription) {
+      return subscription;
+    } else {
+      subscriptions_.erase(subscription_it);
+      return nullptr;
+    }
   }
 }
 

rclcpp/src/rclcpp/logger.cpp

@@ -14,6 +14,8 @@
 
 #include <string>
 
+#include "rcl_logging_interface/rcl_logging_interface.h"
+
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/logger.hpp"
 #include "rclcpp/logging.hpp"
@@ -38,12 +40,28 @@ get_node_logger(const rcl_node_t * node)
   const char * logger_name = rcl_node_get_logger_name(node);
   if (nullptr == logger_name) {
     auto logger = rclcpp::get_logger("rclcpp");
-    RCLCPP_ERROR(logger, "failed to get logger name from node at address %p", node);
+    RCLCPP_ERROR(
+      logger, "failed to get logger name from node at address %p",
+      static_cast<void *>(const_cast<rcl_node_t *>(node)));
     return logger;
   }
   return rclcpp::get_logger(logger_name);
 }
 
+rcpputils::fs::path
+get_logging_directory()
+{
+  char * log_dir = NULL;
+  auto allocator = rcutils_get_default_allocator();
+  rcl_logging_ret_t ret = rcl_logging_get_logging_directory(allocator, &log_dir);
+  if (RCL_LOGGING_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+  std::string path{log_dir};
+  allocator.deallocate(log_dir, allocator.state);
+  return path;
+}
+
 void
 Logger::set_level(Level level)
 {

rclcpp/src/rclcpp/node.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <algorithm>
+#include <array>
 #include <limits>
 #include <map>
 #include <memory>
@@ -20,6 +21,9 @@
 #include <utility>
 #include <vector>
 
+#include "rcl/arguments.h"
+
+#include "rclcpp/detail/qos_parameters.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/graph_listener.hpp"
 #include "rclcpp/node.hpp"
@@ -33,9 +37,12 @@
 #include "rclcpp/node_interfaces/node_timers.hpp"
 #include "rclcpp/node_interfaces/node_topics.hpp"
 #include "rclcpp/node_interfaces/node_waitables.hpp"
+#include "rclcpp/qos_overriding_options.hpp"
 
 #include "rmw/validate_namespace.h"
 
+#include "./detail/resolve_parameter_overrides.hpp"
+
 using rclcpp::Node;
 using rclcpp::NodeOptions;
 using rclcpp::exceptions::throw_from_rcl_error;
@@ -94,6 +101,45 @@ Node::Node(
 {
 }
 
+static
+rclcpp::QoS
+get_parameter_events_qos(
+  rclcpp::node_interfaces::NodeBaseInterface & node_base,
+  const rclcpp::NodeOptions & options)
+{
+  auto final_qos = options.parameter_event_qos();
+  const rcl_arguments_t * global_args = nullptr;
+  auto * rcl_options = options.get_rcl_node_options();
+  if (rcl_options->use_global_arguments) {
+    auto context_ptr = node_base.get_context()->get_rcl_context();
+    global_args = &(context_ptr->global_arguments);
+  }
+
+  auto parameter_overrides = rclcpp::detail::resolve_parameter_overrides(
+    node_base.get_fully_qualified_name(),
+    options.parameter_overrides(),
+    &rcl_options->arguments,
+    global_args);
+
+  auto final_topic_name = node_base.resolve_topic_or_service_name("/parameter_events", false);
+  auto prefix = "qos_overrides." + final_topic_name + ".";
+  std::array<rclcpp::QosPolicyKind, 4> policies = {
+    rclcpp::QosPolicyKind::Depth,
+    rclcpp::QosPolicyKind::Durability,
+    rclcpp::QosPolicyKind::History,
+    rclcpp::QosPolicyKind::Reliability,
+  };
+  for (const auto & policy : policies) {
+    auto param_name = prefix + rclcpp::qos_policy_kind_to_cstr(policy);
+    auto it = parameter_overrides.find(param_name);
+    auto value = it != parameter_overrides.end() ?
+      it->second :
+      rclcpp::detail::get_default_qos_param_value(policy, options.parameter_event_qos());
+    rclcpp::detail::apply_qos_override(policy, value, final_qos);
+  }
+  return final_qos;
+}
+
 Node::Node(
   const std::string & node_name,
   const std::string & namespace_,
@@ -126,7 +172,9 @@ Node::Node(
       options.parameter_overrides(),
       options.start_parameter_services(),
       options.start_parameter_event_publisher(),
-      options.parameter_event_qos(),
+      // This is needed in order to apply parameter overrides to the qos profile provided in
+      // options.
+      get_parameter_events_qos(*node_base_, options),
       options.parameter_event_publisher_options(),
       options.allow_undeclared_parameters(),
       options.automatically_declare_parameters_from_overrides()
@@ -146,6 +194,20 @@ Node::Node(
   sub_namespace_(""),
   effective_namespace_(create_effective_namespace(this->get_namespace(), sub_namespace_))
 {
+  // we have got what we wanted directly from the overrides,
+  // but declare the parameters anyway so they are visible.
+  rclcpp::detail::declare_qos_parameters(
+    rclcpp::QosOverridingOptions
+  {
+    QosPolicyKind::Depth,
+    QosPolicyKind::Durability,
+    QosPolicyKind::History,
+    QosPolicyKind::Reliability,
+  },
+    node_parameters_,
+    node_topics_->resolve_topic_name("/parameter_events"),
+    options.parameter_event_qos(),
+    rclcpp::detail::PublisherQosParametersTraits{});
 }
 
 Node::Node(
@@ -185,7 +247,18 @@ Node::Node(
 }
 
 Node::~Node()
-{}
+{
+  // release sub-interfaces in an order that allows them to consult with node_base during tear-down
+  node_waitables_.reset();
+  node_time_source_.reset();
+  node_parameters_.reset();
+  node_clock_.reset();
+  node_services_.reset();
+  node_topics_.reset();
+  node_timers_.reset();
+  node_logging_.reset();
+  node_graph_.reset();
+}
 
 const char *
 Node::get_name() const
@@ -219,6 +292,24 @@ Node::create_callback_group(
   return node_base_->create_callback_group(group_type, automatically_add_to_executor_with_node);
 }
 
+const rclcpp::ParameterValue &
+Node::declare_parameter(const std::string & name)
+{
+#ifndef _WIN32
+# pragma GCC diagnostic push
+# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#else
+# pragma warning(push)
+# pragma warning(disable: 4996)
+#endif
+  return this->node_parameters_->declare_parameter(name);
+#ifndef _WIN32
+# pragma GCC diagnostic pop
+#else
+# pragma warning(pop)
+#endif
+}
+
 const rclcpp::ParameterValue &
 Node::declare_parameter(
   const std::string & name,
@@ -233,6 +324,20 @@ Node::declare_parameter(
     ignore_override);
 }
 
+const rclcpp::ParameterValue &
+Node::declare_parameter(
+  const std::string & name,
+  rclcpp::ParameterType type,
+  const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor,
+  bool ignore_override)
+{
+  return this->node_parameters_->declare_parameter(
+    name,
+    type,
+    parameter_descriptor,
+    ignore_override);
+}
+
 void
 Node::undeclare_parameter(const std::string & name)
 {

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -16,8 +16,10 @@
 
 #include <rcl_yaml_param_parser/parser.h>
 
+#include <array>
 #include <cmath>
 #include <cstdlib>
+#include <cstring>
 #include <functional>
 #include <limits>
 #include <map>
@@ -34,6 +36,8 @@
 #include "rcutils/logging_macros.h"
 #include "rmw/qos_profiles.h"
 
+#include "../detail/resolve_parameter_overrides.hpp"
+
 using rclcpp::node_interfaces::NodeParameters;
 
 NodeParameters::NodeParameters(
@@ -85,60 +89,27 @@ NodeParameters::NodeParameters(
     throw std::runtime_error("Need valid node options in NodeParameters");
   }
 
-  std::vector<const rcl_arguments_t *> argument_sources;
-  // global before local so that local overwrites global
+  const rcl_arguments_t * global_args = nullptr;
   if (options->use_global_arguments) {
     auto context_ptr = node_base->get_context()->get_rcl_context();
-    argument_sources.push_back(&(context_ptr->global_arguments));
+    global_args = &(context_ptr->global_arguments);
   }
-  argument_sources.push_back(&options->arguments);
-
-  // Get fully qualified node name post-remapping to use to find node's params in yaml files
   combined_name_ = node_base->get_fully_qualified_name();
 
-  for (const rcl_arguments_t * source : argument_sources) {
-    rcl_params_t * params = NULL;
-    rcl_ret_t ret = rcl_arguments_get_param_overrides(source, &params);
-    if (RCL_RET_OK != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret);
-    }
-    if (params) {
-      auto cleanup_params = make_scope_exit(
-        [params]() {
-          rcl_yaml_node_struct_fini(params);
-        });
-      rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params);
-
-      // Enforce wildcard matching precedence
-      // TODO(cottsay) implement further wildcard matching
-      const std::vector<std::string> node_matching_names{"/**", combined_name_};
-      for (const auto & node_name : node_matching_names) {
-        if (initial_map.count(node_name) > 0) {
-          // Combine parameter yaml files, overwriting values in older ones
-          for (const rclcpp::Parameter & param : initial_map.at(node_name)) {
-            parameter_overrides_[param.get_name()] =
-              rclcpp::ParameterValue(param.get_value_message());
-          }
-        }
-      }
-    }
-  }
-
-  // parameter overrides passed to constructor will overwrite overrides from yaml file sources
-  for (auto & param : parameter_overrides) {
-    parameter_overrides_[param.get_name()] =
-      rclcpp::ParameterValue(param.get_value_message());
-  }
+  parameter_overrides_ = rclcpp::detail::resolve_parameter_overrides(
+    combined_name_, parameter_overrides, &options->arguments, global_args);
 
   // If asked, initialize any parameters that ended up in the initial parameter values,
   // but did not get declared explcitily by this point.
   if (automatically_declare_parameters_from_overrides) {
+    rcl_interfaces::msg::ParameterDescriptor descriptor;
+    descriptor.dynamic_typing = true;
     for (const auto & pair : this->get_parameter_overrides()) {
       if (!this->has_parameter(pair.first)) {
         this->declare_parameter(
           pair.first,
           pair.second,
-          rcl_interfaces::msg::ParameterDescriptor(),
+          descriptor,
           true);
       }
     }
@@ -165,14 +136,13 @@ __are_doubles_equal(double x, double y, double ulp = 100.0)
   return std::abs(x - y) <= std::numeric_limits<double>::epsilon() * std::abs(x + y) * ulp;
 }
 
-RCLCPP_LOCAL
-inline
-void
-format_reason(std::string & reason, const std::string & name, const char * range_type)
+static
+std::string
+format_range_reason(const std::string & name, const char * range_type)
 {
   std::ostringstream ss;
   ss << "Parameter {" << name << "} doesn't comply with " << range_type << " range.";
-  reason = ss.str();
+  return ss.str();
 }
 
 RCLCPP_LOCAL
@@ -191,7 +161,7 @@ __check_parameter_value_in_range(
     }
     if ((v < integer_range.from_value) || (v > integer_range.to_value)) {
       result.successful = false;
-      format_reason(result.reason, descriptor.name, "integer");
+      result.reason = format_range_reason(descriptor.name, "integer");
       return result;
     }
     if (integer_range.step == 0) {
@@ -201,7 +171,7 @@ __check_parameter_value_in_range(
       return result;
     }
     result.successful = false;
-    format_reason(result.reason, descriptor.name, "integer");
+    result.reason = format_range_reason(descriptor.name, "integer");
     return result;
   }
 
@@ -213,7 +183,7 @@ __check_parameter_value_in_range(
     }
     if ((v < fp_range.from_value) || (v > fp_range.to_value)) {
       result.successful = false;
-      format_reason(result.reason, descriptor.name, "floating point");
+      result.reason = format_range_reason(descriptor.name, "floating point");
       return result;
     }
     if (fp_range.step == 0.0) {
@@ -224,29 +194,62 @@ __check_parameter_value_in_range(
       return result;
     }
     result.successful = false;
-    format_reason(result.reason, descriptor.name, "floating point");
+    result.reason = format_range_reason(descriptor.name, "floating point");
     return result;
   }
   return result;
 }
 
+static
+std::string
+format_type_reason(
+  const std::string & name, const std::string & old_type, const std::string & new_type)
+{
+  std::ostringstream ss;
+  // WARN: A condition later depends on this message starting with "Wrong parameter type",
+  // check `declare_parameter` if you modify this!
+  ss << "Wrong parameter type, parameter {" << name << "} is of type {" << old_type <<
+    "}, setting it to {" << new_type << "} is not allowed.";
+  return ss.str();
+}
+
 // Return true if parameter values comply with the descriptors in parameter_infos.
 RCLCPP_LOCAL
 rcl_interfaces::msg::SetParametersResult
 __check_parameters(
   std::map<std::string, rclcpp::node_interfaces::ParameterInfo> & parameter_infos,
-  const std::vector<rclcpp::Parameter> & parameters)
+  const std::vector<rclcpp::Parameter> & parameters,
+  bool allow_undeclared)
 {
   rcl_interfaces::msg::SetParametersResult result;
   result.successful = true;
   for (const rclcpp::Parameter & parameter : parameters) {
-    const rcl_interfaces::msg::ParameterDescriptor & descriptor =
-      parameter_infos[parameter.get_name()].descriptor;
+    std::string name = parameter.get_name();
+    rcl_interfaces::msg::ParameterDescriptor descriptor;
+    if (allow_undeclared) {
+      auto it = parameter_infos.find(name);
+      if (it != parameter_infos.cend()) {
+        descriptor = it->second.descriptor;
+      } else {
+        // implicitly declared parameters are dinamically typed!
+        descriptor.dynamic_typing = true;
+      }
+    } else {
+      descriptor = parameter_infos[name].descriptor;
+    }
+    const auto new_type = parameter.get_type();
+    const auto specified_type = static_cast<rclcpp::ParameterType>(descriptor.type);
+    result.successful = descriptor.dynamic_typing || specified_type == new_type;
+    if (!result.successful) {
+      result.reason = format_type_reason(
+        name, rclcpp::to_string(specified_type), rclcpp::to_string(new_type));
+      return result;
+    }
     result = __check_parameter_value_in_range(
       descriptor,
       parameter.get_parameter_value());
     if (!result.successful) {
-      break;
+      return result;
     }
   }
   return result;
@@ -293,7 +296,8 @@ __set_parameters_atomically_common(
   const std::vector<rclcpp::Parameter> & parameters,
   std::map<std::string, rclcpp::node_interfaces::ParameterInfo> & parameter_infos,
   CallbacksContainerType & callback_container,
-  const OnParametersSetCallbackType & callback)
+  const OnParametersSetCallbackType & callback,
+  bool allow_undeclared = false)
 {
   // Call the user callback to see if the new value(s) are allowed.
   rcl_interfaces::msg::SetParametersResult result =
@@ -302,7 +306,7 @@ __set_parameters_atomically_common(
     return result;
   }
   // Check if the value being set complies with the descriptor.
-  result = __check_parameters(parameter_infos, parameters);
+  result = __check_parameters(parameter_infos, parameters, allow_undeclared);
   if (!result.successful) {
     return result;
   }
@@ -353,6 +357,10 @@ __declare_parameter_common(
     callback_container,
     callback);
 
+  if (!result.successful) {
+    return result;
+  }
+
   // Add declared parameters to storage.
   parameters_out[name] = parameter_infos.at(name);
 
@@ -364,6 +372,99 @@ __declare_parameter_common(
   return result;
 }
 
+static
+const rclcpp::ParameterValue &
+declare_parameter_helper(
+  const std::string & name,
+  rclcpp::ParameterType type,
+  const rclcpp::ParameterValue & default_value,
+  rcl_interfaces::msg::ParameterDescriptor parameter_descriptor,
+  bool ignore_override,
+  std::map<std::string, rclcpp::node_interfaces::ParameterInfo> & parameters,
+  const std::map<std::string, rclcpp::ParameterValue> & overrides,
+  CallbacksContainerType & callback_container,
+  const OnParametersSetCallbackType & callback,
+  rclcpp::Publisher<rcl_interfaces::msg::ParameterEvent> * events_publisher,
+  const std::string & combined_name,
+  rclcpp::node_interfaces::NodeClockInterface & node_clock)
+{
+  // TODO(sloretz) parameter name validation
+  if (name.empty()) {
+    throw rclcpp::exceptions::InvalidParametersException("parameter name must not be empty");
+  }
+
+  // Error if this parameter has already been declared and is different
+  if (__lockless_has_parameter(parameters, name)) {
+    throw rclcpp::exceptions::ParameterAlreadyDeclaredException(
+            "parameter '" + name + "' has already been declared");
+  }
+
+  if (!parameter_descriptor.dynamic_typing) {
+    if (rclcpp::PARAMETER_NOT_SET == type) {
+      type = default_value.get_type();
+    }
+    if (rclcpp::PARAMETER_NOT_SET == type) {
+      throw rclcpp::exceptions::InvalidParameterTypeException{
+              name,
+              "cannot declare a statically typed parameter with an uninitialized value"
+      };
+    }
+    parameter_descriptor.type = static_cast<uint8_t>(type);
+  }
+
+  if (
+    rclcpp::PARAMETER_NOT_SET == default_value.get_type() &&
+    overrides.find(name) == overrides.end() &&
+    parameter_descriptor.dynamic_typing == false)
+  {
+    throw rclcpp::exceptions::NoParameterOverrideProvided(name);
+  }
+
+  rcl_interfaces::msg::ParameterEvent parameter_event;
+  auto result = __declare_parameter_common(
+    name,
+    default_value,
+    parameter_descriptor,
+    parameters,
+    overrides,
+    callback_container,
+    callback,
+    &parameter_event,
+    ignore_override);
+
+  // If it failed to be set, then throw an exception.
+  if (!result.successful) {
+    constexpr const char type_error_msg_start[] = "Wrong parameter type";
+    if (
+      0u == std::strncmp(
+        result.reason.c_str(), type_error_msg_start, sizeof(type_error_msg_start) - 1))
+    {
+      // TODO(ivanpauno): Refactor the logic so we don't need the above `strncmp` and we can
+      // detect between both exceptions more elegantly.
+      throw rclcpp::exceptions::InvalidParameterTypeException(name, result.reason);
+    }
+    throw rclcpp::exceptions::InvalidParameterValueException(
+            "parameter '" + name + "' could not be set: " + result.reason);
+  }
+
+  // Publish if events_publisher_ is not nullptr, which may be if disabled in the constructor.
+  if (nullptr != events_publisher) {
+    parameter_event.node = combined_name;
+    parameter_event.stamp = node_clock.get_clock()->now();
+    events_publisher->publish(parameter_event);
+  }
+
+  return parameters.at(name).value;
+}
+
+const rclcpp::ParameterValue &
+NodeParameters::declare_parameter(const std::string & name)
+{
+  rcl_interfaces::msg::ParameterDescriptor descriptor;
+  descriptor.dynamic_typing = true;
+  return this->declare_parameter(name, rclcpp::ParameterValue{}, descriptor, false);
+}
+
 const rclcpp::ParameterValue &
 NodeParameters::declare_parameter(
   const std::string & name,
@@ -372,46 +473,57 @@ NodeParameters::declare_parameter(
   bool ignore_override)
 {
   std::lock_guard<std::recursive_mutex> lock(mutex_);
-
   ParameterMutationRecursionGuard guard(parameter_modification_enabled_);
 
-  // TODO(sloretz) parameter name validation
-  if (name.empty()) {
-    throw rclcpp::exceptions::InvalidParametersException("parameter name must not be empty");
-  }
-
-  // Error if this parameter has already been declared and is different
-  if (__lockless_has_parameter(parameters_, name)) {
-    throw rclcpp::exceptions::ParameterAlreadyDeclaredException(
-            "parameter '" + name + "' has already been declared");
-  }
-
-  rcl_interfaces::msg::ParameterEvent parameter_event;
-  auto result = __declare_parameter_common(
+  return declare_parameter_helper(
     name,
+    rclcpp::PARAMETER_NOT_SET,
     default_value,
     parameter_descriptor,
+    ignore_override,
     parameters_,
     parameter_overrides_,
     on_parameters_set_callback_container_,
     on_parameters_set_callback_,
-    &parameter_event,
-    ignore_override);
+    events_publisher_.get(),
+    combined_name_,
+    *node_clock_);
+}
 
-  // If it failed to be set, then throw an exception.
-  if (!result.successful) {
-    throw rclcpp::exceptions::InvalidParameterValueException(
-            "parameter '" + name + "' could not be set: " + result.reason);
+const rclcpp::ParameterValue &
+NodeParameters::declare_parameter(
+  const std::string & name,
+  rclcpp::ParameterType type,
+  const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor,
+  bool ignore_override)
+{
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+  ParameterMutationRecursionGuard guard(parameter_modification_enabled_);
+
+  if (rclcpp::PARAMETER_NOT_SET == type) {
+    throw std::invalid_argument{
+            "declare_parameter(): the provided parameter type cannot be rclcpp::PARAMETER_NOT_SET"};
   }
 
-  // Publish if events_publisher_ is not nullptr, which may be if disabled in the constructor.
-  if (nullptr != events_publisher_) {
-    parameter_event.node = combined_name_;
-    parameter_event.stamp = node_clock_->get_clock()->now();
-    events_publisher_->publish(parameter_event);
+  if (parameter_descriptor.dynamic_typing == true) {
+    throw std::invalid_argument{
+            "declare_parameter(): cannot declare parameter of specific type and pass descriptor"
+            "with `dynamic_typing=true`"};
   }
 
-  return parameters_.at(name).value;
+  return declare_parameter_helper(
+    name,
+    type,
+    rclcpp::ParameterValue{},
+    parameter_descriptor,
+    ignore_override,
+    parameters_,
+    parameter_overrides_,
+    on_parameters_set_callback_container_,
+    on_parameters_set_callback_,
+    events_publisher_.get(),
+    combined_name_,
+    *node_clock_);
 }
 
 void
@@ -431,6 +543,10 @@ NodeParameters::undeclare_parameter(const std::string & name)
     throw rclcpp::exceptions::ParameterImmutableException(
             "cannot undeclare parameter '" + name + "' because it is read-only");
   }
+  if (!parameter_info->second.descriptor.dynamic_typing) {
+    throw rclcpp::exceptions::InvalidParameterTypeException{
+            name, "cannot undeclare an statically typed parameter"};
+  }
 
   parameters_.erase(parameter_info);
 }
@@ -524,13 +640,17 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
   rcl_interfaces::msg::ParameterEvent parameter_event_msg;
   parameter_event_msg.node = combined_name_;
   CallbacksContainerType empty_callback_container;
+
+  // Implicit declare uses dynamic type descriptor.
+  rcl_interfaces::msg::ParameterDescriptor descriptor;
+  descriptor.dynamic_typing = true;
   for (auto parameter_to_be_declared : parameters_to_be_declared) {
     // This should not throw, because we validated the name and checked that
     // the parameter was not already declared.
     result = __declare_parameter_common(
       parameter_to_be_declared->get_name(),
       parameter_to_be_declared->get_parameter_value(),
-      rcl_interfaces::msg::ParameterDescriptor(),  // Implicit declare uses default descriptor.
+      descriptor,
       staged_parameter_changes,
       parameter_overrides_,
       // Only call callbacks once below
@@ -579,6 +699,11 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
     if (rclcpp::PARAMETER_NOT_SET == parameter.get_type()) {
       auto it = parameters_.find(parameter.get_name());
       if (it != parameters_.end() && rclcpp::PARAMETER_NOT_SET != it->second.value.get_type()) {
+        if (!it->second.descriptor.dynamic_typing) {
+          result.reason = "cannot undeclare an statically typed parameter";
+          result.successful = false;
+          return result;
+        }
         parameters_to_be_undeclared.push_back(&parameter);
       }
     }
@@ -594,7 +719,8 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
     on_parameters_set_callback_container_,
     // These callbacks are called once. When a callback returns an unsuccessful result,
     // the remaining aren't called.
-    on_parameters_set_callback_);
+    on_parameters_set_callback_,
+    allow_undeclared_);  // allow undeclared
 
   // If not successful, then stop here.
   if (!result.successful) {

rclcpp/src/rclcpp/node_interfaces/node_timers.cpp

@@ -16,6 +16,8 @@
 
 #include <string>
 
+#include "tracetools/tracetools.h"
+
 using rclcpp::node_interfaces::NodeTimers;
 
 NodeTimers::NodeTimers(rclcpp::node_interfaces::NodeBaseInterface * node_base)
@@ -44,4 +46,8 @@ NodeTimers::add_timer(
             std::string("Failed to notify wait set on timer creation: ") +
             rmw_get_error_string().str);
   }
+  TRACEPOINT(
+    rclcpp_timer_link_node,
+    static_cast<const void *>(timer->get_timer_handle().get()),
+    static_cast<const void *>(node_base_->get_rcl_node_handle()));
 }

rclcpp/src/rclcpp/parameter_client.cpp

@@ -144,6 +144,35 @@ AsyncParametersClient::get_parameters(
   return future_result;
 }
 
+std::shared_future<std::vector<rcl_interfaces::msg::ParameterDescriptor>>
+AsyncParametersClient::describe_parameters(
+  const std::vector<std::string> & names,
+  std::function<
+    void(std::shared_future<std::vector<rcl_interfaces::msg::ParameterDescriptor>>)
+  > callback)
+{
+  auto promise_result =
+    std::make_shared<std::promise<std::vector<rcl_interfaces::msg::ParameterDescriptor>>>();
+  auto future_result = promise_result->get_future().share();
+
+  auto request = std::make_shared<rcl_interfaces::srv::DescribeParameters::Request>();
+  request->names = names;
+
+  describe_parameters_client_->async_send_request(
+    request,
+    [promise_result, future_result, callback](
+      rclcpp::Client<rcl_interfaces::srv::DescribeParameters>::SharedFuture cb_f)
+    {
+      promise_result->set_value(cb_f.get()->descriptors);
+      if (callback != nullptr) {
+        callback(future_result);
+      }
+    }
+  );
+
+  return future_result;
+}
+
 std::shared_future<std::vector<rclcpp::ParameterType>>
 AsyncParametersClient::get_parameter_types(
   const std::vector<std::string> & names,
@@ -313,14 +342,16 @@ AsyncParametersClient::wait_for_service_nanoseconds(std::chrono::nanoseconds tim
 }
 
 std::vector<rclcpp::Parameter>
-SyncParametersClient::get_parameters(const std::vector<std::string> & parameter_names)
+SyncParametersClient::get_parameters(
+  const std::vector<std::string> & parameter_names,
+  std::chrono::nanoseconds timeout)
 {
   auto f = async_parameters_client_->get_parameters(parameter_names);
   using rclcpp::executors::spin_node_until_future_complete;
   if (
     spin_node_until_future_complete(
-      *executor_, node_base_interface_,
-      f) == rclcpp::FutureReturnCode::SUCCESS)
+      *executor_, node_base_interface_, f,
+      timeout) == rclcpp::FutureReturnCode::SUCCESS)
   {
     return f.get();
   }
@@ -337,16 +368,34 @@ SyncParametersClient::has_parameter(const std::string & parameter_name)
   return vars.names.size() > 0;
 }
 
+std::vector<rcl_interfaces::msg::ParameterDescriptor>
+SyncParametersClient::describe_parameters(
+  const std::vector<std::string> & parameter_names,
+  std::chrono::nanoseconds timeout)
+{
+  auto f = async_parameters_client_->describe_parameters(parameter_names);
+
+  using rclcpp::executors::spin_node_until_future_complete;
+  rclcpp::FutureReturnCode future =
+    spin_node_until_future_complete(*executor_, node_base_interface_, f, timeout);
+  if (future == rclcpp::FutureReturnCode::SUCCESS) {
+    return f.get();
+  }
+  return std::vector<rcl_interfaces::msg::ParameterDescriptor>();
+}
+
 std::vector<rclcpp::ParameterType>
-SyncParametersClient::get_parameter_types(const std::vector<std::string> & parameter_names)
+SyncParametersClient::get_parameter_types(
+  const std::vector<std::string> & parameter_names,
+  std::chrono::nanoseconds timeout)
 {
   auto f = async_parameters_client_->get_parameter_types(parameter_names);
 
   using rclcpp::executors::spin_node_until_future_complete;
   if (
     spin_node_until_future_complete(
-      *executor_, node_base_interface_,
-      f) == rclcpp::FutureReturnCode::SUCCESS)
+      *executor_, node_base_interface_, f,
+      timeout) == rclcpp::FutureReturnCode::SUCCESS)
   {
     return f.get();
   }
@@ -355,15 +404,16 @@ SyncParametersClient::get_parameter_types(const std::vector<std::string> & param
 
 std::vector<rcl_interfaces::msg::SetParametersResult>
 SyncParametersClient::set_parameters(
-  const std::vector<rclcpp::Parameter> & parameters)
+  const std::vector<rclcpp::Parameter> & parameters,
+  std::chrono::nanoseconds timeout)
 {
   auto f = async_parameters_client_->set_parameters(parameters);
 
   using rclcpp::executors::spin_node_until_future_complete;
   if (
     spin_node_until_future_complete(
-      *executor_, node_base_interface_,
-      f) == rclcpp::FutureReturnCode::SUCCESS)
+      *executor_, node_base_interface_, f,
+      timeout) == rclcpp::FutureReturnCode::SUCCESS)
   {
     return f.get();
   }
@@ -372,15 +422,16 @@ SyncParametersClient::set_parameters(
 
 rcl_interfaces::msg::SetParametersResult
 SyncParametersClient::set_parameters_atomically(
-  const std::vector<rclcpp::Parameter> & parameters)
+  const std::vector<rclcpp::Parameter> & parameters,
+  std::chrono::nanoseconds timeout)
 {
   auto f = async_parameters_client_->set_parameters_atomically(parameters);
 
   using rclcpp::executors::spin_node_until_future_complete;
   if (
     spin_node_until_future_complete(
-      *executor_, node_base_interface_,
-      f) == rclcpp::FutureReturnCode::SUCCESS)
+      *executor_, node_base_interface_, f,
+      timeout) == rclcpp::FutureReturnCode::SUCCESS)
   {
     return f.get();
   }
@@ -391,15 +442,16 @@ SyncParametersClient::set_parameters_atomically(
 rcl_interfaces::msg::ListParametersResult
 SyncParametersClient::list_parameters(
   const std::vector<std::string> & parameter_prefixes,
-  uint64_t depth)
+  uint64_t depth,
+  std::chrono::nanoseconds timeout)
 {
   auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth);
 
   using rclcpp::executors::spin_node_until_future_complete;
   if (
     spin_node_until_future_complete(
-      *executor_, node_base_interface_,
-      f) == rclcpp::FutureReturnCode::SUCCESS)
+      *executor_, node_base_interface_, f,
+      timeout) == rclcpp::FutureReturnCode::SUCCESS)
   {
     return f.get();
   }

rclcpp/src/rclcpp/parameter_event_handler.cpp

@@ -0,0 +1,222 @@
+// Copyright 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <functional>
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include "rclcpp/parameter_event_handler.hpp"
+#include "rcpputils/join.hpp"
+
+namespace rclcpp
+{
+
+ParameterEventCallbackHandle::SharedPtr
+ParameterEventHandler::add_parameter_event_callback(
+  ParameterEventCallbackType callback)
+{
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+  auto handle = std::make_shared<ParameterEventCallbackHandle>();
+  handle->callback = callback;
+  event_callbacks_.emplace_front(handle);
+
+  return handle;
+}
+
+template<typename CallbackHandleT>
+struct HandleCompare
+  : public std::unary_function<typename CallbackHandleT::WeakPtr, bool>
+{
+  explicit HandleCompare(const CallbackHandleT * const base)
+  : base_(base) {}
+  bool operator()(const typename CallbackHandleT::WeakPtr & handle)
+  {
+    auto shared_handle = handle.lock();
+    if (base_ == shared_handle.get()) {
+      return true;
+    }
+    return false;
+  }
+  const CallbackHandleT * const base_;
+};
+
+void
+ParameterEventHandler::remove_parameter_event_callback(
+  ParameterEventCallbackHandle::SharedPtr callback_handle)
+{
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+  auto it = std::find_if(
+    event_callbacks_.begin(),
+    event_callbacks_.end(),
+    HandleCompare<ParameterEventCallbackHandle>(callback_handle.get()));
+  if (it != event_callbacks_.end()) {
+    event_callbacks_.erase(it);
+  } else {
+    throw std::runtime_error("Callback doesn't exist");
+  }
+}
+
+ParameterCallbackHandle::SharedPtr
+ParameterEventHandler::add_parameter_callback(
+  const std::string & parameter_name,
+  ParameterCallbackType callback,
+  const std::string & node_name)
+{
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+  auto full_node_name = resolve_path(node_name);
+
+  auto handle = std::make_shared<ParameterCallbackHandle>();
+  handle->callback = callback;
+  handle->parameter_name = parameter_name;
+  handle->node_name = full_node_name;
+  // the last callback registered is executed first.
+  parameter_callbacks_[{parameter_name, full_node_name}].emplace_front(handle);
+
+  return handle;
+}
+
+void
+ParameterEventHandler::remove_parameter_callback(
+  ParameterCallbackHandle::SharedPtr callback_handle)
+{
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+  auto handle = callback_handle.get();
+  auto & container = parameter_callbacks_[{handle->parameter_name, handle->node_name}];
+  auto it = std::find_if(
+    container.begin(),
+    container.end(),
+    HandleCompare<ParameterCallbackHandle>(handle));
+  if (it != container.end()) {
+    container.erase(it);
+    if (container.empty()) {
+      parameter_callbacks_.erase({handle->parameter_name, handle->node_name});
+    }
+  } else {
+    throw std::runtime_error("Callback doesn't exist");
+  }
+}
+
+bool
+ParameterEventHandler::get_parameter_from_event(
+  const rcl_interfaces::msg::ParameterEvent & event,
+  rclcpp::Parameter & parameter,
+  const std::string parameter_name,
+  const std::string node_name)
+{
+  if (event.node != node_name) {
+    return false;
+  }
+
+  for (auto & new_parameter : event.new_parameters) {
+    if (new_parameter.name == parameter_name) {
+      parameter = rclcpp::Parameter::from_parameter_msg(new_parameter);
+      return true;
+    }
+  }
+
+  for (auto & changed_parameter : event.changed_parameters) {
+    if (changed_parameter.name == parameter_name) {
+      parameter = rclcpp::Parameter::from_parameter_msg(changed_parameter);
+      return true;
+    }
+  }
+
+  return false;
+}
+
+rclcpp::Parameter
+ParameterEventHandler::get_parameter_from_event(
+  const rcl_interfaces::msg::ParameterEvent & event,
+  const std::string parameter_name,
+  const std::string node_name)
+{
+  rclcpp::Parameter p;
+  if (!get_parameter_from_event(event, p, parameter_name, node_name)) {
+    throw std::runtime_error(
+            "Parameter '" + parameter_name + "' of node '" + node_name +
+            "' is not part of parameter event");
+  }
+  return p;
+}
+
+std::vector<rclcpp::Parameter>
+ParameterEventHandler::get_parameters_from_event(
+  const rcl_interfaces::msg::ParameterEvent & event)
+{
+  std::vector<rclcpp::Parameter> params;
+
+  for (auto & new_parameter : event.new_parameters) {
+    params.push_back(rclcpp::Parameter::from_parameter_msg(new_parameter));
+  }
+
+  for (auto & changed_parameter : event.changed_parameters) {
+    params.push_back(rclcpp::Parameter::from_parameter_msg(changed_parameter));
+  }
+
+  return params;
+}
+
+void
+ParameterEventHandler::event_callback(
+  const rcl_interfaces::msg::ParameterEvent::SharedPtr event)
+{
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+
+  for (auto it = parameter_callbacks_.begin(); it != parameter_callbacks_.end(); ++it) {
+    rclcpp::Parameter p;
+    if (get_parameter_from_event(*event, p, it->first.first, it->first.second)) {
+      for (auto cb = it->second.begin(); cb != it->second.end(); ++cb) {
+        auto shared_handle = cb->lock();
+        if (nullptr != shared_handle) {
+          shared_handle->callback(p);
+        } else {
+          cb = it->second.erase(cb);
+        }
+      }
+    }
+  }
+
+  for (auto event_cb = event_callbacks_.begin(); event_cb != event_callbacks_.end(); ++event_cb) {
+    auto shared_event_handle = event_cb->lock();
+    if (nullptr != shared_event_handle) {
+      shared_event_handle->callback(event);
+    } else {
+      event_cb = event_callbacks_.erase(event_cb);
+    }
+  }
+}
+
+std::string
+ParameterEventHandler::resolve_path(const std::string & path)
+{
+  std::string full_path;
+
+  if (path == "") {
+    full_path = node_base_->get_fully_qualified_name();
+  } else {
+    full_path = path;
+    if (*path.begin() != '/') {
+      auto ns = node_base_->get_namespace();
+      const std::vector<std::string> paths{ns, path};
+      full_path = (ns == std::string("/")) ? ns + path : rcpputils::join(paths, "/");
+    }
+  }
+
+  return full_path;
+}
+
+}  // namespace rclcpp

rclcpp/src/rclcpp/parameter_service.cpp

@@ -41,12 +41,13 @@ ParameterService::ParameterService(
       const std::shared_ptr<rcl_interfaces::srv::GetParameters::Request> request,
       std::shared_ptr<rcl_interfaces::srv::GetParameters::Response> response)
     {
-      for (const auto & name : request->names) {
-        // Default construct param to NOT_SET
-        rclcpp::Parameter param;
-        node_params->get_parameter(name, param);
-        // push back NOT_SET when get_parameter() call fails
-        response->values.push_back(param.get_value_message());
+      try {
+        auto parameters = node_params->get_parameters(request->names);
+        for (const auto & param : parameters) {
+          response->values.push_back(param.get_value_message());
+        }
+      } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) {
+        RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to get parameters: %s", ex.what());
       }
     },
     qos_profile, nullptr);
@@ -67,7 +68,7 @@ ParameterService::ParameterService(
             return static_cast<rclcpp::ParameterType>(type);
           });
       } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) {
-        RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Failed to get parameter types: %s", ex.what());
+        RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to get parameter types: %s", ex.what());
       }
     },
     qos_profile, nullptr);
@@ -88,7 +89,7 @@ ParameterService::ParameterService(
           result = node_params->set_parameters_atomically(
             {rclcpp::Parameter::from_parameter_msg(p)});
         } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) {
-          RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Failed to set parameter: %s", ex.what());
+          RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to set parameter: %s", ex.what());
           result.successful = false;
           result.reason = ex.what();
         }
@@ -116,7 +117,7 @@ ParameterService::ParameterService(
         auto result = node_params->set_parameters_atomically(pvariants);
         response->result = result;
       } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) {
-        RCLCPP_WARN(
+        RCLCPP_DEBUG(
           rclcpp::get_logger("rclcpp"), "Failed to set parameters atomically: %s", ex.what());
         response->result.successful = false;
         response->result.reason = "One or more parameters were not declared before setting";
@@ -136,7 +137,7 @@ ParameterService::ParameterService(
         auto descriptors = node_params->describe_parameters(request->names);
         response->descriptors = descriptors;
       } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) {
-        RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Failed to describe parameters: %s", ex.what());
+        RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to describe parameters: %s", ex.what());
       }
     },
     qos_profile, nullptr);

rclcpp/src/rclcpp/parameter_value.cpp

@@ -67,7 +67,7 @@ array_to_string(
   type_array << "[";
   type_array.setf(format_flags, std::ios_base::basefield | std::ios::boolalpha);
   type_array << std::showbase;
-  for (const ValType value : array) {
+  for (const ValType & value : array) {
     if (!first_item) {
       type_array << ", ";
     } else {

rclcpp/src/rclcpp/qos.cpp

@@ -16,7 +16,9 @@
 
 #include <string>
 
+#include "rmw/error_handling.h"
 #include "rmw/types.h"
+#include "rmw/qos_profiles.h"
 
 namespace rclcpp
 {
@@ -99,6 +101,13 @@ QoS::history(rmw_qos_history_policy_t history)
   return *this;
 }
 
+QoS &
+QoS::history(HistoryPolicy history)
+{
+  rmw_qos_profile_.history = static_cast<rmw_qos_history_policy_t>(history);
+  return *this;
+}
+
 QoS &
 QoS::keep_last(size_t depth)
 {
@@ -122,6 +131,13 @@ QoS::reliability(rmw_qos_reliability_policy_t reliability)
   return *this;
 }
 
+QoS &
+QoS::reliability(ReliabilityPolicy reliability)
+{
+  rmw_qos_profile_.reliability = static_cast<rmw_qos_reliability_policy_t>(reliability);
+  return *this;
+}
+
 QoS &
 QoS::reliable()
 {
@@ -141,6 +157,13 @@ QoS::durability(rmw_qos_durability_policy_t durability)
   return *this;
 }
 
+QoS &
+QoS::durability(DurabilityPolicy durability)
+{
+  rmw_qos_profile_.durability = static_cast<rmw_qos_durability_policy_t>(durability);
+  return *this;
+}
+
 QoS &
 QoS::durability_volatile()
 {
@@ -186,6 +209,14 @@ QoS::liveliness(rmw_qos_liveliness_policy_t liveliness)
   return *this;
 }
 
+QoS &
+QoS::liveliness(LivelinessPolicy liveliness)
+{
+  rmw_qos_profile_.liveliness = static_cast<rmw_qos_liveliness_policy_t>(liveliness);
+  return *this;
+}
+
+
 QoS &
 QoS::liveliness_lease_duration(rmw_time_t liveliness_lease_duration)
 {
@@ -206,6 +237,51 @@ QoS::avoid_ros_namespace_conventions(bool avoid_ros_namespace_conventions)
   return *this;
 }
 
+HistoryPolicy
+QoS::history() const
+{
+  return static_cast<HistoryPolicy>(rmw_qos_profile_.history);
+}
+
+size_t
+QoS::depth() const {return rmw_qos_profile_.depth;}
+
+ReliabilityPolicy
+QoS::reliability() const
+{
+  return static_cast<ReliabilityPolicy>(rmw_qos_profile_.reliability);
+}
+
+DurabilityPolicy
+QoS::durability() const
+{
+  return static_cast<DurabilityPolicy>(rmw_qos_profile_.durability);
+}
+
+Duration
+QoS::deadline() const {return Duration::from_rmw_time(rmw_qos_profile_.deadline);}
+
+Duration
+QoS::lifespan() const {return Duration::from_rmw_time(rmw_qos_profile_.lifespan);}
+
+LivelinessPolicy
+QoS::liveliness() const
+{
+  return static_cast<LivelinessPolicy>(rmw_qos_profile_.liveliness);
+}
+
+Duration
+QoS::liveliness_lease_duration() const
+{
+  return Duration::from_rmw_time(rmw_qos_profile_.liveliness_lease_duration);
+}
+
+bool
+QoS::avoid_ros_namespace_conventions() const
+{
+  return rmw_qos_profile_.avoid_ros_namespace_conventions;
+}
+
 namespace
 {
 /// Check if two rmw_time_t have the same values.
@@ -235,6 +311,45 @@ bool operator!=(const QoS & left, const QoS & right)
   return !(left == right);
 }
 
+QoSCheckCompatibleResult
+qos_check_compatible(const QoS & publisher_qos, const QoS & subscription_qos)
+{
+  rmw_qos_compatibility_type_t compatible;
+  const size_t reason_size = 2048u;
+  char reason_c_str[reason_size] = "";
+  rmw_ret_t ret = rmw_qos_profile_check_compatible(
+    publisher_qos.get_rmw_qos_profile(),
+    subscription_qos.get_rmw_qos_profile(),
+    &compatible,
+    reason_c_str,
+    reason_size);
+  if (RMW_RET_OK != ret) {
+    std::string error_str(rmw_get_error_string().str);
+    rmw_reset_error();
+    throw rclcpp::exceptions::QoSCheckCompatibleException{error_str};
+  }
+
+  QoSCheckCompatibleResult result;
+  result.reason = std::string(reason_c_str);
+
+  switch (compatible) {
+    case RMW_QOS_COMPATIBILITY_OK:
+      result.compatibility = QoSCompatibility::Ok;
+      break;
+    case RMW_QOS_COMPATIBILITY_WARNING:
+      result.compatibility = QoSCompatibility::Warning;
+      break;
+    case RMW_QOS_COMPATIBILITY_ERROR:
+      result.compatibility = QoSCompatibility::Error;
+      break;
+    default:
+      throw rclcpp::exceptions::QoSCheckCompatibleException{
+              "Unexpected compatibility value returned by rmw '" + std::to_string(compatible) +
+              "'"};
+  }
+  return result;
+}
+
 ClockQoS::ClockQoS(const QoSInitialization & qos_initialization)
 // Using `rmw_qos_profile_sensor_data` intentionally.
 // It's best effort and `qos_initialization` is overriding the depth to 1.

rclcpp/src/rclcpp/signal_handler.cpp

@@ -243,7 +243,7 @@ SignalHandler::deferred_signal_handler()
             get_logger(),
             "deferred_signal_handler(): "
             "shutting down rclcpp::Context @ %p, because it had shutdown_on_sigint == true",
-            context_ptr.get());
+            static_cast<void *>(context_ptr.get()));
           context_ptr->shutdown("signal handler");
         }
       }

rclcpp/src/rclcpp/time_source.cpp

@@ -233,11 +233,22 @@ void TimeSource::create_clock_sub()
     return;
   }
 
+  rclcpp::SubscriptionOptions options;
+  options.qos_overriding_options = rclcpp::QosOverridingOptions(
+    {
+      rclcpp::QosPolicyKind::Depth,
+      rclcpp::QosPolicyKind::Durability,
+      rclcpp::QosPolicyKind::History,
+      rclcpp::QosPolicyKind::Reliability,
+    });
+
   clock_subscription_ = rclcpp::create_subscription<rosgraph_msgs::msg::Clock>(
+    node_parameters_,
     node_topics_,
     "/clock",
     rclcpp::QoS(KeepLast(1)).best_effort(),
-    std::bind(&TimeSource::clock_cb, this, std::placeholders::_1)
+    std::bind(&TimeSource::clock_cb, this, std::placeholders::_1),
+    options
   );
 }
 

rclcpp/test/CMakeLists.txt

@@ -4,6 +4,8 @@ find_package(test_msgs REQUIRED)
 
 include(cmake/rclcpp_add_build_failure_test.cmake)
 
+set(TEST_RESOURCES_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/resources")
+
 add_subdirectory(benchmark)
 add_subdirectory(rclcpp)
 
@@ -11,8 +13,3 @@ 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()
-
-# Install test resources
-install(
-  DIRECTORY resources
-  DESTINATION ${CMAKE_CURRENT_BINARY_DIR})

rclcpp/test/benchmark/CMakeLists.txt

@@ -1,3 +1,4 @@
+find_package(ament_cmake_google_benchmark REQUIRED)
 find_package(performance_test_fixture REQUIRED)
 
 # These benchmarks are only being created and run for the default RMW
@@ -31,6 +32,11 @@ if(TARGET benchmark_node_parameters_interface)
   target_link_libraries(benchmark_node_parameters_interface ${PROJECT_NAME})
 endif()
 
+ament_add_google_benchmark(benchmark_parameter_client benchmark_parameter_client.cpp)
+if(TARGET benchmark_parameter_client)
+  target_link_libraries(benchmark_parameter_client ${PROJECT_NAME})
+endif()
+
 add_performance_test(benchmark_service benchmark_service.cpp)
 if(TARGET benchmark_service)
   target_link_libraries(benchmark_service ${PROJECT_NAME})

rclcpp/test/benchmark/benchmark_node.cpp

@@ -46,7 +46,9 @@ BENCHMARK_F(NodePerformanceTest, create_node)(benchmark::State & state)
   for (auto _ : state) {
     // Using pointer to separate construction and destruction in timing
     auto node = std::make_shared<rclcpp::Node>("node");
+#ifndef __clang_analyzer__
     benchmark::DoNotOptimize(node);
+#endif
     benchmark::ClobberMemory();
 
     // Ensure destruction of node is not counted toward timing
@@ -69,7 +71,9 @@ BENCHMARK_F(NodePerformanceTest, destroy_node)(benchmark::State & state)
     auto node = std::make_shared<rclcpp::Node>("node");
     state.ResumeTiming();
 
+#ifndef __clang_analyzer__
     benchmark::DoNotOptimize(node);
+#endif
     benchmark::ClobberMemory();
 
     node.reset();

rclcpp/test/benchmark/benchmark_node_parameters_interface.cpp

@@ -30,6 +30,7 @@ public:
     param2_name(param_prefix + ".my_param_2"),
     param3_name(param_prefix + ".my_param_3")
   {
+    dynamically_typed_descriptor.dynamic_typing = true;
   }
 
   void SetUp(benchmark::State & state)
@@ -37,9 +38,12 @@ public:
     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->declare_parameter(
+      param1_name, rclcpp::ParameterValue{}, dynamically_typed_descriptor);
+    node->declare_parameter(
+      param2_name, rclcpp::ParameterValue{}, dynamically_typed_descriptor);
+    node->declare_parameter(
+      param3_name, rclcpp::ParameterValue{}, dynamically_typed_descriptor);
     node->undeclare_parameter(param3_name);
 
     performance_test_fixture::PerformanceTest::SetUp(state);
@@ -58,6 +62,7 @@ public:
   const std::string param1_name;
   const std::string param2_name;
   const std::string param3_name;
+  rcl_interfaces::msg::ParameterDescriptor dynamically_typed_descriptor;
 
 protected:
   rclcpp::Node::SharedPtr node;
@@ -66,7 +71,7 @@ protected:
 BENCHMARK_F(NodeParametersInterfaceTest, declare_undeclare)(benchmark::State & state)
 {
   for (auto _ : state) {
-    node->declare_parameter(param3_name);
+    node->declare_parameter(param3_name, rclcpp::ParameterValue{}, dynamically_typed_descriptor);
     node->undeclare_parameter(param3_name);
   }
 }

rclcpp/test/benchmark/benchmark_parameter_client.cpp

@@ -0,0 +1,327 @@
+// 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 <vector>
+#include <string>
+
+#include "benchmark/benchmark.h"
+
+#include "rclcpp/rclcpp.hpp"
+
+class RemoteNodeTest : public benchmark::Fixture
+{
+public:
+  RemoteNodeTest()
+  : remote_node_name("my_remote_node")
+  {
+  }
+
+#ifdef __GNUC__
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Woverloaded-virtual"
+#endif
+  void SetUp(benchmark::State &)
+  {
+    remote_context = std::make_shared<rclcpp::Context>();
+    remote_context->init(0, nullptr, rclcpp::InitOptions().auto_initialize_logging(false));
+
+    rclcpp::ExecutorOptions exec_options;
+    exec_options.context = remote_context;
+
+    remote_executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(exec_options);
+
+    remote_node = std::make_shared<rclcpp::Node>(
+      remote_node_name, rclcpp::NodeOptions().context(remote_context));
+    remote_executor->add_node(remote_node);
+
+    remote_thread = std::thread(&rclcpp::executors::SingleThreadedExecutor::spin, remote_executor);
+  }
+
+  void TearDown(benchmark::State &)
+  {
+    remote_executor->cancel();
+    remote_context->shutdown("Test is complete");
+    remote_thread.join();
+
+    remote_node.reset();
+    remote_executor.reset();
+    remote_context.reset();
+  }
+#ifdef __GNUC__
+#pragma GCC diagnostic pop
+#endif
+
+  const std::string remote_node_name;
+
+protected:
+  rclcpp::Context::SharedPtr remote_context;
+  rclcpp::executors::SingleThreadedExecutor::SharedPtr remote_executor;
+  rclcpp::Node::SharedPtr remote_node;
+  std::thread remote_thread;
+};
+
+class ParameterClientTest : public RemoteNodeTest
+{
+public:
+  ParameterClientTest()
+  : 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)
+  {
+    RemoteNodeTest::SetUp(state);
+
+    rclcpp::init(0, nullptr);
+    node = std::make_shared<rclcpp::Node>(node_name);
+
+    rcl_interfaces::msg::ParameterDescriptor descriptor;
+    descriptor.dynamic_typing = true;
+
+    remote_node->declare_parameter(
+      param1_name, rclcpp::ParameterValue("param1_value"), descriptor);
+    remote_node->declare_parameter(
+      param2_name, rclcpp::ParameterValue(std::vector<int> {1, 2, 3}), descriptor);
+    remote_node->declare_parameter(param3_name, rclcpp::ParameterValue{}, descriptor);
+    remote_node->undeclare_parameter(param3_name);
+
+    params_client = std::make_shared<rclcpp::SyncParametersClient>(node, remote_node_name);
+
+    if (!params_client->wait_for_service()) {
+      state.SkipWithError("Client failed to become ready");
+    }
+  }
+
+  void TearDown(benchmark::State & state)
+  {
+    RemoteNodeTest::TearDown(state);
+
+    rclcpp::shutdown();
+    node.reset();
+    params_client.reset();
+  }
+
+  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;
+  rclcpp::SyncParametersClient::SharedPtr params_client;
+};
+
+static bool result_is_successful(rcl_interfaces::msg::SetParametersResult result)
+{
+  return result.successful;
+}
+
+BENCHMARK_F(ParameterClientTest, create_destroy_client)(benchmark::State & state)
+{
+  for (auto _ : state) {
+    params_client.reset();
+    params_client = std::make_shared<rclcpp::SyncParametersClient>(node, remote_node_name);
+    if (!params_client->wait_for_service()) {
+      state.SkipWithError("Client failed to become ready");
+      break;
+    }
+  }
+}
+
+BENCHMARK_F(ParameterClientTest, has_parameter_hit)(benchmark::State & state)
+{
+  for (auto _ : state) {
+    if (!params_client->has_parameter(param1_name)) {
+      state.SkipWithError("Parameter was expected");
+      break;
+    }
+  }
+}
+
+BENCHMARK_F(ParameterClientTest, has_parameter_miss)(benchmark::State & state)
+{
+  for (auto _ : state) {
+    if (params_client->has_parameter(param3_name)) {
+      state.SkipWithError("Parameter was not expected");
+      break;
+    }
+  }
+}
+
+BENCHMARK_F(ParameterClientTest, 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),
+  };
+
+  for (auto _ : state) {
+    std::vector<rcl_interfaces::msg::SetParametersResult> results =
+      params_client->set_parameters(param_values2);
+    if (!std::all_of(results.begin(), results.end(), result_is_successful)) {
+      state.SkipWithError("Failed to set one or more parameters");
+      break;
+    }
+
+    results = params_client->set_parameters(param_values1);
+    if (!std::all_of(results.begin(), results.end(), result_is_successful)) {
+      state.SkipWithError("Failed to set one or more parameters");
+      break;
+    }
+  }
+}
+
+BENCHMARK_F(ParameterClientTest, 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),
+  };
+
+  for (auto _ : state) {
+    rcl_interfaces::msg::SetParametersResult result =
+      params_client->set_parameters_atomically(param_values2);
+    if (!result.successful) {
+      state.SkipWithError(("Failed to set parameters: " + result.reason).c_str());
+      break;
+    }
+
+    result = params_client->set_parameters_atomically(param_values1);
+    if (!result.successful) {
+      state.SkipWithError(("Failed to set parameters: " + result.reason).c_str());
+      break;
+    }
+  }
+}
+
+BENCHMARK_F(ParameterClientTest, 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"),
+  };
+
+  for (auto _ : state) {
+    std::vector<rcl_interfaces::msg::SetParametersResult> results =
+      params_client->set_parameters(param_values2);
+    if (!std::all_of(results.begin(), results.end(), result_is_successful)) {
+      state.SkipWithError("Failed to set one or more parameters");
+      break;
+    }
+
+    results = params_client->set_parameters(param_values1);
+    if (!std::all_of(results.begin(), results.end(), result_is_successful)) {
+      state.SkipWithError("Failed to set one or more parameters");
+      break;
+    }
+  }
+}
+
+BENCHMARK_F(ParameterClientTest, 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}),
+  };
+
+  for (auto _ : state) {
+    std::vector<rcl_interfaces::msg::SetParametersResult> results =
+      params_client->set_parameters(param_values2);
+    if (!std::all_of(results.begin(), results.end(), result_is_successful)) {
+      state.SkipWithError("Failed to set one or more parameters");
+      break;
+    }
+
+    results = params_client->set_parameters(param_values1);
+    if (!std::all_of(results.begin(), results.end(), result_is_successful)) {
+      state.SkipWithError("Failed to set one or more parameters");
+      break;
+    }
+  }
+}
+
+BENCHMARK_F(ParameterClientTest, get_parameters)(benchmark::State & state)
+{
+  for (auto _ : state) {
+    std::vector<rclcpp::Parameter> results = params_client->get_parameters({param1_name});
+    if (results.size() != 1 || results[0].get_name() != param1_name) {
+      state.SkipWithError("Got the wrong parameter(s)");
+      break;
+    }
+  }
+}
+
+BENCHMARK_F(ParameterClientTest, list_parameters_hit)(benchmark::State & state)
+{
+  const std::vector<std::string> prefixes
+  {
+    param_prefix,
+  };
+
+  for (auto _ : state) {
+    rcl_interfaces::msg::ListParametersResult param_list =
+      params_client->list_parameters(prefixes, 10);
+    if (param_list.names.size() != 2) {
+      state.SkipWithError("Expected parameters");
+      break;
+    }
+  }
+}
+
+BENCHMARK_F(ParameterClientTest, list_parameters_miss)(benchmark::State & state)
+{
+  const std::vector<std::string> prefixes
+  {
+    "your_prefix",
+  };
+
+  for (auto _ : state) {
+    rcl_interfaces::msg::ListParametersResult param_list =
+      params_client->list_parameters(prefixes, 10);
+    if (param_list.names.size() != 0) {
+      state.SkipWithError("Expected no parameters");
+      break;
+    }
+  }
+}

rclcpp/test/rclcpp/CMakeLists.txt

@@ -2,7 +2,7 @@ find_package(ament_cmake_gtest REQUIRED)
 
 find_package(rmw_implementation_cmake REQUIRED)
 
-add_definitions(-DTEST_RESOURCES_DIRECTORY="${CMAKE_CURRENT_BINARY_DIR}/../resources")
+add_definitions(-DTEST_RESOURCES_DIRECTORY="${TEST_RESOURCES_DIRECTORY}")
 
 rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs
   ../msg/Header.msg
@@ -323,6 +323,16 @@ if(TARGET test_parameter)
   )
   target_link_libraries(test_parameter ${PROJECT_NAME})
 endif()
+ament_add_gtest(test_parameter_event_handler test_parameter_event_handler.cpp)
+if(TARGET test_parameter_event_handler)
+  ament_target_dependencies(test_parameter_event_handler
+    "rcl_interfaces"
+    "rmw"
+    "rosidl_generator_cpp"
+    "rosidl_typesupport_cpp"
+  )
+  target_link_libraries(test_parameter_event_handler ${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})

rclcpp/test/rclcpp/allocator/test_allocator_common.cpp

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

rclcpp/test/rclcpp/allocator/test_allocator_deleter.cpp

@@ -38,7 +38,9 @@ TEST(TestAllocatorDeleter, construct_destruct) {
 TEST(TestAllocatorDeleter, delete) {
   std::allocator<int> allocator;
   int * some_mem = allocator.allocate(1u);
-  ASSERT_NE(nullptr, some_mem);
+  // The more natural check here is ASSERT_NE(nullptr, ptr), but clang static
+  // analysis throws a false-positive memory leak warning.  Use ASSERT_TRUE instead.
+  ASSERT_TRUE(nullptr != some_mem);
 
   rclcpp::allocator::AllocatorDeleter<std::allocator<int>> deleter(&allocator);
   EXPECT_NO_THROW(deleter(some_mem));

rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp

@@ -267,7 +267,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) {
   auto client = node->create_client<test_msgs::srv::Empty>("service");
   auto waitable = std::make_shared<TestWaitable>();
 
-  // Adding a subscription with rmw_connext_cpp adds another waitable, so we need to get the
+  // Adding a subscription could add another waitable, so we need to get the
   // current number of waitables just before adding the new waitable.
   expected_number_of_entities->waitables = get_number_of_default_entities(node)->waitables;
   node->get_node_waitables_interface()->add_waitable(waitable, nullptr);

rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp

@@ -14,9 +14,11 @@
 
 #include <gtest/gtest.h>
 
+#include <functional>
 #include <memory>
 #include <string>
 #include <utility>
+#include <vector>
 
 #include "rcl/graph.h"
 #include "rcl/node_options.h"
@@ -66,6 +68,43 @@ protected:
 
   const rclcpp::node_interfaces::NodeGraph * node_graph() const {return node_graph_;}
 
+  size_t get_num_graph_things(std::function<size_t()> predicate)
+  {
+    constexpr std::chrono::milliseconds timeout(100);
+
+    size_t tries = 0;
+    size_t num_things = 0;
+    while (tries++ < 5) {
+      num_things = predicate();
+      if (num_things >= 1) {
+        break;
+      }
+
+      auto event = node()->get_graph_event();
+      EXPECT_NO_THROW(node()->wait_for_graph_change(event, timeout));
+    }
+
+    return num_things;
+  }
+
+  size_t get_num_topics()
+  {
+    return get_num_graph_things(
+      [this]() -> size_t {
+        auto topic_names_and_types = node_graph()->get_topic_names_and_types();
+        return topic_names_and_types.size();
+      });
+  }
+
+  size_t get_num_services()
+  {
+    return get_num_graph_things(
+      [this]() -> size_t {
+        auto service_names_and_types = node_graph()->get_service_names_and_types();
+        return service_names_and_types.size();
+      });
+  }
+
 private:
   std::shared_ptr<rclcpp::Node> node_;
   rclcpp::node_interfaces::NodeGraph * node_graph_;
@@ -73,11 +112,9 @@ private:
 
 TEST_F(TestNodeGraph, construct_from_node)
 {
-  auto topic_names_and_types = node_graph()->get_topic_names_and_types(false);
-  EXPECT_LT(0u, topic_names_and_types.size());
+  EXPECT_LT(0u, get_num_topics());
 
-  auto service_names_and_types = node_graph()->get_service_names_and_types();
-  EXPECT_LT(0u, service_names_and_types.size());
+  EXPECT_LT(0u, get_num_services());
 
   auto names = node_graph()->get_node_names();
   EXPECT_EQ(1u, names.size());
@@ -91,13 +128,12 @@ TEST_F(TestNodeGraph, construct_from_node)
 
   // get_graph_event is non-const
   EXPECT_NE(nullptr, node()->get_node_graph_interface()->get_graph_event());
-  EXPECT_EQ(1u, node_graph()->count_graph_users());
+  EXPECT_LE(1u, node_graph()->count_graph_users());
 }
 
 TEST_F(TestNodeGraph, get_topic_names_and_types)
 {
-  auto topic_names_and_types = node_graph()->get_topic_names_and_types();
-  EXPECT_LT(0u, topic_names_and_types.size());
+  ASSERT_LT(0u, get_num_topics());
 }
 
 TEST_F(TestNodeGraph, get_topic_names_and_types_rcl_error)
@@ -126,8 +162,7 @@ TEST_F(TestNodeGraph, get_topic_names_and_types_rcl_names_and_types_fini_error)
 
 TEST_F(TestNodeGraph, get_service_names_and_types)
 {
-  auto service_names_and_types = node_graph()->get_service_names_and_types();
-  EXPECT_LT(0u, service_names_and_types.size());
+  ASSERT_LT(0u, get_num_services());
 }
 
 TEST_F(TestNodeGraph, get_service_names_and_types_rcl_error)
@@ -309,8 +344,13 @@ TEST_F(TestNodeGraph, get_info_by_topic)
 
   EXPECT_EQ(0u, node_graph()->get_publishers_info_by_topic("topic", true).size());
 
-  auto publishers = node_graph()->get_publishers_info_by_topic("topic", false);
-  ASSERT_EQ(1u, publishers.size());
+  std::vector<rclcpp::TopicEndpointInfo> publishers;
+  size_t num_publishers = get_num_graph_things(
+    [this, &publishers]() {
+      publishers = node_graph()->get_publishers_info_by_topic("topic", false);
+      return publishers.size();
+    });
+  ASSERT_EQ(1u, num_publishers);
 
   auto publisher_endpoint_info = publishers[0];
   const auto const_publisher_endpoint_info = publisher_endpoint_info;

rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp

@@ -125,7 +125,7 @@ TEST_F(TestNodeParameters, parameter_overrides)
   auto * node_parameters_interface =
     dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
     node2->get_node_parameters_interface().get());
-  ASSERT_NE(nullptr, node_parameters);
+  ASSERT_NE(nullptr, node_parameters_interface);
 
   const auto & parameter_overrides = node_parameters_interface->get_parameter_overrides();
   EXPECT_EQ(2u, parameter_overrides.size());

rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp

@@ -511,10 +511,8 @@ TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_subscription) {
   RclWaitSetSizes expected_sizes = {};
   expected_sizes.size_of_subscriptions = 1;
   const std::string implementation_identifier = rmw_get_implementation_identifier();
-  if (implementation_identifier == "rmw_connext_cpp" ||
-    implementation_identifier == "rmw_cyclonedds_cpp")
-  {
-    // For connext, a subscription will also add an event and waitable
+  if (implementation_identifier == "rmw_cyclonedds_cpp") {
+    // For cyclonedds, a subscription will also add an event and waitable
     expected_sizes.size_of_events += 1;
     expected_sizes.size_of_waitables += 1;
   }

rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp

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

rclcpp/test/rclcpp/test_any_subscription_callback.cpp

@@ -26,7 +26,8 @@ class TestAnySubscriptionCallback : public ::testing::Test
 {
 public:
   TestAnySubscriptionCallback()
-  : any_subscription_callback_(allocator_) {}
+  : allocator_(std::make_shared<std::allocator<void>>()),
+    any_subscription_callback_(allocator_) {}
   void SetUp()
   {
     msg_shared_ptr_ = std::make_shared<test_msgs::msg::Empty>();
@@ -45,6 +46,20 @@ protected:
   rclcpp::MessageInfo message_info_;
 };
 
+void construct_with_null_allocator()
+{
+  // We need to wrap this in a function because `EXPECT_THROW` is a macro, and thinks
+  // that the comma in here splits macro arguments, not the template arguments.
+  rclcpp::AnySubscriptionCallback<
+    test_msgs::msg::Empty, std::allocator<void>> any_subscription_callback_(nullptr);
+}
+
+TEST(AnySubscription, null_allocator) {
+  EXPECT_THROW(
+    construct_with_null_allocator(),
+    std::runtime_error);
+}
+
 TEST_F(TestAnySubscriptionCallback, construct_destruct) {
 }
 

rclcpp/test/rclcpp/test_duration.cpp

@@ -138,6 +138,7 @@ TEST_F(TestDuration, maximum_duration) {
 static const int64_t HALF_SEC_IN_NS = 500 * 1000 * 1000;
 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;
+static const int64_t MAX_NANOSECONDS = std::numeric_limits<int64_t>::max();
 
 TEST_F(TestDuration, from_seconds) {
   EXPECT_EQ(rclcpp::Duration(0ns), rclcpp::Duration::from_seconds(0.0));
@@ -148,6 +149,34 @@ TEST_F(TestDuration, from_seconds) {
     rclcpp::Duration::from_seconds(-1.5));
 }
 
+TEST_F(TestDuration, from_rmw_time) {
+  constexpr auto max_rcl_duration = std::numeric_limits<rcl_duration_value_t>::max();
+  {
+    rmw_time_t rmw_duration{};
+    rmw_duration.sec = RCL_NS_TO_S(max_rcl_duration) + 1uLL;
+    EXPECT_EQ(rclcpp::Duration::from_rmw_time(rmw_duration).nanoseconds(), max_rcl_duration);
+  }
+  {
+    rmw_time_t rmw_duration{};
+    rmw_duration.nsec = max_rcl_duration + 1uLL;
+    EXPECT_EQ(rclcpp::Duration::from_rmw_time(rmw_duration).nanoseconds(), max_rcl_duration);
+  }
+  {
+    rmw_time_t rmw_duration{};
+    rmw_duration.nsec = max_rcl_duration;
+    rmw_duration.sec = RCL_NS_TO_S(max_rcl_duration);
+    EXPECT_EQ(rclcpp::Duration::from_rmw_time(rmw_duration).nanoseconds(), max_rcl_duration);
+  }
+  {
+    rmw_time_t rmw_duration{};
+    rmw_duration.sec = 1u;
+    rmw_duration.nsec = 1000u;
+    EXPECT_EQ(
+      rclcpp::Duration::from_rmw_time(rmw_duration).nanoseconds(),
+      static_cast<rcl_duration_value_t>(RCL_S_TO_NS(rmw_duration.sec) + rmw_duration.nsec));
+  }
+}
+
 TEST_F(TestDuration, std_chrono_constructors) {
   EXPECT_EQ(rclcpp::Duration(0ns), rclcpp::Duration(0.0s));
   EXPECT_EQ(rclcpp::Duration(0ns), rclcpp::Duration(0s));
@@ -239,6 +268,34 @@ TEST_F(TestDuration, conversions) {
     auto chrono_duration = duration.to_chrono<std::chrono::nanoseconds>();
     EXPECT_EQ(chrono_duration.count(), -ONE_AND_HALF_SEC_IN_NS);
   }
+
+  {
+    auto duration = rclcpp::Duration::from_nanoseconds(MAX_NANOSECONDS);
+
+    const auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(duration);
+    EXPECT_EQ(duration_msg.sec, std::numeric_limits<int32_t>::max());
+    EXPECT_EQ(duration_msg.nanosec, std::numeric_limits<uint32_t>::max());
+
+    auto rmw_time = duration.to_rmw_time();
+    EXPECT_EQ(rmw_time.sec, 9223372036u);
+    EXPECT_EQ(rmw_time.nsec, 854775807u);
+
+    auto chrono_duration = duration.to_chrono<std::chrono::nanoseconds>();
+    EXPECT_EQ(chrono_duration.count(), MAX_NANOSECONDS);
+  }
+
+  {
+    auto duration = rclcpp::Duration::from_nanoseconds(-MAX_NANOSECONDS);
+
+    const auto duration_msg = static_cast<builtin_interfaces::msg::Duration>(duration);
+    EXPECT_EQ(duration_msg.sec, std::numeric_limits<int32_t>::min());
+    EXPECT_EQ(duration_msg.nanosec, 0u);
+
+    EXPECT_THROW(duration.to_rmw_time(), std::runtime_error);
+
+    auto chrono_duration = duration.to_chrono<std::chrono::nanoseconds>();
+    EXPECT_EQ(chrono_duration.count(), -MAX_NANOSECONDS);
+  }
 }
 
 TEST_F(TestDuration, test_some_constructors) {

rclcpp/test/rclcpp/test_executor.cpp

@@ -21,6 +21,7 @@
 
 #include "rclcpp/executor.hpp"
 #include "rclcpp/memory_strategy.hpp"
+#include "rclcpp/executors/single_threaded_executor.hpp"
 #include "rclcpp/strategies/allocator_memory_strategy.hpp"
 
 #include "../mocking_utils/patch.hpp"
@@ -54,6 +55,7 @@ public:
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr local_get_node_by_group(
     rclcpp::CallbackGroup::SharedPtr group)
   {
+    std::lock_guard<std::mutex> guard_{mutex_};  // only to make the TSA happy
     return get_node_by_group(weak_groups_to_nodes_, group);
   }
 
@@ -87,18 +89,47 @@ MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=)
 MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <)
 MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >)
 
+TEST_F(TestExecutor, add_remove_node_thread_safe) {
+  using namespace std::chrono_literals;
+
+  // Create an Executor
+  rclcpp::executors::SingleThreadedExecutor executor;
+
+  auto future = std::async(std::launch::async, [&executor] {executor.spin();});
+
+  // Add and remove nodes repeatedly
+  // Test that this does not cause a segfault
+  size_t num_nodes = 100;
+  for (size_t i = 0; i < num_nodes; ++i) {
+    std::ostringstream name;
+    name << "node_" << i;
+    auto node = std::make_shared<rclcpp::Node>(name.str());
+    executor.add_node(node);
+    // Sleeping here helps exaggerate the issue
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    executor.remove_node(node);
+  }
+  std::future_status future_status = std::future_status::timeout;
+  do {
+    executor.cancel();
+    future_status = future.wait_for(1s);
+  } while (future_status == std::future_status::timeout);
+  EXPECT_EQ(future_status, std::future_status::ready);
+  future.get();
+}
+
 TEST_F(TestExecutor, constructor_bad_guard_condition_init) {
   auto mock = mocking_utils::patch_and_return(
     "lib:rclcpp", rcl_guard_condition_init, RCL_RET_ERROR);
   EXPECT_THROW(
-    new DummyExecutor(),
+    static_cast<void>(std::make_unique<DummyExecutor>()),
     rclcpp::exceptions::RCLError);
 }
 
 TEST_F(TestExecutor, constructor_bad_wait_set_init) {
   auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_init, RCL_RET_ERROR);
   RCLCPP_EXPECT_THROW_EQ(
-    new DummyExecutor(),
+    static_cast<void>(std::make_unique<DummyExecutor>()),
     std::runtime_error("Failed to create wait set in Executor constructor: error not set"));
 }
 

rclcpp/test/rclcpp/test_loaned_message.cpp

@@ -67,7 +67,7 @@ TEST_F(TestLoanedMessage, release) {
   auto node = std::make_shared<rclcpp::Node>("loaned_message_test_node");
   auto pub = node->create_publisher<MessageT>("loaned_message_test_topic", 1);
 
-  MessageT * msg = nullptr;
+  std::unique_ptr<MessageT, std::function<void(MessageT *)>> msg;
   {
     auto loaned_msg = pub->borrow_loaned_message();
     ASSERT_TRUE(loaned_msg.is_valid());
@@ -81,6 +81,15 @@ TEST_F(TestLoanedMessage, release) {
 
   ASSERT_EQ(42.0f, msg->float64_value);
 
+  // Generally, the memory released from `LoanedMessage::release()` will be freed
+  // in deleter of unique_ptr or is managed in the middleware after calling
+  // `Publisher::do_loaned_message_publish` inside Publisher::publish().
+  if (pub->can_loan_messages()) {
+    ASSERT_EQ(
+      RCL_RET_OK,
+      rcl_return_loaned_message_from_publisher(pub->get_publisher_handle().get(), msg.get()));
+  }
+
   SUCCEED();
 }
 

rclcpp/test/rclcpp/test_logger.cpp

@@ -17,6 +17,8 @@
 #include <memory>
 #include <string>
 
+#include "rcutils/env.h"
+
 #include "rclcpp/logger.hpp"
 #include "rclcpp/logging.hpp"
 #include "rclcpp/node.hpp"
@@ -80,78 +82,99 @@ TEST(TestLogger, set_level) {
   rcutils_logging_set_output_handler(rcutils_logging_console_output_handler);
 
   // default
-  RCLCPP_DEBUG(logger, std::string("message %s"), "debug");
+  RCLCPP_DEBUG(logger, "message %s", "debug");
   EXPECT_FALSE(g_last_log_event.console_output_handler_called);
-  RCLCPP_INFO(logger, std::string("message %s"), "info");
+  RCLCPP_INFO(logger, "message %s", "info");
   EXPECT_TRUE(g_last_log_event.console_output_handler_called);
   EXPECT_EQ("message info", g_last_log_event.message);
 
   // unset
   g_last_log_event.console_output_handler_called = false;
   logger.set_level(rclcpp::Logger::Level::Unset);
-  RCLCPP_DEBUG(logger, std::string("message %s"), "debug");
+  RCLCPP_DEBUG(logger, "message");
+  RCLCPP_DEBUG(logger, "message %s", "debug");
   EXPECT_FALSE(g_last_log_event.console_output_handler_called);
-  RCLCPP_INFO(logger, std::string("message %s"), "info");
+  RCLCPP_INFO(logger, "message");
+  RCLCPP_INFO(logger, "message %s", "info");
   EXPECT_TRUE(g_last_log_event.console_output_handler_called);
   EXPECT_EQ("message info", g_last_log_event.message);
 
   // debug
   g_last_log_event.console_output_handler_called = false;
   logger.set_level(rclcpp::Logger::Level::Debug);
-  RCLCPP_DEBUG(logger, std::string("message %s"), "debug");
+  RCLCPP_DEBUG(logger, "message %s", "debug");
   EXPECT_TRUE(g_last_log_event.console_output_handler_called);
   EXPECT_EQ("message debug", g_last_log_event.message);
-  RCLCPP_INFO(logger, std::string("message %s"), "info");
+  RCLCPP_INFO(logger, "message %s", "info");
   EXPECT_EQ("message info", g_last_log_event.message);
 
   // info
   g_last_log_event.console_output_handler_called = false;
   logger.set_level(rclcpp::Logger::Level::Info);
-  RCLCPP_DEBUG(logger, std::string("message %s"), "debug");
+  RCLCPP_DEBUG(logger, "message %s", "debug");
   EXPECT_FALSE(g_last_log_event.console_output_handler_called);
-  RCLCPP_INFO(logger, std::string("message %s"), "info");
+  RCLCPP_INFO(logger, "message %s", "info");
   EXPECT_TRUE(g_last_log_event.console_output_handler_called);
   EXPECT_EQ("message info", g_last_log_event.message);
 
   // warn
   g_last_log_event.console_output_handler_called = false;
   logger.set_level(rclcpp::Logger::Level::Warn);
-  RCLCPP_DEBUG(logger, std::string("message %s"), "debug");
+  RCLCPP_DEBUG(logger, "message %s", "debug");
   EXPECT_FALSE(g_last_log_event.console_output_handler_called);
-  RCLCPP_INFO(logger, std::string("message %s"), "info");
+  RCLCPP_INFO(logger, "message %s", "info");
   EXPECT_FALSE(g_last_log_event.console_output_handler_called);
-  RCLCPP_WARN(logger, std::string("message %s"), "warn");
+  RCLCPP_WARN(logger, "message %s", "warn");
   EXPECT_TRUE(g_last_log_event.console_output_handler_called);
   EXPECT_EQ("message warn", g_last_log_event.message);
 
   // error
   g_last_log_event.console_output_handler_called = false;
   logger.set_level(rclcpp::Logger::Level::Error);
-  RCLCPP_DEBUG(logger, std::string("message %s"), "debug");
+  RCLCPP_DEBUG(logger, "message %s", "debug");
   EXPECT_FALSE(g_last_log_event.console_output_handler_called);
-  RCLCPP_INFO(logger, std::string("message %s"), "info");
+  RCLCPP_INFO(logger, "message %s", "info");
   EXPECT_FALSE(g_last_log_event.console_output_handler_called);
-  RCLCPP_WARN(logger, std::string("message %s"), "warn");
+  RCLCPP_WARN(logger, "message %s", "warn");
   EXPECT_FALSE(g_last_log_event.console_output_handler_called);
-  RCLCPP_ERROR(logger, std::string("message %s"), "error");
+  RCLCPP_ERROR(logger, "message %s", "error");
   EXPECT_TRUE(g_last_log_event.console_output_handler_called);
   EXPECT_EQ("message error", g_last_log_event.message);
 
   // fatal
   g_last_log_event.console_output_handler_called = false;
   logger.set_level(rclcpp::Logger::Level::Fatal);
-  RCLCPP_DEBUG(logger, std::string("message %s"), "debug");
+  RCLCPP_DEBUG(logger, "message %s", "debug");
   EXPECT_FALSE(g_last_log_event.console_output_handler_called);
-  RCLCPP_INFO(logger, std::string("message %s"), "info");
+  RCLCPP_INFO(logger, "message %s", "info");
   EXPECT_FALSE(g_last_log_event.console_output_handler_called);
-  RCLCPP_WARN(logger, std::string("message %s"), "warn");
+  RCLCPP_WARN(logger, "message %s", "warn");
   EXPECT_FALSE(g_last_log_event.console_output_handler_called);
-  RCLCPP_ERROR(logger, std::string("message %s"), "error");
+  RCLCPP_ERROR(logger, "message %s", "error");
   EXPECT_FALSE(g_last_log_event.console_output_handler_called);
-  RCLCPP_FATAL(logger, std::string("message %s"), "fatal");
+  RCLCPP_FATAL(logger, "message %s", "fatal");
   EXPECT_TRUE(g_last_log_event.console_output_handler_called);
   EXPECT_EQ("message fatal", g_last_log_event.message);
 
   rcutils_logging_set_output_handler(previous_output_handler);
   EXPECT_EQ(RCUTILS_RET_OK, rcutils_logging_shutdown());
 }
+
+TEST(TestLogger, get_logging_directory) {
+  ASSERT_EQ(true, rcutils_set_env("HOME", "/fake_home_dir"));
+  ASSERT_EQ(true, rcutils_set_env("USERPROFILE", nullptr));
+  ASSERT_EQ(true, rcutils_set_env("ROS_LOG_DIR", nullptr));
+  ASSERT_EQ(true, rcutils_set_env("ROS_HOME", nullptr));
+
+  auto path = rclcpp::get_logging_directory();
+  auto expected_path = rcpputils::fs::path{"/fake_home_dir"} / ".ros" / "log";
+
+  // TODO(ivanpauno): Add operator== to rcpputils::fs::path
+  auto it = path.cbegin();
+  auto eit = expected_path.cbegin();
+  for (; it != path.cend() && eit != expected_path.cend(); ++it, ++eit) {
+    EXPECT_EQ(*eit, *it);
+  }
+  EXPECT_EQ(it, path.cend());
+  EXPECT_EQ(eit, expected_path.cend());
+}

rclcpp/test/rclcpp/test_logging.cpp

@@ -110,26 +110,6 @@ TEST_F(TestLoggingMacros, test_logging_named) {
   EXPECT_EQ("message 3", g_last_log_event.message);
 }
 
-TEST_F(TestLoggingMacros, test_logging_string) {
-  for (std::string i : {"one", "two", "three"}) {
-    RCLCPP_DEBUG(g_logger, "message " + i);
-  }
-  EXPECT_EQ(3u, g_log_calls);
-  EXPECT_EQ("message three", g_last_log_event.message);
-
-  RCLCPP_DEBUG(g_logger, "message " "four");
-  EXPECT_EQ("message four", g_last_log_event.message);
-
-  RCLCPP_DEBUG(g_logger, std::string("message " "five"));
-  EXPECT_EQ("message five", g_last_log_event.message);
-
-  RCLCPP_DEBUG(g_logger, std::string("message %s"), "six");
-  EXPECT_EQ("message six", g_last_log_event.message);
-
-  RCLCPP_DEBUG(g_logger, "message seven");
-  EXPECT_EQ("message seven", g_last_log_event.message);
-}
-
 TEST_F(TestLoggingMacros, test_logging_stream) {
   for (std::string i : {"one", "two", "three"}) {
     RCLCPP_DEBUG_STREAM(g_logger, "message " << i);

rclcpp/test/rclcpp/test_node.cpp

@@ -43,6 +43,11 @@ protected:
     rclcpp::init(0, nullptr);
   }
 
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
   void SetUp() override
   {
     test_resources_path /= "test_node";
@@ -325,8 +330,11 @@ TEST_F(TestNode, declare_parameter_with_no_initial_values) {
   // test cases without initial values
   auto node = std::make_shared<rclcpp::Node>("test_declare_parameter_node"_unq);
   {
+    rcl_interfaces::msg::ParameterDescriptor descriptor;
+    descriptor.dynamic_typing = true;
     // no default, no initial
-    rclcpp::ParameterValue value = node->declare_parameter("parameter"_unq);
+    rclcpp::ParameterValue value = node->declare_parameter(
+      "parameter"_unq, rclcpp::ParameterValue{}, descriptor);
     EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_NOT_SET);
   }
   {
@@ -358,15 +366,21 @@ TEST_F(TestNode, declare_parameter_with_no_initial_values) {
   {
     // parameter already declared throws
     auto name = "parameter"_unq;
-    node->declare_parameter(name);
+    rcl_interfaces::msg::ParameterDescriptor descriptor;
+    descriptor.dynamic_typing = true;
+    node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor);
     EXPECT_THROW(
-      {node->declare_parameter(name);},
+    {
+      rcl_interfaces::msg::ParameterDescriptor descriptor;
+      descriptor.dynamic_typing = true;
+      node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor);
+    },
       rclcpp::exceptions::ParameterAlreadyDeclaredException);
   }
   {
     // parameter name invalid throws
     EXPECT_THROW(
-      {node->declare_parameter("");},
+      {node->declare_parameter("", 5);},
       rclcpp::exceptions::InvalidParametersException);
   }
   {
@@ -395,6 +409,46 @@ TEST_F(TestNode, declare_parameter_with_no_initial_values) {
   }
 }
 
+TEST_F(TestNode, declare_parameter_with_allow_undeclared_parameters) {
+  // test cases without initial values
+  auto node = std::make_shared<rclcpp::Node>(
+    "test_declare_parameter_node"_unq, "/",
+    rclcpp::NodeOptions{}.allow_undeclared_parameters(true));
+  {
+    // declared parameters static typing is still enforced
+    auto param_name = "parameter"_unq;
+    auto value = node->declare_parameter(param_name, 5);
+    EXPECT_EQ(value, 5);
+    EXPECT_FALSE(node->set_parameter({param_name, "asd"}).successful);
+    auto param = node->get_parameter(param_name);
+    EXPECT_EQ(param.get_type(), rclcpp::PARAMETER_INTEGER);
+    EXPECT_EQ(param.get_value<int64_t>(), 5);
+  }
+  {
+    // not for automatically declared parameters
+    auto param_name = "parameter"_unq;
+    EXPECT_TRUE(node->set_parameter({param_name, 5}).successful);
+    auto param = node->get_parameter(param_name);
+    EXPECT_EQ(param.get_type(), rclcpp::PARAMETER_INTEGER);
+    EXPECT_EQ(param.get_value<int64_t>(), 5);
+    EXPECT_TRUE(node->set_parameter({param_name, "asd"}).successful);
+    param = node->get_parameter(param_name);
+    EXPECT_EQ(param.get_type(), rclcpp::PARAMETER_STRING);
+    EXPECT_EQ(param.get_value<std::string>(), "asd");
+  }
+  {
+    // declare after set is invalid
+    auto param_name = "parameter"_unq;
+    EXPECT_TRUE(node->set_parameter({param_name, 5}).successful);
+    auto param = node->get_parameter(param_name);
+    EXPECT_EQ(param.get_type(), rclcpp::PARAMETER_INTEGER);
+    EXPECT_EQ(param.get_value<int64_t>(), 5);
+    EXPECT_THROW(
+      node->declare_parameter(param_name, 5),
+      rclcpp::exceptions::ParameterAlreadyDeclaredException);
+  }
+}
+
 auto get_fixed_on_parameter_set_callback(const std::string & name, bool successful)
 {
   return
@@ -492,13 +546,15 @@ TEST_F(TestNode, declare_parameter_with_overrides) {
   auto node = std::make_shared<rclcpp::Node>("test_declare_parameter_node"_unq, no);
   {
     // no default, with override
-    rclcpp::ParameterValue value = node->declare_parameter("parameter_no_default");
+    rclcpp::ParameterValue value = node->declare_parameter(
+      "parameter_no_default", rclcpp::ParameterType::PARAMETER_INTEGER);
     EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER);
     EXPECT_EQ(value.get<int>(), 42);
   }
   {
     // no default, with override, and set after
-    rclcpp::ParameterValue value = node->declare_parameter("parameter_no_default_set");
+    rclcpp::ParameterValue value = node->declare_parameter(
+      "parameter_no_default_set", rclcpp::ParameterType::PARAMETER_INTEGER);
     EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER);
     EXPECT_EQ(value.get<int>(), 42);
     // check that the value is changed after a set
@@ -507,8 +563,8 @@ TEST_F(TestNode, declare_parameter_with_overrides) {
   }
   {
     // no default, with override
-    const rclcpp::ParameterValue & value =
-      node->declare_parameter("parameter_no_default_set_cvref");
+    const rclcpp::ParameterValue & value = node->declare_parameter(
+      "parameter_no_default_set_cvref", rclcpp::ParameterType::PARAMETER_INTEGER);
     EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER);
     EXPECT_EQ(value.get<int>(), 42);
     // check that the value is changed after a set
@@ -555,15 +611,21 @@ TEST_F(TestNode, declare_parameter_with_overrides) {
   {
     // parameter already declared throws
     auto name = "parameter_already_declared";
-    node->declare_parameter(name);
+    rcl_interfaces::msg::ParameterDescriptor descriptor;
+    descriptor.dynamic_typing = true;
+    node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor);
     EXPECT_THROW(
-      {node->declare_parameter(name);},
+    {
+      rcl_interfaces::msg::ParameterDescriptor descriptor;
+      descriptor.dynamic_typing = true;
+      node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor);
+    },
       rclcpp::exceptions::ParameterAlreadyDeclaredException);
   }
   {
     // parameter name invalid throws
     EXPECT_THROW(
-      {node->declare_parameter("");},
+      {node->declare_parameter("", 5);},
       rclcpp::exceptions::InvalidParametersException);
   }
   {
@@ -598,6 +660,22 @@ TEST_F(TestNode, declare_parameter_with_overrides) {
       {node->declare_parameter("parameter_type_mismatch", 42);},
       rclcpp::exceptions::InvalidParameterTypeException);
   }
+  {
+    // default type and expected type do not match
+    EXPECT_THROW(
+      {node->declare_parameter(
+          "parameter_type_mismatch", rclcpp::ParameterType::PARAMETER_INTEGER);},
+      rclcpp::exceptions::InvalidParameterTypeException);
+  }
+  {
+    // cannot pass an expected type and a descriptor with dynamic_typing=True
+    rcl_interfaces::msg::ParameterDescriptor descriptor{};
+    descriptor.dynamic_typing = true;
+    EXPECT_THROW(
+      {node->declare_parameter(
+          "invalid_argument", rclcpp::ParameterType::PARAMETER_INTEGER, descriptor);},
+      std::invalid_argument);
+  }
 }
 
 TEST_F(TestNode, declare_parameters_with_no_initial_values) {
@@ -668,7 +746,7 @@ TEST_F(TestNode, declare_parameters_with_no_initial_values) {
   {
     // parameter already declared throws, even with not_set type
     auto name = "parameter"_unq;
-    node->declare_parameter(name);
+    node->declare_parameter(name, 42);
     EXPECT_THROW(
       {node->declare_parameters<int64_t>("", {{name, 42}});},
       rclcpp::exceptions::ParameterAlreadyDeclaredException);
@@ -727,45 +805,53 @@ TEST_F(TestNode, declare_parameter_with_cli_overrides) {
   // To match parameters YAML file content, use a well-known node name for this test only.
   auto node = std::make_shared<rclcpp::Node>("test_declare_parameter_node", no);
   {
-    rclcpp::ParameterValue value = node->declare_parameter("parameter_bool");
+    rclcpp::ParameterValue value = node->declare_parameter(
+      "parameter_bool", rclcpp::ParameterType::PARAMETER_BOOL);
     EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_BOOL);
     EXPECT_EQ(value.get<bool>(), true);
   }
   {
-    rclcpp::ParameterValue value = node->declare_parameter("parameter_int");
+    rclcpp::ParameterValue value = node->declare_parameter(
+      "parameter_int", rclcpp::ParameterType::PARAMETER_INTEGER);
     EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER);
     EXPECT_EQ(value.get<int64_t>(), 21);  // set to 42 in CLI, overriden by file
   }
   {
-    rclcpp::ParameterValue value = node->declare_parameter("parameter_double");
+    rclcpp::ParameterValue value = node->declare_parameter(
+      "parameter_double", rclcpp::ParameterType::PARAMETER_DOUBLE);
     EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE);
     EXPECT_EQ(value.get<double>(), 0.42);
   }
   {
-    rclcpp::ParameterValue value = node->declare_parameter("parameter_string");
+    rclcpp::ParameterValue value = node->declare_parameter(
+      "parameter_string", rclcpp::ParameterType::PARAMETER_STRING);
     EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_STRING);
     EXPECT_EQ(value.get<std::string>(), "foo");
   }
   {
-    rclcpp::ParameterValue value = node->declare_parameter("parameter_bool_array");
+    rclcpp::ParameterValue value = node->declare_parameter(
+      "parameter_bool_array", rclcpp::ParameterType::PARAMETER_BOOL_ARRAY);
     EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_BOOL_ARRAY);
     std::vector<bool> expected_value{false, true};
     EXPECT_EQ(value.get<std::vector<bool>>(), expected_value);
   }
   {
-    rclcpp::ParameterValue value = node->declare_parameter("parameter_int_array");
+    rclcpp::ParameterValue value = node->declare_parameter(
+      "parameter_int_array", rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY);
     EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER_ARRAY);
     std::vector<int64_t> expected_value{-21, 42};
     EXPECT_EQ(value.get<std::vector<int64_t>>(), expected_value);
   }
   {
-    rclcpp::ParameterValue value = node->declare_parameter("parameter_double_array");
+    rclcpp::ParameterValue value = node->declare_parameter(
+      "parameter_double_array", rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY);
     EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE_ARRAY);
     std::vector<double> expected_value{-1.0, 0.42};
     EXPECT_EQ(value.get<std::vector<double>>(), expected_value);
   }
   {
-    rclcpp::ParameterValue value = node->declare_parameter("parameter_string_array");
+    rclcpp::ParameterValue value = node->declare_parameter(
+      "parameter_string_array", rclcpp::ParameterType::PARAMETER_STRING_ARRAY);
     EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_STRING_ARRAY);
     std::vector<std::string> expected_value{"foo", "bar"};
     // set to [baz, baz, baz] in file, overriden by CLI
@@ -778,7 +864,9 @@ TEST_F(TestNode, undeclare_parameter) {
   {
     // normal use
     auto name = "parameter"_unq;
-    node->declare_parameter(name);
+    rcl_interfaces::msg::ParameterDescriptor descriptor;
+    descriptor.dynamic_typing = true;
+    node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor);
     EXPECT_TRUE(node->has_parameter(name));
     node->undeclare_parameter(name);
     EXPECT_FALSE(node->has_parameter(name));
@@ -791,6 +879,16 @@ TEST_F(TestNode, undeclare_parameter) {
       {node->undeclare_parameter(name);},
       rclcpp::exceptions::ParameterNotDeclaredException);
   }
+  {
+    // statically typed parameter throws
+    auto name = "parameter"_unq;
+    node->declare_parameter(name, 42);
+    EXPECT_TRUE(node->has_parameter(name));
+    EXPECT_THROW(
+      {node->undeclare_parameter(name);},
+      rclcpp::exceptions::InvalidParameterTypeException);
+    EXPECT_TRUE(node->has_parameter(name));
+  }
   {
     // read only parameter throws
     auto name = "parameter"_unq;
@@ -810,7 +908,9 @@ TEST_F(TestNode, has_parameter) {
   // normal use
   auto name = "parameter"_unq;
   EXPECT_FALSE(node->has_parameter(name));
-  node->declare_parameter(name);
+  rcl_interfaces::msg::ParameterDescriptor descriptor;
+  descriptor.dynamic_typing = true;
+  node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor);
   EXPECT_TRUE(node->has_parameter(name));
   node->undeclare_parameter(name);
   EXPECT_FALSE(node->has_parameter(name));
@@ -821,7 +921,9 @@ TEST_F(TestNode, list_parameters) {
   // normal use
   auto name = "parameter"_unq;
   const size_t before_size = node->list_parameters({}, 1u).names.size();
-  node->declare_parameter(name);
+  rcl_interfaces::msg::ParameterDescriptor descriptor;
+  descriptor.dynamic_typing = true;
+  node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor);
   EXPECT_EQ(1u + before_size, node->list_parameters({}, 1u).names.size());
   node->undeclare_parameter(name);
   EXPECT_EQ(before_size, node->list_parameters({}, 1u).names.size());
@@ -842,12 +944,23 @@ TEST_F(TestNode, set_parameter_undeclared_parameters_not_allowed) {
     EXPECT_EQ(node->get_parameter(name).get_value<int>(), 43);
   }
   {
-    // normal use, change type
+    // normal use, change type not allowed
     auto name = "parameter"_unq;
     EXPECT_FALSE(node->has_parameter(name));
     node->declare_parameter(name, 42);
     EXPECT_TRUE(node->has_parameter(name));
     EXPECT_EQ(node->get_parameter(name).get_value<int>(), 42);
+    EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, "not an integer")).successful);
+  }
+  {
+    // normal use, change type
+    auto name = "parameter"_unq;
+    EXPECT_FALSE(node->has_parameter(name));
+    rcl_interfaces::msg::ParameterDescriptor descriptor;
+    descriptor.dynamic_typing = true;
+    node->declare_parameter(name, 42, descriptor);
+    EXPECT_TRUE(node->has_parameter(name));
+    EXPECT_EQ(node->get_parameter(name).get_value<int>(), 42);
     EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, "not an integer")).successful);
     EXPECT_EQ(node->get_parameter(name).get_value<std::string>(), std::string("not an integer"));
   }
@@ -864,8 +977,6 @@ TEST_F(TestNode, set_parameter_undeclared_parameters_not_allowed) {
     EXPECT_TRUE(node->has_parameter(name3));
 
     EXPECT_EQ(node->get_parameter(name1).get_value<int>(), 42);
-    EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name1, "not an integer")).successful);
-    EXPECT_EQ(node->get_parameter(name1).get_value<std::string>(), std::string("not an integer"));
 
     EXPECT_EQ(node->get_parameter(name2).get_value<bool>(), true);
     EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name2, false)).successful);
@@ -900,7 +1011,9 @@ TEST_F(TestNode, set_parameter_undeclared_parameters_not_allowed) {
   {
     // setting type of rclcpp::PARAMETER_NOT_SET, when already not set, does not undeclare
     auto name = "parameter"_unq;
-    auto value = node->declare_parameter(name);
+    rcl_interfaces::msg::ParameterDescriptor descriptor;
+    descriptor.dynamic_typing = true;
+    auto value = node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor);
     EXPECT_TRUE(node->has_parameter(name));
     EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_NOT_SET);
 
@@ -910,13 +1023,26 @@ TEST_F(TestNode, set_parameter_undeclared_parameters_not_allowed) {
     EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_NOT_SET);
   }
   {
-    // setting type of rclcpp::PARAMETER_NOT_SET, when already to another type, will undeclare
+    // setting type of rclcpp::PARAMETER_NOT_SET, when already to another type, will fail
     auto name = "parameter"_unq;
     node->declare_parameter(name, 42);
     EXPECT_TRUE(node->has_parameter(name));
     auto value = node->get_parameter(name);
     EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER);
 
+    EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name)).successful);
+  }
+  {
+    // setting type of rclcpp::PARAMETER_NOT_SET,
+    // when dynamic typing is allowing and already set to another type, will undeclare
+    auto name = "parameter"_unq;
+    rcl_interfaces::msg::ParameterDescriptor descriptor;
+    descriptor.dynamic_typing = true;
+    node->declare_parameter(name, 42, descriptor);
+    EXPECT_TRUE(node->has_parameter(name));
+    auto value = node->get_parameter(name);
+    EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER);
+
     EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name)).successful);
 
     EXPECT_FALSE(node->has_parameter(name));
@@ -1256,15 +1382,9 @@ TEST_F(TestNode, set_parameter_undeclared_parameters_not_allowed) {
     auto name = "parameter"_unq;
     rcl_interfaces::msg::ParameterDescriptor descriptor;
     descriptor.type = rclcpp::PARAMETER_INTEGER;
-    node->declare_parameter(name, 42, descriptor);
+    node->declare_parameter(name, "asd", descriptor);
     EXPECT_TRUE(node->has_parameter(name));
     auto value = node->get_parameter(name);
-    EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER);
-    EXPECT_EQ(value.get_value<int64_t>(), 42);
-
-    EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, "asd")).successful);
-    EXPECT_TRUE(node->has_parameter(name));
-    value = node->get_parameter(name);
     EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_STRING);
     EXPECT_EQ(value.get_value<std::string>(), "asd");
   }
@@ -1412,7 +1532,9 @@ TEST_F(TestNode, set_parameters_undeclared_parameters_not_allowed) {
   {
     // setting type of rclcpp::PARAMETER_NOT_SET, when already not set, does not undeclare
     auto name = "parameter"_unq;
-    auto value = node->declare_parameter(name);
+    rcl_interfaces::msg::ParameterDescriptor descriptor;
+    descriptor.dynamic_typing = true;
+    auto value = node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor);
     EXPECT_TRUE(node->has_parameter(name));
     EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_NOT_SET);
 
@@ -1422,13 +1544,26 @@ TEST_F(TestNode, set_parameters_undeclared_parameters_not_allowed) {
     EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_NOT_SET);
   }
   {
-    // setting type of rclcpp::PARAMETER_NOT_SET, when already to another type, will undeclare
+    // setting type of rclcpp::PARAMETER_NOT_SET, when already to another type, will fail
     auto name = "parameter"_unq;
     node->declare_parameter(name, 42);
     EXPECT_TRUE(node->has_parameter(name));
     auto value = node->get_parameter(name);
     EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER);
 
+    EXPECT_FALSE(node->set_parameters({rclcpp::Parameter(name)})[0].successful);
+  }
+  {
+    // setting type of rclcpp::PARAMETER_NOT_SET,
+    // when already to another type and dynamic typic allowed, will undeclare
+    auto name = "parameter"_unq;
+    rcl_interfaces::msg::ParameterDescriptor descriptor;
+    descriptor.dynamic_typing = true;
+    node->declare_parameter(name, 42, descriptor);
+    EXPECT_TRUE(node->has_parameter(name));
+    auto value = node->get_parameter(name);
+    EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER);
+
     EXPECT_TRUE(node->set_parameters({rclcpp::Parameter(name)})[0].successful);
 
     EXPECT_FALSE(node->has_parameter(name));
@@ -1586,7 +1721,9 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_not_allowed) {
   {
     // setting type of rclcpp::PARAMETER_NOT_SET, when already not set, does not undeclare
     auto name = "parameter"_unq;
-    auto value = node->declare_parameter(name);
+    rcl_interfaces::msg::ParameterDescriptor descriptor;
+    descriptor.dynamic_typing = true;
+    auto value = node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor);
     EXPECT_TRUE(node->has_parameter(name));
     EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_NOT_SET);
 
@@ -1596,13 +1733,26 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_not_allowed) {
     EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_NOT_SET);
   }
   {
-    // setting type of rclcpp::PARAMETER_NOT_SET, when already to another type, will undeclare
+    // setting type of rclcpp::PARAMETER_NOT_SET, when already to another type, will fail
     auto name = "parameter"_unq;
     node->declare_parameter(name, 42);
     EXPECT_TRUE(node->has_parameter(name));
     auto value = node->get_parameter(name);
     EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER);
 
+    EXPECT_FALSE(node->set_parameters_atomically({rclcpp::Parameter(name)}).successful);
+  }
+  {
+    // setting type of rclcpp::PARAMETER_NOT_SET,
+    // when dynamic typing is allowed and already declared to another type, will undeclare
+    auto name = "parameter"_unq;
+    rcl_interfaces::msg::ParameterDescriptor descriptor;
+    descriptor.dynamic_typing = true;
+    node->declare_parameter(name, 42, descriptor);
+    EXPECT_TRUE(node->has_parameter(name));
+    auto value = node->get_parameter(name);
+    EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER);
+
     EXPECT_TRUE(node->set_parameters_atomically({rclcpp::Parameter(name)}).successful);
 
     EXPECT_FALSE(node->has_parameter(name));
@@ -1958,30 +2108,28 @@ TEST_F(TestNode, get_parameters_undeclared_parameters_not_allowed) {
   {
     // templated version with empty prefix will get all parameters
     auto node_local = std::make_shared<rclcpp::Node>("test_get_parameters_node"_unq);
-    auto name1 = "prefix1.parameter"_unq;
-    auto name2 = "prefix2.parameter"_unq;
+    auto name1 = "parameter"_unq;
+    auto name2 = "parameter"_unq;
 
-    node_local->declare_parameter(name1, 42);
-    node_local->declare_parameter(name2, 100);
-    // undeclare so that it doesn't interfere with the test
-    node_local->undeclare_parameter("use_sim_time");
+    node_local->declare_parameter("prefix." + name1, 42);
+    node_local->declare_parameter("prefix." + name2, 100);
 
     {
       std::map<std::string, int64_t> actual;
-      EXPECT_TRUE(node_local->get_parameters("", actual));
+      EXPECT_TRUE(node_local->get_parameters("prefix", actual));
       EXPECT_NE(actual.find(name1), actual.end());
       EXPECT_NE(actual.find(name2), actual.end());
     }
 
     // will throw if set of parameters is non-homogeneous
-    auto name3 = "prefix2.parameter"_unq;
-    node_local->declare_parameter<std::string>(name3, "not an int");
+    auto name3 = "prefix1.parameter"_unq;
+    node_local->declare_parameter<std::string>("prefix." + name3, "not an int");
 
     {
       std::map<std::string, int64_t> actual;
       EXPECT_THROW(
       {
-        node_local->get_parameters("", actual);
+        node_local->get_parameters("prefix", actual);
       },
         rclcpp::exceptions::InvalidParameterTypeException);
     }
@@ -2410,7 +2558,7 @@ TEST_F(TestNode, get_publishers_subscriptions_info_by_topic) {
   auto publisher = node->create_publisher<test_msgs::msg::BasicTypes>(topic_name, qos);
   // List should have one item
   auto publisher_list = node->get_publishers_info_by_topic(fq_topic_name);
-  EXPECT_EQ(publisher_list.size(), (size_t)1);
+  ASSERT_EQ(publisher_list.size(), (size_t)1);
   // Subscription list should be empty
   EXPECT_TRUE(node->get_subscriptions_info_by_topic(fq_topic_name).empty());
   // Verify publisher list has the right data.
@@ -2572,3 +2720,76 @@ TEST_F(TestNode, create_sub_node_rmw_validate_namespace_error) {
       rclcpp::exceptions::RCLError);
   }
 }
+
+TEST_F(TestNode, static_and_dynamic_typing) {
+  rclcpp::NodeOptions no;
+  no.parameter_overrides(
+  {
+    {"integer_parameter_override_ok", 43},
+    {"string_parameter_override_should_throw", 43},
+    {"integer_must_provide_override", 43},
+    {"cool_way_of_declaring_a_string_without_a_default", "hello!"}
+  });
+  auto node = std::make_shared<rclcpp::Node>("node", "ns", no);
+  {
+    auto param = node->declare_parameter("an_int", 42);
+    EXPECT_EQ(42, param);
+    auto result = node->set_parameter({"an_int", "string value"});
+    EXPECT_FALSE(result.successful);
+    result = node->set_parameter({"an_int", 43});
+    EXPECT_TRUE(result.successful);
+    EXPECT_TRUE(node->get_parameter("an_int", param));
+    EXPECT_EQ(43, param);
+  }
+  {
+    auto param = node->declare_parameter("integer_parameter_override_ok", 42);
+    EXPECT_EQ(43, param);
+  }
+  {
+    EXPECT_THROW(
+      node->declare_parameter("string_parameter_override_should_throw", "a string"),
+      rclcpp::exceptions::InvalidParameterTypeException);
+  }
+  {
+    auto param = node->declare_parameter(
+      "integer_must_provide_override", rclcpp::PARAMETER_INTEGER);
+    EXPECT_EQ(43, param.get<int64_t>());
+  }
+  {
+    auto param = node->declare_parameter<std::string>(
+      "cool_way_of_declaring_a_string_without_a_default");
+    EXPECT_EQ("hello!", param);
+  }
+  {
+    EXPECT_THROW(
+      node->declare_parameter("integer_override_not_given", rclcpp::PARAMETER_INTEGER),
+      rclcpp::exceptions::NoParameterOverrideProvided);
+  }
+  {
+    EXPECT_THROW(
+      node->declare_parameter("parameter_not_set_is_not_a_valid_type", rclcpp::PARAMETER_NOT_SET),
+      std::invalid_argument);
+  }
+  {
+    EXPECT_THROW(
+      node->declare_parameter(
+        "uninitialized_not_valid_except_dynamic_typing", rclcpp::ParameterValue{}),
+      rclcpp::exceptions::InvalidParameterTypeException);
+  }
+  {
+#ifndef _WIN32
+# pragma GCC diagnostic push
+# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#else
+# pragma warning(push)
+# pragma warning(disable: 4996)
+#endif
+    auto param = node->declare_parameter("deprecated_way_dynamic_typing");
+    EXPECT_EQ(param, rclcpp::ParameterValue{});
+#ifndef _WIN32
+# pragma GCC diagnostic pop
+#else
+# pragma warning(pop)
+#endif
+  }
+}

rclcpp/test/rclcpp/test_parameter_client.cpp

@@ -28,6 +28,8 @@
 
 #include "rcl_interfaces/msg/parameter_event.hpp"
 
+using namespace std::chrono_literals;
+
 class TestParameterClient : public ::testing::Test
 {
 public:
@@ -45,14 +47,20 @@ protected:
   void SetUp()
   {
     node = std::make_shared<rclcpp::Node>("test_parameter_client", "/ns");
+    node_with_option =
+      std::make_shared<rclcpp::Node>(
+      "test_parameter_client_allow_undeclare", "/ns",
+      rclcpp::NodeOptions().allow_undeclared_parameters(true));
   }
 
   void TearDown()
   {
     node.reset();
+    node_with_option.reset();
   }
 
   rclcpp::Node::SharedPtr node;
+  rclcpp::Node::SharedPtr node_with_option;
 };
 
 /*
@@ -210,12 +218,59 @@ TEST_F(TestParameterClient, async_parameter_get_parameter_types) {
   ASSERT_TRUE(callback_called);
 }
 
+/*
+  Coverage for async get_parameter_types with allow_undeclared_ enabled
+ */
+TEST_F(TestParameterClient, async_parameter_get_parameter_types_allow_undeclared) {
+  auto asynchronous_client =
+    std::make_shared<rclcpp::AsyncParametersClient>(node_with_option);
+  bool callback_called = false;
+  auto callback = [&callback_called](std::shared_future<std::vector<rclcpp::ParameterType>> result)
+    {
+      if (result.valid() && result.get().size() == 1 &&
+        result.get()[0] == rclcpp::PARAMETER_NOT_SET)
+      {
+        callback_called = true;
+      }
+    };
+  std::vector<std::string> names{"foo"};
+  std::shared_future<std::vector<rclcpp::ParameterType>> future =
+    asynchronous_client->get_parameter_types(names, callback);
+  auto return_code = rclcpp::spin_until_future_complete(
+    node_with_option, future, std::chrono::milliseconds(100));
+  ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code);
+  ASSERT_TRUE(callback_called);
+}
+
 /*
   Coverage for async get_parameters
  */
 TEST_F(TestParameterClient, async_parameter_get_parameters) {
   auto asynchronous_client = std::make_shared<rclcpp::AsyncParametersClient>(node);
   bool callback_called = false;
+  auto callback = [&callback_called](std::shared_future<std::vector<rclcpp::Parameter>> result)
+    {
+      // We expect the result to be empty since we tried to get a parameter that didn't exist.
+      if (result.valid() && result.get().size() == 0) {
+        callback_called = true;
+      }
+    };
+  std::vector<std::string> names{"foo"};
+  std::shared_future<std::vector<rclcpp::Parameter>> future = asynchronous_client->get_parameters(
+    names, callback);
+  auto return_code = rclcpp::spin_until_future_complete(
+    node, future, std::chrono::milliseconds(100));
+  ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code);
+  ASSERT_TRUE(callback_called);
+}
+
+/*
+  Coverage for async get_parameters with allow_undeclared_ enabled
+ */
+TEST_F(TestParameterClient, async_parameter_get_parameters_allow_undeclared) {
+  auto asynchronous_client =
+    std::make_shared<rclcpp::AsyncParametersClient>(node_with_option);
+  bool callback_called = false;
   auto callback = [&callback_called](std::shared_future<std::vector<rclcpp::Parameter>> result)
     {
       if (result.valid() && result.get().size() == 1 && result.get()[0].get_name() == "foo") {
@@ -226,7 +281,7 @@ TEST_F(TestParameterClient, async_parameter_get_parameters) {
   std::shared_future<std::vector<rclcpp::Parameter>> future = asynchronous_client->get_parameters(
     names, callback);
   auto return_code = rclcpp::spin_until_future_complete(
-    node, future, std::chrono::milliseconds(100));
+    node_with_option, future, std::chrono::milliseconds(100));
   ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code);
   ASSERT_TRUE(callback_called);
 }
@@ -284,10 +339,525 @@ TEST_F(TestParameterClient, async_parameter_list_parameters) {
  */
 TEST_F(TestParameterClient, sync_parameter_get_parameter_types) {
   node->declare_parameter("foo", 4);
+  node->declare_parameter("bar", "this is bar");
   auto synchronous_client = std::make_shared<rclcpp::SyncParametersClient>(node);
-  std::vector<std::string> names{"foo"};
-  std::vector<rclcpp::ParameterType> parameter_types =
-    synchronous_client->get_parameter_types(names);
-  ASSERT_EQ(1u, parameter_types.size());
-  ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_types[0]);
+
+  {
+    std::vector<std::string> names{"none"};
+    std::vector<rclcpp::ParameterType> parameter_types =
+      synchronous_client->get_parameter_types(names, 10s);
+    ASSERT_EQ(0u, parameter_types.size());
+  }
+
+  {
+    std::vector<std::string> names{"none", "foo", "bar"};
+    std::vector<rclcpp::ParameterType> parameter_types =
+      synchronous_client->get_parameter_types(names, 10s);
+    ASSERT_EQ(0u, parameter_types.size());
+  }
+
+  {
+    std::vector<std::string> names{"foo"};
+    std::vector<rclcpp::ParameterType> parameter_types =
+      synchronous_client->get_parameter_types(names, 10s);
+    ASSERT_EQ(1u, parameter_types.size());
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_types[0]);
+  }
+
+  {
+    std::vector<std::string> names{"bar"};
+    std::vector<rclcpp::ParameterType> parameter_types =
+      synchronous_client->get_parameter_types(names, 10s);
+    ASSERT_EQ(1u, parameter_types.size());
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_STRING, parameter_types[0]);
+  }
+
+  {
+    std::vector<std::string> names{"foo", "bar"};
+    std::vector<rclcpp::ParameterType> parameter_types =
+      synchronous_client->get_parameter_types(names, 10s);
+    ASSERT_EQ(2u, parameter_types.size());
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_types[0]);
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_STRING, parameter_types[1]);
+  }
+}
+
+/*
+  Coverage for sync get_parameter_types with allow_undeclared_ enabled
+ */
+TEST_F(TestParameterClient, sync_parameter_get_parameter_types_allow_undeclared) {
+  node_with_option->declare_parameter("foo", 4);
+  node_with_option->declare_parameter("bar", "this is bar");
+  auto synchronous_client = std::make_shared<rclcpp::SyncParametersClient>(node_with_option);
+
+  {
+    std::vector<std::string> names{"none"};
+    std::vector<rclcpp::ParameterType> parameter_types =
+      synchronous_client->get_parameter_types(names, 10s);
+    ASSERT_EQ(1u, parameter_types.size());
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_NOT_SET, parameter_types[0]);
+  }
+
+  {
+    std::vector<std::string> names{"none", "foo", "bar"};
+    std::vector<rclcpp::ParameterType> parameter_types =
+      synchronous_client->get_parameter_types(names, 10s);
+    ASSERT_EQ(3u, parameter_types.size());
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_NOT_SET, parameter_types[0]);
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_types[1]);
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_STRING, parameter_types[2]);
+  }
+
+  {
+    std::vector<std::string> names{"foo"};
+    std::vector<rclcpp::ParameterType> parameter_types =
+      synchronous_client->get_parameter_types(names, 10s);
+    ASSERT_EQ(1u, parameter_types.size());
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_types[0]);
+  }
+
+  {
+    std::vector<std::string> names{"bar"};
+    std::vector<rclcpp::ParameterType> parameter_types =
+      synchronous_client->get_parameter_types(names, 10s);
+    ASSERT_EQ(1u, parameter_types.size());
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_STRING, parameter_types[0]);
+  }
+
+  {
+    std::vector<std::string> names{"foo", "bar"};
+    std::vector<rclcpp::ParameterType> parameter_types =
+      synchronous_client->get_parameter_types(names, 10s);
+    ASSERT_EQ(2u, parameter_types.size());
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_types[0]);
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_STRING, parameter_types[1]);
+  }
+}
+
+/*
+  Coverage for sync get_parameters
+ */
+TEST_F(TestParameterClient, sync_parameter_get_parameters) {
+  node->declare_parameter("foo", 4);
+  node->declare_parameter("bar", "this is bar");
+  auto synchronous_client = std::make_shared<rclcpp::SyncParametersClient>(node);
+
+  {
+    std::vector<std::string> names{"none"};
+    std::vector<rclcpp::Parameter> parameters = synchronous_client->get_parameters(names, 10s);
+    ASSERT_EQ(0u, parameters.size());
+  }
+
+  {
+    std::vector<std::string> names{"none", "foo", "bar"};
+    std::vector<rclcpp::Parameter> parameters = synchronous_client->get_parameters(names, 10s);
+    ASSERT_EQ(0u, parameters.size());
+  }
+
+  {
+    std::vector<std::string> names{"foo"};
+    std::vector<rclcpp::Parameter> parameters = synchronous_client->get_parameters(names, 10s);
+    ASSERT_EQ(1u, parameters.size());
+    ASSERT_EQ("foo", parameters[0].get_name());
+    ASSERT_EQ(4u, parameters[0].as_int());
+  }
+
+  {
+    std::vector<std::string> names{"bar"};
+    std::vector<rclcpp::Parameter> parameters = synchronous_client->get_parameters(names, 10s);
+    ASSERT_EQ(1u, parameters.size());
+    ASSERT_EQ("bar", parameters[0].get_name());
+    ASSERT_EQ("this is bar", parameters[0].as_string());
+  }
+
+  {
+    std::vector<std::string> names{"foo", "bar"};
+    std::vector<rclcpp::Parameter> parameters = synchronous_client->get_parameters(names, 10s);
+    ASSERT_EQ(2u, parameters.size());
+    ASSERT_EQ("foo", parameters[0].get_name());
+    ASSERT_EQ(4u, parameters[0].as_int());
+    ASSERT_EQ("bar", parameters[1].get_name());
+    ASSERT_EQ("this is bar", parameters[1].as_string());
+  }
+}
+
+/*
+  Coverage for sync get_parameters with allow_undeclared_ enabled
+ */
+TEST_F(TestParameterClient, sync_parameter_get_parameters_allow_undeclared) {
+  node_with_option->declare_parameter("foo", 4);
+  node_with_option->declare_parameter("bar", "this is bar");
+  auto synchronous_client = std::make_shared<rclcpp::SyncParametersClient>(node_with_option);
+
+  {
+    std::vector<std::string> names{"none"};
+    std::vector<rclcpp::Parameter> parameters = synchronous_client->get_parameters(names, 10s);
+    ASSERT_EQ(1u, parameters.size());
+  }
+
+  {
+    std::vector<std::string> names{"none", "foo", "bar"};
+    std::vector<rclcpp::Parameter> parameters = synchronous_client->get_parameters(names, 10s);
+    ASSERT_EQ(3u, parameters.size());
+    ASSERT_EQ("foo", parameters[1].get_name());
+    ASSERT_EQ(4u, parameters[1].as_int());
+    ASSERT_EQ("bar", parameters[2].get_name());
+    ASSERT_EQ("this is bar", parameters[2].as_string());
+  }
+
+  {
+    std::vector<std::string> names{"foo"};
+    std::vector<rclcpp::Parameter> parameters = synchronous_client->get_parameters(names, 10s);
+    ASSERT_EQ(1u, parameters.size());
+    ASSERT_EQ("foo", parameters[0].get_name());
+    ASSERT_EQ(4u, parameters[0].as_int());
+  }
+
+  {
+    std::vector<std::string> names{"bar"};
+    std::vector<rclcpp::Parameter> parameters = synchronous_client->get_parameters(names, 10s);
+    ASSERT_EQ(1u, parameters.size());
+    ASSERT_EQ("bar", parameters[0].get_name());
+    ASSERT_EQ("this is bar", parameters[0].as_string());
+  }
+
+  {
+    std::vector<std::string> names{"foo", "bar"};
+    std::vector<rclcpp::Parameter> parameters = synchronous_client->get_parameters(names, 10s);
+    ASSERT_EQ(2u, parameters.size());
+    ASSERT_EQ("foo", parameters[0].get_name());
+    ASSERT_EQ(4u, parameters[0].as_int());
+    ASSERT_EQ("bar", parameters[1].get_name());
+    ASSERT_EQ("this is bar", parameters[1].as_string());
+  }
+}
+
+/*
+  Coverage for async describe_parameters
+ */
+TEST_F(TestParameterClient, async_parameter_describe_parameters) {
+  node->declare_parameter("foo", 4);
+  node->declare_parameter("bar", "this is bar");
+  auto asynchronous_client = std::make_shared<rclcpp::AsyncParametersClient>(node);
+
+  {
+    bool callback_called = false;
+    auto callback = [&callback_called](
+      std::shared_future<std::vector<rcl_interfaces::msg::ParameterDescriptor>> result)
+      {
+        // We expect the result to be empty since we tried to get a parameter that didn't exist.
+        if (result.valid() && result.get().size() == 0) {
+          callback_called = true;
+        }
+      };
+    std::vector<std::string> names{"none"};
+    std::shared_future<std::vector<rcl_interfaces::msg::ParameterDescriptor>> future =
+      asynchronous_client->describe_parameters(names, callback);
+    auto return_code = rclcpp::spin_until_future_complete(
+      node, future, std::chrono::milliseconds(100));
+    ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code);
+    ASSERT_TRUE(callback_called);
+  }
+
+  {
+    bool callback_called = false;
+    auto callback = [&callback_called](
+      std::shared_future<std::vector<rcl_interfaces::msg::ParameterDescriptor>> result)
+      {
+        if (result.valid() && result.get().size() == 1) {
+          callback_called = true;
+        }
+      };
+    std::vector<std::string> names{"foo"};
+    std::shared_future<std::vector<rcl_interfaces::msg::ParameterDescriptor>> future =
+      asynchronous_client->describe_parameters(names, callback);
+    auto return_code = rclcpp::spin_until_future_complete(
+      node, future, std::chrono::milliseconds(100));
+    ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code);
+    std::vector<rcl_interfaces::msg::ParameterDescriptor> parameter_descs = future.get();
+    ASSERT_EQ(1u, parameter_descs.size());
+    ASSERT_EQ("foo", parameter_descs[0].name);
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_descs[0].type);
+    ASSERT_EQ("", parameter_descs[0].description);
+    ASSERT_EQ("", parameter_descs[0].additional_constraints);
+    ASSERT_FALSE(parameter_descs[0].read_only);
+    ASSERT_TRUE(callback_called);
+  }
+
+  {
+    bool callback_called = false;
+    auto callback = [&callback_called](
+      std::shared_future<std::vector<rcl_interfaces::msg::ParameterDescriptor>> result)
+      {
+        // We expect the result to be empty since we tried to get a parameter that didn't exist.
+        if (result.valid() && result.get().size() == 0) {
+          callback_called = true;
+        }
+      };
+    std::vector<std::string> names{"foo", "baz"};
+    std::shared_future<std::vector<rcl_interfaces::msg::ParameterDescriptor>> future =
+      asynchronous_client->describe_parameters(names, callback);
+    auto return_code = rclcpp::spin_until_future_complete(
+      node, future, std::chrono::milliseconds(100));
+    ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code);
+    ASSERT_TRUE(callback_called);
+  }
+
+  {
+    bool callback_called = false;
+    auto callback = [&callback_called](
+      std::shared_future<std::vector<rcl_interfaces::msg::ParameterDescriptor>> result)
+      {
+        // We expect the result to be empty since we tried to get a parameter that didn't exist.
+        if (result.valid() && result.get().size() == 0) {
+          callback_called = true;
+        }
+      };
+    std::vector<std::string> names{"baz", "foo"};
+    std::shared_future<std::vector<rcl_interfaces::msg::ParameterDescriptor>> future =
+      asynchronous_client->describe_parameters(names, callback);
+    auto return_code = rclcpp::spin_until_future_complete(
+      node, future, std::chrono::milliseconds(100));
+    ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code);
+    ASSERT_TRUE(callback_called);
+  }
+
+  {
+    bool callback_called = false;
+    auto callback = [&callback_called](
+      std::shared_future<std::vector<rcl_interfaces::msg::ParameterDescriptor>> result)
+      {
+        if (result.valid() && result.get().size() == 2) {
+          callback_called = true;
+        }
+      };
+    std::vector<std::string> names{"foo", "bar"};
+    std::shared_future<std::vector<rcl_interfaces::msg::ParameterDescriptor>> future =
+      asynchronous_client->describe_parameters(names, callback);
+    auto return_code = rclcpp::spin_until_future_complete(
+      node, future, std::chrono::milliseconds(100));
+    ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code);
+    std::vector<rcl_interfaces::msg::ParameterDescriptor> parameter_descs = future.get();
+    ASSERT_EQ(2u, parameter_descs.size());
+    ASSERT_EQ("foo", parameter_descs[0].name);
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_descs[0].type);
+    ASSERT_EQ("", parameter_descs[0].description);
+    ASSERT_EQ("", parameter_descs[0].additional_constraints);
+    ASSERT_FALSE(parameter_descs[0].read_only);
+    ASSERT_EQ("bar", parameter_descs[1].name);
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_STRING, parameter_descs[1].type);
+    ASSERT_EQ("", parameter_descs[1].description);
+    ASSERT_EQ("", parameter_descs[1].additional_constraints);
+    ASSERT_FALSE(parameter_descs[1].read_only);
+    ASSERT_TRUE(callback_called);
+  }
+}
+/*
+  Coverage for sync describe_parameters
+ */
+TEST_F(TestParameterClient, sync_parameter_describe_parameters) {
+  node->declare_parameter("foo", 4);
+  node->declare_parameter("bar", "this is bar");
+  auto synchronous_client = std::make_shared<rclcpp::SyncParametersClient>(node);
+
+  {
+    std::vector<std::string> names{"none"};
+    std::vector<rcl_interfaces::msg::ParameterDescriptor> parameter_descs =
+      synchronous_client->describe_parameters(names, 10s);
+    ASSERT_EQ(0u, parameter_descs.size());
+  }
+
+  {
+    std::vector<std::string> names{"foo"};
+    std::vector<rcl_interfaces::msg::ParameterDescriptor> parameter_descs =
+      synchronous_client->describe_parameters(names, 10s);
+    ASSERT_EQ(1u, parameter_descs.size());
+    ASSERT_EQ("foo", parameter_descs[0].name);
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_descs[0].type);
+    ASSERT_EQ("", parameter_descs[0].description);
+    ASSERT_EQ("", parameter_descs[0].additional_constraints);
+    ASSERT_FALSE(parameter_descs[0].read_only);
+  }
+
+  {
+    std::vector<std::string> names{"foo", "baz"};
+    std::vector<rcl_interfaces::msg::ParameterDescriptor> parameter_descs =
+      synchronous_client->describe_parameters(names, 10s);
+    ASSERT_EQ(0u, parameter_descs.size());
+  }
+
+  {
+    std::vector<std::string> names{"baz", "foo"};
+    std::vector<rcl_interfaces::msg::ParameterDescriptor> parameter_descs =
+      synchronous_client->describe_parameters(names, 10s);
+    ASSERT_EQ(0u, parameter_descs.size());
+  }
+
+  {
+    std::vector<std::string> names{"foo", "bar"};
+    std::vector<rcl_interfaces::msg::ParameterDescriptor> parameter_descs =
+      synchronous_client->describe_parameters(names, 10s);
+    ASSERT_EQ(2u, parameter_descs.size());
+    ASSERT_EQ("foo", parameter_descs[0].name);
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_descs[0].type);
+    ASSERT_EQ("", parameter_descs[0].description);
+    ASSERT_EQ("", parameter_descs[0].additional_constraints);
+    ASSERT_FALSE(parameter_descs[0].read_only);
+    ASSERT_EQ("bar", parameter_descs[1].name);
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_STRING, parameter_descs[1].type);
+    ASSERT_EQ("", parameter_descs[1].description);
+    ASSERT_EQ("", parameter_descs[1].additional_constraints);
+    ASSERT_FALSE(parameter_descs[1].read_only);
+  }
+}
+
+/*
+  Coverage for async describe_parameters with allow_undeclared_ enabled
+ */
+TEST_F(TestParameterClient, async_parameter_describe_parameters_allow_undeclared) {
+  node_with_option->declare_parameter("foo", 4);
+  node_with_option->declare_parameter("bar", "this is bar");
+  auto asynchronous_client =
+    std::make_shared<rclcpp::AsyncParametersClient>(node_with_option);
+
+  {
+    bool callback_called = false;
+    auto callback = [&callback_called](
+      std::shared_future<std::vector<rcl_interfaces::msg::ParameterDescriptor>> result)
+      {
+        // We expect the result to be defaut since we tried to get a parameter that didn't exist.
+        if (result.valid() && result.get().size() == 1) {
+          callback_called = true;
+        }
+      };
+    std::vector<std::string> names{"none"};
+    std::shared_future<std::vector<rcl_interfaces::msg::ParameterDescriptor>> future =
+      asynchronous_client->describe_parameters(names, callback);
+    auto return_code = rclcpp::spin_until_future_complete(
+      node_with_option, future, std::chrono::milliseconds(100));
+    ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code);
+    std::vector<rcl_interfaces::msg::ParameterDescriptor> parameter_descs = future.get();
+    ASSERT_EQ(1u, parameter_descs.size());
+    ASSERT_EQ("none", parameter_descs[0].name);
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_NOT_SET, parameter_descs[0].type);
+    ASSERT_EQ("", parameter_descs[0].description);
+    ASSERT_EQ("", parameter_descs[0].additional_constraints);
+    ASSERT_FALSE(parameter_descs[0].read_only);
+    ASSERT_TRUE(callback_called);
+  }
+
+  {
+    bool callback_called = false;
+    auto callback = [&callback_called](
+      std::shared_future<std::vector<rcl_interfaces::msg::ParameterDescriptor>> result)
+      {
+        if (result.valid() && result.get().size() == 2) {
+          callback_called = true;
+        }
+      };
+    std::vector<std::string> names{"foo", "baz"};
+    std::shared_future<std::vector<rcl_interfaces::msg::ParameterDescriptor>> future =
+      asynchronous_client->describe_parameters(names, callback);
+    auto return_code = rclcpp::spin_until_future_complete(
+      node_with_option, future, std::chrono::milliseconds(100));
+    ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code);
+    std::vector<rcl_interfaces::msg::ParameterDescriptor> parameter_descs = future.get();
+    ASSERT_EQ(2u, parameter_descs.size());
+    ASSERT_EQ("foo", parameter_descs[0].name);
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_descs[0].type);
+    ASSERT_EQ("", parameter_descs[0].description);
+    ASSERT_EQ("", parameter_descs[0].additional_constraints);
+    ASSERT_FALSE(parameter_descs[0].read_only);
+    ASSERT_EQ("baz", parameter_descs[1].name);
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_NOT_SET, parameter_descs[1].type);
+    ASSERT_EQ("", parameter_descs[1].description);
+    ASSERT_EQ("", parameter_descs[1].additional_constraints);
+    ASSERT_FALSE(parameter_descs[1].read_only);
+    ASSERT_TRUE(callback_called);
+  }
+
+  {
+    bool callback_called = false;
+    auto callback = [&callback_called](
+      std::shared_future<std::vector<rcl_interfaces::msg::ParameterDescriptor>> result)
+      {
+        if (result.valid() && result.get().size() == 2) {
+          callback_called = true;
+        }
+      };
+    std::vector<std::string> names{"baz", "foo"};
+    std::shared_future<std::vector<rcl_interfaces::msg::ParameterDescriptor>> future =
+      asynchronous_client->describe_parameters(names, callback);
+    auto return_code = rclcpp::spin_until_future_complete(
+      node_with_option, future, std::chrono::milliseconds(100));
+    ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code);
+    std::vector<rcl_interfaces::msg::ParameterDescriptor> parameter_descs = future.get();
+    ASSERT_EQ(2u, parameter_descs.size());
+    ASSERT_EQ("baz", parameter_descs[0].name);
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_NOT_SET, parameter_descs[0].type);
+    ASSERT_EQ("", parameter_descs[0].description);
+    ASSERT_EQ("", parameter_descs[0].additional_constraints);
+    ASSERT_FALSE(parameter_descs[0].read_only);
+    ASSERT_EQ("foo", parameter_descs[1].name);
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_descs[1].type);
+    ASSERT_EQ("", parameter_descs[1].description);
+    ASSERT_EQ("", parameter_descs[1].additional_constraints);
+    ASSERT_FALSE(parameter_descs[1].read_only);
+    ASSERT_TRUE(callback_called);
+  }
+}
+/*
+  Coverage for sync describe_parameters with allow_undeclared_ enabled
+ */
+TEST_F(TestParameterClient, sync_parameter_describe_parameters_allow_undeclared) {
+  node_with_option->declare_parameter("foo", 4);
+  node_with_option->declare_parameter("bar", "this is bar");
+  auto synchronous_client =
+    std::make_shared<rclcpp::SyncParametersClient>(node_with_option);
+
+  {
+    std::vector<std::string> names{"none"};
+    std::vector<rcl_interfaces::msg::ParameterDescriptor> parameter_descs =
+      synchronous_client->describe_parameters(names, 10s);
+    ASSERT_EQ(1u, parameter_descs.size());
+    ASSERT_EQ("none", parameter_descs[0].name);
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_NOT_SET, parameter_descs[0].type);
+    ASSERT_EQ("", parameter_descs[0].description);
+    ASSERT_EQ("", parameter_descs[0].additional_constraints);
+    ASSERT_FALSE(parameter_descs[0].read_only);
+  }
+
+  {
+    std::vector<std::string> names{"foo", "baz"};
+    std::vector<rcl_interfaces::msg::ParameterDescriptor> parameter_descs =
+      synchronous_client->describe_parameters(names, 10s);
+    ASSERT_EQ(2u, parameter_descs.size());
+    ASSERT_EQ("foo", parameter_descs[0].name);
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_descs[0].type);
+    ASSERT_EQ("", parameter_descs[0].description);
+    ASSERT_EQ("", parameter_descs[0].additional_constraints);
+    ASSERT_FALSE(parameter_descs[0].read_only);
+    ASSERT_EQ("baz", parameter_descs[1].name);
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_NOT_SET, parameter_descs[1].type);
+    ASSERT_EQ("", parameter_descs[1].description);
+    ASSERT_EQ("", parameter_descs[1].additional_constraints);
+    ASSERT_FALSE(parameter_descs[1].read_only);
+  }
+
+  {
+    std::vector<std::string> names{"baz", "foo"};
+    std::vector<rcl_interfaces::msg::ParameterDescriptor> parameter_descs =
+      synchronous_client->describe_parameters(names, 10s);
+    ASSERT_EQ(2u, parameter_descs.size());
+    ASSERT_EQ("baz", parameter_descs[0].name);
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_NOT_SET, parameter_descs[0].type);
+    ASSERT_EQ("", parameter_descs[0].description);
+    ASSERT_EQ("", parameter_descs[0].additional_constraints);
+    ASSERT_FALSE(parameter_descs[0].read_only);
+    ASSERT_EQ("foo", parameter_descs[1].name);
+    ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_descs[1].type);
+    ASSERT_EQ("", parameter_descs[1].description);
+    ASSERT_EQ("", parameter_descs[1].additional_constraints);
+    ASSERT_FALSE(parameter_descs[1].read_only);
+  }
 }

rclcpp/test/rclcpp/test_parameter_event_handler.cpp

@@ -0,0 +1,432 @@
+// Copyright 2019 Intel Corporation
+//
+// 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 <chrono>
+#include <memory>
+#include <string>
+#include <thread>
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+
+class TestParameterEventHandler : public rclcpp::ParameterEventHandler
+{
+public:
+  explicit TestParameterEventHandler(rclcpp::Node::SharedPtr node)
+  : ParameterEventHandler(node)
+  {}
+
+  void test_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event)
+  {
+    event_callback(event);
+  }
+
+  size_t num_event_callbacks()
+  {
+    return event_callbacks_.size();
+  }
+
+  size_t num_parameter_callbacks()
+  {
+    return parameter_callbacks_.size();
+  }
+};
+
+class TestNode : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  void SetUp()
+  {
+    rclcpp::NodeOptions options;
+    node = std::make_shared<rclcpp::Node>(
+      "test_parameter_events_subscriber", options);
+
+    remote_node_name = "/remote_node";
+    diff_ns_name = "/ns/remote_node";
+
+    param_handler = std::make_shared<TestParameterEventHandler>(node);
+
+    same_node_int = std::make_shared<rcl_interfaces::msg::ParameterEvent>();
+    same_node_double = std::make_shared<rcl_interfaces::msg::ParameterEvent>();
+    multiple = std::make_shared<rcl_interfaces::msg::ParameterEvent>();
+    remote_node_string = std::make_shared<rcl_interfaces::msg::ParameterEvent>();
+    diff_ns_bool = std::make_shared<rcl_interfaces::msg::ParameterEvent>();
+    diff_node_int = std::make_shared<rcl_interfaces::msg::ParameterEvent>();
+
+    same_node_int->node = node->get_fully_qualified_name();
+    same_node_double->node = node->get_fully_qualified_name();
+    multiple->node = node->get_fully_qualified_name();
+    remote_node_string->node = remote_node_name;
+    diff_ns_bool->node = diff_ns_name;
+    diff_node_int->node = remote_node_name;
+
+    rcl_interfaces::msg::Parameter p;
+    p.name = "my_int";
+    p.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
+    p.value.integer_value = 1;
+    same_node_int->changed_parameters.push_back(p);
+    diff_node_int->changed_parameters.push_back(p);
+    multiple->changed_parameters.push_back(p);
+
+    p.name = "my_double";
+    p.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
+    p.value.double_value = 1.0;
+    same_node_double->changed_parameters.push_back(p);
+    multiple->changed_parameters.push_back(p);
+
+    p.name = "my_string";
+    p.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
+    p.value.string_value = "test";
+    remote_node_string->changed_parameters.push_back(p);
+
+    p.name = "my_bool";
+    p.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
+    p.value.bool_value = true;
+    diff_ns_bool->changed_parameters.push_back(p);
+  }
+
+  void TearDown()
+  {
+    node.reset();
+    param_handler.reset();
+  }
+
+  rcl_interfaces::msg::ParameterEvent::SharedPtr same_node_int;
+  rcl_interfaces::msg::ParameterEvent::SharedPtr same_node_double;
+  rcl_interfaces::msg::ParameterEvent::SharedPtr diff_node_int;
+  rcl_interfaces::msg::ParameterEvent::SharedPtr remote_node_string;
+  rcl_interfaces::msg::ParameterEvent::SharedPtr multiple;
+  rcl_interfaces::msg::ParameterEvent::SharedPtr diff_ns_bool;
+
+  rclcpp::Node::SharedPtr node;
+  std::string remote_node_name;
+  std::string diff_ns_name;
+  std::shared_ptr<TestParameterEventHandler> param_handler;
+};
+
+TEST_F(TestNode, RegisterParameterCallback)
+{
+  bool received;
+  auto cb = [&received](const rclcpp::Parameter &) {received = true;};
+
+  auto h1 = param_handler->add_parameter_callback("my_double", cb);
+  auto h2 = param_handler->add_parameter_callback("my_int", cb);
+  auto h3 = param_handler->add_parameter_callback("my_string", cb, remote_node_name);
+  auto h4 = param_handler->add_parameter_callback("my_bool", cb, diff_ns_name);
+
+  received = false;
+  param_handler->test_event(same_node_double);
+  EXPECT_EQ(received, true);
+
+  received = false;
+  param_handler->test_event(same_node_int);
+  EXPECT_EQ(received, true);
+
+  received = false;
+  param_handler->test_event(remote_node_string);
+  EXPECT_EQ(received, true);
+
+  received = false;
+  param_handler->test_event(diff_ns_bool);
+  EXPECT_EQ(received, true);
+}
+
+TEST_F(TestNode, SameParameterDifferentNode)
+{
+  int64_t int_param_node1;
+  int64_t int_param_node2;
+
+  auto cb1 = [&int_param_node1](const rclcpp::Parameter & p) {
+      int_param_node1 = p.get_value<int64_t>();
+    };
+  auto cb2 = [&int_param_node2](const rclcpp::Parameter & p) {
+      int_param_node2 = p.get_value<int64_t>();
+    };
+
+  // Set individual parameters
+  auto h1 = param_handler->add_parameter_callback("my_int", cb1);
+  auto h2 = param_handler->add_parameter_callback("my_int", cb2, remote_node_name);
+
+  param_handler->test_event(same_node_int);
+  EXPECT_EQ(int_param_node1, 1);
+  EXPECT_NE(int_param_node2, 1);
+
+  int_param_node1 = 0;
+  int_param_node2 = 0;
+
+  param_handler->test_event(diff_node_int);
+  EXPECT_NE(int_param_node1, 1);
+  EXPECT_EQ(int_param_node2, 1);
+
+  param_handler->remove_parameter_callback(h1);
+  param_handler->remove_parameter_callback(h2);
+  EXPECT_EQ(param_handler->num_parameter_callbacks(), 0UL);
+}
+
+TEST_F(TestNode, GetParameterFromEvent)
+{
+  using rclcpp::ParameterEventHandler;
+  std::string node_name = node->get_fully_qualified_name();
+  std::string wrong_name = "/wrong_node_name";
+
+  rclcpp::Parameter p;
+  EXPECT_TRUE(
+    ParameterEventHandler::get_parameter_from_event(*multiple, p, "my_int", node_name));
+  EXPECT_EQ(p.get_value<int>(), 1);
+  // False if parameter not with correct node name
+  EXPECT_FALSE(
+    ParameterEventHandler::get_parameter_from_event(*multiple, p, "my_int", wrong_name));
+  // False if parameter not part of event
+  EXPECT_FALSE(
+    ParameterEventHandler::get_parameter_from_event(*diff_ns_bool, p, "my_int", node_name));
+
+  EXPECT_NO_THROW(
+    ParameterEventHandler::get_parameter_from_event(*multiple, "my_int", node_name));
+  // Throws if parameter not with correct node name
+  EXPECT_THROW(
+    ParameterEventHandler::get_parameter_from_event(*multiple, "my_int", wrong_name),
+    std::runtime_error);
+  // Throws if parameter not part of event
+  EXPECT_THROW(
+    ParameterEventHandler::get_parameter_from_event(*diff_ns_bool, "my_int", node_name),
+    std::runtime_error);
+}
+
+TEST_F(TestNode, GetParametersFromEvent)
+{
+  using rclcpp::ParameterEventHandler;
+  std::string node_name = node->get_fully_qualified_name();
+
+  auto params = ParameterEventHandler::get_parameters_from_event(*multiple);
+  EXPECT_EQ(params.size(), 2u);
+  bool found_int = false;
+  bool found_double = false;
+  for (auto & p : params) {
+    if (p.get_name() == std::string("my_int")) {
+      found_int = true;
+      EXPECT_EQ(p.get_value<int>(), 1);
+    } else if (p.get_name() == std::string("my_double")) {
+      found_double = true;
+      EXPECT_EQ(p.get_value<double>(), 1.0);
+    }
+  }
+  EXPECT_EQ(found_int, true);
+  EXPECT_EQ(found_double, true);
+
+  params = ParameterEventHandler::get_parameters_from_event(*remote_node_string);
+  EXPECT_EQ(params.size(), 1u);
+  bool found_string = false;
+  for (auto & p : params) {
+    if (p.get_name() == std::string("my_string")) {
+      found_string = true;
+      EXPECT_EQ(p.get_value<std::string>(), std::string("test"));
+    }
+  }
+  EXPECT_EQ(found_string, true);
+
+  params = ParameterEventHandler::get_parameters_from_event(*diff_ns_bool);
+  EXPECT_EQ(params.size(), 1u);
+  bool found_bool = false;
+  for (auto & p : params) {
+    if (p.get_name() == std::string("my_bool")) {
+      found_bool = true;
+      EXPECT_EQ(p.get_value<bool>(), true);
+    }
+  }
+  EXPECT_EQ(found_bool, true);
+}
+
+TEST_F(TestNode, EventCallback)
+{
+  using rclcpp::ParameterEventHandler;
+
+  double double_param = 0.0;
+  int64_t int_param = 0;
+  bool bool_param{false};
+  bool received{false};
+
+  double product;
+  auto cb =
+    [&int_param, &double_param, &product, &received,
+      this](const rcl_interfaces::msg::ParameterEvent::SharedPtr & event)
+    {
+      auto node_name = node->get_fully_qualified_name();
+
+      if (event->node == node_name) {
+        received = true;
+      }
+
+      rclcpp::Parameter p;
+      if (ParameterEventHandler::get_parameter_from_event(*event, p, "my_int", node_name)) {
+        int_param = p.get_value<int64_t>();
+      }
+      try {
+        p = ParameterEventHandler::get_parameter_from_event(*event, "my_double", node_name);
+        double_param = p.get_value<double>();
+      } catch (...) {
+      }
+
+      product = static_cast<double>(int_param) * double_param;
+    };
+
+  auto cb2 =
+    [&bool_param, this](const rcl_interfaces::msg::ParameterEvent::SharedPtr & event)
+    {
+      rclcpp::Parameter p;
+      if (event->node == diff_ns_name) {
+        if (ParameterEventHandler::get_parameter_from_event(
+            *event, p, "my_bool", diff_ns_name))
+        {
+          bool_param = p.get_value<bool>();
+        }
+      }
+    };
+
+  auto event_handle1 = param_handler->add_parameter_event_callback(cb);
+  auto event_handle2 = param_handler->add_parameter_event_callback(cb2);
+
+  bool_param = false;
+  param_handler->test_event(multiple);
+  EXPECT_EQ(received, true);
+  EXPECT_EQ(product, 1.0);
+  EXPECT_EQ(bool_param, false);
+
+  param_handler->test_event(diff_ns_bool);
+  EXPECT_EQ(bool_param, true);
+
+  // Test removal of event callback
+  received = false;
+  bool_param = false;
+  param_handler->remove_parameter_event_callback(event_handle1);
+  param_handler->test_event(multiple);
+  param_handler->test_event(diff_ns_bool);
+  EXPECT_EQ(received, false);
+  EXPECT_EQ(bool_param, true);
+
+  // Should throw if callback handle no longer exists or already removed
+  EXPECT_THROW(
+    param_handler->remove_parameter_event_callback(event_handle1), std::runtime_error);
+}
+
+TEST_F(TestNode, MultipleParameterCallbacks)
+{
+  bool received_1{false};
+  bool received_2{false};
+
+  auto cb1 = [&received_1](const rclcpp::Parameter &) {received_1 = true;};
+  auto cb2 = [&received_2](const rclcpp::Parameter &) {received_2 = true;};
+  auto cb3 = [](const rclcpp::Parameter &) { /*do nothing*/};
+
+  auto h1 = param_handler->add_parameter_callback("my_int", cb1);
+  auto h2 = param_handler->add_parameter_callback("my_int", cb2);
+  auto h3 = param_handler->add_parameter_callback("my_double", cb3);
+
+  // Test multiple callbacks per parameter
+  param_handler->test_event(same_node_int);
+  EXPECT_EQ(received_1, true);
+  EXPECT_EQ(received_2, true);
+
+  // Test removal of parameter callback by callback handle
+  received_1 = false;
+  received_2 = false;
+  param_handler->remove_parameter_callback(h1);
+  param_handler->test_event(same_node_int);
+  EXPECT_EQ(received_1, false);
+  EXPECT_EQ(received_2, true);
+
+  // Test removal of parameter callback by name
+  received_2 = false;
+  param_handler->remove_parameter_callback(h2);
+  param_handler->test_event(same_node_int);
+  EXPECT_EQ(received_2, false);
+
+  // Should throw if callback handle no longer exists or already removed
+  EXPECT_THROW(param_handler->remove_parameter_callback(h1), std::runtime_error);
+  EXPECT_THROW(param_handler->remove_parameter_callback(h2), std::runtime_error);
+
+  param_handler->remove_parameter_callback(h3);
+
+  // All callbacks should have been removed
+  EXPECT_EQ(received_2, 0);
+  EXPECT_EQ(param_handler->num_event_callbacks(), 0UL);
+}
+
+TEST_F(TestNode, LastInFirstCallForParameterCallbacks)
+{
+  rclcpp::Time time_1;
+  rclcpp::Time time_2;
+
+  // The callbacks will log the current time for comparison purposes. Add a bit of a stall
+  // to ensure that the time noted in the back-to-back calls isn't the same
+  auto cb1 = [this, &time_1](const rclcpp::Parameter &) {
+      time_1 = node->now();
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    };
+  auto cb2 = [this, &time_2](const rclcpp::Parameter &) {
+      time_2 = node->now();
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    };
+
+  auto h1 = param_handler->add_parameter_callback("my_int", cb1);
+  auto h2 = param_handler->add_parameter_callback("my_int", cb2);
+
+  // Test multiple callbacks per parameter
+  param_handler->test_event(same_node_int);
+
+  // The most-recently install handler should be called first
+  EXPECT_EQ(time_2 < time_1, true);
+
+  param_handler->remove_parameter_callback(h1);
+  param_handler->remove_parameter_callback(h2);
+  EXPECT_EQ(param_handler->num_parameter_callbacks(), 0UL);
+}
+
+TEST_F(TestNode, LastInFirstCallForParameterEventCallbacks)
+{
+  rclcpp::Time time_1;
+  rclcpp::Time time_2;
+
+  // The callbacks will log the current time for comparison purposes. Add a bit of a stall
+  // to ensure that the time noted in the back-to-back calls isn't the same
+  auto cb1 =
+    [this, &time_1](const rcl_interfaces::msg::ParameterEvent::SharedPtr &)
+    {
+      time_1 = node->now();
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    };
+  auto cb2 =
+    [this, &time_2](const rcl_interfaces::msg::ParameterEvent::SharedPtr &)
+    {
+      time_2 = node->now();
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    };
+
+  auto h1 = param_handler->add_parameter_event_callback(cb1);
+  auto h2 = param_handler->add_parameter_event_callback(cb2);
+
+  // Test multiple callbacks per parameter
+  param_handler->test_event(same_node_int);
+
+  // The most-recently install handler should be called first
+  EXPECT_EQ(time_2 < time_1, true);
+
+  param_handler->remove_parameter_event_callback(h1);
+  param_handler->remove_parameter_event_callback(h2);
+  EXPECT_EQ(param_handler->num_event_callbacks(), 0UL);
+}

rclcpp/test/rclcpp/test_parameter_service.cpp

@@ -22,6 +22,8 @@
 #include "rclcpp/rclcpp.hpp"
 #include "../../src/rclcpp/parameter_service_names.hpp"
 
+using namespace std::chrono_literals;
+
 // This tests the ParameterService as it is included in an rclcpp::Node. Creating a separate
 // ParameterService would interfere with the built-in one
 class TestParameterService : public ::testing::Test
@@ -59,12 +61,12 @@ TEST_F(TestParameterService, get_parameter_types) {
   node->declare_parameter("parameter1", rclcpp::ParameterValue(42));
 
   const std::vector<std::string> declared_parameters = {"parameter1"};
-  const auto parameter_types = client->get_parameter_types(declared_parameters);
+  const auto parameter_types = client->get_parameter_types(declared_parameters, 10s);
   ASSERT_EQ(1u, parameter_types.size());
   EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_types[0]);
 
   const std::vector<std::string> undeclared_parameters = {"parameter2"};
-  const auto undeclared_parameter_types = client->get_parameter_types(undeclared_parameters);
+  const auto undeclared_parameter_types = client->get_parameter_types(undeclared_parameters, 10s);
   EXPECT_EQ(0u, undeclared_parameter_types.size());
 }
 
@@ -75,7 +77,7 @@ TEST_F(TestParameterService, set_parameters) {
   const std::vector<rclcpp::Parameter> parameters = {
     rclcpp::Parameter("parameter1", 0),
   };
-  client->set_parameters(parameters);
+  client->set_parameters(parameters, 10s);
   EXPECT_EQ(0, client->get_parameter("parameter1", 100));
 }
 
@@ -86,54 +88,34 @@ TEST_F(TestParameterService, set_parameters_atomically) {
   const std::vector<rclcpp::Parameter> parameters = {
     rclcpp::Parameter("parameter1", 0),
   };
-  client->set_parameters_atomically(parameters);
+  client->set_parameters_atomically(parameters, 10s);
   EXPECT_EQ(0, client->get_parameter("parameter1", 100));
 }
 
 TEST_F(TestParameterService, list_parameters) {
-  const size_t number_parameters_in_basic_node = client->list_parameters({}, 1).names.size();
+  const size_t number_parameters_in_basic_node = client->list_parameters({}, 1, 10s).names.size();
   node->declare_parameter("parameter1", rclcpp::ParameterValue(42));
 
-  const auto list_result = client->list_parameters({}, 1);
+  const auto list_result = client->list_parameters({}, 1, 10s);
   EXPECT_EQ(1u + number_parameters_in_basic_node, list_result.names.size());
 }
 
 TEST_F(TestParameterService, describe_parameters) {
-  // There is no current API in ParameterClient for calling the describe_parameters service
-  // Update this test when https://github.com/ros2/rclcpp/issues/1354 is resolved
-
-  const std::string describe_parameters_service_name =
-    node->get_fully_qualified_name() + std::string("/") +
-    rclcpp::parameter_service_names::describe_parameters;
-  using ServiceT = rcl_interfaces::srv::DescribeParameters;
-  auto client =
-    node->create_client<ServiceT>(describe_parameters_service_name);
-
-  ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1)));
   node->declare_parameter("parameter1", rclcpp::ParameterValue(42));
 
   {
-    auto request = std::make_shared<rcl_interfaces::srv::DescribeParameters::Request>();
-    request->names.push_back("parameter1");
-    auto future = client->async_send_request(request);
-
-    rclcpp::spin_until_future_complete(node, future, std::chrono::seconds(1));
-    auto response = future.get();
-    ASSERT_NE(nullptr, response);
-    EXPECT_EQ(1u, response->descriptors.size());
-
-    auto descriptor = response->descriptors[0];
-    EXPECT_EQ("parameter1", descriptor.name);
-    EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, descriptor.type);
+    const std::vector<std::string> names{"parameter1"};
+    std::vector<rcl_interfaces::msg::ParameterDescriptor> parameter_descs =
+      client->describe_parameters(names, 10s);
+    ASSERT_EQ(1u, parameter_descs.size());
+    EXPECT_EQ("parameter1", parameter_descs[0].name);
+    EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_descs[0].type);
   }
-  {
-    auto request = std::make_shared<rcl_interfaces::srv::DescribeParameters::Request>();
-    request->names.push_back("undeclared_parameter");
-    auto future = client->async_send_request(request);
 
-    rclcpp::spin_until_future_complete(node, future, std::chrono::seconds(1));
-    auto response = future.get();
-    ASSERT_NE(nullptr, response);
-    EXPECT_EQ(0u, response->descriptors.size());
+  {
+    const std::vector<std::string> names{"undeclared_parameter"};
+    std::vector<rcl_interfaces::msg::ParameterDescriptor> parameter_descs =
+      client->describe_parameters(names, 10s);
+    EXPECT_EQ(0u, parameter_descs.size());
   }
 }

rclcpp/test/rclcpp/test_publisher.cpp

@@ -29,10 +29,6 @@
 
 #include "test_msgs/msg/empty.hpp"
 
-// Note: This is a long running test with rmw_connext_cpp, if you change this file, please check
-// that this test can complete fully, or adjust the timeout as necessary.
-// See https://github.com/ros2/rmw_connext/issues/325 for resolution
-
 class TestPublisher : public ::testing::Test
 {
 public:
@@ -462,9 +458,9 @@ class TestPublisherProtectedMethods : public rclcpp::Publisher<MessageT, Allocat
 public:
   using rclcpp::Publisher<MessageT, AllocatorT>::Publisher;
 
-  void publish_loaned_message(MessageT * msg)
+  void publish_loaned_message(rclcpp::LoanedMessage<MessageT, AllocatorT> && loaned_msg)
   {
-    this->do_loaned_message_publish(msg);
+    this->do_loaned_message_publish(std::move(loaned_msg.release()));
   }
 
   void call_default_incompatible_qos_callback(rclcpp::QOSOfferedIncompatibleQoSInfo & event) const
@@ -479,13 +475,14 @@ TEST_F(TestPublisher, do_loaned_message_publish_error) {
   auto publisher =
     node->create_publisher<test_msgs::msg::Empty, std::allocator<void>, PublisherT>("topic", 10);
 
-  auto msg = std::make_shared<test_msgs::msg::Empty>();
+  auto msg = publisher->borrow_loaned_message();
+
   {
     // Using 'self' instead of 'lib:rclcpp' because `rcl_publish_loaned_message` is entirely
     // defined in a header
     auto mock = mocking_utils::patch_and_return(
       "self", rcl_publish_loaned_message, RCL_RET_PUBLISHER_INVALID);
-    EXPECT_THROW(publisher->publish_loaned_message(msg.get()), rclcpp::exceptions::RCLError);
+    EXPECT_THROW(publisher->publish_loaned_message(std::move(msg)), rclcpp::exceptions::RCLError);
   }
 }
 
@@ -506,6 +503,6 @@ TEST_F(TestPublisher, run_event_handlers) {
 
   for (const auto & handler : publisher->get_event_handlers()) {
     std::shared_ptr<void> data = handler->take_data();
-    EXPECT_THROW(handler->execute(data), std::runtime_error);
+    handler->execute(data);
   }
 }

rclcpp/test/rclcpp/test_qos.cpp

@@ -14,6 +14,8 @@
 
 #include <gtest/gtest.h>
 
+#include <string>
+
 #include "rclcpp/qos.hpp"
 
 #include "rmw/types.h"
@@ -78,33 +80,41 @@ TEST(TestQoS, equality_namespace) {
   EXPECT_NE(a, b);
 }
 
-TEST(TestQoS, setters) {
+TEST(TestQoS, setters_and_getters) {
   rclcpp::QoS qos(10);
 
   qos.keep_all();
-  EXPECT_EQ(RMW_QOS_POLICY_HISTORY_KEEP_ALL, qos.get_rmw_qos_profile().history);
+  EXPECT_EQ(rclcpp::HistoryPolicy::KeepAll, qos.history());
 
   qos.keep_last(20);
-  EXPECT_EQ(RMW_QOS_POLICY_HISTORY_KEEP_LAST, qos.get_rmw_qos_profile().history);
-  EXPECT_EQ(20u, qos.get_rmw_qos_profile().depth);
+  EXPECT_EQ(rclcpp::HistoryPolicy::KeepLast, qos.history());
+  EXPECT_EQ(20u, qos.depth());
 
   qos.reliable();
-  EXPECT_EQ(RMW_QOS_POLICY_RELIABILITY_RELIABLE, qos.get_rmw_qos_profile().reliability);
+  EXPECT_EQ(rclcpp::ReliabilityPolicy::Reliable, qos.reliability());
+
+  qos.reliability(rclcpp::ReliabilityPolicy::BestEffort);
+  EXPECT_EQ(rclcpp::ReliabilityPolicy::BestEffort, qos.reliability());
 
   qos.durability_volatile();
-  EXPECT_EQ(RMW_QOS_POLICY_DURABILITY_VOLATILE, qos.get_rmw_qos_profile().durability);
+  EXPECT_EQ(rclcpp::DurabilityPolicy::Volatile, qos.durability());
 
   qos.transient_local();
-  EXPECT_EQ(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, qos.get_rmw_qos_profile().durability);
+  EXPECT_EQ(rclcpp::DurabilityPolicy::TransientLocal, qos.durability());
+
+  qos.durability(rclcpp::DurabilityPolicy::Volatile);
+  EXPECT_EQ(rclcpp::DurabilityPolicy::Volatile, qos.durability());
 
   qos.history(RMW_QOS_POLICY_HISTORY_KEEP_ALL);
-  EXPECT_EQ(RMW_QOS_POLICY_HISTORY_KEEP_ALL, qos.get_rmw_qos_profile().history);
+  EXPECT_EQ(rclcpp::HistoryPolicy::KeepAll, qos.history());
 
-  constexpr size_t duration_ns = 12345;
+  qos.history(rclcpp::HistoryPolicy::KeepLast);
+  EXPECT_EQ(rclcpp::HistoryPolicy::KeepLast, qos.history());
+
+  constexpr rcl_duration_value_t duration_ns = 12345;
   constexpr std::chrono::nanoseconds duration(duration_ns);
   qos.deadline(duration);
-  EXPECT_EQ(0u, qos.get_rmw_qos_profile().deadline.sec);
-  EXPECT_EQ(duration_ns, qos.get_rmw_qos_profile().deadline.nsec);
+  EXPECT_EQ(duration_ns, qos.deadline().nanoseconds());
 
   const rmw_time_t rmw_time {0, 54321};
   qos.deadline(rmw_time);
@@ -112,28 +122,29 @@ TEST(TestQoS, setters) {
   EXPECT_EQ(rmw_time.nsec, qos.get_rmw_qos_profile().deadline.nsec);
 
   qos.lifespan(duration);
-  EXPECT_EQ(0u, qos.get_rmw_qos_profile().lifespan.sec);
-  EXPECT_EQ(duration_ns, qos.get_rmw_qos_profile().lifespan.nsec);
+  EXPECT_EQ(duration_ns, qos.lifespan().nanoseconds());
 
   qos.lifespan(rmw_time);
   EXPECT_EQ(rmw_time.sec, qos.get_rmw_qos_profile().lifespan.sec);
   EXPECT_EQ(rmw_time.nsec, qos.get_rmw_qos_profile().lifespan.nsec);
 
   qos.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC);
-  EXPECT_EQ(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC, qos.get_rmw_qos_profile().liveliness);
+  EXPECT_EQ(rclcpp::LivelinessPolicy::ManualByTopic, qos.liveliness());
+
+  qos.liveliness(rclcpp::LivelinessPolicy::Automatic);
+  EXPECT_EQ(rclcpp::LivelinessPolicy::Automatic, qos.liveliness());
 
   qos.liveliness_lease_duration(duration);
-  EXPECT_EQ(0u, qos.get_rmw_qos_profile().liveliness_lease_duration.sec);
-  EXPECT_EQ(duration_ns, qos.get_rmw_qos_profile().liveliness_lease_duration.nsec);
+  EXPECT_EQ(duration_ns, qos.liveliness_lease_duration().nanoseconds());
 
   qos.liveliness_lease_duration(rmw_time);
   EXPECT_EQ(rmw_time.sec, qos.get_rmw_qos_profile().liveliness_lease_duration.sec);
   EXPECT_EQ(rmw_time.nsec, qos.get_rmw_qos_profile().liveliness_lease_duration.nsec);
 
   qos.avoid_ros_namespace_conventions(true);
-  EXPECT_TRUE(qos.get_rmw_qos_profile().avoid_ros_namespace_conventions);
+  EXPECT_TRUE(qos.avoid_ros_namespace_conventions());
   qos.avoid_ros_namespace_conventions(false);
-  EXPECT_FALSE(qos.get_rmw_qos_profile().avoid_ros_namespace_conventions);
+  EXPECT_FALSE(qos.avoid_ros_namespace_conventions());
 }
 
 bool operator==(const rmw_qos_profile_t & lhs, const rmw_qos_profile_t & rhs)
@@ -199,3 +210,43 @@ TEST(TestQoS, policy_name_from_kind) {
     "LIFESPAN_QOS_POLICY",
     rclcpp::qos_policy_name_from_kind(RMW_QOS_POLICY_LIFESPAN));
 }
+
+TEST(TestQoS, qos_check_compatible)
+{
+  // Compatible
+  {
+    rclcpp::QoS qos = rclcpp::QoS(1)
+      .reliable()
+      .durability_volatile()
+      .deadline(rclcpp::Duration(1, 0u))
+      .lifespan(rclcpp::Duration(1, 0u))
+      .liveliness(rclcpp::LivelinessPolicy::Automatic)
+      .liveliness_lease_duration(rclcpp::Duration(1, 0u));
+    rclcpp::QoSCheckCompatibleResult ret = rclcpp::qos_check_compatible(qos, qos);
+    EXPECT_EQ(ret.compatibility, rclcpp::QoSCompatibility::Ok);
+    EXPECT_EQ(ret.reason, std::string(""));
+  }
+
+  // Note, the following incompatible tests assume we are using a DDS middleware,
+  // and may not be valid for other RMWs.
+  // TODO(jacobperron): programmatically check if current RMW is one of the officially
+  //                    supported DDS middlewares before running the following tests
+
+  // Incompatible
+  {
+    rclcpp::QoS pub_qos = rclcpp::QoS(1).best_effort();
+    rclcpp::QoS sub_qos = rclcpp::QoS(1).reliable();
+    rclcpp::QoSCheckCompatibleResult ret = rclcpp::qos_check_compatible(pub_qos, sub_qos);
+    EXPECT_EQ(ret.compatibility, rclcpp::QoSCompatibility::Error);
+    EXPECT_FALSE(ret.reason.empty());
+  }
+
+  // Warn of possible incompatibility
+  {
+    rclcpp::SystemDefaultsQoS pub_qos;
+    rclcpp::QoS sub_qos = rclcpp::QoS(1).reliable();
+    rclcpp::QoSCheckCompatibleResult ret = rclcpp::qos_check_compatible(pub_qos, sub_qos);
+    EXPECT_EQ(ret.compatibility, rclcpp::QoSCompatibility::Warning);
+    EXPECT_FALSE(ret.reason.empty());
+  }
+}

rclcpp/test/rclcpp/test_qos_parameters.cpp

@@ -234,7 +234,8 @@ TEST(TestQosParameters, declare_no_parameters_interface) {
 
   std::map<std::string, rclcpp::Parameter> qos_params;
   EXPECT_FALSE(
-    node->get_node_parameters_interface()->get_parameters_by_prefix("qos_overrides", qos_params));
+    node->get_node_parameters_interface()->get_parameters_by_prefix(
+      "qos_overrides./my/fully/qualified/topic_name", qos_params));
 
   rclcpp::shutdown();
 }

rclcpp/test/rclcpp/test_serialized_message.cpp

@@ -212,8 +212,6 @@ TEST(TestSerializedMessage, assignment_operators) {
   serialized_msg_move = std::move(serialized_message_to_assign);
   EXPECT_EQ(13u, serialized_msg_move.capacity());
   EXPECT_EQ(content_size, serialized_msg_move.size());
-  EXPECT_EQ(0u, serialized_message_to_assign.capacity());
-  EXPECT_EQ(0u, serialized_message_to_assign.size());
 
   // Test move assignment with = operator, with a rcl_serialized_message_t
   rclcpp::SerializedMessage serialized_msg_move_rcl(2);
@@ -222,7 +220,6 @@ TEST(TestSerializedMessage, assignment_operators) {
   serialized_msg_move_rcl = std::move(rcl_serialized_msg);
   EXPECT_EQ(13u, serialized_msg_move_rcl.capacity());
   EXPECT_EQ(content_size, serialized_msg_move_rcl.size());
-  EXPECT_EQ(0u, rcl_serialized_msg.buffer_capacity);
 
   // Error because it was moved
   EXPECT_EQ(RCUTILS_RET_INVALID_ARGUMENT, rmw_serialized_message_fini(&rcl_serialized_msg));

rclcpp/test/rclcpp/test_subscription.cpp

@@ -29,10 +29,6 @@
 
 #include "test_msgs/msg/empty.hpp"
 
-// Note: This is a long running test with rmw_connext_cpp, if you change this file, please check
-// that this test can complete fully, or adjust the timeout as necessary.
-// See https://github.com/ros2/rmw_connext/issues/325 for resolution
-
 using namespace std::chrono_literals;
 
 class TestSubscription : public ::testing::Test

rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp

@@ -45,6 +45,7 @@
 
 namespace
 {
+constexpr const std::chrono::seconds defaultStatisticsPublishPeriod{1};
 constexpr const char kTestPubNodeName[]{"test_pub_stats_node"};
 constexpr const char kTestSubNodeName[]{"test_sub_stats_node"};
 constexpr const char kTestSubStatsTopic[]{"/test_sub_stats_topic"};
@@ -53,10 +54,17 @@ constexpr const char kTestTopicStatisticsTopic[]{"/test_topic_statistics_topic"}
 constexpr const char kMessageAgeSourceLabel[]{"message_age"};
 constexpr const char kMessagePeriodSourceLabel[]{"message_period"};
 constexpr const uint64_t kNoSamples{0};
-constexpr const std::chrono::seconds kTestDuration{10};
-constexpr const uint64_t kNumExpectedMessages{8};
-constexpr const uint64_t kNumExpectedMessageAgeMessages{kNumExpectedMessages / 2};
-constexpr const uint64_t kNumExpectedMessagePeriodMessages{kNumExpectedMessages / 2};
+constexpr const std::chrono::seconds kTestTimeout{10};
+constexpr const uint64_t kNumExpectedWindows{4};
+constexpr const uint64_t kNumExpectedMessages{kNumExpectedWindows * 2};
+constexpr const uint64_t kNumExpectedMessageAgeMessages{kNumExpectedWindows};
+constexpr const uint64_t kNumExpectedMessagePeriodMessages{kNumExpectedWindows};
+constexpr const std::chrono::seconds kUnstableMessageAgeWindowDuration{
+  defaultStatisticsPublishPeriod * (kNumExpectedWindows / 2)};
+// kUnstableMessageAgeWindowDuration can take following value.
+// Min: defaultStatisticsPublishPeriod * 2
+// Max: defaultStatisticsPublishPeriod * (kNumExpectedWindows - 2)
+constexpr const std::chrono::seconds kUnstableMessageAgeOffset{std::chrono::seconds{1}};
 }  // namespace
 
 using rclcpp::msg::MessageWithHeader;
@@ -161,6 +169,49 @@ private:
   std::uniform_int_distribution<uint32_t> uniform_dist_;
 };
 
+/**
+ * TransitionMessageStamp publisher node : used to publish MessageWithHeader with `header` value set
+ * The message age results change during the test.
+ */
+
+class TransitionMessageStampPublisher : public rclcpp::Node
+{
+public:
+  TransitionMessageStampPublisher(
+    const std::string & name, const std::string & topic,
+    const std::chrono::seconds transition_duration, const std::chrono::seconds message_age_offset,
+    const std::chrono::milliseconds & publish_period = std::chrono::milliseconds{100})
+  : Node(name), transition_duration_(transition_duration), message_age_offset_(message_age_offset)
+  {
+    publisher_ = create_publisher<MessageWithHeader>(topic, 10);
+    publish_timer_ = this->create_wall_timer(publish_period, [this]() {this->publish_message();});
+    start_time_ = this->now();
+  }
+
+private:
+  void publish_message()
+  {
+    auto msg = MessageWithHeader{};
+    auto now = this->now();
+    auto elapsed_time = now - start_time_;
+    if (elapsed_time < transition_duration_) {
+      // Apply only to the topic statistics in the first half
+      // Subtract offset so message_age is always >= offset.
+      msg.header.stamp = now - message_age_offset_;
+    } else {
+      msg.header.stamp = now;
+    }
+    publisher_->publish(msg);
+  }
+
+  std::chrono::seconds transition_duration_;
+  std::chrono::seconds message_age_offset_;
+  rclcpp::Time start_time_;
+
+  rclcpp::Publisher<MessageWithHeader>::SharedPtr publisher_;
+  rclcpp::TimerBase::SharedPtr publish_timer_;
+};
+
 /**
  * Empty subscriber node: used to create subscriber topic statistics requirements
  */
@@ -202,6 +253,7 @@ public:
     // manually enable topic statistics via options
     auto options = rclcpp::SubscriptionOptions();
     options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable;
+    options.topic_stats_options.publish_period = defaultStatisticsPublishPeriod;
 
     auto callback = [](MessageWithHeader::UniquePtr msg) {
         (void) msg;
@@ -278,7 +330,7 @@ void check_if_statistic_message_is_populated(const MetricsMessage & message_to_c
         EXPECT_LT(0, stats_point.data) << "unexpected avg " << stats_point.data;
         break;
       case StatisticDataType::STATISTICS_DATA_TYPE_MINIMUM:
-        EXPECT_LT(0, stats_point.data) << "unexpected mi n" << stats_point.data;
+        EXPECT_LT(0, stats_point.data) << "unexpected min " << stats_point.data;
         break;
       case StatisticDataType::STATISTICS_DATA_TYPE_MAXIMUM:
         EXPECT_LT(0, stats_point.data) << "unexpected max " << stats_point.data;
@@ -382,7 +434,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no
   // Spin and get future
   ex.spin_until_future_complete(
     statistics_listener->GetFuture(),
-    kTestDuration);
+    kTestTimeout);
 
   // Compare message counts, sample count should be the same as published and received count
   EXPECT_EQ(kNumExpectedMessages, statistics_listener->GetNumberOfMessagesReceived());
@@ -447,7 +499,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_wi
   // Spin and get future
   ex.spin_until_future_complete(
     statistics_listener->GetFuture(),
-    kTestDuration);
+    kTestTimeout);
 
   // Compare message counts, sample count should be the same as published and received count
   EXPECT_EQ(kNumExpectedMessages, statistics_listener->GetNumberOfMessagesReceived());
@@ -476,3 +528,61 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_wi
     check_if_statistic_message_is_populated(msg);
   }
 }
+
+TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_include_window_reset)
+{
+  // Create a MessageWithHeader publisher
+  auto msg_with_header_publisher = std::make_shared<TransitionMessageStampPublisher>(
+    kTestPubNodeName, kTestSubStatsTopic, kUnstableMessageAgeWindowDuration,
+    kUnstableMessageAgeOffset);
+
+  // msg_with_header_subscriber has a topic statistics instance as part of its
+  // subscription this will listen to and generate statistics
+  auto msg_with_header_subscriber =
+    std::make_shared<MessageWithHeaderSubscriber>(kTestSubNodeName, kTestSubStatsTopic);
+  // Create a listener for topic statistics messages
+  auto statistics_listener = std::make_shared<rclcpp::topic_statistics::MetricsMessageSubscriber>(
+    "test_receive_stats_include_window_reset", "/statistics", kNumExpectedMessages);
+
+  rclcpp::executors::SingleThreadedExecutor ex;
+  ex.add_node(msg_with_header_publisher);
+  ex.add_node(statistics_listener);
+  ex.add_node(msg_with_header_subscriber);
+
+  // Spin and get future
+  ex.spin_until_future_complete(statistics_listener->GetFuture(), kTestTimeout);
+
+  const auto received_messages = statistics_listener->GetReceivedMessages();
+  EXPECT_EQ(kNumExpectedMessages, received_messages.size());
+
+  auto message_age_offset =
+    std::chrono::duration<double, std::milli>(kUnstableMessageAgeOffset).count();
+
+  // Check that the first statistic contains the offset inside of its window
+  auto head_message = received_messages[0];
+  for (const auto & stats_point : head_message.statistics) {
+    const auto type = stats_point.data_type;
+    switch (type) {
+      case StatisticDataType::STATISTICS_DATA_TYPE_MINIMUM:
+      case StatisticDataType::STATISTICS_DATA_TYPE_MAXIMUM:
+        EXPECT_GE(stats_point.data, message_age_offset);
+        break;
+      default:
+        break;
+    }
+  }
+
+  // Check that the last statistic does not contain the offset outside of its window
+  auto tail_message = received_messages[received_messages.size() - 1];
+  for (const auto & stats_point : tail_message.statistics) {
+    const auto type = stats_point.data_type;
+    switch (type) {
+      case StatisticDataType::STATISTICS_DATA_TYPE_MINIMUM:
+      case StatisticDataType::STATISTICS_DATA_TYPE_MAXIMUM:
+        EXPECT_LT(stats_point.data, message_age_offset);
+        break;
+      default:
+        break;
+    }
+  }
+}

rclcpp_action/CHANGELOG.rst

@@ -3,6 +3,43 @@ Changelog for package rclcpp_action
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 
+8.1.0 (2021-03-25)
+------------------
+
+8.0.0 (2021-03-23)
+------------------
+
+7.0.1 (2021-03-22)
+------------------
+
+7.0.0 (2021-03-18)
+------------------
+* Add support for rmw_connextdds (`#1574 <https://github.com/ros2/rclcpp/issues/1574>`_)
+* node_handle must be destroyed after client_handle to prevent memory leak (`#1562 <https://github.com/ros2/rclcpp/issues/1562>`_)
+* Contributors: Andrea Sorbini, Tomoya Fujita
+
+6.3.1 (2021-02-08)
+------------------
+* Finalize rcl_handle to prevent leak (`#1528 <https://github.com/ros2/rclcpp/issues/1528>`_) (`#1529 <https://github.com/ros2/rclcpp/issues/1529>`_)
+* Fix `#1526 <https://github.com/ros2/rclcpp/issues/1526>`_. (`#1527 <https://github.com/ros2/rclcpp/issues/1527>`_)
+* Contributors: y-okumura-isp
+
+6.3.0 (2021-01-25)
+------------------
+* Fix action server deadlock (`#1285 <https://github.com/ros2/rclcpp/issues/1285>`_) (`#1313 <https://github.com/ros2/rclcpp/issues/1313>`_)
+* Contributors: Daisuke Sato
+
+6.2.0 (2021-01-08)
+------------------
+* Goal response callback compatibility shim with deprecation of old signature (`#1495 <https://github.com/ros2/rclcpp/issues/1495>`_)
+* [rclcpp_action] Add warnings (`#1405 <https://github.com/ros2/rclcpp/issues/1405>`_)
+* Contributors: Audrow Nash, Ivan Santiago Paunovic
+
+6.1.0 (2020-12-10)
+------------------
+* Update QDs to QL 1 (`#1477 <https://github.com/ros2/rclcpp/issues/1477>`_)
+* Contributors: Stephen Brawner
+
 6.0.0 (2020-11-18)
 ------------------
 * Add `take_data` to `Waitable` and `data` to `AnyExecutable` (`#1241 <https://github.com/ros2/rclcpp/issues/1241>`_)

rclcpp_action/CMakeLists.txt

@@ -13,7 +13,10 @@ if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 14)
 endif()
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic -Wnon-virtual-dtor -Woverloaded-virtual)
+  add_compile_options(
+    -Wall -Wextra -Wpedantic -Wnon-virtual-dtor -Woverloaded-virtual
+    -Wformat=2 -Wconversion -Wshadow -Wsign-conversion -Wcast-qual
+  )
 endif()
 
 set(${PROJECT_NAME}_SRCS

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 2** category.
+The package `rclcpp_action` claims to be in the **Quality Level 1** category when it is used with a **Quality Level 1** middleware.
 
 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.
 
@@ -80,13 +80,13 @@ The API is publicly available in its [ROS 2 API documentation](http://docs.ros2.
 
 The license for `rclcpp_action` is Apache 2.0, and a summary is in each source file, the type is declared in the [`package.xml`](./package.xml) manifest file, and a full copy of the license is in the [`LICENSE`](../LICENSE) file.
 
-There is an automated test which runs a linter that ensures each file has a license statement. [Here](http://build.ros2.org/view/Fpr/job/Fpr__rclcpp__ubuntu_focal_amd64/lastSuccessfulBuild/testReport/rclcpp_action/) can be found a list with the latest results of the various linters being run on the package.
+There is an automated test which runs a linter that ensures each file has a license statement. [Here](http://build.ros2.org/view/Rpr/job/Rpr__rclcpp__ubuntu_focal_amd64/lastCompletedBuild/testReport/rclcpp_action/) can be found a list with the latest results of the various linters being run on the package.
 
 ### Copyright Statements [3.iv]
 
 The copyright holders each provide a statement of copyright in each source code file in `rclcpp_action`.
 
-There is an automated test which runs a linter that ensures each file has at least one copyright statement. Latest linter result report can be seen [here](http://build.ros2.org/view/Fpr/job/Fpr__rclcpp__ubuntu_focal_amd64/lastSuccessfulBuild/testReport/rclcpp_action/copyright/).
+There is an automated test which runs a linter that ensures each file has at least one copyright statement. Latest linter result report can be seen [here](http://build.ros2.org/view/Rpr/job/Rpr__rclcpp__ubuntu_focal_amd64/lastCompletedBuild/testReport/rclcpp_action/copyright/).
 
 ## Testing [4]
 
@@ -119,11 +119,19 @@ This includes:
 
 Changes are required to make a best effort to keep or increase coverage before being accepted, but decreases are allowed if properly justified and accepted by reviewers.
 
-Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_coverage/lastSuccessfulBuild/cobertura/src_ros2_rclcpp_rclcpp_action_src/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://index.ros.org/doc/ros2/Contributing/ROS-2-On-boarding-Guide/#note-on-coverage-runs).
+Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_coverage/lastCompletedBuild/cobertura/src_ros2_rclcpp_rclcpp_action_src/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://index.ros.org/doc/ros2/Contributing/ROS-2-On-boarding-Guide/#note-on-coverage-runs).
 
 ### Performance [4.iv]
 
-It is not yet defined if this package requires performance testing and how addresses this topic.
+`rclcpp_action` follows the recommendations for performance testing of C/C++ code in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#performance), and opts to do performance analysis on each release rather than each change.
+
+The performance tests of `rclcpp_action` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/master/rclcpp_action/test/benchmark).
+
+System level performance benchmarks that cover features of `rclcpp_action` can be found at:
+* [Benchmarks](http://build.ros2.org/view/Rci/job/Rci__benchmark_ubuntu_focal_amd64/BenchmarkTable/)
+* [Performance](http://build.ros2.org/view/Rci/job/Rci__nightly-performance_ubuntu_focal_amd64/lastCompletedBuild/)
+
+Changes that introduce regressions in performance must be adequately justified in order to be accepted and merged.
 
 ### Linters and Static Analysis [4.v]
 
@@ -151,19 +159,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 2**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/action_msgs/QUALITY_DECLARATION.md).
+It is **Quality Level 1**, 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 2**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/master/rclcpp/QUALITY_DECLARATION.md).
+It is **Quality Level 1**, 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 2**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl_action/QUALITY_DECLARATION.md).
+It is **Quality Level 1**, 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 2** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details.
+This package claims to be in the **Quality Level 1** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details.

rclcpp_action/include/rclcpp_action/client.hpp

@@ -268,13 +268,107 @@ public:
   using Feedback = typename ActionT::Feedback;
   using GoalHandle = ClientGoalHandle<ActionT>;
   using WrappedResult = typename GoalHandle::WrappedResult;
-  using GoalResponseCallback = std::function<void (typename GoalHandle::SharedPtr)>;
   using FeedbackCallback = typename GoalHandle::FeedbackCallback;
   using ResultCallback = typename GoalHandle::ResultCallback;
   using CancelRequest = typename ActionT::Impl::CancelGoalService::Request;
   using CancelResponse = typename ActionT::Impl::CancelGoalService::Response;
   using CancelCallback = std::function<void (typename CancelResponse::SharedPtr)>;
 
+  /// Compatibility wrapper for `goal_response_callback`.
+  class GoalResponseCallback
+  {
+public:
+    using NewSignature = std::function<void (typename GoalHandle::SharedPtr)>;
+    using OldSignature = std::function<void (std::shared_future<typename GoalHandle::SharedPtr>)>;
+
+    GoalResponseCallback() = default;
+
+    GoalResponseCallback(std::nullptr_t) {}  // NOLINT, intentionally implicit.
+
+    // implicit constructor
+    [[deprecated(
+      "Use new goal response callback signature "
+      "`std::function<void (Client<ActionT>::GoalHandle::SharedPtr)>` "
+      "instead of the old "
+      "`std::function<void (std::shared_future<Client<ActionT>::GoalHandle::SharedPtr>)>`.\n"
+      "e.g.:\n"
+      "```cpp\n"
+      "Client<ActionT>::SendGoalOptions options;\n"
+      "options.goal_response_callback = [](Client<ActionT>::GoalHandle::SharedPtr goal) {\n"
+      "  // do something with `goal` here\n"
+      "};")]]
+    GoalResponseCallback(OldSignature old_callback)  // NOLINT, intentionally implicit.
+    : old_callback_(std::move(old_callback)) {}
+
+    GoalResponseCallback(NewSignature new_callback)  // NOLINT, intentionally implicit.
+    : new_callback_(std::move(new_callback)) {}
+
+    GoalResponseCallback &
+    operator=(OldSignature old_callback) {old_callback_ = std::move(old_callback); return *this;}
+
+    GoalResponseCallback &
+    operator=(NewSignature new_callback) {new_callback_ = std::move(new_callback); return *this;}
+
+    void
+    operator()(typename GoalHandle::SharedPtr goal_handle) const
+    {
+      if (new_callback_) {
+        new_callback_(std::move(goal_handle));
+        return;
+      }
+      if (old_callback_) {
+        throw std::runtime_error{
+                "Cannot call GoalResponseCallback(GoalHandle::SharedPtr) "
+                "if using the old goal response callback signature."};
+      }
+      throw std::bad_function_call{};
+    }
+
+    [[deprecated(
+      "Calling "
+      "`void goal_response_callback("
+      "   std::shared_future<Client<ActionT>::GoalHandle::SharedPtr> goal_handle_shared_future)`"
+      " is deprecated.")]]
+    void
+    operator()(std::shared_future<typename GoalHandle::SharedPtr> goal_handle_future) const
+    {
+      if (old_callback_) {
+        old_callback_(std::move(goal_handle_future));
+        return;
+      }
+      if (new_callback_) {
+        new_callback_(std::move(goal_handle_future).get_future().share());
+        return;
+      }
+      throw std::bad_function_call{};
+    }
+
+    explicit operator bool() const noexcept {
+      return new_callback_ || old_callback_;
+    }
+
+private:
+    friend class Client;
+    void
+    operator()(
+      typename GoalHandle::SharedPtr goal_handle,
+      std::shared_future<typename GoalHandle::SharedPtr> goal_handle_future) const
+    {
+      if (new_callback_) {
+        new_callback_(std::move(goal_handle));
+        return;
+      }
+      if (old_callback_) {
+        old_callback_(std::move(goal_handle_future));
+        return;
+      }
+      throw std::bad_function_call{};
+    }
+
+    NewSignature new_callback_;
+    OldSignature old_callback_;
+  };
+
   /// Options for sending a goal.
   /**
    * This struct is used to pass parameters to the function `async_send_goal`.
@@ -363,7 +457,7 @@ public:
         if (!goal_response->accepted) {
           promise->set_value(nullptr);
           if (options.goal_response_callback) {
-            options.goal_response_callback(nullptr);
+            options.goal_response_callback(nullptr, future);
           }
           return;
         }
@@ -379,7 +473,7 @@ public:
         }
         promise->set_value(goal_handle);
         if (options.goal_response_callback) {
-          options.goal_response_callback(goal_handle);
+          options.goal_response_callback(goal_handle, future);
         }
 
         if (options.result_callback) {

rclcpp_action/include/rclcpp_action/server.hpp

@@ -381,31 +381,31 @@ protected:
     std::weak_ptr<Server<ActionT>> weak_this = this->shared_from_this();
 
     std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state =
-      [weak_this](const GoalUUID & uuid, std::shared_ptr<void> result_message)
+      [weak_this](const GoalUUID & goal_uuid, std::shared_ptr<void> result_message)
       {
         std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock();
         if (!shared_this) {
           return;
         }
         // Send result message to anyone that asked
-        shared_this->publish_result(uuid, result_message);
+        shared_this->publish_result(goal_uuid, result_message);
         // Publish a status message any time a goal handle changes state
         shared_this->publish_status();
         // notify base so it can recalculate the expired goal timer
         shared_this->notify_goal_terminal_state();
         // Delete data now (ServerBase and rcl_action_server_t keep data until goal handle expires)
         std::lock_guard<std::mutex> lock(shared_this->goal_handles_mutex_);
-        shared_this->goal_handles_.erase(uuid);
+        shared_this->goal_handles_.erase(goal_uuid);
       };
 
     std::function<void(const GoalUUID &)> on_executing =
-      [weak_this](const GoalUUID & uuid)
+      [weak_this](const GoalUUID & goal_uuid)
       {
         std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock();
         if (!shared_this) {
           return;
         }
-        (void)uuid;
+        (void)goal_uuid;
         // Publish a status message any time a goal handle changes state
         shared_this->publish_status();
       };

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>6.0.0</version>
+  <version>8.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>

rclcpp_action/src/client.cpp

@@ -102,8 +102,9 @@ public:
 
   rclcpp::Context::SharedPtr context_;
   rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;
-  std::shared_ptr<rcl_action_client_t> client_handle{nullptr};
+  // node_handle must be destroyed after client_handle to prevent memory leak
   std::shared_ptr<rcl_node_t> node_handle{nullptr};
+  std::shared_ptr<rcl_action_client_t> client_handle{nullptr};
   rclcpp::Logger logger;
 
   using ResponseCallback = std::function<void (std::shared_ptr<void> response)>;

rclcpp_action/src/server.cpp

@@ -45,23 +45,27 @@ public:
   {
   }
 
-  // Lock everything except user callbacks
+  // Lock for action_server_
   std::recursive_mutex action_server_reentrant_mutex_;
 
-  std::shared_ptr<rcl_action_server_t> action_server_;
-
   rclcpp::Clock::SharedPtr clock_;
 
+  // Do not declare this before clock_ as this depends on clock_(see #1526)
+  std::shared_ptr<rcl_action_server_t> action_server_;
+
   size_t num_subscriptions_ = 0;
   size_t num_timers_ = 0;
   size_t num_clients_ = 0;
   size_t num_services_ = 0;
   size_t num_guard_conditions_ = 0;
 
-  bool goal_request_ready_ = false;
-  bool cancel_request_ready_ = false;
-  bool result_request_ready_ = false;
-  bool goal_expired_ = false;
+  std::atomic<bool> goal_request_ready_{false};
+  std::atomic<bool> cancel_request_ready_{false};
+  std::atomic<bool> result_request_ready_{false};
+  std::atomic<bool> goal_expired_{false};
+
+  // Lock for unordered_maps
+  std::recursive_mutex unordered_map_mutex_;
 
   // Results to be kept until the goal expires after reaching a terminal state
   std::unordered_map<GoalUUID, std::shared_ptr<void>> goal_results_;
@@ -171,29 +175,41 @@ 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_->action_server_reentrant_mutex_);
-  rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready(
-    wait_set,
-    pimpl_->action_server_.get(),
-    &pimpl_->goal_request_ready_,
-    &pimpl_->cancel_request_ready_,
-    &pimpl_->result_request_ready_,
-    &pimpl_->goal_expired_);
+  bool goal_request_ready;
+  bool cancel_request_ready;
+  bool result_request_ready;
+  bool goal_expired;
+  rcl_ret_t ret;
+  {
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+    ret = rcl_action_server_wait_set_get_entities_ready(
+      wait_set,
+      pimpl_->action_server_.get(),
+      &goal_request_ready,
+      &cancel_request_ready,
+      &result_request_ready,
+      &goal_expired);
+  }
+
+  pimpl_->goal_request_ready_ = goal_request_ready;
+  pimpl_->cancel_request_ready_ = cancel_request_ready;
+  pimpl_->result_request_ready_ = result_request_ready;
+  pimpl_->goal_expired_ = goal_expired;
 
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
 
-  return pimpl_->goal_request_ready_ ||
-         pimpl_->cancel_request_ready_ ||
-         pimpl_->result_request_ready_ ||
-         pimpl_->goal_expired_;
+  return pimpl_->goal_request_ready_.load() ||
+         pimpl_->cancel_request_ready_.load() ||
+         pimpl_->result_request_ready_.load() ||
+         pimpl_->goal_expired_.load();
 }
 
 std::shared_ptr<void>
 ServerBase::take_data()
 {
-  if (pimpl_->goal_request_ready_) {
+  if (pimpl_->goal_request_ready_.load()) {
     rcl_ret_t ret;
     rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info();
     rmw_request_id_t request_header;
@@ -212,7 +228,7 @@ ServerBase::take_data()
         ret,
         goal_info,
         request_header, message));
-  } else if (pimpl_->cancel_request_ready_) {
+  } else if (pimpl_->cancel_request_ready_.load()) {
     rcl_ret_t ret;
     rmw_request_id_t request_header;
 
@@ -229,7 +245,7 @@ ServerBase::take_data()
       std::make_shared
       <std::tuple<rcl_ret_t, std::shared_ptr<action_msgs::srv::CancelGoal::Request>,
       rmw_request_id_t>>(ret, request, request_header));
-  } else if (pimpl_->result_request_ready_) {
+  } else if (pimpl_->result_request_ready_.load()) {
     rcl_ret_t ret;
     // Get the result request message
     rmw_request_id_t request_header;
@@ -241,7 +257,7 @@ ServerBase::take_data()
     return std::static_pointer_cast<void>(
       std::make_shared<std::tuple<rcl_ret_t, std::shared_ptr<void>, rmw_request_id_t>>(
         ret, result_request, request_header));
-  } else if (pimpl_->goal_expired_) {
+  } else if (pimpl_->goal_expired_.load()) {
     return nullptr;
   } else {
     throw std::runtime_error("Taking data from action server but nothing is ready");
@@ -251,17 +267,17 @@ ServerBase::take_data()
 void
 ServerBase::execute(std::shared_ptr<void> & data)
 {
-  if (!data && !pimpl_->goal_expired_) {
+  if (!data && !pimpl_->goal_expired_.load()) {
     throw std::runtime_error("'data' is empty");
   }
 
-  if (pimpl_->goal_request_ready_) {
+  if (pimpl_->goal_request_ready_.load()) {
     execute_goal_request_received(data);
-  } else if (pimpl_->cancel_request_ready_) {
+  } else if (pimpl_->cancel_request_ready_.load()) {
     execute_cancel_request_received(data);
-  } else if (pimpl_->result_request_ready_) {
+  } else if (pimpl_->result_request_ready_.load()) {
     execute_result_request_received(data);
-  } else if (pimpl_->goal_expired_) {
+  } else if (pimpl_->goal_expired_.load()) {
     execute_check_expired_goals();
   } else {
     throw std::runtime_error("Executing action server but nothing is ready");
@@ -286,10 +302,10 @@ ServerBase::execute_goal_request_received(std::shared_ptr<void> & data)
   rmw_request_id_t request_header = std::get<2>(*shared_ptr);
   std::shared_ptr<void> message = std::get<3>(*shared_ptr);
 
-
-  std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
-
-  pimpl_->goal_request_ready_ = false;
+  bool expected = true;
+  if (!pimpl_->goal_request_ready_.compare_exchange_strong(expected, false)) {
+    return;
+  }
 
   GoalUUID uuid = get_goal_id_from_goal_request(message.get());
   convert(uuid, &goal_info);
@@ -297,10 +313,13 @@ ServerBase::execute_goal_request_received(std::shared_ptr<void> & data)
   // Call user's callback, getting the user's response and a ros message to send back
   auto response_pair = call_handle_goal_callback(uuid, message);
 
-  ret = rcl_action_send_goal_response(
-    pimpl_->action_server_.get(),
-    &request_header,
-    response_pair.second.get());
+  {
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+    ret = rcl_action_send_goal_response(
+      pimpl_->action_server_.get(),
+      &request_header,
+      response_pair.second.get());
+  }
 
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
@@ -325,7 +344,10 @@ ServerBase::execute_goal_request_received(std::shared_ptr<void> & data)
         }
       };
     rcl_action_goal_handle_t * rcl_handle;
-    rcl_handle = rcl_action_accept_new_goal(pimpl_->action_server_.get(), &goal_info);
+    {
+      std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+      rcl_handle = rcl_action_accept_new_goal(pimpl_->action_server_.get(), &goal_info);
+    }
     if (!rcl_handle) {
       throw std::runtime_error("Failed to accept new goal\n");
     }
@@ -334,7 +356,10 @@ ServerBase::execute_goal_request_received(std::shared_ptr<void> & data)
     // Copy out goal handle since action server storage disappears when it is fini'd
     *handle = *rcl_handle;
 
-    pimpl_->goal_handles_[uuid] = handle;
+    {
+      std::lock_guard<std::recursive_mutex> lock(pimpl_->unordered_map_mutex_);
+      pimpl_->goal_handles_[uuid] = handle;
+    }
 
     if (GoalResponse::ACCEPT_AND_EXECUTE == status) {
       // Change status to executing
@@ -359,7 +384,6 @@ ServerBase::execute_cancel_request_received(std::shared_ptr<void> & data)
     <std::tuple<rcl_ret_t, std::shared_ptr<action_msgs::srv::CancelGoal::Request>,
       rmw_request_id_t>>(data);
   auto ret = std::get<0>(*shared_ptr);
-  std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
   if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) {
     // Ignore take failure because connext fails if it receives a sample without valid data.
     // This happens when a client shuts down and connext receives a sample saying the client is
@@ -380,10 +404,14 @@ ServerBase::execute_cancel_request_received(std::shared_ptr<void> & data)
   // Get a list of goal info that should be attempted to be cancelled
   rcl_action_cancel_response_t cancel_response = rcl_action_get_zero_initialized_cancel_response();
 
-  ret = rcl_action_process_cancel_request(
-    pimpl_->action_server_.get(),
-    &cancel_request,
-    &cancel_response);
+  {
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+    ret = rcl_action_process_cancel_request(
+      pimpl_->action_server_.get(),
+      &cancel_request,
+      &cancel_response);
+  }
+
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
@@ -426,8 +454,12 @@ ServerBase::execute_cancel_request_received(std::shared_ptr<void> & data)
     publish_status();
   }
 
-  ret = rcl_action_send_cancel_response(
-    pimpl_->action_server_.get(), &request_header, response.get());
+  {
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+    ret = rcl_action_send_cancel_response(
+      pimpl_->action_server_.get(), &request_header, response.get());
+  }
+
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
@@ -440,7 +472,6 @@ ServerBase::execute_result_request_received(std::shared_ptr<void> & data)
   auto shared_ptr = std::static_pointer_cast
     <std::tuple<rcl_ret_t, std::shared_ptr<void>, rmw_request_id_t>>(data);
   auto ret = std::get<0>(*shared_ptr);
-  std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
   if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) {
     // Ignore take failure because connext fails if it receives a sample without valid data.
     // This happens when a client shuts down and connext receives a sample saying the client is
@@ -460,7 +491,10 @@ ServerBase::execute_result_request_received(std::shared_ptr<void> & data)
   rcl_action_goal_info_t goal_info;
   convert(uuid, &goal_info);
   bool goal_exists;
-  goal_exists = rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info);
+  {
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+    goal_exists = rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info);
+  }
   if (!goal_exists) {
     // Goal does not exists
     result_response = create_result_response(action_msgs::msg::GoalStatus::STATUS_UNKNOWN);
@@ -474,13 +508,15 @@ ServerBase::execute_result_request_received(std::shared_ptr<void> & data)
 
   if (result_response) {
     // Send the result now
-    rcl_ret_t ret = rcl_action_send_result_response(
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+    rcl_ret_t rcl_ret = rcl_action_send_result_response(
       pimpl_->action_server_.get(), &request_header, result_response.get());
-    if (RCL_RET_OK != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret);
+    if (RCL_RET_OK != rcl_ret) {
+      rclcpp::exceptions::throw_from_rcl_error(rcl_ret);
     }
   } else {
     // Store the request so it can be responded to later
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->unordered_map_mutex_);
     pimpl_->result_requests_[uuid].push_back(request_header);
   }
   data.reset();
@@ -495,9 +531,11 @@ 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_->action_server_reentrant_mutex_);
     rcl_ret_t ret;
-    ret = rcl_action_expire_goals(pimpl_->action_server_.get(), expired_goals, 1, &num_expired);
+    {
+      std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+      ret = rcl_action_expire_goals(pimpl_->action_server_.get(), expired_goals, 1, &num_expired);
+    }
     if (RCL_RET_OK != ret) {
       rclcpp::exceptions::throw_from_rcl_error(ret);
     } else if (num_expired) {
@@ -505,6 +543,7 @@ ServerBase::execute_check_expired_goals()
       GoalUUID uuid;
       convert(expired_goals[0], &uuid);
       RCLCPP_DEBUG(pimpl_->logger_, "Expired goal %s", to_string(uuid).c_str());
+      std::lock_guard<std::recursive_mutex> lock(pimpl_->unordered_map_mutex_);
       pimpl_->goal_results_.erase(uuid);
       pimpl_->result_requests_.erase(uuid);
       pimpl_->goal_handles_.erase(uuid);
@@ -577,20 +616,26 @@ 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_->action_server_reentrant_mutex_);
   bool goal_exists;
-  goal_exists = rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info);
+  {
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
+    goal_exists = rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info);
+  }
 
   if (!goal_exists) {
     throw std::runtime_error("Asked to publish result for goal that does not exist");
   }
 
-  pimpl_->goal_results_[uuid] = result_msg;
+  {
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->unordered_map_mutex_);
+    pimpl_->goal_results_[uuid] = result_msg;
+  }
 
   // if there are clients who already asked for the result, send it to them
   auto iter = pimpl_->result_requests_.find(uuid);
   if (iter != pimpl_->result_requests_.end()) {
     for (auto & request_header : iter->second) {
+      std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
       rcl_ret_t ret = rcl_action_send_result_response(
         pimpl_->action_server_.get(), &request_header, result_msg.get());
       if (RCL_RET_OK != ret) {

rclcpp_action/test/benchmark/benchmark_action_client.cpp

@@ -81,7 +81,7 @@ public:
 
     // Should be checked by the server above
     assert(goal->order > 0);
-    result->sequence.resize(goal->order);
+    result->sequence.resize(static_cast<size_t>(goal->order));
     result->sequence[0] = 0;
     if (goal->order == 1) {
       current_goal_handle->succeed(result);
@@ -92,7 +92,7 @@ public:
       current_goal_handle->succeed(result);
       return;
     }
-    for (int i = 2; i < goal->order; ++i) {
+    for (size_t i = 2; i < static_cast<size_t>(goal->order); ++i) {
       result->sequence[i] =
         result->sequence[i - 1] + result->sequence[i - 2];
     }
@@ -310,8 +310,8 @@ BENCHMARK_F(ActionClientPerformanceTest, async_cancel_goal)(benchmark::State & s
     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) {
+    using CancelActionResponse = test_msgs::action::Fibonacci::Impl::CancelGoalService::Response;
+    if (CancelActionResponse::ERROR_NONE != cancel_response->return_code) {
       state.SkipWithError("Cancel request did not succeed");
       break;
     }
@@ -345,8 +345,8 @@ BENCHMARK_F(ActionClientPerformanceTest, async_cancel_all_goals)(benchmark::Stat
     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) {
+    using CancelActionResponse = test_msgs::action::Fibonacci::Impl::CancelGoalService::Response;
+    if (CancelActionResponse::ERROR_NONE != cancel_response->return_code) {
       state.SkipWithError("Cancel request did not succeed");
       break;
     }

rclcpp_action/test/benchmark/benchmark_action_server.cpp

@@ -187,8 +187,8 @@ BENCHMARK_F(ActionServerPerformanceTest, action_server_cancel_goal)(benchmark::S
 
     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) {
+    using CancelActionResponse = test_msgs::action::Fibonacci::Impl::CancelGoalService::Response;
+    if (CancelActionResponse::ERROR_NONE != cancel_response->return_code) {
       state.SkipWithError("Cancel request did not succeed");
       break;
     }
@@ -247,12 +247,12 @@ BENCHMARK_F(ActionServerPerformanceTest, action_server_set_success)(benchmark::S
   // 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>();
+      auto action_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);
+        action_result->sequence.push_back(i);
       }
-      return result;
+      return action_result;
     } ();
 
   reset_heap_counters();
@@ -291,12 +291,12 @@ BENCHMARK_F(ActionServerPerformanceTest, action_server_abort)(benchmark::State &
 
   // Capturing with & because MSVC and Clang disagree about how to capture goal_order
   const auto result = [&]() {
-      auto result = std::make_shared<Fibonacci::Result>();
+      auto action_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);
+        action_result->sequence.push_back(i);
       }
-      return result;
+      return action_result;
     } ();
 
   reset_heap_counters();

rclcpp_action/test/test_client.cpp

@@ -132,7 +132,7 @@ protected:
             feedback_message.feedback.sequence.push_back(1);
             feedback_publisher->publish(feedback_message);
             client_executor.spin_once();
-            for (int i = 1; i < goal_request->goal.order; ++i) {
+            for (size_t i = 1; i < static_cast<size_t>(goal_request->goal.order); ++i) {
               feedback_message.feedback.sequence.push_back(
                 feedback_message.feedback.sequence[i] +
                 feedback_message.feedback.sequence[i - 1]);
@@ -536,6 +536,64 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_goal_response_callback_wait
   }
 }
 
+TEST_F(TestClientAgainstServer, async_send_goal_with_deprecated_goal_response_callback)
+{
+  auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+  ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
+
+  bool goal_response_received = false;
+  auto send_goal_ops = rclcpp_action::Client<ActionType>::SendGoalOptions();
+
+#if !defined(_WIN32)
+# pragma GCC diagnostic push
+# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#else
+# pragma warning(push)
+# pragma warning(disable: 4996)
+#endif
+  send_goal_ops.goal_response_callback =
+    [&goal_response_received]
+      (std::shared_future<typename ActionGoalHandle::SharedPtr> goal_future)
+    {
+      if (goal_future.get()) {
+        goal_response_received = true;
+      }
+    };
+#if !defined(_WIN32)
+# pragma GCC diagnostic pop
+#else
+# pragma warning(pop)
+#endif
+
+  {
+    ActionGoal bad_goal;
+    bad_goal.order = -1;
+    auto future_goal_handle = action_client->async_send_goal(bad_goal, send_goal_ops);
+    dual_spin_until_future_complete(future_goal_handle);
+    auto goal_handle = future_goal_handle.get();
+    EXPECT_FALSE(goal_response_received);
+    EXPECT_EQ(nullptr, goal_handle);
+  }
+
+  {
+    ActionGoal goal;
+    goal.order = 4;
+    auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops);
+    dual_spin_until_future_complete(future_goal_handle);
+    auto goal_handle = future_goal_handle.get();
+    EXPECT_TRUE(goal_response_received);
+    EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
+    EXPECT_FALSE(goal_handle->is_feedback_aware());
+    EXPECT_FALSE(goal_handle->is_result_aware());
+    auto future_result = action_client->async_get_result(goal_handle);
+    EXPECT_TRUE(goal_handle->is_result_aware());
+    dual_spin_until_future_complete(future_result);
+    auto wrapped_result = future_result.get();
+    ASSERT_EQ(5u, wrapped_result.result->sequence.size());
+    EXPECT_EQ(3, wrapped_result.result->sequence.back());
+  }
+}
+
 TEST_F(TestClientAgainstServer, async_send_goal_with_feedback_callback_wait_for_result)
 {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);

rclcpp_action/test/test_server.cpp

@@ -29,10 +29,6 @@
 #include "rclcpp_action/server.hpp"
 #include "./mocking_utils/patch.hpp"
 
-// Note: This is a long running test with rmw_connext_cpp, if you change this file, please check
-// that this test can complete fully, or adjust the timeout as necessary.
-// See https://github.com/ros2/rmw_connext/issues/325 for resolution
-
 using Fibonacci = test_msgs::action::Fibonacci;
 using CancelResponse = typename Fibonacci::Impl::CancelGoalService::Response;
 using GoalUUID = rclcpp_action::GoalUUID;
@@ -1221,3 +1217,92 @@ TEST_F(TestCancelRequestServer, publish_status_send_cancel_response_errors)
 
   EXPECT_THROW(SendClientCancelRequest(), std::runtime_error);
 }
+
+class TestDeadlockServer : public TestServer
+{
+public:
+  void SetUp()
+  {
+    node_ = std::make_shared<rclcpp::Node>("goal_request", "/rclcpp_action/goal_request");
+    uuid1_ = {{1, 2, 3, 4, 5, 6, 70, 80, 9, 1, 11, 120, 13, 140, 15, 160}};
+    uuid2_ = {{2, 2, 3, 4, 5, 6, 70, 80, 9, 1, 11, 120, 13, 140, 15, 160}};
+    action_server_ = rclcpp_action::create_server<Fibonacci>(
+      node_, "fibonacci",
+      [this](const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>) {
+        // instead of making a deadlock, check if it can acquire the lock in a second
+        std::unique_lock<std::recursive_timed_mutex> lock(server_mutex_, std::defer_lock);
+        this->TryLockFor(lock, std::chrono::milliseconds(1000));
+        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+      },
+      [this](std::shared_ptr<GoalHandle>) {
+        // instead of making a deadlock, check if it can acquire the lock in a second
+        std::unique_lock<std::recursive_timed_mutex> lock(server_mutex_, std::defer_lock);
+        this->TryLockFor(lock, std::chrono::milliseconds(1000));
+        return rclcpp_action::CancelResponse::ACCEPT;
+      },
+      [this](std::shared_ptr<GoalHandle> handle) {
+        // instead of making a deadlock, check if it can acquire the lock in a second
+        std::unique_lock<std::recursive_timed_mutex> lock(server_mutex_, std::defer_lock);
+        this->TryLockFor(lock, std::chrono::milliseconds(1000));
+        goal_handle_ = handle;
+      });
+  }
+
+  void GoalSucceeded()
+  {
+    std::lock_guard<std::recursive_timed_mutex> lock(server_mutex_);
+    rclcpp::sleep_for(std::chrono::milliseconds(100));
+    auto result = std::make_shared<Fibonacci::Result>();
+    result->sequence = {5, 8, 13, 21};
+    goal_handle_->succeed(result);
+  }
+
+  void GoalCanceled()
+  {
+    std::lock_guard<std::recursive_timed_mutex> lock(server_mutex_);
+    rclcpp::sleep_for(std::chrono::milliseconds(100));
+    auto result = std::make_shared<Fibonacci::Result>();
+    goal_handle_->canceled(result);
+  }
+
+  void TryLockFor(
+    std::unique_lock<std::recursive_timed_mutex> & lock,
+    std::chrono::milliseconds timeout
+  )
+  {
+    ASSERT_TRUE(lock.try_lock_for(timeout));
+  }
+
+protected:
+  std::recursive_timed_mutex server_mutex_;
+  GoalUUID uuid1_, uuid2_;
+  std::shared_ptr<rclcpp::Node> node_;
+  std::shared_ptr<rclcpp_action::Server<Fibonacci>> action_server_;
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+  std::shared_ptr<GoalHandle> goal_handle_;
+};
+
+TEST_F(TestDeadlockServer, deadlock_while_succeed)
+{
+  send_goal_request(node_, uuid1_);
+  // this will lock wrapper's mutex and intentionally wait 100ms for calling succeed
+  // to try to acquire the lock of rclcpp_action mutex
+  std::thread t(&TestDeadlockServer::GoalSucceeded, this);
+  // after the wrapper's mutex is locked and before succeed is called
+  rclcpp::sleep_for(std::chrono::milliseconds(50));
+  // call next goal request to intentionally reproduce deadlock
+  // this first locks rclcpp_action mutex and then call callback to lock wrapper's mutex
+  send_goal_request(node_, uuid2_);
+  t.join();
+}
+
+TEST_F(TestDeadlockServer, deadlock_while_canceled)
+{
+  send_goal_request(node_, uuid1_);
+  send_cancel_request(node_, uuid1_);
+  std::thread t(&TestDeadlockServer::GoalCanceled, this);
+  rclcpp::sleep_for(std::chrono::milliseconds(50));
+  send_goal_request(node_, uuid2_);  // deadlock here
+  t.join();
+}