Force segfault to test ament_cmake_test code

* correct use of move semantics

Signed-off-by: Knese Karsten <karsten@openrobotics.org>

* more tests

Signed-off-by: Knese Karsten <karsten@openrobotics.org>

* make error message more exact

Signed-off-by: Karsten Knese <karsten@openrobotics.org>

* use std::exchange

Signed-off-by: Karsten Knese <karsten@openrobotics.org>

* fix typo

Signed-off-by: Karsten Knese <karsten@openrobotics.org>

.github/ISSUE_TEMPLATE.md

@@ -0,0 +1,45 @@
+<!--
+For general questions, please ask on ROS answers: https://answers.ros.org, make sure to include at least the `ros2` tag and the rosdistro version you are running, e.g. `ardent`.
+For general design discussions, please post on discourse: https://discourse.ros.org/c/ng-ros
+Not sure if this is the right repository? Open an issue on https://github.com/ros2/ros2/issues
+For Bug report or feature requests, please fill out the relevant category below
+-->
+
+## Bug report
+
+**Required Info:**
+
+- Operating System:
+  - <!-- OS and version (e.g. Windows 10, Ubuntu 16.04...) -->
+- Installation type:
+  - <!-- binaries or from source  -->
+- Version or commit hash:
+  - <!-- Output of git rev-parse HEAD, release version, or repos file  -->
+- DDS implementation:
+  - <!-- rmw_implementation used (e.g. Fast-RTPS, RTI Connext, etc -->
+- Client library (if applicable):
+  - <!-- e.g. rclcpp, rclpy, or N/A -->
+
+#### Steps to reproduce issue
+<!-- Detailed instructions on how to reliably reproduce this issue http://sscce.org/
+``` code that can be copy-pasted is preferred ``` -->
+```
+
+```
+
+#### Expected behavior
+
+#### Actual behavior
+
+#### Additional information
+
+<!-- If you are reporting a bug delete everything below
+     If you are requesting a feature deleted everything above this line -->
+----
+## Feature request
+
+#### Feature description
+<!-- Description in a few sentences what the feature consists of and what problem it will solve -->
+
+#### Implementation considerations
+<!-- Relevant information on how the feature could be implemented and pros and cons of the different solutions -->

.gitignore

@@ -0,0 +1 @@
+.DS_Store

CONTRIBUTING.md

@@ -11,3 +11,8 @@ be under the Apache 2 License, as dictated by that
    the terms of any separate license agreement you may have executed
    with Licensor regarding such Contributions.
 ~~~
+
+Contributors must sign-off each commit by adding a `Signed-off-by: ...`
+line to commit messages to certify that they have the right to submit
+the code they are contributing to the project according to the
+[Developer Certificate of Origin (DCO)](https://developercertificate.org/).

README.md

@@ -0,0 +1,17 @@
+# rclcpp
+
+This repository contains the source code for the ROS Client Library for C++ package, included with a standard install of any ROS 2 distro.
+
+rclcpp provides the standard C++ API for interacting with ROS 2.
+
+## Usage
+
+`#include "rclcpp/rclcpp.hpp"` allows use of the most common elements of the ROS 2 system.
+
+Visit the [rclcpp API documentation](http://docs.ros2.org/eloquent/api/rclcpp/) for a complete list of its main components.
+
+### Examples
+
+The ROS 2 tutorials [Writing a simple publisher and subscriber](https://index.ros.org/doc/ros2/Tutorials/Writing-A-Simple-Cpp-Publisher-And-Subscriber/)
+and [Writing a simple service and client](https://index.ros.org/doc/ros2/Tutorials/Writing-A-Simple-Cpp-Service-And-Client/)
+contain some examples of rclcpp APIs in use.

rclcpp/.gitignore

@@ -0,0 +1 @@
+doc_output

rclcpp/CHANGELOG.rst

@@ -0,0 +1,258 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package rclcpp
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.8.3 (2019-11-19)
+------------------
+
+0.8.2 (2019-11-18)
+------------------
+* Updated tracing logic to match changes in rclcpp's intra-process system (`#918 <https://github.com/ros2/rclcpp/issues/918>`_)
+* Fixed a bug that prevented the ``shutdown_on_sigint`` option to not work correctly (`#850 <https://github.com/ros2/rclcpp/issues/850>`_)
+* Added support for STREAM logging macros (`#926 <https://github.com/ros2/rclcpp/issues/926>`_)
+* Relaxed multithreaded test constraint (`#907 <https://github.com/ros2/rclcpp/issues/907>`_)
+* Contributors: Anas Abou Allaban, Christophe Bedard, Dirk Thomas, alexfneves
+
+0.8.1 (2019-10-23)
+------------------
+* De-flake tests for rmw_connext (`#899 <https://github.com/ros2/rclcpp/issues/899>`_)
+* rename return functions for loaned messages (`#896 <https://github.com/ros2/rclcpp/issues/896>`_)
+* Enable throttling logs (`#879 <https://github.com/ros2/rclcpp/issues/879>`_)
+* New Intra-Process Communication (`#778 <https://github.com/ros2/rclcpp/issues/778>`_)
+* Instrumentation update (`#789 <https://github.com/ros2/rclcpp/issues/789>`_)
+* Zero copy api (`#864 <https://github.com/ros2/rclcpp/issues/864>`_)
+* Drop rclcpp remove_ros_arguments_null test case. (`#894 <https://github.com/ros2/rclcpp/issues/894>`_)
+* add mechanism to pass rmw impl specific payloads during pub/sub creation (`#882 <https://github.com/ros2/rclcpp/issues/882>`_)
+* make get_actual_qos return a rclcpp::QoS (`#883 <https://github.com/ros2/rclcpp/issues/883>`_)
+* Fix Compiler Warning (`#881 <https://github.com/ros2/rclcpp/issues/881>`_)
+* Add callback handler for use_sim_time parameter `#802 <https://github.com/ros2/rclcpp/issues/802>`_ (`#875 <https://github.com/ros2/rclcpp/issues/875>`_)
+* Contributors: Alberto Soragna, Brian Marchi, Hunter L. Allen, Ingo Lütkebohle, Karsten Knese, Michael Carroll, Michel Hidalgo, William Woodall
+
+0.8.0 (2019-09-26)
+------------------
+* clean up publisher and subscription creation logic (`#867 <https://github.com/ros2/rclcpp/issues/867>`_)
+* Take parameter overrides provided through the CLI. (`#865 <https://github.com/ros2/rclcpp/issues/865>`_)
+* add more context to exception message (`#858 <https://github.com/ros2/rclcpp/issues/858>`_)
+* remove features and related code which were deprecated in dashing (`#852 <https://github.com/ros2/rclcpp/issues/852>`_)
+* check valid timer handler 1st to reduce the time window for scan. (`#841 <https://github.com/ros2/rclcpp/issues/841>`_)
+* Add throwing parameter name if parameter is not set (`#833 <https://github.com/ros2/rclcpp/issues/833>`_)
+* Fix typo in deprecated warning. (`#848 <https://github.com/ros2/rclcpp/issues/848>`_)
+* Fail on invalid and unknown ROS specific arguments (`#842 <https://github.com/ros2/rclcpp/issues/842>`_)
+* Force explicit --ros-args in NodeOptions::arguments(). (`#845 <https://github.com/ros2/rclcpp/issues/845>`_)
+* Use of -r/--remap flags where appropriate. (`#834 <https://github.com/ros2/rclcpp/issues/834>`_)
+* Fix hang with timers in MultiThreadedExecutor (`#835 <https://github.com/ros2/rclcpp/issues/835>`_) (`#836 <https://github.com/ros2/rclcpp/issues/836>`_)
+* add mutex in add/remove_node and wait_for_work to protect concurrent use/change of memory_strategy\_ (`#837 <https://github.com/ros2/rclcpp/issues/837>`_)
+* Crash in callback group pointer vector iterator (`#814 <https://github.com/ros2/rclcpp/issues/814>`_)
+* Wrap documentation examples in code blocks (`#830 <https://github.com/ros2/rclcpp/issues/830>`_)
+* add callback group as member variable and constructor arg (`#811 <https://github.com/ros2/rclcpp/issues/811>`_)
+* Fix get_node_interfaces functions taking a pointer (`#821 <https://github.com/ros2/rclcpp/issues/821>`_)
+* Delete unnecessary call for get_node_by_group (`#823 <https://github.com/ros2/rclcpp/issues/823>`_)
+* Allow passing logger by const ref (`#820 <https://github.com/ros2/rclcpp/issues/820>`_)
+* Explain return value of spin_until_future_complete (`#792 <https://github.com/ros2/rclcpp/issues/792>`_)
+* Adapt to '--ros-args ... [--]'-based ROS args extraction (`#816 <https://github.com/ros2/rclcpp/issues/816>`_)
+* Add line break after first open paren in multiline function call (`#785 <https://github.com/ros2/rclcpp/issues/785>`_)
+* remove mock msgs from rclcpp (`#800 <https://github.com/ros2/rclcpp/issues/800>`_)
+* Make TimeSource ignore use_sim_time events coming from other nodes. (`#799 <https://github.com/ros2/rclcpp/issues/799>`_)
+* Allow registering multiple on_parameters_set_callback (`#772 <https://github.com/ros2/rclcpp/issues/772>`_)
+* Add free function for creating service clients (`#788 <https://github.com/ros2/rclcpp/issues/788>`_)
+* Include missing rcl headers in use. (`#782 <https://github.com/ros2/rclcpp/issues/782>`_)
+* Switch the NodeParameters lock to recursive. (`#781 <https://github.com/ros2/rclcpp/issues/781>`_)
+* changed on_parameter_event qos profile to rmw_qos_profile_parameter_events (`#774 <https://github.com/ros2/rclcpp/issues/774>`_)
+* Adding a factory method to create a Duration from seconds (`#567 <https://github.com/ros2/rclcpp/issues/567>`_)
+* Fix a comparison with a sign mismatch (`#771 <https://github.com/ros2/rclcpp/issues/771>`_)
+* delete superfluous spaces (`#770 <https://github.com/ros2/rclcpp/issues/770>`_)
+* Use params from node '/\*\*' from parameter YAML file (`#762 <https://github.com/ros2/rclcpp/issues/762>`_)
+* Add ignore override argument to declare parameter (`#767 <https://github.com/ros2/rclcpp/issues/767>`_)
+* use default parameter descriptor in parameters interface (`#765 <https://github.com/ros2/rclcpp/issues/765>`_)
+* Added support for const member functions (`#763 <https://github.com/ros2/rclcpp/issues/763>`_)
+* add get_actual_qos() feature to subscriptions (`#754 <https://github.com/ros2/rclcpp/issues/754>`_)
+* Ignore parameters overrides in set parameter methods when allowing undeclared parameters (`#756 <https://github.com/ros2/rclcpp/issues/756>`_)
+* Add rclcpp::create_timer() (`#757 <https://github.com/ros2/rclcpp/issues/757>`_)
+* checking origin of intra-process msg before taking them (`#753 <https://github.com/ros2/rclcpp/issues/753>`_)
+* Contributors: Alberto Soragna, Carl Delsey, Chris Lalancette, Dan Rose, Dirk Thomas, Esteve Fernandez, Guillaume Autran, Jacob Perron, Karsten Knese, Luca Della Vedova, M. M, Michel Hidalgo, Scott K Logan, Shane Loretz, Todd Malsbary, William Woodall, bpwilcox, fujitatomoya, ivanpauno
+
+0.7.5 (2019-05-30)
+------------------
+* Avoid 'Intra process message no longer being stored when trying to handle it' warning (`#749 <https://github.com/ros2/rclcpp/issues/749>`_)
+* Contributors: ivanpauno
+
+0.7.4 (2019-05-29)
+------------------
+* Rename parameter options (`#745 <https://github.com/ros2/rclcpp/issues/745>`_)
+* Bionic use of strerror_r (`#742 <https://github.com/ros2/rclcpp/issues/742>`_)
+* Enforce parameter ranges (`#735 <https://github.com/ros2/rclcpp/issues/735>`_)
+* removed not used parameter client (`#740 <https://github.com/ros2/rclcpp/issues/740>`_)
+* ensure removal of guard conditions of expired nodes from memory strategy (`#741 <https://github.com/ros2/rclcpp/issues/741>`_)
+* Fix typo in log warning message (`#737 <https://github.com/ros2/rclcpp/issues/737>`_)
+* Throw nice errors when creating a publisher with intraprocess communication and incompatible qos policy (`#729 <https://github.com/ros2/rclcpp/issues/729>`_)
+* Contributors: Alberto Soragna, Dirk Thomas, Jacob Perron, William Woodall, ivanpauno, roderick-koehle
+
+0.7.3 (2019-05-20)
+------------------
+* Fixed misspelling, volitile -> volatile (`#724 <https://github.com/ros2/rclcpp/issues/724>`_), and then fixed that since it is a C++ keyword to be ``durability_volatile`` (`#725 <https://github.com/ros2/rclcpp/issues/725>`_)
+* Fixed a clang warning (`#723 <https://github.com/ros2/rclcpp/issues/723>`_)
+* Added ``on_parameter_event`` static method to the ``AsyncParametersClient`` (`#688 <https://github.com/ros2/rclcpp/issues/688>`_)
+* Added a guard against ``ParameterNotDeclaredException`` throwing from within the parameter service callbacks. (`#718 <https://github.com/ros2/rclcpp/issues/718>`_)
+* Added missing template functionality to lifecycle_node. (`#707 <https://github.com/ros2/rclcpp/issues/707>`_)
+* Fixed heap-use-after-free and memory leaks reported from ``test_node.cpp`` (`#719 <https://github.com/ros2/rclcpp/issues/719>`_)
+* Contributors: Alberto Soragna, Dirk Thomas, Emerson Knapp, Jacob Perron, Michael Jeronimo, Prajakta Gokhale
+
+0.7.2 (2019-05-08)
+------------------
+* Added new way to specify QoS settings for publishers and subscriptions. (`#713 <https://github.com/ros2/rclcpp/issues/713>`_)
+  * The new way requires that you specify a history depth when creating a publisher or subscription.
+  * In the past it was possible to create one without specifying any history depth, but these signatures have been deprecated.
+* Deprecated ``shared_ptr`` and raw pointer versions of ``Publisher<T>::publish()``. (`#709 <https://github.com/ros2/rclcpp/issues/709>`_)
+* Implemented API to set callbacks for liveliness and deadline QoS events for publishers and subscriptions. (`#695 <https://github.com/ros2/rclcpp/issues/695>`_)
+* Fixed a segmentation fault when publishing a parameter event when they ought to be disabled. (`#714 <https://github.com/ros2/rclcpp/issues/714>`_)
+* Changes required for upcoming pre-allocation API. (`#711 <https://github.com/ros2/rclcpp/issues/711>`_)
+* Changed ``Node::get_node_names()`` to return the full node names rather than just the base name. (`#698 <https://github.com/ros2/rclcpp/issues/698>`_)
+* Remove logic made redundant by the `ros2/rcl#255 <https://github.com/ros2/rcl/issues/255>`_ pull request. (`#712 <https://github.com/ros2/rclcpp/issues/712>`_)
+* Various improvements for ``rclcpp::Clock``. (`#696 <https://github.com/ros2/rclcpp/issues/696>`_)
+  * Fixed uninitialized bool in ``clock.cpp``.
+  * Fixed up includes of ``clock.hpp/cpp``.
+  * Added documentation for exceptions to ``clock.hpp``.
+  * Adjusted function signature of getters of ``clock.hpp/cpp``.
+  * Removed raw pointers to ``Clock::create_jump_callback``.
+  * Removed unnecessary ``rclcpp`` namespace reference from ``clock.cpp``.
+  * Changed exception to ``bad_alloc`` on ``JumpHandler`` allocation failure.
+  * Fixed missing ``nullptr`` check in ``Clock::on_time_jump``.
+  * Added ``JumpHandler::callback`` types.
+  * Added warning for lifetime of Clock and JumpHandler
+* Fixed bug left over from the `pull request #495 <https://github.com/ros2/rclcpp/pull/495>`_. (`#708 <https://github.com/ros2/rclcpp/issues/708>`_)
+* Changed the ``IntraProcessManager`` to be capable of storing ``shared_ptr<const T>`` in addition to ``unique_ptr<T>``. (`#690 <https://github.com/ros2/rclcpp/issues/690>`_)
+* Contributors: Alberto Soragna, Dima Dorezyuk, M. M, Michael Carroll, Michael Jeronimo, Tully Foote, William Woodall, ivanpauno, jhdcs
+
+0.7.1 (2019-04-26)
+------------------
+* Added read only parameters. (`#495 <https://github.com/ros2/rclcpp/issues/495>`_)
+* Fixed a concurrency problem in the multithreaded executor. (`#703 <https://github.com/ros2/rclcpp/issues/703>`_)
+* Fixup utilities. (`#692 <https://github.com/ros2/rclcpp/issues/692>`_)
+* Added method to read timer cancellation. (`#697 <https://github.com/ros2/rclcpp/issues/697>`_)
+* Added Exception Generator function for implementing "from_rcl_error". (`#678 <https://github.com/ros2/rclcpp/issues/678>`_)
+* Updated initialization of rmw_qos_profile_t struct instances. (`#691 <https://github.com/ros2/rclcpp/issues/691>`_)
+* Removed the const value from the logger before comparison. (`#680 <https://github.com/ros2/rclcpp/issues/680>`_)
+* Contributors: Devin Bonnie, Dima Dorezyuk, Guillaume Autran, M. M, Shane Loretz, Víctor Mayoral Vilches, William Woodall, jhdcs
+
+0.7.0 (2019-04-14)
+------------------
+* Added Options-struct interfaces for creating publishers/subscribers (pre-QoS, standalone). (`#673 <https://github.com/ros2/rclcpp/issues/673>`_)
+* Replaced strncpy with memcpy. (`#684 <https://github.com/ros2/rclcpp/issues/684>`_)
+* Replaced const char * with a std::array<char, TOPIC_NAME_LENGTH> as the key of IPM IDTopicMap. (`#671 <https://github.com/ros2/rclcpp/issues/671>`_)
+* Refactored SignalHandler logger to avoid race during destruction. (`#682 <https://github.com/ros2/rclcpp/issues/682>`_)
+* Introduce rclcpp_components to implement composition. (`#665 <https://github.com/ros2/rclcpp/issues/665>`_)
+* Added QoS policy check when configuring intraprocess, skip interprocess publish when possible. (`#674 <https://github.com/ros2/rclcpp/issues/674>`_)
+* Updated to use do { .. } while(0) around content of logging macros. (`#681 <https://github.com/ros2/rclcpp/issues/681>`_)
+* Added function to get publisher's actual QoS settings. (`#667 <https://github.com/ros2/rclcpp/issues/667>`_)
+* Updated to avoid race that triggers timer too often. (`#621 <https://github.com/ros2/rclcpp/issues/621>`_)
+* Exposed get_fully_qualified_name in NodeBase API. (`#662 <https://github.com/ros2/rclcpp/issues/662>`_)
+* Updated to use ament_target_dependencies where possible. (`#659 <https://github.com/ros2/rclcpp/issues/659>`_)
+* Fixed wait for service memory leak bug. (`#656 <https://github.com/ros2/rclcpp/issues/656>`_)
+* Fixed test_time_source test. (`#639 <https://github.com/ros2/rclcpp/issues/639>`_)
+* Fixed hard-coded duration type representation so int64_t isn't assumed. (`#648 <https://github.com/ros2/rclcpp/issues/648>`_)
+* Fixed cppcheck warning. (`#646 <https://github.com/ros2/rclcpp/issues/646>`_)
+* Added count matching api and intra-process subscriber count. (`#628 <https://github.com/ros2/rclcpp/issues/628>`_)
+* Added Sub Node alternative. (`#581 <https://github.com/ros2/rclcpp/issues/581>`_)
+* Replaced 'auto' with 'const auto &'. (`#630 <https://github.com/ros2/rclcpp/issues/630>`_)
+* Set Parameter Event Publisher settings. `#591 <https://github.com/ros2/rclcpp/issues/591>`_ (`#614 <https://github.com/ros2/rclcpp/issues/614>`_)
+* Replaced node constructor arguments with NodeOptions. (`#622 <https://github.com/ros2/rclcpp/issues/622>`_)
+* Updated to pass context to wait set (`#617 <https://github.com/ros2/rclcpp/issues/617>`_)
+* Added API to get parameters in a map. (`#575 <https://github.com/ros2/rclcpp/issues/575>`_)
+* Updated Bind usage since it is is no longer in std::__1. (`#618 <https://github.com/ros2/rclcpp/issues/618>`_)
+* Fixed errors from uncrustify v0.68. (`#613 <https://github.com/ros2/rclcpp/issues/613>`_)
+* Added new constructors for SyncParameterClient. (`#612 <https://github.com/ros2/rclcpp/issues/612>`_)
+* Contributors: Alberto Soragna, Chris Lalancette, Dirk Thomas, Emerson Knapp, Francisco Martín Rico, Jacob Perron, Marko Durkovic, Michael Carroll, Peter Baughman, Shane Loretz, Wei Liu, William Woodall, Yutaka Kondo, ivanpauno, kuzai, rarvolt
+
+0.6.2 (2018-12-13)
+------------------
+* Updated to use signal safe synchronization with platform specific semaphores (`#607 <https://github.com/ros2/rclcpp/issues/607>`_)
+* Resolved startup race condition for sim time (`#608 <https://github.com/ros2/rclcpp/issues/608>`_)
+  Resolves `#595 <https://github.com/ros2/rclcpp/issues/595>`_
+* Contributors: Tully Foote, William Woodall
+
+0.6.1 (2018-12-07)
+------------------
+* Added wait_for_action_server() for action clients (`#598 <https://github.com/ros2/rclcpp/issues/598>`_)
+* Added node path and time stamp to parameter event message (`#584 <https://github.com/ros2/rclcpp/issues/584>`_)
+* Updated to allow removing a waitable (`#597 <https://github.com/ros2/rclcpp/issues/597>`_)
+* Refactored init to allow for non-global init (`#587 <https://github.com/ros2/rclcpp/issues/587>`_)
+* Fixed wrong use of constructor and hanging test (`#596 <https://github.com/ros2/rclcpp/issues/596>`_)
+* Added class Waitable (`#589 <https://github.com/ros2/rclcpp/issues/589>`_)
+* Updated rcl_wait_set_add\_* calls (`#586 <https://github.com/ros2/rclcpp/issues/586>`_)
+* Contributors: Dirk Thomas, Jacob Perron, Shane Loretz, William Woodall, bpwilcox
+
+0.6.0 (2018-11-19)
+------------------
+* Updated to use new error handling API from rcutils (`#577 <https://github.com/ros2/rclcpp/issues/577>`_)
+* Added a warning when publishing if publisher is not active (`#574 <https://github.com/ros2/rclcpp/issues/574>`_)
+* Added logging macro signature that accepts std::string (`#573 <https://github.com/ros2/rclcpp/issues/573>`_)
+* Added virtual destructors to classes with virtual functions. (`#566 <https://github.com/ros2/rclcpp/issues/566>`_)
+* Added semicolons to all RCLCPP and RCUTILS macros. (`#565 <https://github.com/ros2/rclcpp/issues/565>`_)
+* Removed std::binary_function usage (`#561 <https://github.com/ros2/rclcpp/issues/561>`_)
+* Updated to avoid auto-activating ROS time if clock topic is being published (`#559 <https://github.com/ros2/rclcpp/issues/559>`_)
+* Fixed cpplint on xenial (`#556 <https://github.com/ros2/rclcpp/issues/556>`_)
+* Added get_parameter_or_set_default. (`#551 <https://github.com/ros2/rclcpp/issues/551>`_)
+* Added max_duration to spin_some() (`#558 <https://github.com/ros2/rclcpp/issues/558>`_)
+* Updated to output rcl error message when yaml parsing fails (`#557 <https://github.com/ros2/rclcpp/issues/557>`_)
+* Updated to make sure timer is fini'd before clock (`#553 <https://github.com/ros2/rclcpp/issues/553>`_)
+* Get node names and namespaces (`#545 <https://github.com/ros2/rclcpp/issues/545>`_)
+* Fixed and improved documentation  (`#546 <https://github.com/ros2/rclcpp/issues/546>`_)
+* Updated to use rcl_clock_t jump callbacks (`#543 <https://github.com/ros2/rclcpp/issues/543>`_)
+* Updated to use rcl consolidated wait set functions (`#540 <https://github.com/ros2/rclcpp/issues/540>`_)
+* Addeed TIME_MAX and DURATION_MAX functions (`#538 <https://github.com/ros2/rclcpp/issues/538>`_)
+* Updated to publish shared_ptr of rcl_serialized_message (`#541 <https://github.com/ros2/rclcpp/issues/541>`_)
+* Added Time::is_zero and Duration::seconds (`#536 <https://github.com/ros2/rclcpp/issues/536>`_)
+* Changed to log an error message instead of throwing exception in destructor (`#535 <https://github.com/ros2/rclcpp/issues/535>`_)
+* Updated to relax tolerance of now test because timing affected by OS scheduling (`#533 <https://github.com/ros2/rclcpp/issues/533>`_)
+* Removed incorrect exception on sec < 0 (`#527 <https://github.com/ros2/rclcpp/issues/527>`_)
+* Added rclcpp::Time::seconds() (`#526 <https://github.com/ros2/rclcpp/issues/526>`_)
+* Updated Timer API to construct TimerBase/GenericTimer with Clock (`#523 <https://github.com/ros2/rclcpp/issues/523>`_)
+* Added rclcpp::is_initialized() (`#522 <https://github.com/ros2/rclcpp/issues/522>`_)
+* Added support for jump handlers with only pre- or post-jump callback (`#517 <https://github.com/ros2/rclcpp/issues/517>`_)
+* Removed use of uninitialized CMake var (`#512 <https://github.com/ros2/rclcpp/issues/512>`_)
+* Updated for Uncrustify 0.67 (`#510 <https://github.com/ros2/rclcpp/issues/510>`_)
+* Added get_node_names API from node. (`#508 <https://github.com/ros2/rclcpp/issues/508>`_)
+* Contributors: Anis Ladram, Chris Lalancette, Dirk Thomas, Francisco Martín Rico, Karsten Knese, Michael Carroll, Mikael Arguedas, Sagnik Basu, Shane Loretz, Sriram Raghunathan, William Woodall, chapulina, dhood
+
+0.5.0 (2018-06-25)
+------------------
+* Fixed a bug in the multi-threaded executor which could cause it to take a timer (potentially other types of wait-able items) more than once to be worked one. (`#383 <https://github.com/ros2/rclcpp/issues/383>`_)
+  * Specifically this could result in a timer getting called more often that it should when using the multi-threaded executor.
+* Added functions that allow you to publish serialized messages and received serialized messages in your subscription callback. (`#388 <https://github.com/ros2/rclcpp/issues/388>`_)
+* Changed code to always get the Service name from ``rcl`` to ensure the remapped name is returned. (`#498 <https://github.com/ros2/rclcpp/issues/498>`_)
+* Added previously missing ``set_parameters_atomically()`` method to the Service client interface. (`#494 <https://github.com/ros2/rclcpp/issues/494>`_)
+* Added ability to initialize parameter values in a Node via a YAML file passed on the command line. (`#488 <https://github.com/ros2/rclcpp/issues/488>`_)
+* Fixed the ROS parameter interface which got parameters that aren't set. (`#493 <https://github.com/ros2/rclcpp/issues/493>`_)
+* Added ability to initialize parameter values in a node with an argument to the Node constructor. (`#486 <https://github.com/ros2/rclcpp/issues/486>`_)
+* Added a ``Subscription`` tests which uses ``std::bind`` to a class member callback. (`#480 <https://github.com/ros2/rclcpp/issues/480>`_)
+* Refactored the ``ParameterVariant`` class into the ``Parameter`` and ``ParameterValue`` classes. (`#481 <https://github.com/ros2/rclcpp/issues/481>`_)
+* Relaxed template matching rules for ``std::bind`` and ``GNU C++ >= 7.1``. (`#484 <https://github.com/ros2/rclcpp/issues/484>`_)
+* Changed to use the new ``rosgraph_msgs/Clock`` message type for the ``/clock`` topic. (`#474 <https://github.com/ros2/rclcpp/issues/474>`_)
+* Fixed a flaky ROS time test due to not spinning before getting the time. (`#483 <https://github.com/ros2/rclcpp/issues/483>`_)
+* Nodes now autostart the ROS parameter services which let you get, set, and list parameters in a node. (`#478 <https://github.com/ros2/rclcpp/issues/478>`_)
+* Added support for arrays in Parameters. (`#443 <https://github.com/ros2/rclcpp/issues/443>`_)
+* Changed how executors use ``AnyExecutable`` objects so that they are a reference instead of a shared pointer, in order to avoid memory allocation in the "common case". (`#463 <https://github.com/ros2/rclcpp/issues/463>`_)
+* Added ability to pass command line arguments to the Node constructor. (`#461 <https://github.com/ros2/rclcpp/issues/461>`_)
+* Added an argument to specify the number of threads a multithreaded executor should create. (`#442 <https://github.com/ros2/rclcpp/issues/442>`_)
+* Changed library export order for static linking. (`#446 <https://github.com/ros2/rclcpp/issues/446>`_)
+* Fixed some typos in the time unit tests. (`#453 <https://github.com/ros2/rclcpp/issues/453>`_)
+  Obviously it mean RCL_SYSTEM_TIME but not RCL_ROS_TIME in some test cases
+  * Signed-off-by: jwang <jing.j.wang@intel.com>
+* Added the scale operation to ``rclcpp::Duration``.
+  * Signed-off-by: jwang <jing.j.wang@intel.com>
+* Changed API of the log location parameter to be ``const``. (`#451 <https://github.com/ros2/rclcpp/issues/451>`_)
+* Changed how the subscriber, client, service, and timer handles are stored to resolve shutdown order issues. (`#431 <https://github.com/ros2/rclcpp/issues/431>`_ and `#448 <https://github.com/ros2/rclcpp/issues/448>`_)
+* Updated to get the node's logger name from ``rcl``. (`#433 <https://github.com/ros2/rclcpp/issues/433>`_)
+* Now depends on ``ament_cmake_ros``. (`#444 <https://github.com/ros2/rclcpp/issues/444>`_)
+* Updaed code to use logging macros rather than ``fprintf()``. (`#439 <https://github.com/ros2/rclcpp/issues/439>`_)
+* Fixed a bug that was using an invalid iterator when erasing items using an iterator in a loop. (`#436 <https://github.com/ros2/rclcpp/issues/436>`_)
+* Changed code to support move of ``rcutils_time_point_value_t`` type from ``uint64_t`` to ``int64_t``. (`#429 <https://github.com/ros2/rclcpp/issues/429>`_)
+* Renamed parameter byte type to ``byte_values`` from ``bytes_value``. (`#428 <https://github.com/ros2/rclcpp/issues/428>`_)
+* Changed executor code to clear the wait set before resizing and waiting. (`#427 <https://github.com/ros2/rclcpp/issues/427>`_)
+* Fixed a potential dereference of nullptr in the topic name validation error string. (`#405 <https://github.com/ros2/rclcpp/issues/405>`_)
+  * Signed-off-by: Ethan Gao <ethan.gao@linux.intel.com>
+* Changed to use ``rcl_count_publishers()`` like API's rather than the lower level ``rmw_count_publishers()`` API. (`#425 <https://github.com/ros2/rclcpp/issues/425>`_)
+  * Signed-off-by: Sriram Raghunathan <rsriram7@visteon.com>
+* Fix potential segmentation fault due to ``get_topic_name()`` or ``rcl_service_get_service_name()`` returning nullptr and that not being checked before access in ``rclcpp``. (`#426 <https://github.com/ros2/rclcpp/issues/426>`_)
+  * Signed-off-by: Ethan Gao <ethan.gao@linux.intel.com>
+* Contributors: Denise Eng, Dirk Thomas, Ernesto Corbellini, Esteve Fernandez, Ethan Gao, Guillaume Autran, Karsten Knese, Matthew, Michael Carroll, Mikael Arguedas, Shane Loretz, Sriram Raghunathan, Tom Moore, William Woodall, dhood, jwang, jwang11, serge-nikulin

rclcpp/CMakeLists.txt

@@ -1,47 +1,657 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 
 project(rclcpp)
 
-find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(libstatistics_collector REQUIRED)
+find_package(rcl REQUIRED)
 find_package(rcl_interfaces REQUIRED)
+find_package(rcl_yaml_param_parser REQUIRED)
+find_package(rcpputils REQUIRED)
+find_package(rcutils REQUIRED)
+find_package(rmw REQUIRED)
+find_package(rosgraph_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(rosidl_runtime_cpp REQUIRED)
+find_package(rosidl_typesupport_c REQUIRED)
+find_package(rosidl_typesupport_cpp REQUIRED)
+find_package(statistics_msgs REQUIRED)
+find_package(tracetools REQUIRED)
 
-ament_export_dependencies(rmw)
-ament_export_dependencies(rcl_interfaces)
+# Default to C++14
+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)
+endif()
 
+set(${PROJECT_NAME}_SRCS
+  src/rclcpp/any_executable.cpp
+  src/rclcpp/callback_group.cpp
+  src/rclcpp/client.cpp
+  src/rclcpp/clock.cpp
+  src/rclcpp/context.cpp
+  src/rclcpp/contexts/default_context.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
+  src/rclcpp/detail/utilities.cpp
+  src/rclcpp/duration.cpp
+  src/rclcpp/event.cpp
+  src/rclcpp/exceptions/exceptions.cpp
+  src/rclcpp/executable_list.cpp
+  src/rclcpp/executor.cpp
+  src/rclcpp/executors.cpp
+  src/rclcpp/expand_topic_or_service_name.cpp
+  src/rclcpp/executors/multi_threaded_executor.cpp
+  src/rclcpp/executors/single_threaded_executor.cpp
+  src/rclcpp/executors/static_executor_entities_collector.cpp
+  src/rclcpp/executors/static_single_threaded_executor.cpp
+  src/rclcpp/future_return_code.cpp
+  src/rclcpp/graph_listener.cpp
+  src/rclcpp/guard_condition.cpp
+  src/rclcpp/init_options.cpp
+  src/rclcpp/intra_process_manager.cpp
+  src/rclcpp/logger.cpp
+  src/rclcpp/memory_strategies.cpp
+  src/rclcpp/memory_strategy.cpp
+  src/rclcpp/message_info.cpp
+  src/rclcpp/node.cpp
+  src/rclcpp/node_options.cpp
+  src/rclcpp/node_interfaces/node_base.cpp
+  src/rclcpp/node_interfaces/node_clock.cpp
+  src/rclcpp/node_interfaces/node_graph.cpp
+  src/rclcpp/node_interfaces/node_logging.cpp
+  src/rclcpp/node_interfaces/node_parameters.cpp
+  src/rclcpp/node_interfaces/node_services.cpp
+  src/rclcpp/node_interfaces/node_time_source.cpp
+  src/rclcpp/node_interfaces/node_timers.cpp
+  src/rclcpp/node_interfaces/node_topics.cpp
+  src/rclcpp/node_interfaces/node_waitables.cpp
+  src/rclcpp/parameter.cpp
+  src/rclcpp/parameter_value.cpp
+  src/rclcpp/parameter_client.cpp
+  src/rclcpp/parameter_events_filter.cpp
+  src/rclcpp/parameter_map.cpp
+  src/rclcpp/parameter_service.cpp
+  src/rclcpp/publisher_base.cpp
+  src/rclcpp/qos.cpp
+  src/rclcpp/qos_event.cpp
+  src/rclcpp/serialization.cpp
+  src/rclcpp/serialized_message.cpp
+  src/rclcpp/service.cpp
+  src/rclcpp/signal_handler.cpp
+  src/rclcpp/subscription_base.cpp
+  src/rclcpp/subscription_intra_process_base.cpp
+  src/rclcpp/time.cpp
+  src/rclcpp/time_source.cpp
+  src/rclcpp/timer.cpp
+  src/rclcpp/type_support.cpp
+  src/rclcpp/utilities.cpp
+  src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp
+  src/rclcpp/waitable.cpp
+)
+
+# "watch" template for changes
+configure_file(
+  "resource/logging.hpp.em"
+  "logging.hpp.em.watch"
+  COPYONLY
+)
+# generate header with logging macros
+set(python_code_logging
+  "import em"
+  "em.invoke(['-o', 'include/rclcpp/logging.hpp', '${CMAKE_CURRENT_SOURCE_DIR}/resource/logging.hpp.em'])")
+string(REPLACE ";" "$<SEMICOLON>" python_code_logging "${python_code_logging}")
+add_custom_command(OUTPUT include/rclcpp/logging.hpp
+  COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp"
+  COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_code_logging}"
+  DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/logging.hpp.em.watch"
+  COMMENT "Expanding logging.hpp.em"
+  VERBATIM
+)
+list(APPEND ${PROJECT_NAME}_SRCS
+  include/rclcpp/logging.hpp)
+
+file(GLOB interface_files "include/rclcpp/node_interfaces/node_*_interface.hpp")
+foreach(interface_file ${interface_files})
+  get_filename_component(interface_name ${interface_file} NAME_WE)
+
+  # "watch" template for changes
+  configure_file(
+    "resource/interface_traits.hpp.em"
+    "${CMAKE_CURRENT_BINARY_DIR}/${interface_name}_traits.hpp.em.watch"
+    COPYONLY
+  )
+  set(python_${interface_name}_traits
+    "import em"
+    "em.invoke(['-D', 'interface_name = \\'${interface_name}\\'', '-o', 'include/rclcpp/node_interfaces/${interface_name}_traits.hpp', '${CMAKE_CURRENT_SOURCE_DIR}/resource/interface_traits.hpp.em'])")
+  string(REPLACE ";" "$<SEMICOLON>" python_${interface_name}_traits "${python_${interface_name}_traits}")
+  add_custom_command(OUTPUT include/rclcpp/node_interfaces/${interface_name}_traits.hpp
+    COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp/node_interfaces"
+    COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_${interface_name}_traits}"
+    DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/${interface_name}_traits.hpp.em.watch"
+    COMMENT "Expanding interface_traits.hpp.em into ${interface_name}_traits.hpp"
+    VERBATIM
+  )
+  list(APPEND ${PROJECT_NAME}_SRCS
+    include/rclcpp/node_interfaces/${interface_name}_traits.hpp)
+
+  # "watch" template for changes
+  configure_file(
+    "resource/get_interface.hpp.em"
+    "get_${interface_name}.hpp.em.watch"
+    COPYONLY
+  )
+  set(python_get_${interface_name}
+    "import em"
+    "em.invoke(['-D', 'interface_name = \\'${interface_name}\\'', '-o', 'include/rclcpp/node_interfaces/get_${interface_name}.hpp', '${CMAKE_CURRENT_SOURCE_DIR}/resource/get_interface.hpp.em'])")
+  string(REPLACE ";" "$<SEMICOLON>" python_get_${interface_name} "${python_get_${interface_name}}")
+  add_custom_command(OUTPUT include/rclcpp/node_interfaces/get_${interface_name}.hpp
+    COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp/node_interfaces"
+    COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_get_${interface_name}}"
+    DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/get_${interface_name}.hpp.em.watch"
+    COMMENT "Expanding get_interface.hpp.em into get_${interface_file}.hpp"
+    VERBATIM
+  )
+  list(APPEND ${PROJECT_NAME}_SRCS
+    include/rclcpp/node_interfaces/get_${interface_name}.hpp)
+endforeach()
+
+add_library(${PROJECT_NAME}
+  ${${PROJECT_NAME}_SRCS})
+target_include_directories(${PROJECT_NAME} PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>"
+  "$<INSTALL_INTERFACE:include>")
+# specific order: dependents before dependencies
+ament_target_dependencies(${PROJECT_NAME}
+  "libstatistics_collector"
+  "rcl"
+  "rcl_yaml_param_parser"
+  "rcpputils"
+  "rcutils"
+  "builtin_interfaces"
+  "rosgraph_msgs"
+  "rosidl_typesupport_cpp"
+  "rosidl_runtime_cpp"
+  "statistics_msgs"
+  "tracetools"
+)
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(${PROJECT_NAME}
+  PRIVATE "RCLCPP_BUILDING_LIBRARY")
+
+install(
+  TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+# specific order: dependents before dependencies
 ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+ament_export_targets(${PROJECT_NAME})
 
-if(AMENT_ENABLE_TESTING)
+ament_export_dependencies(libstatistics_collector)
+ament_export_dependencies(rcl)
+ament_export_dependencies(rcpputils)
+ament_export_dependencies(rcutils)
+ament_export_dependencies(builtin_interfaces)
+ament_export_dependencies(rosgraph_msgs)
+ament_export_dependencies(rosidl_typesupport_cpp)
+ament_export_dependencies(rosidl_typesupport_c)
+ament_export_dependencies(rosidl_runtime_cpp)
+ament_export_dependencies(rcl_yaml_param_parser)
+ament_export_dependencies(statistics_msgs)
+ament_export_dependencies(tracetools)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
 
-  find_package(rmw REQUIRED)
+  find_package(rmw_implementation_cmake REQUIRED)
 
-  if(NOT WIN32)
-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Wextra")
+  find_package(test_msgs REQUIRED)
+
+  include(cmake/rclcpp_add_build_failure_test.cmake)
+
+  add_definitions(-DTEST_RESOURCES_DIRECTORY="${CMAKE_CURRENT_BINARY_DIR}/test/resources")
+
+  rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs
+    test/msg/Header.msg
+    test/msg/MessageWithHeader.msg
+    DEPENDENCIES builtin_interfaces
+    LIBRARY_NAME ${PROJECT_NAME}
+    SKIP_INSTALL
+  )
+
+  ament_add_gtest(test_client test/test_client.cpp)
+  if(TARGET test_client)
+    ament_target_dependencies(test_client
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_runtime_cpp"
+      "rosidl_typesupport_cpp"
+    )
+    target_link_libraries(test_client ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_create_timer test/test_create_timer.cpp)
+  if(TARGET test_create_timer)
+    ament_target_dependencies(test_create_timer
+      "rcl_interfaces"
+      "rmw"
+      "rcl"
+      "rosidl_runtime_cpp"
+      "rosidl_typesupport_cpp"
+    )
+    target_link_libraries(test_create_timer ${PROJECT_NAME})
+    target_include_directories(test_create_timer PRIVATE test/)
+  endif()
+  ament_add_gtest(test_expand_topic_or_service_name test/test_expand_topic_or_service_name.cpp)
+  if(TARGET test_expand_topic_or_service_name)
+    ament_target_dependencies(test_expand_topic_or_service_name
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_runtime_cpp"
+      "rosidl_typesupport_cpp"
+    )
+    target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME})
   endif()
-
-  include_directories(include)
-
   ament_add_gtest(test_function_traits test/test_function_traits.cpp)
-  ament_add_gtest(test_mapped_ring_buffer test/test_mapped_ring_buffer.cpp)
-  ament_add_gtest(test_intra_process_manager test/test_intra_process_manager.cpp)
-  if(TARGET test_intra_process_manager)
-    target_include_directories(test_intra_process_manager PUBLIC
-      "${rcl_interfaces_INCLUDE_DIRS}"
-      "${rmw_INCLUDE_DIRS}")
+  if(TARGET test_function_traits)
+    target_include_directories(test_function_traits PUBLIC include)
+    ament_target_dependencies(test_function_traits
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_runtime_cpp"
+      "rosidl_typesupport_cpp"
+    )
   endif()
+  ament_add_gmock(test_intra_process_manager test/test_intra_process_manager.cpp)
+  if(TARGET test_intra_process_manager)
+    ament_target_dependencies(test_intra_process_manager
+      "rcl"
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_runtime_cpp"
+      "rosidl_typesupport_cpp"
+    )
+    target_link_libraries(test_intra_process_manager ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_ring_buffer_implementation test/test_ring_buffer_implementation.cpp)
+  if(TARGET test_ring_buffer_implementation)
+    ament_target_dependencies(test_ring_buffer_implementation
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_runtime_cpp"
+      "rosidl_typesupport_cpp"
+    )
+    target_link_libraries(test_ring_buffer_implementation ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_intra_process_buffer test/test_intra_process_buffer.cpp)
+  if(TARGET test_intra_process_buffer)
+    ament_target_dependencies(test_intra_process_buffer
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_runtime_cpp"
+      "rosidl_typesupport_cpp"
+    )
+    target_link_libraries(test_intra_process_buffer ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_loaned_message test/test_loaned_message.cpp)
+  ament_target_dependencies(test_loaned_message
+    "test_msgs"
+  )
+  target_link_libraries(test_loaned_message ${PROJECT_NAME})
+
+  ament_add_gtest(test_node test/test_node.cpp TIMEOUT 240)
+  if(TARGET test_node)
+    ament_target_dependencies(test_node
+      "rcl_interfaces"
+      "rcpputils"
+      "rmw"
+      "rosidl_runtime_cpp"
+      "rosidl_typesupport_cpp"
+      "test_msgs"
+    )
+    target_link_libraries(test_node ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_node_interfaces__get_node_interfaces
+    test/node_interfaces/test_get_node_interfaces.cpp)
+  if(TARGET test_node_interfaces__get_node_interfaces)
+    target_link_libraries(test_node_interfaces__get_node_interfaces ${PROJECT_NAME})
+  endif()
+
+  # TODO(wjwwood): reenable these build failure tests when I can get Jenkins to ignore their output
+  # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_rclcpp_node
+  #   test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp)
+  # target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node
+  #   ${PROJECT_NAME})
+
+  # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_rclcpp_node
+  #   test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp)
+  # target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node
+  #   ${PROJECT_NAME})
+
+  # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_wrapped_node
+  #   test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp)
+  # target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node
+  #   ${PROJECT_NAME})
+
+  # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_wrapped_node
+  #   test/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp)
+  # target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node
+  #   ${PROJECT_NAME})
+
+  ament_add_gtest(test_node_global_args test/test_node_global_args.cpp)
+  if(TARGET test_node_global_args)
+    ament_target_dependencies(test_node_global_args
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_runtime_cpp"
+      "rosidl_typesupport_cpp"
+    )
+    target_link_libraries(test_node_global_args ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_node_options test/test_node_options.cpp)
+  if(TARGET test_node_options)
+    ament_target_dependencies(test_node_options "rcl")
+    target_link_libraries(test_node_options ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_parameter_client test/test_parameter_client.cpp)
+  if(TARGET test_parameter_client)
+    ament_target_dependencies(test_parameter_client
+      "rcl_interfaces"
+    )
+    target_link_libraries(test_parameter_client ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_parameter_events_filter test/test_parameter_events_filter.cpp)
+  if(TARGET test_parameter_events_filter)
+    ament_target_dependencies(test_parameter_events_filter
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_runtime_cpp"
+      "rosidl_typesupport_cpp"
+    )
+    target_link_libraries(test_parameter_events_filter ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_parameter test/test_parameter.cpp)
+  if(TARGET test_parameter)
+    ament_target_dependencies(test_parameter
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_runtime_cpp"
+      "rosidl_typesupport_cpp"
+    )
+    target_link_libraries(test_parameter ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_parameter_map test/test_parameter_map.cpp)
+  if(TARGET test_parameter_map)
+    target_link_libraries(test_parameter_map ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_publisher test/test_publisher.cpp)
+  if(TARGET test_publisher)
+    ament_target_dependencies(test_publisher
+      "rmw"
+      "rosidl_runtime_cpp"
+      "rosidl_typesupport_cpp"
+      "test_msgs"
+    )
+    target_link_libraries(test_publisher ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_publisher_subscription_count_api test/test_publisher_subscription_count_api.cpp)
+  if(TARGET test_publisher_subscription_count_api)
+    ament_target_dependencies(test_publisher_subscription_count_api
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_runtime_cpp"
+      "rosidl_typesupport_cpp"
+      "test_msgs"
+    )
+    target_link_libraries(test_publisher_subscription_count_api ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_qos test/test_qos.cpp)
+  if(TARGET test_qos)
+    ament_target_dependencies(test_qos
+      "rmw"
+    )
+    target_link_libraries(test_qos
+      ${PROJECT_NAME}
+    )
+  endif()
+  ament_add_gtest(test_qos_event test/test_qos_event.cpp)
+  if(TARGET test_qos_event)
+    ament_target_dependencies(test_qos_event
+      "rmw"
+      "test_msgs"
+    )
+    target_link_libraries(test_qos_event
+      ${PROJECT_NAME}
+    )
+  endif()
+  ament_add_gtest(test_rate test/test_rate.cpp)
+  if(TARGET test_rate)
+    ament_target_dependencies(test_rate
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_runtime_cpp"
+      "rosidl_typesupport_cpp"
+    )
+    target_link_libraries(test_rate
+      ${PROJECT_NAME}
+    )
+  endif()
+  ament_add_gtest(test_serialized_message_allocator test/test_serialized_message_allocator.cpp)
+  if(TARGET test_serialized_message_allocator)
+    ament_target_dependencies(test_serialized_message_allocator
+      test_msgs
+    )
+    target_link_libraries(test_serialized_message_allocator
+      ${PROJECT_NAME}
+    )
+  endif()
+  ament_add_gtest(test_serialized_message test/test_serialized_message.cpp)
+  if(TARGET test_serialized_message)
+    ament_target_dependencies(test_serialized_message
+      test_msgs
+    )
+    target_link_libraries(test_serialized_message
+      ${PROJECT_NAME}
+    )
+  endif()
+  ament_add_gtest(test_service test/test_service.cpp)
+  if(TARGET test_service)
+    ament_target_dependencies(test_service
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_runtime_cpp"
+      "rosidl_typesupport_cpp"
+    )
+    target_link_libraries(test_service ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_subscription test/test_subscription.cpp)
+  if(TARGET test_subscription)
+    ament_target_dependencies(test_subscription
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_runtime_cpp"
+      "rosidl_typesupport_cpp"
+      "test_msgs"
+    )
+    target_link_libraries(test_subscription ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_subscription_publisher_count_api test/test_subscription_publisher_count_api.cpp)
+  if(TARGET test_subscription_publisher_count_api)
+    ament_target_dependencies(test_subscription_publisher_count_api
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_runtime_cpp"
+      "rosidl_typesupport_cpp"
+      "test_msgs"
+    )
+    target_link_libraries(test_subscription_publisher_count_api ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_subscription_traits test/test_subscription_traits.cpp)
+  if(TARGET test_subscription_traits)
+    ament_target_dependencies(test_subscription_traits
+      "rcl"
+      "test_msgs"
+    )
+    target_link_libraries(test_subscription_traits ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_find_weak_nodes test/test_find_weak_nodes.cpp)
+  if(TARGET test_find_weak_nodes)
+    ament_target_dependencies(test_find_weak_nodes
+      "rcl"
+    )
+    target_link_libraries(test_find_weak_nodes ${PROJECT_NAME})
+  endif()
+
+  set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}")
+  if(WIN32)
+    set(append_library_dirs "${append_library_dirs}/$<CONFIG>")
+  endif()
+
+  ament_add_gtest(test_externally_defined_services test/test_externally_defined_services.cpp)
+  ament_target_dependencies(test_externally_defined_services
+    "rcl"
+    "test_msgs"
+  )
+  target_link_libraries(test_externally_defined_services ${PROJECT_NAME})
+
+  ament_add_gtest(test_duration test/test_duration.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_duration)
+    ament_target_dependencies(test_duration
+      "rcl")
+    target_link_libraries(test_duration ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_executor test/test_executor.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_executor)
+    ament_target_dependencies(test_executor
+      "rcl")
+    target_link_libraries(test_executor ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_logger test/test_logger.cpp)
+  target_link_libraries(test_logger ${PROJECT_NAME})
+
+  ament_add_gmock(test_logging test/test_logging.cpp)
+  target_link_libraries(test_logging ${PROJECT_NAME})
+
+  ament_add_gtest(test_time test/test_time.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_time)
+    ament_target_dependencies(test_time
+      "rcl")
+    target_link_libraries(test_time ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_timer test/test_timer.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_timer)
+    ament_target_dependencies(test_timer
+      "rcl")
+    target_link_libraries(test_timer ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_time_source test/test_time_source.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_time_source)
+    ament_target_dependencies(test_time_source
+      "rcl")
+    target_link_libraries(test_time_source ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_utilities test/test_utilities.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_utilities)
+    ament_target_dependencies(test_utilities
+      "rcl")
+    target_link_libraries(test_utilities ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_init test/test_init.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_init)
+    ament_target_dependencies(test_init
+      "rcl")
+    target_link_libraries(test_init ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_interface_traits test/test_interface_traits.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_interface_traits)
+    ament_target_dependencies(test_interface_traits
+      "rcl")
+    target_link_libraries(test_interface_traits ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_multi_threaded_executor test/executors/test_multi_threaded_executor.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_multi_threaded_executor)
+    ament_target_dependencies(test_multi_threaded_executor
+      "rcl")
+    target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_guard_condition test/test_guard_condition.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_guard_condition)
+    target_link_libraries(test_guard_condition ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_wait_set test/test_wait_set.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_wait_set)
+    ament_target_dependencies(test_wait_set "test_msgs")
+    target_link_libraries(test_wait_set ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_subscription_topic_statistics test/topic_statistics/test_subscription_topic_statistics.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}"
+  )
+  if(TARGET test_subscription_topic_statistics)
+    ament_target_dependencies(test_subscription_topic_statistics
+      "builtin_interfaces"
+      "libstatistics_collector"
+      "rcl_interfaces"
+      "rcutils"
+      "rmw"
+      "rosidl_runtime_cpp"
+      "rosidl_typesupport_cpp"
+      "statistics_msgs"
+      "test_msgs")
+    rosidl_target_interfaces(test_subscription_topic_statistics ${PROJECT_NAME}_test_msgs "rosidl_typesupport_cpp")
+    target_link_libraries(test_subscription_topic_statistics ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_subscription_options test/test_subscription_options.cpp)
+  if(TARGET test_subscription_options)
+    ament_target_dependencies(test_subscription_options "rcl")
+    target_link_libraries(test_subscription_options ${PROJECT_NAME})
+  endif()
+
+  # Install test resources
+  install(
+    DIRECTORY test/resources
+    DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test)
 endif()
 
-ament_package(
-  CONFIG_EXTRAS rclcpp-extras.cmake
-)
+ament_package()
 
 install(
-  DIRECTORY include/
+  DIRECTORY include/ ${CMAKE_CURRENT_BINARY_DIR}/include/
   DESTINATION include
 )
-
-install(
-  DIRECTORY src/
-  DESTINATION src/rclcpp
-)

rclcpp/Doxyfile

@@ -0,0 +1,33 @@
+# All settings not listed here will use the Doxygen default values.
+
+PROJECT_NAME           = "rclcpp"
+PROJECT_NUMBER         = master
+PROJECT_BRIEF          = "C++ ROS Client Library API"
+
+# Use these lines to include the generated logging.hpp (update install path if needed)
+#INPUT                  = ../../../../install_isolated/rclcpp/include
+#STRIP_FROM_PATH        = /Users/william/ros2_ws/install_isolated/rclcpp/include
+# Otherwise just generate for the local (non-generated header files)
+INPUT                  = ./include
+
+RECURSIVE              = YES
+OUTPUT_DIRECTORY       = doc_output
+
+EXTRACT_ALL            = YES
+SORT_MEMBER_DOCS       = NO
+
+GENERATE_LATEX         = NO
+
+ENABLE_PREPROCESSING   = YES
+MACRO_EXPANSION        = YES
+EXPAND_ONLY_PREDEF     = YES
+PREDEFINED             = RCLCPP_PUBLIC=
+
+# Tag files that do not exist will produce a warning and cross-project linking will not work.
+TAGFILES += "../../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/"
+# Consider changing "latest" to the version you want to reference (e.g. beta1 or 1.0.0)
+TAGFILES += "../../../../doxygen_tag_files/rcl.tag=http://docs.ros2.org/latest/api/rcl/"
+TAGFILES += "../../../../doxygen_tag_files/rmw.tag=http://docs.ros2.org/latest/api/rmw/"
+TAGFILES += "../../../../doxygen_tag_files/rcutils.tag=http://docs.ros2.org/latest/api/rcutils/"
+# Uncomment to generate tag files for cross-project linking.
+#GENERATE_TAGFILE = "../../../../doxygen_tag_files/rclcpp.tag"

rclcpp/cmake/rclcpp_add_build_failure_test.cmake

@@ -0,0 +1,56 @@
+# Copyright 2019 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.
+
+#
+# Register a test which tries to compile a file and expects it to fail to build.
+#
+# This will create two targets, one by the given target name and a test target
+# which has the same name prefixed with `test_`.
+# For example, if target is `should_not_compile__use_const_argument` then there
+# will be an executable target called `should_not_compile__use_const_argument`
+# and a test target called `test_should_not_compile__use_const_argument`.
+#
+# :param target: the name of the target to be created
+# :type target: string
+# :param ARGN: the list of source files to be used to create the test executable
+# :type ARGN: list of strings
+#
+macro(rclcpp_add_build_failure_test target)
+  if(${ARGC} EQUAL 0)
+    message(
+      FATAL_ERROR
+      "rclcpp_add_build_failure_test() requires a target name and "
+      "at least one source file")
+  endif()
+
+  add_executable(${target} ${ARGN})
+  set_target_properties(${target}
+    PROPERTIES
+      EXCLUDE_FROM_ALL TRUE
+      EXCLUDE_FROM_DEFAULT_BUILD TRUE)
+
+  add_test(
+    NAME test_${target}
+    COMMAND
+      ${CMAKE_COMMAND}
+      --build .
+      --target ${target}
+      --config $<CONFIGURATION>
+    WORKING_DIRECTORY ${CMAKE_BINARY_DIR})
+  set_tests_properties(test_${target}
+    PROPERTIES
+      WILL_FAIL TRUE
+      LABELS "build_failure"
+  )
+endmacro()

rclcpp/include/rclcpp/allocator/allocator_common.hpp

@@ -17,6 +17,8 @@
 
 #include <memory>
 
+#include "rcl/allocator.h"
+
 #include "rclcpp/allocator/allocator_deleter.hpp"
 
 namespace rclcpp
@@ -27,6 +29,70 @@ namespace allocator
 template<typename T, typename Alloc>
 using AllocRebind = typename std::allocator_traits<Alloc>::template rebind_traits<T>;
 
+template<typename Alloc>
+void * retyped_allocate(size_t size, void * untyped_allocator)
+{
+  auto typed_allocator = static_cast<Alloc *>(untyped_allocator);
+  if (!typed_allocator) {
+    throw std::runtime_error("Received incorrect allocator type");
+  }
+  return std::allocator_traits<Alloc>::allocate(*typed_allocator, size);
+}
+
+template<typename T, typename Alloc>
+void retyped_deallocate(void * untyped_pointer, void * untyped_allocator)
+{
+  auto typed_allocator = static_cast<Alloc *>(untyped_allocator);
+  if (!typed_allocator) {
+    throw std::runtime_error("Received incorrect allocator type");
+  }
+  auto typed_ptr = static_cast<T *>(untyped_pointer);
+  std::allocator_traits<Alloc>::deallocate(*typed_allocator, typed_ptr, 1);
+}
+
+template<typename T, typename Alloc>
+void * retyped_reallocate(void * untyped_pointer, size_t size, void * untyped_allocator)
+{
+  auto typed_allocator = static_cast<Alloc *>(untyped_allocator);
+  if (!typed_allocator) {
+    throw std::runtime_error("Received incorrect allocator type");
+  }
+  auto typed_ptr = static_cast<T *>(untyped_pointer);
+  std::allocator_traits<Alloc>::deallocate(*typed_allocator, typed_ptr, 1);
+  return std::allocator_traits<Alloc>::allocate(*typed_allocator, size);
+}
+
+
+// Convert a std::allocator_traits-formatted Allocator into an rcl allocator
+template<
+  typename T,
+  typename Alloc,
+  typename std::enable_if<!std::is_same<Alloc, std::allocator<void>>::value>::type * = nullptr>
+rcl_allocator_t get_rcl_allocator(Alloc & allocator)
+{
+  rcl_allocator_t rcl_allocator = rcl_get_default_allocator();
+#ifndef _WIN32
+  rcl_allocator.allocate = &retyped_allocate<Alloc>;
+  rcl_allocator.deallocate = &retyped_deallocate<T, Alloc>;
+  rcl_allocator.reallocate = &retyped_reallocate<T, Alloc>;
+  rcl_allocator.state = &allocator;
+#else
+  (void)allocator;  // Remove warning
+#endif
+  return rcl_allocator;
+}
+
+// TODO(jacquelinekay) Workaround for an incomplete implementation of std::allocator<void>
+template<
+  typename T,
+  typename Alloc,
+  typename std::enable_if<std::is_same<Alloc, std::allocator<void>>::value>::type * = nullptr>
+rcl_allocator_t get_rcl_allocator(Alloc & allocator)
+{
+  (void)allocator;
+  return rcl_get_default_allocator();
+}
+
 }  // namespace allocator
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/allocator/allocator_deleter.hpp

@@ -16,10 +16,10 @@
 #define RCLCPP__ALLOCATOR__ALLOCATOR_DELETER_HPP_
 
 #include <memory>
+#include <stdexcept>
 
 namespace rclcpp
 {
-
 namespace allocator
 {
 
@@ -94,11 +94,11 @@ void set_allocator_for_deleter(AllocatorDeleter<T> * deleter, Alloc * alloc)
 
 template<typename Alloc, typename T>
 using Deleter = typename std::conditional<
-    std::is_same<typename std::allocator_traits<Alloc>::template rebind_alloc<T>,
-    typename std::allocator<void>::template rebind<T>::other>::value,
-    std::default_delete<T>,
-    AllocatorDeleter<Alloc>
-    >::type;
+  std::is_same<typename std::allocator_traits<Alloc>::template rebind_alloc<T>,
+  typename std::allocator<void>::template rebind<T>::other>::value,
+  std::default_delete<T>,
+  AllocatorDeleter<Alloc>
+  >::type;
 }  // namespace allocator
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/any_executable.hpp

@@ -12,37 +12,49 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_ANY_EXECUTABLE_HPP_
-#define RCLCPP_RCLCPP_ANY_EXECUTABLE_HPP_
+#ifndef RCLCPP__ANY_EXECUTABLE_HPP_
+#define RCLCPP__ANY_EXECUTABLE_HPP_
 
 #include <memory>
 
-#include <rclcpp/macros.hpp>
-#include <rclcpp/node.hpp>
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/client.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/subscription.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
 
 namespace rclcpp
 {
-namespace executor
-{
 
 struct AnyExecutable
 {
-  RCLCPP_SMART_PTR_DEFINITIONS(AnyExecutable);
-  AnyExecutable()
-  : subscription(0), timer(0), callback_group(0), node(0)
-  {}
+  RCLCPP_PUBLIC
+  AnyExecutable();
+
+  RCLCPP_PUBLIC
+  virtual ~AnyExecutable();
+
   // Only one of the following pointers will be set.
-  rclcpp::subscription::SubscriptionBase::SharedPtr subscription;
-  rclcpp::subscription::SubscriptionBase::SharedPtr subscription_intra_process;
-  rclcpp::timer::TimerBase::SharedPtr timer;
-  rclcpp::service::ServiceBase::SharedPtr service;
-  rclcpp::client::ClientBase::SharedPtr client;
+  rclcpp::SubscriptionBase::SharedPtr subscription;
+  rclcpp::TimerBase::SharedPtr timer;
+  rclcpp::ServiceBase::SharedPtr service;
+  rclcpp::ClientBase::SharedPtr client;
+  rclcpp::Waitable::SharedPtr waitable;
   // These are used to keep the scope on the containing items
-  rclcpp::callback_group::CallbackGroup::SharedPtr callback_group;
-  rclcpp::node::Node::SharedPtr node;
+  rclcpp::CallbackGroup::SharedPtr callback_group;
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base;
 };
 
-} /* executor */
-} /* rclcpp */
+namespace executor
+{
 
-#endif
+using AnyExecutable [[deprecated("use rclcpp::AnyExecutable instead")]] = AnyExecutable;
+
+}  // namespace executor
+}  // namespace rclcpp
+
+#endif  // RCLCPP__ANY_EXECUTABLE_HPP_

rclcpp/include/rclcpp/any_service_callback.hpp

@@ -12,36 +12,38 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_ANY_SERVICE_CALLBACK_HPP_
-#define RCLCPP_RCLCPP_ANY_SERVICE_CALLBACK_HPP_
-
-#include <rclcpp/function_traits.hpp>
+#ifndef RCLCPP__ANY_SERVICE_CALLBACK_HPP_
+#define RCLCPP__ANY_SERVICE_CALLBACK_HPP_
 
 #include <functional>
 #include <memory>
+#include <stdexcept>
 #include <type_traits>
 
-#include <rmw/types.h>
+#include "rclcpp/function_traits.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rmw/types.h"
+#include "tracetools/tracetools.h"
+#include "tracetools/utils.hpp"
 
 namespace rclcpp
 {
 
-namespace any_service_callback
-{
-
 template<typename ServiceT>
 class AnyServiceCallback
 {
 private:
-  using SharedPtrCallback = std::function<void(
-        const std::shared_ptr<typename ServiceT::Request>,
-        std::shared_ptr<typename ServiceT::Response>
-      )>;
-  using SharedPtrWithRequestHeaderCallback = std::function<void(
-        const std::shared_ptr<rmw_request_id_t>,
-        const std::shared_ptr<typename ServiceT::Request>,
-        std::shared_ptr<typename ServiceT::Response>
-      )>;
+  using SharedPtrCallback = std::function<
+    void (
+      const std::shared_ptr<typename ServiceT::Request>,
+      std::shared_ptr<typename ServiceT::Response>
+    )>;
+  using SharedPtrWithRequestHeaderCallback = std::function<
+    void (
+      const std::shared_ptr<rmw_request_id_t>,
+      const std::shared_ptr<typename ServiceT::Request>,
+      std::shared_ptr<typename ServiceT::Response>
+    )>;
 
   SharedPtrCallback shared_ptr_callback_;
   SharedPtrWithRequestHeaderCallback shared_ptr_with_request_header_callback_;
@@ -56,7 +58,7 @@ public:
   template<
     typename CallbackT,
     typename std::enable_if<
-      rclcpp::same_arguments<
+      rclcpp::function_traits::same_arguments<
         CallbackT,
         SharedPtrCallback
       >::value
@@ -70,7 +72,7 @@ public:
   template<
     typename CallbackT,
     typename std::enable_if<
-      rclcpp::same_arguments<
+      rclcpp::function_traits::same_arguments<
         CallbackT,
         SharedPtrWithRequestHeaderCallback
       >::value
@@ -86,6 +88,7 @@ public:
     std::shared_ptr<typename ServiceT::Request> request,
     std::shared_ptr<typename ServiceT::Response> response)
   {
+    TRACEPOINT(callback_start, (const void *)this, false);
     if (shared_ptr_callback_ != nullptr) {
       (void)request_header;
       shared_ptr_callback_(request, response);
@@ -94,10 +97,27 @@ public:
     } else {
       throw std::runtime_error("unexpected request without any callback set");
     }
+    TRACEPOINT(callback_end, (const void *)this);
+  }
+
+  void register_callback_for_tracing()
+  {
+#ifndef TRACETOOLS_DISABLED
+    if (shared_ptr_callback_) {
+      TRACEPOINT(
+        rclcpp_callback_register,
+        (const void *)this,
+        get_symbol(shared_ptr_callback_));
+    } else if (shared_ptr_with_request_header_callback_) {
+      TRACEPOINT(
+        rclcpp_callback_register,
+        (const void *)this,
+        get_symbol(shared_ptr_with_request_header_callback_));
+    }
+#endif  // TRACETOOLS_DISABLED
   }
 };
 
-} /* namespace any_service_callback */
-} /* namespace rclcpp */
+}  // namespace rclcpp
 
-#endif /* RCLCPP_RCLCPP_ANY_SERVICE_CALLBACK_HPP_ */
+#endif  // RCLCPP__ANY_SERVICE_CALLBACK_HPP_

rclcpp/include/rclcpp/any_subscription_callback.hpp

@@ -19,34 +19,38 @@
 
 #include <functional>
 #include <memory>
+#include <stdexcept>
 #include <type_traits>
+#include <utility>
 
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/function_traits.hpp"
+#include "rclcpp/message_info.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "tracetools/tracetools.h"
+#include "tracetools/utils.hpp"
 
 namespace rclcpp
 {
 
-namespace any_subscription_callback
-{
-
 template<typename MessageT, typename Alloc>
 class AnySubscriptionCallback
 {
   using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
   using MessageAlloc = typename MessageAllocTraits::allocator_type;
   using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
+  using ConstMessageSharedPtr = std::shared_ptr<const MessageT>;
   using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
 
-  using SharedPtrCallback = std::function<void(const std::shared_ptr<MessageT>)>;
+  using SharedPtrCallback = std::function<void (const std::shared_ptr<MessageT>)>;
   using SharedPtrWithInfoCallback =
-      std::function<void(const std::shared_ptr<MessageT>, const rmw_message_info_t &)>;
-  using ConstSharedPtrCallback = std::function<void(const std::shared_ptr<const MessageT>)>;
+    std::function<void (const std::shared_ptr<MessageT>, const rclcpp::MessageInfo &)>;
+  using ConstSharedPtrCallback = std::function<void (const std::shared_ptr<const MessageT>)>;
   using ConstSharedPtrWithInfoCallback =
-      std::function<void(const std::shared_ptr<const MessageT>, const rmw_message_info_t &)>;
-  using UniquePtrCallback = std::function<void(MessageUniquePtr)>;
+    std::function<void (const std::shared_ptr<const MessageT>, const rclcpp::MessageInfo &)>;
+  using UniquePtrCallback = std::function<void (MessageUniquePtr)>;
   using UniquePtrWithInfoCallback =
-      std::function<void(MessageUniquePtr, const rmw_message_info_t &)>;
+    std::function<void (MessageUniquePtr, const rclcpp::MessageInfo &)>;
 
   SharedPtrCallback shared_ptr_callback_;
   SharedPtrWithInfoCallback shared_ptr_with_info_callback_;
@@ -70,7 +74,7 @@ public:
   template<
     typename CallbackT,
     typename std::enable_if<
-      rclcpp::same_arguments<
+      rclcpp::function_traits::same_arguments<
         CallbackT,
         SharedPtrCallback
       >::value
@@ -84,7 +88,7 @@ public:
   template<
     typename CallbackT,
     typename std::enable_if<
-      rclcpp::same_arguments<
+      rclcpp::function_traits::same_arguments<
         CallbackT,
         SharedPtrWithInfoCallback
       >::value
@@ -98,7 +102,7 @@ public:
   template<
     typename CallbackT,
     typename std::enable_if<
-      rclcpp::same_arguments<
+      rclcpp::function_traits::same_arguments<
         CallbackT,
         ConstSharedPtrCallback
       >::value
@@ -112,7 +116,7 @@ public:
   template<
     typename CallbackT,
     typename std::enable_if<
-      rclcpp::same_arguments<
+      rclcpp::function_traits::same_arguments<
         CallbackT,
         ConstSharedPtrWithInfoCallback
       >::value
@@ -126,7 +130,7 @@ public:
   template<
     typename CallbackT,
     typename std::enable_if<
-      rclcpp::same_arguments<
+      rclcpp::function_traits::same_arguments<
         CallbackT,
         UniquePtrCallback
       >::value
@@ -140,7 +144,7 @@ public:
   template<
     typename CallbackT,
     typename std::enable_if<
-      rclcpp::same_arguments<
+      rclcpp::function_traits::same_arguments<
         CallbackT,
         UniquePtrWithInfoCallback
       >::value
@@ -152,9 +156,9 @@ public:
   }
 
   void dispatch(
-    std::shared_ptr<MessageT> message, const rmw_message_info_t & message_info)
+    std::shared_ptr<MessageT> message, const rclcpp::MessageInfo & message_info)
   {
-    (void)message_info;
+    TRACEPOINT(callback_start, (const void *)this, false);
     if (shared_ptr_callback_) {
       shared_ptr_callback_(message);
     } else if (shared_ptr_with_info_callback_) {
@@ -174,31 +178,86 @@ public:
     } else {
       throw std::runtime_error("unexpected message without any callback set");
     }
+    TRACEPOINT(callback_end, (const void *)this);
   }
 
   void dispatch_intra_process(
-    MessageUniquePtr & message, const rmw_message_info_t & message_info)
+    ConstMessageSharedPtr message, const rclcpp::MessageInfo & message_info)
   {
-    (void)message_info;
+    TRACEPOINT(callback_start, (const void *)this, true);
+    if (const_shared_ptr_callback_) {
+      const_shared_ptr_callback_(message);
+    } else if (const_shared_ptr_with_info_callback_) {
+      const_shared_ptr_with_info_callback_(message, message_info);
+    } else {
+      if (
+        unique_ptr_callback_ || unique_ptr_with_info_callback_ ||
+        shared_ptr_callback_ || shared_ptr_with_info_callback_)
+      {
+        throw std::runtime_error(
+                "unexpected dispatch_intra_process const shared "
+                "message call with no const shared_ptr callback");
+      } else {
+        throw std::runtime_error("unexpected message without any callback set");
+      }
+    }
+    TRACEPOINT(callback_end, (const void *)this);
+  }
+
+  void dispatch_intra_process(
+    MessageUniquePtr message, const rclcpp::MessageInfo & message_info)
+  {
+    TRACEPOINT(callback_start, (const void *)this, true);
     if (shared_ptr_callback_) {
-      typename MessageT::SharedPtr shared_message = std::move(message);
+      typename std::shared_ptr<MessageT> shared_message = std::move(message);
       shared_ptr_callback_(shared_message);
     } else if (shared_ptr_with_info_callback_) {
-      typename MessageT::SharedPtr shared_message = std::move(message);
+      typename std::shared_ptr<MessageT> shared_message = std::move(message);
       shared_ptr_with_info_callback_(shared_message, message_info);
-    } else if (const_shared_ptr_callback_) {
-      typename MessageT::ConstSharedPtr const_shared_message = std::move(message);
-      const_shared_ptr_callback_(const_shared_message);
-    } else if (const_shared_ptr_with_info_callback_) {
-      typename MessageT::ConstSharedPtr const_shared_message = std::move(message);
-      const_shared_ptr_with_info_callback_(const_shared_message, message_info);
     } else if (unique_ptr_callback_) {
       unique_ptr_callback_(std::move(message));
     } else if (unique_ptr_with_info_callback_) {
       unique_ptr_with_info_callback_(std::move(message), message_info);
+    } else if (const_shared_ptr_callback_ || const_shared_ptr_with_info_callback_) {
+      throw std::runtime_error(
+              "unexpected dispatch_intra_process unique message call"
+              " with const shared_ptr callback");
     } else {
       throw std::runtime_error("unexpected message without any callback set");
     }
+    TRACEPOINT(callback_end, (const void *)this);
+  }
+
+  bool use_take_shared_method() const
+  {
+    return const_shared_ptr_callback_ || const_shared_ptr_with_info_callback_;
+  }
+
+  void register_callback_for_tracing()
+  {
+#ifndef TRACETOOLS_DISABLED
+    if (shared_ptr_callback_) {
+      TRACEPOINT(
+        rclcpp_callback_register,
+        (const void *)this,
+        get_symbol(shared_ptr_callback_));
+    } else if (shared_ptr_with_info_callback_) {
+      TRACEPOINT(
+        rclcpp_callback_register,
+        (const void *)this,
+        get_symbol(shared_ptr_with_info_callback_));
+    } else if (unique_ptr_callback_) {
+      TRACEPOINT(
+        rclcpp_callback_register,
+        (const void *)this,
+        get_symbol(unique_ptr_callback_));
+    } else if (unique_ptr_with_info_callback_) {
+      TRACEPOINT(
+        rclcpp_callback_register,
+        (const void *)this,
+        get_symbol(unique_ptr_with_info_callback_));
+    }
+#endif  // TRACETOOLS_DISABLED
   }
 
 private:
@@ -206,7 +265,6 @@ private:
   MessageDeleter message_deleter_;
 };
 
-}  // namespace any_subscription_callback
 }  // namespace rclcpp
 
 #endif  // RCLCPP__ANY_SUBSCRIPTION_CALLBACK_HPP_

rclcpp/include/rclcpp/callback_group.hpp

@@ -12,29 +12,33 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_CALLBACK_GROUP_HPP_
-#define RCLCPP_RCLCPP_CALLBACK_GROUP_HPP_
+#ifndef RCLCPP__CALLBACK_GROUP_HPP_
+#define RCLCPP__CALLBACK_GROUP_HPP_
 
 #include <atomic>
+#include <mutex>
 #include <string>
 #include <vector>
 
-#include <rclcpp/subscription.hpp>
-#include <rclcpp/timer.hpp>
-#include <rclcpp/service.hpp>
-#include <rclcpp/client.hpp>
+#include "rclcpp/client.hpp"
+#include "rclcpp/publisher_base.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/subscription_base.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
 
 namespace rclcpp
 {
 
 // Forward declarations for friend statement in class CallbackGroup
-namespace node
-{
-class Node;
-} // namespace node
-
-namespace callback_group
+namespace node_interfaces
 {
+class NodeServices;
+class NodeTimers;
+class NodeTopics;
+class NodeWaitables;
+}  // namespace node_interfaces
 
 enum class CallbackGroupType
 {
@@ -44,87 +48,124 @@ enum class CallbackGroupType
 
 class CallbackGroup
 {
-  friend class rclcpp::node::Node;
+  friend class rclcpp::node_interfaces::NodeServices;
+  friend class rclcpp::node_interfaces::NodeTimers;
+  friend class rclcpp::node_interfaces::NodeTopics;
+  friend class rclcpp::node_interfaces::NodeWaitables;
 
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup);
+  RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup)
 
-  CallbackGroup(CallbackGroupType group_type)
-  : type_(group_type), can_be_taken_from_(true)
-  {}
+  RCLCPP_PUBLIC
+  explicit CallbackGroup(CallbackGroupType group_type);
 
-  const std::vector<subscription::SubscriptionBase::WeakPtr> &
-  get_subscription_ptrs() const
+  template<typename Function>
+  rclcpp::SubscriptionBase::SharedPtr
+  find_subscription_ptrs_if(Function func) const
   {
-    return subscription_ptrs_;
+    return _find_ptrs_if_impl<rclcpp::SubscriptionBase, Function>(func, subscription_ptrs_);
   }
 
-  const std::vector<timer::TimerBase::WeakPtr> &
-  get_timer_ptrs() const
+  template<typename Function>
+  rclcpp::TimerBase::SharedPtr
+  find_timer_ptrs_if(Function func) const
   {
-    return timer_ptrs_;
+    return _find_ptrs_if_impl<rclcpp::TimerBase, Function>(func, timer_ptrs_);
   }
 
-  const std::vector<service::ServiceBase::SharedPtr> &
-  get_service_ptrs() const
+  template<typename Function>
+  rclcpp::ServiceBase::SharedPtr
+  find_service_ptrs_if(Function func) const
   {
-    return service_ptrs_;
+    return _find_ptrs_if_impl<rclcpp::ServiceBase, Function>(func, service_ptrs_);
   }
 
-  const std::vector<client::ClientBase::SharedPtr> &
-  get_client_ptrs() const
+  template<typename Function>
+  rclcpp::ClientBase::SharedPtr
+  find_client_ptrs_if(Function func) const
   {
-    return client_ptrs_;
+    return _find_ptrs_if_impl<rclcpp::ClientBase, Function>(func, client_ptrs_);
   }
 
-  std::atomic_bool & can_be_taken_from()
+  template<typename Function>
+  rclcpp::Waitable::SharedPtr
+  find_waitable_ptrs_if(Function func) const
   {
-    return can_be_taken_from_;
+    return _find_ptrs_if_impl<rclcpp::Waitable, Function>(func, waitable_ptrs_);
   }
 
-  const CallbackGroupType & type() const
-  {
-    return type_;
-  }
+  RCLCPP_PUBLIC
+  std::atomic_bool &
+  can_be_taken_from();
 
-private:
-  RCLCPP_DISABLE_COPY(CallbackGroup);
+  RCLCPP_PUBLIC
+  const CallbackGroupType &
+  type() const;
 
+protected:
+  RCLCPP_DISABLE_COPY(CallbackGroup)
+
+  RCLCPP_PUBLIC
   void
-  add_subscription(
-    const subscription::SubscriptionBase::SharedPtr subscription_ptr)
-  {
-    subscription_ptrs_.push_back(subscription_ptr);
-  }
+  add_publisher(const rclcpp::PublisherBase::SharedPtr publisher_ptr);
 
+  RCLCPP_PUBLIC
   void
-  add_timer(const timer::TimerBase::SharedPtr timer_ptr)
-  {
-    timer_ptrs_.push_back(timer_ptr);
-  }
+  add_subscription(const rclcpp::SubscriptionBase::SharedPtr subscription_ptr);
 
+  RCLCPP_PUBLIC
   void
-  add_service(const service::ServiceBase::SharedPtr service_ptr)
-  {
-    service_ptrs_.push_back(service_ptr);
-  }
+  add_timer(const rclcpp::TimerBase::SharedPtr timer_ptr);
 
+  RCLCPP_PUBLIC
   void
-  add_client(const client::ClientBase::SharedPtr client_ptr)
-  {
-    client_ptrs_.push_back(client_ptr);
-  }
+  add_service(const rclcpp::ServiceBase::SharedPtr service_ptr);
+
+  RCLCPP_PUBLIC
+  void
+  add_client(const rclcpp::ClientBase::SharedPtr client_ptr);
+
+  RCLCPP_PUBLIC
+  void
+  add_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr);
+
+  RCLCPP_PUBLIC
+  void
+  remove_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr) noexcept;
 
   CallbackGroupType type_;
-  std::vector<subscription::SubscriptionBase::WeakPtr> subscription_ptrs_;
-  std::vector<timer::TimerBase::WeakPtr> timer_ptrs_;
-  std::vector<service::ServiceBase::SharedPtr> service_ptrs_;
-  std::vector<client::ClientBase::SharedPtr> client_ptrs_;
+  // Mutex to protect the subsequent vectors of pointers.
+  mutable std::mutex mutex_;
+  std::vector<rclcpp::SubscriptionBase::WeakPtr> subscription_ptrs_;
+  std::vector<rclcpp::TimerBase::WeakPtr> timer_ptrs_;
+  std::vector<rclcpp::ServiceBase::WeakPtr> service_ptrs_;
+  std::vector<rclcpp::ClientBase::WeakPtr> client_ptrs_;
+  std::vector<rclcpp::Waitable::WeakPtr> waitable_ptrs_;
   std::atomic_bool can_be_taken_from_;
 
+private:
+  template<typename TypeT, typename Function>
+  typename TypeT::SharedPtr _find_ptrs_if_impl(
+    Function func, const std::vector<typename TypeT::WeakPtr> & vect_ptrs) const
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    for (auto & weak_ptr : vect_ptrs) {
+      auto ref_ptr = weak_ptr.lock();
+      if (ref_ptr && func(ref_ptr)) {
+        return ref_ptr;
+      }
+    }
+    return typename TypeT::SharedPtr();
+  }
 };
 
-} /* namespace callback_group */
-} /* namespace rclcpp */
+namespace callback_group
+{
 
-#endif /* RCLCPP_RCLCPP_CALLBACK_GROUP_HPP_ */
+using CallbackGroupType [[deprecated("use rclcpp::CallbackGroupType instead")]] = CallbackGroupType;
+using CallbackGroup [[deprecated("use rclcpp::CallbackGroup instead")]] = CallbackGroup;
+
+}  // namespace callback_group
+}  // namespace rclcpp
+
+#endif  // RCLCPP__CALLBACK_GROUP_HPP_

rclcpp/include/rclcpp/client.hpp

@@ -12,153 +12,328 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_CLIENT_HPP_
-#define RCLCPP_RCLCPP_CLIENT_HPP_
+#ifndef RCLCPP__CLIENT_HPP_
+#define RCLCPP__CLIENT_HPP_
 
+#include <atomic>
 #include <future>
-#include <iostream>
 #include <map>
 #include <memory>
 #include <sstream>
+#include <string>
+#include <tuple>
 #include <utility>
 
-#include <rmw/error_handling.h>
-#include <rmw/rmw.h>
+#include "rcl/client.h"
+#include "rcl/error_handling.h"
+#include "rcl/wait.h"
 
-#include <rclcpp/macros.hpp>
-#include <rclcpp/utilities.hpp>
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/function_traits.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_graph_interface.hpp"
+#include "rclcpp/type_support_decl.hpp"
+#include "rclcpp/utilities.hpp"
+#include "rclcpp/expand_topic_or_service_name.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+#include "rcutils/logging_macros.h"
+
+#include "rmw/error_handling.h"
+#include "rmw/rmw.h"
 
 namespace rclcpp
 {
 
-namespace client
+namespace node_interfaces
 {
+class NodeBaseInterface;
+}  // namespace node_interfaces
 
 class ClientBase
 {
-
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientBase);
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientBase)
 
+  RCLCPP_PUBLIC
   ClientBase(
-    std::shared_ptr<rmw_node_t> node_handle,
-    rmw_client_t * client_handle,
-    const std::string & service_name)
-  : node_handle_(node_handle), client_handle_(client_handle), service_name_(service_name)
-  {}
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph);
 
-  virtual ~ClientBase()
-  {
-    if (client_handle_) {
-      if (rmw_destroy_client(client_handle_) != RMW_RET_OK) {
-        fprintf(stderr,
-          "Error in destruction of rmw client handle: %s\n", rmw_get_error_string_safe());
-      }
-    }
-  }
+  RCLCPP_PUBLIC
+  virtual ~ClientBase();
 
-  const std::string & get_service_name() const
-  {
-    return this->service_name_;
-  }
+  /// Take the next response for this client as a type erased pointer.
+  /**
+   * The type erased pointer allows for this method to be used in a type
+   * agnostic way along with ClientBase::create_response(),
+   * ClientBase::create_request_header(), and ClientBase::handle_response().
+   * The typed version of this can be used if the Service type is known,
+   * \sa Client::take_response().
+   *
+   * \param[out] response_out The type erased pointer to a Service Response into
+   *   which the middleware will copy the response being taken.
+   * \param[out] request_header_out The request header to be filled by the
+   *   middleware when taking, and which can be used to associte the response
+   *   to a specific request.
+   * \returns true if the response was taken, otherwise false.
+   * \throws rclcpp::exceptions::RCLError based exceptions if the underlying
+   *   rcl function fail.
+   */
+  RCLCPP_PUBLIC
+  bool
+  take_type_erased_response(void * response_out, rmw_request_id_t & request_header_out);
 
-  const rmw_client_t * get_client_handle() const
+  RCLCPP_PUBLIC
+  const char *
+  get_service_name() const;
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<rcl_client_t>
+  get_client_handle();
+
+  RCLCPP_PUBLIC
+  std::shared_ptr<const rcl_client_t>
+  get_client_handle() const;
+
+  RCLCPP_PUBLIC
+  bool
+  service_is_ready() const;
+
+  template<typename RepT = int64_t, typename RatioT = std::milli>
+  bool
+  wait_for_service(
+    std::chrono::duration<RepT, RatioT> timeout = std::chrono::duration<RepT, RatioT>(-1))
   {
-    return this->client_handle_;
+    return wait_for_service_nanoseconds(
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
+    );
   }
 
   virtual std::shared_ptr<void> create_response() = 0;
-  virtual std::shared_ptr<void> create_request_header() = 0;
+  virtual std::shared_ptr<rmw_request_id_t> create_request_header() = 0;
   virtual void handle_response(
-    std::shared_ptr<void> & request_header, std::shared_ptr<void> & response) = 0;
+    std::shared_ptr<rmw_request_id_t> request_header, std::shared_ptr<void> response) = 0;
 
-private:
-  RCLCPP_DISABLE_COPY(ClientBase);
+  /// Exchange the "in use by wait set" state for this client.
+  /**
+   * This is used to ensure this client is not used by multiple
+   * wait sets at the same time.
+   *
+   * \param[in] in_use_state the new state to exchange into the state, true
+   *   indicates it is now in use by a wait set, and false is that it is no
+   *   longer in use by a wait set.
+   * \returns the previous state.
+   */
+  RCLCPP_PUBLIC
+  bool
+  exchange_in_use_by_wait_set_state(bool in_use_state);
 
-  std::shared_ptr<rmw_node_t> node_handle_;
+protected:
+  RCLCPP_DISABLE_COPY(ClientBase)
 
-  rmw_client_t * client_handle_;
-  std::string service_name_;
+  RCLCPP_PUBLIC
+  bool
+  wait_for_service_nanoseconds(std::chrono::nanoseconds timeout);
 
+  RCLCPP_PUBLIC
+  rcl_node_t *
+  get_rcl_node_handle();
+
+  RCLCPP_PUBLIC
+  const rcl_node_t *
+  get_rcl_node_handle() const;
+
+  rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;
+  std::shared_ptr<rcl_node_t> node_handle_;
+  std::shared_ptr<rclcpp::Context> context_;
+
+  std::shared_ptr<rcl_client_t> client_handle_;
+
+  std::atomic<bool> in_use_by_wait_set_{false};
 };
 
 template<typename ServiceT>
 class Client : public ClientBase
 {
 public:
-  using Promise = std::promise<typename ServiceT::Response::SharedPtr>;
+  using SharedRequest = typename ServiceT::Request::SharedPtr;
+  using SharedResponse = typename ServiceT::Response::SharedPtr;
+
+  using Promise = std::promise<SharedResponse>;
+  using PromiseWithRequest = std::promise<std::pair<SharedRequest, SharedResponse>>;
+
   using SharedPromise = std::shared_ptr<Promise>;
-  using SharedFuture = std::shared_future<typename ServiceT::Response::SharedPtr>;
+  using SharedPromiseWithRequest = std::shared_ptr<PromiseWithRequest>;
 
-  using CallbackType = std::function<void(SharedFuture)>;
+  using SharedFuture = std::shared_future<SharedResponse>;
+  using SharedFutureWithRequest = std::shared_future<std::pair<SharedRequest, SharedResponse>>;
 
-  RCLCPP_SMART_PTR_DEFINITIONS(Client);
+  using CallbackType = std::function<void (SharedFuture)>;
+  using CallbackWithRequestType = std::function<void (SharedFutureWithRequest)>;
+
+  RCLCPP_SMART_PTR_DEFINITIONS(Client)
 
   Client(
-    std::shared_ptr<rmw_node_t> node_handle,
-    rmw_client_t * client_handle,
-    const std::string & service_name)
-  : ClientBase(node_handle, client_handle, service_name)
-  {}
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
+    const std::string & service_name,
+    rcl_client_options_t & client_options)
+  : ClientBase(node_base, node_graph)
+  {
+    using rosidl_typesupport_cpp::get_service_type_support_handle;
+    auto service_type_support_handle =
+      get_service_type_support_handle<ServiceT>();
+    rcl_ret_t ret = rcl_client_init(
+      this->get_client_handle().get(),
+      this->get_rcl_node_handle(),
+      service_type_support_handle,
+      service_name.c_str(),
+      &client_options);
+    if (ret != RCL_RET_OK) {
+      if (ret == RCL_RET_SERVICE_NAME_INVALID) {
+        auto rcl_node_handle = this->get_rcl_node_handle();
+        // this will throw on any validation problem
+        rcl_reset_error();
+        expand_topic_or_service_name(
+          service_name,
+          rcl_node_get_name(rcl_node_handle),
+          rcl_node_get_namespace(rcl_node_handle),
+          true);
+      }
+      rclcpp::exceptions::throw_from_rcl_error(ret, "could not create client");
+    }
+  }
 
-  std::shared_ptr<void> create_response()
+  virtual ~Client()
+  {
+  }
+
+  /// Take the next response for this client.
+  /**
+   * \sa ClientBase::take_type_erased_response().
+   *
+   * \param[out] response_out The reference to a Service Response into
+   *   which the middleware will copy the response being taken.
+   * \param[out] request_header_out The request header to be filled by the
+   *   middleware when taking, and which can be used to associte the response
+   *   to a specific request.
+   * \returns true if the response was taken, otherwise false.
+   * \throws rclcpp::exceptions::RCLError based exceptions if the underlying
+   *   rcl function fail.
+   */
+  bool
+  take_response(typename ServiceT::Response & response_out, rmw_request_id_t & request_header_out)
+  {
+    return this->take_type_erased_response(&response_out, request_header_out);
+  }
+
+  std::shared_ptr<void>
+  create_response() override
   {
     return std::shared_ptr<void>(new typename ServiceT::Response());
   }
 
-  std::shared_ptr<void> create_request_header()
+  std::shared_ptr<rmw_request_id_t>
+  create_request_header() override
   {
     // TODO(wjwwood): This should probably use rmw_request_id's allocator.
     //                (since it is a C type)
-    return std::shared_ptr<void>(new rmw_request_id_t);
+    return std::shared_ptr<rmw_request_id_t>(new rmw_request_id_t);
   }
 
-  void handle_response(std::shared_ptr<void> & request_header, std::shared_ptr<void> & response)
+  void
+  handle_response(
+    std::shared_ptr<rmw_request_id_t> request_header,
+    std::shared_ptr<void> response) override
   {
-    auto typed_request_header = std::static_pointer_cast<rmw_request_id_t>(request_header);
+    std::unique_lock<std::mutex> lock(pending_requests_mutex_);
     auto typed_response = std::static_pointer_cast<typename ServiceT::Response>(response);
-    int64_t sequence_number = typed_request_header->sequence_number;
-    // TODO this must check if the sequence_number is valid otherwise the call_promise will be null
+    int64_t sequence_number = request_header->sequence_number;
+    // TODO(esteve) this should throw instead since it is not expected to happen in the first place
+    if (this->pending_requests_.count(sequence_number) == 0) {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "Received invalid sequence number. Ignoring...");
+      return;
+    }
     auto tuple = this->pending_requests_[sequence_number];
     auto call_promise = std::get<0>(tuple);
     auto callback = std::get<1>(tuple);
     auto future = std::get<2>(tuple);
     this->pending_requests_.erase(sequence_number);
+    // Unlock here to allow the service to be called recursively from one of its callbacks.
+    lock.unlock();
+
     call_promise->set_value(typed_response);
     callback(future);
   }
 
-  SharedFuture async_send_request(
-    typename ServiceT::Request::SharedPtr request)
+  SharedFuture
+  async_send_request(SharedRequest request)
   {
     return async_send_request(request, [](SharedFuture) {});
   }
 
-  SharedFuture async_send_request(
-    typename ServiceT::Request::SharedPtr request,
-    CallbackType cb)
+  template<
+    typename CallbackT,
+    typename std::enable_if<
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        CallbackType
+      >::value
+    >::type * = nullptr
+  >
+  SharedFuture
+  async_send_request(SharedRequest request, CallbackT && cb)
   {
+    std::lock_guard<std::mutex> lock(pending_requests_mutex_);
     int64_t sequence_number;
-    if (RMW_RET_OK != rmw_send_request(get_client_handle(), request.get(), &sequence_number)) {
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-      throw std::runtime_error(
-        std::string("failed to send request: ") + rmw_get_error_string_safe());
-      // *INDENT-ON*
+    rcl_ret_t ret = rcl_send_request(get_client_handle().get(), request.get(), &sequence_number);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request");
     }
 
     SharedPromise call_promise = std::make_shared<Promise>();
     SharedFuture f(call_promise->get_future());
-    pending_requests_[sequence_number] = std::make_tuple(call_promise, cb, f);
+    pending_requests_[sequence_number] =
+      std::make_tuple(call_promise, std::forward<CallbackType>(cb), f);
     return f;
   }
 
+  template<
+    typename CallbackT,
+    typename std::enable_if<
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        CallbackWithRequestType
+      >::value
+    >::type * = nullptr
+  >
+  SharedFutureWithRequest
+  async_send_request(SharedRequest request, CallbackT && cb)
+  {
+    SharedPromiseWithRequest promise = std::make_shared<PromiseWithRequest>();
+    SharedFutureWithRequest future_with_request(promise->get_future());
+
+    auto wrapping_cb = [future_with_request, promise, request, &cb](SharedFuture future) {
+        auto response = future.get();
+        promise->set_value(std::make_pair(request, response));
+        cb(future_with_request);
+      };
+
+    async_send_request(request, wrapping_cb);
+
+    return future_with_request;
+  }
+
 private:
-  RCLCPP_DISABLE_COPY(Client);
+  RCLCPP_DISABLE_COPY(Client)
 
   std::map<int64_t, std::tuple<SharedPromise, CallbackType, SharedFuture>> pending_requests_;
+  std::mutex pending_requests_mutex_;
 };
 
-} /* namespace client */
-} /* namespace rclcpp */
+}  // namespace rclcpp
 
-#endif /* RCLCPP_RCLCPP_CLIENT_HPP_ */
+#endif  // RCLCPP__CLIENT_HPP_

rclcpp/include/rclcpp/clock.hpp

@@ -0,0 +1,149 @@
+// Copyright 2017 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__CLOCK_HPP_
+#define RCLCPP__CLOCK_HPP_
+
+#include <functional>
+#include <memory>
+#include <mutex>
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/time.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+#include "rcl/time.h"
+#include "rcutils/time.h"
+#include "rcutils/types/rcutils_ret.h"
+
+namespace rclcpp
+{
+
+class TimeSource;
+
+class JumpHandler
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(JumpHandler)
+
+  using pre_callback_t = std::function<void ()>;
+  using post_callback_t = std::function<void (const rcl_time_jump_t &)>;
+
+  JumpHandler(
+    pre_callback_t pre_callback,
+    post_callback_t post_callback,
+    const rcl_jump_threshold_t & threshold);
+
+  pre_callback_t pre_callback;
+  post_callback_t post_callback;
+  rcl_jump_threshold_t notice_threshold;
+};
+
+class Clock
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(Clock)
+
+  /// Default c'tor
+  /**
+   * Initializes the clock instance with the given clock_type.
+   *
+   * \param clock_type type of the clock.
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
+   */
+  RCLCPP_PUBLIC
+  explicit Clock(rcl_clock_type_t clock_type = RCL_SYSTEM_TIME);
+
+  RCLCPP_PUBLIC
+  ~Clock();
+
+  /**
+   * Returns current time from the time source specified by clock_type.
+   *
+   * \return current time.
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
+   */
+  RCLCPP_PUBLIC
+  Time
+  now();
+
+  /**
+   * Returns the clock of the type `RCL_ROS_TIME` is active.
+   *
+   * \return true if the clock is active
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw if
+   * the current clock does not have the clock_type `RCL_ROS_TIME`.
+   */
+  RCLCPP_PUBLIC
+  bool
+  ros_time_is_active();
+
+  RCLCPP_PUBLIC
+  rcl_clock_t *
+  get_clock_handle() noexcept;
+
+  RCLCPP_PUBLIC
+  rcl_clock_type_t
+  get_clock_type() const noexcept;
+
+  RCLCPP_PUBLIC
+  std::mutex &
+  get_clock_mutex() noexcept;
+
+  // Add a callback to invoke if the jump threshold is exceeded.
+  /**
+   * These callback functions must remain valid as long as the
+   * returned shared pointer is valid.
+   *
+   * Function will register callbacks to the callback queue. On time jump all
+   * callbacks will be executed whose threshold is greater then the time jump;
+   * The logic will first call selected pre_callbacks and then all selected
+   * post_callbacks.
+   *
+   * Function is only applicable if the clock_type is `RCL_ROS_TIME`
+   *
+   * \param pre_callback. Must be non-throwing
+   * \param post_callback. Must be non-throwing.
+   * \param threshold. Callbacks will be triggered if the time jump is greater
+   * then the threshold.
+   * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
+   * \throws std::bad_alloc if the allocation of the JumpHandler fails.
+   * \warning the instance of the clock must remain valid as long as any created
+   * JumpHandler.
+   */
+  RCLCPP_PUBLIC
+  JumpHandler::SharedPtr
+  create_jump_callback(
+    JumpHandler::pre_callback_t pre_callback,
+    JumpHandler::post_callback_t post_callback,
+    const rcl_jump_threshold_t & threshold);
+
+private:
+  // Invoke time jump callback
+  RCLCPP_PUBLIC
+  static void
+  on_time_jump(
+    const struct rcl_time_jump_t * time_jump,
+    bool before_jump,
+    void * user_data);
+
+  /// Private internal storage
+  class Impl;
+
+  std::shared_ptr<Impl> impl_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__CLOCK_HPP_

rclcpp/include/rclcpp/context.hpp

@@ -12,52 +12,306 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_CONTEXT_HPP_
-#define RCLCPP_RCLCPP_CONTEXT_HPP_
-
-#include <rclcpp/macros.hpp>
-
-#include <iostream>
+#ifndef RCLCPP__CONTEXT_HPP_
+#define RCLCPP__CONTEXT_HPP_
 
+#include <condition_variable>
+#include <functional>
 #include <memory>
 #include <mutex>
-#include <typeinfo>
+#include <string>
 #include <typeindex>
+#include <typeinfo>
 #include <unordered_map>
+#include <utility>
+#include <vector>
 
-#include <rmw/rmw.h>
+#include "rcl/context.h"
+#include "rcl/guard_condition.h"
+#include "rcl/wait.h"
+#include "rclcpp/init_options.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
 
-namespace context
-{
-
-class Context
+/// Thrown when init is called on an already initialized context.
+class ContextAlreadyInitialized : public std::runtime_error
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(Context);
+  ContextAlreadyInitialized()
+  : std::runtime_error("context is already initialized") {}
+};
 
-  Context() {}
+/// Context which encapsulates shared state between nodes and other similar entities.
+/**
+ * A context also represents the lifecycle between init and shutdown of rclcpp.
+ * It is often used in conjunction with rclcpp::init, or rclcpp::init_local,
+ * and rclcpp::shutdown.
+ */
+class Context : public std::enable_shared_from_this<Context>
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(Context)
 
+  /// Default constructor, after which the Context is still not "initialized".
+  /**
+   * Every context which is constructed is added to a global vector of contexts,
+   * which is used by the signal handler to conditionally shutdown each context
+   * on SIGINT.
+   * See the shutdown_on_sigint option in the InitOptions class.
+   */
+  RCLCPP_PUBLIC
+  Context();
+
+  RCLCPP_PUBLIC
+  virtual
+  ~Context();
+
+  /// Initialize the context, and the underlying elements like the rcl context.
+  /**
+   * This method must be called before passing this context to things like the
+   * constructor of Node.
+   * It must also be called before trying to shutdown the context.
+   *
+   * Note that this function does not setup any signal handlers, so if you want
+   * it to be shutdown by the signal handler, then you need to either install
+   * them manually with rclcpp::install_signal_handers() or use rclcpp::init().
+   * In addition to installing the signal handlers, the shutdown_on_sigint
+   * of the InitOptions needs to be `true` for this context to be shutdown by
+   * the signal handler, otherwise it will be passed over.
+   *
+   * After calling this method, shutdown() can be called to invalidate the
+   * context for derived entities, e.g. nodes, guard conditions, etc.
+   * However, the underlying rcl context is not finalized until this Context's
+   * destructor is called or this function is called again.
+   * Allowing this class to go out of scope and get destructed or calling this
+   * function a second time while derived entities are still using the context
+   * is undefined behavior and should be avoided.
+   * It's a good idea to not reuse context objects and instead create a new one
+   * each time you need to shutdown and init again.
+   * This allows derived entities to hold on to shard pointers to the first
+   * context object until they are done.
+   *
+   * This function is thread-safe.
+   *
+   * \param[in] argc number of arguments
+   * \param[in] argv argument array which may contain arguments intended for ROS
+   * \param[in] init_options initialization options for rclcpp and underlying layers
+   * \throw ContextAlreadyInitialized if called if init is called more than once
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  init(
+    int argc,
+    char const * const argv[],
+    const rclcpp::InitOptions & init_options = rclcpp::InitOptions());
+
+  /// Return true if the context is valid, otherwise false.
+  /**
+   * The context is valid if it has been initialized but not shutdown.
+   *
+   * This function is thread-safe.
+   * This function is lock free so long as pointers and uint64_t atomics are
+   * lock free.
+   *
+   * \return true if valid, otherwise false
+   */
+  RCLCPP_PUBLIC
+  bool
+  is_valid() const;
+
+  /// Return the init options used during init.
+  RCLCPP_PUBLIC
+  const rclcpp::InitOptions &
+  get_init_options() const;
+
+  /// Return a copy of the init options used during init.
+  RCLCPP_PUBLIC
+  rclcpp::InitOptions
+  get_init_options();
+
+  /// Return the shutdown reason, or empty string if not shutdown.
+  /**
+   * This function is thread-safe.
+   */
+  RCLCPP_PUBLIC
+  std::string
+  shutdown_reason();
+
+  /// Shutdown the context, making it uninitialized and therefore invalid for derived entities.
+  /**
+   * Several things happen when the context is shutdown, in this order:
+   *
+   * - acquires a lock to prevent race conditions with init, on_shutdown, etc.
+   * - if the context is not initialized, return false
+   * - rcl_shutdown() is called on the internal rcl_context_t instance
+   * - the shutdown reason is set
+   * - each on_shutdown callback is called, in the order that they were added
+   * - interrupt blocking sleep_for() calls, so they return early due to shutdown
+   * - interrupt blocking executors and wait sets
+   *
+   * The underlying rcl context is not finalized by this function.
+   *
+   * This function is thread-safe.
+   *
+   * \param[in] reason the description of why shutdown happened
+   * \return true if shutdown was successful, false if context was already shutdown
+   * \throw various exceptions derived from rclcpp::exceptions::RCLError, if rcl_shutdown fails
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  shutdown(const std::string & reason);
+
+  using OnShutdownCallback = std::function<void ()>;
+
+  /// Add a on_shutdown callback to be called when shutdown is called for this context.
+  /**
+   * These callbacks will be called in the order they are added as the second
+   * to last step in shutdown().
+   *
+   * When shutdown occurs due to the signal handler, these callbacks are run
+   * asynchronoulsy in the dedicated singal handling thread.
+   *
+   * Also, shutdown() may be called from the destructor of this function.
+   * Therefore, it is not safe to throw exceptions from these callbacks.
+   * Instead, log errors or use some other mechanism to indicate an error has
+   * occurred.
+   *
+   * On shutdown callbacks may be registered before init and after shutdown,
+   * and persist on repeated init's.
+   *
+   * \param[in] callback the on shutdown callback to be registered
+   * \return the callback passed, for convenience when storing a passed lambda
+   */
+  RCLCPP_PUBLIC
+  virtual
+  OnShutdownCallback
+  on_shutdown(OnShutdownCallback callback);
+
+  /// Return the shutdown callbacks as const.
+  /**
+   * Using the returned reference is not thread-safe with calls that modify
+   * the list of "on shutdown" callbacks, i.e. on_shutdown().
+   */
+  RCLCPP_PUBLIC
+  const std::vector<OnShutdownCallback> &
+  get_on_shutdown_callbacks() const;
+
+  /// Return the shutdown callbacks.
+  /**
+   * Using the returned reference is not thread-safe with calls that modify
+   * the list of "on shutdown" callbacks, i.e. on_shutdown().
+   */
+  RCLCPP_PUBLIC
+  std::vector<OnShutdownCallback> &
+  get_on_shutdown_callbacks();
+
+  /// Return the internal rcl context.
+  RCLCPP_PUBLIC
+  std::shared_ptr<rcl_context_t>
+  get_rcl_context();
+
+  /// Sleep for a given period of time or until shutdown() is called.
+  /**
+   * This function can be interrupted early if:
+   *
+   *   - this context is shutdown()
+   *   - this context is destructed (resulting in shutdown)
+   *   - this context has shutdown_on_sigint=true and SIGINT occurs (resulting in shutdown)
+   *   - interrupt_all_sleep_for() is called
+   *
+   * \param[in] nanoseconds A std::chrono::duration representing how long to sleep for.
+   * \return true if the condition variable did not timeout, i.e. you were interrupted.
+   */
+  RCLCPP_PUBLIC
+  bool
+  sleep_for(const std::chrono::nanoseconds & nanoseconds);
+
+  /// Interrupt any blocking sleep_for calls, causing them to return immediately and return true.
+  RCLCPP_PUBLIC
+  virtual
+  void
+  interrupt_all_sleep_for();
+
+  /// Get a handle to the guard condition which is triggered when interrupted.
+  /**
+   * This guard condition is triggered any time interrupt_all_wait_sets() is
+   * called, which may be called by the user, or shutdown().
+   * And in turn, shutdown() may be called by the user, the destructor of this
+   * context, or the signal handler if installed and shutdown_on_sigint is true
+   * for this context.
+   *
+   * The first time that this function is called for a given wait set a new guard
+   * condition will be created and returned; thereafter the same guard condition
+   * will be returned for the same wait set.
+   * This mechanism is designed to ensure that the same guard condition is not
+   * reused across wait sets (e.g., when using multiple executors in the same
+   * process).
+   * This method will throw an exception if initialization of the guard
+   * condition fails.
+   *
+   * The returned guard condition needs to be released with the
+   * release_interrupt_guard_condition() method in order to reclaim resources.
+   *
+   * \param[in] wait_set Pointer to the rcl_wait_set_t that will be using the
+   *   resulting guard condition.
+   * \return Pointer to the guard condition.
+   */
+  RCLCPP_PUBLIC
+  rcl_guard_condition_t *
+  get_interrupt_guard_condition(rcl_wait_set_t * wait_set);
+
+  /// Release the previously allocated guard condition which is triggered when interrupted.
+  /**
+   * If you previously called get_interrupt_guard_condition() for a given wait
+   * set to get a interrupt guard condition, then you should call
+   * release_interrupt_guard_condition() when you're done, to free that
+   * condition.
+   * Will throw an exception if get_interrupt_guard_condition() wasn't
+   * previously called for the given wait set.
+   *
+   * After calling this, the pointer returned by get_interrupt_guard_condition()
+   * for the given wait_set is invalid.
+   *
+   * \param[in] wait_set Pointer to the rcl_wait_set_t that was using the
+   *   resulting guard condition.
+   */
+  RCLCPP_PUBLIC
+  void
+  release_interrupt_guard_condition(rcl_wait_set_t * wait_set);
+
+  /// Nothrow version of release_interrupt_guard_condition(), logs to RCLCPP_ERROR instead.
+  RCLCPP_PUBLIC
+  void
+  release_interrupt_guard_condition(rcl_wait_set_t * wait_set, const std::nothrow_t &) noexcept;
+
+  /// Interrupt any blocking executors, or wait sets associated with this context.
+  RCLCPP_PUBLIC
+  virtual
+  void
+  interrupt_all_wait_sets();
+
+  /// Return a singleton instance for the SubContext type, constructing one if necessary.
   template<typename SubContext, typename ... Args>
   std::shared_ptr<SubContext>
   get_sub_context(Args && ... args)
   {
-    std::lock_guard<std::mutex> lock(mutex_);
+    std::lock_guard<std::recursive_mutex> lock(sub_contexts_mutex_);
 
     std::type_index type_i(typeid(SubContext));
     std::shared_ptr<SubContext> sub_context;
     auto it = sub_contexts_.find(type_i);
     if (it == sub_contexts_.end()) {
       // It doesn't exist yet, make it
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
       sub_context = std::shared_ptr<SubContext>(
         new SubContext(std::forward<Args>(args) ...),
-        [] (SubContext * sub_context_ptr) {
+        [](SubContext * sub_context_ptr) {
           delete sub_context_ptr;
         });
-      // *INDENT-ON*
       sub_contexts_[type_i] = sub_context;
     } else {
       // It exists, get it out and cast it.
@@ -66,15 +320,54 @@ public:
     return sub_context;
   }
 
+protected:
+  // Called by constructor and destructor to clean up by finalizing the
+  // shutdown rcl context and preparing for a new init cycle.
+  RCLCPP_PUBLIC
+  virtual
+  void
+  clean_up();
+
 private:
-  RCLCPP_DISABLE_COPY(Context);
+  RCLCPP_DISABLE_COPY(Context)
+
+  // This mutex is recursive so that the destructor can ensure atomicity
+  // between is_initialized and shutdown.
+  std::recursive_mutex init_mutex_;
+  std::shared_ptr<rcl_context_t> rcl_context_;
+  rclcpp::InitOptions init_options_;
+  std::string shutdown_reason_;
+
+  // Keep shared ownership of global logging configure mutex.
+  std::shared_ptr<std::mutex> logging_configure_mutex_;
 
   std::unordered_map<std::type_index, std::shared_ptr<void>> sub_contexts_;
-  std::mutex mutex_;
+  // This mutex is recursive so that the constructor of a sub context may
+  // attempt to acquire another sub context.
+  std::recursive_mutex sub_contexts_mutex_;
 
+  std::vector<OnShutdownCallback> on_shutdown_callbacks_;
+  std::mutex on_shutdown_callbacks_mutex_;
+
+  /// Condition variable for timed sleep (see sleep_for).
+  std::condition_variable interrupt_condition_variable_;
+  /// Mutex for protecting the global condition variable.
+  std::mutex interrupt_mutex_;
+
+  /// Mutex to protect sigint_guard_cond_handles_.
+  std::mutex interrupt_guard_cond_handles_mutex_;
+  /// Guard conditions for interrupting of associated wait sets on interrupt_all_wait_sets().
+  std::unordered_map<rcl_wait_set_t *, rcl_guard_condition_t> interrupt_guard_cond_handles_;
 };
 
-} /* namespace context */
-} /* namespace rclcpp */
+/// Return a copy of the list of context shared pointers.
+/**
+ * This function is thread-safe.
+ */
+RCLCPP_PUBLIC
+std::vector<Context::SharedPtr>
+get_contexts();
 
-#endif /* RCLCPP_RCLCPP_CONTEXT_HPP_ */
+}  // namespace rclcpp
+
+#endif  // RCLCPP__CONTEXT_HPP_

rclcpp/include/rclcpp/contexts/default_context.hpp

@@ -12,36 +12,47 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_CONTEXTS_DEFAULT_CONTEXT_HPP_
-#define RCLCPP_RCLCPP_CONTEXTS_DEFAULT_CONTEXT_HPP_
+#ifndef RCLCPP__CONTEXTS__DEFAULT_CONTEXT_HPP_
+#define RCLCPP__CONTEXTS__DEFAULT_CONTEXT_HPP_
 
-#include <rclcpp/context.hpp>
+#include "rclcpp/context.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
 namespace contexts
 {
+
+class DefaultContext : public rclcpp::Context
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(DefaultContext)
+
+  RCLCPP_PUBLIC
+  DefaultContext();
+};
+
+RCLCPP_PUBLIC
+DefaultContext::SharedPtr
+get_global_default_context();
+
 namespace default_context
 {
 
-class DefaultContext : public rclcpp::context::Context
-{
-public:
-  RCLCPP_SMART_PTR_DEFINITIONS(DefaultContext);
-
-  DefaultContext() {}
-
-};
+using DefaultContext
+[[deprecated("use rclcpp::contexts::DefaultContext instead")]] = DefaultContext;
 
+[[deprecated("use rclcpp::contexts::get_global_default_context() instead")]]
+RCLCPP_PUBLIC
+inline
 DefaultContext::SharedPtr
 get_global_default_context()
 {
-  static DefaultContext::SharedPtr default_context = DefaultContext::make_shared();
-  return default_context;
+  return rclcpp::contexts::get_global_default_context();
 }
 
-} // namespace default_context
-} // namespace contexts
-} // namespace rclcpp
+}  // namespace default_context
+}  // namespace contexts
+}  // namespace rclcpp
 
-#endif /* RCLCPP_RCLCPP_CONTEXTS_DEFAULT_CONTEXT_HPP_ */
+#endif  // RCLCPP__CONTEXTS__DEFAULT_CONTEXT_HPP_

rclcpp/include/rclcpp/create_client.hpp

@@ -0,0 +1,56 @@
+// Copyright 2019 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__CREATE_CLIENT_HPP_
+#define RCLCPP__CREATE_CLIENT_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_services_interface.hpp"
+#include "rmw/rmw.h"
+
+namespace rclcpp
+{
+
+/// Create a service client with a given type.
+/// \internal
+template<typename ServiceT>
+typename rclcpp::Client<ServiceT>::SharedPtr
+create_client(
+  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
+  std::shared_ptr<node_interfaces::NodeGraphInterface> node_graph,
+  std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+  const std::string & service_name,
+  const rmw_qos_profile_t & qos_profile,
+  rclcpp::CallbackGroup::SharedPtr group)
+{
+  rcl_client_options_t options = rcl_client_get_default_options();
+  options.qos = qos_profile;
+
+  auto cli = rclcpp::Client<ServiceT>::make_shared(
+    node_base.get(),
+    node_graph,
+    service_name,
+    options);
+
+  auto cli_base_ptr = std::dynamic_pointer_cast<rclcpp::ClientBase>(cli);
+  node_services->add_client(cli_base_ptr, group);
+  return cli;
+}
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__CREATE_CLIENT_HPP_

rclcpp/include/rclcpp/create_publisher.hpp

@@ -0,0 +1,71 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__CREATE_PUBLISHER_HPP_
+#define RCLCPP__CREATE_PUBLISHER_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/node_interfaces/get_node_topics_interface.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/node_options.hpp"
+#include "rclcpp/publisher_factory.hpp"
+#include "rclcpp/publisher_options.hpp"
+#include "rclcpp/qos.hpp"
+#include "rmw/qos_profiles.h"
+
+namespace rclcpp
+{
+
+/// Create and return a publisher of the given MessageT type.
+/**
+ * The NodeT type only needs to have a method called get_node_topics_interface()
+ * which returns a shared_ptr to a NodeTopicsInterface.
+ */
+template<
+  typename MessageT,
+  typename AllocatorT = std::allocator<void>,
+  typename PublisherT = rclcpp::Publisher<MessageT, AllocatorT>,
+  typename NodeT>
+std::shared_ptr<PublisherT>
+create_publisher(
+  NodeT & node,
+  const std::string & topic_name,
+  const rclcpp::QoS & qos,
+  const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options = (
+    rclcpp::PublisherOptionsWithAllocator<AllocatorT>()
+  )
+)
+{
+  // Extract the NodeTopicsInterface from the NodeT.
+  using rclcpp::node_interfaces::get_node_topics_interface;
+  auto node_topics = get_node_topics_interface(node);
+
+  // Create the publisher.
+  auto pub = node_topics->create_publisher(
+    topic_name,
+    rclcpp::create_publisher_factory<MessageT, AllocatorT, PublisherT>(options),
+    qos
+  );
+
+  // Add the publisher to the node topics interface.
+  node_topics->add_publisher(pub, options.callback_group);
+
+  return std::dynamic_pointer_cast<PublisherT>(pub);
+}
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__CREATE_PUBLISHER_HPP_

rclcpp/include/rclcpp/create_service.hpp

@@ -0,0 +1,58 @@
+// Copyright 2018 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__CREATE_SERVICE_HPP_
+#define RCLCPP__CREATE_SERVICE_HPP_
+
+#include <memory>
+#include <string>
+#include <utility>
+
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_services_interface.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rmw/rmw.h"
+
+namespace rclcpp
+{
+
+/// Create a service with a given type.
+/// \internal
+template<typename ServiceT, typename CallbackT>
+typename rclcpp::Service<ServiceT>::SharedPtr
+create_service(
+  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
+  std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+  const std::string & service_name,
+  CallbackT && callback,
+  const rmw_qos_profile_t & qos_profile,
+  rclcpp::CallbackGroup::SharedPtr group)
+{
+  rclcpp::AnyServiceCallback<ServiceT> any_service_callback;
+  any_service_callback.set(std::forward<CallbackT>(callback));
+
+  rcl_service_options_t service_options = rcl_service_get_default_options();
+  service_options.qos = qos_profile;
+
+  auto serv = Service<ServiceT>::make_shared(
+    node_base->get_shared_rcl_node_handle(),
+    service_name, any_service_callback, service_options);
+  auto serv_base_ptr = std::dynamic_pointer_cast<ServiceBase>(serv);
+  node_services->add_service(serv_base_ptr, group);
+  return serv;
+}
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__CREATE_SERVICE_HPP_

rclcpp/include/rclcpp/create_subscription.hpp

@@ -0,0 +1,127 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__CREATE_SUBSCRIPTION_HPP_
+#define RCLCPP__CREATE_SUBSCRIPTION_HPP_
+
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+#include <utility>
+
+#include "rclcpp/detail/resolve_enable_topic_statistics.hpp"
+
+#include "rclcpp/node_interfaces/get_node_timers_interface.hpp"
+#include "rclcpp/node_interfaces/get_node_topics_interface.hpp"
+#include "rclcpp/node_interfaces/node_timers_interface.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+
+#include "rclcpp/create_timer.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp/subscription_factory.hpp"
+#include "rclcpp/subscription_options.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp"
+#include "rmw/qos_profiles.h"
+
+namespace rclcpp
+{
+
+/// Create and return a subscription of the given MessageT type.
+/**
+ * The NodeT type only needs to have a method called get_node_topics_interface()
+ * which returns a shared_ptr to a NodeTopicsInterface, or be a
+ * NodeTopicsInterface pointer itself.
+ */
+template<
+  typename MessageT,
+  typename CallbackT,
+  typename AllocatorT = std::allocator<void>,
+  typename CallbackMessageT =
+  typename rclcpp::subscription_traits::has_message_type<CallbackT>::type,
+  typename SubscriptionT = rclcpp::Subscription<CallbackMessageT, AllocatorT>,
+  typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy<
+    CallbackMessageT,
+    AllocatorT
+  >,
+  typename NodeT>
+typename std::shared_ptr<SubscriptionT>
+create_subscription(
+  NodeT && node,
+  const std::string & topic_name,
+  const rclcpp::QoS & qos,
+  CallbackT && callback,
+  const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
+    rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
+  ),
+  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = (
+    MessageMemoryStrategyT::create_default()
+  )
+)
+{
+  using rclcpp::node_interfaces::get_node_topics_interface;
+  auto node_topics = get_node_topics_interface(std::forward<NodeT>(node));
+
+  std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>>
+  subscription_topic_stats = nullptr;
+
+  if (rclcpp::detail::resolve_enable_topic_statistics(
+      options,
+      *node_topics->get_node_base_interface()))
+  {
+    std::shared_ptr<Publisher<statistics_msgs::msg::MetricsMessage>> publisher =
+      create_publisher<statistics_msgs::msg::MetricsMessage>(
+      node,
+      options.topic_stats_options.publish_topic,
+      qos);
+
+    subscription_topic_stats = std::make_shared<
+      rclcpp::topic_statistics::SubscriptionTopicStatistics<CallbackMessageT>
+      >(node_topics->get_node_base_interface()->get_name(), publisher);
+
+    auto sub_call_back = [subscription_topic_stats]() {
+        subscription_topic_stats->publish_message();
+      };
+
+    auto node_timer_interface = node_topics->get_node_timers_interface();
+
+    auto timer = create_wall_timer(
+      std::chrono::duration_cast<std::chrono::nanoseconds>(
+        options.topic_stats_options.publish_period),
+      sub_call_back,
+      options.callback_group,
+      node_topics->get_node_base_interface(),
+      node_timer_interface
+    );
+
+    subscription_topic_stats->set_publisher_timer(timer);
+  }
+
+  auto factory = rclcpp::create_subscription_factory<MessageT>(
+    std::forward<CallbackT>(callback),
+    options,
+    msg_mem_strat,
+    subscription_topic_stats
+  );
+
+  auto sub = node_topics->create_subscription(topic_name, factory, qos);
+  node_topics->add_subscription(sub, options.callback_group);
+
+  return std::dynamic_pointer_cast<SubscriptionT>(sub);
+}
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__CREATE_SUBSCRIPTION_HPP_

rclcpp/include/rclcpp/create_timer.hpp

@@ -0,0 +1,115 @@
+// Copyright 2019 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__CREATE_TIMER_HPP_
+#define RCLCPP__CREATE_TIMER_HPP_
+
+#include <chrono>
+#include <exception>
+#include <memory>
+#include <string>
+#include <utility>
+
+#include "rclcpp/duration.hpp"
+#include "rclcpp/node_interfaces/get_node_base_interface.hpp"
+#include "rclcpp/node_interfaces/get_node_timers_interface.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_timers_interface.hpp"
+
+namespace rclcpp
+{
+/// Create a timer with a given clock
+/// \internal
+template<typename CallbackT>
+typename rclcpp::TimerBase::SharedPtr
+create_timer(
+  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
+  std::shared_ptr<node_interfaces::NodeTimersInterface> node_timers,
+  rclcpp::Clock::SharedPtr clock,
+  rclcpp::Duration period,
+  CallbackT && callback,
+  rclcpp::CallbackGroup::SharedPtr group = nullptr)
+{
+  auto timer = rclcpp::GenericTimer<CallbackT>::make_shared(
+    clock,
+    period.to_chrono<std::chrono::nanoseconds>(),
+    std::forward<CallbackT>(callback),
+    node_base->get_context());
+
+  node_timers->add_timer(timer, group);
+  return timer;
+}
+
+/// Create a timer with a given clock
+template<typename NodeT, typename CallbackT>
+typename rclcpp::TimerBase::SharedPtr
+create_timer(
+  NodeT node,
+  rclcpp::Clock::SharedPtr clock,
+  rclcpp::Duration period,
+  CallbackT && callback,
+  rclcpp::CallbackGroup::SharedPtr group = nullptr)
+{
+  return create_timer(
+    rclcpp::node_interfaces::get_node_base_interface(node),
+    rclcpp::node_interfaces::get_node_timers_interface(node),
+    clock,
+    period,
+    std::forward<CallbackT>(callback),
+    group);
+}
+
+/// Convenience method to create a timer with node resources.
+/**
+ *
+ * \tparam DurationRepT
+ * \tparam DurationT
+ * \tparam CallbackT
+ * \param period period to exectute callback
+ * \param callback callback to execute via the timer period
+ * \param group
+ * \param node_base
+ * \param node_timers
+ * \return
+ * \throws std::invalid argument if either node_base or node_timers
+ * are null
+ */
+template<typename DurationRepT, typename DurationT, typename CallbackT>
+typename rclcpp::WallTimer<CallbackT>::SharedPtr
+create_wall_timer(
+  std::chrono::duration<DurationRepT, DurationT> period,
+  CallbackT callback,
+  rclcpp::CallbackGroup::SharedPtr group,
+  node_interfaces::NodeBaseInterface * node_base,
+  node_interfaces::NodeTimersInterface * node_timers)
+{
+  if (node_base == nullptr) {
+    throw std::invalid_argument{"input node_base cannot be null"};
+  }
+
+  if (node_timers == nullptr) {
+    throw std::invalid_argument{"input node_timers cannot be null"};
+  }
+
+  auto timer = rclcpp::WallTimer<CallbackT>::make_shared(
+    std::chrono::duration_cast<std::chrono::nanoseconds>(period),
+    std::move(callback),
+    node_base->get_context());
+  node_timers->add_timer(timer, group);
+  return timer;
+}
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__CREATE_TIMER_HPP_

rclcpp/include/rclcpp/detail/README.md

@@ -0,0 +1,3 @@
+Notice that headers in this folder should only provide symbols in the rclcpp::detail namespace.
+
+Also that these headers are not considered part of the public API and are subject to change without notice.

rclcpp/include/rclcpp/detail/resolve_enable_topic_statistics.hpp

@@ -0,0 +1,53 @@
+// Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved.
+//
+// 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_ENABLE_TOPIC_STATISTICS_HPP_
+#define RCLCPP__DETAIL__RESOLVE_ENABLE_TOPIC_STATISTICS_HPP_
+
+#include <stdexcept>
+
+#include "rclcpp/topic_statistics_state.hpp"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+/// Return whether or not topic statistics is enabled, resolving "NodeDefault" if needed.
+template<typename OptionsT, typename NodeBaseT>
+bool
+resolve_enable_topic_statistics(const OptionsT & options, const NodeBaseT & node_base)
+{
+  bool topic_stats_enabled;
+  switch (options.topic_stats_options.state) {
+    case TopicStatisticsState::Enable:
+      topic_stats_enabled = true;
+      break;
+    case TopicStatisticsState::Disable:
+      topic_stats_enabled = false;
+      break;
+    case TopicStatisticsState::NodeDefault:
+      topic_stats_enabled = node_base.get_enable_topic_statistics_default();
+      break;
+    default:
+      throw std::runtime_error("Unrecognized EnableTopicStatistics value");
+  }
+
+  return topic_stats_enabled;
+}
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__RESOLVE_ENABLE_TOPIC_STATISTICS_HPP_

rclcpp/include/rclcpp/detail/resolve_intra_process_buffer_type.hpp

@@ -0,0 +1,54 @@
+// Copyright 2019 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_INTRA_PROCESS_BUFFER_TYPE_HPP_
+#define RCLCPP__DETAIL__RESOLVE_INTRA_PROCESS_BUFFER_TYPE_HPP_
+
+#include <stdexcept>
+
+#include "rclcpp/any_subscription_callback.hpp"
+#include "rclcpp/intra_process_buffer_type.hpp"
+
+namespace rclcpp
+{
+
+namespace detail
+{
+
+/// Return the buffer type, resolving the "CallbackDefault" type to an actual type if needed.
+template<typename CallbackMessageT, typename AllocatorT>
+rclcpp::IntraProcessBufferType
+resolve_intra_process_buffer_type(
+  const rclcpp::IntraProcessBufferType buffer_type,
+  const rclcpp::AnySubscriptionCallback<CallbackMessageT, AllocatorT> & any_subscription_callback)
+{
+  rclcpp::IntraProcessBufferType resolved_buffer_type = buffer_type;
+
+  // If the user has not specified a type for the intra-process buffer, use the callback's type.
+  if (resolved_buffer_type == IntraProcessBufferType::CallbackDefault) {
+    if (any_subscription_callback.use_take_shared_method()) {
+      resolved_buffer_type = IntraProcessBufferType::SharedPtr;
+    } else {
+      resolved_buffer_type = IntraProcessBufferType::UniquePtr;
+    }
+  }
+
+  return resolved_buffer_type;
+}
+
+}  // namespace detail
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__RESOLVE_INTRA_PROCESS_BUFFER_TYPE_HPP_

rclcpp/include/rclcpp/detail/resolve_use_intra_process.hpp

@@ -0,0 +1,56 @@
+// Copyright 2019 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_USE_INTRA_PROCESS_HPP_
+#define RCLCPP__DETAIL__RESOLVE_USE_INTRA_PROCESS_HPP_
+
+#include <stdexcept>
+
+#include "rclcpp/intra_process_setting.hpp"
+
+namespace rclcpp
+{
+
+namespace detail
+{
+
+/// Return whether or not intra process is enabled, resolving "NodeDefault" if needed.
+template<typename OptionsT, typename NodeBaseT>
+bool
+resolve_use_intra_process(const OptionsT & options, const NodeBaseT & node_base)
+{
+  bool use_intra_process;
+  switch (options.use_intra_process_comm) {
+    case IntraProcessSetting::Enable:
+      use_intra_process = true;
+      break;
+    case IntraProcessSetting::Disable:
+      use_intra_process = false;
+      break;
+    case IntraProcessSetting::NodeDefault:
+      use_intra_process = node_base.get_use_intra_process_default();
+      break;
+    default:
+      throw std::runtime_error("Unrecognized IntraProcessSetting value");
+      break;
+  }
+
+  return use_intra_process;
+}
+
+}  // namespace detail
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__RESOLVE_USE_INTRA_PROCESS_HPP_

rclcpp/include/rclcpp/detail/rmw_implementation_specific_payload.hpp

@@ -0,0 +1,51 @@
+// Copyright 2019 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__RMW_IMPLEMENTATION_SPECIFIC_PAYLOAD_HPP_
+#define RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_PAYLOAD_HPP_
+
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+/// Mechanism for passing rmw implementation specific settings through the ROS interfaces.
+class RCLCPP_PUBLIC RMWImplementationSpecificPayload
+{
+public:
+  virtual
+  ~RMWImplementationSpecificPayload() = default;
+
+  /// Return false if this class has not been customized, otherwise true.
+  /**
+   * It does this based on the value of the rmw implementation identifier that
+   * this class reports, and so it is important for a specialization of this
+   * class to override the get_rmw_implementation_identifier() method to return
+   * something other than nullptr.
+   */
+  bool
+  has_been_customized() const;
+
+  /// Derrived classes should override this and return the identifier of its rmw implementation.
+  virtual
+  const char *
+  get_implementation_identifier() const;
+};
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_PAYLOAD_HPP_

rclcpp/include/rclcpp/detail/rmw_implementation_specific_publisher_payload.hpp

@@ -0,0 +1,52 @@
+// Copyright 2019 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__RMW_IMPLEMENTATION_SPECIFIC_PUBLISHER_PAYLOAD_HPP_
+#define RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_PUBLISHER_PAYLOAD_HPP_
+
+#include "rcl/publisher.h"
+
+#include "rclcpp/detail/rmw_implementation_specific_payload.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+class RCLCPP_PUBLIC RMWImplementationSpecificPublisherPayload
+  : public RMWImplementationSpecificPayload
+{
+public:
+  ~RMWImplementationSpecificPublisherPayload() override = default;
+
+  /// Opportunity for a derived class to inject information into the rcl options.
+  /**
+   * This is called after the rcl_publisher_options_t has been prepared by
+   * rclcpp, but before rcl_publisher_init() is called.
+   *
+   * The passed option is the rmw_publisher_options field of the
+   * rcl_publisher_options_t that will be passed to rcl_publisher_init().
+   *
+   * By default the options are unmodified.
+   */
+  virtual
+  void
+  modify_rmw_publisher_options(rmw_publisher_options_t & rmw_publisher_options) const;
+};
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_PUBLISHER_PAYLOAD_HPP_

rclcpp/include/rclcpp/detail/rmw_implementation_specific_subscription_payload.hpp

@@ -0,0 +1,53 @@
+// Copyright 2019 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__RMW_IMPLEMENTATION_SPECIFIC_SUBSCRIPTION_PAYLOAD_HPP_
+#define RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_SUBSCRIPTION_PAYLOAD_HPP_
+
+#include "rcl/subscription.h"
+
+#include "rclcpp/detail/rmw_implementation_specific_payload.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+/// Subscription payload that may be rmw implementation specific.
+class RCLCPP_PUBLIC RMWImplementationSpecificSubscriptionPayload
+  : public RMWImplementationSpecificPayload
+{
+public:
+  ~RMWImplementationSpecificSubscriptionPayload() override = default;
+
+  /// Opportunity for a derived class to inject information into the rcl options.
+  /**
+   * This is called after the rcl_subscription_options_t has been prepared by
+   * rclcpp, but before rcl_subscription_init() is called.
+   *
+   * The passed option is the rmw_subscription_options field of the
+   * rcl_subscription_options_t that will be passed to rcl_subscription_init().
+   *
+   * By default the options are unmodified.
+   */
+  virtual
+  void
+  modify_rmw_subscription_options(rmw_subscription_options_t & rmw_subscription_options) const;
+};
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_SUBSCRIPTION_PAYLOAD_HPP_

rclcpp/include/rclcpp/detail/utilities.hpp

@@ -0,0 +1,40 @@
+// Copyright 2019 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__UTILITIES_HPP_
+#define RCLCPP__DETAIL__UTILITIES_HPP_
+
+#include "rclcpp/detail/utilities.hpp"
+
+#include <string>
+#include <vector>
+
+#include "rcl/allocator.h"
+#include "rcl/arguments.h"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+std::vector<std::string>
+get_unparsed_ros_arguments(
+  int argc, char const * const argv[],
+  rcl_arguments_t * arguments,
+  rcl_allocator_t allocator);
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__UTILITIES_HPP_

rclcpp/include/rclcpp/duration.hpp

@@ -0,0 +1,119 @@
+// Copyright 2017 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__DURATION_HPP_
+#define RCLCPP__DURATION_HPP_
+
+#include <chrono>
+
+#include "builtin_interfaces/msg/duration.hpp"
+#include "rcl/time.h"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+class RCLCPP_PUBLIC Duration
+{
+public:
+  Duration(int32_t seconds, uint32_t nanoseconds);
+
+  // This constructor matches any numeric value - ints or floats
+  explicit Duration(rcl_duration_value_t nanoseconds);
+
+  explicit Duration(std::chrono::nanoseconds nanoseconds);
+
+  // This constructor matches any std::chrono value other than nanoseconds
+  // intentionally not using explicit to create a conversion constructor
+  template<class Rep, class Period>
+  // cppcheck-suppress noExplicitConstructor
+  Duration(const std::chrono::duration<Rep, Period> & duration)  // NOLINT(runtime/explicit)
+  : Duration(std::chrono::duration_cast<std::chrono::nanoseconds>(duration))
+  {}
+
+  // cppcheck-suppress noExplicitConstructor
+  Duration(const builtin_interfaces::msg::Duration & duration_msg);  // NOLINT(runtime/explicit)
+
+  explicit Duration(const rcl_duration_t & duration);
+
+  Duration(const Duration & rhs);
+
+  virtual ~Duration() = default;
+
+  operator builtin_interfaces::msg::Duration() const;
+
+  // cppcheck-suppress operatorEq // this is a false positive from cppcheck
+  Duration &
+  operator=(const Duration & rhs);
+
+  Duration &
+  operator=(const builtin_interfaces::msg::Duration & Duration_msg);
+
+  bool
+  operator==(const rclcpp::Duration & rhs) const;
+
+  bool
+  operator<(const rclcpp::Duration & rhs) const;
+
+  bool
+  operator<=(const rclcpp::Duration & rhs) const;
+
+  bool
+  operator>=(const rclcpp::Duration & rhs) const;
+
+  bool
+  operator>(const rclcpp::Duration & rhs) const;
+
+  Duration
+  operator+(const rclcpp::Duration & rhs) const;
+
+  Duration
+  operator-(const rclcpp::Duration & rhs) const;
+
+  static
+  Duration
+  max();
+
+  Duration
+  operator*(double scale) const;
+
+  rcl_duration_value_t
+  nanoseconds() const;
+
+  /// \return the duration in seconds as a floating point number.
+  /// \warning Depending on sizeof(double) there could be significant precision loss.
+  /// When an exact time is required use nanoseconds() instead.
+  double
+  seconds() const;
+
+  // Create a duration object from a floating point number representing seconds
+  static Duration
+  from_seconds(double seconds);
+
+  template<class DurationT>
+  DurationT
+  to_chrono() const
+  {
+    return std::chrono::duration_cast<DurationT>(std::chrono::nanoseconds(this->nanoseconds()));
+  }
+
+  rmw_time_t
+  to_rmw_time() const;
+
+private:
+  rcl_duration_t rcl_duration_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DURATION_HPP_

rclcpp/include/rclcpp/event.hpp

@@ -0,0 +1,55 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__EVENT_HPP_
+#define RCLCPP__EVENT_HPP_
+
+#include <atomic>
+#include <memory>
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+class Event
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Event)
+
+  RCLCPP_PUBLIC
+  Event();
+
+  RCLCPP_PUBLIC
+  bool
+  set();
+
+  RCLCPP_PUBLIC
+  bool
+  check();
+
+  RCLCPP_PUBLIC
+  bool
+  check_and_clear();
+
+private:
+  RCLCPP_DISABLE_COPY(Event)
+
+  std::atomic_bool state_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EVENT_HPP_

rclcpp/include/rclcpp/exceptions.hpp

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

rclcpp/include/rclcpp/exceptions/exceptions.hpp

@@ -0,0 +1,279 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__EXCEPTIONS__EXCEPTIONS_HPP_
+#define RCLCPP__EXCEPTIONS__EXCEPTIONS_HPP_
+
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#include "rcl/error_handling.h"
+#include "rcl/types.h"
+#include "rclcpp/visibility_control.hpp"
+
+#include "rcpputils/join.hpp"
+
+namespace rclcpp
+{
+namespace exceptions
+{
+
+/// Thrown when a method is trying to use a node, but it is invalid.
+class InvalidNodeError : public std::runtime_error
+{
+public:
+  InvalidNodeError()
+  : std::runtime_error("node is invalid") {}
+};
+
+/// Thrown when a any kind of name (node, namespace, topic, etc.) is invalid.
+class NameValidationError : public std::invalid_argument
+{
+public:
+  NameValidationError(
+    const char * name_type_,
+    const char * name_,
+    const char * error_msg_,
+    size_t invalid_index_)
+  : std::invalid_argument(format_error(name_type_, name_, error_msg_, invalid_index_)),
+    name_type(name_type_), name(name_), error_msg(error_msg_), invalid_index(invalid_index_)
+  {}
+
+  static std::string
+  format_error(
+    const char * name_type,
+    const char * name,
+    const char * error_msg,
+    size_t invalid_index);
+
+  const std::string name_type;
+  const std::string name;
+  const std::string error_msg;
+  const size_t invalid_index;
+};
+
+/// Thrown when a node name is invalid.
+class InvalidNodeNameError : public NameValidationError
+{
+public:
+  InvalidNodeNameError(const char * node_name, const char * error_msg, size_t invalid_index)
+  : NameValidationError("node name", node_name, error_msg, invalid_index)
+  {}
+};
+
+/// Thrown when a node namespace is invalid.
+class InvalidNamespaceError : public NameValidationError
+{
+public:
+  InvalidNamespaceError(const char * namespace_, const char * error_msg, size_t invalid_index)
+  : NameValidationError("namespace", namespace_, error_msg, invalid_index)
+  {}
+};
+
+/// Thrown when a topic name is invalid.
+class InvalidTopicNameError : public NameValidationError
+{
+public:
+  InvalidTopicNameError(const char * namespace_, const char * error_msg, size_t invalid_index)
+  : NameValidationError("topic name", namespace_, error_msg, invalid_index)
+  {}
+};
+
+/// Thrown when a service name is invalid.
+class InvalidServiceNameError : public NameValidationError
+{
+public:
+  InvalidServiceNameError(const char * namespace_, const char * error_msg, size_t invalid_index)
+  : NameValidationError("service name", namespace_, error_msg, invalid_index)
+  {}
+};
+
+/// Throw a C++ std::exception which was created based on an rcl error.
+/**
+ * Passing nullptr for reset_error is safe and will avoid calling any function
+ * to reset the error.
+ *
+ * \param ret the return code for the current error state
+ * \param prefix string to prefix to the error if applicable (not all errors have custom messages)
+ * \param error_state error state to create exception from, if nullptr rcl_get_error_state is used
+ * \param reset_error function to be called before throwing which whill clear the error state
+ * \throws std::invalid_argument if ret is RCL_RET_OK
+ * \throws std::runtime_error if the rcl_get_error_state returns 0
+ * \throws RCLErrorBase some child class exception based on ret
+ */
+/* *INDENT-OFF* */  // Uncrustify cannot yet understand [[noreturn]] properly
+RCLCPP_PUBLIC
+void
+throw_from_rcl_error [[noreturn]] (
+  rcl_ret_t ret,
+  const std::string & prefix = "",
+  const rcl_error_state_t * error_state = nullptr,
+  void (* reset_error)() = rcl_reset_error);
+/* *INDENT-ON* */
+
+class RCLErrorBase
+{
+public:
+  RCLCPP_PUBLIC
+  RCLErrorBase(rcl_ret_t ret, const rcl_error_state_t * error_state);
+  virtual ~RCLErrorBase() {}
+
+  rcl_ret_t ret;
+  std::string message;
+  std::string file;
+  size_t line;
+  std::string formatted_message;
+};
+
+/// Created when the return code does not match one of the other specialized exceptions.
+class RCLError : public RCLErrorBase, public std::runtime_error
+{
+public:
+  RCLCPP_PUBLIC
+  RCLError(rcl_ret_t ret, const rcl_error_state_t * error_state, const std::string & prefix);
+  RCLCPP_PUBLIC
+  RCLError(const RCLErrorBase & base_exc, const std::string & prefix);
+};
+
+/// Created when the ret is RCL_RET_BAD_ALLOC.
+class RCLBadAlloc : public RCLErrorBase, public std::bad_alloc
+{
+public:
+  RCLCPP_PUBLIC
+  RCLBadAlloc(rcl_ret_t ret, const rcl_error_state_t * error_state);
+  RCLCPP_PUBLIC
+  explicit RCLBadAlloc(const RCLErrorBase & base_exc);
+};
+
+/// Created when the ret is RCL_RET_INVALID_ARGUMENT.
+class RCLInvalidArgument : public RCLErrorBase, public std::invalid_argument
+{
+public:
+  RCLCPP_PUBLIC
+  RCLInvalidArgument(
+    rcl_ret_t ret,
+    const rcl_error_state_t * error_state,
+    const std::string & prefix);
+  RCLCPP_PUBLIC
+  RCLInvalidArgument(const RCLErrorBase & base_exc, const std::string & prefix);
+};
+
+/// Created when the ret is RCL_RET_INVALID_ROS_ARGS.
+class RCLInvalidROSArgsError : public RCLErrorBase, public std::runtime_error
+{
+public:
+  RCLCPP_PUBLIC
+  RCLInvalidROSArgsError(
+    rcl_ret_t ret, const rcl_error_state_t * error_state, const std::string & prefix);
+  RCLCPP_PUBLIC
+  RCLInvalidROSArgsError(const RCLErrorBase & base_exc, const std::string & prefix);
+};
+
+/// Thrown when unparsed ROS specific arguments are found.
+class UnknownROSArgsError : public std::runtime_error
+{
+public:
+  explicit UnknownROSArgsError(std::vector<std::string> && unknown_ros_args_in)
+  : std::runtime_error(
+      "found unknown ROS arguments: '" + rcpputils::join(unknown_ros_args_in, "', '") + "'"),
+    unknown_ros_args(unknown_ros_args_in)
+  {
+  }
+
+  const std::vector<std::string> unknown_ros_args;
+};
+
+/// Thrown when an invalid rclcpp::Event object or SharedPtr is encountered.
+class InvalidEventError : public std::runtime_error
+{
+public:
+  InvalidEventError()
+  : std::runtime_error("event is invalid") {}
+};
+
+/// Thrown when an unregistered rclcpp::Event is encountered where a registered one was expected.
+class EventNotRegisteredError : public std::runtime_error
+{
+public:
+  EventNotRegisteredError()
+  : std::runtime_error("event already registered") {}
+};
+
+/// Thrown if passed parameters are inconsistent or invalid
+class InvalidParametersException : public std::runtime_error
+{
+public:
+  // Inherit constructors from runtime_error.
+  using std::runtime_error::runtime_error;
+};
+
+/// Thrown if passed parameter value is invalid.
+class InvalidParameterValueException : public std::runtime_error
+{
+  // Inherit constructors from runtime_error.
+  using std::runtime_error::runtime_error;
+};
+
+/// Thrown if requested parameter type is invalid.
+/**
+ * Essentially the same as rclcpp::ParameterTypeException, but with parameter
+ * name in the error message.
+ */
+class InvalidParameterTypeException : public std::runtime_error
+{
+public:
+  /// Construct an instance.
+  /**
+   * \param[in] name the name of the parameter.
+   * \param[in] message custom exception message.
+   */
+  RCLCPP_PUBLIC
+  InvalidParameterTypeException(const std::string & name, const std::string message)
+  : std::runtime_error("parameter '" + name + "' has invalid type: " + message)
+  {}
+};
+
+/// Thrown if parameter is already declared.
+class ParameterAlreadyDeclaredException : public std::runtime_error
+{
+  // Inherit constructors from runtime_error.
+  using std::runtime_error::runtime_error;
+};
+
+/// Thrown if parameter is not declared, e.g. either set or get was called without first declaring.
+class ParameterNotDeclaredException : public std::runtime_error
+{
+  // Inherit constructors from runtime_error.
+  using std::runtime_error::runtime_error;
+};
+
+/// Thrown if parameter is immutable and therefore cannot be undeclared.
+class ParameterImmutableException : public std::runtime_error
+{
+  // Inherit constructors from runtime_error.
+  using std::runtime_error::runtime_error;
+};
+
+/// Thrown if parameter is modified while in a set callback.
+class ParameterModifiedInCallbackException : public std::runtime_error
+{
+  // Inherit constructors from runtime_error.
+  using std::runtime_error::runtime_error;
+};
+
+}  // namespace exceptions
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXCEPTIONS__EXCEPTIONS_HPP_

rclcpp/include/rclcpp/executor.hpp

@@ -17,25 +17,32 @@
 
 #include <algorithm>
 #include <cassert>
+#include <chrono>
 #include <cstdlib>
 #include <iostream>
 #include <list>
 #include <memory>
+#include <mutex>
+#include <string>
 #include <vector>
 
-#include "rcl_interfaces/msg/intra_process_message.hpp"
+#include "rcl/guard_condition.h"
+#include "rcl/wait.h"
 
-#include "rclcpp/any_executable.hpp"
-#include "rclcpp/macros.hpp"
+#include "rclcpp/contexts/default_context.hpp"
+#include "rclcpp/executor_options.hpp"
+#include "rclcpp/future_return_code.hpp"
 #include "rclcpp/memory_strategies.hpp"
 #include "rclcpp/memory_strategy.hpp"
-#include "rclcpp/node.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/utilities.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
-namespace executor
-{
+
+// Forward declaration is used in convenience method signature.
+class Node;
 
 /// Coordinate the order and timing of available communication tasks.
 /**
@@ -50,33 +57,23 @@ namespace executor
 class Executor
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Executor);
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Executor)
 
   /// Default constructor.
-  // \param[in] ms The memory strategy to be used with this executor.
-  explicit Executor(memory_strategy::MemoryStrategy::SharedPtr ms =
-    memory_strategies::create_default_strategy())
-  : interrupt_guard_condition_(rmw_create_guard_condition()),
-    memory_strategy_(ms)
-  {
-  }
+  /**
+   * \param[in] options Options used to configure the executor.
+   */
+  RCLCPP_PUBLIC
+  explicit Executor(const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions());
 
   /// Default destructor.
-  virtual ~Executor()
-  {
-    // Try to deallocate the interrupt guard condition.
-    if (interrupt_guard_condition_ != nullptr) {
-      rmw_ret_t status = rmw_destroy_guard_condition(interrupt_guard_condition_);
-      if (status != RMW_RET_OK) {
-        fprintf(stderr,
-          "[rclcpp::error] failed to destroy guard condition: %s\n", rmw_get_error_string_safe());
-      }
-    }
-  }
+  RCLCPP_PUBLIC
+  virtual ~Executor();
 
   /// Do work periodically as it becomes available to us. Blocking call, may block indefinitely.
   // It is up to the implementation of Executor to implement spin.
-  virtual void spin() = 0;
+  virtual void
+  spin() = 0;
 
   /// Add a node to the executor.
   /**
@@ -86,26 +83,14 @@ public:
    * the executor is blocked at the rmw layer while waiting for work and it is notified that a new
    * node was added, it will wake up.
    */
+  RCLCPP_PUBLIC
   virtual void
-  add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify = true)
-  {
-    // Check to ensure node not already added
-    for (auto & weak_node : weak_nodes_) {
-      auto node = weak_node.lock();
-      if (node == node_ptr) {
-        // TODO(jacquelinekay): Use a different error here?
-        throw std::runtime_error("Cannot add node to executor, node already added.");
-      }
-    }
-    weak_nodes_.push_back(node_ptr);
-    if (notify) {
-      // Interrupt waiting to handle new node
-      rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_);
-      if (status != RMW_RET_OK) {
-        throw std::runtime_error(rmw_get_error_string_safe());
-      }
-    }
-  }
+  add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true);
+
+  /// Convenience function which takes Node and forwards NodeBaseInterface.
+  RCLCPP_PUBLIC
+  virtual void
+  add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true);
 
   /// Remove a node from the executor.
   /**
@@ -114,29 +99,14 @@ public:
    * This is useful if the last node was removed from the executor while the executor was blocked
    * waiting for work in another thread, because otherwise the executor would never be notified.
    */
+  RCLCPP_PUBLIC
   virtual void
-  remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify = true)
-  {
-    bool node_removed = false;
-    weak_nodes_.erase(
-      std::remove_if(
-        weak_nodes_.begin(), weak_nodes_.end(),
-        [&](std::weak_ptr<rclcpp::node::Node> & i)
-        {
-          bool matched = (i.lock() == node_ptr);
-          node_removed |= matched;
-          return matched;
-        }));
-    if (notify) {
-      // If the node was matched and removed, interrupt waiting
-      if (node_removed) {
-        rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_);
-        if (status != RMW_RET_OK) {
-          throw std::runtime_error(rmw_get_error_string_safe());
-        }
-      }
-    }
-  }
+  remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true);
+
+  /// Convenience function which takes Node and forwards NodeBaseInterface.
+  RCLCPP_PUBLIC
+  virtual void
+  remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true);
 
   /// Add a node to executor, execute the next available unit of work, and remove the node.
   /**
@@ -145,29 +115,43 @@ public:
    * spin_node_once to block indefinitely (the default behavior). A timeout of 0 causes this
    * function to be non-blocking.
    */
-  template<typename T = std::milli>
-  void spin_node_once(rclcpp::node::Node::SharedPtr node,
-    std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t, T>(-1))
+  template<typename RepT = int64_t, typename T = std::milli>
+  void
+  spin_node_once(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
+    std::chrono::duration<RepT, T> timeout = std::chrono::duration<RepT, T>(-1))
   {
-    this->add_node(node, false);
-    // non-blocking = true
-    auto any_exec = get_next_executable(timeout);
-    if (any_exec) {
-      execute_any_executable(any_exec);
-    }
-    this->remove_node(node, false);
+    return spin_node_once_nanoseconds(
+      node,
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
+    );
+  }
+
+  /// Convenience function which takes Node and forwards NodeBaseInterface.
+  template<typename NodeT = rclcpp::Node, typename RepT = int64_t, typename T = std::milli>
+  void
+  spin_node_once(
+    std::shared_ptr<NodeT> node,
+    std::chrono::duration<RepT, T> timeout = std::chrono::duration<RepT, T>(-1))
+  {
+    return spin_node_once_nanoseconds(
+      node->get_node_base_interface(),
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
+    );
   }
 
   /// Add a node, complete all immediately available work, and remove the node.
   /**
    * \param[in] node Shared pointer to the node to add.
    */
-  void spin_node_some(rclcpp::node::Node::SharedPtr node)
-  {
-    this->add_node(node, false);
-    spin_some();
-    this->remove_node(node, false);
-  }
+  RCLCPP_PUBLIC
+  void
+  spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node);
+
+  /// Convenience function which takes Node and forwards NodeBaseInterface.
+  RCLCPP_PUBLIC
+  void
+  spin_node_some(std::shared_ptr<rclcpp::Node> node);
 
   /// Complete all available queued work without blocking.
   /**
@@ -175,441 +159,177 @@ public:
    * single-threaded model of execution.
    * Adding subscriptions, timers, services, etc. with blocking callbacks will cause this function
    * to block (which may have unintended consequences).
+   *
+   * \param[in] max_duration The maximum amount of time to spend executing work, or 0 for no limit.
+   * Note that spin_some() may take longer than this time as it only returns once max_duration has
+   * been exceeded.
    */
-  virtual void spin_some()
+  RCLCPP_PUBLIC
+  virtual void
+  spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0));
+
+  RCLCPP_PUBLIC
+  virtual void
+  spin_once(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+
+  /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted.
+  /**
+   * \param[in] future The future to wait on. If this function returns SUCCESS, the future can be
+   *   accessed without blocking (though it may still throw an exception).
+   * \param[in] timeout Optional timeout parameter, which gets passed to Executor::spin_node_once.
+   *   `-1` is block forever, `0` is non-blocking.
+   *   If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return
+   *   code.
+   * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`.
+   */
+  template<typename ResponseT, typename TimeRepT = int64_t, typename TimeT = std::milli>
+  FutureReturnCode
+  spin_until_future_complete(
+    const std::shared_future<ResponseT> & future,
+    std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
   {
-    while (AnyExecutable::SharedPtr any_exec =
-      get_next_executable(std::chrono::milliseconds::zero()))
-    {
-      execute_any_executable(any_exec);
+    // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete
+    // inside a callback executed by an executor.
+
+    // Check the future before entering the while loop.
+    // If the future is already complete, don't try to spin.
+    std::future_status status = future.wait_for(std::chrono::seconds(0));
+    if (status == std::future_status::ready) {
+      return FutureReturnCode::SUCCESS;
     }
+
+    auto end_time = std::chrono::steady_clock::now();
+    std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
+      timeout);
+    if (timeout_ns > std::chrono::nanoseconds::zero()) {
+      end_time += timeout_ns;
+    }
+    std::chrono::nanoseconds timeout_left = timeout_ns;
+
+    while (rclcpp::ok(this->context_)) {
+      // Do one item of work.
+      spin_once(timeout_left);
+      // Check if the future is set, return SUCCESS if it is.
+      status = future.wait_for(std::chrono::seconds(0));
+      if (status == std::future_status::ready) {
+        return FutureReturnCode::SUCCESS;
+      }
+      // If the original timeout is < 0, then this is blocking, never TIMEOUT.
+      if (timeout_ns < std::chrono::nanoseconds::zero()) {
+        continue;
+      }
+      // Otherwise check if we still have time to wait, return TIMEOUT if not.
+      auto now = std::chrono::steady_clock::now();
+      if (now >= end_time) {
+        return FutureReturnCode::TIMEOUT;
+      }
+      // Subtract the elapsed time from the original timeout.
+      timeout_left = std::chrono::duration_cast<std::chrono::nanoseconds>(end_time - now);
+    }
+
+    // The future did not complete before ok() returned false, return INTERRUPTED.
+    return FutureReturnCode::INTERRUPTED;
   }
 
+  /// Cancel any running spin* function, causing it to return.
+  /* This function can be called asynchonously from any thread. */
+  RCLCPP_PUBLIC
+  void
+  cancel();
+
   /// Support dynamic switching of the memory strategy.
   /**
    * Switching the memory strategy while the executor is spinning in another threading could have
    * unintended consequences.
    * \param[in] memory_strategy Shared pointer to the memory strategy to set.
    */
+  RCLCPP_PUBLIC
   void
-  set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy)
-  {
-    if (memory_strategy == nullptr) {
-      throw std::runtime_error("Received NULL memory strategy in executor.");
-    }
-    memory_strategy_ = memory_strategy;
-  }
+  set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy);
 
 protected:
+  RCLCPP_PUBLIC
+  void
+  spin_node_once_nanoseconds(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
+    std::chrono::nanoseconds timeout);
+
   /// Find the next available executable and do the work associated with it.
   /** \param[in] any_exec Union structure that can hold any executable type (timer, subscription,
    * service, client).
    */
+  RCLCPP_PUBLIC
   void
-  execute_any_executable(AnyExecutable::SharedPtr any_exec)
-  {
-    if (!any_exec) {
-      return;
-    }
-    if (any_exec->timer) {
-      execute_timer(any_exec->timer);
-    }
-    if (any_exec->subscription) {
-      execute_subscription(any_exec->subscription);
-    }
-    if (any_exec->subscription_intra_process) {
-      execute_intra_process_subscription(any_exec->subscription_intra_process);
-    }
-    if (any_exec->service) {
-      execute_service(any_exec->service);
-    }
-    if (any_exec->client) {
-      execute_client(any_exec->client);
-    }
-    // Reset the callback_group, regardless of type
-    any_exec->callback_group->can_be_taken_from().store(true);
-    // Wake the wait, because it may need to be recalculated or work that
-    // was previously blocked is now available.
-    rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_);
-    if (status != RMW_RET_OK) {
-      throw std::runtime_error(rmw_get_error_string_safe());
-    }
-  }
+  execute_any_executable(AnyExecutable & any_exec);
 
+  RCLCPP_PUBLIC
   static void
   execute_subscription(
-    rclcpp::subscription::SubscriptionBase::SharedPtr subscription)
-  {
-    std::shared_ptr<void> message = subscription->create_message();
-    bool taken = false;
-    rmw_message_info_t message_info;
-    auto ret =
-      rmw_take_with_info(subscription->get_subscription_handle(),
-        message.get(), &taken, &message_info);
-    if (ret == RMW_RET_OK) {
-      if (taken) {
-        message_info.from_intra_process = false;
-        subscription->handle_message(message, message_info);
-      }
-    } else {
-      fprintf(stderr,
-        "[rclcpp::error] take failed for subscription on topic '%s': %s\n",
-        subscription->get_topic_name().c_str(), rmw_get_error_string_safe());
-    }
-    subscription->return_message(message);
-  }
+    rclcpp::SubscriptionBase::SharedPtr subscription);
 
+  RCLCPP_PUBLIC
   static void
-  execute_intra_process_subscription(
-    rclcpp::subscription::SubscriptionBase::SharedPtr subscription)
-  {
-    rcl_interfaces::msg::IntraProcessMessage ipm;
-    bool taken = false;
-    rmw_message_info_t message_info;
-    rmw_ret_t status = rmw_take_with_info(
-      subscription->get_intra_process_subscription_handle(),
-      &ipm,
-      &taken,
-      &message_info);
-    if (status == RMW_RET_OK) {
-      if (taken) {
-        message_info.from_intra_process = true;
-        subscription->handle_intra_process_message(ipm, message_info);
-      }
-    } else {
-      fprintf(stderr,
-        "[rclcpp::error] take failed for intra process subscription on topic '%s': %s\n",
-        subscription->get_topic_name().c_str(), rmw_get_error_string_safe());
-    }
-  }
+  execute_timer(rclcpp::TimerBase::SharedPtr timer);
 
+  RCLCPP_PUBLIC
   static void
-  execute_timer(
-    rclcpp::timer::TimerBase::SharedPtr timer)
-  {
-    timer->execute_callback();
-  }
+  execute_service(rclcpp::ServiceBase::SharedPtr service);
 
+  RCLCPP_PUBLIC
   static void
-  execute_service(
-    rclcpp::service::ServiceBase::SharedPtr service)
-  {
-    std::shared_ptr<void> request_header = service->create_request_header();
-    std::shared_ptr<void> request = service->create_request();
-    bool taken = false;
-    rmw_ret_t status = rmw_take_request(
-      service->get_service_handle(),
-      request_header.get(),
-      request.get(),
-      &taken);
-    if (status == RMW_RET_OK) {
-      if (taken) {
-        service->handle_request(request_header, request);
-      }
-    } else {
-      fprintf(stderr,
-        "[rclcpp::error] take request failed for server of service '%s': %s\n",
-        service->get_service_name().c_str(), rmw_get_error_string_safe());
-    }
-  }
+  execute_client(rclcpp::ClientBase::SharedPtr client);
 
-  static void
-  execute_client(
-    rclcpp::client::ClientBase::SharedPtr client)
-  {
-    std::shared_ptr<void> request_header = client->create_request_header();
-    std::shared_ptr<void> response = client->create_response();
-    bool taken = false;
-    rmw_ret_t status = rmw_take_response(
-      client->get_client_handle(),
-      request_header.get(),
-      response.get(),
-      &taken);
-    if (status == RMW_RET_OK) {
-      if (taken) {
-        client->handle_response(request_header, response);
-      }
-    } else {
-      fprintf(stderr,
-        "[rclcpp::error] take response failed for client of service '%s': %s\n",
-        client->get_service_name().c_str(), rmw_get_error_string_safe());
-    }
-  }
-
-/*** Populate class storage from stored weak node pointers and wait. ***/
-
-  template<typename T = std::milli>
+  RCLCPP_PUBLIC
   void
-  wait_for_work(std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t, T>(-1))
-  {
-    memory_strategy_->clear_active_entities();
+  wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
-    // Collect the subscriptions and timers to be waited on
-    bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_);
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
+  get_node_by_group(rclcpp::CallbackGroup::SharedPtr group);
 
-    // Clean up any invalid nodes, if they were detected
-    if (has_invalid_weak_nodes) {
-      weak_nodes_.erase(
-        remove_if(weak_nodes_.begin(), weak_nodes_.end(),
-        [](std::weak_ptr<rclcpp::node::Node> i)
-          {
-            return i.expired();
-          }));
-    }
+  RCLCPP_PUBLIC
+  rclcpp::CallbackGroup::SharedPtr
+  get_group_by_timer(rclcpp::TimerBase::SharedPtr timer);
 
-    // Use the number of subscriptions to allocate memory in the handles
-    rmw_subscriptions_t subscriber_handles;
-    subscriber_handles.subscriber_count =
-      memory_strategy_->fill_subscriber_handles(subscriber_handles.subscribers);
+  RCLCPP_PUBLIC
+  bool
+  get_next_ready_executable(AnyExecutable & any_executable);
 
-    rmw_services_t service_handles;
-    service_handles.service_count =
-      memory_strategy_->fill_service_handles(service_handles.services);
+  RCLCPP_PUBLIC
+  bool
+  get_next_executable(
+    AnyExecutable & any_executable,
+    std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
-    rmw_clients_t client_handles;
-    client_handles.client_count =
-      memory_strategy_->fill_client_handles(client_handles.clients);
-
-    // The number of guard conditions is fixed at 2: 1 for the ctrl-c guard cond,
-    // and one for the executor's guard cond (interrupt_guard_condition_)
-    size_t number_of_guard_conds = 2;
-    rmw_guard_conditions_t guard_condition_handles;
-    guard_condition_handles.guard_condition_count = number_of_guard_conds;
-    guard_condition_handles.guard_conditions = static_cast<void **>(guard_cond_handles_.data());
-    if (guard_condition_handles.guard_conditions == NULL &&
-      number_of_guard_conds > 0)
-    {
-      // TODO(wjwwood): Use a different error here? maybe std::bad_alloc?
-      throw std::runtime_error("Could not malloc for guard condition pointers.");
-    }
-    // Put the global ctrl-c guard condition in
-    assert(guard_condition_handles.guard_condition_count > 1);
-    guard_condition_handles.guard_conditions[0] = \
-      rclcpp::utilities::get_global_sigint_guard_condition()->data;
-    // Put the executor's guard condition in
-    guard_condition_handles.guard_conditions[1] = \
-      interrupt_guard_condition_->data;
-
-    rmw_time_t * wait_timeout = NULL;
-    rmw_time_t rmw_timeout;
-
-    auto next_timer_duration = get_earliest_timer();
-    // If the next timer timeout must preempt the requested timeout
-    // or if the requested timeout blocks forever, and there exists a valid timer,
-    // replace the requested timeout with the next timeout.
-    bool has_valid_timer = next_timer_duration >= std::chrono::nanoseconds::zero();
-    if ((next_timer_duration < timeout ||
-      timeout < std::chrono::duration<int64_t, T>::zero()) && has_valid_timer)
-    {
-      rmw_timeout.sec =
-        std::chrono::duration_cast<std::chrono::seconds>(next_timer_duration).count();
-      rmw_timeout.nsec = next_timer_duration.count() % (1000 * 1000 * 1000);
-      wait_timeout = &rmw_timeout;
-    } else if (timeout >= std::chrono::duration<int64_t, T>::zero()) {
-      // Convert timeout representation to rmw_time
-      rmw_timeout.sec = std::chrono::duration_cast<std::chrono::seconds>(timeout).count();
-      rmw_timeout.nsec = std::chrono::duration_cast<std::chrono::nanoseconds>(timeout).count() %
-        (1000 * 1000 * 1000);
-      wait_timeout = &rmw_timeout;
-    }
-
-    // Now wait on the waitable subscriptions and timers
-    rmw_ret_t status = rmw_wait(
-      &subscriber_handles,
-      &guard_condition_handles,
-      &service_handles,
-      &client_handles,
-      wait_timeout);
-    if (status != RMW_RET_OK && status != RMW_RET_TIMEOUT) {
-      throw std::runtime_error(rmw_get_error_string_safe());
-    }
-    // If ctrl-c guard condition, return directly
-    if (guard_condition_handles.guard_conditions[0] != 0) {
-      // Make sure to free or clean memory
-      memory_strategy_->clear_handles();
-      return;
-    }
-
-    memory_strategy_->revalidate_handles();
-  }
-
-/******************************/
-  rclcpp::node::Node::SharedPtr
-  get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group)
-  {
-    if (!group) {
-      return rclcpp::node::Node::SharedPtr();
-    }
-    for (auto & weak_node : weak_nodes_) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->get_callback_groups()) {
-        auto callback_group = weak_group.lock();
-        if (callback_group == group) {
-          return node;
-        }
-      }
-    }
-    return rclcpp::node::Node::SharedPtr();
-  }
-
-  rclcpp::callback_group::CallbackGroup::SharedPtr
-  get_group_by_timer(
-    rclcpp::timer::TimerBase::SharedPtr timer)
-  {
-    for (auto & weak_node : weak_nodes_) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->get_callback_groups()) {
-        auto group = weak_group.lock();
-        if (!group) {
-          continue;
-        }
-        for (auto & weak_timer : group->get_timer_ptrs()) {
-          auto t = weak_timer.lock();
-          if (t == timer) {
-            return group;
-          }
-        }
-      }
-    }
-    return rclcpp::callback_group::CallbackGroup::SharedPtr();
-  }
-
-  void
-  get_next_timer(AnyExecutable::SharedPtr any_exec)
-  {
-    for (auto & weak_node : weak_nodes_) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->get_callback_groups()) {
-        auto group = weak_group.lock();
-        if (!group || !group->can_be_taken_from().load()) {
-          continue;
-        }
-        for (auto & timer_ref : group->get_timer_ptrs()) {
-          auto timer = timer_ref.lock();
-          if (timer && timer->check_and_trigger()) {
-            any_exec->timer = timer;
-            any_exec->callback_group = group;
-            node = get_node_by_group(group);
-            return;
-          }
-        }
-      }
-    }
-  }
-
-  std::chrono::nanoseconds
-  get_earliest_timer()
-  {
-    std::chrono::nanoseconds latest = std::chrono::nanoseconds::max();
-    bool timers_empty = true;
-    for (auto & weak_node : weak_nodes_) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->get_callback_groups()) {
-        auto group = weak_group.lock();
-        if (!group || !group->can_be_taken_from().load()) {
-          continue;
-        }
-        for (auto & timer_ref : group->get_timer_ptrs()) {
-          timers_empty = false;
-          // Check the expected trigger time
-          auto timer = timer_ref.lock();
-          if (timer && timer->time_until_trigger() < latest) {
-            latest = timer->time_until_trigger();
-          }
-        }
-      }
-    }
-    if (timers_empty) {
-      return std::chrono::nanoseconds(-1);
-    }
-    return latest;
-  }
-
-  AnyExecutable::SharedPtr
-  get_next_ready_executable()
-  {
-    auto any_exec = memory_strategy_->instantiate_next_executable();
-    // Check the timers to see if there are any that are ready, if so return
-    get_next_timer(any_exec);
-    if (any_exec->timer) {
-      return any_exec;
-    }
-    // Check the subscriptions to see if there are any that are ready
-    memory_strategy_->get_next_subscription(any_exec, weak_nodes_);
-    if (any_exec->subscription || any_exec->subscription_intra_process) {
-      return any_exec;
-    }
-    // Check the services to see if there are any that are ready
-    memory_strategy_->get_next_service(any_exec, weak_nodes_);
-    if (any_exec->service) {
-      return any_exec;
-    }
-    // Check the clients to see if there are any that are ready
-    memory_strategy_->get_next_client(any_exec, weak_nodes_);
-    if (any_exec->client) {
-      return any_exec;
-    }
-    // If there is no ready executable, return a null ptr
-    return nullptr;
-  }
-
-  template<typename T = std::milli>
-  AnyExecutable::SharedPtr
-  get_next_executable(std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t,
-    T>(-1))
-  {
-    // Check to see if there are any subscriptions or timers needing service
-    // TODO(wjwwood): improve run to run efficiency of this function
-    auto any_exec = get_next_ready_executable();
-    // If there are none
-    if (!any_exec) {
-      // Wait for subscriptions or timers to work on
-      wait_for_work(timeout);
-      // Try again
-      any_exec = get_next_ready_executable();
-    }
-    // At this point any_exec should be valid with either a valid subscription
-    // or a valid timer, or it should be a null shared_ptr
-    if (any_exec) {
-      // If it is valid, check to see if the group is mutually exclusive or
-      // not, then mark it accordingly
-      if (any_exec->callback_group && any_exec->callback_group->type() == \
-        callback_group::CallbackGroupType::MutuallyExclusive)
-      {
-        // It should not have been taken otherwise
-        assert(any_exec->callback_group->can_be_taken_from().load());
-        // Set to false to indicate something is being run from this group
-        any_exec->callback_group->can_be_taken_from().store(false);
-      }
-    }
-    return any_exec;
-  }
+  /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins.
+  std::atomic_bool spinning;
 
   /// Guard condition for signaling the rmw layer to wake up for special events.
-  rmw_guard_condition_t * interrupt_guard_condition_;
+  rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition();
+
+  /// Wait set for managing entities that the rmw layer waits on.
+  rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set();
+
+  // Mutex to protect the subsequent memory_strategy_.
+  std::mutex memory_strategy_mutex_;
 
   /// The memory strategy: an interface for handling user-defined memory allocation strategies.
   memory_strategy::MemoryStrategy::SharedPtr memory_strategy_;
 
-private:
-  RCLCPP_DISABLE_COPY(Executor);
+  /// The context associated with this executor.
+  std::shared_ptr<rclcpp::Context> context_;
 
-  std::vector<std::weak_ptr<rclcpp::node::Node>> weak_nodes_;
-  std::array<void *, 2> guard_cond_handles_;
+  RCLCPP_DISABLE_COPY(Executor)
+
+  std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
+  std::list<const rcl_guard_condition_t *> guard_conditions_;
 };
 
+namespace executor
+{
+
+using Executor [[deprecated("use rclcpp::Executor instead")]] = rclcpp::Executor;
+
 }  // namespace executor
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/executor_options.hpp

@@ -0,0 +1,57 @@
+// Copyright 2014-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.
+
+#ifndef RCLCPP__EXECUTOR_OPTIONS_HPP_
+#define RCLCPP__EXECUTOR_OPTIONS_HPP_
+
+#include "rclcpp/context.hpp"
+#include "rclcpp/contexts/default_context.hpp"
+#include "rclcpp/memory_strategies.hpp"
+#include "rclcpp/memory_strategy.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Options to be passed to the executor constructor.
+struct ExecutorOptions
+{
+  ExecutorOptions()
+  : memory_strategy(rclcpp::memory_strategies::create_default_strategy()),
+    context(rclcpp::contexts::get_global_default_context()),
+    max_conditions(0)
+  {}
+
+  rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy;
+  rclcpp::Context::SharedPtr context;
+  size_t max_conditions;
+};
+
+namespace executor
+{
+
+using ExecutorArgs [[deprecated("use rclcpp::ExecutorOptions instead")]] = ExecutorOptions;
+
+[[deprecated("use rclcpp::ExecutorOptions() instead")]]
+inline
+rclcpp::ExecutorOptions
+create_default_executor_arguments()
+{
+  return rclcpp::ExecutorOptions();
+}
+
+}  // namespace executor
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXECUTOR_OPTIONS_HPP_

rclcpp/include/rclcpp/executors.hpp

@@ -12,76 +12,116 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_EXECUTORS_HPP_
-#define RCLCPP_RCLCPP_EXECUTORS_HPP_
+#ifndef RCLCPP__EXECUTORS_HPP_
+#define RCLCPP__EXECUTORS_HPP_
 
 #include <future>
-#include <rclcpp/executors/multi_threaded_executor.hpp>
-#include <rclcpp/executors/single_threaded_executor.hpp>
-#include <rclcpp/node.hpp>
-#include <rclcpp/utilities.hpp>
+#include <memory>
+
+#include "rclcpp/executors/multi_threaded_executor.hpp"
+#include "rclcpp/executors/single_threaded_executor.hpp"
+#include "rclcpp/executors/static_single_threaded_executor.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/utilities.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
+
+/// Create a default single-threaded executor and execute any immediately available work.
+/** \param[in] node_ptr Shared pointer to the node to spin. */
+RCLCPP_PUBLIC
+void
+spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
+
+RCLCPP_PUBLIC
+void
+spin_some(rclcpp::Node::SharedPtr node_ptr);
+
+/// Create a default single-threaded executor and spin the specified node.
+/** \param[in] node_ptr Shared pointer to the node to spin. */
+RCLCPP_PUBLIC
+void
+spin(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
+
+RCLCPP_PUBLIC
+void
+spin(rclcpp::Node::SharedPtr node_ptr);
+
 namespace executors
 {
 
-using rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor;
-using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor;
+using rclcpp::executors::MultiThreadedExecutor;
+using rclcpp::executors::SingleThreadedExecutor;
 
-/// Return codes to be used with spin_until_future_complete.
-/**
- * SUCCESS: The future is complete and can be accessed with "get" without blocking.
- * INTERRUPTED: The future is not complete, spinning was interrupted by Ctrl-C or another error.
- * TIMEOUT: Spinning timed out.
- */
-enum FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT};
-
-/// Spin (blocking) until the future is complete, until the function times out (if applicable),
-/// or until rclcpp is interrupted.
+/// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted.
 /**
  * \param[in] executor The executor which will spin the node.
  * \param[in] node_ptr The node to spin.
- * \param[in] future The future to wait on. If SUCCESS, the future is safe to access after this function
- * \param[in] timeout Optional timeout parameter, which gets passed to Executor::spin_node_once.
-   -1 is block forever, 0 is non-blocking.
-   If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return code.
- * \return The return code, one of SUCCESS, INTERRUPTED, or TIMEOUT.
+ * \param[in] future The future to wait on. If `SUCCESS`, the future is safe to
+ *   access after this function
+ * \param[in] timeout Optional timeout parameter, which gets passed to
+ *   Executor::spin_node_once.
+ *   `-1` is block forever, `0` is non-blocking.
+ *   If the time spent inside the blocking loop exceeds this timeout, return a `TIMEOUT` return code.
+ * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`.
  */
-template<typename ResponseT, typename TimeT = std::milli>
-FutureReturnCode
+template<typename ResponseT, typename TimeRepT = int64_t, typename TimeT = std::milli>
+rclcpp::FutureReturnCode
 spin_node_until_future_complete(
-  rclcpp::executor::Executor & executor, rclcpp::node::Node::SharedPtr node_ptr,
-  std::shared_future<ResponseT> & future,
-  std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
+  rclcpp::Executor & executor,
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+  const std::shared_future<ResponseT> & future,
+  std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
-  // TODO(wjwwood): does not work recursively right, can't call spin_node_until_future_complete
+  // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete
   // inside a callback executed by an executor.
-
-  // Check the future before entering the while loop.
-  // If the future is already complete, don't try to spin.
-  std::future_status status = future.wait_for(std::chrono::seconds(0));
-
-  auto start_time = std::chrono::system_clock::now();
-
-  while (status != std::future_status::ready && rclcpp::utilities::ok()) {
-    executor.spin_node_once(node_ptr, timeout);
-    if (timeout.count() >= 0) {
-      if (start_time + timeout < std::chrono::system_clock::now()) {
-        return TIMEOUT;
-      }
-    }
-    status = future.wait_for(std::chrono::seconds(0));
-  }
-
-  // If the future completed, and we weren't interrupted by ctrl-C, return the response
-  if (status == std::future_status::ready) {
-    return FutureReturnCode::SUCCESS;
-  }
-  return FutureReturnCode::INTERRUPTED;
+  executor.add_node(node_ptr);
+  auto retcode = executor.spin_until_future_complete(future, timeout);
+  executor.remove_node(node_ptr);
+  return retcode;
 }
 
-} // namespace executors
-} // namespace rclcpp
+template<typename NodeT = rclcpp::Node, typename ResponseT, typename TimeRepT = int64_t,
+  typename TimeT = std::milli>
+rclcpp::FutureReturnCode
+spin_node_until_future_complete(
+  rclcpp::Executor & executor,
+  std::shared_ptr<NodeT> node_ptr,
+  const std::shared_future<ResponseT> & future,
+  std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
+{
+  return rclcpp::executors::spin_node_until_future_complete(
+    executor,
+    node_ptr->get_node_base_interface(),
+    future,
+    timeout);
+}
 
-#endif /* RCLCPP_RCLCPP_EXECUTORS_HPP_ */
+}  // namespace executors
+
+template<typename FutureT, typename TimeRepT = int64_t, typename TimeT = std::milli>
+rclcpp::FutureReturnCode
+spin_until_future_complete(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+  const std::shared_future<FutureT> & future,
+  std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
+{
+  rclcpp::executors::SingleThreadedExecutor executor;
+  return executors::spin_node_until_future_complete<FutureT>(executor, node_ptr, future, timeout);
+}
+
+template<typename NodeT = rclcpp::Node, typename FutureT, typename TimeRepT = int64_t,
+  typename TimeT = std::milli>
+rclcpp::FutureReturnCode
+spin_until_future_complete(
+  std::shared_ptr<NodeT> node_ptr,
+  const std::shared_future<FutureT> & future,
+  std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
+{
+  return rclcpp::spin_until_future_complete(node_ptr->get_node_base_interface(), future, timeout);
+}
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXECUTORS_HPP_

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -15,91 +15,75 @@
 #ifndef RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
 #define RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
 
-#include <rmw/rmw.h>
-
-#include <cassert>
-#include <cstdlib>
+#include <chrono>
 #include <memory>
 #include <mutex>
-#include <vector>
+#include <set>
+#include <thread>
+#include <unordered_map>
 
 #include "rclcpp/executor.hpp"
 #include "rclcpp/macros.hpp"
-#include "rclcpp/node.hpp"
-#include "rclcpp/utilities.hpp"
+#include "rclcpp/memory_strategies.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
 namespace executors
 {
-namespace multi_threaded_executor
-{
 
-class MultiThreadedExecutor : public executor::Executor
+class MultiThreadedExecutor : public rclcpp::Executor
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(MultiThreadedExecutor);
+  RCLCPP_SMART_PTR_DEFINITIONS(MultiThreadedExecutor)
 
-  MultiThreadedExecutor(memory_strategy::MemoryStrategy::SharedPtr ms =
-    memory_strategies::create_default_strategy())
-  : executor::Executor(ms)
-  {
-    number_of_threads_ = std::thread::hardware_concurrency();
-    if (number_of_threads_ == 0) {
-      number_of_threads_ = 1;
-    }
-  }
+  /// Constructor for MultiThreadedExecutor.
+  /**
+   * For the yield_before_execute option, when true std::this_thread::yield()
+   * will be called after acquiring work (as an AnyExecutable) and
+   * releasing the spinning lock, but before executing the work.
+   * This is useful for reproducing some bugs related to taking work more than
+   * once.
+   *
+   * \param options common options for all executors
+   * \param number_of_threads number of threads to have in the thread pool,
+   *   the default 0 will use the number of cpu cores found instead
+   * \param yield_before_execute if true std::this_thread::yield() is called
+   */
+  RCLCPP_PUBLIC
+  MultiThreadedExecutor(
+    const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions(),
+    size_t number_of_threads = 0,
+    bool yield_before_execute = false,
+    std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
-  virtual ~MultiThreadedExecutor() {}
+  RCLCPP_PUBLIC
+  virtual ~MultiThreadedExecutor();
 
+  RCLCPP_PUBLIC
   void
-  spin()
-  {
-    std::vector<std::thread> threads;
-    {
-      std::lock_guard<std::mutex> wait_lock(wait_mutex_);
-      size_t thread_id_ = 1;  // Use a _ suffix to avoid shadowing `rclcpp::thread_id`
-      for (size_t i = number_of_threads_; i > 0; --i) {
-        std::this_thread::sleep_for(std::chrono::milliseconds(100));
-        auto func = std::bind(&MultiThreadedExecutor::run, this, thread_id_++);
-        threads.emplace_back(func);
-      }
-    }
-    for (auto & thread : threads) {
-      thread.join();
-    }
-  }
+  spin() override;
 
+  RCLCPP_PUBLIC
   size_t
-  get_number_of_threads()
-  {
-    return number_of_threads_;
-  }
+  get_number_of_threads();
+
+protected:
+  RCLCPP_PUBLIC
+  void
+  run(size_t this_thread_number);
 
 private:
-  void run(size_t this_thread_id)
-  {
-    rclcpp::thread_id = this_thread_id;
-    while (rclcpp::utilities::ok()) {
-      executor::AnyExecutable::SharedPtr any_exec;
-      {
-        std::lock_guard<std::mutex> wait_lock(wait_mutex_);
-        if (!rclcpp::utilities::ok()) {
-          return;
-        }
-        any_exec = get_next_executable();
-      }
-      execute_any_executable(any_exec);
-    }
-  }
-
-  RCLCPP_DISABLE_COPY(MultiThreadedExecutor);
+  RCLCPP_DISABLE_COPY(MultiThreadedExecutor)
 
   std::mutex wait_mutex_;
   size_t number_of_threads_;
+  bool yield_before_execute_;
+  std::chrono::nanoseconds next_exec_timeout_;
+
+  std::set<TimerBase::SharedPtr> scheduled_timers_;
 };
 
-}  // namespace multi_threaded_executor
 }  // namespace executors
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/executors/single_threaded_executor.hpp

@@ -28,45 +28,46 @@
 #include "rclcpp/node.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/rate.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
 namespace executors
 {
-namespace single_threaded_executor
-{
 
-/// Single-threaded executor implementation
-// This is the default executor created by rclcpp::spin.
-class SingleThreadedExecutor : public executor::Executor
+/// Single-threaded executor implementation.
+/**
+ * This is the default executor created by rclcpp::spin.
+ */
+class SingleThreadedExecutor : public rclcpp::Executor
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(SingleThreadedExecutor);
+  RCLCPP_SMART_PTR_DEFINITIONS(SingleThreadedExecutor)
 
   /// Default constructor. See the default constructor for Executor.
-  SingleThreadedExecutor(memory_strategy::MemoryStrategy::SharedPtr ms =
-    memory_strategies::create_default_strategy())
-  : executor::Executor(ms) {}
+  RCLCPP_PUBLIC
+  explicit SingleThreadedExecutor(
+    const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions());
 
-  /// Default destrcutor.
-  virtual ~SingleThreadedExecutor() {}
+  /// Default destructor.
+  RCLCPP_PUBLIC
+  virtual ~SingleThreadedExecutor();
 
   /// Single-threaded implementation of spin.
-  // This function will block until work comes in, execute it, and keep blocking.
-  // It will only be interrupt by a CTRL-C (managed by the global signal handler).
-  void spin()
-  {
-    while (rclcpp::utilities::ok()) {
-      auto any_exec = get_next_executable();
-      execute_any_executable(any_exec);
-    }
-  }
+  /**
+   * This function will block until work comes in, execute it, and then repeat
+   * the process until canceled.
+   * It may be interrupt by a call to rclcpp::Executor::cancel() or by ctrl-c
+   * if the associated context is configured to shutdown on SIGINT.
+   */
+  RCLCPP_PUBLIC
+  void
+  spin() override;
 
 private:
-  RCLCPP_DISABLE_COPY(SingleThreadedExecutor);
+  RCLCPP_DISABLE_COPY(SingleThreadedExecutor)
 };
 
-}  // namespace single_threaded_executor
 }  // namespace executors
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp

@@ -0,0 +1,168 @@
+// 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.
+
+#ifndef RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_
+#define RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_
+
+#include <chrono>
+#include <list>
+#include <memory>
+
+#include "rcl/guard_condition.h"
+#include "rcl/wait.h"
+
+#include "rclcpp/experimental/executable_list.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/memory_strategy.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+namespace executors
+{
+
+class StaticExecutorEntitiesCollector final
+  : public rclcpp::Waitable,
+  public std::enable_shared_from_this<StaticExecutorEntitiesCollector>
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(StaticExecutorEntitiesCollector)
+
+  // Constructor
+  RCLCPP_PUBLIC
+  StaticExecutorEntitiesCollector() = default;
+
+  // Destructor
+  ~StaticExecutorEntitiesCollector();
+
+  RCLCPP_PUBLIC
+  void
+  init(
+    rcl_wait_set_t * p_wait_set,
+    rclcpp::memory_strategy::MemoryStrategy::SharedPtr & memory_strategy,
+    rcl_guard_condition_t * executor_guard_condition);
+
+  RCLCPP_PUBLIC
+  void
+  execute() override;
+
+  RCLCPP_PUBLIC
+  void
+  fill_memory_strategy();
+
+  RCLCPP_PUBLIC
+  void
+  fill_executable_list();
+
+  /// Function to reallocate space for entities in the wait set.
+  RCLCPP_PUBLIC
+  void
+  prepare_wait_set();
+
+  /// Function to add_handles_to_wait_set and wait for work and
+  // block until the wait set is ready or until the timeout has been exceeded.
+  RCLCPP_PUBLIC
+  void
+  refresh_wait_set(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+
+  RCLCPP_PUBLIC
+  bool
+  add_to_wait_set(rcl_wait_set_t * wait_set) override;
+
+  RCLCPP_PUBLIC
+  size_t
+  get_number_of_ready_guard_conditions() override;
+
+  RCLCPP_PUBLIC
+  void
+  add_node(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
+
+  RCLCPP_PUBLIC
+  bool
+  remove_node(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
+
+  /// Complete all available queued work without blocking.
+  /**
+   * This function checks if after the guard condition was triggered
+   * (or a spurious wakeup happened) we are really ready to execute
+   * i.e. re-collect entities
+   */
+  RCLCPP_PUBLIC
+  bool
+  is_ready(rcl_wait_set_t * wait_set) override;
+
+  RCLCPP_PUBLIC
+  size_t
+  get_number_of_timers() {return exec_list_.number_of_timers;}
+
+  RCLCPP_PUBLIC
+  size_t
+  get_number_of_subscriptions() {return exec_list_.number_of_subscriptions;}
+
+  RCLCPP_PUBLIC
+  size_t
+  get_number_of_services() {return exec_list_.number_of_services;}
+
+  RCLCPP_PUBLIC
+  size_t
+  get_number_of_clients() {return exec_list_.number_of_clients;}
+
+  RCLCPP_PUBLIC
+  size_t
+  get_number_of_waitables() {return exec_list_.number_of_waitables;}
+
+  RCLCPP_PUBLIC
+  rclcpp::SubscriptionBase::SharedPtr
+  get_subscription(size_t i) {return exec_list_.subscription[i];}
+
+  RCLCPP_PUBLIC
+  rclcpp::TimerBase::SharedPtr
+  get_timer(size_t i) {return exec_list_.timer[i];}
+
+  RCLCPP_PUBLIC
+  rclcpp::ServiceBase::SharedPtr
+  get_service(size_t i) {return exec_list_.service[i];}
+
+  RCLCPP_PUBLIC
+  rclcpp::ClientBase::SharedPtr
+  get_client(size_t i) {return exec_list_.client[i];}
+
+  RCLCPP_PUBLIC
+  rclcpp::Waitable::SharedPtr
+  get_waitable(size_t i) {return exec_list_.waitable[i];}
+
+private:
+  /// Nodes guard conditions which trigger this waitable
+  std::list<const rcl_guard_condition_t *> guard_conditions_;
+
+  /// Memory strategy: an interface for handling user-defined memory allocation strategies.
+  rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy_;
+
+  /// List of weak nodes registered in the static executor
+  std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
+
+  /// Wait set for managing entities that the rmw layer waits on.
+  rcl_wait_set_t * p_wait_set_ = nullptr;
+
+  /// Executable list: timers, subscribers, clients, services and waitables
+  rclcpp::experimental::ExecutableList exec_list_;
+};
+
+}  // namespace executors
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_

rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp

@@ -0,0 +1,200 @@
+// Copyright 2019 Nobleo Technology
+//
+// 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__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_
+#define RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_
+
+#include <cassert>
+#include <cstdlib>
+#include <memory>
+#include <vector>
+#include <string>
+
+#include "rmw/rmw.h"
+
+#include "rclcpp/executor.hpp"
+#include "rclcpp/executors/static_executor_entities_collector.hpp"
+#include "rclcpp/experimental/executable_list.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/memory_strategies.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/rate.hpp"
+#include "rclcpp/utilities.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace executors
+{
+
+/// Static executor implementation
+/**
+ * This executor is a static version of the original single threaded executor.
+ * It's static because it doesn't reconstruct the executable list for every iteration.
+ * All nodes, callbackgroups, timers, subscriptions etc. are created before
+ * spin() is called, and modified only when an entity is added/removed to/from a node.
+ *
+ * To run this executor instead of SingleThreadedExecutor replace:
+ * rclcpp::executors::SingleThreadedExecutor exec;
+ * by
+ * rclcpp::executors::StaticSingleThreadedExecutor exec;
+ * in your source code and spin node(s) in the following way:
+ * exec.add_node(node);
+ * exec.spin();
+ * exec.remove_node(node);
+ */
+class StaticSingleThreadedExecutor : public rclcpp::Executor
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(StaticSingleThreadedExecutor)
+
+  /// Default constructor. See the default constructor for Executor.
+  RCLCPP_PUBLIC
+  explicit StaticSingleThreadedExecutor(
+    const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions());
+
+  /// Default destrcutor.
+  RCLCPP_PUBLIC
+  virtual ~StaticSingleThreadedExecutor();
+
+  /// Static executor implementation of spin.
+  // This function will block until work comes in, execute it, and keep blocking.
+  // It will only be interrupt by a CTRL-C (managed by the global signal handler).
+  RCLCPP_PUBLIC
+  void
+  spin() override;
+
+  /// Add a node to the executor.
+  /**
+   * An executor can have zero or more nodes which provide work during `spin` functions.
+   * \param[in] node_ptr Shared pointer to the node to be added.
+   * \param[in] notify True to trigger the interrupt guard condition during this function. If
+   * the executor is blocked at the rmw layer while waiting for work and it is notified that a new
+   * node was added, it will wake up.
+   */
+  RCLCPP_PUBLIC
+  void
+  add_node(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+    bool notify = true) override;
+
+  /// Convenience function which takes Node and forwards NodeBaseInterface.
+  RCLCPP_PUBLIC
+  void
+  add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
+
+  /// Remove a node from the executor.
+  /**
+   * \param[in] node_ptr Shared pointer to the node to remove.
+   * \param[in] notify True to trigger the interrupt guard condition and wake up the executor.
+   * This is useful if the last node was removed from the executor while the executor was blocked
+   * waiting for work in another thread, because otherwise the executor would never be notified.
+   */
+  RCLCPP_PUBLIC
+  void
+  remove_node(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+    bool notify = true) override;
+
+  /// Convenience function which takes Node and forwards NodeBaseInterface.
+  RCLCPP_PUBLIC
+  void
+  remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
+
+  /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted.
+  /**
+   * \param[in] future The future to wait on. If this function returns SUCCESS, the future can be
+   *   accessed without blocking (though it may still throw an exception).
+   * \param[in] timeout Optional timeout parameter, which gets passed to
+   *    Executor::execute_ready_executables.
+   *   `-1` is block forever, `0` is non-blocking.
+   *   If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return
+   *   code.
+   * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`.
+   *
+   *  Example usage:
+   *  rclcpp::executors::StaticSingleThreadedExecutor exec;
+   *  // ... other part of code like creating node
+   *  // define future
+   *  exec.add_node(node);
+   *  exec.spin_until_future_complete(future);
+   */
+  template<typename ResponseT, typename TimeRepT = int64_t, typename TimeT = std::milli>
+  rclcpp::FutureReturnCode
+  spin_until_future_complete(
+    std::shared_future<ResponseT> & future,
+    std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
+  {
+    std::future_status status = future.wait_for(std::chrono::seconds(0));
+    if (status == std::future_status::ready) {
+      return rclcpp::FutureReturnCode::SUCCESS;
+    }
+
+    auto end_time = std::chrono::steady_clock::now();
+    std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
+      timeout);
+    if (timeout_ns > std::chrono::nanoseconds::zero()) {
+      end_time += timeout_ns;
+    }
+    std::chrono::nanoseconds timeout_left = timeout_ns;
+
+    entities_collector_ = std::make_shared<StaticExecutorEntitiesCollector>();
+    entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_);
+
+    while (rclcpp::ok(this->context_)) {
+      // Do one set of work.
+      entities_collector_->refresh_wait_set(timeout_left);
+      execute_ready_executables();
+      // Check if the future is set, return SUCCESS if it is.
+      status = future.wait_for(std::chrono::seconds(0));
+      if (status == std::future_status::ready) {
+        return rclcpp::FutureReturnCode::SUCCESS;
+      }
+      // If the original timeout is < 0, then this is blocking, never TIMEOUT.
+      if (timeout_ns < std::chrono::nanoseconds::zero()) {
+        continue;
+      }
+      // Otherwise check if we still have time to wait, return TIMEOUT if not.
+      auto now = std::chrono::steady_clock::now();
+      if (now >= end_time) {
+        return rclcpp::FutureReturnCode::TIMEOUT;
+      }
+      // Subtract the elapsed time from the original timeout.
+      timeout_left = std::chrono::duration_cast<std::chrono::nanoseconds>(end_time - now);
+    }
+
+    // The future did not complete before ok() returned false, return INTERRUPTED.
+    return rclcpp::FutureReturnCode::INTERRUPTED;
+  }
+
+protected:
+  /// Check which executables in ExecutableList struct are ready from wait_set and execute them.
+  /**
+   * \param[in] exec_list Structure that can hold subscriptionbases, timerbases, etc
+   * \param[in] timeout Optional timeout parameter.
+   */
+  RCLCPP_PUBLIC
+  void
+  execute_ready_executables();
+
+private:
+  RCLCPP_DISABLE_COPY(StaticSingleThreadedExecutor)
+
+  StaticExecutorEntitiesCollector::SharedPtr entities_collector_;
+};
+
+}  // namespace executors
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_

rclcpp/include/rclcpp/expand_topic_or_service_name.hpp

@@ -0,0 +1,63 @@
+// Copyright 2017 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__EXPAND_TOPIC_OR_SERVICE_NAME_HPP_
+#define RCLCPP__EXPAND_TOPIC_OR_SERVICE_NAME_HPP_
+
+#include <string>
+
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Expand a topic or service name and throw if it is not valid.
+/**
+ * This function can be used to "just" validate a topic or service name too,
+ * since expanding the topic name is required to fully validate a name.
+ *
+ * If the name is invalid, then InvalidTopicNameError is thrown or
+ * InvalidServiceNameError if is_service is true.
+ *
+ * This function can take any form of a topic or service name, i.e. it does not
+ * have to be a fully qualified name.
+ * The node name and namespace are used to expand it if necessary while
+ * validating it.
+ *
+ * Expansion is done with rcl_expand_topic_name.
+ * The validation is doen with rcl_validate_topic_name and
+ * rmw_validate_full_topic_name, so details about failures can be found in the
+ * documentation for those functions.
+ *
+ * \param name the topic or service name to be validated
+ * \param node_name the name of the node associated with the name
+ * \param namespace_ the namespace of the node associated with the name
+ * \param is_service if true InvalidServiceNameError is thrown instead
+ * \returns expanded (and validated) topic name
+ * \throws InvalidTopicNameError if name is invalid and is_service is false
+ * \throws InvalidServiceNameError if name is invalid and is_service is true
+ * \throws std::bad_alloc if memory cannot be allocated
+ * \throws RCLError if an unexpect error occurs
+ */
+RCLCPP_PUBLIC
+std::string
+expand_topic_or_service_name(
+  const std::string & name,
+  const std::string & node_name,
+  const std::string & namespace_,
+  bool is_service = false);
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPAND_TOPIC_OR_SERVICE_NAME_HPP_

rclcpp/include/rclcpp/experimental/README.md

@@ -0,0 +1,4 @@
+Notice that headers in this folder should only provide symbols in the rclcpp::experimental namespace.
+
+Also notice that these headers are not considered part of the public API as they have not yet been stabilized.
+And therefore they are subject to change without notice.

rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp

@@ -0,0 +1,42 @@
+// Copyright 2019 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__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_
+#define RCLCPP__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_
+
+namespace rclcpp
+{
+namespace experimental
+{
+namespace buffers
+{
+
+template<typename BufferT>
+class BufferImplementationBase
+{
+public:
+  virtual ~BufferImplementationBase() {}
+
+  virtual BufferT dequeue() = 0;
+  virtual void enqueue(BufferT request) = 0;
+
+  virtual void clear() = 0;
+  virtual bool has_data() const = 0;
+};
+
+}  // namespace buffers
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_

rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp

@@ -0,0 +1,241 @@
+// Copyright 2019 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__EXPERIMENTAL__BUFFERS__INTRA_PROCESS_BUFFER_HPP_
+#define RCLCPP__EXPERIMENTAL__BUFFERS__INTRA_PROCESS_BUFFER_HPP_
+
+#include <memory>
+#include <type_traits>
+#include <utility>
+
+#include "rclcpp/allocator/allocator_common.hpp"
+#include "rclcpp/allocator/allocator_deleter.hpp"
+#include "rclcpp/experimental/buffers/buffer_implementation_base.hpp"
+#include "rclcpp/macros.hpp"
+
+namespace rclcpp
+{
+namespace experimental
+{
+namespace buffers
+{
+
+class IntraProcessBufferBase
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(IntraProcessBufferBase)
+
+  virtual ~IntraProcessBufferBase() {}
+
+  virtual void clear() = 0;
+
+  virtual bool has_data() const = 0;
+  virtual bool use_take_shared_method() const = 0;
+};
+
+template<
+  typename MessageT,
+  typename Alloc = std::allocator<void>,
+  typename MessageDeleter = std::default_delete<MessageT>>
+class IntraProcessBuffer : public IntraProcessBufferBase
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(IntraProcessBuffer)
+
+  virtual ~IntraProcessBuffer() {}
+
+  using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
+  using MessageSharedPtr = std::shared_ptr<const MessageT>;
+
+  virtual void add_shared(MessageSharedPtr msg) = 0;
+  virtual void add_unique(MessageUniquePtr msg) = 0;
+
+  virtual MessageSharedPtr consume_shared() = 0;
+  virtual MessageUniquePtr consume_unique() = 0;
+};
+
+template<
+  typename MessageT,
+  typename Alloc = std::allocator<void>,
+  typename MessageDeleter = std::default_delete<MessageT>,
+  typename BufferT = std::unique_ptr<MessageT>>
+class TypedIntraProcessBuffer : public IntraProcessBuffer<MessageT, Alloc, MessageDeleter>
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(TypedIntraProcessBuffer)
+
+  using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
+  using MessageAlloc = typename MessageAllocTraits::allocator_type;
+  using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
+  using MessageSharedPtr = std::shared_ptr<const MessageT>;
+
+  explicit
+  TypedIntraProcessBuffer(
+    std::unique_ptr<BufferImplementationBase<BufferT>> buffer_impl,
+    std::shared_ptr<Alloc> allocator = nullptr)
+  {
+    bool valid_type = (std::is_same<BufferT, MessageSharedPtr>::value ||
+      std::is_same<BufferT, MessageUniquePtr>::value);
+    if (!valid_type) {
+      throw std::runtime_error("Creating TypedIntraProcessBuffer with not valid BufferT");
+    }
+
+    buffer_ = std::move(buffer_impl);
+
+    if (!allocator) {
+      message_allocator_ = std::make_shared<MessageAlloc>();
+    } else {
+      message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
+    }
+  }
+
+  virtual ~TypedIntraProcessBuffer() {}
+
+  void add_shared(MessageSharedPtr msg) override
+  {
+    add_shared_impl<BufferT>(std::move(msg));
+  }
+
+  void add_unique(MessageUniquePtr msg) override
+  {
+    buffer_->enqueue(std::move(msg));
+  }
+
+  MessageSharedPtr consume_shared() override
+  {
+    return consume_shared_impl<BufferT>();
+  }
+
+  MessageUniquePtr consume_unique() override
+  {
+    return consume_unique_impl<BufferT>();
+  }
+
+  bool has_data() const override
+  {
+    return buffer_->has_data();
+  }
+
+  void clear() override
+  {
+    buffer_->clear();
+  }
+
+  bool use_take_shared_method() const override
+  {
+    return std::is_same<BufferT, MessageSharedPtr>::value;
+  }
+
+private:
+  std::unique_ptr<BufferImplementationBase<BufferT>> buffer_;
+
+  std::shared_ptr<MessageAlloc> message_allocator_;
+
+  // MessageSharedPtr to MessageSharedPtr
+  template<typename DestinationT>
+  typename std::enable_if<
+    std::is_same<DestinationT, MessageSharedPtr>::value
+  >::type
+  add_shared_impl(MessageSharedPtr shared_msg)
+  {
+    buffer_->enqueue(std::move(shared_msg));
+  }
+
+  // MessageSharedPtr to MessageUniquePtr
+  template<typename DestinationT>
+  typename std::enable_if<
+    std::is_same<DestinationT, MessageUniquePtr>::value
+  >::type
+  add_shared_impl(MessageSharedPtr shared_msg)
+  {
+    // This should not happen: here a copy is unconditionally made, while the intra-process manager
+    // can decide whether a copy is needed depending on the number and the type of buffers
+
+    MessageUniquePtr unique_msg;
+    MessageDeleter * deleter = std::get_deleter<MessageDeleter, const MessageT>(shared_msg);
+    auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
+    MessageAllocTraits::construct(*message_allocator_.get(), ptr, *shared_msg);
+    if (deleter) {
+      unique_msg = MessageUniquePtr(ptr, *deleter);
+    } else {
+      unique_msg = MessageUniquePtr(ptr);
+    }
+
+    buffer_->enqueue(std::move(unique_msg));
+  }
+
+  // MessageSharedPtr to MessageSharedPtr
+  template<typename OriginT>
+  typename std::enable_if<
+    std::is_same<OriginT, MessageSharedPtr>::value,
+    MessageSharedPtr
+  >::type
+  consume_shared_impl()
+  {
+    return buffer_->dequeue();
+  }
+
+  // MessageUniquePtr to MessageSharedPtr
+  template<typename OriginT>
+  typename std::enable_if<
+    (std::is_same<OriginT, MessageUniquePtr>::value),
+    MessageSharedPtr
+  >::type
+  consume_shared_impl()
+  {
+    // automatic cast from unique ptr to shared ptr
+    return buffer_->dequeue();
+  }
+
+  // MessageSharedPtr to MessageUniquePtr
+  template<typename OriginT>
+  typename std::enable_if<
+    (std::is_same<OriginT, MessageSharedPtr>::value),
+    MessageUniquePtr
+  >::type
+  consume_unique_impl()
+  {
+    MessageSharedPtr buffer_msg = buffer_->dequeue();
+
+    MessageUniquePtr unique_msg;
+    MessageDeleter * deleter = std::get_deleter<MessageDeleter, const MessageT>(buffer_msg);
+    auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
+    MessageAllocTraits::construct(*message_allocator_.get(), ptr, *buffer_msg);
+    if (deleter) {
+      unique_msg = MessageUniquePtr(ptr, *deleter);
+    } else {
+      unique_msg = MessageUniquePtr(ptr);
+    }
+
+    return unique_msg;
+  }
+
+  // MessageUniquePtr to MessageUniquePtr
+  template<typename OriginT>
+  typename std::enable_if<
+    (std::is_same<OriginT, MessageUniquePtr>::value),
+    MessageUniquePtr
+  >::type
+  consume_unique_impl()
+  {
+    return buffer_->dequeue();
+  }
+};
+
+}  // namespace buffers
+}  // namespace experimental
+}  // namespace rclcpp
+
+
+#endif  // RCLCPP__EXPERIMENTAL__BUFFERS__INTRA_PROCESS_BUFFER_HPP_

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

@@ -0,0 +1,122 @@
+// Copyright 2019 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__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_
+#define RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_
+
+#include <algorithm>
+#include <cstddef>
+#include <cstdint>
+#include <memory>
+#include <mutex>
+#include <stdexcept>
+#include <utility>
+#include <vector>
+
+#include "rclcpp/experimental/buffers/buffer_implementation_base.hpp"
+#include "rclcpp/logger.hpp"
+#include "rclcpp/logging.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace experimental
+{
+namespace buffers
+{
+
+template<typename BufferT>
+class RingBufferImplementation : public BufferImplementationBase<BufferT>
+{
+public:
+  explicit RingBufferImplementation(size_t capacity)
+  : capacity_(capacity),
+    ring_buffer_(capacity),
+    write_index_(capacity_ - 1),
+    read_index_(0),
+    size_(0)
+  {
+    if (capacity == 0) {
+      throw std::invalid_argument("capacity must be a positive, non-zero value");
+    }
+  }
+
+  virtual ~RingBufferImplementation() {}
+
+  void enqueue(BufferT request)
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+
+    write_index_ = next(write_index_);
+    ring_buffer_[write_index_] = std::move(request);
+
+    if (is_full()) {
+      read_index_ = next(read_index_);
+    } else {
+      size_++;
+    }
+  }
+
+  BufferT dequeue()
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+
+    if (!has_data()) {
+      RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Calling dequeue on empty intra-process buffer");
+      throw std::runtime_error("Calling dequeue on empty intra-process buffer");
+    }
+
+    auto request = std::move(ring_buffer_[read_index_]);
+    read_index_ = next(read_index_);
+
+    size_--;
+
+    return request;
+  }
+
+  inline size_t next(size_t val)
+  {
+    return (val + 1) % capacity_;
+  }
+
+  inline bool has_data() const
+  {
+    return size_ != 0;
+  }
+
+  inline bool is_full()
+  {
+    return size_ == capacity_;
+  }
+
+  void clear() {}
+
+private:
+  size_t capacity_;
+
+  std::vector<BufferT> ring_buffer_;
+
+  size_t write_index_;
+  size_t read_index_;
+  size_t size_;
+
+  std::mutex mutex_;
+};
+
+}  // namespace buffers
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_

rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp

@@ -0,0 +1,99 @@
+// Copyright 2019 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__EXPERIMENTAL__CREATE_INTRA_PROCESS_BUFFER_HPP_
+#define RCLCPP__EXPERIMENTAL__CREATE_INTRA_PROCESS_BUFFER_HPP_
+
+#include <memory>
+#include <type_traits>
+#include <utility>
+
+#include "rcl/subscription.h"
+
+#include "rclcpp/experimental/buffers/intra_process_buffer.hpp"
+#include "rclcpp/experimental/buffers/ring_buffer_implementation.hpp"
+#include "rclcpp/intra_process_buffer_type.hpp"
+
+namespace rclcpp
+{
+namespace experimental
+{
+
+template<
+  typename MessageT,
+  typename Alloc = std::allocator<void>,
+  typename Deleter = std::default_delete<MessageT>>
+typename rclcpp::experimental::buffers::IntraProcessBuffer<MessageT, Alloc, Deleter>::UniquePtr
+create_intra_process_buffer(
+  IntraProcessBufferType buffer_type,
+  rmw_qos_profile_t qos,
+  std::shared_ptr<Alloc> allocator)
+{
+  using MessageSharedPtr = std::shared_ptr<const MessageT>;
+  using MessageUniquePtr = std::unique_ptr<MessageT, Deleter>;
+
+  size_t buffer_size = qos.depth;
+
+  using rclcpp::experimental::buffers::IntraProcessBuffer;
+  typename IntraProcessBuffer<MessageT, Alloc, Deleter>::UniquePtr buffer;
+
+  switch (buffer_type) {
+    case IntraProcessBufferType::SharedPtr:
+      {
+        using BufferT = MessageSharedPtr;
+
+        auto buffer_implementation =
+          std::make_unique<rclcpp::experimental::buffers::RingBufferImplementation<BufferT>>(
+          buffer_size);
+
+        // Construct the intra_process_buffer
+        buffer =
+          std::make_unique<rclcpp::experimental::buffers::TypedIntraProcessBuffer<MessageT, Alloc,
+            Deleter, BufferT>>(
+          std::move(buffer_implementation),
+          allocator);
+
+        break;
+      }
+    case IntraProcessBufferType::UniquePtr:
+      {
+        using BufferT = MessageUniquePtr;
+
+        auto buffer_implementation =
+          std::make_unique<rclcpp::experimental::buffers::RingBufferImplementation<BufferT>>(
+          buffer_size);
+
+        // Construct the intra_process_buffer
+        buffer =
+          std::make_unique<rclcpp::experimental::buffers::TypedIntraProcessBuffer<MessageT, Alloc,
+            Deleter, BufferT>>(
+          std::move(buffer_implementation),
+          allocator);
+
+        break;
+      }
+    default:
+      {
+        throw std::runtime_error("Unrecognized IntraProcessBufferType value");
+        break;
+      }
+  }
+
+  return buffer;
+}
+
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__CREATE_INTRA_PROCESS_BUFFER_HPP_

rclcpp/include/rclcpp/experimental/executable_list.hpp

@@ -0,0 +1,92 @@
+// Copyright 2019 Nobleo Technology
+//
+// 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__EXPERIMENTAL__EXECUTABLE_LIST_HPP_
+#define RCLCPP__EXPERIMENTAL__EXECUTABLE_LIST_HPP_
+
+#include <memory>
+#include <vector>
+
+#include "rclcpp/client.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/subscription_base.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+namespace experimental
+{
+
+/// This class contains subscriptionbase, timerbase, etc. which can be used to run callbacks.
+class ExecutableList final
+{
+public:
+  RCLCPP_PUBLIC
+  ExecutableList();
+
+  RCLCPP_PUBLIC
+  ~ExecutableList();
+
+  RCLCPP_PUBLIC
+  void
+  clear();
+
+  RCLCPP_PUBLIC
+  void
+  add_subscription(rclcpp::SubscriptionBase::SharedPtr subscription);
+
+  RCLCPP_PUBLIC
+  void
+  add_timer(rclcpp::TimerBase::SharedPtr timer);
+
+  RCLCPP_PUBLIC
+  void
+  add_service(rclcpp::ServiceBase::SharedPtr service);
+
+  RCLCPP_PUBLIC
+  void
+  add_client(rclcpp::ClientBase::SharedPtr client);
+
+  RCLCPP_PUBLIC
+  void
+  add_waitable(rclcpp::Waitable::SharedPtr waitable);
+
+  // Vector containing the SubscriptionBase of all the subscriptions added to the executor.
+  std::vector<rclcpp::SubscriptionBase::SharedPtr> subscription;
+  // Contains the count of added subscriptions
+  size_t number_of_subscriptions;
+  // Vector containing the TimerBase of all the timers added to the executor.
+  std::vector<rclcpp::TimerBase::SharedPtr> timer;
+  // Contains the count of added timers
+  size_t number_of_timers;
+  // Vector containing the ServiceBase of all the services added to the executor.
+  std::vector<rclcpp::ServiceBase::SharedPtr> service;
+  // Contains the count of added services
+  size_t number_of_services;
+  // Vector containing the ClientBase of all the clients added to the executor.
+  std::vector<rclcpp::ClientBase::SharedPtr> client;
+  // Contains the count of added clients
+  size_t number_of_clients;
+  // Vector containing all the waitables added to the executor.
+  std::vector<rclcpp::Waitable::SharedPtr> waitable;
+  // Contains the count of added waitables
+  size_t number_of_waitables;
+};
+
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__EXECUTABLE_LIST_HPP_

rclcpp/include/rclcpp/experimental/intra_process_manager.hpp

@@ -0,0 +1,424 @@
+// Copyright 2019 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__EXPERIMENTAL__INTRA_PROCESS_MANAGER_HPP_
+#define RCLCPP__EXPERIMENTAL__INTRA_PROCESS_MANAGER_HPP_
+
+#include <rmw/types.h>
+
+#include <shared_mutex>
+
+#include <algorithm>
+#include <atomic>
+#include <cstdint>
+#include <exception>
+#include <map>
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include "rclcpp/allocator/allocator_deleter.hpp"
+#include "rclcpp/experimental/subscription_intra_process.hpp"
+#include "rclcpp/experimental/subscription_intra_process_base.hpp"
+#include "rclcpp/logger.hpp"
+#include "rclcpp/logging.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/publisher_base.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+namespace experimental
+{
+
+/// This class performs intra process communication between nodes.
+/**
+ * This class is used in the creation of publishers and subscriptions.
+ * A singleton instance of this class is owned by a rclcpp::Context and a
+ * rclcpp::Node can use an associated Context to get an instance of this class.
+ * Nodes which do not have a common Context will not exchange intra process
+ * messages because they do not share access to the same instance of this class.
+ *
+ * When a Node creates a subscription, it can also create a helper class,
+ * called SubscriptionIntraProcess, meant to receive intra process messages.
+ * It can be registered with this class.
+ * It is also allocated an id which is unique among all publishers
+ * and subscriptions in this process and that is associated to the subscription.
+ *
+ * When a Node creates a publisher, as with subscriptions, a helper class can
+ * be registered with this class.
+ * This is required in order to publish intra-process messages.
+ * It is also allocated an id which is unique among all publishers
+ * and subscriptions in this process and that is associated to the publisher.
+ *
+ * When a publisher or a subscription are registered, this class checks to see
+ * which other subscriptions or publishers it will communicate with,
+ * i.e. they have the same topic and compatible QoS.
+ *
+ * When the user publishes a message, if intra-process communication is enabled
+ * on the publisher, the message is given to this class.
+ * Using the publisher id, a list of recipients for the message is selected.
+ * For each subscription in the list, this class stores the message, whether
+ * sharing ownership or making a copy, in a buffer associated with the
+ * subscription helper class.
+ *
+ * The subscription helper class contains a buffer where published
+ * intra-process messages are stored until they are taken from the subscription.
+ * Depending on the data type stored in the buffer, the subscription helper
+ * class can request either shared or exclusive ownership on the message.
+ *
+ * Thus, when an intra-process message is published, this class knows how many
+ * intra-process subscriptions needs it and how many require ownership.
+ * This information allows this class to operate efficiently by performing the
+ * fewest number of copies of the message required.
+ *
+ * This class is neither CopyConstructable nor CopyAssignable.
+ */
+class IntraProcessManager
+{
+private:
+  RCLCPP_DISABLE_COPY(IntraProcessManager)
+
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManager)
+
+  RCLCPP_PUBLIC
+  IntraProcessManager();
+
+  RCLCPP_PUBLIC
+  virtual ~IntraProcessManager();
+
+  /// Register a subscription with the manager, returns subscriptions unique id.
+  /**
+   * This method stores the subscription intra process object, together with
+   * the information of its wrapped subscription (i.e. topic name and QoS).
+   *
+   * In addition this generates a unique intra process id for the subscription.
+   *
+   * \param subscription the SubscriptionIntraProcess to register.
+   * \return an unsigned 64-bit integer which is the subscription's unique id.
+   */
+  RCLCPP_PUBLIC
+  uint64_t
+  add_subscription(rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription);
+
+  /// Unregister a subscription using the subscription's unique id.
+  /**
+   * This method does not allocate memory.
+   *
+   * \param intra_process_subscription_id id of the subscription to remove.
+   */
+  RCLCPP_PUBLIC
+  void
+  remove_subscription(uint64_t intra_process_subscription_id);
+
+  /// Register a publisher with the manager, returns the publisher unique id.
+  /**
+   * This method stores the publisher intra process object, together with
+   * the information of its wrapped publisher (i.e. topic name and QoS).
+   *
+   * In addition this generates a unique intra process id for the publisher.
+   *
+   * \param publisher publisher to be registered with the manager.
+   * \return an unsigned 64-bit integer which is the publisher's unique id.
+   */
+  RCLCPP_PUBLIC
+  uint64_t
+  add_publisher(rclcpp::PublisherBase::SharedPtr publisher);
+
+  /// Unregister a publisher using the publisher's unique id.
+  /**
+   * This method does not allocate memory.
+   *
+   * \param intra_process_publisher_id id of the publisher to remove.
+   */
+  RCLCPP_PUBLIC
+  void
+  remove_publisher(uint64_t intra_process_publisher_id);
+
+  /// Publishes an intra-process message, passed as a unique pointer.
+  /**
+   * This is one of the two methods for publishing intra-process.
+   *
+   * Using the intra-process publisher id, a list of recipients is obtained.
+   * This list is split in half, depending whether they require ownership or not.
+   *
+   * This particular method takes a unique pointer as input.
+   * The pointer can be promoted to a shared pointer and passed to all the subscriptions
+   * that do not require ownership.
+   * In case of subscriptions requiring ownership, the message will be copied for all of
+   * them except the last one, when ownership can be transferred.
+   *
+   * This method can save an additional copy compared to the shared pointer one.
+   *
+   * This method can throw an exception if the publisher id is not found or
+   * if the publisher shared_ptr given to add_publisher has gone out of scope.
+   *
+   * This method does allocate memory.
+   *
+   * \param intra_process_publisher_id the id of the publisher of this message.
+   * \param message the message that is being stored.
+   */
+  template<
+    typename MessageT,
+    typename Alloc = std::allocator<void>,
+    typename Deleter = std::default_delete<MessageT>>
+  void
+  do_intra_process_publish(
+    uint64_t intra_process_publisher_id,
+    std::unique_ptr<MessageT, Deleter> message,
+    std::shared_ptr<typename allocator::AllocRebind<MessageT, Alloc>::allocator_type> allocator)
+  {
+    using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
+    using MessageAllocatorT = typename MessageAllocTraits::allocator_type;
+
+    std::shared_lock<std::shared_timed_mutex> lock(mutex_);
+
+    auto publisher_it = pub_to_subs_.find(intra_process_publisher_id);
+    if (publisher_it == pub_to_subs_.end()) {
+      // Publisher is either invalid or no longer exists.
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling do_intra_process_publish for invalid or no longer existing publisher id");
+      return;
+    }
+    const auto & sub_ids = publisher_it->second;
+
+    if (sub_ids.take_ownership_subscriptions.empty()) {
+      // None of the buffers require ownership, so we promote the pointer
+      std::shared_ptr<MessageT> msg = std::move(message);
+
+      this->template add_shared_msg_to_buffers<MessageT>(msg, sub_ids.take_shared_subscriptions);
+    } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT
+      sub_ids.take_shared_subscriptions.size() <= 1)
+    {
+      // There is at maximum 1 buffer that does not require ownership.
+      // So we this case is equivalent to all the buffers requiring ownership
+
+      // Merge the two vector of ids into a unique one
+      std::vector<uint64_t> concatenated_vector(sub_ids.take_shared_subscriptions);
+      concatenated_vector.insert(
+        concatenated_vector.end(),
+        sub_ids.take_ownership_subscriptions.begin(),
+        sub_ids.take_ownership_subscriptions.end());
+
+      this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter>(
+        std::move(message),
+        concatenated_vector,
+        allocator);
+    } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT
+      sub_ids.take_shared_subscriptions.size() > 1)
+    {
+      // Construct a new shared pointer from the message
+      // for the buffers that do not require ownership
+      auto shared_msg = std::allocate_shared<MessageT, MessageAllocatorT>(*allocator, *message);
+
+      this->template add_shared_msg_to_buffers<MessageT>(
+        shared_msg, sub_ids.take_shared_subscriptions);
+      this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter>(
+        std::move(message), sub_ids.take_ownership_subscriptions, allocator);
+    }
+  }
+
+  template<
+    typename MessageT,
+    typename Alloc = std::allocator<void>,
+    typename Deleter = std::default_delete<MessageT>>
+  std::shared_ptr<const MessageT>
+  do_intra_process_publish_and_return_shared(
+    uint64_t intra_process_publisher_id,
+    std::unique_ptr<MessageT, Deleter> message,
+    std::shared_ptr<typename allocator::AllocRebind<MessageT, Alloc>::allocator_type> allocator)
+  {
+    using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
+    using MessageAllocatorT = typename MessageAllocTraits::allocator_type;
+
+    std::shared_lock<std::shared_timed_mutex> lock(mutex_);
+
+    auto publisher_it = pub_to_subs_.find(intra_process_publisher_id);
+    if (publisher_it == pub_to_subs_.end()) {
+      // Publisher is either invalid or no longer exists.
+      RCLCPP_WARN(
+        rclcpp::get_logger("rclcpp"),
+        "Calling do_intra_process_publish for invalid or no longer existing publisher id");
+      return nullptr;
+    }
+    const auto & sub_ids = publisher_it->second;
+
+    if (sub_ids.take_ownership_subscriptions.empty()) {
+      // If there are no owning, just convert to shared.
+      std::shared_ptr<MessageT> shared_msg = std::move(message);
+      if (!sub_ids.take_shared_subscriptions.empty()) {
+        this->template add_shared_msg_to_buffers<MessageT>(
+          shared_msg, sub_ids.take_shared_subscriptions);
+      }
+      return shared_msg;
+    } else {
+      // Construct a new shared pointer from the message for the buffers that
+      // do not require ownership and to return.
+      auto shared_msg = std::allocate_shared<MessageT, MessageAllocatorT>(*allocator, *message);
+
+      if (!sub_ids.take_shared_subscriptions.empty()) {
+        this->template add_shared_msg_to_buffers<MessageT>(
+          shared_msg,
+          sub_ids.take_shared_subscriptions);
+      }
+      if (!sub_ids.take_ownership_subscriptions.empty()) {
+        this->template add_owned_msg_to_buffers<MessageT, Alloc, Deleter>(
+          std::move(message),
+          sub_ids.take_ownership_subscriptions,
+          allocator);
+      }
+
+      return shared_msg;
+    }
+  }
+
+  /// Return true if the given rmw_gid_t matches any stored Publishers.
+  RCLCPP_PUBLIC
+  bool
+  matches_any_publishers(const rmw_gid_t * id) const;
+
+  /// Return the number of intraprocess subscriptions that are matched with a given publisher id.
+  RCLCPP_PUBLIC
+  size_t
+  get_subscription_count(uint64_t intra_process_publisher_id) const;
+
+  RCLCPP_PUBLIC
+  rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr
+  get_subscription_intra_process(uint64_t intra_process_subscription_id);
+
+private:
+  struct SubscriptionInfo
+  {
+    SubscriptionInfo() = default;
+
+    rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription;
+    rmw_qos_profile_t qos;
+    const char * topic_name;
+    bool use_take_shared_method;
+  };
+
+  struct PublisherInfo
+  {
+    PublisherInfo() = default;
+
+    rclcpp::PublisherBase::WeakPtr publisher;
+    rmw_qos_profile_t qos;
+    const char * topic_name;
+  };
+
+  struct SplittedSubscriptions
+  {
+    std::vector<uint64_t> take_shared_subscriptions;
+    std::vector<uint64_t> take_ownership_subscriptions;
+  };
+
+  using SubscriptionMap =
+    std::unordered_map<uint64_t, SubscriptionInfo>;
+
+  using PublisherMap =
+    std::unordered_map<uint64_t, PublisherInfo>;
+
+  using PublisherToSubscriptionIdsMap =
+    std::unordered_map<uint64_t, SplittedSubscriptions>;
+
+  RCLCPP_PUBLIC
+  static
+  uint64_t
+  get_next_unique_id();
+
+  RCLCPP_PUBLIC
+  void
+  insert_sub_id_for_pub(uint64_t sub_id, uint64_t pub_id, bool use_take_shared_method);
+
+  RCLCPP_PUBLIC
+  bool
+  can_communicate(PublisherInfo pub_info, SubscriptionInfo sub_info) const;
+
+  template<typename MessageT>
+  void
+  add_shared_msg_to_buffers(
+    std::shared_ptr<const MessageT> message,
+    std::vector<uint64_t> subscription_ids)
+  {
+    for (auto id : subscription_ids) {
+      auto subscription_it = subscriptions_.find(id);
+      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 = std::static_pointer_cast<
+        rclcpp::experimental::SubscriptionIntraProcess<MessageT>
+        >(subscription_base);
+
+      subscription->provide_intra_process_message(message);
+    }
+  }
+
+  template<
+    typename MessageT,
+    typename Alloc = std::allocator<void>,
+    typename Deleter = std::default_delete<MessageT>>
+  void
+  add_owned_msg_to_buffers(
+    std::unique_ptr<MessageT, Deleter> message,
+    std::vector<uint64_t> subscription_ids,
+    std::shared_ptr<typename allocator::AllocRebind<MessageT, Alloc>::allocator_type> allocator)
+  {
+    using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
+    using MessageUniquePtr = std::unique_ptr<MessageT, Deleter>;
+
+    for (auto it = subscription_ids.begin(); it != subscription_ids.end(); it++) {
+      auto subscription_it = subscriptions_.find(*it);
+      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 = 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);
+
+        subscription->provide_intra_process_message(std::move(copy_message));
+      }
+    }
+  }
+
+  PublisherToSubscriptionIdsMap pub_to_subs_;
+  SubscriptionMap subscriptions_;
+  PublisherMap publishers_;
+
+  mutable std::shared_timed_mutex mutex_;
+};
+
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__INTRA_PROCESS_MANAGER_HPP_

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -0,0 +1,176 @@
+// Copyright 2019 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__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_
+#define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_
+
+#include <rmw/rmw.h>
+
+#include <functional>
+#include <memory>
+#include <string>
+#include <utility>
+
+#include "rcl/error_handling.h"
+
+#include "rclcpp/any_subscription_callback.hpp"
+#include "rclcpp/experimental/buffers/intra_process_buffer.hpp"
+#include "rclcpp/experimental/create_intra_process_buffer.hpp"
+#include "rclcpp/experimental/subscription_intra_process_base.hpp"
+#include "rclcpp/type_support_decl.hpp"
+#include "rclcpp/waitable.hpp"
+#include "tracetools/tracetools.h"
+
+namespace rclcpp
+{
+namespace experimental
+{
+
+template<
+  typename MessageT,
+  typename Alloc = std::allocator<void>,
+  typename Deleter = std::default_delete<MessageT>,
+  typename CallbackMessageT = MessageT>
+class SubscriptionIntraProcess : public SubscriptionIntraProcessBase
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcess)
+
+  using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
+  using MessageAlloc = typename MessageAllocTraits::allocator_type;
+  using ConstMessageSharedPtr = std::shared_ptr<const MessageT>;
+  using MessageUniquePtr = std::unique_ptr<MessageT, Deleter>;
+
+  using BufferUniquePtr = typename rclcpp::experimental::buffers::IntraProcessBuffer<
+    MessageT,
+    Alloc,
+    Deleter
+    >::UniquePtr;
+
+  SubscriptionIntraProcess(
+    AnySubscriptionCallback<CallbackMessageT, Alloc> callback,
+    std::shared_ptr<Alloc> allocator,
+    rclcpp::Context::SharedPtr context,
+    const std::string & topic_name,
+    rmw_qos_profile_t qos_profile,
+    rclcpp::IntraProcessBufferType buffer_type)
+  : SubscriptionIntraProcessBase(topic_name, qos_profile),
+    any_callback_(callback)
+  {
+    if (!std::is_same<MessageT, CallbackMessageT>::value) {
+      throw std::runtime_error("SubscriptionIntraProcess wrong callback type");
+    }
+
+    // Create the intra-process buffer.
+    buffer_ = rclcpp::experimental::create_intra_process_buffer<MessageT, Alloc, Deleter>(
+      buffer_type,
+      qos_profile,
+      allocator);
+
+    // Create the guard condition.
+    rcl_guard_condition_options_t guard_condition_options =
+      rcl_guard_condition_get_default_options();
+
+    gc_ = rcl_get_zero_initialized_guard_condition();
+    rcl_ret_t ret = rcl_guard_condition_init(
+      &gc_, context->get_rcl_context().get(), guard_condition_options);
+
+    if (RCL_RET_OK != ret) {
+      throw std::runtime_error("SubscriptionIntraProcess init error initializing guard condition");
+    }
+
+    TRACEPOINT(
+      rclcpp_subscription_callback_added,
+      (const void *)this,
+      (const void *)&any_callback_);
+    // The callback object gets copied, so if registration is done too early/before this point
+    // (e.g. in `AnySubscriptionCallback::set()`), its address won't match any address used later
+    // in subsequent tracepoints.
+#ifndef TRACETOOLS_DISABLED
+    any_callback_.register_callback_for_tracing();
+#endif
+  }
+
+  bool
+  is_ready(rcl_wait_set_t * wait_set)
+  {
+    (void)wait_set;
+    return buffer_->has_data();
+  }
+
+  void execute()
+  {
+    execute_impl<CallbackMessageT>();
+  }
+
+  void
+  provide_intra_process_message(ConstMessageSharedPtr message)
+  {
+    buffer_->add_shared(std::move(message));
+    trigger_guard_condition();
+  }
+
+  void
+  provide_intra_process_message(MessageUniquePtr message)
+  {
+    buffer_->add_unique(std::move(message));
+    trigger_guard_condition();
+  }
+
+  bool
+  use_take_shared_method() const
+  {
+    return buffer_->use_take_shared_method();
+  }
+
+private:
+  void
+  trigger_guard_condition()
+  {
+    rcl_ret_t ret = rcl_trigger_guard_condition(&gc_);
+    (void)ret;
+  }
+
+  template<typename T>
+  typename std::enable_if<std::is_same<T, rcl_serialized_message_t>::value, void>::type
+  execute_impl()
+  {
+    throw std::runtime_error("Subscription intra-process can't handle serialized messages");
+  }
+
+  template<class T>
+  typename std::enable_if<!std::is_same<T, rcl_serialized_message_t>::value, void>::type
+  execute_impl()
+  {
+    rmw_message_info_t msg_info;
+    msg_info.publisher_gid = {0, {0}};
+    msg_info.from_intra_process = true;
+
+    if (any_callback_.use_take_shared_method()) {
+      ConstMessageSharedPtr msg = buffer_->consume_shared();
+      any_callback_.dispatch_intra_process(msg, msg_info);
+    } else {
+      MessageUniquePtr msg = buffer_->consume_unique();
+      any_callback_.dispatch_intra_process(std::move(msg), msg_info);
+    }
+  }
+
+  AnySubscriptionCallback<CallbackMessageT, Alloc> any_callback_;
+  BufferUniquePtr buffer_;
+};
+
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_

rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp

@@ -0,0 +1,88 @@
+// Copyright 2019 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__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_
+#define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_
+
+#include <rmw/rmw.h>
+
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <utility>
+
+#include "rcl/error_handling.h"
+
+#include "rclcpp/type_support_decl.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+namespace experimental
+{
+
+class SubscriptionIntraProcessBase : public rclcpp::Waitable
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase)
+
+  RCLCPP_PUBLIC
+  SubscriptionIntraProcessBase(const std::string & topic_name, rmw_qos_profile_t qos_profile)
+  : topic_name_(topic_name), qos_profile_(qos_profile)
+  {}
+
+  virtual ~SubscriptionIntraProcessBase() = default;
+
+  RCLCPP_PUBLIC
+  size_t
+  get_number_of_ready_guard_conditions() {return 1;}
+
+  RCLCPP_PUBLIC
+  bool
+  add_to_wait_set(rcl_wait_set_t * wait_set);
+
+  virtual bool
+  is_ready(rcl_wait_set_t * wait_set) = 0;
+
+  virtual void
+  execute() = 0;
+
+  virtual bool
+  use_take_shared_method() const = 0;
+
+  RCLCPP_PUBLIC
+  const char *
+  get_topic_name() const;
+
+  RCLCPP_PUBLIC
+  rmw_qos_profile_t
+  get_actual_qos() const;
+
+protected:
+  std::recursive_mutex reentrant_mutex_;
+  rcl_guard_condition_t gc_;
+
+private:
+  virtual void
+  trigger_guard_condition() = 0;
+
+  std::string topic_name_;
+  rmw_qos_profile_t qos_profile_;
+};
+
+}  // namespace experimental
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_

rclcpp/include/rclcpp/function_traits.hpp

@@ -12,13 +12,19 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_FUNCTION_TRAITS_HPP_
-#define RCLCPP_RCLCPP_FUNCTION_TRAITS_HPP_
+#ifndef RCLCPP__FUNCTION_TRAITS_HPP_
+#define RCLCPP__FUNCTION_TRAITS_HPP_
+
+#include <functional>
 #include <memory>
+#include <tuple>
 
 namespace rclcpp
 {
 
+namespace function_traits
+{
+
 /* NOTE(esteve):
  * We support service callbacks that can optionally take the request id,
  * which should be possible with two overloaded create_service methods,
@@ -43,12 +49,14 @@ template<typename FunctionT>
 struct function_traits
 {
   using arguments = typename tuple_tail<
-      typename function_traits<decltype( & FunctionT::operator())>::arguments>::type;
+    typename function_traits<decltype( &FunctionT::operator())>::arguments>::type;
 
   static constexpr std::size_t arity = std::tuple_size<arguments>::value;
 
   template<std::size_t N>
   using argument_type = typename std::tuple_element<N, arguments>::type;
+
+  using return_type = typename function_traits<decltype( &FunctionT::operator())>::return_type;
 };
 
 // Free functions
@@ -61,17 +69,75 @@ struct function_traits<ReturnTypeT(Args ...)>
 
   template<std::size_t N>
   using argument_type = typename std::tuple_element<N, arguments>::type;
+
+  using return_type = ReturnTypeT;
 };
 
 // Function pointers
 template<typename ReturnTypeT, typename ... Args>
-struct function_traits<ReturnTypeT (*)(Args ...)>: public function_traits<ReturnTypeT(Args ...)>
+struct function_traits<ReturnTypeT (*)(Args ...)>: function_traits<ReturnTypeT(Args ...)>
+{};
+
+// std::bind for object methods
+template<typename ClassT, typename ReturnTypeT, typename ... Args, typename ... FArgs>
+#if defined _LIBCPP_VERSION  // libc++ (Clang)
+struct function_traits<std::__bind<ReturnTypeT (ClassT::*)(Args ...), FArgs ...>>
+#elif defined _GLIBCXX_RELEASE  // glibc++ (GNU C++ >= 7.1)
+struct function_traits<std::_Bind<ReturnTypeT(ClassT::*(FArgs ...))(Args ...)>>
+#elif defined __GLIBCXX__  // glibc++ (GNU C++)
+struct function_traits<std::_Bind<std::_Mem_fn<ReturnTypeT (ClassT::*)(Args ...)>(FArgs ...)>>
+#elif defined _MSC_VER  // MS Visual Studio
+struct function_traits<
+  std::_Binder<std::_Unforced, ReturnTypeT (ClassT::*)(Args ...), FArgs ...>>
+#else
+#error "Unsupported C++ compiler / standard library"
+#endif
+  : function_traits<ReturnTypeT(Args ...)>
+{};
+
+// std::bind for object const methods
+template<typename ClassT, typename ReturnTypeT, typename ... Args, typename ... FArgs>
+#if defined _LIBCPP_VERSION  // libc++ (Clang)
+struct function_traits<std::__bind<ReturnTypeT (ClassT::*)(Args ...) const, FArgs ...>>
+#elif defined _GLIBCXX_RELEASE  // glibc++ (GNU C++ >= 7.1)
+struct function_traits<std::_Bind<ReturnTypeT(ClassT::*(FArgs ...))(Args ...) const>>
+#elif defined __GLIBCXX__  // glibc++ (GNU C++)
+struct function_traits<std::_Bind<std::_Mem_fn<ReturnTypeT (ClassT::*)(Args ...) const>(FArgs ...)>>
+#elif defined _MSC_VER  // MS Visual Studio
+struct function_traits<
+  std::_Binder<std::_Unforced, ReturnTypeT (ClassT::*)(Args ...) const, FArgs ...>>
+#else
+#error "Unsupported C++ compiler / standard library"
+#endif
+  : function_traits<ReturnTypeT(Args ...)>
+{};
+
+// std::bind for free functions
+template<typename ReturnTypeT, typename ... Args, typename ... FArgs>
+#if defined _LIBCPP_VERSION  // libc++ (Clang)
+struct function_traits<std::__bind<ReturnTypeT( &)(Args ...), FArgs ...>>
+#elif defined __GLIBCXX__  // glibc++ (GNU C++)
+struct function_traits<std::_Bind<ReturnTypeT(*(FArgs ...))(Args ...)>>
+#elif defined _MSC_VER  // MS Visual Studio
+struct function_traits<std::_Binder<std::_Unforced, ReturnTypeT( &)(Args ...), FArgs ...>>
+#else
+#error "Unsupported C++ compiler / standard library"
+#endif
+  : function_traits<ReturnTypeT(Args ...)>
 {};
 
 // Lambdas
 template<typename ClassT, typename ReturnTypeT, typename ... Args>
 struct function_traits<ReturnTypeT (ClassT::*)(Args ...) const>
-  : public function_traits<ReturnTypeT(ClassT &, Args ...)>
+  : function_traits<ReturnTypeT(ClassT &, Args ...)>
+{};
+
+template<typename FunctionT>
+struct function_traits<FunctionT &>: function_traits<FunctionT>
+{};
+
+template<typename FunctionT>
+struct function_traits<FunctionT &&>: function_traits<FunctionT>
 {};
 
 /* NOTE(esteve):
@@ -80,21 +146,24 @@ struct function_traits<ReturnTypeT (ClassT::*)(Args ...) const>
  */
 template<std::size_t Arity, typename FunctorT>
 struct arity_comparator : std::integral_constant<
-    bool, (Arity == function_traits<FunctorT>::arity)>{};
+    bool, (Arity == function_traits<FunctorT>::arity)> {};
 
 template<typename FunctorT, typename ... Args>
 struct check_arguments : std::is_same<
     typename function_traits<FunctorT>::arguments,
     std::tuple<Args ...>
-  >
+>
 {};
 
 template<typename FunctorAT, typename FunctorBT>
 struct same_arguments : std::is_same<
     typename function_traits<FunctorAT>::arguments,
     typename function_traits<FunctorBT>::arguments
-  >
+>
 {};
-} /* namespace rclcpp */
 
-#endif /* RCLCPP_RCLCPP_FUNCTION_TRAITS_HPP_ */
+}  // namespace function_traits
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__FUNCTION_TRAITS_HPP_

rclcpp/include/rclcpp/future_return_code.hpp

@@ -0,0 +1,61 @@
+// Copyright 2014 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__FUTURE_RETURN_CODE_HPP_
+#define RCLCPP__FUTURE_RETURN_CODE_HPP_
+
+#include <iostream>
+#include <string>
+
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Return codes to be used with spin_until_future_complete.
+/**
+ * SUCCESS: The future is complete and can be accessed with "get" without blocking.
+ *          This does not indicate that the operation succeeded; "get" may still throw an exception.
+ * INTERRUPTED: The future is not complete, spinning was interrupted by Ctrl-C or another error.
+ * TIMEOUT: Spinning timed out.
+ */
+enum class FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT};
+
+/// Stream operator for FutureReturnCode.
+RCLCPP_PUBLIC
+std::ostream &
+operator<<(std::ostream & os, const FutureReturnCode & future_return_code);
+
+/// String conversion function for FutureReturnCode.
+RCLCPP_PUBLIC
+std::string
+to_string(const FutureReturnCode & future_return_code);
+
+namespace executor
+{
+
+using FutureReturnCode [[deprecated("use rclcpp::FutureReturnCode instead")]] = FutureReturnCode;
+
+[[deprecated("use rclcpp::to_string(const rclcpp::FutureReturnCode &) instead")]]
+inline
+std::string
+to_string(const rclcpp::FutureReturnCode & future_return_code)
+{
+  return rclcpp::to_string(future_return_code);
+}
+
+}  // namespace executor
+}  // namespace rclcpp
+
+#endif  // RCLCPP__FUTURE_RETURN_CODE_HPP_

rclcpp/include/rclcpp/graph_listener.hpp

@@ -0,0 +1,187 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__GRAPH_LISTENER_HPP_
+#define RCLCPP__GRAPH_LISTENER_HPP_
+
+#include <atomic>
+#include <memory>
+#include <mutex>
+#include <thread>
+#include <vector>
+
+#include "rcl/guard_condition.h"
+#include "rcl/wait.h"
+#include "rclcpp/context.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_graph_interface.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+namespace graph_listener
+{
+
+/// Thrown when a function is called on a GraphListener that is already shutdown.
+class GraphListenerShutdownError : public std::runtime_error
+{
+public:
+  GraphListenerShutdownError()
+  : std::runtime_error("GraphListener already shutdown") {}
+};
+
+/// Thrown when a node has already been added to the GraphListener.
+class NodeAlreadyAddedError : public std::runtime_error
+{
+public:
+  NodeAlreadyAddedError()
+  : std::runtime_error("node already added") {}
+};
+
+/// Thrown when the given node is not in the GraphListener.
+class NodeNotFoundError : public std::runtime_error
+{
+public:
+  NodeNotFoundError()
+  : std::runtime_error("node not found") {}
+};
+
+/// Notifies many nodes of graph changes by listening in a thread.
+class GraphListener : public std::enable_shared_from_this<GraphListener>
+{
+public:
+  RCLCPP_PUBLIC
+  explicit GraphListener(std::shared_ptr<rclcpp::Context> parent_context);
+
+  RCLCPP_PUBLIC
+  virtual ~GraphListener();
+
+  /// Start the graph listener's listen thread if it hasn't been started.
+  /**
+   * This function is thread-safe.
+   *
+   * \throws GraphListenerShutdownError if the GraphListener is shutdown
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  start_if_not_started();
+
+  /// Add a node to the graph listener's list of nodes.
+  /**
+   * \throws GraphListenerShutdownError if the GraphListener is shutdown
+   * \throws NodeAlreadyAddedError if the given node is already in the list
+   * \throws std::invalid_argument if node is nullptr
+   * \throws std::system_error anything std::mutex::lock() throws
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph);
+
+  /// Return true if the given node is in the graph listener's list of nodes.
+  /**
+   * Also return false if given nullptr.
+   *
+   * \throws std::system_error anything std::mutex::lock() throws
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  has_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph);
+
+  /// Remove a node from the graph listener's list of nodes.
+  /**
+   *
+   * \throws NodeNotFoundError if the given node is not in the list
+   * \throws std::invalid_argument if node is nullptr
+   * \throws std::system_error anything std::mutex::lock() throws
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  remove_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph);
+
+  /// Stop the listening thread.
+  /**
+   * The thread cannot be restarted, and the class is defunct after calling.
+   * This function is called by the ~GraphListener() and does nothing if
+   * shutdown() was already called.
+   * This function exists separately from the ~GraphListener() so that it can
+   * be called before and exceptions can be caught.
+   *
+   * If start_if_not_started() was never called, this function still succeeds,
+   * but start_if_not_started() still cannot be called after this function.
+   *
+   * \throws rclcpp::execptions::RCLError from rcl_guard_condition_fini()
+   * \throws rclcpp::execptions::RCLError from rcl_wait_set_fini()
+   * \throws std::system_error anything std::mutex::lock() throws
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  shutdown();
+
+  /// Nothrow version of shutdown(), logs to RCLCPP_ERROR instead.
+  RCLCPP_PUBLIC
+  virtual
+  void
+  shutdown(const std::nothrow_t &) noexcept;
+
+  /// Return true if shutdown() has been called, else false.
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  is_shutdown();
+
+protected:
+  /// Main function for the listening thread.
+  RCLCPP_PUBLIC
+  virtual
+  void
+  run();
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  run_loop();
+
+private:
+  RCLCPP_DISABLE_COPY(GraphListener)
+
+  /** \internal */
+  void
+  __shutdown(bool);
+
+  rclcpp::Context::WeakPtr parent_context_;
+
+  std::thread listener_thread_;
+  bool is_started_;
+  std::atomic_bool is_shutdown_;
+  mutable std::mutex shutdown_mutex_;
+
+  mutable std::mutex node_graph_interfaces_barrier_mutex_;
+  mutable std::mutex node_graph_interfaces_mutex_;
+  std::vector<rclcpp::node_interfaces::NodeGraphInterface *> node_graph_interfaces_;
+
+  rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition();
+  rcl_guard_condition_t * shutdown_guard_condition_;
+  rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set();
+};
+
+}  // namespace graph_listener
+}  // namespace rclcpp
+
+#endif  // RCLCPP__GRAPH_LISTENER_HPP_

rclcpp/include/rclcpp/guard_condition.hpp

@@ -0,0 +1,100 @@
+// 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.
+
+#ifndef RCLCPP__GUARD_CONDITION_HPP_
+#define RCLCPP__GUARD_CONDITION_HPP_
+
+#include <atomic>
+
+#include "rcl/guard_condition.h"
+
+#include "rclcpp/context.hpp"
+#include "rclcpp/contexts/default_context.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// A condition that can be waited on in a single wait set and asynchronously triggered.
+class GuardCondition
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(GuardCondition)
+
+  // TODO(wjwwood): support custom allocator, maybe restrict to polymorphic allocator
+  /// Construct the guard condition, optionally specifying which Context to use.
+  /**
+   * \param[in] context Optional custom context to be used.
+   *   Defaults to using the global default context singleton.
+   *   Shared ownership of the context is held with the guard condition until
+   *   destruction.
+   * \throws std::invalid_argument if the context is nullptr.
+   * \throws rclcpp::exceptions::RCLError based exceptions when underlying
+   *   rcl functions fail.
+   */
+  RCLCPP_PUBLIC
+  explicit GuardCondition(
+    rclcpp::Context::SharedPtr context =
+    rclcpp::contexts::get_global_default_context());
+
+  RCLCPP_PUBLIC
+  virtual
+  ~GuardCondition();
+
+  /// Return the context used when creating this guard condition.
+  RCLCPP_PUBLIC
+  rclcpp::Context::SharedPtr
+  get_context() const;
+
+  /// Return the underlying rcl guard condition structure.
+  RCLCPP_PUBLIC
+  const rcl_guard_condition_t &
+  get_rcl_guard_condition() const;
+
+  /// Notify the wait set waiting on this condition, if any, that the condition had been met.
+  /**
+   * This function is thread-safe, and may be called concurrently with waiting
+   * on this guard condition in a wait set.
+   *
+   * \throws rclcpp::exceptions::RCLError based exceptions when underlying
+   *   rcl functions fail.
+   */
+  RCLCPP_PUBLIC
+  void
+  trigger();
+
+  /// Exchange the "in use by wait set" state for this guard condition.
+  /**
+   * This is used to ensure this guard condition is not used by multiple
+   * wait sets at the same time.
+   *
+   * \param[in] in_use_state the new state to exchange into the state, true
+   *   indicates it is now in use by a wait set, and false is that it is no
+   *   longer in use by a wait set.
+   * \returns the previous state.
+   */
+  RCLCPP_PUBLIC
+  bool
+  exchange_in_use_by_wait_set_state(bool in_use_state);
+
+protected:
+  rclcpp::Context::SharedPtr context_;
+  rcl_guard_condition_t rcl_guard_condition_;
+  std::atomic<bool> in_use_by_wait_set_{false};
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__GUARD_CONDITION_HPP_

rclcpp/include/rclcpp/init_options.hpp

@@ -0,0 +1,80 @@
+// Copyright 2018 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__INIT_OPTIONS_HPP_
+#define RCLCPP__INIT_OPTIONS_HPP_
+
+#include <memory>
+
+#include "rcl/init_options.h"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Encapsulation of options for initializing rclcpp.
+class InitOptions
+{
+public:
+  /// If true, the context will be shutdown on SIGINT by the signal handler (if it was installed).
+  bool shutdown_on_sigint = true;
+
+  /// Constructor which allows you to specify the allocator used within the init options.
+  RCLCPP_PUBLIC
+  explicit InitOptions(rcl_allocator_t allocator = rcl_get_default_allocator());
+
+  /// Constructor which is initialized by an existing init_options.
+  RCLCPP_PUBLIC
+  explicit InitOptions(const rcl_init_options_t & init_options);
+
+  /// Copy constructor.
+  RCLCPP_PUBLIC
+  InitOptions(const InitOptions & other);
+
+  /// Return `true` if logging should be initialized when `rclcpp::Context::init` is called.
+  RCLCPP_PUBLIC
+  bool
+  auto_initialize_logging() const;
+
+  /// Set flag indicating if logging should be initialized or not.
+  RCLCPP_PUBLIC
+  InitOptions &
+  auto_initialize_logging(bool initialize_logging);
+
+  /// Assignment operator.
+  RCLCPP_PUBLIC
+  InitOptions &
+  operator=(const InitOptions & other);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~InitOptions();
+
+  /// Return the rcl init options.
+  RCLCPP_PUBLIC
+  const rcl_init_options_t *
+  get_rcl_init_options() const;
+
+protected:
+  void
+  finalize_init_options();
+
+private:
+  std::unique_ptr<rcl_init_options_t> init_options_;
+  bool initialize_logging_{true};
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__INIT_OPTIONS_HPP_

rclcpp/include/rclcpp/intra_process_buffer_type.hpp

@@ -0,0 +1,35 @@
+// Copyright 2019 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__INTRA_PROCESS_BUFFER_TYPE_HPP_
+#define RCLCPP__INTRA_PROCESS_BUFFER_TYPE_HPP_
+
+namespace rclcpp
+{
+
+/// Used as argument in create_publisher and create_subscriber
+/// when intra-process communication is enabled
+enum class IntraProcessBufferType
+{
+  /// Set the data type used in the intra-process buffer as std::shared_ptr<MessageT>
+  SharedPtr,
+  /// Set the data type used in the intra-process buffer as std::unique_ptr<MessageT>
+  UniquePtr,
+  /// Set the data type used in the intra-process buffer as the same used in the callback
+  CallbackDefault
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__INTRA_PROCESS_BUFFER_TYPE_HPP_

rclcpp/include/rclcpp/intra_process_manager.hpp

@@ -1,377 +0,0 @@
-// Copyright 2015 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__INTRA_PROCESS_MANAGER_HPP_
-#define RCLCPP__INTRA_PROCESS_MANAGER_HPP_
-
-#include <rmw/types.h>
-
-#include <algorithm>
-#include <atomic>
-#include <cstdint>
-#include <exception>
-#include <map>
-#include <memory>
-#include <unordered_map>
-#include <set>
-
-#include "rclcpp/allocator/allocator_deleter.hpp"
-#include "rclcpp/intra_process_manager_state.hpp"
-#include "rclcpp/mapped_ring_buffer.hpp"
-#include "rclcpp/macros.hpp"
-#include "rclcpp/publisher.hpp"
-#include "rclcpp/subscription.hpp"
-
-namespace rclcpp
-{
-namespace intra_process_manager
-{
-
-/// This class facilitates intra process communication between nodes.
-/* This class is used in the creation of publishers and subscriptions.
- * A singleton instance of this class is owned by a rclcpp::Context and a
- * rclcpp::Node can use an associated Context to get an instance of this class.
- * Nodes which do not have a common Context will not exchange intra process
- * messages because they will not share access to an instance of this class.
- *
- * When a Node creates a publisher or subscription, it will register them
- * with this class.
- * The node will also hook into the publisher's publish call
- * in order to do intra process related work.
- *
- * When a publisher is created, it advertises on the topic the user provided,
- * as well as a "shadowing" topic of type rcl_interfaces/IntraProcessMessage.
- * For instance, if the user specified the topic '/namespace/chatter', then the
- * corresponding intra process topic might be '/namespace/chatter__intra'.
- * The publisher is also allocated an id which is unique among all publishers
- * and subscriptions in this process.
- * Additionally, when registered with this class a ring buffer is created and
- * owned by this class as a temporary place to hold messages destined for intra
- * process subscriptions.
- *
- * When a subscription is created, it subscribes to the topic provided by the
- * user as well as to the corresponding intra process topic.
- * It is also gets a unique id from the singleton instance of this class which
- * is unique among publishers and subscriptions.
- *
- * When the user publishes a message, the message is stored by calling
- * store_intra_process_message on this class.
- * The instance of that message is uniquely identified by a publisher id and a
- * message sequence number.
- * The publisher id, message sequence pair is unique with in the process.
- * At that point a list of the id's of intra process subscriptions which have
- * been registered with the singleton instance of this class are stored with
- * the message instance so that delivery is only made to those subscriptions.
- * Then an instance of rcl_interfaces/IntraProcessMessage is published to the
- * intra process topic which is specific to the topic specified by the user.
- *
- * When an instance of rcl_interfaces/IntraProcessMessage is received by a
- * subscription, then it is handled by calling take_intra_process_message
- * on a singleton of this class.
- * The subscription passes a publisher id, message sequence pair which
- * uniquely identifies the message instance it was suppose to receive as well
- * as the subscriptions unique id.
- * If the message is still being held by this class and the subscription's id
- * is in the list of intended subscriptions then the message is returned.
- * If either of those predicates are not satisfied then the message is not
- * returned and the subscription does not call the users callback.
- *
- * Since the publisher builds a list of destined subscriptions on publish, and
- * other requests are ignored, this class knows how many times a message
- * instance should be requested.
- * The final time a message is requested, the ownership is passed out of this
- * class and passed to the final subscription, effectively freeing space in
- * this class's internal storage.
- *
- * Since a topic is being used to ferry notifications about new intra process
- * messages between publishers and subscriptions, it is possible for that
- * notification to be lost.
- * It is also possible that a subscription which was available when publish was
- * called will no longer exist once the notification gets posted.
- * In both cases this might result in a message instance getting requested
- * fewer times than expected.
- * This is why the internal storage of this class is a ring buffer.
- * That way if a message is orphaned it will eventually be dropped from storage
- * when a new message instance is stored and will not result in a memory leak.
- *
- * However, since the storage system is finite, this also means that a message
- * instance might get displaced by an incoming message instance before all
- * interested parties have called take_intra_process_message.
- * Because of this the size of the internal storage should be carefully
- * considered.
- *
- * /TODO(wjwwood): update to include information about handling latching.
- * /TODO(wjwwood): consider thread safety of the class.
- *
- * This class is neither CopyConstructable nor CopyAssignable.
- */
-class IntraProcessManager
-{
-private:
-  RCLCPP_DISABLE_COPY(IntraProcessManager);
-
-public:
-  RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManager);
-
-  explicit IntraProcessManager(
-    IntraProcessManagerStateBase::SharedPtr state = create_default_state()
-  )
-  : state_(state)
-  {
-  }
-
-  /// Register a subscription with the manager, returns subscriptions unique id.
-  /* In addition to generating a unique intra process id for the subscription,
-   * this method also stores the topic name of the subscription.
-   *
-   * This method is normally called during the creation of a subscription,
-   * but after it creates the internal intra process rmw_subscription_t.
-   *
-   * This method will allocate memory.
-   *
-   * \param subscription the Subscription to register.
-   * \return an unsigned 64-bit integer which is the subscription's unique id.
-   */
-  uint64_t
-  add_subscription(subscription::SubscriptionBase::SharedPtr subscription)
-  {
-    auto id = IntraProcessManager::get_next_unique_id();
-    state_->add_subscription(id, subscription);
-    return id;
-  }
-
-  /// Unregister a subscription using the subscription's unique id.
-  /* This method does not allocate memory.
-   *
-   * \param intra_process_subscription_id id of the subscription to remove.
-   */
-  void
-  remove_subscription(uint64_t intra_process_subscription_id)
-  {
-    state_->remove_subscription(intra_process_subscription_id);
-  }
-
-  /// Register a publisher with the manager, returns the publisher unique id.
-  /* In addition to generating and returning a unique id for the publisher,
-   * this method creates internal ring buffer storage for "in-flight" intra
-   * process messages which are stored when store_intra_process_message is
-   * called with this publisher's unique id.
-   *
-   * The buffer_size must be less than or equal to the max uint64_t value.
-   * If the buffer_size is 0 then a buffer size is calculated using the
-   * publisher's QoS settings.
-   * The default is to use the depth field of the publisher's QoS.
-   * TODO(wjwwood): Consider doing depth *= 1.2, round up, or similar.
-   * TODO(wjwwood): Consider what to do for keep all.
-   *
-   * This method is templated on the publisher's message type so that internal
-   * storage of the same type can be allocated.
-   *
-   * This method will allocate memory.
-   *
-   * \param publisher publisher to be registered with the manager.
-   * \param buffer_size if 0 (default) a size is calculated based on the QoS.
-   * \return an unsigned 64-bit integer which is the publisher's unique id.
-   */
-  template<typename MessageT, typename Alloc>
-  uint64_t
-  add_publisher(typename publisher::Publisher<MessageT, Alloc>::SharedPtr publisher,
-    size_t buffer_size = 0)
-  {
-    auto id = IntraProcessManager::get_next_unique_id();
-    size_t size = buffer_size > 0 ? buffer_size : publisher->get_queue_size();
-    auto mrb = mapped_ring_buffer::MappedRingBuffer<MessageT,
-      typename publisher::Publisher<MessageT, Alloc>::MessageAlloc>::make_shared(
-      size, publisher->get_allocator());
-    state_->add_publisher(id, publisher, mrb, size);
-    return id;
-  }
-
-  /// Unregister a publisher using the publisher's unique id.
-  /* This method does not allocate memory.
-   *
-   * \param intra_process_publisher_id id of the publisher to remove.
-   */
-  void
-  remove_publisher(uint64_t intra_process_publisher_id)
-  {
-    state_->remove_publisher(intra_process_publisher_id);
-  }
-
-  /// Store a message in the manager, and return the message sequence number.
-  /* The given message is stored in internal storage using the given publisher
-   * id and the newly generated message sequence, which is also returned.
-   * The combination of publisher id and message sequence number can later
-   * be used with a subscription id to retrieve the message by calling
-   * take_intra_process_message.
-   * The number of times take_intra_process_message can be called with this
-   * unique pair of id's is determined by the number of subscriptions currently
-   * subscribed to the same topic and which share the same Context, i.e. once
-   * for each subscription which should receive the intra process message.
-   *
-   * The ownership of the incoming message is transfered to the internal
-   * storage in order to avoid copying the message data.
-   * Therefore, the message parameter will no longer contain the original
-   * message after calling this method.
-   * Instead it will either be a nullptr or it will contain the ownership of
-   * the message instance which was displaced.
-   * If the message parameter is not equal to nullptr after calling this method
-   * then a message was prematurely displaced, i.e. take_intra_process_message
-   * had not been called on it as many times as was expected.
-   *
-   * This method can throw an exception if the publisher id is not found or
-   * if the publisher shared_ptr given to add_publisher has gone out of scope.
-   *
-   * This method does allocate memory.
-   *
-   * \param intra_process_publisher_id the id of the publisher of this message.
-   * \param message the message that is being stored.
-   * \return the message sequence number.
-   */
-  template<typename MessageT, typename Alloc = std::allocator<void>,
-  typename Deleter = std::default_delete<MessageT>>
-  uint64_t
-  store_intra_process_message(
-    uint64_t intra_process_publisher_id,
-    std::unique_ptr<MessageT, Deleter> & message)
-  {
-    using MRBMessageAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>;
-    using TypedMRB = typename mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc>;
-    uint64_t message_seq = 0;
-    mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = state_->get_publisher_info_for_id(
-      intra_process_publisher_id, message_seq);
-    typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast<TypedMRB>(buffer);
-    if (!typed_buffer) {
-      throw std::runtime_error("Typecast failed due to incorrect message type");
-    }
-
-    // Insert the message into the ring buffer using the message_seq to identify it.
-    bool did_replace = typed_buffer->push_and_replace(message_seq, message);
-    // TODO(wjwwood): do something when a message was displaced. log debug?
-    (void)did_replace;  // Avoid unused variable warning.
-
-    state_->store_intra_process_message(intra_process_publisher_id, message_seq);
-
-    // Return the message sequence which is sent to the subscription.
-    return message_seq;
-  }
-
-  /// Take an intra process message.
-  /* The intra_process_publisher_id and message_sequence_number parameters
-   * uniquely identify a message instance, which should be taken.
-   *
-   * The requesting_subscriptions_intra_process_id parameter is used to make
-   * sure the requesting subscription was intended to receive this message
-   * instance.
-   * This check is made because it could happen that the requester
-   * comes up after the publish event, so it still receives the notification of
-   * a new intra process message, but it wasn't registered with the manager at
-   * the time of publishing, causing it to take when it wasn't intended.
-   * This should be avioded unless latching-like behavior is involved.
-   *
-   * The message parameter is used to store the taken message.
-   * On the last expected call to this method, the ownership is transfered out
-   * of internal storage and into the message parameter.
-   * On all previous calls a copy of the internally stored message is made and
-   * the ownership of the copy is transfered to the message parameter.
-   * TODO(wjwwood): update this documentation when latching is supported.
-   *
-   * The message parameter can be set to nullptr if:
-   *
-   * - The publisher id is not found.
-   * - The message sequence is not found for the given publisher id.
-   * - The requesting subscription's id is not in the list of intended takers.
-   * - The requesting subscription's id has been used before with this message.
-   *
-   * This method may allocate memory to copy the stored message.
-   *
-   * \param intra_process_publisher_id the id of the message's publisher.
-   * \param message_sequence_number the sequence number of the message.
-   * \param requesting_subscriptions_intra_process_id the subscription's id.
-   * \param message the message typed unique_ptr used to return the message.
-   */
-  template<typename MessageT, typename Alloc = std::allocator<void>,
-  typename Deleter = std::default_delete<MessageT>>
-  void
-  take_intra_process_message(
-    uint64_t intra_process_publisher_id,
-    uint64_t message_sequence_number,
-    uint64_t requesting_subscriptions_intra_process_id,
-    std::unique_ptr<MessageT, Deleter> & message)
-  {
-    using MRBMessageAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>;
-    using TypedMRB = mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc>;
-    message = nullptr;
-
-    size_t target_subs_size = 0;
-    mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = state_->take_intra_process_message(
-      intra_process_publisher_id,
-      message_sequence_number,
-      requesting_subscriptions_intra_process_id,
-      target_subs_size
-      );
-    typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast<TypedMRB>(buffer);
-    if (!typed_buffer) {
-      return;
-    }
-    // Return a copy or the unique_ptr (ownership) depending on how many subscriptions are left.
-    if (target_subs_size) {
-      // There are more subscriptions to serve, return a copy.
-      typed_buffer->get_copy_at_key(message_sequence_number, message);
-    } else {
-      // This is the last one to be returned, transfer ownership.
-      typed_buffer->pop_at_key(message_sequence_number, message);
-    }
-  }
-
-  /// Return true if the given rmw_gid_t matches any stored Publishers.
-  bool
-  matches_any_publishers(const rmw_gid_t * id) const
-  {
-    return state_->matches_any_publishers(id);
-  }
-
-private:
-  static uint64_t get_next_unique_id()
-  {
-    auto next_id = next_unique_id_.fetch_add(1, std::memory_order_relaxed);
-    // Check for rollover (we started at 1).
-    if (0 == next_id) {
-      // This puts a technical limit on the number of times you can add a publisher or subscriber.
-      // But even if you could add (and remove) them at 1 kHz (very optimistic rate)
-      // it would still be a very long time before you could exhaust the pool of id's:
-      //   2^64 / 1000 times per sec / 60 sec / 60 min / 24 hours / 365 days = 584,942,417 years
-      // So around 585 million years. Even at 1 GHz, it would take 585 years.
-      // I think it's safe to avoid trying to handle overflow.
-      // If we roll over then it's most likely a bug.
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-      throw std::overflow_error(
-        "exhausted the unique id's for publishers and subscribers in this process "
-        "(congratulations your computer is either extremely fast or extremely old)");
-      // *INDENT-ON*
-    }
-    return next_id;
-  }
-
-  static std::atomic<uint64_t> next_unique_id_;
-
-  IntraProcessManagerStateBase::SharedPtr state_;
-};
-
-std::atomic<uint64_t> IntraProcessManager::next_unique_id_ {1};
-
-}  // namespace intra_process_manager
-}  // namespace rclcpp
-
-#endif  // RCLCPP__INTRA_PROCESS_MANAGER_HPP_

rclcpp/include/rclcpp/intra_process_manager_state.hpp

@@ -1,270 +0,0 @@
-// Copyright 2015 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__INTRA_PROCESS_MANAGER_STATE_HPP_
-#define RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_
-
-#include <rclcpp/mapped_ring_buffer.hpp>
-#include <rclcpp/publisher.hpp>
-#include <rclcpp/subscription.hpp>
-
-#include <algorithm>
-#include <atomic>
-#include <functional>
-#include <limits>
-#include <memory>
-#include <map>
-#include <unordered_map>
-#include <set>
-#include <string>
-#include <utility>
-
-namespace rclcpp
-{
-namespace intra_process_manager
-{
-
-class IntraProcessManagerStateBase
-{
-public:
-  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(IntraProcessManagerStateBase);
-
-  virtual void
-  add_subscription(uint64_t id, subscription::SubscriptionBase::SharedPtr subscription) = 0;
-
-  virtual void
-  remove_subscription(uint64_t intra_process_subscription_id) = 0;
-
-  virtual void add_publisher(uint64_t id,
-    publisher::PublisherBase::WeakPtr publisher,
-    mapped_ring_buffer::MappedRingBufferBase::SharedPtr mrb,
-    size_t size) = 0;
-
-  virtual void
-  remove_publisher(uint64_t intra_process_publisher_id) = 0;
-
-  virtual mapped_ring_buffer::MappedRingBufferBase::SharedPtr
-  get_publisher_info_for_id(
-    uint64_t intra_process_publisher_id,
-    uint64_t & message_seq) = 0;
-
-  virtual void
-  store_intra_process_message(uint64_t intra_process_publisher_id, uint64_t message_seq) = 0;
-
-  virtual mapped_ring_buffer::MappedRingBufferBase::SharedPtr
-  take_intra_process_message(uint64_t intra_process_publisher_id,
-    uint64_t message_sequence_number,
-    uint64_t requesting_subscriptions_intra_process_id,
-    size_t & size) = 0;
-
-  virtual bool
-  matches_any_publishers(const rmw_gid_t * id) const = 0;
-};
-
-template<typename Allocator = std::allocator<void>>
-class IntraProcessManagerState : public IntraProcessManagerStateBase
-{
-public:
-  void
-  add_subscription(uint64_t id, subscription::SubscriptionBase::SharedPtr subscription)
-  {
-    subscriptions_[id] = subscription;
-    subscription_ids_by_topic_[subscription->get_topic_name()].insert(id);
-  }
-
-  void
-  remove_subscription(uint64_t intra_process_subscription_id)
-  {
-    subscriptions_.erase(intra_process_subscription_id);
-    for (auto & pair : subscription_ids_by_topic_) {
-      pair.second.erase(intra_process_subscription_id);
-    }
-    // Iterate over all publisher infos and all stored subscription id's and
-    // remove references to this subscription's id.
-    for (auto & publisher_pair : publishers_) {
-      for (auto & sub_pair : publisher_pair.second.target_subscriptions_by_message_sequence) {
-        sub_pair.second.erase(intra_process_subscription_id);
-      }
-    }
-  }
-
-  void add_publisher(uint64_t id,
-    publisher::PublisherBase::WeakPtr publisher,
-    mapped_ring_buffer::MappedRingBufferBase::SharedPtr mrb,
-    size_t size)
-  {
-    publishers_[id].publisher = publisher;
-    // As long as the size of the ring buffer is less than the max sequence number, we're safe.
-    if (size > std::numeric_limits<uint64_t>::max()) {
-      throw std::invalid_argument("the calculated buffer size is too large");
-    }
-    publishers_[id].sequence_number.store(0);
-
-    publishers_[id].buffer = mrb;
-    publishers_[id].target_subscriptions_by_message_sequence.reserve(size);
-  }
-
-  void
-  remove_publisher(uint64_t intra_process_publisher_id)
-  {
-    publishers_.erase(intra_process_publisher_id);
-  }
-
-  // return message_seq and mrb
-  mapped_ring_buffer::MappedRingBufferBase::SharedPtr
-  get_publisher_info_for_id(
-    uint64_t intra_process_publisher_id,
-    uint64_t & message_seq)
-  {
-    auto it = publishers_.find(intra_process_publisher_id);
-    if (it == publishers_.end()) {
-      throw std::runtime_error("store_intra_process_message called with invalid publisher id");
-    }
-    PublisherInfo & info = it->second;
-    // Calculate the next message sequence number.
-    message_seq = info.sequence_number.fetch_add(1, std::memory_order_relaxed);
-
-    return info.buffer;
-  }
-
-  void
-  store_intra_process_message(uint64_t intra_process_publisher_id, uint64_t message_seq)
-  {
-    auto it = publishers_.find(intra_process_publisher_id);
-    if (it == publishers_.end()) {
-      throw std::runtime_error("store_intra_process_message called with invalid publisher id");
-    }
-    PublisherInfo & info = it->second;
-    auto publisher = info.publisher.lock();
-    if (!publisher) {
-      throw std::runtime_error("publisher has unexpectedly gone out of scope");
-    }
-
-    // Figure out what subscriptions should receive the message.
-    auto & destined_subscriptions = subscription_ids_by_topic_[publisher->get_topic_name()];
-    // Store the list for later comparison.
-    info.target_subscriptions_by_message_sequence[message_seq].clear();
-    std::copy(
-      destined_subscriptions.begin(), destined_subscriptions.end(),
-      // Memory allocation occurs in info.target_subscriptions_by_message_sequence[message_seq]
-      std::inserter(
-        info.target_subscriptions_by_message_sequence[message_seq],
-        // This ends up only being a hint to std::set, could also be .begin().
-        info.target_subscriptions_by_message_sequence[message_seq].end()
-      )
-    );
-  }
-
-  mapped_ring_buffer::MappedRingBufferBase::SharedPtr
-  take_intra_process_message(uint64_t intra_process_publisher_id,
-    uint64_t message_sequence_number,
-    uint64_t requesting_subscriptions_intra_process_id,
-    size_t & size
-  )
-  {
-    PublisherInfo * info;
-    {
-      auto it = publishers_.find(intra_process_publisher_id);
-      if (it == publishers_.end()) {
-        // Publisher is either invalid or no longer exists.
-        return 0;
-      }
-      info = &it->second;
-    }
-    // Figure out how many subscriptions are left.
-    AllocSet * target_subs;
-    {
-      auto it = info->target_subscriptions_by_message_sequence.find(message_sequence_number);
-      if (it == info->target_subscriptions_by_message_sequence.end()) {
-        // Message is no longer being stored by this publisher.
-        return 0;
-      }
-      target_subs = &it->second;
-    }
-    {
-      auto it = std::find(
-        target_subs->begin(), target_subs->end(),
-        requesting_subscriptions_intra_process_id);
-      if (it == target_subs->end()) {
-        // This publisher id/message seq pair was not intended for this subscription.
-        return 0;
-      }
-      target_subs->erase(it);
-    }
-    size = target_subs->size();
-    return info->buffer;
-  }
-
-  bool
-  matches_any_publishers(const rmw_gid_t * id) const
-  {
-    for (auto & publisher_pair : publishers_) {
-      auto publisher = publisher_pair.second.publisher.lock();
-      if (!publisher) {
-        continue;
-      }
-      if (*publisher.get() == id) {
-        return true;
-      }
-    }
-    return false;
-  }
-
-private:
-  template<typename T>
-  using RebindAlloc = typename std::allocator_traits<Allocator>::template rebind_alloc<T>;
-
-  using AllocSet = std::set<uint64_t, std::less<uint64_t>, RebindAlloc<uint64_t>>;
-  using SubscriptionMap = std::unordered_map<uint64_t, subscription::SubscriptionBase::WeakPtr,
-      std::hash<uint64_t>, std::equal_to<uint64_t>,
-      RebindAlloc<std::pair<const uint64_t, subscription::SubscriptionBase::WeakPtr>>>;
-  using IDTopicMap = std::map<std::string, AllocSet,
-      std::less<std::string>, RebindAlloc<std::pair<std::string, AllocSet>>>;
-
-  SubscriptionMap subscriptions_;
-
-  IDTopicMap subscription_ids_by_topic_;
-
-  struct PublisherInfo
-  {
-    RCLCPP_DISABLE_COPY(PublisherInfo);
-
-    PublisherInfo() = default;
-
-    publisher::PublisherBase::WeakPtr publisher;
-    std::atomic<uint64_t> sequence_number;
-    mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer;
-
-    using TargetSubscriptionsMap = std::unordered_map<uint64_t, AllocSet,
-        std::hash<uint64_t>, std::equal_to<uint64_t>,
-        RebindAlloc<std::pair<const uint64_t, AllocSet>>>;
-    TargetSubscriptionsMap target_subscriptions_by_message_sequence;
-  };
-
-  using PublisherMap = std::unordered_map<uint64_t, PublisherInfo,
-      std::hash<uint64_t>, std::equal_to<uint64_t>,
-      RebindAlloc<std::pair<const uint64_t, PublisherInfo>>>;
-
-  PublisherMap publishers_;
-};
-
-static IntraProcessManagerStateBase::SharedPtr create_default_state()
-{
-  return std::make_shared<IntraProcessManagerState<>>();
-}
-
-}  // namespace intra_process_manager
-}  // namespace rclcpp
-
-#endif  // RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_

rclcpp/include/rclcpp/intra_process_setting.hpp

@@ -0,0 +1,34 @@
+// Copyright 2019 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__INTRA_PROCESS_SETTING_HPP_
+#define RCLCPP__INTRA_PROCESS_SETTING_HPP_
+
+namespace rclcpp
+{
+
+/// Used as argument in create_publisher and create_subscriber.
+enum class IntraProcessSetting
+{
+  /// Explicitly enable intraprocess comm at publisher/subscription level.
+  Enable,
+  /// Explicitly disable intraprocess comm at publisher/subscription level.
+  Disable,
+  /// Take intraprocess configuration from the node.
+  NodeDefault
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__INTRA_PROCESS_SETTING_HPP_

rclcpp/include/rclcpp/loaned_message.hpp

@@ -0,0 +1,207 @@
+// Copyright 2019 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__LOANED_MESSAGE_HPP_
+#define RCLCPP__LOANED_MESSAGE_HPP_
+
+#include <memory>
+#include <utility>
+
+#include "rclcpp/allocator/allocator_common.hpp"
+#include "rclcpp/logging.hpp"
+#include "rclcpp/publisher_base.hpp"
+
+#include "rcl/allocator.h"
+#include "rcl/publisher.h"
+
+namespace rclcpp
+{
+
+template<typename MessageT, typename AllocatorT = std::allocator<void>>
+class LoanedMessage
+{
+  using MessageAllocatorTraits = rclcpp::allocator::AllocRebind<MessageT, AllocatorT>;
+  using MessageAllocator = typename MessageAllocatorTraits::allocator_type;
+
+public:
+  /// Constructor of the LoanedMessage class.
+  /**
+   * The constructor of this class allocates memory for a given message type
+   * and associates this with a given publisher.
+   *
+   * Given the publisher instance, a case differentiation is being performaned
+   * which decides whether the underlying middleware is able to allocate the appropriate
+   * memory for this message type or not.
+   * In the case that the middleware can not loan messages, the passed in allocator instance
+   * is being used to allocate the message within the scope of this class.
+   * Otherwise, the allocator is being ignored and the allocation is solely performaned
+   * in the underlying middleware with its appropriate allocation strategy.
+   * The need for this arises as the user code can be written explicitly targeting a middleware
+   * capable of loaning messages.
+   * However, this user code is ought to be usable even when dynamically linked against
+   * a middleware which doesn't support message loaning in which case the allocator will be used.
+   *
+   * \param pub rclcpp::Publisher instance to which the memory belongs
+   * \param allocator Allocator instance in case middleware can not allocate messages
+   */
+  LoanedMessage(
+    const rclcpp::PublisherBase & pub,
+    std::allocator<MessageT> allocator)
+  : pub_(pub),
+    message_(nullptr),
+    message_allocator_(std::move(allocator))
+  {
+    if (pub_.can_loan_messages()) {
+      void * message_ptr = nullptr;
+      auto ret = rcl_borrow_loaned_message(
+        pub_.get_publisher_handle(),
+        rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>(),
+        &message_ptr);
+      if (RCL_RET_OK != ret) {
+        rclcpp::exceptions::throw_from_rcl_error(ret);
+      }
+      message_ = static_cast<MessageT *>(message_ptr);
+    } else {
+      RCLCPP_INFO_ONCE(
+        rclcpp::get_logger("rclcpp"),
+        "Currently used middleware can't loan messages. Local allocator will be used.");
+      message_ = message_allocator_.allocate(1);
+      new (message_) MessageT();
+    }
+  }
+
+  /// Constructor of the LoanedMessage class.
+  /**
+   * The constructor of this class allocates memory for a given message type
+   * and associates this with a given publisher.
+   *
+   * Given the publisher instance, a case differentiation is being performaned
+   * which decides whether the underlying middleware is able to allocate the appropriate
+   * memory for this message type or not.
+   * In the case that the middleware can not loan messages, the passed in allocator instance
+   * is being used to allocate the message within the scope of this class.
+   * Otherwise, the allocator is being ignored and the allocation is solely performaned
+   * in the underlying middleware with its appropriate allocation strategy.
+   * The need for this arises as the user code can be written explicitly targeting a middleware
+   * capable of loaning messages.
+   * However, this user code is ought to be usable even when dynamically linked against
+   * a middleware which doesn't support message loaning in which case the allocator will be used.
+   *
+   * \param pub rclcpp::Publisher instance to which the memory belongs
+   * \param allocator Allocator instance in case middleware can not allocate messages
+   */
+  LoanedMessage(
+    const rclcpp::PublisherBase * pub,
+    std::shared_ptr<std::allocator<MessageT>> allocator)
+  : LoanedMessage(*pub, *allocator)
+  {}
+
+  /// Move semantic for RVO
+  LoanedMessage(LoanedMessage<MessageT> && other)
+  : pub_(std::move(other.pub_)),
+    message_(std::move(other.message_)),
+    message_allocator_(std::move(other.message_allocator_))
+  {}
+
+  /// Destructor of the LoanedMessage class.
+  /**
+   * The destructor has the explicit task to return the allocated memory for its message
+   * instance.
+   * If the message was previously allocated via the middleware, the message is getting
+   * returned to the middleware to cleanly destroy the allocation.
+   * In the case that the local allocator instance was used, the same instance is then
+   * being used to destroy the allocated memory.
+   *
+   * The contract here is that the memory for this message is valid as long as this instance
+   * of the LoanedMessage class is alive.
+   */
+  virtual ~LoanedMessage()
+  {
+    auto error_logger = rclcpp::get_logger("LoanedMessage");
+
+    if (message_ == nullptr) {
+      return;
+    }
+
+    if (pub_.can_loan_messages()) {
+      // return allocated memory to the middleware
+      auto ret =
+        rcl_return_loaned_message_from_publisher(pub_.get_publisher_handle(), message_);
+      if (ret != RCL_RET_OK) {
+        RCLCPP_ERROR(
+          error_logger, "rcl_deallocate_loaned_message failed: %s", rcl_get_error_string().str);
+        rcl_reset_error();
+      }
+    } else {
+      // call destructor before deallocating
+      message_->~MessageT();
+      message_allocator_.deallocate(message_, 1);
+    }
+    message_ = nullptr;
+  }
+
+  /// Validate if the message was correctly allocated.
+  /**
+   * The allocated memory might not be always consistent and valid.
+   * Reasons why this could fail is that an allocation step was failing,
+   * e.g. just like malloc could fail or a maximum amount of previously allocated
+   * messages is exceeded in which case the loaned messages have to be returned
+   * to the middleware prior to be able to allocate a new one.
+   */
+  bool is_valid() const
+  {
+    return message_ != nullptr;
+  }
+
+  /// Access the ROS message instance.
+  /**
+   * A call to `get()` will return a mutable reference to the underlying ROS message instance.
+   * This allows a user to modify the content of the message prior to publishing it.
+   *
+   * If this reference is copied, the memory for this copy is no longer managed
+   * by the LoanedMessage instance and has to be cleanup individually.
+   */
+  MessageT & get() const
+  {
+    return *message_;
+  }
+
+  /// Release ownership of the ROS message instance.
+  /**
+   * 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.
+   *
+   * \return Raw pointer to the message instance.
+   */
+  MessageT * release()
+  {
+    auto msg = message_;
+    message_ = nullptr;
+    return msg;
+  }
+
+protected:
+  const rclcpp::PublisherBase & pub_;
+
+  MessageT * message_;
+
+  MessageAllocator message_allocator_;
+
+  /// Deleted copy constructor to preserve memory integrity.
+  LoanedMessage(const LoanedMessage<MessageT> & other) = delete;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__LOANED_MESSAGE_HPP_

rclcpp/include/rclcpp/logger.hpp

@@ -0,0 +1,145 @@
+// Copyright 2017 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__LOGGER_HPP_
+#define RCLCPP__LOGGER_HPP_
+
+#include <memory>
+#include <string>
+
+#include "rclcpp/visibility_control.hpp"
+
+#include "rcl/node.h"
+
+/**
+ * \def RCLCPP_LOGGING_ENABLED
+ * When this define evaluates to true (default), logger factory functions will
+ * behave normally.
+ * When false, logger factory functions will create dummy loggers to avoid
+ * computational expense in manipulating objects.
+ * This should be used in combination with `RCLCPP_LOG_MIN_SEVERITY` to compile
+ * out logging macros.
+ */
+// TODO(dhood): determine this automatically from `RCLCPP_LOG_MIN_SEVERITY`
+#ifndef RCLCPP_LOGGING_ENABLED
+#define RCLCPP_LOGGING_ENABLED 1
+#endif
+
+namespace rclcpp
+{
+
+// Forward declaration is used for friend statement.
+namespace node_interfaces
+{
+class NodeLogging;
+}
+
+class Logger;
+
+/// Return a named logger.
+/**
+ * The returned logger's name will include any naming conventions, such as a
+ * name prefix.
+ * Currently there are no such naming conventions but they may be introduced in
+ * the future.
+ *
+ * \param[in] name the name of the logger
+ * \return a logger with the fully-qualified name including naming conventions, or
+ * \return a dummy logger if logging is disabled.
+ */
+RCLCPP_PUBLIC
+Logger
+get_logger(const std::string & name);
+
+/// Return a named logger using an rcl_node_t.
+/**
+ * This is a convenience function that does error checking and returns the node
+ * logger name, or "rclcpp" if it is unable to get the node name.
+ *
+ * \param[in] node the rcl node from which to get the logger name
+ * \return a logger based on the node name, or "rclcpp" if there's an error
+ */
+RCLCPP_PUBLIC
+Logger
+get_node_logger(const rcl_node_t * node);
+
+class Logger
+{
+private:
+  friend Logger rclcpp::get_logger(const std::string & name);
+  friend ::rclcpp::node_interfaces::NodeLogging;
+
+  /// Constructor of a dummy logger.
+  /**
+   * This is used when logging is disabled: see `RCLCPP_LOGGING_ENABLED`.
+   * This cannot be called directly, see `rclcpp::get_logger` instead.
+   */
+  Logger()
+  : name_(nullptr) {}
+
+  /// Constructor of a named logger.
+  /**
+   * This cannot be called directly, see `rclcpp::get_logger` instead.
+   */
+  explicit Logger(const std::string & name)
+  : name_(new std::string(name)) {}
+
+  std::shared_ptr<const std::string> name_;
+
+public:
+  RCLCPP_PUBLIC
+  Logger(const Logger &) = default;
+
+  /// Get the name of this logger.
+  /**
+   * \return the full name of the logger including any prefixes, or
+   * \return `nullptr` if this logger is invalid (e.g. because logging is
+   *   disabled).
+   */
+  RCLCPP_PUBLIC
+  const char *
+  get_name() const
+  {
+    if (!name_) {
+      return nullptr;
+    }
+    return name_->c_str();
+  }
+
+  /// Return a logger that is a descendant of this logger.
+  /**
+   * The child logger's full name will include any hierarchy conventions that
+   * indicate it is a descendant of this logger.
+   * For example, ```get_logger('abc').get_child('def')``` will return a logger
+   * with name `abc.def`.
+   *
+   * \param[in] suffix the child logger's suffix
+   * \return a logger with the fully-qualified name including the suffix, or
+   * \return a dummy logger if this logger is invalid (e.g. because logging is
+   *   disabled).
+   */
+  RCLCPP_PUBLIC
+  Logger
+  get_child(const std::string & suffix)
+  {
+    if (!name_) {
+      return Logger();
+    }
+    return Logger(*name_ + "." + suffix);
+  }
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__LOGGER_HPP_

rclcpp/include/rclcpp/macros.hpp

@@ -12,10 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_MACROS_HPP_
-#define RCLCPP_RCLCPP_MACROS_HPP_
+#include <memory>
+#include <utility>
 
-/* Disables the copy constructor and operator= for the given class.
+#ifndef RCLCPP__MACROS_HPP_
+#define RCLCPP__MACROS_HPP_
+
+/**
+ * Disables the copy constructor and operator= for the given class.
  *
  * Use in the private section of the class.
  */
@@ -23,31 +27,51 @@
   __VA_ARGS__(const __VA_ARGS__ &) = delete; \
   __VA_ARGS__ & operator=(const __VA_ARGS__ &) = delete;
 
-/* Defines aliases and static functions for using the Class with smart pointers.
+/**
+ * Defines aliases and static functions for using the Class with smart pointers.
  *
  * Use in the public section of the class.
- * Make sure to include <memory> in the header when using this.
+ * Make sure to include `<memory>` in the header when using this.
  */
 #define RCLCPP_SMART_PTR_DEFINITIONS(...) \
   RCLCPP_SHARED_PTR_DEFINITIONS(__VA_ARGS__) \
   RCLCPP_WEAK_PTR_DEFINITIONS(__VA_ARGS__) \
   RCLCPP_UNIQUE_PTR_DEFINITIONS(__VA_ARGS__)
 
-/* Defines aliases and static functions for using the Class with smart pointers.
+/**
+ * Defines aliases and static functions for using the Class with smart pointers.
  *
- * Same as RCLCPP_SMART_PTR_DEFINITIONS expect it excludes the static
+ * Same as RCLCPP_SMART_PTR_DEFINITIONS except it excludes the static
  * Class::make_unique() method definition which does not work on classes which
  * are not CopyConstructable.
  *
  * Use in the public section of the class.
- * Make sure to include <memory> in the header when using this.
+ * Make sure to include `<memory>` in the header when using this.
  */
 #define RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(...) \
   RCLCPP_SHARED_PTR_DEFINITIONS(__VA_ARGS__) \
   RCLCPP_WEAK_PTR_DEFINITIONS(__VA_ARGS__) \
   __RCLCPP_UNIQUE_PTR_ALIAS(__VA_ARGS__)
 
-#define __RCLCPP_SHARED_PTR_ALIAS(...) using SharedPtr = std::shared_ptr<__VA_ARGS__>;
+/**
+ * Defines aliases only for using the Class with smart pointers.
+ *
+ * Same as RCLCPP_SMART_PTR_DEFINITIONS except it excludes the static
+ * method definitions which do not work on pure virtual classes and classes
+ * which are not CopyConstructable.
+ *
+ * Use in the public section of the class.
+ * Make sure to include `<memory>` in the header when using this.
+ */
+#define RCLCPP_SMART_PTR_ALIASES_ONLY(...) \
+  __RCLCPP_SHARED_PTR_ALIAS(__VA_ARGS__) \
+  __RCLCPP_WEAK_PTR_ALIAS(__VA_ARGS__) \
+  __RCLCPP_UNIQUE_PTR_ALIAS(__VA_ARGS__) \
+  __RCLCPP_MAKE_SHARED_DEFINITION(__VA_ARGS__)
+
+#define __RCLCPP_SHARED_PTR_ALIAS(...) \
+  using SharedPtr = std::shared_ptr<__VA_ARGS__>; \
+  using ConstSharedPtr = std::shared_ptr<const __VA_ARGS__>;
 
 #define __RCLCPP_MAKE_SHARED_DEFINITION(...) \
   template<typename ... Args> \
@@ -62,7 +86,9 @@
   __RCLCPP_SHARED_PTR_ALIAS(__VA_ARGS__) \
   __RCLCPP_MAKE_SHARED_DEFINITION(__VA_ARGS__)
 
-#define __RCLCPP_WEAK_PTR_ALIAS(...) using WeakPtr = std::weak_ptr<__VA_ARGS__>;
+#define __RCLCPP_WEAK_PTR_ALIAS(...) \
+  using WeakPtr = std::weak_ptr<__VA_ARGS__>; \
+  using ConstWeakPtr = std::weak_ptr<const __VA_ARGS__>;
 
 /// Defines aliases and static functions for using the Class with weak_ptrs.
 #define RCLCPP_WEAK_PTR_DEFINITIONS(...) __RCLCPP_WEAK_PTR_ALIAS(__VA_ARGS__)
@@ -76,11 +102,13 @@
   { \
     return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward<Args>(args) ...)); \
   }
+
 /// Defines aliases and static functions for using the Class with unique_ptrs.
 #define RCLCPP_UNIQUE_PTR_DEFINITIONS(...) \
   __RCLCPP_UNIQUE_PTR_ALIAS(__VA_ARGS__) \
   __RCLCPP_MAKE_UNIQUE_DEFINITION(__VA_ARGS__)
 
-#define RCLCPP_INFO(Args) std::cout << Args << std::endl;
+#define RCLCPP_STRING_JOIN(arg1, arg2) RCLCPP_DO_STRING_JOIN(arg1, arg2)
+#define RCLCPP_DO_STRING_JOIN(arg1, arg2) arg1 ## arg2
 
-#endif /* RCLCPP_RCLCPP_MACROS_HPP_ */
+#endif  // RCLCPP__MACROS_HPP_

rclcpp/include/rclcpp/mapped_ring_buffer.hpp

@@ -1,232 +0,0 @@
-// Copyright 2015 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__MAPPED_RING_BUFFER_HPP_
-#define RCLCPP__MAPPED_RING_BUFFER_HPP_
-
-#include <algorithm>
-#include <cstddef>
-#include <cstdint>
-#include <memory>
-#include <vector>
-
-#include "rclcpp/allocator/allocator_common.hpp"
-#include "rclcpp/macros.hpp"
-
-namespace rclcpp
-{
-namespace mapped_ring_buffer
-{
-
-class MappedRingBufferBase
-{
-public:
-  RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBufferBase);
-};
-
-/// Ring buffer container of unique_ptr's of T, which can be accessed by a key.
-/* T must be a CopyConstructable and CopyAssignable.
- * This class can be used in a container by using the base class MappedRingBufferBase.
- * This class must have a positive, non-zero size.
- * This class cannot be resized nor can it reserve additional space after construction.
- * This class is not CopyConstructable nor CopyAssignable.
- *
- * The key's are not guaranteed to be unique because push_and_replace does not
- * check for colliding keys.
- * It is up to the user to only use unique keys.
- * A side effect of this is that when get_copy_at_key or pop_at_key are called,
- * they return the first encountered instance of the key.
- * But iteration does not begin with the ring buffer's head, and therefore
- * there is no guarantee on which value is returned if a key is used multiple
- * times.
- */
-template<typename T, typename Alloc = std::allocator<void>>
-class MappedRingBuffer : public MappedRingBufferBase
-{
-public:
-  RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBuffer<T, Alloc>);
-  using ElemAllocTraits = allocator::AllocRebind<T, Alloc>;
-  using ElemAlloc = typename ElemAllocTraits::allocator_type;
-  using ElemDeleter = allocator::Deleter<ElemAlloc, T>;
-
-  using ElemUniquePtr = std::unique_ptr<T, ElemDeleter>;
-
-  /// Constructor.
-  /* The constructor will allocate memory while reserving space.
-   *
-   * \param size size of the ring buffer; must be positive and non-zero.
-   */
-  explicit MappedRingBuffer(size_t size, std::shared_ptr<Alloc> allocator = nullptr)
-  : elements_(size), head_(0)
-  {
-    if (size == 0) {
-      throw std::invalid_argument("size must be a positive, non-zero value");
-    }
-    if (!allocator) {
-      allocator_ = std::make_shared<ElemAlloc>();
-    } else {
-      allocator_ = std::make_shared<ElemAlloc>(*allocator.get());
-    }
-  }
-
-  virtual ~MappedRingBuffer() {}
-
-  /// Return a copy of the value stored in the ring buffer at the given key.
-  /* The key is matched if an element in the ring buffer has a matching key.
-   * This method will allocate in order to return a copy.
-   *
-   * The key is not guaranteed to be unique, see the class docs for more.
-   *
-   * The contents of value before the method is called are discarded.
-   *
-   * \param key the key associated with the stored value
-   * \param value if the key is found, the value is stored in this parameter
-   */
-  void
-  get_copy_at_key(uint64_t key, ElemUniquePtr & value)
-  {
-    auto it = get_iterator_of_key(key);
-    value = nullptr;
-    if (it != elements_.end() && it->in_use) {
-      auto ptr = ElemAllocTraits::allocate(*allocator_.get(), 1);
-      ElemAllocTraits::construct(*allocator_.get(), ptr, *it->value);
-      value = ElemUniquePtr(ptr);
-    }
-  }
-
-  /// Return ownership of the value stored in the ring buffer, leaving a copy.
-  /* The key is matched if an element in the ring bufer has a matching key.
-   * This method will allocate in order to store a copy.
-   *
-   * The key is not guaranteed to be unique, see the class docs for more.
-   *
-   * The ownership of the currently stored object is returned, but a copy is
-   * made and stored in its place.
-   * This means that multiple calls to this function for a particular element
-   * will result in returning the copied and stored object not the original.
-   * This also means that later calls to pop_at_key will not return the
-   * originally stored object, since it was returned by the first call to this
-   * method.
-   *
-   * The contents of value before the method is called are discarded.
-   *
-   * \param key the key associated with the stored value
-   * \param value if the key is found, the value is stored in this parameter
-   */
-  void
-  get_ownership_at_key(uint64_t key, ElemUniquePtr & value)
-  {
-    auto it = get_iterator_of_key(key);
-    value = nullptr;
-    if (it != elements_.end() && it->in_use) {
-      // Make a copy.
-      auto ptr = ElemAllocTraits::allocate(*allocator_.get(), 1);
-      ElemAllocTraits::construct(*allocator_.get(), ptr, *it->value);
-      auto copy = ElemUniquePtr(ptr);
-      // Return the original.
-      value.swap(it->value);
-      // Store the copy.
-      it->value.swap(copy);
-    }
-  }
-
-  /// Return ownership of the value stored in the ring buffer at the given key.
-  /* The key is matched if an element in the ring buffer has a matching key.
-   *
-   * The key is not guaranteed to be unique, see the class docs for more.
-   *
-   * The contents of value before the method is called are discarded.
-   *
-   * \param key the key associated with the stored value
-   * \param value if the key is found, the value is stored in this parameter
-   */
-  void
-  pop_at_key(uint64_t key, ElemUniquePtr & value)
-  {
-    auto it = get_iterator_of_key(key);
-    value = nullptr;
-    if (it != elements_.end() && it->in_use) {
-      value.swap(it->value);
-      it->in_use = false;
-    }
-  }
-
-  /// Insert a key-value pair, displacing an existing pair if necessary.
-  /* The key's uniqueness is not checked on insertion.
-   * It is up to the user to ensure the key is unique.
-   * This method should not allocate memory.
-   *
-   * After insertion, if a pair was replaced, then value will contain ownership
-   * of that displaced value. Otherwise it will be a nullptr.
-   *
-   * \param key the key associated with the value to be stored
-   * \param value the value to store, and optionally the value displaced
-   */
-  bool
-  push_and_replace(uint64_t key, ElemUniquePtr & value)
-  {
-    bool did_replace = elements_[head_].in_use;
-    elements_[head_].key = key;
-    elements_[head_].value.swap(value);
-    elements_[head_].in_use = true;
-    head_ = (head_ + 1) % elements_.size();
-    return did_replace;
-  }
-
-  bool
-  push_and_replace(uint64_t key, ElemUniquePtr && value)
-  {
-    ElemUniquePtr temp = std::move(value);
-    return push_and_replace(key, temp);
-  }
-
-  /// Return true if the key is found in the ring buffer, otherwise false.
-  bool
-  has_key(uint64_t key)
-  {
-    return elements_.end() != get_iterator_of_key(key);
-  }
-
-private:
-  RCLCPP_DISABLE_COPY(MappedRingBuffer<T, Alloc>);
-
-  struct element
-  {
-    uint64_t key;
-    ElemUniquePtr value;
-    bool in_use;
-  };
-
-  using VectorAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<element>;
-
-  typename std::vector<element, VectorAlloc>::iterator
-  get_iterator_of_key(uint64_t key)
-  {
-    // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-    auto it = std::find_if(elements_.begin(), elements_.end(), [key](element & e) -> bool {
-      return e.key == key && e.in_use;
-    });
-    // *INDENT-ON*
-    return it;
-  }
-
-  std::vector<element, VectorAlloc> elements_;
-  size_t head_;
-  std::shared_ptr<ElemAlloc> allocator_;
-};
-
-}  // namespace mapped_ring_buffer
-}  // namespace rclcpp
-
-#endif  // RCLCPP__MAPPED_RING_BUFFER_HPP_

rclcpp/include/rclcpp/memory_strategies.hpp

@@ -15,20 +15,17 @@
 #ifndef RCLCPP__MEMORY_STRATEGIES_HPP_
 #define RCLCPP__MEMORY_STRATEGIES_HPP_
 
-#include <rclcpp/memory_strategy.hpp>
-#include <rclcpp/strategies/allocator_memory_strategy.hpp>
+#include "rclcpp/memory_strategy.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
 namespace memory_strategies
 {
 
-using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy;
-
-static memory_strategy::MemoryStrategy::SharedPtr create_default_strategy()
-{
-  return std::make_shared<AllocatorMemoryStrategy<>>();
-}
+RCLCPP_PUBLIC
+memory_strategy::MemoryStrategy::SharedPtr
+create_default_strategy();
 
 }  // namespace memory_strategies
 }  // namespace rclcpp

rclcpp/include/rclcpp/memory_strategy.hpp

@@ -15,12 +15,17 @@
 #ifndef RCLCPP__MEMORY_STRATEGY_HPP_
 #define RCLCPP__MEMORY_STRATEGY_HPP_
 
+#include <list>
 #include <memory>
-#include <vector>
+
+#include "rcl/allocator.h"
+#include "rcl/wait.h"
 
 #include "rclcpp/any_executable.hpp"
 #include "rclcpp/macros.hpp"
-#include "rclcpp/node.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
 
 namespace rclcpp
 {
@@ -33,223 +38,113 @@ namespace memory_strategy
  * the rmw implementation after the executor waits for work, based on the number of entities that
  * come through.
  */
-class MemoryStrategy
+class RCLCPP_PUBLIC MemoryStrategy
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy);
-  using WeakNodeVector = std::vector<std::weak_ptr<node::Node>>;
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy)
+  using WeakNodeList = std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>;
 
-  // return the new number of subscribers
-  virtual size_t fill_subscriber_handles(void ** & ptr) = 0;
+  virtual ~MemoryStrategy() = default;
 
-  // return the new number of services
-  virtual size_t fill_service_handles(void ** & ptr) = 0;
+  virtual bool collect_entities(const WeakNodeList & weak_nodes) = 0;
 
-  // return the new number of clients
-  virtual size_t fill_client_handles(void ** & ptr) = 0;
-
-  virtual void clear_active_entities() = 0;
+  virtual size_t number_of_ready_subscriptions() const = 0;
+  virtual size_t number_of_ready_services() const = 0;
+  virtual size_t number_of_ready_clients() const = 0;
+  virtual size_t number_of_ready_events() const = 0;
+  virtual size_t number_of_ready_timers() const = 0;
+  virtual size_t number_of_guard_conditions() const = 0;
+  virtual size_t number_of_waitables() const = 0;
 
+  virtual void add_waitable_handle(const rclcpp::Waitable::SharedPtr & waitable) = 0;
+  virtual bool add_handles_to_wait_set(rcl_wait_set_t * wait_set) = 0;
   virtual void clear_handles() = 0;
-  virtual void revalidate_handles() = 0;
-  virtual bool collect_entities(const WeakNodeVector & weak_nodes) = 0;
+  virtual void remove_null_handles(rcl_wait_set_t * wait_set) = 0;
 
-  /// Provide a newly initialized AnyExecutable object.
-  // \return Shared pointer to the fresh executable.
-  virtual executor::AnyExecutable::SharedPtr instantiate_next_executable() = 0;
+  virtual void add_guard_condition(const rcl_guard_condition_t * guard_condition) = 0;
+
+  virtual void remove_guard_condition(const rcl_guard_condition_t * guard_condition) = 0;
 
   virtual void
-  get_next_subscription(executor::AnyExecutable::SharedPtr any_exec,
-    const WeakNodeVector & weak_nodes) = 0;
+  get_next_subscription(
+    rclcpp::AnyExecutable & any_exec,
+    const WeakNodeList & weak_nodes) = 0;
 
   virtual void
-  get_next_service(executor::AnyExecutable::SharedPtr any_exec,
-    const WeakNodeVector & weak_nodes) = 0;
+  get_next_service(
+    rclcpp::AnyExecutable & any_exec,
+    const WeakNodeList & weak_nodes) = 0;
 
   virtual void
-  get_next_client(executor::AnyExecutable::SharedPtr any_exec,
-    const WeakNodeVector & weak_nodes) = 0;
+  get_next_client(
+    rclcpp::AnyExecutable & any_exec,
+    const WeakNodeList & weak_nodes) = 0;
 
-  static rclcpp::subscription::SubscriptionBase::SharedPtr
-  get_subscription_by_handle(void * subscriber_handle,
-    const WeakNodeVector & weak_nodes)
-  {
-    for (auto & weak_node : weak_nodes) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->get_callback_groups()) {
-        auto group = weak_group.lock();
-        if (!group) {
-          continue;
-        }
-        for (auto & weak_subscription : group->get_subscription_ptrs()) {
-          auto subscription = weak_subscription.lock();
-          if (subscription) {
-            if (subscription->get_subscription_handle()->data == subscriber_handle) {
-              return subscription;
-            }
-            if (subscription->get_intra_process_subscription_handle() &&
-              subscription->get_intra_process_subscription_handle()->data == subscriber_handle)
-            {
-              return subscription;
-            }
-          }
-        }
-      }
-    }
-    return nullptr;
-  }
+  virtual void
+  get_next_timer(
+    rclcpp::AnyExecutable & any_exec,
+    const WeakNodeList & weak_nodes) = 0;
 
-  static rclcpp::service::ServiceBase::SharedPtr
-  get_service_by_handle(void * service_handle, const WeakNodeVector & weak_nodes)
-  {
-    for (auto & weak_node : weak_nodes) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->get_callback_groups()) {
-        auto group = weak_group.lock();
-        if (!group) {
-          continue;
-        }
-        for (auto & service : group->get_service_ptrs()) {
-          if (service->get_service_handle()->data == service_handle) {
-            return service;
-          }
-        }
-      }
-    }
-    return nullptr;
-  }
+  virtual void
+  get_next_waitable(
+    rclcpp::AnyExecutable & any_exec,
+    const WeakNodeList & weak_nodes) = 0;
 
-  static rclcpp::client::ClientBase::SharedPtr
-  get_client_by_handle(void * client_handle, const WeakNodeVector & weak_nodes)
-  {
-    for (auto & weak_node : weak_nodes) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->get_callback_groups()) {
-        auto group = weak_group.lock();
-        if (!group) {
-          continue;
-        }
-        for (auto & client : group->get_client_ptrs()) {
-          if (client->get_client_handle()->data == client_handle) {
-            return client;
-          }
-        }
-      }
-    }
-    return nullptr;
-  }
+  virtual rcl_allocator_t
+  get_allocator() = 0;
 
-  static rclcpp::node::Node::SharedPtr
-  get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group,
-    const WeakNodeVector & weak_nodes)
-  {
-    if (!group) {
-      return nullptr;
-    }
-    for (auto & weak_node : weak_nodes) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->get_callback_groups()) {
-        auto callback_group = weak_group.lock();
-        if (callback_group == group) {
-          return node;
-        }
-      }
-    }
-    return nullptr;
-  }
+  static rclcpp::SubscriptionBase::SharedPtr
+  get_subscription_by_handle(
+    std::shared_ptr<const rcl_subscription_t> subscriber_handle,
+    const WeakNodeList & weak_nodes);
 
-  static rclcpp::callback_group::CallbackGroup::SharedPtr
+  static rclcpp::ServiceBase::SharedPtr
+  get_service_by_handle(
+    std::shared_ptr<const rcl_service_t> service_handle,
+    const WeakNodeList & weak_nodes);
+
+  static rclcpp::ClientBase::SharedPtr
+  get_client_by_handle(
+    std::shared_ptr<const rcl_client_t> client_handle,
+    const WeakNodeList & weak_nodes);
+
+  static rclcpp::TimerBase::SharedPtr
+  get_timer_by_handle(
+    std::shared_ptr<const rcl_timer_t> timer_handle,
+    const WeakNodeList & weak_nodes);
+
+  static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
+  get_node_by_group(
+    rclcpp::CallbackGroup::SharedPtr group,
+    const WeakNodeList & weak_nodes);
+
+  static rclcpp::CallbackGroup::SharedPtr
   get_group_by_subscription(
-    rclcpp::subscription::SubscriptionBase::SharedPtr subscription,
-    const WeakNodeVector & weak_nodes)
-  {
-    for (auto & weak_node : weak_nodes) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->get_callback_groups()) {
-        auto group = weak_group.lock();
-        if (!group) {
-          continue;
-        }
-        for (auto & weak_sub : group->get_subscription_ptrs()) {
-          auto sub = weak_sub.lock();
-          if (sub == subscription) {
-            return group;
-          }
-        }
-      }
-    }
-    return nullptr;
-  }
+    rclcpp::SubscriptionBase::SharedPtr subscription,
+    const WeakNodeList & weak_nodes);
 
-  static rclcpp::callback_group::CallbackGroup::SharedPtr
+  static rclcpp::CallbackGroup::SharedPtr
   get_group_by_service(
-    rclcpp::service::ServiceBase::SharedPtr service,
-    const WeakNodeVector & weak_nodes)
-  {
-    for (auto & weak_node : weak_nodes) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->get_callback_groups()) {
-        auto group = weak_group.lock();
-        if (!group) {
-          continue;
-        }
-        for (auto & serv : group->get_service_ptrs()) {
-          if (serv == service) {
-            return group;
-          }
-        }
-      }
-    }
-    return nullptr;
-  }
+    rclcpp::ServiceBase::SharedPtr service,
+    const WeakNodeList & weak_nodes);
 
-  static rclcpp::callback_group::CallbackGroup::SharedPtr
-  get_group_by_client(rclcpp::client::ClientBase::SharedPtr client,
-    const WeakNodeVector & weak_nodes)
-  {
-    for (auto & weak_node : weak_nodes) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->get_callback_groups()) {
-        auto group = weak_group.lock();
-        if (!group) {
-          continue;
-        }
-        for (auto & cli : group->get_client_ptrs()) {
-          if (cli == client) {
-            return group;
-          }
-        }
-      }
-    }
-    return nullptr;
-  }
+  static rclcpp::CallbackGroup::SharedPtr
+  get_group_by_client(
+    rclcpp::ClientBase::SharedPtr client,
+    const WeakNodeList & weak_nodes);
 
+  static rclcpp::CallbackGroup::SharedPtr
+  get_group_by_timer(
+    rclcpp::TimerBase::SharedPtr timer,
+    const WeakNodeList & weak_nodes);
 
+  static rclcpp::CallbackGroup::SharedPtr
+  get_group_by_waitable(
+    rclcpp::Waitable::SharedPtr waitable,
+    const WeakNodeList & weak_nodes);
 };
 
 }  // namespace memory_strategy
-
 }  // namespace rclcpp
 
 #endif  // RCLCPP__MEMORY_STRATEGY_HPP_

rclcpp/include/rclcpp/message_info.hpp

@@ -0,0 +1,52 @@
+// 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.
+
+#ifndef RCLCPP__MESSAGE_INFO_HPP_
+#define RCLCPP__MESSAGE_INFO_HPP_
+
+#include "rmw/types.h"
+
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Additional meta data about messages taken from subscriptions.
+class RCLCPP_PUBLIC MessageInfo
+{
+public:
+  /// Default empty constructor.
+  MessageInfo() = default;
+
+  /// Conversion constructor, which is intentionally not marked as explicit.
+  // cppcheck-suppress noExplicitConstructor
+  MessageInfo(const rmw_message_info_t & rmw_message_info);  // NOLINT(runtime/explicit)
+
+  virtual ~MessageInfo();
+
+  /// Return the message info as the underlying rmw message info type.
+  const rmw_message_info_t &
+  get_rmw_message_info() const;
+
+  /// Return the message info as the underlying rmw message info type.
+  rmw_message_info_t &
+  get_rmw_message_info();
+
+private:
+  rmw_message_info_t rmw_message_info_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__MESSAGE_INFO_HPP_

rclcpp/include/rclcpp/message_memory_strategy.hpp

@@ -16,9 +16,19 @@
 #define RCLCPP__MESSAGE_MEMORY_STRATEGY_HPP_
 
 #include <memory>
+#include <stdexcept>
+
+#include "rcl/types.h"
 
 #include "rclcpp/allocator/allocator_common.hpp"
+#include "rclcpp/exceptions.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/serialized_message.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+#include "rcutils/logging_macros.h"
+
+#include "rmw/serialized_message.h"
 
 namespace rclcpp
 {
@@ -26,27 +36,44 @@ namespace message_memory_strategy
 {
 
 /// Default allocation strategy for messages received by subscriptions.
-// A message memory strategy must be templated on the type of the subscription it belongs to.
+/** A message memory strategy must be templated on the type of the subscription it belongs to. */
 template<typename MessageT, typename Alloc = std::allocator<void>>
 class MessageMemoryStrategy
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(MessageMemoryStrategy);
+  RCLCPP_SMART_PTR_DEFINITIONS(MessageMemoryStrategy)
 
   using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
   using MessageAlloc = typename MessageAllocTraits::allocator_type;
   using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
 
+  using SerializedMessageAllocTraits = allocator::AllocRebind<rclcpp::SerializedMessage, Alloc>;
+  using SerializedMessageAlloc = typename SerializedMessageAllocTraits::allocator_type;
+  using SerializedMessageDeleter =
+    allocator::Deleter<SerializedMessageAlloc, rclcpp::SerializedMessage>;
+
+  using BufferAllocTraits = allocator::AllocRebind<char, Alloc>;
+  using BufferAlloc = typename BufferAllocTraits::allocator_type;
+  using BufferDeleter = allocator::Deleter<BufferAlloc, char>;
+
   MessageMemoryStrategy()
   {
     message_allocator_ = std::make_shared<MessageAlloc>();
+    serialized_message_allocator_ = std::make_shared<SerializedMessageAlloc>();
+    buffer_allocator_ = std::make_shared<BufferAlloc>();
+    rcutils_allocator_ = allocator::get_rcl_allocator<char, BufferAlloc>(*buffer_allocator_.get());
   }
 
   explicit MessageMemoryStrategy(std::shared_ptr<Alloc> allocator)
   {
     message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
+    serialized_message_allocator_ = std::make_shared<SerializedMessageAlloc>(*allocator.get());
+    buffer_allocator_ = std::make_shared<BufferAlloc>(*allocator.get());
+    rcutils_allocator_ = allocator::get_rcl_allocator<char, BufferAlloc>(*buffer_allocator_.get());
   }
 
+  virtual ~MessageMemoryStrategy() = default;
+
   /// Default factory method
   static SharedPtr create_default()
   {
@@ -54,21 +81,51 @@ public:
   }
 
   /// By default, dynamically allocate a new message.
-  // \return Shared pointer to the new message.
+  /** \return Shared pointer to the new message. */
   virtual std::shared_ptr<MessageT> borrow_message()
   {
     return std::allocate_shared<MessageT, MessageAlloc>(*message_allocator_.get());
   }
 
+  virtual std::shared_ptr<rclcpp::SerializedMessage> borrow_serialized_message(size_t capacity)
+  {
+    return std::make_shared<rclcpp::SerializedMessage>(capacity);
+  }
+
+  virtual std::shared_ptr<rclcpp::SerializedMessage> borrow_serialized_message()
+  {
+    return borrow_serialized_message(default_buffer_capacity_);
+  }
+
+  virtual void set_default_buffer_capacity(size_t capacity)
+  {
+    default_buffer_capacity_ = capacity;
+  }
+
   /// Release ownership of the message, which will deallocate it if it has no more owners.
-  // \param[in] Shared pointer to the message we are returning.
+  /** \param[in] msg Shared pointer to the message we are returning. */
   virtual void return_message(std::shared_ptr<MessageT> & msg)
   {
     msg.reset();
   }
 
+  virtual void return_serialized_message(
+    std::shared_ptr<rclcpp::SerializedMessage> & serialized_msg)
+  {
+    serialized_msg.reset();
+  }
+
   std::shared_ptr<MessageAlloc> message_allocator_;
   MessageDeleter message_deleter_;
+
+  std::shared_ptr<SerializedMessageAlloc> serialized_message_allocator_;
+  SerializedMessageDeleter serialized_message_deleter_;
+
+  std::shared_ptr<BufferAlloc> buffer_allocator_;
+  BufferDeleter buffer_deleter_;
+  size_t default_buffer_capacity_ = 0;
+
+  rcutils_allocator_t rcutils_allocator_;
 };
 
 }  // namespace message_memory_strategy

rclcpp/include/rclcpp/node_impl.hpp

@@ -19,6 +19,7 @@
 #include <rmw/rmw.h>
 
 #include <algorithm>
+#include <chrono>
 #include <cstdlib>
 #include <iostream>
 #include <limits>
@@ -30,647 +31,248 @@
 #include <utility>
 #include <vector>
 
-#include "rcl_interfaces/msg/intra_process_message.hpp"
-#include "rosidl_generator_cpp/message_type_support.hpp"
-#include "rosidl_generator_cpp/service_type_support.hpp"
+#include "rcl/publisher.h"
+#include "rcl/subscription.h"
 
 #include "rclcpp/contexts/default_context.hpp"
-#include "rclcpp/intra_process_manager.hpp"
+#include "rclcpp/create_client.hpp"
+#include "rclcpp/create_publisher.hpp"
+#include "rclcpp/create_service.hpp"
+#include "rclcpp/create_timer.hpp"
+#include "rclcpp/create_subscription.hpp"
+#include "rclcpp/detail/resolve_enable_topic_statistics.hpp"
 #include "rclcpp/parameter.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/type_support_decl.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 #ifndef RCLCPP__NODE_HPP_
 #include "node.hpp"
 #endif
 
-using namespace rclcpp;
-using namespace node;
-
-Node::Node(const std::string & node_name, bool use_intra_process_comms)
-: Node(
-    node_name,
-    rclcpp::contexts::default_context::get_global_default_context(),
-    use_intra_process_comms)
-{}
-
-Node::Node(
-  const std::string & node_name,
-  context::Context::SharedPtr context,
-  bool use_intra_process_comms)
-: name_(node_name), context_(context),
-  number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0),
-  use_intra_process_comms_(use_intra_process_comms)
+namespace rclcpp
 {
-  size_t domain_id = 0;
-  char * ros_domain_id = nullptr;
-  const char * env_var = "ROS_DOMAIN_ID";
-#ifndef _WIN32
-  ros_domain_id = getenv(env_var);
-#else
-  size_t ros_domain_id_size;
-  _dupenv_s(&ros_domain_id, &ros_domain_id_size, env_var);
-#endif
-  if (ros_domain_id) {
-    uint32_t number = strtoul(ros_domain_id, NULL, 0);
-    if (number == (std::numeric_limits<uint32_t>::max)()) {
-      throw std::runtime_error("failed to interpret ROS_DOMAIN_ID as integral number");
-    }
-    domain_id = static_cast<size_t>(number);
-#ifdef _WIN32
-    free(ros_domain_id);
-#endif
-  }
 
-  auto node = rmw_create_node(name_.c_str(), domain_id);
-  if (!node) {
-    // *INDENT-OFF*
-    throw std::runtime_error(
-      std::string("could not create node: ") +
-      rmw_get_error_string_safe());
-    // *INDENT-ON*
+RCLCPP_LOCAL
+inline
+std::string
+extend_name_with_sub_namespace(const std::string & name, const std::string & sub_namespace)
+{
+  std::string name_with_sub_namespace(name);
+  if (sub_namespace != "" && name.front() != '/' && name.front() != '~') {
+    name_with_sub_namespace = sub_namespace + "/" + name;
   }
-  // Initialize node handle shared_ptr with custom deleter.
-  // *INDENT-OFF*
-  node_handle_.reset(node, [](rmw_node_t * node) {
-    auto ret = rmw_destroy_node(node);
-    if (ret != RMW_RET_OK) {
-      fprintf(
-        stderr, "Error in destruction of rmw node handle: %s\n", rmw_get_error_string_safe());
-    }
-  });
-  // *INDENT-ON*
-
-  using rclcpp::callback_group::CallbackGroupType;
-  default_callback_group_ = create_callback_group(
-    CallbackGroupType::MutuallyExclusive);
-  events_publisher_ = create_publisher<rcl_interfaces::msg::ParameterEvent>(
-    "parameter_events", rmw_qos_profile_parameter_events);
+  return name_with_sub_namespace;
 }
 
-rclcpp::callback_group::CallbackGroup::SharedPtr
-Node::create_callback_group(
-  rclcpp::callback_group::CallbackGroupType group_type)
-{
-  using rclcpp::callback_group::CallbackGroup;
-  using rclcpp::callback_group::CallbackGroupType;
-  auto group = CallbackGroup::SharedPtr(new CallbackGroup(group_type));
-  callback_groups_.push_back(group);
-  return group;
-}
-
-template<typename MessageT, typename Alloc>
-typename rclcpp::publisher::Publisher<MessageT, Alloc>::SharedPtr
+template<typename MessageT, typename AllocatorT, typename PublisherT>
+std::shared_ptr<PublisherT>
 Node::create_publisher(
-  const std::string & topic_name, size_t qos_history_depth,
-  std::shared_ptr<Alloc> allocator)
-{
-  if (!allocator) {
-    allocator = std::make_shared<Alloc>();
-  }
-  rmw_qos_profile_t qos = rmw_qos_profile_default;
-  qos.depth = qos_history_depth;
-  return this->create_publisher<MessageT, Alloc>(topic_name, qos, allocator);
-}
-
-template<typename MessageT, typename Alloc>
-typename publisher::Publisher<MessageT, Alloc>::SharedPtr
-Node::create_publisher(
-  const std::string & topic_name, const rmw_qos_profile_t & qos_profile,
-  std::shared_ptr<Alloc> allocator)
-{
-  if (!allocator) {
-    allocator = std::make_shared<Alloc>();
-  }
-  using rosidl_generator_cpp::get_message_type_support_handle;
-  auto type_support_handle = get_message_type_support_handle<MessageT>();
-  rmw_publisher_t * publisher_handle = rmw_create_publisher(
-    node_handle_.get(), type_support_handle, topic_name.c_str(), qos_profile);
-  if (!publisher_handle) {
-    // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-    throw std::runtime_error(
-      std::string("could not create publisher: ") +
-      rmw_get_error_string_safe());
-    // *INDENT-ON*
-  }
-
-  auto publisher = publisher::Publisher<MessageT, Alloc>::make_shared(
-    node_handle_, publisher_handle, topic_name, qos_profile.depth, allocator);
-
-  if (use_intra_process_comms_) {
-    rmw_publisher_t * intra_process_publisher_handle = rmw_create_publisher(
-      node_handle_.get(), ipm_ts_, (topic_name + "__intra").c_str(), qos_profile);
-    if (!intra_process_publisher_handle) {
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-      throw std::runtime_error(
-        std::string("could not create intra process publisher: ") +
-        rmw_get_error_string_safe());
-      // *INDENT-ON*
-    }
-
-    auto intra_process_manager =
-      context_->get_sub_context<rclcpp::intra_process_manager::IntraProcessManager>();
-    uint64_t intra_process_publisher_id =
-      intra_process_manager->add_publisher<MessageT, Alloc>(publisher);
-    rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = intra_process_manager;
-    // *INDENT-OFF*
-    auto shared_publish_callback =
-      [weak_ipm](uint64_t publisher_id, void * msg, const std::type_info & type_info) -> uint64_t
-    {
-      auto ipm = weak_ipm.lock();
-      if (!ipm) {
-        // TODO(wjwwood): should this just return silently? Or maybe return with a logged warning?
-        throw std::runtime_error(
-          "intra process publish called after destruction of intra process manager");
-      }
-      if (!msg) {
-        throw std::runtime_error("cannot publisher msg which is a null pointer");
-      }
-      auto & message_type_info = typeid(MessageT);
-      if (message_type_info != type_info) {
-        throw std::runtime_error(
-          std::string("published type '") + type_info.name() +
-          "' is incompatible from the publisher type '" + message_type_info.name() + "'");
-      }
-      MessageT * typed_message_ptr = static_cast<MessageT *>(msg);
-      using MessageDeleter = typename publisher::Publisher<MessageT, Alloc>::MessageDeleter;
-      std::unique_ptr<MessageT, MessageDeleter> unique_msg(typed_message_ptr);
-      uint64_t message_seq =
-        ipm->store_intra_process_message<MessageT, Alloc>(publisher_id, unique_msg);
-      return message_seq;
-    };
-    // *INDENT-ON*
-    publisher->setup_intra_process(
-      intra_process_publisher_id,
-      shared_publish_callback,
-      intra_process_publisher_handle);
-  }
-  return publisher;
-}
-
-bool
-Node::group_in_node(callback_group::CallbackGroup::SharedPtr group)
-{
-  bool group_belongs_to_this_node = false;
-  for (auto & weak_group : this->callback_groups_) {
-    auto cur_group = weak_group.lock();
-    if (cur_group && (cur_group == group)) {
-      group_belongs_to_this_node = true;
-    }
-  }
-  return group_belongs_to_this_node;
-}
-
-template<typename MessageT, typename CallbackT, typename Alloc>
-typename rclcpp::subscription::Subscription<MessageT, Alloc>::SharedPtr
-Node::create_subscription(
   const std::string & topic_name,
-  CallbackT callback,
-  const rmw_qos_profile_t & qos_profile,
-  rclcpp::callback_group::CallbackGroup::SharedPtr group,
-  bool ignore_local_publications,
-  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
-  msg_mem_strat,
-  typename std::shared_ptr<Alloc> allocator)
+  const rclcpp::QoS & qos,
+  const PublisherOptionsWithAllocator<AllocatorT> & options)
 {
-  if (!allocator) {
-    allocator = std::make_shared<Alloc>();
-  }
-
-  rclcpp::subscription::AnySubscriptionCallback<MessageT,
-  Alloc> any_subscription_callback(allocator);
-  any_subscription_callback.set(callback);
-
-  using rosidl_generator_cpp::get_message_type_support_handle;
-
-  if (!msg_mem_strat) {
-    msg_mem_strat =
-      rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::create_default();
-  }
-
-  auto type_support_handle = get_message_type_support_handle<MessageT>();
-  rmw_subscription_t * subscriber_handle = rmw_create_subscription(
-    node_handle_.get(), type_support_handle,
-    topic_name.c_str(), qos_profile, ignore_local_publications);
-  if (!subscriber_handle) {
-    // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-    throw std::runtime_error(
-      std::string("could not create subscription: ") + rmw_get_error_string_safe());
-    // *INDENT-ON*
-  }
-
-  using namespace rclcpp::subscription;
-
-  auto sub = Subscription<MessageT, Alloc>::make_shared(
-    node_handle_,
-    subscriber_handle,
-    topic_name,
-    ignore_local_publications,
-    any_subscription_callback,
-    msg_mem_strat);
-  auto sub_base_ptr = std::dynamic_pointer_cast<SubscriptionBase>(sub);
-  // Setup intra process.
-  if (use_intra_process_comms_) {
-    rmw_subscription_t * intra_process_subscriber_handle = rmw_create_subscription(
-      node_handle_.get(), ipm_ts_,
-      (topic_name + "__intra").c_str(), qos_profile, false);
-    if (!subscriber_handle) {
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-      throw std::runtime_error(
-        std::string("could not create intra process subscription: ") + rmw_get_error_string_safe());
-      // *INDENT-ON*
-    }
-    auto intra_process_manager =
-      context_->get_sub_context<rclcpp::intra_process_manager::IntraProcessManager>();
-    rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = intra_process_manager;
-    uint64_t intra_process_subscription_id =
-      intra_process_manager->add_subscription(sub_base_ptr);
-    // *INDENT-OFF*
-    sub->setup_intra_process(
-      intra_process_subscription_id,
-      intra_process_subscriber_handle,
-      [weak_ipm](
-        uint64_t publisher_id,
-        uint64_t message_sequence,
-        uint64_t subscription_id,
-        typename Subscription<MessageT, Alloc>::MessageUniquePtr & message)
-      {
-        auto ipm = weak_ipm.lock();
-        if (!ipm) {
-          // TODO(wjwwood): should this just return silently? Or maybe return with a logged warning?
-          throw std::runtime_error(
-            "intra process take called after destruction of intra process manager");
-        }
-        ipm->take_intra_process_message<MessageT, Alloc>(
-          publisher_id, message_sequence, subscription_id, message);
-      },
-      [weak_ipm](const rmw_gid_t * sender_gid) -> bool {
-        auto ipm = weak_ipm.lock();
-        if (!ipm) {
-          throw std::runtime_error(
-            "intra process publisher check called after destruction of intra process manager");
-        }
-        return ipm->matches_any_publishers(sender_gid);
-      }
-    );
-    // *INDENT-ON*
-  }
-  // Assign to a group.
-  if (group) {
-    if (!group_in_node(group)) {
-      // TODO(jacquelinekay): use custom exception
-      throw std::runtime_error("Cannot create subscription, group not in node.");
-    }
-    group->add_subscription(sub_base_ptr);
-  } else {
-    default_callback_group_->add_subscription(sub_base_ptr);
-  }
-  number_of_subscriptions_++;
-  return sub;
-}
-
-template<typename MessageT, typename CallbackT, typename Alloc>
-typename rclcpp::subscription::Subscription<MessageT, Alloc>::SharedPtr
-Node::create_subscription(
-  const std::string & topic_name,
-  size_t qos_history_depth,
-  CallbackT callback,
-  rclcpp::callback_group::CallbackGroup::SharedPtr group,
-  bool ignore_local_publications,
-  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
-  msg_mem_strat,
-  std::shared_ptr<Alloc> allocator)
-{
-  rmw_qos_profile_t qos = rmw_qos_profile_default;
-  qos.depth = qos_history_depth;
-  return this->create_subscription<MessageT, CallbackT, Alloc>(
-    topic_name,
-    callback,
+  return rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>(
+    *this,
+    extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()),
     qos,
-    group,
-    ignore_local_publications,
-    msg_mem_strat,
-    allocator);
+    options);
 }
 
-rclcpp::timer::WallTimer::SharedPtr
-Node::create_wall_timer(
-  std::chrono::nanoseconds period,
-  rclcpp::timer::CallbackType callback,
-  rclcpp::callback_group::CallbackGroup::SharedPtr group)
+template<
+  typename MessageT,
+  typename CallbackT,
+  typename AllocatorT,
+  typename CallbackMessageT,
+  typename SubscriptionT,
+  typename MessageMemoryStrategyT>
+std::shared_ptr<SubscriptionT>
+Node::create_subscription(
+  const std::string & topic_name,
+  const rclcpp::QoS & qos,
+  CallbackT && callback,
+  const SubscriptionOptionsWithAllocator<AllocatorT> & options,
+  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat)
 {
-  auto timer = rclcpp::timer::WallTimer::make_shared(period, callback);
-  if (group) {
-    if (!group_in_node(group)) {
-      // TODO(jacquelinekay): use custom exception
-      throw std::runtime_error("Cannot create timer, group not in node.");
-    }
-    group->add_timer(timer);
-  } else {
-    default_callback_group_->add_timer(timer);
-  }
-  number_of_timers_++;
-  return timer;
+  return rclcpp::create_subscription<MessageT>(
+    *this,
+    extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()),
+    qos,
+    std::forward<CallbackT>(callback),
+    options,
+    msg_mem_strat);
 }
 
-// TODO(wjwwood): reenable this once I figure out why the demo doesn't build with it.
-// rclcpp::timer::WallTimer::SharedPtr
-// Node::create_wall_timer(
-//   std::chrono::duration<long double, std::nano> period,
-//   rclcpp::timer::CallbackType callback,
-//   rclcpp::callback_group::CallbackGroup::SharedPtr group)
-// {
-//   return create_wall_timer(
-//     std::chrono::duration_cast<std::chrono::nanoseconds>(period),
-//     callback,
-//     group);
-// }
+template<typename DurationRepT, typename DurationT, typename CallbackT>
+typename rclcpp::WallTimer<CallbackT>::SharedPtr
+Node::create_wall_timer(
+  std::chrono::duration<DurationRepT, DurationT> period,
+  CallbackT callback,
+  rclcpp::CallbackGroup::SharedPtr group)
+{
+  return rclcpp::create_wall_timer(
+    period,
+    std::move(callback),
+    group,
+    this->node_base_.get(),
+    this->node_timers_.get());
+}
 
 template<typename ServiceT>
-typename client::Client<ServiceT>::SharedPtr
+typename Client<ServiceT>::SharedPtr
 Node::create_client(
   const std::string & service_name,
-  rclcpp::callback_group::CallbackGroup::SharedPtr group)
+  const rmw_qos_profile_t & qos_profile,
+  rclcpp::CallbackGroup::SharedPtr group)
 {
-  using rosidl_generator_cpp::get_service_type_support_handle;
-  auto service_type_support_handle =
-    get_service_type_support_handle<ServiceT>();
-
-  rmw_client_t * client_handle = rmw_create_client(
-    this->node_handle_.get(), service_type_support_handle, service_name.c_str());
-  if (!client_handle) {
-    // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-    throw std::runtime_error(
-      std::string("could not create client: ") +
-      rmw_get_error_string_safe());
-    // *INDENT-ON*
-  }
-
-  using namespace rclcpp::client;
-
-  auto cli = Client<ServiceT>::make_shared(
-    node_handle_,
-    client_handle,
-    service_name);
-
-  auto cli_base_ptr = std::dynamic_pointer_cast<ClientBase>(cli);
-  if (group) {
-    if (!group_in_node(group)) {
-      // TODO(esteve): use custom exception
-      throw std::runtime_error("Cannot create client, group not in node.");
-    }
-    group->add_client(cli_base_ptr);
-  } else {
-    default_callback_group_->add_client(cli_base_ptr);
-  }
-  number_of_clients_++;
-
-  return cli;
+  return rclcpp::create_client<ServiceT>(
+    node_base_,
+    node_graph_,
+    node_services_,
+    extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
+    qos_profile,
+    group);
 }
 
-template<typename ServiceT, typename FunctorT>
-typename rclcpp::service::Service<ServiceT>::SharedPtr
+template<typename ServiceT, typename CallbackT>
+typename rclcpp::Service<ServiceT>::SharedPtr
 Node::create_service(
   const std::string & service_name,
-  FunctorT callback,
-  rclcpp::callback_group::CallbackGroup::SharedPtr group)
+  CallbackT && callback,
+  const rmw_qos_profile_t & qos_profile,
+  rclcpp::CallbackGroup::SharedPtr group)
 {
-  using rosidl_generator_cpp::get_service_type_support_handle;
-  auto service_type_support_handle =
-    get_service_type_support_handle<ServiceT>();
-
-  rclcpp::service::AnyServiceCallback<ServiceT> any_service_callback;
-  any_service_callback.set(callback);
-
-  rmw_service_t * service_handle = rmw_create_service(
-    node_handle_.get(), service_type_support_handle, service_name.c_str());
-  if (!service_handle) {
-    // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-    throw std::runtime_error(
-      std::string("could not create service: ") +
-      rmw_get_error_string_safe());
-    // *INDENT-ON*
-  }
-
-  auto serv = service::Service<ServiceT>::make_shared(
-    node_handle_, service_handle, service_name, any_service_callback);
-  auto serv_base_ptr = std::dynamic_pointer_cast<service::ServiceBase>(serv);
-  if (group) {
-    if (!group_in_node(group)) {
-      // TODO(jacquelinekay): use custom exception
-      throw std::runtime_error("Cannot create service, group not in node.");
-    }
-    group->add_service(serv_base_ptr);
-  } else {
-    default_callback_group_->add_service(serv_base_ptr);
-  }
-  number_of_services_++;
-  return serv;
+  return rclcpp::create_service<ServiceT, CallbackT>(
+    node_base_,
+    node_services_,
+    extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
+    std::forward<CallbackT>(callback),
+    qos_profile,
+    group);
 }
 
-std::vector<rcl_interfaces::msg::SetParametersResult>
-Node::set_parameters(
-  const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
+template<typename ParameterT>
+auto
+Node::declare_parameter(
+  const std::string & name,
+  const ParameterT & default_value,
+  const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor,
+  bool ignore_override)
 {
-  std::vector<rcl_interfaces::msg::SetParametersResult> results;
-  for (auto p : parameters) {
-    auto result = set_parameters_atomically({{p}});
-    results.push_back(result);
+  try {
+    return this->declare_parameter(
+      name,
+      rclcpp::ParameterValue(default_value),
+      parameter_descriptor,
+      ignore_override
+    ).get<ParameterT>();
+  } catch (const ParameterTypeException & ex) {
+    throw exceptions::InvalidParameterTypeException(name, ex.what());
   }
-  return results;
 }
 
-rcl_interfaces::msg::SetParametersResult
-Node::set_parameters_atomically(
-  const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
+template<typename ParameterT>
+std::vector<ParameterT>
+Node::declare_parameters(
+  const std::string & namespace_,
+  const std::map<std::string, ParameterT> & parameters,
+  bool ignore_overrides)
 {
-  std::lock_guard<std::mutex> lock(mutex_);
-  std::map<std::string, rclcpp::parameter::ParameterVariant> tmp_map;
-  auto parameter_event = std::make_shared<rcl_interfaces::msg::ParameterEvent>();
-
-  for (auto p : parameters) {
-    if (parameters_.find(p.get_name()) == parameters_.end()) {
-      if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) {
-        parameter_event->new_parameters.push_back(p.to_parameter());
-      }
-    } else if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) {
-      parameter_event->changed_parameters.push_back(p.to_parameter());
-    } else {
-      parameter_event->deleted_parameters.push_back(p.to_parameter());
+  std::vector<ParameterT> result;
+  std::string normalized_namespace = namespace_.empty() ? "" : (namespace_ + ".");
+  std::transform(
+    parameters.begin(), parameters.end(), std::back_inserter(result),
+    [this, &normalized_namespace, ignore_overrides](auto element) {
+      return this->declare_parameter(
+        normalized_namespace + element.first,
+        element.second,
+        rcl_interfaces::msg::ParameterDescriptor(),
+        ignore_overrides);
     }
-    tmp_map[p.get_name()] = p;
+  );
+  return result;
+}
+
+template<typename ParameterT>
+std::vector<ParameterT>
+Node::declare_parameters(
+  const std::string & namespace_,
+  const std::map<
+    std::string,
+    std::pair<ParameterT, rcl_interfaces::msg::ParameterDescriptor>
+  > & parameters,
+  bool ignore_overrides)
+{
+  std::vector<ParameterT> result;
+  std::string normalized_namespace = namespace_.empty() ? "" : (namespace_ + ".");
+  std::transform(
+    parameters.begin(), parameters.end(), std::back_inserter(result),
+    [this, &normalized_namespace, ignore_overrides](auto element) {
+      return static_cast<ParameterT>(
+        this->declare_parameter(
+          normalized_namespace + element.first,
+          element.second.first,
+          element.second.second,
+          ignore_overrides)
+      );
+    }
+  );
+  return result;
+}
+
+template<typename ParameterT>
+bool
+Node::get_parameter(const std::string & name, ParameterT & parameter) const
+{
+  std::string sub_name = extend_name_with_sub_namespace(name, this->get_sub_namespace());
+
+  rclcpp::Parameter parameter_variant;
+
+  bool result = get_parameter(sub_name, parameter_variant);
+  if (result) {
+    parameter = static_cast<ParameterT>(parameter_variant.get_value<ParameterT>());
   }
-  // std::map::insert will not overwrite elements, so we'll keep the new
-  // ones and add only those that already exist in the Node's internal map
-  tmp_map.insert(parameters_.begin(), parameters_.end());
-  std::swap(tmp_map, parameters_);
-
-  // TODO(jacquelinekay): handle parameter constraints
-  rcl_interfaces::msg::SetParametersResult result;
-  result.successful = true;
-
-  events_publisher_->publish(parameter_event);
 
   return result;
 }
 
-std::vector<rclcpp::parameter::ParameterVariant>
+template<typename ParameterT>
+bool
+Node::get_parameter_or(
+  const std::string & name,
+  ParameterT & parameter,
+  const ParameterT & alternative_value) const
+{
+  std::string sub_name = extend_name_with_sub_namespace(name, this->get_sub_namespace());
+
+  bool got_parameter = get_parameter(sub_name, parameter);
+  if (!got_parameter) {
+    parameter = alternative_value;
+  }
+  return got_parameter;
+}
+
+// this is a partially-specialized version of get_parameter above,
+// where our concrete type for ParameterT is std::map, but the to-be-determined
+// type is the value in the map.
+template<typename ParameterT>
+bool
 Node::get_parameters(
-  const std::vector<std::string> & names) const
+  const std::string & prefix,
+  std::map<std::string, ParameterT> & values) const
 {
-  std::lock_guard<std::mutex> lock(mutex_);
-  std::vector<rclcpp::parameter::ParameterVariant> results;
-
-  for (auto & name : names) {
-    if (std::any_of(parameters_.cbegin(), parameters_.cend(),
-      [&name](const std::pair<std::string, rclcpp::parameter::ParameterVariant> & kv) {
-      return name == kv.first;
-    }))
-    {
-      results.push_back(parameters_.at(name));
+  std::map<std::string, rclcpp::Parameter> params;
+  bool result = node_parameters_->get_parameters_by_prefix(prefix, params);
+  if (result) {
+    for (const auto & param : params) {
+      values[param.first] = static_cast<ParameterT>(param.second.get_value<ParameterT>());
     }
   }
-  return results;
-}
 
-std::vector<rcl_interfaces::msg::ParameterDescriptor>
-Node::describe_parameters(
-  const std::vector<std::string> & names) const
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  std::vector<rcl_interfaces::msg::ParameterDescriptor> results;
-  for (auto & kv : parameters_) {
-    if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) {
-      return name == kv.first;
-    }))
-    {
-      rcl_interfaces::msg::ParameterDescriptor parameter_descriptor;
-      parameter_descriptor.name = kv.first;
-      parameter_descriptor.type = kv.second.get_type();
-      results.push_back(parameter_descriptor);
-    }
-  }
-  return results;
-}
-
-std::vector<uint8_t>
-Node::get_parameter_types(
-  const std::vector<std::string> & names) const
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  std::vector<uint8_t> results;
-  for (auto & kv : parameters_) {
-    if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) {
-      return name == kv.first;
-    }))
-    {
-      results.push_back(kv.second.get_type());
-    } else {
-      results.push_back(rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET);
-    }
-  }
-  return results;
-}
-
-rcl_interfaces::msg::ListParametersResult
-Node::list_parameters(
-  const std::vector<std::string> & prefixes, uint64_t depth) const
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  rcl_interfaces::msg::ListParametersResult result;
-
-  // TODO(esteve): define parameter separator, use "." for now
-  for (auto & kv : parameters_) {
-    if (std::any_of(prefixes.cbegin(), prefixes.cend(), [&kv, &depth](const std::string & prefix) {
-      if (kv.first == prefix) {
-        return true;
-      } else if (kv.first.find(prefix + ".") == 0) {
-        size_t length = prefix.length();
-        std::string substr = kv.first.substr(length);
-        // Cast as unsigned integer to avoid warning
-        return static_cast<uint64_t>(std::count(substr.begin(), substr.end(), '.')) < depth;
-      }
-      return false;
-    }))
-    {
-      result.names.push_back(kv.first);
-      size_t last_separator = kv.first.find_last_of('.');
-      if (std::string::npos != last_separator) {
-        std::string prefix = kv.first.substr(0, last_separator);
-        if (std::find(result.prefixes.cbegin(), result.prefixes.cend(), prefix) ==
-          result.prefixes.cend())
-        {
-          result.prefixes.push_back(prefix);
-        }
-      }
-    }
-  }
   return result;
 }
 
-std::map<std::string, std::string>
-Node::get_topic_names_and_types() const
-{
-  rmw_topic_names_and_types_t topic_names_and_types;
-  topic_names_and_types.topic_count = 0;
-  topic_names_and_types.topic_names = nullptr;
-  topic_names_and_types.type_names = nullptr;
-
-  auto ret = rmw_get_topic_names_and_types(node_handle_.get(), &topic_names_and_types);
-  if (ret != RMW_RET_OK) {
-    // *INDENT-OFF*
-    throw std::runtime_error(
-      std::string("could not get topic names and types: ") + rmw_get_error_string_safe());
-    // *INDENT-ON*
-  }
-
-  std::map<std::string, std::string> topics;
-  for (size_t i = 0; i < topic_names_and_types.topic_count; ++i) {
-    topics[topic_names_and_types.topic_names[i]] = topic_names_and_types.type_names[i];
-  }
-
-  ret = rmw_destroy_topic_names_and_types(&topic_names_and_types);
-  if (ret != RMW_RET_OK) {
-    // *INDENT-OFF*
-    throw std::runtime_error(
-      std::string("could not destroy topic names and types: ") + rmw_get_error_string_safe());
-    // *INDENT-ON*
-  }
-
-  return topics;
-}
-
-size_t
-Node::count_publishers(const std::string & topic_name) const
-{
-  size_t count;
-  auto ret = rmw_count_publishers(node_handle_.get(), topic_name.c_str(), &count);
-  if (ret != RMW_RET_OK) {
-    // *INDENT-OFF*
-    throw std::runtime_error(
-      std::string("could not count publishers: ") + rmw_get_error_string_safe());
-    // *INDENT-ON*
-  }
-  return count;
-}
-
-size_t
-Node::count_subscribers(const std::string & topic_name) const
-{
-  size_t count;
-  auto ret = rmw_count_subscribers(node_handle_.get(), topic_name.c_str(), &count);
-  if (ret != RMW_RET_OK) {
-    // *INDENT-OFF*
-    throw std::runtime_error(
-      std::string("could not count subscribers: ") + rmw_get_error_string_safe());
-    // *INDENT-ON*
-  }
-  return count;
-}
-
-
-const Node::CallbackGroupWeakPtrList &
-Node::get_callback_groups() const
-{
-  return callback_groups_;
-}
+}  // namespace rclcpp
 
 #endif  // RCLCPP__NODE_IMPL_HPP_

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -0,0 +1,163 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rcl/node.h"
+#include "rclcpp/context.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Implementation of the NodeBase part of the Node API.
+class NodeBase : public NodeBaseInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBase)
+
+  RCLCPP_PUBLIC
+  NodeBase(
+    const std::string & node_name,
+    const std::string & namespace_,
+    rclcpp::Context::SharedPtr context,
+    const rcl_node_options_t & rcl_node_options,
+    bool use_intra_process_default,
+    bool enable_topic_statistics_default);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeBase();
+
+  RCLCPP_PUBLIC
+
+  const char *
+  get_name() const override;
+
+  RCLCPP_PUBLIC
+
+  const char *
+  get_namespace() const override;
+
+  RCLCPP_PUBLIC
+
+  const char *
+  get_fully_qualified_name() const override;
+
+  RCLCPP_PUBLIC
+
+  rclcpp::Context::SharedPtr
+  get_context() override;
+
+  RCLCPP_PUBLIC
+
+  rcl_node_t *
+  get_rcl_node_handle() override;
+
+  RCLCPP_PUBLIC
+
+  const rcl_node_t *
+  get_rcl_node_handle() const override;
+
+  RCLCPP_PUBLIC
+
+  std::shared_ptr<rcl_node_t>
+  get_shared_rcl_node_handle() override;
+
+  RCLCPP_PUBLIC
+
+  std::shared_ptr<const rcl_node_t>
+  get_shared_rcl_node_handle() const override;
+
+  RCLCPP_PUBLIC
+
+  bool
+  assert_liveliness() const override;
+
+  RCLCPP_PUBLIC
+
+  rclcpp::CallbackGroup::SharedPtr
+  create_callback_group(rclcpp::CallbackGroupType group_type) override;
+
+  RCLCPP_PUBLIC
+
+  rclcpp::CallbackGroup::SharedPtr
+  get_default_callback_group() override;
+
+  RCLCPP_PUBLIC
+
+  bool
+  callback_group_in_node(rclcpp::CallbackGroup::SharedPtr group) override;
+
+  RCLCPP_PUBLIC
+
+  const std::vector<rclcpp::CallbackGroup::WeakPtr> &
+  get_callback_groups() const override;
+
+  RCLCPP_PUBLIC
+
+  std::atomic_bool &
+  get_associated_with_executor_atomic() override;
+
+  RCLCPP_PUBLIC
+
+  rcl_guard_condition_t *
+  get_notify_guard_condition() override;
+
+  RCLCPP_PUBLIC
+
+  std::unique_lock<std::recursive_mutex>
+  acquire_notify_guard_condition_lock() const override;
+
+  RCLCPP_PUBLIC
+
+  bool
+  get_use_intra_process_default() const override;
+
+  bool
+  get_enable_topic_statistics_default() const override;
+
+private:
+  RCLCPP_DISABLE_COPY(NodeBase)
+
+  rclcpp::Context::SharedPtr context_;
+  bool use_intra_process_default_;
+  bool enable_topic_statistics_default_;
+
+  std::shared_ptr<rcl_node_t> node_handle_;
+
+  rclcpp::CallbackGroup::SharedPtr default_callback_group_;
+  std::vector<rclcpp::CallbackGroup::WeakPtr> callback_groups_;
+
+  std::atomic_bool associated_with_executor_;
+
+  /// Guard condition for notifying the Executor of changes to this node.
+  mutable std::recursive_mutex notify_guard_condition_mutex_;
+  rcl_guard_condition_t notify_guard_condition_ = rcl_get_zero_initialized_guard_condition();
+  bool notify_guard_condition_is_valid_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -0,0 +1,175 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_
+
+#include <memory>
+#include <mutex>
+#include <string>
+#include <vector>
+
+#include "rcl/node.h"
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/context.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeBase part of the Node API.
+class NodeBaseInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBaseInterface)
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeBaseInterface() = default;
+
+  /// Return the name of the node.
+  /** \return The name of the node. */
+  RCLCPP_PUBLIC
+  virtual
+  const char *
+  get_name() const = 0;
+
+  /// Return the namespace of the node.
+  /** \return The namespace of the node. */
+  RCLCPP_PUBLIC
+  virtual
+  const char *
+  get_namespace() const = 0;
+
+  /// Return the fully qualified name of the node.
+  /** \return The fully qualified name of the node. */
+  RCLCPP_PUBLIC
+  virtual
+  const char *
+  get_fully_qualified_name() const = 0;
+
+  /// Return the context of the node.
+  /** \return SharedPtr to the node's context. */
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::Context::SharedPtr
+  get_context() = 0;
+
+  /// Return the rcl_node_t node handle (non-const version).
+  RCLCPP_PUBLIC
+  virtual
+  rcl_node_t *
+  get_rcl_node_handle() = 0;
+
+  /// Return the rcl_node_t node handle (const version).
+  RCLCPP_PUBLIC
+  virtual
+  const rcl_node_t *
+  get_rcl_node_handle() const = 0;
+
+  /// Return the rcl_node_t node handle in a std::shared_ptr.
+  /**
+   * This handle remains valid after the Node is destroyed.
+   * The actual rcl node is not finalized until it is out of scope everywhere.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::shared_ptr<rcl_node_t>
+  get_shared_rcl_node_handle() = 0;
+
+  /// Return the rcl_node_t node handle in a std::shared_ptr.
+  /**
+   * This handle remains valid after the Node is destroyed.
+   * The actual rcl node is not finalized until it is out of scope everywhere.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::shared_ptr<const rcl_node_t>
+  get_shared_rcl_node_handle() const = 0;
+
+  /// Manually assert that this Node is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE).
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  assert_liveliness() const = 0;
+
+  /// Create and return a callback group.
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::CallbackGroup::SharedPtr
+  create_callback_group(rclcpp::CallbackGroupType group_type) = 0;
+
+  /// Return the default callback group.
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::CallbackGroup::SharedPtr
+  get_default_callback_group() = 0;
+
+  /// Return true if the given callback group is associated with this node.
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  callback_group_in_node(rclcpp::CallbackGroup::SharedPtr group) = 0;
+
+  /// Return list of callback groups associated with this node.
+  RCLCPP_PUBLIC
+  virtual
+  const std::vector<rclcpp::CallbackGroup::WeakPtr> &
+  get_callback_groups() const = 0;
+
+  /// Return the atomic bool which is used to ensure only one executor is used.
+  RCLCPP_PUBLIC
+  virtual
+  std::atomic_bool &
+  get_associated_with_executor_atomic() = 0;
+
+  /// Return guard condition that should be notified when the internal node state changes.
+  /**
+   * For example, this should be notified when a publisher is added or removed.
+   *
+   * \return the rcl_guard_condition_t if it is valid, else nullptr
+   */
+  RCLCPP_PUBLIC
+  virtual
+  rcl_guard_condition_t *
+  get_notify_guard_condition() = 0;
+
+  /// Acquire and return a scoped lock that protects the notify guard condition.
+  /** This should be used when triggering the notify guard condition. */
+  RCLCPP_PUBLIC
+  virtual
+  std::unique_lock<std::recursive_mutex>
+  acquire_notify_guard_condition_lock() const = 0;
+
+  /// Return the default preference for using intra process communication.
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  get_use_intra_process_default() const = 0;
+
+  /// Return the default preference for enabling topic statistics collection.
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  get_enable_topic_statistics_default() const = 0;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_clock.hpp

@@ -0,0 +1,76 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_
+
+#include "rclcpp/clock.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_clock_interface.hpp"
+#include "rclcpp/node_interfaces/node_graph_interface.hpp"
+#include "rclcpp/node_interfaces/node_logging_interface.hpp"
+#include "rclcpp/node_interfaces/node_services_interface.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Implementation of the NodeClock part of the Node API.
+class NodeClock : public NodeClockInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeClock)
+
+  RCLCPP_PUBLIC
+  explicit NodeClock(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
+    rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeClock();
+
+  /// Get a clock which will be kept up to date by the node.
+  RCLCPP_PUBLIC
+  rclcpp::Clock::SharedPtr
+  get_clock() override;
+
+  /// Get a clock which will be kept up to date by the node.
+  RCLCPP_PUBLIC
+  rclcpp::Clock::ConstSharedPtr
+  get_clock() const override;
+
+private:
+  RCLCPP_DISABLE_COPY(NodeClock)
+
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
+  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
+  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
+
+  rclcpp::Clock::SharedPtr ros_clock_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_

rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp

@@ -0,0 +1,53 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_CLOCK_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_CLOCK_INTERFACE_HPP_
+
+#include "rclcpp/clock.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeClock part of the Node API.
+class NodeClockInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeClockInterface)
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeClockInterface() = default;
+
+  /// Get a ROS clock which will be kept up to date by the node.
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::Clock::SharedPtr
+  get_clock() = 0;
+
+  /// Get a const ROS clock which will be kept up to date by the node.
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::Clock::ConstSharedPtr
+  get_clock() const = 0;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_CLOCK_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_graph.hpp

@@ -0,0 +1,158 @@
+// Copyright 2016-2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_
+
+#include <chrono>
+#include <map>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "rcl/guard_condition.h"
+
+#include "rclcpp/event.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_graph_interface.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rmw/topic_endpoint_info_array.h"
+
+namespace rclcpp
+{
+
+namespace graph_listener
+{
+class GraphListener;
+}  // namespace graph_listener
+
+namespace node_interfaces
+{
+
+/// Implementation the NodeGraph part of the Node API.
+class NodeGraph : public NodeGraphInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeGraph)
+
+  RCLCPP_PUBLIC
+  explicit NodeGraph(rclcpp::node_interfaces::NodeBaseInterface * node_base);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeGraph();
+
+  RCLCPP_PUBLIC
+
+  std::map<std::string, std::vector<std::string>>
+  get_topic_names_and_types(bool no_demangle = false) const override;
+
+  RCLCPP_PUBLIC
+
+  std::map<std::string, std::vector<std::string>>
+  get_service_names_and_types() const override;
+
+  RCLCPP_PUBLIC
+
+  std::vector<std::string>
+  get_node_names() const override;
+
+  RCLCPP_PUBLIC
+
+  std::vector<std::pair<std::string, std::string>>
+  get_node_names_and_namespaces() const override;
+
+  RCLCPP_PUBLIC
+
+  size_t
+  count_publishers(const std::string & topic_name) const override;
+
+  RCLCPP_PUBLIC
+
+  size_t
+  count_subscribers(const std::string & topic_name) const override;
+
+  RCLCPP_PUBLIC
+
+  const rcl_guard_condition_t *
+  get_graph_guard_condition() const override;
+
+  RCLCPP_PUBLIC
+
+  void
+  notify_graph_change() override;
+
+  RCLCPP_PUBLIC
+
+  void
+  notify_shutdown() override;
+
+  RCLCPP_PUBLIC
+
+  rclcpp::Event::SharedPtr
+  get_graph_event() override;
+
+  RCLCPP_PUBLIC
+
+  void
+  wait_for_graph_change(
+    rclcpp::Event::SharedPtr event,
+    std::chrono::nanoseconds timeout) override;
+
+  RCLCPP_PUBLIC
+
+  size_t
+  count_graph_users() override;
+
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::TopicEndpointInfo>
+  get_publishers_info_by_topic(
+    const std::string & topic_name,
+    bool no_mangle = false) const override;
+
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::TopicEndpointInfo>
+  get_subscriptions_info_by_topic(
+    const std::string & topic_name,
+    bool no_mangle = false) const override;
+
+private:
+  RCLCPP_DISABLE_COPY(NodeGraph)
+
+  /// Handle to the NodeBaseInterface given in the constructor.
+  rclcpp::node_interfaces::NodeBaseInterface * node_base_;
+
+  /// Graph Listener which waits on graph changes for the node and is shared across nodes.
+  std::shared_ptr<rclcpp::graph_listener::GraphListener> graph_listener_;
+  /// Whether or not this node needs to be added to the graph listener.
+  std::atomic_bool should_add_to_graph_listener_;
+
+  /// Mutex to guard the graph event related data structures.
+  mutable std::mutex graph_mutex_;
+  /// For notifying waiting threads (wait_for_graph_change()) on changes (notify_graph_change()).
+  std::condition_variable graph_cv_;
+  /// Weak references to graph events out on loan.
+  std::vector<rclcpp::Event::WeakPtr> graph_events_;
+  /// Number of graph events out on loan, used to determine if the graph should be monitored.
+  /** graph_users_count_ is atomic so that it can be accessed without acquiring the graph_mutex_ */
+  std::atomic_size_t graph_users_count_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_

rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp

@@ -0,0 +1,276 @@
+// Copyright 2016-2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_
+
+#include <algorithm>
+#include <array>
+#include <chrono>
+#include <map>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "rcl/graph.h"
+#include "rcl/guard_condition.h"
+
+#include "rclcpp/event.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+enum class EndpointType
+{
+  Invalid = RMW_ENDPOINT_INVALID,
+  Publisher = RMW_ENDPOINT_PUBLISHER,
+  Subscription = RMW_ENDPOINT_SUBSCRIPTION
+};
+
+/**
+ * Struct that contains topic endpoint information like the associated node name, node namespace,
+ * topic type, endpoint type, endpoint GID, and its QoS.
+ */
+class TopicEndpointInfo
+{
+public:
+  /// Construct a TopicEndpointInfo from a rcl_topic_endpoint_info_t.
+  RCLCPP_PUBLIC
+  explicit TopicEndpointInfo(const rcl_topic_endpoint_info_t & info)
+  : node_name_(info.node_name),
+    node_namespace_(info.node_namespace),
+    topic_type_(info.topic_type),
+    endpoint_type_(static_cast<rclcpp::EndpointType>(info.endpoint_type)),
+    qos_profile_({info.qos_profile.history, info.qos_profile.depth}, info.qos_profile)
+  {
+    std::copy(info.endpoint_gid, info.endpoint_gid + RMW_GID_STORAGE_SIZE, endpoint_gid_.begin());
+  }
+
+  /// Get a mutable reference to the node name.
+  RCLCPP_PUBLIC
+  std::string &
+  node_name();
+
+  /// Get a const reference to the node name.
+  RCLCPP_PUBLIC
+  const std::string &
+  node_name() const;
+
+  /// Get a mutable reference to the node namespace.
+  RCLCPP_PUBLIC
+  std::string &
+  node_namespace();
+
+  /// Get a const reference to the node namespace.
+  RCLCPP_PUBLIC
+  const std::string &
+  node_namespace() const;
+
+  /// Get a mutable reference to the topic type string.
+  RCLCPP_PUBLIC
+  std::string &
+  topic_type();
+
+  /// Get a const reference to the topic type string.
+  RCLCPP_PUBLIC
+  const std::string &
+  topic_type() const;
+
+  /// Get a mutable reference to the topic endpoint type.
+  RCLCPP_PUBLIC
+  rclcpp::EndpointType &
+  endpoint_type();
+
+  /// Get a const reference to the topic endpoint type.
+  RCLCPP_PUBLIC
+  const rclcpp::EndpointType &
+  endpoint_type() const;
+
+  /// Get a mutable reference to the GID of the topic endpoint.
+  RCLCPP_PUBLIC
+  std::array<uint8_t, RMW_GID_STORAGE_SIZE> &
+  endpoint_gid();
+
+  /// Get a const reference to the GID of the topic endpoint.
+  RCLCPP_PUBLIC
+  const std::array<uint8_t, RMW_GID_STORAGE_SIZE> &
+  endpoint_gid() const;
+
+  /// Get a mutable reference to the QoS profile of the topic endpoint.
+  RCLCPP_PUBLIC
+  rclcpp::QoS &
+  qos_profile();
+
+  /// Get a const reference to the QoS profile of the topic endpoint.
+  RCLCPP_PUBLIC
+  const rclcpp::QoS &
+  qos_profile() const;
+
+private:
+  std::string node_name_;
+  std::string node_namespace_;
+  std::string topic_type_;
+  rclcpp::EndpointType endpoint_type_;
+  std::array<uint8_t, RMW_GID_STORAGE_SIZE> endpoint_gid_;
+  rclcpp::QoS qos_profile_;
+};
+
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeGraph part of the Node API.
+class NodeGraphInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeGraphInterface)
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeGraphInterface() = default;
+
+  /// Return a map of existing topic names to list of topic types.
+  /**
+   * A topic is considered to exist when at least one publisher or subscriber
+   * exists for it, whether they be local or remote to this process.
+   *
+   * \param[in] no_demangle if true, topic names and types are not demangled
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::map<std::string, std::vector<std::string>>
+  get_topic_names_and_types(bool no_demangle = false) const = 0;
+
+  /// Return a map of existing service names to list of service types.
+  /**
+   * A service is considered to exist when at least one service server or
+   * service client exists for it, whether they be local or remote to this
+   * process.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::map<std::string, std::vector<std::string>>
+  get_service_names_and_types() const = 0;
+
+  /// Return a vector of existing node names (string).
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<std::string>
+  get_node_names() const = 0;
+
+  /// Return a vector of existing node names and namespaces (pair of string).
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<std::pair<std::string, std::string>>
+  get_node_names_and_namespaces() const = 0;
+
+  /// Return the number of publishers that are advertised on a given topic.
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  count_publishers(const std::string & topic_name) const = 0;
+
+  /// Return the number of subscribers who have created a subscription for a given topic.
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  count_subscribers(const std::string & topic_name) const = 0;
+
+  /// Return the rcl guard condition which is triggered when the ROS graph changes.
+  RCLCPP_PUBLIC
+  virtual
+  const rcl_guard_condition_t *
+  get_graph_guard_condition() const = 0;
+
+  /// Notify threads waiting on graph changes.
+  /**
+   * Affects threads waiting on the notify guard condition, see:
+   * get_notify_guard_condition(), as well as the threads waiting on graph
+   * changes using a graph Event, see: wait_for_graph_change().
+   *
+   * This is typically only used by the rclcpp::graph_listener::GraphListener.
+   *
+   * \throws RCLBaseError (a child of that exception) when an rcl error occurs
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  notify_graph_change() = 0;
+
+  /// Notify any and all blocking node actions that shutdown has occurred.
+  RCLCPP_PUBLIC
+  virtual
+  void
+  notify_shutdown() = 0;
+
+  /// Return a graph event, which will be set anytime a graph change occurs.
+  /**
+   * The graph Event object is a loan which must be returned.
+   * The Event object is scoped and therefore to return the load just let it go
+   * out of scope.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::Event::SharedPtr
+  get_graph_event() = 0;
+
+  /// Wait for a graph event to occur by waiting on an Event to become set.
+  /**
+   * The given Event must be acquire through the get_graph_event() method.
+   *
+   * \throws InvalidEventError if the given event is nullptr
+   * \throws EventNotRegisteredError if the given event was not acquired with
+   *   get_graph_event().
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  wait_for_graph_change(
+    rclcpp::Event::SharedPtr event,
+    std::chrono::nanoseconds timeout) = 0;
+
+  /// Return the number of on loan graph events, see get_graph_event().
+  /**
+   * This is typically only used by the rclcpp::graph_listener::GraphListener.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  count_graph_users() = 0;
+
+  /// Return the topic endpoint information about publishers on a given topic.
+  /**
+   * \sa rclcpp::Node::get_publishers_info_by_topic
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<rclcpp::TopicEndpointInfo>
+  get_publishers_info_by_topic(const std::string & topic_name, bool no_mangle = false) const = 0;
+
+  /// Return the topic endpoint information about subscriptions on a given topic.
+  /**
+   * \sa rclcpp::Node::get_subscriptions_info_by_topic
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<rclcpp::TopicEndpointInfo>
+  get_subscriptions_info_by_topic(const std::string & topic_name, bool no_mangle = false) const = 0;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_logging.hpp

@@ -0,0 +1,66 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_LOGGING_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_LOGGING_HPP_
+
+#include <memory>
+
+#include "rclcpp/logger.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_logging_interface.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Implementation of the NodeLogging part of the Node API.
+class NodeLogging : public NodeLoggingInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeLoggingInterface)
+
+  RCLCPP_PUBLIC
+  explicit NodeLogging(rclcpp::node_interfaces::NodeBaseInterface * node_base);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeLogging();
+
+  RCLCPP_PUBLIC
+
+  rclcpp::Logger
+  get_logger() const override;
+
+  RCLCPP_PUBLIC
+
+  const char *
+  get_logger_name() const override;
+
+private:
+  RCLCPP_DISABLE_COPY(NodeLogging)
+
+  /// Handle to the NodeBaseInterface given in the constructor.
+  rclcpp::node_interfaces::NodeBaseInterface * node_base_;
+
+  rclcpp::Logger logger_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_LOGGING_HPP_

rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp

@@ -0,0 +1,57 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_LOGGING_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_LOGGING_INTERFACE_HPP_
+
+#include <memory>
+
+#include "rclcpp/logger.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeLogging part of the Node API.
+class NodeLoggingInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeLoggingInterface)
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeLoggingInterface() = default;
+
+  /// Return the logger of the node.
+  /** \return The logger of the node. */
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::Logger
+  get_logger() const = 0;
+
+  /// Return the logger name associated with the node.
+  /** \return The logger name associated with the node. */
+  RCLCPP_PUBLIC
+  virtual
+  const char *
+  get_logger_name() const = 0;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_LOGGING_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -0,0 +1,213 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_
+
+#include <map>
+#include <memory>
+#include <list>
+#include <string>
+#include <vector>
+
+#include "rcl_interfaces/msg/list_parameters_result.hpp"
+#include "rcl_interfaces/msg/parameter_descriptor.hpp"
+#include "rcl_interfaces/msg/parameter_event.hpp"
+#include "rcl_interfaces/msg/set_parameters_result.hpp"
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_logging_interface.hpp"
+#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
+#include "rclcpp/node_interfaces/node_services_interface.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/parameter_service.hpp"
+#include "rclcpp/publisher.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+// Internal struct for holding useful info about parameters
+struct ParameterInfo
+{
+  /// Current value of the parameter.
+  rclcpp::ParameterValue value;
+
+  /// A description of the parameter
+  rcl_interfaces::msg::ParameterDescriptor descriptor;
+};
+
+// Internal RAII-style guard for mutation recursion
+class ParameterMutationRecursionGuard
+{
+public:
+  explicit ParameterMutationRecursionGuard(bool & allow_mod)
+  : allow_modification_(allow_mod)
+  {
+    if (!allow_modification_) {
+      throw rclcpp::exceptions::ParameterModifiedInCallbackException(
+              "cannot set or declare a parameter, or change the callback from within set callback");
+    }
+
+    allow_modification_ = false;
+  }
+
+  ~ParameterMutationRecursionGuard()
+  {
+    allow_modification_ = true;
+  }
+
+private:
+  bool & allow_modification_;
+};
+
+/// Implementation of the NodeParameters part of the Node API.
+class NodeParameters : public NodeParametersInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeParameters)
+
+  RCLCPP_PUBLIC
+  NodeParameters(
+    const node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    const node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+    const node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+    const node_interfaces::NodeServicesInterface::SharedPtr node_services,
+    const node_interfaces::NodeClockInterface::SharedPtr node_clock,
+    const std::vector<Parameter> & parameter_overrides,
+    bool start_parameter_services,
+    bool start_parameter_event_publisher,
+    const rclcpp::QoS & parameter_event_qos,
+    const rclcpp::PublisherOptionsBase & parameter_event_publisher_options,
+    bool allow_undeclared_parameters,
+    bool automatically_declare_parameters_from_overrides);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeParameters();
+
+  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;
+
+  RCLCPP_PUBLIC
+  void
+  undeclare_parameter(const std::string & name) override;
+
+  RCLCPP_PUBLIC
+  bool
+  has_parameter(const std::string & name) const override;
+
+  RCLCPP_PUBLIC
+  std::vector<rcl_interfaces::msg::SetParametersResult>
+  set_parameters(
+    const std::vector<rclcpp::Parameter> & parameters) override;
+
+  RCLCPP_PUBLIC
+  rcl_interfaces::msg::SetParametersResult
+  set_parameters_atomically(
+    const std::vector<rclcpp::Parameter> & parameters) override;
+
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::Parameter>
+  get_parameters(const std::vector<std::string> & names) const override;
+
+  RCLCPP_PUBLIC
+  rclcpp::Parameter
+  get_parameter(const std::string & name) const override;
+
+  RCLCPP_PUBLIC
+  bool
+  get_parameter(
+    const std::string & name,
+    rclcpp::Parameter & parameter) const override;
+
+  RCLCPP_PUBLIC
+  bool
+  get_parameters_by_prefix(
+    const std::string & prefix,
+    std::map<std::string, rclcpp::Parameter> & parameters) const override;
+
+  RCLCPP_PUBLIC
+  std::vector<rcl_interfaces::msg::ParameterDescriptor>
+  describe_parameters(const std::vector<std::string> & names) const override;
+
+  RCLCPP_PUBLIC
+  std::vector<uint8_t>
+  get_parameter_types(const std::vector<std::string> & names) const override;
+
+  RCLCPP_PUBLIC
+  rcl_interfaces::msg::ListParametersResult
+  list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const override;
+
+  RCLCPP_PUBLIC
+  OnSetParametersCallbackHandle::SharedPtr
+  add_on_set_parameters_callback(OnParametersSetCallbackType callback) override;
+
+  RCLCPP_PUBLIC
+  void
+  remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) override;
+
+  RCLCPP_PUBLIC
+  OnParametersSetCallbackType
+  set_on_parameters_set_callback(OnParametersSetCallbackType callback) override;
+
+  RCLCPP_PUBLIC
+  const std::map<std::string, rclcpp::ParameterValue> &
+  get_parameter_overrides() const override;
+
+  using CallbacksContainerType = std::list<OnSetParametersCallbackHandle::WeakPtr>;
+
+private:
+  RCLCPP_DISABLE_COPY(NodeParameters)
+
+  mutable std::recursive_mutex mutex_;
+
+  // There are times when we don't want to allow modifications to parameters
+  // (particularly when a set_parameter callback tries to call set_parameter,
+  // declare_parameter, etc).  In those cases, this will be set to false.
+  bool parameter_modification_enabled_{true};
+
+  OnParametersSetCallbackType on_parameters_set_callback_ = nullptr;
+
+  CallbacksContainerType on_parameters_set_callback_container_;
+
+  std::map<std::string, ParameterInfo> parameters_;
+
+  std::map<std::string, rclcpp::ParameterValue> parameter_overrides_;
+
+  bool allow_undeclared_ = false;
+
+  Publisher<rcl_interfaces::msg::ParameterEvent>::SharedPtr events_publisher_;
+
+  std::shared_ptr<ParameterService> parameter_service_;
+
+  std::string combined_name_;
+
+  node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
+  node_interfaces::NodeClockInterface::SharedPtr node_clock_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_

rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp

@@ -0,0 +1,213 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_
+
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rcl_interfaces/msg/list_parameters_result.hpp"
+#include "rcl_interfaces/msg/parameter_descriptor.hpp"
+#include "rcl_interfaces/msg/set_parameters_result.hpp"
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+struct OnSetParametersCallbackHandle
+{
+  RCLCPP_SMART_PTR_DEFINITIONS(OnSetParametersCallbackHandle)
+
+  using OnParametersSetCallbackType =
+    std::function<
+    rcl_interfaces::msg::SetParametersResult(
+      const std::vector<rclcpp::Parameter> &)>;
+
+  OnParametersSetCallbackType callback;
+};
+
+/// Pure virtual interface class for the NodeParameters part of the Node API.
+class NodeParametersInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeParametersInterface)
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeParametersInterface() = default;
+
+  /// Declare and initialize a parameter.
+  /**
+   * \sa rclcpp::Node::declare_parameter
+   */
+  RCLCPP_PUBLIC
+  virtual
+  const rclcpp::ParameterValue &
+  declare_parameter(
+    const std::string & name,
+    const rclcpp::ParameterValue & default_value = rclcpp::ParameterValue(),
+    const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
+    rcl_interfaces::msg::ParameterDescriptor(),
+    bool ignore_override = false) = 0;
+
+  /// Undeclare a parameter.
+  /**
+   * \sa rclcpp::Node::undeclare_parameter
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  undeclare_parameter(const std::string & name) = 0;
+
+  /// Return true if the parameter has been declared, otherwise false.
+  /**
+   * \sa rclcpp::Node::has_parameter
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  has_parameter(const std::string & name) const = 0;
+
+  /// Set one or more parameters, one at a time.
+  /**
+   * \sa rclcpp::Node::set_parameters
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<rcl_interfaces::msg::SetParametersResult>
+  set_parameters(const std::vector<rclcpp::Parameter> & parameters) = 0;
+
+  /// Set and initialize a parameter, all at once.
+  /**
+   * \sa rclcpp::Node::set_parameters_atomically
+   */
+  RCLCPP_PUBLIC
+  virtual
+  rcl_interfaces::msg::SetParametersResult
+  set_parameters_atomically(
+    const std::vector<rclcpp::Parameter> & parameters) = 0;
+
+  /// Get descriptions of parameters given their names.
+  /*
+   * \param[in] names a list of parameter names to check.
+   * \return the list of parameters that were found.
+   * Any parameter not found is omitted from the returned list.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<rclcpp::Parameter>
+  get_parameters(const std::vector<std::string> & names) const = 0;
+
+  /// Get the description of one parameter given a name.
+  /*
+   * \param[in] name the name of the parameter to look for.
+   * \return the parameter if it exists on the node.
+   * \throws std::out_of_range if the parameter does not exist on the node.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::Parameter
+  get_parameter(const std::string & name) const = 0;
+
+  /// Get the description of one parameter given a name.
+  /*
+   * \param[in] name the name of the parameter to look for.
+   * \param[out] parameter the description if parameter exists on the node.
+   * \return true if the parameter exists on the node, or
+   * \return false if the parameter does not exist.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  get_parameter(
+    const std::string & name,
+    rclcpp::Parameter & parameter) const = 0;
+
+  /// Get all parameters that have the specified prefix into the parameters map.
+  /*
+   * \param[in] prefix the name of the prefix to look for.
+   * \param[out] parameters a map of parameters that matched the prefix.
+   * \return true if any parameters with the prefix exists on the node, or
+   * \return false otherwise.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  get_parameters_by_prefix(
+    const std::string & prefix,
+    std::map<std::string, rclcpp::Parameter> & parameters) const = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<rcl_interfaces::msg::ParameterDescriptor>
+  describe_parameters(const std::vector<std::string> & names) const = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<uint8_t>
+  get_parameter_types(const std::vector<std::string> & names) const = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  rcl_interfaces::msg::ListParametersResult
+  list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const = 0;
+
+  using OnParametersSetCallbackType = OnSetParametersCallbackHandle::OnParametersSetCallbackType;
+
+  /// Add a callback for when parameters are being set.
+  /**
+   * \sa rclcpp::Node::add_on_set_parameters_callback
+   */
+  RCLCPP_PUBLIC
+  virtual
+  OnSetParametersCallbackHandle::SharedPtr
+  add_on_set_parameters_callback(OnParametersSetCallbackType callback) = 0;
+
+  /// Remove a callback registered with `add_on_set_parameters_callback`.
+  /**
+   * \sa rclcpp::Node::remove_on_set_parameters_callback
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) = 0;
+
+  /// Register a callback for when parameters are being set, return an existing one.
+  /**
+   * \sa rclcpp::Node::set_on_parameters_set_callback
+   */
+  RCLCPP_PUBLIC
+  virtual
+  OnParametersSetCallbackType
+  set_on_parameters_set_callback(OnParametersSetCallbackType callback) = 0;
+
+  /// Return the initial parameter values used by the NodeParameters to override default values.
+  RCLCPP_PUBLIC
+  virtual
+  const std::map<std::string, rclcpp::ParameterValue> &
+  get_parameter_overrides() const = 0;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_services.hpp

@@ -0,0 +1,67 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_SERVICES_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_SERVICES_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/client.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_services_interface.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Implementation of the NodeServices part of the Node API.
+class NodeServices : public NodeServicesInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeServices)
+
+  RCLCPP_PUBLIC
+  explicit NodeServices(rclcpp::node_interfaces::NodeBaseInterface * node_base);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeServices();
+
+  RCLCPP_PUBLIC
+
+  void
+  add_client(
+    rclcpp::ClientBase::SharedPtr client_base_ptr,
+    rclcpp::CallbackGroup::SharedPtr group) override;
+
+  RCLCPP_PUBLIC
+
+  void
+  add_service(
+    rclcpp::ServiceBase::SharedPtr service_base_ptr,
+    rclcpp::CallbackGroup::SharedPtr group) override;
+
+private:
+  RCLCPP_DISABLE_COPY(NodeServices)
+
+  rclcpp::node_interfaces::NodeBaseInterface * node_base_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_SERVICES_HPP_

rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp

@@ -0,0 +1,57 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/client.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeServices part of the Node API.
+class NodeServicesInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeServicesInterface)
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeServicesInterface() = default;
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_client(
+    rclcpp::ClientBase::SharedPtr client_base_ptr,
+    rclcpp::CallbackGroup::SharedPtr group) = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_service(
+    rclcpp::ServiceBase::SharedPtr service_base_ptr,
+    rclcpp::CallbackGroup::SharedPtr group) = 0;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp

@@ -0,0 +1,73 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_HPP_
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_clock_interface.hpp"
+#include "rclcpp/node_interfaces/node_graph_interface.hpp"
+#include "rclcpp/node_interfaces/node_logging_interface.hpp"
+#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
+#include "rclcpp/node_interfaces/node_services_interface.hpp"
+#include "rclcpp/node_interfaces/node_time_source_interface.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/time_source.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Implementation of the NodeTimeSource part of the Node API.
+class NodeTimeSource : public NodeTimeSourceInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimeSource)
+
+  RCLCPP_PUBLIC
+  explicit NodeTimeSource(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
+    rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
+    rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters
+  );
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeTimeSource();
+
+private:
+  RCLCPP_DISABLE_COPY(NodeTimeSource)
+
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
+  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
+  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
+
+  rclcpp::TimeSource time_source_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp

@@ -0,0 +1,40 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeTimeSource part of the Node API.
+class NodeTimeSourceInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimeSourceInterface)
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeTimeSourceInterface() = default;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_timers.hpp

@@ -0,0 +1,60 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_TIMERS_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_TIMERS_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_timers_interface.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Implementation of the NodeTimers part of the Node API.
+class NodeTimers : public NodeTimersInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimers)
+
+  RCLCPP_PUBLIC
+  explicit NodeTimers(rclcpp::node_interfaces::NodeBaseInterface * node_base);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeTimers();
+
+  /// Add a timer to the node.
+  RCLCPP_PUBLIC
+
+  void
+  add_timer(
+    rclcpp::TimerBase::SharedPtr timer,
+    rclcpp::CallbackGroup::SharedPtr callback_group) override;
+
+private:
+  RCLCPP_DISABLE_COPY(NodeTimers)
+
+  rclcpp::node_interfaces::NodeBaseInterface * node_base_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_TIMERS_HPP_

rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp

@@ -0,0 +1,50 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_TIMERS_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_TIMERS_INTERFACE_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeTimers part of the Node API.
+class NodeTimersInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimersInterface)
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeTimersInterface() = default;
+
+  /// Add a timer to the node.
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_timer(
+    rclcpp::TimerBase::SharedPtr timer,
+    rclcpp::CallbackGroup::SharedPtr callback_group) = 0;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_TIMERS_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_topics.hpp

@@ -0,0 +1,94 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_TOPICS_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_TOPICS_HPP_
+
+#include <string>
+
+#include "rcl/publisher.h"
+#include "rcl/subscription.h"
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_timers_interface.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/publisher.hpp"
+#include "rclcpp/publisher_factory.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Implementation of the NodeTopics part of the Node API.
+class NodeTopics : public NodeTopicsInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTopicsInterface)
+
+  RCLCPP_PUBLIC
+  NodeTopics(
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    rclcpp::node_interfaces::NodeTimersInterface * node_timers);
+
+  RCLCPP_PUBLIC
+  ~NodeTopics() override;
+
+  RCLCPP_PUBLIC
+  rclcpp::PublisherBase::SharedPtr
+  create_publisher(
+    const std::string & topic_name,
+    const rclcpp::PublisherFactory & publisher_factory,
+    const rclcpp::QoS & qos) override;
+
+  RCLCPP_PUBLIC
+  void
+  add_publisher(
+    rclcpp::PublisherBase::SharedPtr publisher,
+    rclcpp::CallbackGroup::SharedPtr callback_group) override;
+
+  RCLCPP_PUBLIC
+  rclcpp::SubscriptionBase::SharedPtr
+  create_subscription(
+    const std::string & topic_name,
+    const rclcpp::SubscriptionFactory & subscription_factory,
+    const rclcpp::QoS & qos) override;
+
+  RCLCPP_PUBLIC
+  void
+  add_subscription(
+    rclcpp::SubscriptionBase::SharedPtr subscription,
+    rclcpp::CallbackGroup::SharedPtr callback_group) override;
+
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeBaseInterface *
+  get_node_base_interface() const override;
+
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeTimersInterface *
+  get_node_timers_interface() const override;
+
+private:
+  RCLCPP_DISABLE_COPY(NodeTopics)
+
+  rclcpp::node_interfaces::NodeBaseInterface * node_base_;
+  rclcpp::node_interfaces::NodeTimersInterface * node_timers_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_TOPICS_HPP_

rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp

@@ -0,0 +1,94 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_
+
+#include <functional>
+#include <memory>
+#include <string>
+
+#include "rcl/publisher.h"
+#include "rcl/subscription.h"
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_timers_interface.hpp"
+#include "rclcpp/publisher.hpp"
+#include "rclcpp/publisher_factory.hpp"
+#include "rclcpp/subscription.hpp"
+#include "rclcpp/subscription_factory.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeTopics part of the Node API.
+class NodeTopicsInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTopicsInterface)
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeTopicsInterface() = default;
+
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::PublisherBase::SharedPtr
+  create_publisher(
+    const std::string & topic_name,
+    const rclcpp::PublisherFactory & publisher_factory,
+    const rclcpp::QoS & qos) = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_publisher(
+    rclcpp::PublisherBase::SharedPtr publisher,
+    rclcpp::CallbackGroup::SharedPtr callback_group) = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::SubscriptionBase::SharedPtr
+  create_subscription(
+    const std::string & topic_name,
+    const rclcpp::SubscriptionFactory & subscription_factory,
+    const rclcpp::QoS & qos) = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_subscription(
+    rclcpp::SubscriptionBase::SharedPtr subscription,
+    rclcpp::CallbackGroup::SharedPtr callback_group) = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::node_interfaces::NodeBaseInterface *
+  get_node_base_interface() const = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  rclcpp::node_interfaces::NodeTimersInterface *
+  get_node_timers_interface() const = 0;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp

@@ -0,0 +1,66 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_WAITABLES_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_WAITABLES_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_waitables_interface.hpp"
+#include "rclcpp/waitable.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Implementation of the NodeWaitables part of the Node API.
+class NodeWaitables : public NodeWaitablesInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeWaitables)
+
+  RCLCPP_PUBLIC
+  explicit NodeWaitables(rclcpp::node_interfaces::NodeBaseInterface * node_base);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeWaitables();
+
+  RCLCPP_PUBLIC
+
+  void
+  add_waitable(
+    rclcpp::Waitable::SharedPtr waitable_base_ptr,
+    rclcpp::CallbackGroup::SharedPtr group) override;
+
+  RCLCPP_PUBLIC
+
+  void
+  remove_waitable(
+    rclcpp::Waitable::SharedPtr waitable_ptr,
+    rclcpp::CallbackGroup::SharedPtr group) noexcept override;
+
+private:
+  RCLCPP_DISABLE_COPY(NodeWaitables)
+
+  rclcpp::node_interfaces::NodeBaseInterface * node_base_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_WAITABLES_HPP_

rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp

@@ -0,0 +1,57 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeWaitables part of the Node API.
+class NodeWaitablesInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeWaitablesInterface)
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeWaitablesInterface() = default;
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_waitable(
+    rclcpp::Waitable::SharedPtr waitable_ptr,
+    rclcpp::CallbackGroup::SharedPtr group) = 0;
+
+  /// \note this function should not throw because it may be called in destructors
+  RCLCPP_PUBLIC
+  virtual
+  void
+  remove_waitable(
+    rclcpp::Waitable::SharedPtr waitable_ptr,
+    rclcpp::CallbackGroup::SharedPtr group) noexcept = 0;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_

rclcpp/include/rclcpp/node_options.hpp

@@ -0,0 +1,374 @@
+// Copyright 2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_OPTIONS_HPP_
+#define RCLCPP__NODE_OPTIONS_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rcl/node_options.h"
+#include "rclcpp/context.hpp"
+#include "rclcpp/contexts/default_context.hpp"
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/publisher_options.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Encapsulation of options for node initialization.
+class NodeOptions
+{
+public:
+  /// Create NodeOptions with default values, optionally specifying the allocator to use.
+  /**
+   * Default values for the node options:
+   *
+   *   - context = rclcpp::contexts::get_global_default_context()
+   *   - arguments = {}
+   *   - parameter_overrides = {}
+   *   - use_global_arguments = true
+   *   - use_intra_process_comms = false
+   *   - enable_topic_statistics = false
+   *   - start_parameter_services = true
+   *   - start_parameter_event_publisher = true
+   *   - parameter_event_qos = rclcpp::ParameterEventQoS
+   *     - with history setting and depth from rmw_qos_profile_parameter_events
+   *   - parameter_event_publisher_options = rclcpp::PublisherOptionsBase
+   *   - allow_undeclared_parameters = false
+   *   - automatically_declare_parameters_from_overrides = false
+   *   - allocator = rcl_get_default_allocator()
+   *
+   * \param[in] allocator allocator to use in construction of NodeOptions.
+   */
+  RCLCPP_PUBLIC
+  explicit NodeOptions(rcl_allocator_t allocator = rcl_get_default_allocator());
+
+  /// Destructor.
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeOptions() = default;
+
+  /// Copy constructor.
+  RCLCPP_PUBLIC
+  NodeOptions(const NodeOptions & other);
+
+  /// Assignment operator.
+  RCLCPP_PUBLIC
+  NodeOptions &
+  operator=(const NodeOptions & other);
+
+  /// Return the rcl_node_options used by the node.
+  /**
+   * This data structure is created lazily, on the first call to this function.
+   * Repeated calls will not regenerate it unless one of the input settings
+   * changed, like arguments, use_global_arguments, or the rcl allocator.
+   */
+  RCLCPP_PUBLIC
+  const rcl_node_options_t *
+  get_rcl_node_options() const;
+
+  /// Return the context to be used by the node.
+  RCLCPP_PUBLIC
+  rclcpp::Context::SharedPtr
+  context() const;
+
+  /// Set the context, return this for parameter idiom.
+  RCLCPP_PUBLIC
+  NodeOptions &
+  context(rclcpp::Context::SharedPtr context);
+
+  /// Return a reference to the list of arguments for the node.
+  RCLCPP_PUBLIC
+  const std::vector<std::string> &
+  arguments() const;
+
+  /// Set the arguments, return this for parameter idiom.
+  /**
+   * These arguments are used to extract remappings used by the node and other
+   * ROS specific settings, as well as user defined non-ROS arguments.
+   *
+   * This will cause the internal rcl_node_options_t struct to be invalidated.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  arguments(const std::vector<std::string> & arguments);
+
+  /// Return a reference to the list of parameter overrides.
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::Parameter> &
+  parameter_overrides();
+
+  RCLCPP_PUBLIC
+  const std::vector<rclcpp::Parameter> &
+  parameter_overrides() const;
+
+  /// Set the parameters overrides, return this for parameter idiom.
+  /**
+   * These parameter overrides are used to change the initial value
+   * of declared parameters within the node, overriding hard coded default
+   * values if necessary.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  parameter_overrides(const std::vector<rclcpp::Parameter> & parameter_overrides);
+
+  /// Append a single parameter override, parameter idiom style.
+  template<typename ParameterT>
+  NodeOptions &
+  append_parameter_override(const std::string & name, const ParameterT & value)
+  {
+    this->parameter_overrides().emplace_back(name, rclcpp::ParameterValue(value));
+    return *this;
+  }
+
+  /// Return the use_global_arguments flag.
+  RCLCPP_PUBLIC
+  bool
+  use_global_arguments() const;
+
+  /// Set the use_global_arguments flag, return this for parameter idiom.
+  /**
+   * If true this will cause the node's behavior to be influenced by "global"
+   * arguments, i.e. arguments not targeted at specific nodes, as well as the
+   * arguments targeted at the current node.
+   *
+   * This will cause the internal rcl_node_options_t struct to be invalidated.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  use_global_arguments(bool use_global_arguments);
+
+  /// Return the enable_rosout flag.
+  RCLCPP_PUBLIC
+  bool
+  enable_rosout() const;
+
+  /// Set the enable_rosout flag, return this for parameter idiom.
+  /**
+   * If false this will cause the node not to use rosout logging.
+   *
+   * Defaults to true for now, as there are still some cases where it is
+   * desirable.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  enable_rosout(bool enable_rosout);
+
+  /// Return the use_intra_process_comms flag.
+  RCLCPP_PUBLIC
+  bool
+  use_intra_process_comms() const;
+
+  /// Set the use_intra_process_comms flag, return this for parameter idiom.
+  /**
+   * If true, messages on topics which are published and subscribed to within
+   * this context will go through a special intra-process communication code
+   * code path which can avoid serialization and deserialization, unnecessary
+   * copies, and achieve lower latencies in some cases.
+   *
+   * Defaults to false for now, as there are still some cases where it is not
+   * desirable.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  use_intra_process_comms(bool use_intra_process_comms);
+
+  /// Return the enable_topic_statistics flag.
+  RCLCPP_PUBLIC
+  bool
+  enable_topic_statistics() const;
+
+  /// Set the enable_topic_statistics flag, return this for parameter idiom.
+  /**
+   * If true, topic statistics collection and publication will be enabled
+   * for all subscriptions.
+   * This can be used to override the global topic statistics setting.
+   *
+   * Defaults to false.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  enable_topic_statistics(bool enable_topic_statistics);
+
+  /// Return the start_parameter_services flag.
+  RCLCPP_PUBLIC
+  bool
+  start_parameter_services() const;
+
+  /// Set the start_parameter_services flag, return this for parameter idiom.
+  /**
+   * If true, ROS services are created to allow external nodes to list, get,
+   * and request to set parameters of this node.
+   *
+   * If false, parameters will still work locally, but will not be accessible
+   * remotely.
+   *
+   * \sa start_parameter_event_publisher()
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  start_parameter_services(bool start_parameter_services);
+
+  /// Return the start_parameter_event_publisher flag.
+  RCLCPP_PUBLIC
+  bool
+  start_parameter_event_publisher() const;
+
+  /// Set the start_parameter_event_publisher flag, return this for parameter idiom.
+  /**
+   * If true, a publisher is created on which an event message is published
+   * each time a parameter's state changes.
+   * This is used for recording and introspection, but is configurable
+   * separately from the other parameter services.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  start_parameter_event_publisher(bool start_parameter_event_publisher);
+
+  /// Return a reference to the parameter_event_qos QoS.
+  RCLCPP_PUBLIC
+  const rclcpp::QoS &
+  parameter_event_qos() const;
+
+  /// Set the parameter_event_qos QoS, return this for parameter idiom.
+  /**
+   * The QoS settings to be used for the parameter event publisher, if enabled.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  parameter_event_qos(const rclcpp::QoS & parameter_event_qos);
+
+  /// Return a reference to the parameter_event_publisher_options.
+  RCLCPP_PUBLIC
+  const rclcpp::PublisherOptionsBase &
+  parameter_event_publisher_options() const;
+
+  /// Set the parameter_event_publisher_options, return this for parameter idiom.
+  /**
+   * The QoS settings to be used for the parameter event publisher, if enabled.
+   *
+   * \todo(wjwwood): make this take/store an instance of
+   *   rclcpp::PublisherOptionsWithAllocator<Allocator>, but to do that requires
+   *   NodeOptions to also be templated based on the Allocator type.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  parameter_event_publisher_options(
+    const rclcpp::PublisherOptionsBase & parameter_event_publisher_options);
+
+  /// Return the allow_undeclared_parameters flag.
+  RCLCPP_PUBLIC
+  bool
+  allow_undeclared_parameters() const;
+
+  /// Set the allow_undeclared_parameters, return this for parameter idiom.
+  /**
+   * If true, allow any parameter name to be set on the node without first
+   * being declared.
+   * Otherwise, setting an undeclared parameter will raise an exception.
+   *
+   * This option being true does not affect parameter_overrides, as the first
+   * set action will implicitly declare the parameter and therefore consider
+   * any parameter overrides.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  allow_undeclared_parameters(bool allow_undeclared_parameters);
+
+  /// Return the automatically_declare_parameters_from_overrides flag.
+  RCLCPP_PUBLIC
+  bool
+  automatically_declare_parameters_from_overrides() const;
+
+  /// Set the automatically_declare_parameters_from_overrides, return this.
+  /**
+   * If true, automatically iterate through the node's parameter overrides and
+   * implicitly declare any that have not already been declared.
+   * Otherwise, parameters passed to the node's parameter_overrides, and/or the
+   * global arguments (e.g. parameter overrides from a YAML file), which are
+   * not explicitly declared will not appear on the node at all, even if
+   * `allow_undeclared_parameters` is true.
+   * Already declared parameters will not be re-declared, and parameters
+   * declared in this way will use the default constructed ParameterDescriptor.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  automatically_declare_parameters_from_overrides(
+    bool automatically_declare_parameters_from_overrides);
+
+  /// Return the rcl_allocator_t to be used.
+  RCLCPP_PUBLIC
+  const rcl_allocator_t &
+  allocator() const;
+
+  /// Set the rcl_allocator_t to be used, may cause deallocation of existing rcl_node_options_t.
+  /**
+   * This will cause the internal rcl_node_options_t struct to be invalidated.
+   */
+  RCLCPP_PUBLIC
+  NodeOptions &
+  allocator(rcl_allocator_t allocator);
+
+protected:
+  /// Retrieve the ROS_DOMAIN_ID environment variable and populate options.
+  size_t
+  get_domain_id_from_env() const;
+
+private:
+  // This is mutable to allow for a const accessor which lazily creates the node options instance.
+  /// Underlying rcl_node_options structure.
+  mutable std::unique_ptr<rcl_node_options_t, void (*)(rcl_node_options_t *)> node_options_;
+
+  // IMPORTANT: if any of these default values are changed, please update the
+  // documentation in this class.
+
+  rclcpp::Context::SharedPtr context_ {
+    rclcpp::contexts::get_global_default_context()};
+
+  std::vector<std::string> arguments_ {};
+
+  std::vector<rclcpp::Parameter> parameter_overrides_ {};
+
+  bool use_global_arguments_ {true};
+
+  bool enable_rosout_ {true};
+
+  bool use_intra_process_comms_ {false};
+
+  bool enable_topic_statistics_ {false};
+
+  bool start_parameter_services_ {true};
+
+  bool start_parameter_event_publisher_ {true};
+
+  rclcpp::QoS parameter_event_qos_ = rclcpp::ParameterEventsQoS(
+    rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)
+  );
+
+  rclcpp::PublisherOptionsBase parameter_event_publisher_options_ = rclcpp::PublisherOptionsBase();
+
+  bool allow_undeclared_parameters_ {false};
+
+  bool automatically_declare_parameters_from_overrides_ {false};
+
+  rcl_allocator_t allocator_ {rcl_get_default_allocator()};
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_OPTIONS_HPP_

rclcpp/include/rclcpp/parameter.hpp

@@ -12,326 +12,239 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_PARAMETER_HPP_
-#define RCLCPP_RCLCPP_PARAMETER_HPP_
+#ifndef RCLCPP__PARAMETER_HPP_
+#define RCLCPP__PARAMETER_HPP_
 
+#include <iostream>
 #include <ostream>
 #include <sstream>
 #include <string>
+#include <vector>
 
-#include <rmw/rmw.h>
-
-#include <rcl_interfaces/msg/parameter.hpp>
-#include <rcl_interfaces/msg/parameter_type.hpp>
-#include <rcl_interfaces/msg/parameter_value.hpp>
+#include "rcl_interfaces/msg/parameter.hpp"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/parameter_value.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
 
-namespace parameter
+class Parameter;
+
+namespace node_interfaces
+{
+struct ParameterInfo;
+}  // namespace node_interfaces
+
+namespace detail
 {
 
-enum ParameterType
-{
-  PARAMETER_NOT_SET=rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET,
-  PARAMETER_BOOL=rcl_interfaces::msg::ParameterType::PARAMETER_BOOL,
-  PARAMETER_INTEGER=rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER,
-  PARAMETER_DOUBLE=rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE,
-  PARAMETER_STRING=rcl_interfaces::msg::ParameterType::PARAMETER_STRING,
-  PARAMETER_BYTES=rcl_interfaces::msg::ParameterType::PARAMETER_BYTES,
-};
+// This helper function is required because you cannot do specialization on a
+// class method, so instead we specialize this template function and call it
+// from the unspecialized, but dependent, class method.
+template<typename T>
+auto
+get_value_helper(const rclcpp::Parameter * parameter);
 
-// Structure to store an arbitrary parameter with templated get/set methods
-class ParameterVariant
+}  // namespace detail
+
+/// Structure to store an arbitrary parameter with templated get/set methods.
+class Parameter
 {
 public:
-  ParameterVariant()
-  : name_("")
-  {
-    value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET;
-  }
-  explicit ParameterVariant(const std::string & name, const bool bool_value)
-  : name_(name)
-  {
-    value_.bool_value = bool_value;
-    value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
-  }
-  explicit ParameterVariant(const std::string & name, const int int_value)
-  : name_(name)
-  {
-    value_.integer_value = int_value;
-    value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
-  }
-  explicit ParameterVariant(const std::string & name, const int64_t int_value)
-  : name_(name)
-  {
-    value_.integer_value = int_value;
-    value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
-  }
-  explicit ParameterVariant(const std::string & name, const float double_value)
-  : name_(name)
-  {
-    value_.double_value = double_value;
-    value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
-  }
-  explicit ParameterVariant(const std::string & name, const double double_value)
-  : name_(name)
-  {
-    value_.double_value = double_value;
-    value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
-  }
-  explicit ParameterVariant(const std::string & name, const std::string & string_value)
-  : name_(name)
-  {
-    value_.string_value = string_value;
-    value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
-  }
-  explicit ParameterVariant(const std::string & name, const char * string_value)
-  : ParameterVariant(name, std::string(string_value)) {}
-  explicit ParameterVariant(const std::string & name, const std::vector<uint8_t> & bytes_value)
-  : name_(name)
-  {
-    value_.bytes_value = bytes_value;
-    value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BYTES;
-  }
+  /// Construct with an empty name and a parameter value of type rclcpp::PARAMETER_NOT_SET.
+  RCLCPP_PUBLIC
+  Parameter();
 
-  inline ParameterType get_type() const {return static_cast<ParameterType>(value_.type); }
+  /// Construct with given name and a parameter value of type rclcpp::PARAMETER_NOT_SET.
+  RCLCPP_PUBLIC
+  explicit Parameter(const std::string & name);
 
-  inline std::string get_type_name() const
-  {
-    switch (get_type()) {
-      case rclcpp::parameter::ParameterType::PARAMETER_BOOL:
-        return "bool";
-      case rclcpp::parameter::ParameterType::PARAMETER_INTEGER:
-        return "integer";
-      case rclcpp::parameter::ParameterType::PARAMETER_DOUBLE:
-        return "double";
-      case rclcpp::parameter::ParameterType::PARAMETER_STRING:
-        return "string";
-      case rclcpp::parameter::ParameterType::PARAMETER_BYTES:
-        return "bytes";
-      case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET:
-        return "not set";
-      default:
-        // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-        throw std::runtime_error(
-          "Unexpected type from ParameterVariant: " + std::to_string(get_type()));
-        // *INDENT-ON*
-    }
-  }
+  /// Construct with given name and given parameter value.
+  RCLCPP_PUBLIC
+  Parameter(const std::string & name, const ParameterValue & value);
 
-  inline std::string get_name() const & {return name_; }
+  /// Construct with given name and given parameter value.
+  template<typename ValueTypeT>
+  Parameter(const std::string & name, ValueTypeT value)
+  : Parameter(name, ParameterValue(value))
+  {}
 
-  inline rcl_interfaces::msg::ParameterValue get_parameter_value() const
-  {
-    return value_;
-  }
+  RCLCPP_PUBLIC
+  explicit Parameter(const rclcpp::node_interfaces::ParameterInfo & parameter_info);
 
-  template<ParameterType type>
-  typename std::enable_if<type == ParameterType::PARAMETER_INTEGER, int64_t>::type
+  /// Equal operator.
+  RCLCPP_PUBLIC
+  bool
+  operator==(const Parameter & rhs) const;
+
+  /// Not equal operator.
+  RCLCPP_PUBLIC
+  bool
+  operator!=(const Parameter & rhs) const;
+
+  RCLCPP_PUBLIC
+  ParameterType
+  get_type() const;
+
+  RCLCPP_PUBLIC
+  std::string
+  get_type_name() const;
+
+  RCLCPP_PUBLIC
+  const std::string &
+  get_name() const;
+
+  RCLCPP_PUBLIC
+  rcl_interfaces::msg::ParameterValue
+  get_value_message() const;
+
+  /// Get the internal storage for the parameter value.
+  RCLCPP_PUBLIC
+  const rclcpp::ParameterValue &
+  get_parameter_value() const;
+
+  /// Get value of parameter using rclcpp::ParameterType as template argument.
+  template<ParameterType ParamT>
+  decltype(auto)
   get_value() const
   {
-    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) {
-      // TODO: use custom exception
-      throw std::runtime_error("Invalid type");
-    }
-    return value_.integer_value;
-  }
-  template<ParameterType type>
-  typename std::enable_if<type == ParameterType::PARAMETER_DOUBLE, double>::type
-  get_value() const
-  {
-    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) {
-      // TODO: use custom exception
-      throw std::runtime_error("Invalid type");
-    }
-    return value_.double_value;
-  }
-  template<ParameterType type>
-  typename std::enable_if<type == ParameterType::PARAMETER_STRING, const std::string &>::type
-  get_value() const
-  {
-    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_STRING) {
-      // TODO: use custom exception
-      throw std::runtime_error("Invalid type");
-    }
-    return value_.string_value;
-  }
-  template<ParameterType type>
-  typename std::enable_if<type == ParameterType::PARAMETER_BOOL, bool>::type
-  get_value() const
-  {
-    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) {
-      // TODO: use custom exception
-      throw std::runtime_error("Invalid type");
-    }
-    return value_.bool_value;
-  }
-  template<ParameterType type>
-  typename std::enable_if<type == ParameterType::PARAMETER_BYTES,
-  const std::vector<uint8_t> &>::type
-  get_value() const
-  {
-    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BYTES) {
-      // TODO: use custom exception
-      throw std::runtime_error("Invalid type");
-    }
-    return value_.bytes_value;
+    return value_.get<ParamT>();
   }
 
-  int64_t as_int() const {return get_value<ParameterType::PARAMETER_INTEGER>(); }
+  /// Get value of parameter using c++ types as template argument.
+  template<typename T>
+  decltype(auto)
+  get_value() const;
 
-  double as_double() const {return get_value<ParameterType::PARAMETER_DOUBLE>(); }
+  RCLCPP_PUBLIC
+  bool
+  as_bool() const;
 
-  const std::string & as_string() const {return get_value<ParameterType::PARAMETER_STRING>(); }
+  RCLCPP_PUBLIC
+  int64_t
+  as_int() const;
 
-  bool as_bool() const {return get_value<ParameterType::PARAMETER_BOOL>(); }
+  RCLCPP_PUBLIC
+  double
+  as_double() const;
 
-  const std::vector<uint8_t> & as_bytes() const
-  {
-    return get_value<ParameterType::PARAMETER_BYTES>();
-  }
+  RCLCPP_PUBLIC
+  const std::string &
+  as_string() const;
 
-  static ParameterVariant from_parameter(const rcl_interfaces::msg::Parameter & parameter)
-  {
-    switch (parameter.value.type) {
-      case PARAMETER_BOOL:
-        return ParameterVariant(parameter.name, parameter.value.bool_value);
-      case PARAMETER_INTEGER:
-        return ParameterVariant(parameter.name, parameter.value.integer_value);
-      case PARAMETER_DOUBLE:
-        return ParameterVariant(parameter.name, parameter.value.double_value);
-      case PARAMETER_STRING:
-        return ParameterVariant(parameter.name, parameter.value.string_value);
-      case PARAMETER_BYTES:
-        return ParameterVariant(parameter.name, parameter.value.bytes_value);
-      case PARAMETER_NOT_SET:
-        throw std::runtime_error("Type from ParameterValue is not set");
-      default:
-        // TODO(wjwwood): use custom exception
-        // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-        throw std::runtime_error(
-          "Unexpected type from ParameterVariant: " + std::to_string(parameter.value.type));
-        // *INDENT-ON*
-    }
-  }
+  RCLCPP_PUBLIC
+  const std::vector<uint8_t> &
+  as_byte_array() const;
 
-  rcl_interfaces::msg::Parameter to_parameter()
-  {
-    rcl_interfaces::msg::Parameter parameter;
-    parameter.name = name_;
-    parameter.value = value_;
-    return parameter;
-  }
+  RCLCPP_PUBLIC
+  const std::vector<bool> &
+  as_bool_array() const;
 
-  std::string value_to_string() const
-  {
-    switch (get_type()) {
-      case rclcpp::parameter::ParameterType::PARAMETER_BOOL:
-        return as_bool() ? "true" : "false";
-      case rclcpp::parameter::ParameterType::PARAMETER_INTEGER:
-        return std::to_string(as_int());
-      case rclcpp::parameter::ParameterType::PARAMETER_DOUBLE:
-        return std::to_string(as_double());
-      case rclcpp::parameter::ParameterType::PARAMETER_STRING:
-        return as_string();
-      case rclcpp::parameter::ParameterType::PARAMETER_BYTES:
-        {
-          std::stringstream bytes;
-          bool first_byte = true;
-          bytes << "[" << std::hex;
-          for (auto & byte : as_bytes()) {
-            bytes << "0x" << byte;
-            if (!first_byte) {
-              bytes << ", ";
-            } else {
-              first_byte = false;
-            }
-          }
-          return bytes.str();
-        }
-      case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET:
-        return "not set";
-      default:
-        // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-        throw std::runtime_error(
-          "Unexpected type from ParameterVariant: " + std::to_string(get_type()));
-        // *INDENT-ON*
-    }
-  }
+  RCLCPP_PUBLIC
+  const std::vector<int64_t> &
+  as_integer_array() const;
+
+  RCLCPP_PUBLIC
+  const std::vector<double> &
+  as_double_array() const;
+
+  RCLCPP_PUBLIC
+  const std::vector<std::string> &
+  as_string_array() const;
+
+  RCLCPP_PUBLIC
+  static Parameter
+  from_parameter_msg(const rcl_interfaces::msg::Parameter & parameter);
+
+  RCLCPP_PUBLIC
+  rcl_interfaces::msg::Parameter
+  to_parameter_msg() const;
+
+  RCLCPP_PUBLIC
+  std::string
+  value_to_string() const;
 
 private:
   std::string name_;
-  rcl_interfaces::msg::ParameterValue value_;
+  ParameterValue value_;
 };
 
+/// Return a json encoded version of the parameter intended for a dict.
+RCLCPP_PUBLIC
+std::string
+_to_json_dict_entry(const Parameter & param);
 
-/* Return a json encoded version of the parameter intended for a dict. */
-std::string _to_json_dict_entry(const ParameterVariant & param)
+RCLCPP_PUBLIC
+std::ostream &
+operator<<(std::ostream & os, const rclcpp::Parameter & pv);
+
+RCLCPP_PUBLIC
+std::ostream &
+operator<<(std::ostream & os, const std::vector<Parameter> & parameters);
+
+namespace detail
 {
-  std::stringstream ss;
-  ss << "\"" << param.get_name() << "\": ";
-  ss << "{\"type\": \"" << param.get_type_name() << "\", ";
-  ss << "\"value\": \"" << param.value_to_string() << "\"}";
-  return ss.str();
+
+template<typename T>
+auto
+get_value_helper(const rclcpp::Parameter * parameter)
+{
+  return parameter->get_parameter_value().get<T>();
 }
 
+// Specialization allowing Parameter::get() to return a const ref to the parameter value object.
+template<>
+inline
+auto
+get_value_helper<rclcpp::ParameterValue>(const rclcpp::Parameter * parameter)
+{
+  return parameter->get_parameter_value();
+}
 
-} /* namespace parameter */
+// Specialization allowing Parameter::get() to return a const ref to the parameter itself.
+template<>
+inline
+auto
+get_value_helper<rclcpp::Parameter>(const rclcpp::Parameter * parameter)
+{
+  // Use this lambda to ensure it's a const reference being returned (and not a copy).
+  auto type_enforcing_lambda =
+    [&parameter]() -> const rclcpp::Parameter & {
+      return *parameter;
+    };
+  return type_enforcing_lambda();
+}
 
-} /* namespace rclcpp */
+}  // namespace detail
+
+template<typename T>
+decltype(auto)
+Parameter::get_value() const
+{
+  try {
+    // use the helper to specialize for the ParameterValue and Parameter cases.
+    return detail::get_value_helper<T>(this);
+  } catch (const ParameterTypeException & ex) {
+    throw exceptions::InvalidParameterTypeException(this->name_, ex.what());
+  }
+}
+
+}  // namespace rclcpp
 
 namespace std
 {
-/* Return a json encoded version of the parameter intended for a list. */
-inline std::string to_string(const rclcpp::parameter::ParameterVariant & param)
-{
-  std::stringstream ss;
-  ss << "{\"name\": \"" << param.get_name() << "\", ";
-  ss << "\"type\": \"" << param.get_type_name() << "\", ";
-  ss << "\"value\": \"" << param.value_to_string() << "\"}";
-  return ss.str();
-}
 
-/* Return a json encoded version of a vector of parameters, as a string*/
-inline std::string to_string(const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
-{
-  std::stringstream ss;
-  ss << "{";
-  bool first = true;
-  for (const auto & pv : parameters) {
-    if (first == false) {
-      ss << ", ";
-    } else {
-      first = false;
-    }
-    ss << rclcpp::parameter::_to_json_dict_entry(pv);
-  }
-  ss << "}";
-  return ss.str();
-}
+/// Return a json encoded version of the parameter intended for a list.
+RCLCPP_PUBLIC
+std::string
+to_string(const rclcpp::Parameter & param);
 
-} /* namespace std */
+/// Return a json encoded version of a vector of parameters, as a string.
+RCLCPP_PUBLIC
+std::string
+to_string(const std::vector<rclcpp::Parameter> & parameters);
 
-namespace rclcpp
-{
-namespace parameter
-{
+}  // namespace std
 
-std::ostream & operator<<(std::ostream & os, const rclcpp::parameter::ParameterVariant & pv)
-{
-  os << std::to_string(pv);
-  return os;
-}
-
-std::ostream & operator<<(std::ostream & os, const std::vector<ParameterVariant> & parameters)
-{
-  os << std::to_string(parameters);
-  return os;
-}
-
-} /* namespace parameter */
-
-} /* namespace rclcpp */
-
-#endif /* RCLCPP_RCLCPP_PARAMETER_HPP_ */
+#endif  // RCLCPP__PARAMETER_HPP_

rclcpp/include/rclcpp/parameter_client.hpp

@@ -12,239 +12,182 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_PARAMETER_CLIENT_HPP_
-#define RCLCPP_RCLCPP_PARAMETER_CLIENT_HPP_
+#ifndef RCLCPP__PARAMETER_CLIENT_HPP_
+#define RCLCPP__PARAMETER_CLIENT_HPP_
 
+#include <memory>
 #include <string>
+#include <utility>
+#include <vector>
 
-#include <rmw/rmw.h>
-
-#include <rclcpp/executors.hpp>
-#include <rclcpp/macros.hpp>
-#include <rclcpp/node.hpp>
-#include <rclcpp/parameter.hpp>
-
-#include <rcl_interfaces/msg/parameter.hpp>
-#include <rcl_interfaces/msg/parameter_event.hpp>
-#include <rcl_interfaces/msg/parameter_value.hpp>
-#include <rcl_interfaces/srv/describe_parameters.hpp>
-#include <rcl_interfaces/srv/get_parameters.hpp>
-#include <rcl_interfaces/srv/get_parameter_types.hpp>
-#include <rcl_interfaces/srv/list_parameters.hpp>
-#include <rcl_interfaces/srv/set_parameters.hpp>
-#include <rcl_interfaces/srv/set_parameters_atomically.hpp>
+#include "rcl_interfaces/msg/parameter.hpp"
+#include "rcl_interfaces/msg/parameter_event.hpp"
+#include "rcl_interfaces/msg/parameter_value.hpp"
+#include "rcl_interfaces/srv/describe_parameters.hpp"
+#include "rcl_interfaces/srv/get_parameter_types.hpp"
+#include "rcl_interfaces/srv/get_parameters.hpp"
+#include "rcl_interfaces/srv/list_parameters.hpp"
+#include "rcl_interfaces/srv/set_parameters.hpp"
+#include "rcl_interfaces/srv/set_parameters_atomically.hpp"
+#include "rclcpp/executors.hpp"
+#include "rclcpp/create_subscription.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/type_support_decl.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rmw/rmw.h"
 
 namespace rclcpp
 {
 
-namespace parameter_client
-{
-
 class AsyncParametersClient
 {
-
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(AsyncParametersClient);
+  RCLCPP_SMART_PTR_DEFINITIONS(AsyncParametersClient)
 
-  AsyncParametersClient(const rclcpp::node::Node::SharedPtr node,
-    const std::string & remote_node_name = "")
-  : node_(node)
-  {
-    if (remote_node_name != "") {
-      remote_node_name_ = remote_node_name;
-    } else {
-      remote_node_name_ = node_->get_name();
-    }
-    get_parameters_client_ = node_->create_client<rcl_interfaces::srv::GetParameters>(
-      remote_node_name_ + "__get_parameters");
-    get_parameter_types_client_ = node_->create_client<rcl_interfaces::srv::GetParameterTypes>(
-      remote_node_name_ + "__get_parameter_types");
-    set_parameters_client_ = node_->create_client<rcl_interfaces::srv::SetParameters>(
-      remote_node_name_ + "__set_parameters");
-    list_parameters_client_ = node_->create_client<rcl_interfaces::srv::ListParameters>(
-      remote_node_name_ + "__list_parameters");
-    describe_parameters_client_ = node_->create_client<rcl_interfaces::srv::DescribeParameters>(
-      remote_node_name_ + "__describe_parameters");
-  }
+  RCLCPP_PUBLIC
+  AsyncParametersClient(
+    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
+    const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
+    const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
+    const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
+    const std::string & remote_node_name = "",
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters,
+    rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
-  std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>
+  RCLCPP_PUBLIC
+  AsyncParametersClient(
+    const rclcpp::Node::SharedPtr node,
+    const std::string & remote_node_name = "",
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters,
+    rclcpp::CallbackGroup::SharedPtr group = nullptr);
+
+  RCLCPP_PUBLIC
+  AsyncParametersClient(
+    rclcpp::Node * node,
+    const std::string & remote_node_name = "",
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters,
+    rclcpp::CallbackGroup::SharedPtr group = nullptr);
+
+  RCLCPP_PUBLIC
+  std::shared_future<std::vector<rclcpp::Parameter>>
   get_parameters(
     const std::vector<std::string> & names,
-    std::function<void(
-      std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>)> callback = nullptr)
-  {
-    auto promise_result =
-      std::make_shared<std::promise<std::vector<rclcpp::parameter::ParameterVariant>>>();
-    auto future_result = promise_result->get_future().share();
+    std::function<
+      void(std::shared_future<std::vector<rclcpp::Parameter>>)
+    > callback = nullptr);
 
-    auto request = std::make_shared<rcl_interfaces::srv::GetParameters::Request>();
-    request->names = names;
-
-    get_parameters_client_->async_send_request(
-      request,
-      [request, promise_result, future_result, &callback](
-        rclcpp::client::Client<rcl_interfaces::srv::GetParameters>::SharedFuture cb_f) {
-          std::vector<rclcpp::parameter::ParameterVariant> parameter_variants;
-          auto & pvalues = cb_f.get()->values;
-
-          for (auto & pvalue : pvalues) {
-            auto i = &pvalue - &pvalues[0];
-            rcl_interfaces::msg::Parameter parameter;
-            parameter.name = request->names[i];
-            parameter.value = pvalue;
-            parameter_variants.push_back(rclcpp::parameter::ParameterVariant::from_parameter(
-              parameter));
-          }
-
-          promise_result->set_value(parameter_variants);
-          if (callback != nullptr) {
-            callback(future_result);
-          }
-        }
-    );
-
-    return future_result;
-  }
-
-  std::shared_future<std::vector<rclcpp::parameter::ParameterType>>
+  RCLCPP_PUBLIC
+  std::shared_future<std::vector<rclcpp::ParameterType>>
   get_parameter_types(
     const std::vector<std::string> & names,
-    std::function<void(
-      std::shared_future<std::vector<rclcpp::parameter::ParameterType>>)> callback = nullptr)
-  {
-    auto promise_result =
-      std::make_shared<std::promise<std::vector<rclcpp::parameter::ParameterType>>>();
-    auto future_result = promise_result->get_future().share();
-
-    auto request = std::make_shared<rcl_interfaces::srv::GetParameterTypes::Request>();
-    request->names = names;
-
-    get_parameter_types_client_->async_send_request(
-      request,
-      [promise_result, future_result, &callback](
-        rclcpp::client::Client<rcl_interfaces::srv::GetParameterTypes>::SharedFuture cb_f) {
-          std::vector<rclcpp::parameter::ParameterType> types;
-          auto & pts = cb_f.get()->types;
-          for (auto & pt : pts) {
-            pts.push_back(static_cast<rclcpp::parameter::ParameterType>(pt));
-          }
-          promise_result->set_value(types);
-          if (callback != nullptr) {
-            callback(future_result);
-          }
-        }
-    );
-
-    return future_result;
-  }
+    std::function<
+      void(std::shared_future<std::vector<rclcpp::ParameterType>>)
+    > callback = nullptr);
 
+  RCLCPP_PUBLIC
   std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>
   set_parameters(
-    const std::vector<rclcpp::parameter::ParameterVariant> & parameters,
-    std::function<void(std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>)> callback =
-    nullptr)
-  {
-    auto promise_result =
-      std::make_shared<std::promise<std::vector<rcl_interfaces::msg::SetParametersResult>>>();
-    auto future_result = promise_result->get_future().share();
-
-    auto request = std::make_shared<rcl_interfaces::srv::SetParameters::Request>();
-
-    std::transform(parameters.begin(), parameters.end(), std::back_inserter(
-        request->parameters), [](
-        rclcpp::parameter::ParameterVariant p) {return p.to_parameter(); });
-
-    set_parameters_client_->async_send_request(
-      request,
-      [promise_result, future_result, &callback](
-        rclcpp::client::Client<rcl_interfaces::srv::SetParameters>::SharedFuture cb_f) {
-          promise_result->set_value(cb_f.get()->results);
-          if (callback != nullptr) {
-            callback(future_result);
-          }
-        }
-    );
-
-    return future_result;
-  }
+    const std::vector<rclcpp::Parameter> & parameters,
+    std::function<
+      void(std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>)
+    > callback = nullptr);
 
+  RCLCPP_PUBLIC
   std::shared_future<rcl_interfaces::msg::SetParametersResult>
   set_parameters_atomically(
-    const std::vector<rclcpp::parameter::ParameterVariant> & parameters,
-    std::function<void(std::shared_future<rcl_interfaces::msg::SetParametersResult>)> callback =
-    nullptr)
-  {
-    auto promise_result =
-      std::make_shared<std::promise<rcl_interfaces::msg::SetParametersResult>>();
-    auto future_result = promise_result->get_future().share();
-
-    auto request = std::make_shared<rcl_interfaces::srv::SetParametersAtomically::Request>();
-
-    std::transform(parameters.begin(), parameters.end(), std::back_inserter(
-        request->parameters), [](
-        rclcpp::parameter::ParameterVariant p) {return p.to_parameter(); });
-
-    set_parameters_atomically_client_->async_send_request(
-      request,
-      [promise_result, future_result, &callback](
-        rclcpp::client::Client<rcl_interfaces::srv::SetParametersAtomically>::SharedFuture cb_f) {
-          promise_result->set_value(cb_f.get()->result);
-          if (callback != nullptr) {
-            callback(future_result);
-          }
-        }
-    );
-
-    return future_result;
-  }
+    const std::vector<rclcpp::Parameter> & parameters,
+    std::function<
+      void(std::shared_future<rcl_interfaces::msg::SetParametersResult>)
+    > callback = nullptr);
 
+  RCLCPP_PUBLIC
   std::shared_future<rcl_interfaces::msg::ListParametersResult>
   list_parameters(
     const std::vector<std::string> & prefixes,
     uint64_t depth,
-    std::function<void(std::shared_future<rcl_interfaces::msg::ListParametersResult>)> callback =
-    nullptr)
+    std::function<
+      void(std::shared_future<rcl_interfaces::msg::ListParametersResult>)
+    > callback = nullptr);
+
+  template<
+    typename CallbackT,
+    typename AllocatorT = std::allocator<void>>
+  typename rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
+  on_parameter_event(
+    CallbackT && callback,
+    const rclcpp::QoS & qos = (
+      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events))
+    ),
+    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
+      rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
+  ))
   {
-    auto promise_result =
-      std::make_shared<std::promise<rcl_interfaces::msg::ListParametersResult>>();
-    auto future_result = promise_result->get_future().share();
+    return this->on_parameter_event(
+      this->node_topics_interface_,
+      callback,
+      qos,
+      options);
+  }
 
-    auto request = std::make_shared<rcl_interfaces::srv::ListParameters::Request>();
-    request->prefixes = prefixes;
-    request->depth = depth;
+  /**
+   * The NodeT type only needs to have a method called get_node_topics_interface()
+   * which returns a shared_ptr to a NodeTopicsInterface, or be a
+   * NodeTopicsInterface pointer itself.
+   */
+  template<
+    typename CallbackT,
+    typename NodeT,
+    typename AllocatorT = std::allocator<void>>
+  static typename rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
+  on_parameter_event(
+    NodeT && node,
+    CallbackT && callback,
+    const rclcpp::QoS & qos = (
+      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events))
+    ),
+    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
+      rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
+  ))
+  {
+    return rclcpp::create_subscription<rcl_interfaces::msg::ParameterEvent>(
+      node,
+      "parameter_events",
+      qos,
+      std::forward<CallbackT>(callback),
+      options);
+  }
 
-    list_parameters_client_->async_send_request(
-      request,
-      [promise_result, future_result, &callback](
-        rclcpp::client::Client<rcl_interfaces::srv::ListParameters>::SharedFuture cb_f) {
-          promise_result->set_value(cb_f.get()->result);
-          if (callback != nullptr) {
-            callback(future_result);
-          }
-        }
+  RCLCPP_PUBLIC
+  bool
+  service_is_ready() const;
+
+  template<typename RepT = int64_t, typename RatioT = std::milli>
+  bool
+  wait_for_service(
+    std::chrono::duration<RepT, RatioT> timeout = std::chrono::duration<RepT, RatioT>(-1))
+  {
+    return wait_for_service_nanoseconds(
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
     );
-
-    return future_result;
   }
 
-  template<typename FunctorT>
-  typename rclcpp::subscription::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
-  on_parameter_event(FunctorT callback)
-  {
-    return node_->create_subscription<rcl_interfaces::msg::ParameterEvent>(
-      "parameter_events", callback, rmw_qos_profile_parameter_events);
-  }
+protected:
+  RCLCPP_PUBLIC
+  bool
+  wait_for_service_nanoseconds(std::chrono::nanoseconds timeout);
 
 private:
-  const rclcpp::node::Node::SharedPtr node_;
-  rclcpp::client::Client<rcl_interfaces::srv::GetParameters>::SharedPtr get_parameters_client_;
-  rclcpp::client::Client<rcl_interfaces::srv::GetParameterTypes>::SharedPtr
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface_;
+  rclcpp::Client<rcl_interfaces::srv::GetParameters>::SharedPtr get_parameters_client_;
+  rclcpp::Client<rcl_interfaces::srv::GetParameterTypes>::SharedPtr
     get_parameter_types_client_;
-  rclcpp::client::Client<rcl_interfaces::srv::SetParameters>::SharedPtr set_parameters_client_;
-  rclcpp::client::Client<rcl_interfaces::srv::SetParametersAtomically>::SharedPtr
+  rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedPtr set_parameters_client_;
+  rclcpp::Client<rcl_interfaces::srv::SetParametersAtomically>::SharedPtr
     set_parameters_atomically_client_;
-  rclcpp::client::Client<rcl_interfaces::srv::ListParameters>::SharedPtr list_parameters_client_;
-  rclcpp::client::Client<rcl_interfaces::srv::DescribeParameters>::SharedPtr
+  rclcpp::Client<rcl_interfaces::srv::ListParameters>::SharedPtr list_parameters_client_;
+  rclcpp::Client<rcl_interfaces::srv::DescribeParameters>::SharedPtr
     describe_parameters_client_;
   std::string remote_node_name_;
 };
@@ -252,108 +195,155 @@ private:
 class SyncParametersClient
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(SyncParametersClient);
+  RCLCPP_SMART_PTR_DEFINITIONS(SyncParametersClient)
 
+  RCLCPP_PUBLIC
+  explicit SyncParametersClient(
+    rclcpp::Node::SharedPtr node,
+    const std::string & remote_node_name = "",
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
+
+  RCLCPP_PUBLIC
   SyncParametersClient(
-    rclcpp::node::Node::SharedPtr node)
-  : node_(node)
-  {
-    executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
-    async_parameters_client_ = std::make_shared<AsyncParametersClient>(node);
-  }
+    rclcpp::Executor::SharedPtr executor,
+    rclcpp::Node::SharedPtr node,
+    const std::string & remote_node_name = "",
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
 
+  RCLCPP_PUBLIC
+  explicit SyncParametersClient(
+    rclcpp::Node * node,
+    const std::string & remote_node_name = "",
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
+
+  RCLCPP_PUBLIC
   SyncParametersClient(
-    rclcpp::executor::Executor::SharedPtr executor,
-    rclcpp::node::Node::SharedPtr node)
-  : executor_(executor), node_(node)
-  {
-    async_parameters_client_ = std::make_shared<AsyncParametersClient>(node);
-  }
+    rclcpp::Executor::SharedPtr executor,
+    rclcpp::Node * node,
+    const std::string & remote_node_name = "",
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
 
-  std::vector<rclcpp::parameter::ParameterVariant>
-  get_parameters(const std::vector<std::string> & parameter_names)
+  RCLCPP_PUBLIC
+  SyncParametersClient(
+    rclcpp::Executor::SharedPtr executor,
+    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
+    const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
+    const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
+    const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
+    const std::string & remote_node_name = "",
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
+
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::Parameter>
+  get_parameters(const std::vector<std::string> & parameter_names);
+
+  RCLCPP_PUBLIC
+  bool
+  has_parameter(const std::string & parameter_name);
+
+  template<typename T>
+  T
+  get_parameter_impl(
+    const std::string & parameter_name, std::function<T()> parameter_not_found_handler)
   {
-    auto f = async_parameters_client_->get_parameters(parameter_names);
-    if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
-      rclcpp::executors::FutureReturnCode::SUCCESS)
-    {
-      return f.get();
+    std::vector<std::string> names;
+    names.push_back(parameter_name);
+    auto vars = get_parameters(names);
+    if ((vars.size() != 1) || (vars[0].get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET)) {
+      return parameter_not_found_handler();
+    } else {
+      return static_cast<T>(vars[0].get_value<T>());
     }
-    // Return an empty vector if unsuccessful
-    return std::vector<rclcpp::parameter::ParameterVariant>();
   }
 
-  std::vector<rclcpp::parameter::ParameterType>
-  get_parameter_types(const std::vector<std::string> & parameter_names)
+  template<typename T>
+  T
+  get_parameter(const std::string & parameter_name, const T & default_value)
   {
-    auto f = async_parameters_client_->get_parameter_types(parameter_names);
-
-    if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
-      rclcpp::executors::FutureReturnCode::SUCCESS)
-    {
-      return f.get();
-    }
-    return std::vector<rclcpp::parameter::ParameterType>();
+    return get_parameter_impl(
+      parameter_name,
+      std::function<T()>([&default_value]() -> T {return default_value;}));
   }
 
+  template<typename T>
+  T
+  get_parameter(const std::string & parameter_name)
+  {
+    return get_parameter_impl(
+      parameter_name,
+      std::function<T()>(
+        [&parameter_name]() -> T
+        {
+          throw std::runtime_error("Parameter '" + parameter_name + "' is not set");
+        })
+    );
+  }
+
+  RCLCPP_PUBLIC
+  std::vector<rclcpp::ParameterType>
+  get_parameter_types(const std::vector<std::string> & parameter_names);
+
+  RCLCPP_PUBLIC
   std::vector<rcl_interfaces::msg::SetParametersResult>
-  set_parameters(const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
-  {
-    auto f = async_parameters_client_->set_parameters(parameters);
-
-    if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
-      rclcpp::executors::FutureReturnCode::SUCCESS)
-    {
-      return f.get();
-    }
-    return std::vector<rcl_interfaces::msg::SetParametersResult>();
-  }
+  set_parameters(const std::vector<rclcpp::Parameter> & parameters);
 
+  RCLCPP_PUBLIC
   rcl_interfaces::msg::SetParametersResult
-  set_parameters_atomically(const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
-  {
-    auto f = async_parameters_client_->set_parameters_atomically(parameters);
-
-    if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
-      rclcpp::executors::FutureReturnCode::SUCCESS)
-    {
-      return f.get();
-    }
-
-    throw std::runtime_error("Unable to get result of set parameters service call.");
-  }
+  set_parameters_atomically(const std::vector<rclcpp::Parameter> & parameters);
 
+  RCLCPP_PUBLIC
   rcl_interfaces::msg::ListParametersResult
   list_parameters(
     const std::vector<std::string> & parameter_prefixes,
-    uint64_t depth)
+    uint64_t depth);
+
+  template<typename CallbackT>
+  typename rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
+  on_parameter_event(CallbackT && callback)
   {
-    auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth);
-
-    if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
-      rclcpp::executors::FutureReturnCode::SUCCESS)
-    {
-      return f.get();
-    }
-
-    throw std::runtime_error("Unable to get result of list parameters service call.");
+    return async_parameters_client_->on_parameter_event(
+      std::forward<CallbackT>(callback));
   }
 
-  template<typename FunctorT>
-  typename rclcpp::subscription::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
-  on_parameter_event(FunctorT callback)
+  /**
+   * The NodeT type only needs to have a method called get_node_topics_interface()
+   * which returns a shared_ptr to a NodeTopicsInterface, or be a
+   * NodeTopicsInterface pointer itself.
+   */
+  template<
+    typename CallbackT,
+    typename NodeT>
+  static typename rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
+  on_parameter_event(
+    NodeT && node,
+    CallbackT && callback)
   {
-    return async_parameters_client_->on_parameter_event(callback);
+    return AsyncParametersClient::on_parameter_event(
+      node,
+      std::forward<CallbackT>(callback));
+  }
+
+  RCLCPP_PUBLIC
+  bool
+  service_is_ready() const
+  {
+    return async_parameters_client_->service_is_ready();
+  }
+
+  template<typename RepT = int64_t, typename RatioT = std::milli>
+  bool
+  wait_for_service(
+    std::chrono::duration<RepT, RatioT> timeout = std::chrono::duration<RepT, RatioT>(-1))
+  {
+    return async_parameters_client_->wait_for_service(timeout);
   }
 
 private:
-  rclcpp::executor::Executor::SharedPtr executor_;
-  rclcpp::node::Node::SharedPtr node_;
+  rclcpp::Executor::SharedPtr executor_;
+  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_;
   AsyncParametersClient::SharedPtr async_parameters_client_;
 };
 
-} /* namespace parameter_client */
+}  // namespace rclcpp
 
-} /* namespace rclcpp */
-
-#endif /* RCLCPP_RCLCPP_PARAMETER_CLIENT_HPP_ */
+#endif  // RCLCPP__PARAMETER_CLIENT_HPP_

rclcpp/include/rclcpp/parameter_events_filter.hpp

@@ -0,0 +1,82 @@
+// Copyright 2017 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__PARAMETER_EVENTS_FILTER_HPP_
+#define RCLCPP__PARAMETER_EVENTS_FILTER_HPP_
+
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "rcl_interfaces/msg/parameter.hpp"
+#include "rcl_interfaces/msg/parameter_event.hpp"
+#include "rcl_interfaces/msg/parameter_value.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rmw/rmw.h"
+
+namespace rclcpp
+{
+
+class ParameterEventsFilter
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(ParameterEventsFilter)
+  enum class EventType {NEW, DELETED, CHANGED};  ///< An enum for the type of event.
+  /// Used for the listed results
+  using EventPair = std::pair<EventType, rcl_interfaces::msg::Parameter *>;
+
+  /// Construct a filtered view of a parameter event.
+  /**
+   * \param[in] event The parameter event message to filter.
+   * \param[in] names A list of parameter names of interest.
+   * \param[in] types A list of the types of parameter events of iterest.
+   *    EventType NEW, DELETED, or CHANGED
+   *
+   * Example Usage:
+   *
+   * If you have recieved a parameter event and are only interested in parameters foo and
+   * bar being added or changed but don't care about deletion.
+   *
+   * ```cpp
+   * auto res = rclcpp::ParameterEventsFilter(
+   *   event_shared_ptr,
+   *   {"foo", "bar"},
+   *   {rclcpp::ParameterEventsFilter::EventType::NEW, rclcpp::ParameterEventsFilter::EventType::CHANGED});
+   * ```
+   */
+  RCLCPP_PUBLIC
+  ParameterEventsFilter(
+    rcl_interfaces::msg::ParameterEvent::SharedPtr event,
+    const std::vector<std::string> & names,
+    const std::vector<EventType> & types);
+
+  /// Get the result of the filter
+  /**
+   * \return A std::vector<EventPair> of all matching parameter changes in this event.
+   */
+  RCLCPP_PUBLIC
+  const std::vector<EventPair> & get_events() const;
+
+private:
+  // access only allowed via const accessor.
+  std::vector<EventPair> result_;  ///< Storage of the resultant vector
+  rcl_interfaces::msg::ParameterEvent::SharedPtr event_;  ///< Keep event in scope
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__PARAMETER_EVENTS_FILTER_HPP_

rclcpp/include/rclcpp/parameter_map.hpp

@@ -0,0 +1,53 @@
+// Copyright 2018 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__PARAMETER_MAP_HPP_
+#define RCLCPP__PARAMETER_MAP_HPP_
+
+#include <rcl_yaml_param_parser/types.h>
+
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/parameter_value.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// A map of fully qualified node names to a list of parameters
+using ParameterMap = std::unordered_map<std::string, std::vector<Parameter>>;
+
+/// Convert parameters from rcl_yaml_param_parser into C++ class instances.
+/// \param[in] c_params C structures containing parameters for multiple nodes.
+/// \returns a map where the keys are fully qualified node names and values a list of parameters.
+/// \throws InvalidParametersException if the `rcl_params_t` is inconsistent or invalid.
+RCLCPP_PUBLIC
+ParameterMap
+parameter_map_from(const rcl_params_t * const c_params);
+
+/// Convert parameter value from rcl_yaml_param_parser into a C++ class instance.
+/// \param[in] c_value C structure containing a value of a parameter.
+/// \returns an instance of a parameter value
+/// \throws InvalidParameterValueException if the `rcl_variant_t` is inconsistent or invalid.
+RCLCPP_PUBLIC
+ParameterValue
+parameter_value_from(const rcl_variant_t * const c_value);
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__PARAMETER_MAP_HPP_

rclcpp/include/rclcpp/parameter_service.hpp

@@ -12,166 +12,52 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_PARAMETER_SERVICE_HPP_
-#define RCLCPP_RCLCPP_PARAMETER_SERVICE_HPP_
+#ifndef RCLCPP__PARAMETER_SERVICE_HPP_
+#define RCLCPP__PARAMETER_SERVICE_HPP_
 
+#include <memory>
 #include <string>
 
-#include <rmw/rmw.h>
-
-#include <rclcpp/executors.hpp>
-#include <rclcpp/macros.hpp>
-#include <rclcpp/node.hpp>
-#include <rclcpp/parameter.hpp>
-
-#include <rcl_interfaces/srv/describe_parameters.hpp>
-#include <rcl_interfaces/srv/get_parameters.hpp>
-#include <rcl_interfaces/srv/get_parameter_types.hpp>
-#include <rcl_interfaces/srv/list_parameters.hpp>
-#include <rcl_interfaces/srv/set_parameters.hpp>
-#include <rcl_interfaces/srv/set_parameters_atomically.hpp>
+#include "rcl_interfaces/srv/describe_parameters.hpp"
+#include "rcl_interfaces/srv/get_parameter_types.hpp"
+#include "rcl_interfaces/srv/get_parameters.hpp"
+#include "rcl_interfaces/srv/list_parameters.hpp"
+#include "rcl_interfaces/srv/set_parameters.hpp"
+#include "rcl_interfaces/srv/set_parameters_atomically.hpp"
+#include "rclcpp/executors.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rmw/rmw.h"
 
 namespace rclcpp
 {
 
-namespace parameter_service
-{
-
 class ParameterService
 {
-
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(ParameterService);
+  RCLCPP_SMART_PTR_DEFINITIONS(ParameterService)
 
-  ParameterService(const rclcpp::node::Node::SharedPtr node)
-  : node_(node)
-  {
-    std::weak_ptr<rclcpp::node::Node> captured_node = node_;
-    get_parameters_service_ = node_->create_service<rcl_interfaces::srv::GetParameters>(
-      node_->get_name() + "__get_parameters", [captured_node](
-        const std::shared_ptr<rmw_request_id_t>,
-        const std::shared_ptr<rcl_interfaces::srv::GetParameters::Request> request,
-        std::shared_ptr<rcl_interfaces::srv::GetParameters::Response> response)
-        {
-          auto node = captured_node.lock();
-          if (!node) {
-            return;
-          }
-          auto values = node->get_parameters(request->names);
-          for (auto & pvariant : values) {
-            response->values.push_back(pvariant.get_parameter_value());
-          }
-        }
-      );
-
-    get_parameter_types_service_ = node_->create_service<rcl_interfaces::srv::GetParameterTypes>(
-      node_->get_name() + "__get_parameter_types", [captured_node](
-        const std::shared_ptr<rmw_request_id_t>,
-        const std::shared_ptr<rcl_interfaces::srv::GetParameterTypes::Request> request,
-        std::shared_ptr<rcl_interfaces::srv::GetParameterTypes::Response> response)
-        {
-          auto node = captured_node.lock();
-          if (!node) {
-            return;
-          }
-          auto types = node->get_parameter_types(request->names);
-          std::transform(types.cbegin(), types.cend(),
-          std::back_inserter(response->types), [](const uint8_t & type) {
-            return static_cast<rclcpp::parameter::ParameterType>(type);
-          });
-        }
-      );
-
-    set_parameters_service_ = node_->create_service<rcl_interfaces::srv::SetParameters>(
-      node_->get_name() + "__set_parameters", [captured_node](
-        const std::shared_ptr<rmw_request_id_t>,
-        const std::shared_ptr<rcl_interfaces::srv::SetParameters::Request> request,
-        std::shared_ptr<rcl_interfaces::srv::SetParameters::Response> response)
-        {
-          auto node = captured_node.lock();
-          if (!node) {
-            return;
-          }
-          std::vector<rclcpp::parameter::ParameterVariant> pvariants;
-          for (auto & p : request->parameters) {
-            pvariants.push_back(rclcpp::parameter::ParameterVariant::from_parameter(p));
-          }
-          auto results = node->set_parameters(pvariants);
-          response->results = results;
-        }
-      );
-
-    set_parameters_atomically_service_ =
-      node_->create_service<rcl_interfaces::srv::SetParametersAtomically>(
-      node_->get_name() + "__set_parameters_atomically", [captured_node](
-        const std::shared_ptr<rmw_request_id_t>,
-        const std::shared_ptr<rcl_interfaces::srv::SetParametersAtomically::Request> request,
-        std::shared_ptr<rcl_interfaces::srv::SetParametersAtomically::Response> response)
-        {
-          auto node = captured_node.lock();
-          if (!node) {
-            return;
-          }
-          std::vector<rclcpp::parameter::ParameterVariant> pvariants;
-          std::transform(request->parameters.cbegin(), request->parameters.cend(),
-          std::back_inserter(pvariants),
-          [](const rcl_interfaces::msg::Parameter & p) {
-            return rclcpp::parameter::ParameterVariant::
-            from_parameter(p);
-          });
-          auto result = node->set_parameters_atomically(pvariants);
-          response->result = result;
-        }
-      );
-
-    describe_parameters_service_ = node_->create_service<rcl_interfaces::srv::DescribeParameters>(
-      node_->get_name() + "__describe_parameters", [captured_node](
-        const std::shared_ptr<rmw_request_id_t>,
-        const std::shared_ptr<rcl_interfaces::srv::DescribeParameters::Request> request,
-        std::shared_ptr<rcl_interfaces::srv::DescribeParameters::Response> response)
-        {
-          auto node = captured_node.lock();
-          if (!node) {
-            return;
-          }
-          auto descriptors = node->describe_parameters(request->names);
-          response->descriptors = descriptors;
-        }
-      );
-
-    list_parameters_service_ = node_->create_service<rcl_interfaces::srv::ListParameters>(
-      node_->get_name() + "__list_parameters", [captured_node](
-        const std::shared_ptr<rmw_request_id_t>,
-        const std::shared_ptr<rcl_interfaces::srv::ListParameters::Request> request,
-        std::shared_ptr<rcl_interfaces::srv::ListParameters::Response> response)
-        {
-          auto node = captured_node.lock();
-          if (!node) {
-            return;
-          }
-          auto result = node->list_parameters(request->prefixes, request->depth);
-          response->result = result;
-        }
-      );
-
-
-  }
+  RCLCPP_PUBLIC
+  explicit ParameterService(
+    const std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
+    const std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+    rclcpp::node_interfaces::NodeParametersInterface * node_params,
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
 
 private:
-  const rclcpp::node::Node::SharedPtr node_;
-  rclcpp::service::Service<rcl_interfaces::srv::GetParameters>::SharedPtr get_parameters_service_;
-  rclcpp::service::Service<rcl_interfaces::srv::GetParameterTypes>::SharedPtr
+  rclcpp::Service<rcl_interfaces::srv::GetParameters>::SharedPtr get_parameters_service_;
+  rclcpp::Service<rcl_interfaces::srv::GetParameterTypes>::SharedPtr
     get_parameter_types_service_;
-  rclcpp::service::Service<rcl_interfaces::srv::SetParameters>::SharedPtr set_parameters_service_;
-  rclcpp::service::Service<rcl_interfaces::srv::SetParametersAtomically>::SharedPtr
+  rclcpp::Service<rcl_interfaces::srv::SetParameters>::SharedPtr set_parameters_service_;
+  rclcpp::Service<rcl_interfaces::srv::SetParametersAtomically>::SharedPtr
     set_parameters_atomically_service_;
-  rclcpp::service::Service<rcl_interfaces::srv::DescribeParameters>::SharedPtr
+  rclcpp::Service<rcl_interfaces::srv::DescribeParameters>::SharedPtr
     describe_parameters_service_;
-  rclcpp::service::Service<rcl_interfaces::srv::ListParameters>::SharedPtr list_parameters_service_;
+  rclcpp::Service<rcl_interfaces::srv::ListParameters>::SharedPtr list_parameters_service_;
 };
 
-} /* namespace parameter_service */
+}  // namespace rclcpp
 
-} /* namespace rclcpp */
-
-#endif /* RCLCPP_RCLCPP_PARAMETER_SERVICE_HPP_ */
+#endif  // RCLCPP__PARAMETER_SERVICE_HPP_

rclcpp/include/rclcpp/parameter_value.hpp

@@ -0,0 +1,345 @@
+// Copyright 2018 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__PARAMETER_VALUE_HPP_
+#define RCLCPP__PARAMETER_VALUE_HPP_
+
+#include <exception>
+#include <iostream>
+#include <ostream>
+#include <sstream>
+#include <string>
+#include <vector>
+
+#include "rcl_interfaces/msg/parameter_type.hpp"
+#include "rcl_interfaces/msg/parameter_value.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+enum ParameterType : uint8_t
+{
+  PARAMETER_NOT_SET = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET,
+  PARAMETER_BOOL = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL,
+  PARAMETER_INTEGER = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER,
+  PARAMETER_DOUBLE = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE,
+  PARAMETER_STRING = rcl_interfaces::msg::ParameterType::PARAMETER_STRING,
+  PARAMETER_BYTE_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY,
+  PARAMETER_BOOL_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY,
+  PARAMETER_INTEGER_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY,
+  PARAMETER_DOUBLE_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY,
+  PARAMETER_STRING_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY,
+};
+
+/// Return the name of a parameter type
+RCLCPP_PUBLIC
+std::string
+to_string(ParameterType type);
+
+RCLCPP_PUBLIC
+std::ostream &
+operator<<(std::ostream & os, ParameterType type);
+
+/// Indicate the parameter type does not match the expected type.
+class ParameterTypeException : public std::runtime_error
+{
+public:
+  /// Construct an instance.
+  /**
+   * \param[in] expected the expected parameter type.
+   * \param[in] actual the actual parameter type.
+   */
+  RCLCPP_PUBLIC
+  ParameterTypeException(ParameterType expected, ParameterType actual)
+  : std::runtime_error("expected [" + to_string(expected) + "] got [" + to_string(actual) + "]")
+  {}
+};
+
+/// Store the type and value of a parameter.
+class ParameterValue
+{
+public:
+  /// Construct a parameter value with type PARAMETER_NOT_SET.
+  RCLCPP_PUBLIC
+  ParameterValue();
+  /// Construct a parameter value from a message.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const rcl_interfaces::msg::ParameterValue & value);
+  /// Construct a parameter value with type PARAMETER_BOOL.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const bool bool_value);
+  /// Construct a parameter value with type PARAMETER_INTEGER.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const int int_value);
+  /// Construct a parameter value with type PARAMETER_INTEGER.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const int64_t int_value);
+  /// Construct a parameter value with type PARAMETER_DOUBLE.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const float double_value);
+  /// Construct a parameter value with type PARAMETER_DOUBLE.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const double double_value);
+  /// Construct a parameter value with type PARAMETER_STRING.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const std::string & string_value);
+  /// Construct a parameter value with type PARAMETER_STRING.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const char * string_value);
+  /// Construct a parameter value with type PARAMETER_BYTE_ARRAY.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const std::vector<uint8_t> & byte_array_value);
+  /// Construct a parameter value with type PARAMETER_BOOL_ARRAY.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const std::vector<bool> & bool_array_value);
+  /// Construct a parameter value with type PARAMETER_INTEGER_ARRAY.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const std::vector<int> & int_array_value);
+  /// Construct a parameter value with type PARAMETER_INTEGER_ARRAY.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const std::vector<int64_t> & int_array_value);
+  /// Construct a parameter value with type PARAMETER_DOUBLE_ARRAY.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const std::vector<float> & double_array_value);
+  /// Construct a parameter value with type PARAMETER_DOUBLE_ARRAY.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const std::vector<double> & double_array_value);
+  /// Construct a parameter value with type PARAMETER_STRING_ARRAY.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const std::vector<std::string> & string_array_value);
+
+  /// Return an enum indicating the type of the set value.
+  RCLCPP_PUBLIC
+  ParameterType
+  get_type() const;
+
+  /// Return a message populated with the parameter value
+  RCLCPP_PUBLIC
+  rcl_interfaces::msg::ParameterValue
+  to_value_msg() const;
+
+  /// Equal operator.
+  RCLCPP_PUBLIC
+  bool
+  operator==(const ParameterValue & rhs) const;
+
+  /// Not equal operator.
+  RCLCPP_PUBLIC
+  bool
+  operator!=(const ParameterValue & rhs) const;
+
+  // The following get() variants require the use of ParameterType
+
+  template<ParameterType type>
+  constexpr
+  typename std::enable_if<type == ParameterType::PARAMETER_BOOL, const bool &>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) {
+      throw ParameterTypeException(ParameterType::PARAMETER_BOOL, get_type());
+    }
+    return value_.bool_value;
+  }
+
+  template<ParameterType type>
+  constexpr
+  typename std::enable_if<type == ParameterType::PARAMETER_INTEGER, const int64_t &>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) {
+      throw ParameterTypeException(ParameterType::PARAMETER_INTEGER, get_type());
+    }
+    return value_.integer_value;
+  }
+
+  template<ParameterType type>
+  constexpr
+  typename std::enable_if<type == ParameterType::PARAMETER_DOUBLE, const double &>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) {
+      throw ParameterTypeException(ParameterType::PARAMETER_DOUBLE, get_type());
+    }
+    return value_.double_value;
+  }
+
+  template<ParameterType type>
+  constexpr
+  typename std::enable_if<type == ParameterType::PARAMETER_STRING, const std::string &>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_STRING) {
+      throw ParameterTypeException(ParameterType::PARAMETER_STRING, get_type());
+    }
+    return value_.string_value;
+  }
+
+  template<ParameterType type>
+  constexpr
+  typename std::enable_if<
+    type == ParameterType::PARAMETER_BYTE_ARRAY, const std::vector<uint8_t> &>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY) {
+      throw ParameterTypeException(ParameterType::PARAMETER_BYTE_ARRAY, get_type());
+    }
+    return value_.byte_array_value;
+  }
+
+  template<ParameterType type>
+  constexpr
+  typename std::enable_if<
+    type == ParameterType::PARAMETER_BOOL_ARRAY, const std::vector<bool> &>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY) {
+      throw ParameterTypeException(ParameterType::PARAMETER_BOOL_ARRAY, get_type());
+    }
+    return value_.bool_array_value;
+  }
+
+  template<ParameterType type>
+  constexpr
+  typename std::enable_if<
+    type == ParameterType::PARAMETER_INTEGER_ARRAY, const std::vector<int64_t> &>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY) {
+      throw ParameterTypeException(ParameterType::PARAMETER_INTEGER_ARRAY, get_type());
+    }
+    return value_.integer_array_value;
+  }
+
+  template<ParameterType type>
+  constexpr
+  typename std::enable_if<
+    type == ParameterType::PARAMETER_DOUBLE_ARRAY, const std::vector<double> &>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY) {
+      throw ParameterTypeException(ParameterType::PARAMETER_DOUBLE_ARRAY, get_type());
+    }
+    return value_.double_array_value;
+  }
+
+  template<ParameterType type>
+  constexpr
+  typename std::enable_if<
+    type == ParameterType::PARAMETER_STRING_ARRAY, const std::vector<std::string> &>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY) {
+      throw ParameterTypeException(ParameterType::PARAMETER_STRING_ARRAY, get_type());
+    }
+    return value_.string_array_value;
+  }
+
+  // The following get() variants allow the use of primitive types
+
+  template<typename type>
+  constexpr
+  typename std::enable_if<std::is_same<type, bool>::value, const bool &>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_BOOL>();
+  }
+
+  template<typename type>
+  constexpr
+  typename std::enable_if<
+    std::is_integral<type>::value && !std::is_same<type, bool>::value, const int64_t &>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_INTEGER>();
+  }
+
+  template<typename type>
+  constexpr
+  typename std::enable_if<std::is_floating_point<type>::value, const double &>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_DOUBLE>();
+  }
+
+  template<typename type>
+  constexpr
+  typename std::enable_if<std::is_convertible<type, std::string>::value, const std::string &>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_STRING>();
+  }
+
+  template<typename type>
+  constexpr
+  typename std::enable_if<
+    std::is_convertible<
+      type, const std::vector<uint8_t> &>::value, const std::vector<uint8_t> &>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_BYTE_ARRAY>();
+  }
+
+  template<typename type>
+  constexpr
+  typename std::enable_if<
+    std::is_convertible<
+      type, const std::vector<bool> &>::value, const std::vector<bool> &>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_BOOL_ARRAY>();
+  }
+
+  template<typename type>
+  constexpr
+  typename std::enable_if<
+    std::is_convertible<
+      type, const std::vector<int64_t> &>::value, const std::vector<int64_t> &>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_INTEGER_ARRAY>();
+  }
+
+  template<typename type>
+  constexpr
+  typename std::enable_if<
+    std::is_convertible<
+      type, const std::vector<double> &>::value, const std::vector<double> &>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_DOUBLE_ARRAY>();
+  }
+
+  template<typename type>
+  constexpr
+  typename std::enable_if<
+    std::is_convertible<
+      type, const std::vector<std::string> &>::value, const std::vector<std::string> &>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_STRING_ARRAY>();
+  }
+
+private:
+  rcl_interfaces::msg::ParameterValue value_;
+};
+
+/// Return the value of a parameter as a string
+RCLCPP_PUBLIC
+std::string
+to_string(const ParameterValue & type);
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__PARAMETER_VALUE_HPP_

rclcpp/include/rclcpp/publisher.hpp

@@ -18,337 +18,354 @@
 #include <rmw/error_handling.h>
 #include <rmw/rmw.h>
 
+#include <functional>
 #include <iostream>
 #include <memory>
-#include <mutex>
 #include <sstream>
 #include <string>
+#include <utility>
 
-#include "rcl_interfaces/msg/intra_process_message.hpp"
-#include "rmw/impl/cpp/demangle.hpp"
+#include "rcl/error_handling.h"
+#include "rcl/publisher.h"
 
+#include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/allocator/allocator_deleter.hpp"
+#include "rclcpp/detail/resolve_use_intra_process.hpp"
+#include "rclcpp/experimental/intra_process_manager.hpp"
+#include "rclcpp/loaned_message.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/publisher_base.hpp"
+#include "rclcpp/publisher_options.hpp"
+#include "rclcpp/type_support_decl.hpp"
+#include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
 
-// Forward declaration for friend statement
-namespace node
-{
-class Node;
-}  // namespace node
-
-namespace publisher
-{
-
-class PublisherBase
-{
-public:
-  RCLCPP_SMART_PTR_DEFINITIONS(PublisherBase);
-  /// Default constructor.
-  /**
-   * Typically, a publisher is not created through this method, but instead is created through a
-   * call to `Node::create_publisher`.
-   * \param[in] node_handle The corresponding rmw representation of the owner node.
-   * \param[in] publisher_handle The rmw publisher handle corresponding to this publisher.
-   * \param[in] topic The topic that this publisher publishes on.
-   * \param[in] queue_size The maximum number of unpublished messages to queue.
-   */
-  PublisherBase(
-    std::shared_ptr<rmw_node_t> node_handle,
-    rmw_publisher_t * publisher_handle,
-    std::string topic,
-    size_t queue_size)
-  : node_handle_(node_handle), publisher_handle_(publisher_handle),
-    intra_process_publisher_handle_(nullptr),
-    topic_(topic), queue_size_(queue_size),
-    intra_process_publisher_id_(0), store_intra_process_message_(nullptr)
-  {
-    // Life time of this object is tied to the publisher handle.
-    if (rmw_get_gid_for_publisher(publisher_handle_, &rmw_gid_) != RMW_RET_OK) {
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-      throw std::runtime_error(
-        std::string("failed to get publisher gid: ") + rmw_get_error_string_safe());
-      // *INDENT-ON*
-    }
-  }
-
-  /// Default destructor.
-  virtual ~PublisherBase()
-  {
-    if (intra_process_publisher_handle_) {
-      if (rmw_destroy_publisher(node_handle_.get(), intra_process_publisher_handle_)) {
-        fprintf(
-          stderr,
-          "Error in destruction of intra process rmw publisher handle: %s\n",
-          rmw_get_error_string_safe());
-      }
-    }
-    if (publisher_handle_) {
-      if (rmw_destroy_publisher(node_handle_.get(), publisher_handle_) != RMW_RET_OK) {
-        fprintf(
-          stderr,
-          "Error in destruction of rmw publisher handle: %s\n",
-          rmw_get_error_string_safe());
-      }
-    }
-  }
-
-  /// Get the topic that this publisher publishes on.
-  // \return The topic name.
-  const std::string &
-  get_topic_name() const
-  {
-    return topic_;
-  }
-
-  /// Get the queue size for this publisher.
-  // \return The queue size.
-  size_t
-  get_queue_size() const
-  {
-    return queue_size_;
-  }
-
-  /// Get the global identifier for this publisher (used in rmw and by DDS).
-  // \return The gid.
-  const rmw_gid_t &
-  get_gid() const
-  {
-    return rmw_gid_;
-  }
-
-  /// Get the global identifier for this publisher used by intra-process communication.
-  // \return The intra-process gid.
-  const rmw_gid_t &
-  get_intra_process_gid() const
-  {
-    return intra_process_rmw_gid_;
-  }
-
-  /// Compare this publisher to a gid.
-  /**
-   * Note that this function calls the next function.
-   * \param[in] gid Reference to a gid.
-   * \return True if the publisher's gid matches the input.
-   */
-  bool
-  operator==(const rmw_gid_t & gid) const
-  {
-    return *this == &gid;
-  }
-
-  /// Compare this publisher to a pointer gid.
-  /**
-   * A wrapper for comparing this publisher's gid to the input using rmw_compare_gids_equal.
-   * \param[in] gid A pointer to a gid.
-   * \return True if this publisher's gid matches the input.
-   */
-  bool
-  operator==(const rmw_gid_t * gid) const
-  {
-    bool result = false;
-    auto ret = rmw_compare_gids_equal(gid, &this->get_gid(), &result);
-    if (ret != RMW_RET_OK) {
-      throw std::runtime_error(
-              std::string("failed to compare gids: ") + rmw_get_error_string_safe());
-    }
-    if (!result) {
-      ret = rmw_compare_gids_equal(gid, &this->get_intra_process_gid(), &result);
-      if (ret != RMW_RET_OK) {
-        throw std::runtime_error(
-                std::string("failed to compare gids: ") + rmw_get_error_string_safe());
-      }
-    }
-    return result;
-  }
-
-  typedef std::function<uint64_t(uint64_t, void *, const std::type_info &)> StoreMessageCallbackT;
-
-protected:
-  void
-  setup_intra_process(
-    uint64_t intra_process_publisher_id,
-    StoreMessageCallbackT callback,
-    rmw_publisher_t * intra_process_publisher_handle)
-  {
-    intra_process_publisher_id_ = intra_process_publisher_id;
-    store_intra_process_message_ = callback;
-    intra_process_publisher_handle_ = intra_process_publisher_handle;
-    // Life time of this object is tied to the publisher handle.
-    auto ret = rmw_get_gid_for_publisher(intra_process_publisher_handle_, &intra_process_rmw_gid_);
-    if (ret != RMW_RET_OK) {
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-      throw std::runtime_error(
-        std::string("failed to create intra process publisher gid: ") +
-        rmw_get_error_string_safe());
-      // *INDENT-ON*
-    }
-  }
-
-  std::shared_ptr<rmw_node_t> node_handle_;
-
-  rmw_publisher_t * publisher_handle_;
-  rmw_publisher_t * intra_process_publisher_handle_;
-
-  std::string topic_;
-  size_t queue_size_;
-
-  uint64_t intra_process_publisher_id_;
-  StoreMessageCallbackT store_intra_process_message_;
-
-  rmw_gid_t rmw_gid_;
-  rmw_gid_t intra_process_rmw_gid_;
-
-  std::mutex intra_process_publish_mutex_;
-};
+template<typename MessageT, typename AllocatorT>
+class LoanedMessage;
 
 /// A publisher publishes messages of any type to a topic.
-template<typename MessageT, typename Alloc = std::allocator<void>>
+template<typename MessageT, typename AllocatorT = std::allocator<void>>
 class Publisher : public PublisherBase
 {
-  friend rclcpp::node::Node;
-
 public:
-  using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
-  using MessageAlloc = typename MessageAllocTraits::allocator_type;
-  using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
+  using MessageAllocatorTraits = allocator::AllocRebind<MessageT, AllocatorT>;
+  using MessageAllocator = typename MessageAllocatorTraits::allocator_type;
+  using MessageDeleter = allocator::Deleter<MessageAllocator, MessageT>;
   using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
+  using MessageSharedPtr = std::shared_ptr<const MessageT>;
 
-  RCLCPP_SMART_PTR_DEFINITIONS(Publisher<MessageT, Alloc>);
+  RCLCPP_SMART_PTR_DEFINITIONS(Publisher<MessageT, AllocatorT>)
 
   Publisher(
-    std::shared_ptr<rmw_node_t> node_handle,
-    rmw_publisher_t * publisher_handle,
-    std::string topic,
-    size_t queue_size,
-    std::shared_ptr<Alloc> allocator)
-  : PublisherBase(node_handle, publisher_handle, topic, queue_size)
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    const std::string & topic,
+    const rclcpp::QoS & qos,
+    const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options)
+  : PublisherBase(
+      node_base,
+      topic,
+      *rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>(),
+      options.template to_rcl_publisher_options<MessageT>(qos)),
+    options_(options),
+    message_allocator_(new MessageAllocator(*options.get_allocator().get()))
   {
-    message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
     allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get());
+
+    if (options_.event_callbacks.deadline_callback) {
+      this->add_event_handler(
+        options_.event_callbacks.deadline_callback,
+        RCL_PUBLISHER_OFFERED_DEADLINE_MISSED);
+    }
+    if (options_.event_callbacks.liveliness_callback) {
+      this->add_event_handler(
+        options_.event_callbacks.liveliness_callback,
+        RCL_PUBLISHER_LIVELINESS_LOST);
+    }
+    if (options_.event_callbacks.incompatible_qos_callback) {
+      this->add_event_handler(
+        options_.event_callbacks.incompatible_qos_callback,
+        RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
+    } else if (options_.use_default_callbacks) {
+      // Register default callback when not specified
+      try {
+        this->add_event_handler(
+          [this](QOSOfferedIncompatibleQoSInfo & info) {
+            this->default_incompatible_qos_callback(info);
+          },
+          RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);
+      } catch (UnsupportedEventTypeException & /*exc*/) {
+        // pass
+      }
+    }
+    // Setup continues in the post construction method, post_init_setup().
   }
 
+  /// Called post construction, so that construction may continue after shared_from_this() works.
+  virtual
+  void
+  post_init_setup(
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    const std::string & topic,
+    const rclcpp::QoS & qos,
+    const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options)
+  {
+    // Topic is unused for now.
+    (void)topic;
+    (void)options;
+
+    // If needed, setup intra process communication.
+    if (rclcpp::detail::resolve_use_intra_process(options_, *node_base)) {
+      auto context = node_base->get_context();
+      // Get the intra process manager instance for this context.
+      auto ipm = context->get_sub_context<rclcpp::experimental::IntraProcessManager>();
+      // Register the publisher with the intra process manager.
+      if (qos.get_rmw_qos_profile().history == RMW_QOS_POLICY_HISTORY_KEEP_ALL) {
+        throw std::invalid_argument(
+                "intraprocess communication is not allowed with keep all history qos policy");
+      }
+      if (qos.get_rmw_qos_profile().depth == 0) {
+        throw std::invalid_argument(
+                "intraprocess communication is not allowed with a zero qos history depth value");
+      }
+      if (qos.get_rmw_qos_profile().durability != RMW_QOS_POLICY_DURABILITY_VOLATILE) {
+        throw std::invalid_argument(
+                "intraprocess communication allowed only with volatile durability");
+      }
+      uint64_t intra_process_publisher_id = ipm->add_publisher(this->shared_from_this());
+      this->setup_intra_process(
+        intra_process_publisher_id,
+        ipm);
+    }
+  }
+
+  virtual ~Publisher()
+  {}
+
+  /// Borrow a loaned ROS message from the middleware.
+  /**
+   * If the middleware is capable of loaning memory for a ROS message instance,
+   * the loaned message will be directly allocated in the middleware.
+   * If not, the message allocator of this rclcpp::Publisher instance is being used.
+   *
+   * With a call to \sa `publish` the LoanedMessage instance is being returned to the middleware
+   * or free'd accordingly to the allocator.
+   * If the message is not being published but processed differently, the destructor of this
+   * class will either return the message to the middleware or deallocate it via the internal
+   * allocator.
+   * \sa rclcpp::LoanedMessage for details of the LoanedMessage class.
+   *
+   * \return LoanedMessage containing memory for a ROS message of type MessageT
+   */
+  rclcpp::LoanedMessage<MessageT, AllocatorT>
+  borrow_loaned_message()
+  {
+    return rclcpp::LoanedMessage<MessageT, AllocatorT>(this, this->get_allocator());
+  }
 
   /// Send a message to the topic for this publisher.
   /**
    * This function is templated on the input message type, MessageT.
    * \param[in] msg A shared pointer to the message to send.
    */
-  void
-  publish(std::unique_ptr<MessageT, MessageDeleter> & msg)
+  virtual void
+  publish(std::unique_ptr<MessageT, MessageDeleter> msg)
   {
-    this->do_inter_process_publish(msg.get());
-    if (store_intra_process_message_) {
-      // Take the pointer from the unique_msg, release it and pass as a void *
-      // to the ipm. The ipm should then capture it again as a unique_ptr of
-      // the correct type.
-      // TODO(wjwwood):
-      //   investigate how to transfer the custom deleter (if there is one)
-      //   from the incoming unique_ptr through to the ipm's unique_ptr.
-      //   See: http://stackoverflow.com/questions/11002641/dynamic-casting-for-unique-ptr
-      MessageT * msg_ptr = msg.get();
-      msg.release();
-      uint64_t message_seq;
-      {
-        std::lock_guard<std::mutex> lock(intra_process_publish_mutex_);
-        message_seq =
-          store_intra_process_message_(intra_process_publisher_id_, msg_ptr, typeid(MessageT));
-      }
-      rcl_interfaces::msg::IntraProcessMessage ipm;
-      ipm.publisher_id = intra_process_publisher_id_;
-      ipm.message_sequence = message_seq;
-      auto status = rmw_publish(intra_process_publisher_handle_, &ipm);
-      if (status != RMW_RET_OK) {
-        // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-        throw std::runtime_error(
-          std::string("failed to publish intra process message: ") + rmw_get_error_string_safe());
-        // *INDENT-ON*
-      }
+    if (!intra_process_is_enabled_) {
+      this->do_inter_process_publish(*msg);
+      return;
+    }
+    // If an interprocess subscription exist, then the unique_ptr is promoted
+    // to a shared_ptr and published.
+    // This allows doing the intraprocess publish first and then doing the
+    // interprocess publish, resulting in lower publish-to-subscribe latency.
+    // It's not possible to do that with an unique_ptr,
+    // as do_intra_process_publish takes the ownership of the message.
+    bool inter_process_publish_needed =
+      get_subscription_count() > get_intra_process_subscription_count();
+
+    if (inter_process_publish_needed) {
+      auto shared_msg = this->do_intra_process_publish_and_return_shared(std::move(msg));
+      this->do_inter_process_publish(*shared_msg);
     } else {
-      // Always destroy the message, even if we don't consume it, for consistency.
-      msg.reset();
+      this->do_intra_process_publish(std::move(msg));
     }
   }
 
-  void
-  publish(const std::shared_ptr<MessageT> & msg)
-  {
-    // Avoid allocating when not using intra process.
-    if (!store_intra_process_message_) {
-      // In this case we're not using intra process.
-      return this->do_inter_process_publish(msg.get());
-    }
-    // Otherwise we have to allocate memory in a unique_ptr and pass it along.
-    // TODO(wjwwood):
-    //   The intra process manager should probably also be able to store
-    //   shared_ptr's and do the "smart" thing based on other intra process
-    //   subscriptions. For now call the other publish().
-    auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
-    MessageAllocTraits::construct(*message_allocator_.get(), ptr, *msg.get());
-    MessageUniquePtr unique_msg(ptr, message_deleter_);
-    return this->publish(unique_msg);
-  }
-
-  void
-  publish(std::shared_ptr<const MessageT> msg)
-  {
-    // Avoid allocating when not using intra process.
-    if (!store_intra_process_message_) {
-      // In this case we're not using intra process.
-      return this->do_inter_process_publish(msg.get());
-    }
-    // Otherwise we have to allocate memory in a unique_ptr and pass it along.
-    // TODO(wjwwood):
-    //   The intra process manager should probably also be able to store
-    //   shared_ptr's and do the "smart" thing based on other intra process
-    //   subscriptions. For now call the other publish().
-    auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
-    MessageAllocTraits::construct(*message_allocator_.get(), ptr, *msg.get());
-    MessageUniquePtr unique_msg(ptr, message_deleter_);
-    return this->publish(unique_msg);
-  }
-
-  void
+  virtual void
   publish(const MessageT & msg)
   {
     // Avoid allocating when not using intra process.
-    if (!store_intra_process_message_) {
+    if (!intra_process_is_enabled_) {
       // In this case we're not using intra process.
-      return this->do_inter_process_publish(&msg);
+      return this->do_inter_process_publish(msg);
     }
     // Otherwise we have to allocate memory in a unique_ptr and pass it along.
-    auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
-    MessageAllocTraits::construct(*message_allocator_.get(), ptr, msg);
+    // As the message is not const, a copy should be made.
+    // A shared_ptr<const MessageT> could also be constructed here.
+    auto ptr = MessageAllocatorTraits::allocate(*message_allocator_.get(), 1);
+    MessageAllocatorTraits::construct(*message_allocator_.get(), ptr, msg);
     MessageUniquePtr unique_msg(ptr, message_deleter_);
-    return this->publish(unique_msg);
+    this->publish(std::move(unique_msg));
   }
 
-  std::shared_ptr<MessageAlloc> get_allocator() const
+  void
+  publish(const rcl_serialized_message_t & serialized_msg)
+  {
+    return this->do_serialized_publish(&serialized_msg);
+  }
+
+  void
+  publish(const SerializedMessage & serialized_msg)
+  {
+    return this->do_serialized_publish(&serialized_msg.get_rcl_serialized_message());
+  }
+
+  /// Publish an instance of a LoanedMessage.
+  /**
+   * When publishing a loaned message, the memory for this ROS message will be deallocated
+   * after being published.
+   * The instance of the loaned message is no longer valid after this call.
+   *
+   * \param loaned_msg The LoanedMessage instance to be published.
+   */
+  void
+  publish(rclcpp::LoanedMessage<MessageT, AllocatorT> && loaned_msg)
+  {
+    if (!loaned_msg.is_valid()) {
+      throw std::runtime_error("loaned message is not valid");
+    }
+    if (intra_process_is_enabled_) {
+      // TODO(Karsten1987): support loaned message passed by intraprocess
+      throw std::runtime_error("storing loaned messages in intra process is not supported yet");
+    }
+
+    // verify that publisher supports loaned messages
+    // TODO(Karsten1987): This case separation has to be done in rclcpp
+    // otherwise we have to ensure that every middleware implements
+    // `rmw_publish_loaned_message` explicitly the same way as `rmw_publish`
+    // by taking a copy of the ros message.
+    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());
+    } 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.
+      this->do_inter_process_publish(loaned_msg.get());
+    }
+  }
+
+  std::shared_ptr<MessageAllocator>
+  get_allocator() const
   {
     return message_allocator_;
   }
 
 protected:
   void
-  do_inter_process_publish(const MessageT * msg)
+  do_inter_process_publish(const MessageT & msg)
   {
-    auto status = rmw_publish(publisher_handle_, msg);
-    if (status != RMW_RET_OK) {
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-      throw std::runtime_error(
-        std::string("failed to publish message: ") + rmw_get_error_string_safe());
-      // *INDENT-ON*
+    auto status = rcl_publish(&publisher_handle_, &msg, nullptr);
+
+    if (RCL_RET_PUBLISHER_INVALID == status) {
+      rcl_reset_error();  // next call will reset error message if not context
+      if (rcl_publisher_is_valid_except_context(&publisher_handle_)) {
+        rcl_context_t * context = rcl_publisher_get_context(&publisher_handle_);
+        if (nullptr != context && !rcl_context_is_valid(context)) {
+          // publisher is invalid due to context being shutdown
+          return;
+        }
+      }
+    }
+    if (RCL_RET_OK != status) {
+      rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish message");
     }
   }
 
-  std::shared_ptr<MessageAlloc> message_allocator_;
+  void
+  do_serialized_publish(const rcl_serialized_message_t * serialized_msg)
+  {
+    if (intra_process_is_enabled_) {
+      // TODO(Karsten1987): support serialized message passed by intraprocess
+      throw std::runtime_error("storing serialized messages in intra process is not supported yet");
+    }
+    auto status = rcl_publish_serialized_message(&publisher_handle_, serialized_msg, nullptr);
+    if (RCL_RET_OK != status) {
+      rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish serialized message");
+    }
+  }
+
+  void
+  do_loaned_message_publish(MessageT * msg)
+  {
+    auto status = rcl_publish_loaned_message(&publisher_handle_, msg, nullptr);
+
+    if (RCL_RET_PUBLISHER_INVALID == status) {
+      rcl_reset_error();  // next call will reset error message if not context
+      if (rcl_publisher_is_valid_except_context(&publisher_handle_)) {
+        rcl_context_t * context = rcl_publisher_get_context(&publisher_handle_);
+        if (nullptr != context && !rcl_context_is_valid(context)) {
+          // publisher is invalid due to context being shutdown
+          return;
+        }
+      }
+    }
+    if (RCL_RET_OK != status) {
+      rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish message");
+    }
+  }
+
+  void
+  do_intra_process_publish(std::unique_ptr<MessageT, MessageDeleter> msg)
+  {
+    auto ipm = weak_ipm_.lock();
+    if (!ipm) {
+      throw std::runtime_error(
+              "intra process publish called after destruction of intra process manager");
+    }
+    if (!msg) {
+      throw std::runtime_error("cannot publish msg which is a null pointer");
+    }
+
+    ipm->template do_intra_process_publish<MessageT, AllocatorT>(
+      intra_process_publisher_id_,
+      std::move(msg),
+      message_allocator_);
+  }
+
+  std::shared_ptr<const MessageT>
+  do_intra_process_publish_and_return_shared(std::unique_ptr<MessageT, MessageDeleter> msg)
+  {
+    auto ipm = weak_ipm_.lock();
+    if (!ipm) {
+      throw std::runtime_error(
+              "intra process publish called after destruction of intra process manager");
+    }
+    if (!msg) {
+      throw std::runtime_error("cannot publish msg which is a null pointer");
+    }
+
+    return ipm->template do_intra_process_publish_and_return_shared<MessageT, AllocatorT>(
+      intra_process_publisher_id_,
+      std::move(msg),
+      message_allocator_);
+  }
+
+  /// Copy of original options passed during construction.
+  /**
+   * It is important to save a copy of this so that the rmw payload which it
+   * may contain is kept alive for the duration of the publisher.
+   */
+  const rclcpp::PublisherOptionsWithAllocator<AllocatorT> options_;
+
+  std::shared_ptr<MessageAllocator> message_allocator_;
 
   MessageDeleter message_deleter_;
 };
 
-}  // namespace publisher
 }  // namespace rclcpp
 
 #endif  // RCLCPP__PUBLISHER_HPP_